feat(demo): add mavlink with cdc acm

Signed-off-by: sakumisu <1203593632@qq.com>

demo/cdc_acm_mavlink_template.c

@@ -0,0 +1,337 @@
+/*
+ * Copyright (c) 2025, sakumisu
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#include "usbd_core.h"
+#include "usbd_cdc_acm.h"
+#include "chry_ringbuffer.h"
+#include <mavlink.h>
+
+/*!< endpoint address */
+#define CDC_IN_EP          0x81
+#define CDC_OUT_EP         0x02
+#define CDC_INT_EP         0x83
+
+#define USBD_VID           0xFFFF
+#define USBD_PID           0xFFFF
+#define USBD_MAX_POWER     100
+#define USBD_LANGID_STRING 1033
+
+/*!< config descriptor size */
+#define USB_CONFIG_SIZE    (9 + CDC_ACM_DESCRIPTOR_LEN)
+
+#ifdef CONFIG_USB_HS
+#define CDC_MAX_MPS 512
+#else
+#define CDC_MAX_MPS 64
+#endif
+
+#ifdef CONFIG_USBDEV_ADVANCE_DESC
+static const uint8_t device_descriptor[] = {
+    USB_DEVICE_DESCRIPTOR_INIT(USB_2_0, 0xEF, 0x02, 0x01, USBD_VID, USBD_PID, 0x0100, 0x01)
+};
+
+static const uint8_t config_descriptor[] = {
+    USB_CONFIG_DESCRIPTOR_INIT(USB_CONFIG_SIZE, 0x02, 0x01, USB_CONFIG_BUS_POWERED, USBD_MAX_POWER),
+    CDC_ACM_DESCRIPTOR_INIT(0x00, CDC_INT_EP, CDC_OUT_EP, CDC_IN_EP, CDC_MAX_MPS, 0x02)
+};
+
+static const uint8_t device_quality_descriptor[] = {
+    ///////////////////////////////////////
+    /// device qualifier descriptor
+    ///////////////////////////////////////
+    0x0a,
+    USB_DESCRIPTOR_TYPE_DEVICE_QUALIFIER,
+    0x00,
+    0x02,
+    0x00,
+    0x00,
+    0x00,
+    0x40,
+    0x00,
+    0x00,
+};
+
+static const char *string_descriptors[] = {
+    (const char[]){ 0x09, 0x04 }, /* Langid */
+    "CherryUSB",                  /* Manufacturer */
+    "CherryUSB CDC DEMO",         /* Product */
+    "2022123456",                 /* Serial Number */
+};
+
+static const uint8_t *device_descriptor_callback(uint8_t speed)
+{
+    return device_descriptor;
+}
+
+static const uint8_t *config_descriptor_callback(uint8_t speed)
+{
+    return config_descriptor;
+}
+
+static const uint8_t *device_quality_descriptor_callback(uint8_t speed)
+{
+    return device_quality_descriptor;
+}
+
+static const char *string_descriptor_callback(uint8_t speed, uint8_t index)
+{
+    if (index > 3) {
+        return NULL;
+    }
+    return string_descriptors[index];
+}
+
+const struct usb_descriptor cdc_descriptor = {
+    .device_descriptor_callback = device_descriptor_callback,
+    .config_descriptor_callback = config_descriptor_callback,
+    .device_quality_descriptor_callback = device_quality_descriptor_callback,
+    .string_descriptor_callback = string_descriptor_callback
+};
+#else
+/*!< global descriptor */
+static const uint8_t cdc_descriptor[] = {
+    USB_DEVICE_DESCRIPTOR_INIT(USB_2_0, 0xEF, 0x02, 0x01, USBD_VID, USBD_PID, 0x0100, 0x01),
+    USB_CONFIG_DESCRIPTOR_INIT(USB_CONFIG_SIZE, 0x02, 0x01, USB_CONFIG_BUS_POWERED, USBD_MAX_POWER),
+    CDC_ACM_DESCRIPTOR_INIT(0x00, CDC_INT_EP, CDC_OUT_EP, CDC_IN_EP, CDC_MAX_MPS, 0x02),
+    ///////////////////////////////////////
+    /// string0 descriptor
+    ///////////////////////////////////////
+    USB_LANGID_INIT(USBD_LANGID_STRING),
+    ///////////////////////////////////////
+    /// string1 descriptor
+    ///////////////////////////////////////
+    0x14,                       /* bLength */
+    USB_DESCRIPTOR_TYPE_STRING, /* bDescriptorType */
+    'C', 0x00,                  /* wcChar0 */
+    'h', 0x00,                  /* wcChar1 */
+    'e', 0x00,                  /* wcChar2 */
+    'r', 0x00,                  /* wcChar3 */
+    'r', 0x00,                  /* wcChar4 */
+    'y', 0x00,                  /* wcChar5 */
+    'U', 0x00,                  /* wcChar6 */
+    'S', 0x00,                  /* wcChar7 */
+    'B', 0x00,                  /* wcChar8 */
+    ///////////////////////////////////////
+    /// string2 descriptor
+    ///////////////////////////////////////
+    0x26,                       /* bLength */
+    USB_DESCRIPTOR_TYPE_STRING, /* bDescriptorType */
+    'C', 0x00,                  /* wcChar0 */
+    'h', 0x00,                  /* wcChar1 */
+    'e', 0x00,                  /* wcChar2 */
+    'r', 0x00,                  /* wcChar3 */
+    'r', 0x00,                  /* wcChar4 */
+    'y', 0x00,                  /* wcChar5 */
+    'U', 0x00,                  /* wcChar6 */
+    'S', 0x00,                  /* wcChar7 */
+    'B', 0x00,                  /* wcChar8 */
+    ' ', 0x00,                  /* wcChar9 */
+    'C', 0x00,                  /* wcChar10 */
+    'D', 0x00,                  /* wcChar11 */
+    'C', 0x00,                  /* wcChar12 */
+    ' ', 0x00,                  /* wcChar13 */
+    'D', 0x00,                  /* wcChar14 */
+    'E', 0x00,                  /* wcChar15 */
+    'M', 0x00,                  /* wcChar16 */
+    'O', 0x00,                  /* wcChar17 */
+    ///////////////////////////////////////
+    /// string3 descriptor
+    ///////////////////////////////////////
+    0x16,                       /* bLength */
+    USB_DESCRIPTOR_TYPE_STRING, /* bDescriptorType */
+    '2', 0x00,                  /* wcChar0 */
+    '0', 0x00,                  /* wcChar1 */
+    '2', 0x00,                  /* wcChar2 */
+    '2', 0x00,                  /* wcChar3 */
+    '1', 0x00,                  /* wcChar4 */
+    '2', 0x00,                  /* wcChar5 */
+    '3', 0x00,                  /* wcChar6 */
+    '4', 0x00,                  /* wcChar7 */
+    '5', 0x00,                  /* wcChar8 */
+    '6', 0x00,                  /* wcChar9 */
+#ifdef CONFIG_USB_HS
+    ///////////////////////////////////////
+    /// device qualifier descriptor
+    ///////////////////////////////////////
+    0x0a,
+    USB_DESCRIPTOR_TYPE_DEVICE_QUALIFIER,
+    0x00,
+    0x02,
+    0x00,
+    0x00,
+    0x00,
+    0x40,
+    0x00,
+    0x00,
+#endif
+    0x00
+};
+#endif
+
+chry_ringbuffer_t usb_rx_rb;
+uint8_t usb_rx_buffer[2048];
+
+USB_NOCACHE_RAM_SECTION USB_MEM_ALIGNX uint8_t temp_rx_buffer[512];
+USB_NOCACHE_RAM_SECTION USB_MEM_ALIGNX uint8_t usb_tx_buffer[MAVLINK_MAX_PACKET_LEN];
+
+volatile bool ep_tx_busy_flag = false;
+
+static void usbd_event_handler(uint8_t busid, uint8_t event)
+{
+    switch (event) {
+        case USBD_EVENT_RESET:
+            break;
+        case USBD_EVENT_CONNECTED:
+            break;
+        case USBD_EVENT_DISCONNECTED:
+            break;
+        case USBD_EVENT_RESUME:
+            break;
+        case USBD_EVENT_SUSPEND:
+            break;
+        case USBD_EVENT_CONFIGURED:
+            ep_tx_busy_flag = false;
+            /* setup first out ep read transfer */
+            usbd_ep_start_read(busid, CDC_OUT_EP, temp_rx_buffer, usbd_get_ep_mps(busid, CDC_OUT_EP));
+            break;
+        case USBD_EVENT_SET_REMOTE_WAKEUP:
+            break;
+        case USBD_EVENT_CLR_REMOTE_WAKEUP:
+            break;
+
+        default:
+            break;
+    }
+}
+
+void usbd_cdc_acm_bulk_out(uint8_t busid, uint8_t ep, uint32_t nbytes)
+{
+    USB_LOG_RAW("actual out len:%d\r\n", (unsigned int)nbytes);
+
+    chry_ringbuffer_write(&usb_rx_rb, temp_rx_buffer, nbytes);
+    usbd_ep_start_read(busid, CDC_OUT_EP, temp_rx_buffer, usbd_get_ep_mps(busid, CDC_OUT_EP));
+}
+
+void usbd_cdc_acm_bulk_in(uint8_t busid, uint8_t ep, uint32_t nbytes)
+{
+    USB_LOG_RAW("actual in len:%d\r\n", (unsigned int)nbytes);
+
+    if ((nbytes % usbd_get_ep_mps(busid, ep)) == 0 && nbytes) {
+        /* send zlp */
+        usbd_ep_start_write(busid, CDC_IN_EP, NULL, 0);
+    } else {
+        ep_tx_busy_flag = false;
+    }
+}
+
+/*!< endpoint call back */
+struct usbd_endpoint cdc_out_ep = {
+    .ep_addr = CDC_OUT_EP,
+    .ep_cb = usbd_cdc_acm_bulk_out
+};
+
+struct usbd_endpoint cdc_in_ep = {
+    .ep_addr = CDC_IN_EP,
+    .ep_cb = usbd_cdc_acm_bulk_in
+};
+
+static struct usbd_interface intf0;
+static struct usbd_interface intf1;
+
+void cdc_acm_mavlink_init(uint8_t busid, uintptr_t reg_base)
+{
+    chry_ringbuffer_init(&usb_rx_rb, usb_rx_buffer, sizeof(usb_rx_buffer));
+#ifdef CONFIG_USBDEV_ADVANCE_DESC
+    usbd_desc_register(busid, &cdc_descriptor);
+#else
+    usbd_desc_register(busid, cdc_descriptor);
+#endif
+    usbd_add_interface(busid, usbd_cdc_acm_init_intf(busid, &intf0));
+    usbd_add_interface(busid, usbd_cdc_acm_init_intf(busid, &intf1));
+    usbd_add_endpoint(busid, &cdc_out_ep);
+    usbd_add_endpoint(busid, &cdc_in_ep);
+    usbd_initialize(busid, reg_base, usbd_event_handler);
+}
+
+void cdc_acm_mavlink_write(uint8_t *data, uint32_t len)
+{
+    if (!usb_device_is_configured(0)) {
+        return;
+    }
+    ep_tx_busy_flag = true;
+    usbd_ep_start_write(0, CDC_IN_EP, data, len);
+    while (ep_tx_busy_flag) {
+    }
+}
+
+void send_heartbeat(void)
+{
+    mavlink_message_t message;
+
+    const uint8_t system_id = 42;
+    const uint8_t base_mode = 0;
+    const uint8_t custom_mode = 0;
+    mavlink_msg_heartbeat_pack_chan(
+        system_id,
+        MAV_COMP_ID_PERIPHERAL,
+        MAVLINK_COMM_0,
+        &message,
+        MAV_TYPE_GENERIC,
+        MAV_AUTOPILOT_GENERIC,
+        base_mode,
+        custom_mode,
+        MAV_STATE_STANDBY);
+
+    const int len = mavlink_msg_to_send_buffer(usb_tx_buffer, &message);
+    cdc_acm_mavlink_write(usb_tx_buffer, len);
+}
+
+void handle_heartbeat(const mavlink_message_t *message)
+{
+    mavlink_heartbeat_t heartbeat;
+    mavlink_msg_heartbeat_decode(message, &heartbeat);
+
+    USB_LOG_RAW("Got heartbeat from ");
+    switch (heartbeat.autopilot) {
+        case MAV_AUTOPILOT_GENERIC:
+            USB_LOG_RAW("generic");
+            break;
+        case MAV_AUTOPILOT_ARDUPILOTMEGA:
+            USB_LOG_RAW("ArduPilot");
+            break;
+        case MAV_AUTOPILOT_PX4:
+            USB_LOG_RAW("PX4");
+            break;
+        default:
+            USB_LOG_RAW("other");
+            break;
+    }
+    USB_LOG_RAW(" autopilot\n");
+
+    send_heartbeat();
+}
+
+void mavlink_polling(void)
+{
+    uint8_t ch;
+    bool ret;
+    mavlink_message_t message;
+    mavlink_status_t status;
+
+    ret = chry_ringbuffer_read_byte(&usb_rx_rb, &ch);
+    if (ret) {
+        if (mavlink_parse_char(MAVLINK_COMM_0, ch, &message, &status) == 1) {
+            USB_LOG_INFO(
+                "Received message %d from %d/%d\n",
+                message.msgid, message.sysid, message.compid);
+
+            switch (message.msgid) {
+                case MAVLINK_MSG_ID_HEARTBEAT:
+                    handle_heartbeat(&message);
+                    break;
+            }
+        }
+    }
+}