Post

Scout USB Driver Documentation

Posted
By Jacob Chisholm 11 min read

USB Interface: Documentation for the “Scout” USB CDC-ACM (Common Device Class - Abstract Control Model) interface built using FreeRTOS and Dmitry Filimonchuk’s libusb_stm32 Hardware Abstraction Layer (HAL).

It is designed to be event driven, utilizing FreeRTOS queues to safely offload data processing to FreeRTOS tasks.

Device Specifications

The following section details the high-level device specifications, shown from the viewpoint of the host as per USB IF specifications.

  • Vendor ID: 0xFFFE
  • Product ID: 0xD40C
  • Control Endpoint (CDC-ACM)
    • NTF Interface Number: 0x00
    • Data Interface Number: 0x01
    • Data RX EP: 0x01
    • Data TX EP: 0x81
    • Maximum Data Packet Size: 0x40
    • NTF EP: 0x82
    • Maximum NTF Packet Size: 0x08
  • LiDAR Endpoint (CDC-ACM)
    • NTF Interface Number: 0x02
    • Data Interface Number: 0x03
    • Data RX EP: 0x03
    • Data TX EP: 0x83
    • Maximum Data Packet Size: 0x40
    • NTF EP: 0x84
    • Maximum NTF Packet Size: 0x08

Endpoint Descriptions

Control Endpoint

The control endpoint is configured as a CDC-ACM endpoint designed for the exchange of control data between the control board and a higher level controller. The endpoint expects different packets for RX (Receive) and TX (Transmit) events. The TX packet (shown below) describes the expected data format of control commands sent to the control board from the high-level host.

1
2
3
4
5
6
7
8
9

struct udev_pkt_ctrl_tx {
    union {
        struct {
            float x, y, z, w;
        };
        float data[4];
    } vel;
    uint8_t mode;
} __attribute__((packed, aligned(4)));

A description of the command structure is provided below:

  • vel - Velocity Command
    • x - Forwards/Reverse Velocity Command (-1 to 1)
    • y - Sideways Velocity Command (-1 to 1)
    • z - Height Velocity Command (-1 to 1)
    • w - Yaw/Rotate Velocity Command (-1 to 1)
  • mode - Request Mode Change (enum eCBMode)

Note that the mode field is described within the command structure as a uint8_t to ensure identical bit widths between the embedded device and host controller. In practice, the mode field should be interpreted as a member of the eCBMode enumeration (shown below).

1
2
3
4
5
6
7

enum eCBMode {
    eModeStalled,
    eModeRC,
    eModeNormal,
    eModeSim,
    eModeFault
};

The RX packet (shown below) describes the data format sent from the control board to the high-level host.

1
2
3
4
5
6
7
8
9
10
11
12

struct udev_pkt_ctrl_rx {
    union {
        struct {
            float x, y, z, w;
        };
        float data[4];
    } vel;
    uint8_t vBatt;
    uint8_t rssi;
    uint8_t status;
    uint8_t mode;
} __attribute__((packed, aligned(4)));

A description of the command structure is provided below:

  • vel - Actual Velocity command that was forwarded to the drone Flight Controller (FC)
    • x - Forwards/Reverse value currently applied to the FC (-1 to 1)
    • y - Sideways value currently applied to the FC (-1 to 1)
    • z - Height value currently applied to the FC (-1 to 1)
    • w - Yaw/Rotate currently applied to the FC (-1 to 1)
  • vBatt - Battery Voltage in $\frac{V}{4}$
  • rssi - Received Signal Strength Intensity from controller (0 to 100%)
  • status - Control Board Status (enum eCBStatus)
  • mode - Current Control Board Mode (enum eCBMode)

Again, note that the mode and status fields are described as uint8_t to ensure identical bit widths between the embedded device and the high-level host controller. Both fields should be interpreted as members of enum eCBMode and enum eCBStatus respectively.

LiDAR Endpoint

