mirror of
https://github.com/odriverobotics/ODrive.git
synced 2026-08-21 14:52:16 +08:00
247 lines
9.8 KiB
C++
247 lines
9.8 KiB
C++
|
|
/* Includes ------------------------------------------------------------------*/
|
|
|
|
// TODO: remove this option
|
|
//#define ENABLE_LEGACY_PROTOCOL
|
|
|
|
#include "low_level.h"
|
|
#include "protocol.hpp"
|
|
#include "freertos_vars.h"
|
|
#include "commands.h"
|
|
|
|
#ifdef ENABLE_LEGACY_PROTOCOL
|
|
#include "legacy_commands.h"
|
|
#endif
|
|
|
|
#include <cmsis_os.h>
|
|
#include <memory>
|
|
#include <usbd_cdc_if.h>
|
|
#include <usb_device.h>
|
|
#include <usart.h>
|
|
#include <gpio.h>
|
|
|
|
#define UART_TX_BUFFER_SIZE 64
|
|
|
|
extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
|
|
|
|
/* Private defines -----------------------------------------------------------*/
|
|
/* Private macros ------------------------------------------------------------*/
|
|
/* Private typedef -----------------------------------------------------------*/
|
|
/* Global constant data ------------------------------------------------------*/
|
|
/* Global variables ----------------------------------------------------------*/
|
|
/* Private constant data -----------------------------------------------------*/
|
|
// TODO: make command to switch gpio_mode during run-time
|
|
static const GpioMode_t gpio_mode = GPIO_MODE_UART; //GPIO 1,2 is UART Tx,Rx
|
|
// static const GpioMode_t gpio_mode = GPIO_MODE_STEP_DIR; //GPIO 1,2 is M0 Step,Dir
|
|
|
|
/* Private variables ---------------------------------------------------------*/
|
|
|
|
/* Variables exposed to USB & UART via read/write commands */
|
|
// TODO: include range information in JSON description
|
|
|
|
std::function<void(void)> motors_0_set_pos_setpoint_func = std::bind(set_pos_setpoint, &motors[0],
|
|
std::ref(motors[0].set_pos_setpoint_args.pos_setpoint),
|
|
std::ref(motors[0].set_pos_setpoint_args.vel_feed_forward),
|
|
std::ref(motors[0].set_pos_setpoint_args.current_feed_forward)
|
|
);
|
|
std::function<void(void)> motors_0_set_vel_setpoint_func = std::bind(set_vel_setpoint, &motors[0],
|
|
std::ref(motors[0].set_vel_setpoint_args.vel_setpoint),
|
|
std::ref(motors[0].set_vel_setpoint_args.current_feed_forward)
|
|
);
|
|
std::function<void(void)> motors_0_set_current_setpoint_func = std::bind(set_current_setpoint, &motors[0],
|
|
std::ref(motors[0].set_current_setpoint_args.current_setpoint)
|
|
);
|
|
|
|
// clang-format off
|
|
// TODO: Autogenerate this table. It will come up again very soon in the Arduino library.
|
|
const Endpoint endpoints[] = {
|
|
Endpoint::make_property("vbus_voltage", const_cast<const float*>(&vbus_voltage)),
|
|
Endpoint::make_property("elec_rad_per_enc", const_cast<const float*>(&elec_rad_per_enc)),
|
|
Endpoint::make_object("motor0"),
|
|
Endpoint::make_property("pos_setpoint", &motors[0].pos_setpoint),
|
|
Endpoint::make_property("pos_gain", &motors[0].pos_gain),
|
|
Endpoint::make_property("vel_setpoint", &motors[0].vel_setpoint),
|
|
Endpoint::make_function("set_pos_setpoint", &motors_0_set_pos_setpoint_func),
|
|
Endpoint::make_property("pos_setpoint", &motors[0].set_pos_setpoint_args.pos_setpoint),
|
|
Endpoint::make_property("vel_feed_forward", &motors[0].set_pos_setpoint_args.vel_feed_forward),
|
|
Endpoint::make_property("current_feed_forward", &motors[0].set_pos_setpoint_args.current_feed_forward),
|
|
Endpoint::close_tree(),
|
|
Endpoint::make_function("set_vel_setpoint", &motors_0_set_vel_setpoint_func),
|
|
Endpoint::make_property("vel_setpoint", &motors[0].set_vel_setpoint_args.vel_setpoint),
|
|
Endpoint::make_property("current_feed_forward", &motors[0].set_vel_setpoint_args.current_feed_forward),
|
|
Endpoint::close_tree(),
|
|
Endpoint::make_function("set_current_setpoint", &motors_0_set_current_setpoint_func),
|
|
Endpoint::make_property("current_setpoint", &motors[0].set_current_setpoint_args.current_setpoint),
|
|
Endpoint::close_tree(),
|
|
Endpoint::close_tree() // motor0
|
|
};
|
|
// clang-format on
|
|
|
|
constexpr size_t NUM_ENDPOINTS = sizeof(endpoints) / sizeof(endpoints[0]);
|
|
|
|
|
|
|
|
//#define STREAM_ON_USB
|
|
#ifdef STREAM_ON_USB
|
|
// We could theoretically implement the USB channel as a packet based channel,
|
|
// but on some platforms there's no direct USB endpoint access, so the device
|
|
// should better just behave like a serial device.
|
|
|
|
class USBSender : public StreamSink {
|
|
public:
|
|
int process_bytes(const uint8_t* buffer, size_t length) {
|
|
// Loop to ensure all bytes get sent
|
|
// TODO: add timeout
|
|
while (length) {
|
|
size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE;
|
|
while (CDC_Transmit_FS(
|
|
const_cast<uint8_t*>(buffer) /* casting this const away is safe because...
|
|
well... it's not actually. Stupid STM. */, chunk) != USBD_OK)
|
|
osDelay(1);
|
|
buffer += chunk;
|
|
length -= chunk;printf("got packet of length %d: \r\n", length); osDelay(5); hexdump(buffer, length);
|
|
}
|
|
//printf("USB TX done\r\n"); osDelay(5);
|
|
return 0;
|
|
}
|
|
|
|
size_t get_free_space() { return SIZE_MAX; }
|
|
} usb_sender;
|
|
|
|
PacketToStreamConverter usb_packet_sender(usb_sender);
|
|
BidirectionalPacketBasedChannel usb_connection(endpoints, NUM_ENDPOINTS, usb_packet_sender);
|
|
StreamToPacketConverter usb_stream_sink(usb_connection);
|
|
|
|
#else
|
|
|
|
class USBSender : public PacketSink {
|
|
public:
|
|
int process_packet(const uint8_t* buffer, size_t length) {
|
|
// cannot send partial packets
|
|
if (length > USB_TX_DATA_SIZE)
|
|
return -1;
|
|
while (CDC_Transmit_FS(
|
|
const_cast<uint8_t*>(buffer) /* casting this const away is safe because...
|
|
well... it's not actually. Stupid STM. */, length) != USBD_OK)
|
|
osDelay(1);
|
|
//printf("USB TX done\r\n"); osDelay(5);
|
|
return 0;
|
|
}
|
|
} usb_sender;
|
|
|
|
BidirectionalPacketBasedChannel usb_connection(endpoints, NUM_ENDPOINTS, usb_sender);
|
|
#endif
|
|
|
|
class UART4Sender : public StreamSink {
|
|
public:
|
|
int process_bytes(const uint8_t* buffer, size_t length) {
|
|
//Check length
|
|
if (length > UART_TX_BUFFER_SIZE)
|
|
return -1;
|
|
// Check if transfer is already ongoing
|
|
if (huart4.gState != HAL_UART_STATE_READY)
|
|
return -1;
|
|
// memcpy data into uart_tx_buf
|
|
memcpy(tx_buf_, buffer, length);
|
|
// Start DMA background trasnfer
|
|
HAL_UART_Transmit_DMA(&huart4, tx_buf_, length);
|
|
return 0;
|
|
}
|
|
|
|
size_t get_free_space() { return SIZE_MAX; }
|
|
private:
|
|
uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
|
|
} uart4_sender;
|
|
|
|
PacketToStreamConverter uart4_packet_sender(uart4_sender);
|
|
BidirectionalPacketBasedChannel uart4_connection(endpoints, NUM_ENDPOINTS, uart4_packet_sender);
|
|
StreamToPacketConverter UART4_stream_sink(uart4_connection);
|
|
|
|
/* Private function prototypes -----------------------------------------------*/
|
|
/* Function implementations --------------------------------------------------*/
|
|
|
|
void init_communication(void) {
|
|
switch (gpio_mode) {
|
|
case GPIO_MODE_UART: {
|
|
SetGPIO12toUART();
|
|
} break;
|
|
case GPIO_MODE_STEP_DIR: {
|
|
SetGPIO12toStepDir();
|
|
}
|
|
}
|
|
}
|
|
|
|
// Thread to handle deffered processing of USB interrupt, and
|
|
// read commands out of the UART DMA circular buffer
|
|
void communication_task(void const * argument) {
|
|
(void) argument;
|
|
|
|
//DMA open loop continous circular buffer
|
|
//1ms delay periodic, chase DMA ptr around
|
|
|
|
#define UART_RX_BUFFER_SIZE 64
|
|
static uint8_t dma_circ_buffer[UART_RX_BUFFER_SIZE];
|
|
|
|
// DMA is set up to recieve in a circular buffer forever.
|
|
// We dont use interrupts to fetch the data, instead we periodically read
|
|
// data out of the circular buffer into a parse buffer, controlled by a state machine
|
|
HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer));
|
|
|
|
uint32_t last_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
|
|
|
|
// Re-run state-machine forever
|
|
for (;;) {
|
|
// Check for UART errors and restart recieve DMA transfer if required
|
|
if (huart4.ErrorCode != HAL_UART_ERROR_NONE) {
|
|
HAL_UART_AbortReceive(&huart4);
|
|
HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer));
|
|
}
|
|
// Fetch the circular buffer "write pointer", where it would write next
|
|
uint32_t rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
|
|
// During sleeping, we may have fallen several characters behind, so we keep
|
|
// going until we are caught up, before we sleep again
|
|
while (rcv_idx != last_rcv_idx) {
|
|
// Fetch the next char, rotate read ptr
|
|
uint8_t c = dma_circ_buffer[last_rcv_idx];
|
|
if (++last_rcv_idx == UART_RX_BUFFER_SIZE)
|
|
last_rcv_idx = 0;
|
|
UART4_stream_sink.process_bytes(&c, 1);
|
|
}
|
|
|
|
// When we reach here, we are out of immediate characters to fetch out of UART buffer
|
|
// Now we check if there is any USB processing to do: we wait for up to 1 ms,
|
|
// before going back to checking UART again.
|
|
int USB_check_timeout = 1;
|
|
int32_t status = osSemaphoreWait(sem_usb_irq, USB_check_timeout);
|
|
if (status == osOK) {
|
|
// We have a new incoming USB transmission: handle it
|
|
HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS);
|
|
// Let the irq (OTG_FS_IRQHandler) fire again.
|
|
HAL_NVIC_EnableIRQ(OTG_FS_IRQn);
|
|
}
|
|
}
|
|
|
|
// If we get here, then this task is done
|
|
vTaskDelete(osThreadGetId());
|
|
}
|
|
|
|
void USB_receive_packet(const uint8_t *buffer, size_t length) {
|
|
//printf("[USB] got %d bytes, first is %c\r\n", length, buffer[0]); osDelay(5);
|
|
#ifdef ENABLE_LEGACY_PROTOCOL
|
|
const uint8_t* legacy_commands = (const uint8_t*)"pvcgsmo";
|
|
while (*legacy_commands && length) {
|
|
if (buffer[0] == *(legacy_commands++)) {
|
|
//printf("[USB] process legacy command %c\r\n", buffer[0]); osDelay(5);
|
|
legacy_parse_cmd(buffer, length);
|
|
length = 0;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
#ifdef STREAM_ON_USB
|
|
usb_stream_sink.process_bytes(buffer, length);
|
|
#else
|
|
usb_connection.process_packet(buffer, length);
|
|
#endif
|
|
}
|