From 5a3b87e08c89af599012d6a8e62605561d7961bf Mon Sep 17 00:00:00 2001 From: sakumisu <1203593632@qq.com> Date: Thu, 26 Jun 2025 18:28:46 +0800 Subject: [PATCH] feat(demo): add mavlink with cdc acm Signed-off-by: sakumisu <1203593632@qq.com> --- demo/cdc_acm_mavlink_template.c | 337 ++++++++++++++++++++++++++++++++ 1 file changed, 337 insertions(+) create mode 100644 demo/cdc_acm_mavlink_template.c diff --git a/demo/cdc_acm_mavlink_template.c b/demo/cdc_acm_mavlink_template.c new file mode 100644 index 00000000..0a8bc3b6 --- /dev/null +++ b/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 + +/*!< 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; + } + } + } +} \ No newline at end of file