The LiDAR endpoint is configured as a CDC-ACM endpoint designed for bidirectional and unidirectional exchange of LiDAR data between the control board and higher level controller. Given that LiDAR stream data is typically larger than the maximum USB packet size (0x40 or 64 bytes), the data packets used within the LiDAR endpoint have been designed to support unordered LiDAR data transmission. As a result, LiDAR data may be transmitted or received out of order, but can be processed without reordering. The LiDAR endpoint can be operated in one of two modes (Simulation or Normal), configured via the mode parameter sent inside of the control endpoint.

Simulation Mode: When operating in Simulation mode, the LiDAR endpoint is used for bidirectional data exchange. In this mode, the Control Board expects to receive simulation LiDAR data from the LiDAR endpoint. The Control Board will then be able to use the simulated LiDAR data as if it were real data. While in Simulation Mode, all other LiDAR endpoint functionality is still enabled. Thus, the transmission of LiDAR data to the high-level controller can be tested while in Simulation Mode.

All/Normal Mode(s): In all modes, the LiDAR endpoint acts as a unidirectional endpoint, transmitting LiDAR data captured from the LiDAR input (either sensors or simulation). This enables the high-level control device to gain direct access to the LiDAR data being used for hover and collision control.

LiDAR packet formatting is universal for both RX and TX events. The packet format is designed to handle 180 LiDAR points from two LiDAR data streams. The packet format assumes all distance measurements are equidistant apart (2 deg for 180 points).

1
2
3
4
5
6
7
8
9
10
11
12
13
14

enum eCBLidar {
    eLidarFront = 0U,
    eLidarVertical = 1U
};
struct udev_pkt_lidar {
    struct {
        uint8_t id : 1;
        uint8_t sequence : 7;
        uint8_t len;
    } __attribute__((packed)) hdr;
    uint16_t distance_sum;
    // distance (mm) = distance / 4.0
    uint16_t distances[UDEV_LIDAR_POINTS];
} __attribute__((aligned(4)));

A description of the LiDAR data packet is shown below:

  • hdr - Header Data
    • id - ID of the LiDAR (enum eCBLidar)
    • sequence - Packet Sequence Number (0 to UDEV_LIDAR_SEQ_MAX)
    • len - Number of LiDAR distances in this packet
  • distance_sum - OPTIONAL Average distance of all LiDAR distances in this packet
  • distances - LiDAR measured distance in $\frac{mm}{4.0}$

Note that the LiDAR packet is configurable via the following definitions placed in the usb_packet.h header.

1
2
3

#define UDEV_LIDAR_RANGE 180
#define UDEV_LIDAR_POINTS ((LIDAR_DATA_SZ - 4) / 2)
#define UDEV_LIDAR_SEQ_MAX ((UDEV_LIDAR_RANGE - 1 + UDEV_LIDAR_POINTS) / UDEV_LIDAR_POINTS)

Note that the UDEV_LIDAR_POINTS definition is configured to calculate the maximum number of distance points that can be placed within a single packet. This number is calculated based on the maximum packet size (0x40 or 64 bytes), the header size (2 bytes) and the size of distance_sum (2 bytes).

The following extraneous definitions and helper functions are also available within the usb_lidar.h header.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

#define UDEV_LIDAR_SEQ_STEP ((float) (3.1415926535f / (int) UDEV_LIDAR_SEQ_MAX))
#define UDEV_LIDAR_ITER_STEP ((float) (UDEV_LIDAR_SEQ_STEP / (int) UDEV_LIDAR_POINTS))

/**
 * @brief Calculate the angle of a given seq number and iterator
 *
 * @param seq Packet Sequence Number
 * @param iterator Packet Iterator Number
 * @return
 */
static inline float udev_lidar_angle(int seq, int iterator) {
    return (float) ((float) seq * UDEV_LIDAR_SEQ_STEP +
                    (float) iterator * UDEV_LIDAR_ITER_STEP);
}

