RT-Thread
/
rt-thread
mirror of https://github.com/RT-Thread/rt-thread
1
0
Fork 0
Code Issues Projects Releases Wiki Activity
rt-thread/components/drivers/usb/cherryusb/demo/cdc_acm_mavlink_template.c

337 lines
9.8 KiB
C
Raw Blame History

/*
* 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;
}
}
}
}
Reference in New Issue View Git Blame Copy Permalink