/**
 * @brief Calculate the seq and iterator for a given angle
 * * @param angle    The input angle in radians
 * @param out_seq  Pointer to store the calculated sequence number
 * @param out_iter Pointer to store the calculated iterator/index
 */
static inline void udev_lidar_index(float angle, int* out_seq, int* out_iter) {
    int global_index = (int) ((angle / UDEV_LIDAR_ITER_STEP) + 0.5f);
    *out_seq = global_index / UDEV_LIDAR_POINTS;
    *out_iter = global_index % UDEV_LIDAR_POINTS;
}

These functions allow for simplistic conversions between the LiDAR packet structure and conventional angle-based structures.

Implementation/Usage

The following section details the implementation and usage of the USB interface layer designed for the Scout Control Board.

Initialization

The USB interface can be initialized by calling the usbi_init(void) function. This function must only be called once by the Init routine present in main.

The initialization function serves two purposes. First, it sets up the USB hardware to contain the appropriate endpoints and descriptions for host interfacing. Second, it sets up and returns the FreeRTOS stream buffers used for USB-Task communication.

Hardware Initialization

Most of the hardware initialization is covered by Dmitry Filimonchuk’s libusb_stm32. The small portion remaining covers initializing the USB GPIO pins, enabling the correct interface within the library, and configuring the USB endpoint callbacks.

FreeRTOS Queue Initialization

The USB interface utilizes four FreeRTOS queues for interrupt-task communication. Two queues are used for each USB endpoint, and are initialized with the following code snippet:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

usbi.lidar_rx = xQueueCreateStatic(USBI_LIDAR_BUF_SIZE,
				sizeof(struct udev_pkt_lidar),
				(uint8_t *) usbi_lidar_rx_buf,
				&usbi_lidar_rx_sqh);

usbi.lidar_tx = xQueueCreateStatic(USBI_LIDAR_BUF_SIZE,
                                sizeof(struct udev_pkt_lidar),
                                (uint8_t *) usbi_lidar_tx_buf,
                                &usbi_lidar_tx_sqh);

usbi.ctrl_tx = xQueueCreateStatic(USBI_CTRL_BUF_SIZE,
                                sizeof(struct udev_pkt_ctrl_tx),
                                (uint8_t *) usbi_ctrl_tx_buf,
                                &usbi_ctrl_tx_sqh);

usbi.ctrl_rx = xQueueCreateStatic(USBI_CTRL_BUF_SIZE,
				sizeof(struct udev_pkt_ctrl_rx),
		                (uint8_t *) usbi_ctrl_rx_buf,
		                &usbi_ctrl_rx_sqh);

All queue data storage is statically initialized within the USB interface. Queue handles are returned by the usbi_init function through a struct usbi*, which points to internal USB memory. This structure and its members should not be made globally accessible. Rather, the Queue handles present within the structure should be locally distributed to the correct tasks during the initialization phase.

Internal Event Handling

This subsection explains how the USB layer internally handles USB events.

Control Event Handling

The following internal callback is used by the USB layer to handle RX and TX events that occur on the Control Endpoint. To ensure that the control data being exchanged with the high-level controller is the most recent available, the callback uses xQueueOverwrite and a queue size of 1. This queue size must not be changed, or xQueueOverwrite may fail.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

static void ctrl_rxtx(usbd_device *dev, uint8_t evt, uint8_t ep) {
    BaseType_t higher_woken = pdFALSE;
    if (evt == usbd_evt_eprx) {
        struct udev_pkt_ctrl_tx pkt_ctrl_tx = {0};
        (void) usbd_ep_read(
            dev, ep, (void *) &pkt_ctrl_tx, sizeof(struct udev_pkt_ctrl_tx));
        (void) xQueueOverwriteFromISR(usbi.ctrl_tx,
                                      &pkt_ctrl_tx,
                                      &higher_woken);

    } else {
        struct udev_pkt_ctrl_rx pkt_ctrl_rx = {0};
        if (xQueueReceiveFromISR(usbi.ctrl_rx, &pkt_ctrl_rx, &higher_woken) == pdTRUE) {
            (void)usbd_ep_write(dev,
                          ep,
                          (void *) &pkt_ctrl_rx,
                          sizeof(struct udev_pkt_ctrl_rx));
        } else {
            (void)usbd_ep_write(dev, ep, (void *) &pkt_ctrl_rx, 0);
        }
    }
    portYIELD_FROM_ISR(higher_woken);
}

LiDAR Event Handling

The following internal callback is used by the USB layer to handle RX and TX events that occur on the LiDAR Endpoint. Unlike the Control Endpoint, the LiDAR endpoint uses a medium size queue to handle large burst transfers of LiDAR data. Additionally, the LiDAR endpoint does not contain any retry logic, and will drop packets when the LiDAR packet queue is full.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

static void lidar_rxtx(usbd_device *dev, uint8_t evt, uint8_t ep) {
    BaseType_t higher_woken = pdFALSE;
    struct udev_pkt_lidar pkt_lidar = {0};
    if (evt == usbd_evt_eprx) {
        usbd_ep_read(
            dev, ep, (void *) &pkt_lidar, sizeof(struct udev_pkt_lidar));
        // Dont worry about dropping packets
        (void) xQueueSendToBackFromISR(usbi.lidar_rx,
                                       &pkt_lidar,
                                       &higher_woken);
    } else {
        if (xQueueReceiveFromISR(usbi.lidar_tx, &pkt_lidar, &higher_woken) ==
            pdTRUE) {
            usbd_ep_write(
                dev, ep, (void *) &pkt_lidar, sizeof(struct udev_pkt_lidar));
        } else {
            usbd_ep_write(dev, ep, (void *) 0, 0);
        }
    }
    portYIELD_FROM_ISR(higher_woken);
}

External Event Handling

This subsection explains how USB events should be handled externally by FreeRTOS tasks.

Control Event Handling

The control event queue is designed to always contain the most recent control commands. Thus, if the queue is empty, the embedded device has not received a new control command.

Therefore, RX events can be handled as follows:

1
2
3

if (xQueueReceive(pHndl->col_rx, &ct, 0) == pdTRUE) {
	// Process the Control Event
}

Additionally, TX events can be sent to the host with:

1
2
3

struct udev_pkt_ctrl_rx pkt_rx = (struct udev_pkt_ctrl_rx) {0};
// Load Data
(void) xQueueGenericSend(pHndl->usb.rx, &pkt_rx, 1, queueOVERWRITE);

When transmitting packets back to the host, ensure that the queueOVERWRITE flag is used. Otherwise, the task may hang waiting for the USB host to request a control packet. Using the above methods, neither the USB host or device are required to wait for data.

LiDAR Event Handling

LiDAR packets may be delivered out-of-order, therefore, any task that handles LiDAR events must be able to handle out-of-order packets. Note that the simulation LiDAR task does not reorder packets. For more information, consult the Simulation LiDAR Processing documentation.

Receiving simulation LiDAR data can be performed as follows:

1
2
3
4
5
6

static struct udev_pkt_lidar ldrpkt = {0};
if (xQueueReceive(pHndl->usb.rx, &ldrpkt, 100) != pdTRUE) {
    // Input Queue Empty
    printf("Lidar Input Queue Empty\n");
    continue;
}

Transmitting LiDAR packets to the host can be done as follows provided the LiDAR data is pre-arranged into the packet.

1
2

// Send the newly processed packet over USB for ROS LaserScan
(void) xQueueSendToBack(pHndl->usb.tx, &ldrpkt, 10);
Firmware, Scout
STM32 FreeRTOS Scout
This post is licensed under CC BY 4.0 by the author.