Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-08-21 06:23:07 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity
Files
e71a38751fbc17fccb5e0263511a5809065272b0
ODrive/Firmware/MotorControl/commands.cpp
T
Samuel Sadok 95994309ae fix bug where an invalid motor config could be loaded 
Before this commit, if the NVM is deemed valid and but the subsequent config load fails (e.g. because of a checksum mismatch), the motor config could be left in an undefined state.

Also a variable user_config_loaded is added to indicate whether the device is using user-saved config or factory defaults.
2018-04-07 12:49:29 -07:00

553 lines
28 KiB
C++
Raw Blame History

/* Includes ------------------------------------------------------------------*/
// TODO: remove this option
// and once the legacy protocol is phased out, remove the seq-no hack in protocol.py
// todo: make clean switches for protocol
#define ENABLE_LEGACY_PROTOCOL
#include "commands.h"
#include "low_level.h"
#include "axis.h"
#include "protocol.hpp"
#include "freertos_vars.h"
#include "utils.h"
#include "config.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
/* Private defines -----------------------------------------------------------*/
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
extern USBD_HandleTypeDef hUsbDeviceFS;
uint64_t serial_number;
/* Private constant data -----------------------------------------------------*/
// TODO: make command to switch gpio_mode during run-time
typedef enum {
GPIO_MODE_NONE,
GPIO_MODE_UART,
GPIO_MODE_STEP_DIR,
} GpioMode_t;
#if defined(USE_GPIO_MODE_STEP_DIR)
static const GpioMode_t gpio_mode = GPIO_MODE_STEP_DIR; //GPIO 1,2 is M0 Step,Dir
#elif !defined(UART_PROTOCOL_NONE)
static const GpioMode_t gpio_mode = GPIO_MODE_UART; //GPIO 1,2 is UART Tx,Rx
#else
static const GpioMode_t gpio_mode = GPIO_MODE_NONE; //GPIO 1,2 is not configured
#endif
/* Private variables ---------------------------------------------------------*/
static uint8_t* usb_buf;
static uint32_t usb_len;
// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable
static thread_local uint32_t deadline_ms = 0;
/* Variables exposed to USB & UART via read/write commands */
// TODO: include range information in JSON description
// TODO: Autogenerate these functions
void motors_0_set_pos_setpoint_func(void) {
set_pos_setpoint(&motors[0],
motors[0].set_pos_setpoint_args.pos_setpoint,
motors[0].set_pos_setpoint_args.vel_feed_forward,
motors[0].set_pos_setpoint_args.current_feed_forward);
}
void motors_0_set_vel_setpoint_func(void) {
set_vel_setpoint(&motors[0],
motors[0].set_vel_setpoint_args.vel_setpoint,
motors[0].set_vel_setpoint_args.current_feed_forward);
}
void motors_0_set_current_setpoint_func(void) {
set_current_setpoint(&motors[0],
motors[0].set_current_setpoint_args.current_setpoint);
}
void motors_1_set_pos_setpoint_func(void) {
set_pos_setpoint(&motors[1],
motors[1].set_pos_setpoint_args.pos_setpoint,
motors[1].set_pos_setpoint_args.vel_feed_forward,
motors[1].set_pos_setpoint_args.current_feed_forward);
}
void motors_1_set_vel_setpoint_func(void) {
set_vel_setpoint(&motors[1],
motors[1].set_vel_setpoint_args.vel_setpoint,
motors[1].set_vel_setpoint_args.current_feed_forward);
}
void motors_1_set_current_setpoint_func(void) {
set_current_setpoint(&motors[1],
motors[1].set_current_setpoint_args.current_setpoint);
}
void motors_run_anticogging_calibration_func() {
for (uint8_t i = 0; i < num_motors; i++) {
// Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating
if (motors[i].anticogging.cogging_map != NULL && motors[i].error == ERROR_NO_ERROR) {
motors[i].anticogging.calib_anticogging = true;
}
}
}
void enter_dfu_mode() {
*((unsigned long *)0x2001C000) = 0xDEADBEEF;
NVIC_SystemReset();
}
// This table specifies which fields and functions are exposed on the USB and UART ports.
// TODO: Autogenerate this table. It will come up again very soon in the Arduino library.
// clang-format off
const Endpoint endpoints[] = {
Endpoint::make_property("vbus_voltage", const_cast<const float*>(&vbus_voltage)),
Endpoint::make_property("serial_number", const_cast<const uint64_t *>(&serial_number)),
Endpoint::make_function("run_anticogging_calibration", &motors_run_anticogging_calibration_func),
// No parameters, but still requires a close_tree()
Endpoint::close_tree(),
Endpoint::make_object("config"),
Endpoint::make_property("brake_resistance", &brake_resistance),
Endpoint::close_tree(),
Endpoint::make_property("user_config_loaded", const_cast<const bool *>(&user_config_loaded)),
Endpoint::make_object("axis0"),
Endpoint::make_object("config"),
Endpoint::make_property("enable_control_at_start", &axis_configs[0].enable_control_at_start),
Endpoint::make_property("do_calibration_at_start", &axis_configs[0].do_calibration_at_start),
Endpoint::close_tree(),
Endpoint::close_tree(),
Endpoint::make_object("motor0"),
Endpoint::make_object("config"),
Endpoint::make_property("control_mode", reinterpret_cast<int32_t*>(&motors[0].control_mode)),
Endpoint::make_property("counts_per_step", &motors[0].counts_per_step),
Endpoint::make_property("pole_pairs", &motors[0].pole_pairs),
Endpoint::make_property("pos_gain", &motors[0].pos_gain),
Endpoint::make_property("vel_gain", &motors[0].vel_gain),
Endpoint::make_property("vel_integrator_gain", &motors[0].vel_integrator_gain),
Endpoint::make_property("vel_limit", &motors[0].vel_limit),
Endpoint::make_property("calibration_current", &motors[0].calibration_current),
Endpoint::make_property("resistance_calib_max_voltage", &motors[0].resistance_calib_max_voltage),
Endpoint::make_property("phase_inductance", &motors[0].phase_inductance),
Endpoint::make_property("phase_resistance", &motors[0].phase_resistance),
Endpoint::make_property("motor_type", reinterpret_cast<int32_t*>(&motors[0].motor_type)),
Endpoint::make_property("rotor_mode", reinterpret_cast<int32_t*>(&motors[0].rotor_mode)),
Endpoint::close_tree(),
Endpoint::make_property("error", reinterpret_cast<int32_t*>(&motors[0].error)),
Endpoint::make_property("pos_setpoint", &motors[0].pos_setpoint),
Endpoint::make_property("vel_setpoint", &motors[0].vel_setpoint),
Endpoint::make_property("vel_integrator_current", &motors[0].vel_integrator_current),
Endpoint::make_property("current_setpoint", &motors[0].current_setpoint),
Endpoint::make_property("current_meas_phB", const_cast<const float*>(&motors[0].current_meas.phB)),
Endpoint::make_property("current_meas_phC", const_cast<const float*>(&motors[0].current_meas.phC)),
Endpoint::make_property("DC_calib.phB", &motors[0].DC_calib.phB),
Endpoint::make_property("DC_calib.phC", &motors[0].DC_calib.phC),
Endpoint::make_property("shunt_conductance", &motors[0].shunt_conductance),
Endpoint::make_property("phase_current_rev_gain", &motors[0].phase_current_rev_gain),
Endpoint::make_property("thread_ready", &motors[0].thread_ready),
Endpoint::make_property("control_deadline", &motors[0].control_deadline),
Endpoint::make_property("last_cpu_time", &motors[0].last_cpu_time),
Endpoint::make_property("loop_counter", &motors[0].loop_counter),
Endpoint::make_object("current_control"),
Endpoint::make_object("config"),
Endpoint::make_property("current_lim", &motors[0].current_control.current_lim),
Endpoint::close_tree(),
Endpoint::make_property("p_gain", &motors[0].current_control.p_gain),
Endpoint::make_property("i_gain", &motors[0].current_control.i_gain),
Endpoint::make_property("v_current_control_integral_d", &motors[0].current_control.v_current_control_integral_d),
Endpoint::make_property("v_current_control_integral_q", &motors[0].current_control.v_current_control_integral_q),
Endpoint::make_property("Iq_setpoint", &motors[0].current_control.Iq_setpoint),
Endpoint::make_property("Iq_measured", &motors[0].current_control.Iq_measured),
Endpoint::make_property("Ibus", const_cast<const float*>(&motors[0].current_control.Ibus)),
Endpoint::close_tree(),
Endpoint::make_object("gate_driver"),
Endpoint::make_property("drv_fault", reinterpret_cast<int32_t*>(&motors[0].drv_fault)),
Endpoint::make_property("status_reg_1", (&motors[0].gate_driver_regs.Stat_Reg_1_Value)),
Endpoint::make_property("status_reg_2", (&motors[0].gate_driver_regs.Stat_Reg_2_Value)),
Endpoint::make_property("ctrl_reg_1", (&motors[0].gate_driver_regs.Ctrl_Reg_1_Value)),
Endpoint::make_property("ctrl_reg_2", (&motors[0].gate_driver_regs.Ctrl_Reg_2_Value)),
Endpoint::close_tree(),
Endpoint::make_object("encoder"),
Endpoint::make_object("config"),
Endpoint::make_property("use_index", &motors[0].encoder.use_index),
Endpoint::make_property("manually_calibrated", &motors[0].encoder.manually_calibrated),
Endpoint::make_property("idx_search_speed", &motors[0].encoder.idx_search_speed),
Endpoint::make_property("cpr", &motors[0].encoder.encoder_cpr),
Endpoint::make_property("offset", &motors[0].encoder.encoder_offset),
Endpoint::make_property("motor_dir", &motors[0].encoder.motor_dir),
Endpoint::close_tree(),
Endpoint::make_property("phase", const_cast<const float*>(&motors[0].encoder.phase)),
Endpoint::make_property("pll_pos", &motors[0].encoder.pll_pos),
Endpoint::make_property("pll_vel", &motors[0].encoder.pll_vel),
Endpoint::make_property("pll_kp", &motors[0].encoder.pll_kp),
Endpoint::make_property("pll_ki", &motors[0].encoder.pll_ki),
Endpoint::make_property("encoder_offset", &motors[0].encoder.encoder_offset),
Endpoint::make_property("encoder_state", &motors[0].encoder.encoder_state),
Endpoint::make_property("motor_dir", &motors[0].encoder.motor_dir),
Endpoint::close_tree(),
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::make_object("timing_log"),
Endpoint::make_property("TIMING_LOG_GENERAL", &motors[0].timing_log[TIMING_LOG_GENERAL]),
Endpoint::make_property("TIMING_LOG_ADC_CB_M0_I", &motors[0].timing_log[TIMING_LOG_ADC_CB_M0_I]),
Endpoint::make_property("TIMING_LOG_ADC_CB_M0_DC", &motors[0].timing_log[TIMING_LOG_ADC_CB_M0_DC]),
Endpoint::make_property("TIMING_LOG_ADC_CB_M1_I", &motors[0].timing_log[TIMING_LOG_ADC_CB_M1_I]),
Endpoint::make_property("TIMING_LOG_ADC_CB_M1_DC", &motors[0].timing_log[TIMING_LOG_ADC_CB_M1_DC]),
Endpoint::make_property("TIMING_LOG_MEAS_R", &motors[0].timing_log[TIMING_LOG_MEAS_R]),
Endpoint::make_property("TIMING_LOG_MEAS_L", &motors[0].timing_log[TIMING_LOG_MEAS_L]),
Endpoint::make_property("TIMING_LOG_ENC_CALIB", &motors[0].timing_log[TIMING_LOG_ENC_CALIB]),
Endpoint::make_property("TIMING_LOG_IDX_SEARCH", &motors[0].timing_log[TIMING_LOG_IDX_SEARCH]),
Endpoint::make_property("TIMING_LOG_FOC_VOLTAGE", &motors[0].timing_log[TIMING_LOG_FOC_VOLTAGE]),
Endpoint::make_property("TIMING_LOG_FOC_CURRENT", &motors[0].timing_log[TIMING_LOG_FOC_CURRENT]),
Endpoint::close_tree(),
Endpoint::close_tree(), // motor0
Endpoint::make_object("axis1"),
Endpoint::make_object("config"),
Endpoint::make_property("enable_control_at_start", &axis_configs[1].enable_control_at_start),
Endpoint::make_property("do_calibration_at_start", &axis_configs[1].do_calibration_at_start),
Endpoint::close_tree(),
Endpoint::close_tree(),
Endpoint::make_object("motor1"),
Endpoint::make_object("config"),
Endpoint::make_property("control_mode", reinterpret_cast<int32_t*>(&motors[1].control_mode)),
Endpoint::make_property("counts_per_step", &motors[1].counts_per_step),
Endpoint::make_property("pole_pairs", &motors[1].pole_pairs),
Endpoint::make_property("pos_gain", &motors[1].pos_gain),
Endpoint::make_property("vel_gain", &motors[1].vel_gain),
Endpoint::make_property("vel_integrator_gain", &motors[1].vel_integrator_gain),
Endpoint::make_property("vel_limit", &motors[1].vel_limit),
Endpoint::make_property("calibration_current", &motors[1].calibration_current),
Endpoint::make_property("resistance_calib_max_voltage", &motors[1].resistance_calib_max_voltage),
Endpoint::make_property("phase_inductance", &motors[1].phase_inductance),
Endpoint::make_property("phase_resistance", &motors[1].phase_resistance),
Endpoint::make_property("motor_type", reinterpret_cast<int32_t*>(&motors[1].motor_type)),
Endpoint::make_property("rotor_mode", reinterpret_cast<int32_t*>(&motors[1].rotor_mode)),
Endpoint::close_tree(),
Endpoint::make_property("error", reinterpret_cast<int32_t*>(&motors[1].error)),
Endpoint::make_property("pos_setpoint", &motors[1].pos_setpoint),
Endpoint::make_property("vel_setpoint", &motors[1].vel_setpoint),
Endpoint::make_property("vel_integrator_current", &motors[1].vel_integrator_current),
Endpoint::make_property("current_setpoint", &motors[1].current_setpoint),
Endpoint::make_property("current_meas_phB", const_cast<const float*>(&motors[1].current_meas.phB)),
Endpoint::make_property("current_meas_phC", const_cast<const float*>(&motors[1].current_meas.phC)),
Endpoint::make_property("DC_calib.phB", &motors[1].DC_calib.phB),
Endpoint::make_property("DC_calib.phC", &motors[1].DC_calib.phC),
Endpoint::make_property("shunt_conductance", &motors[1].shunt_conductance),
Endpoint::make_property("phase_current_rev_gain", &motors[1].phase_current_rev_gain),
Endpoint::make_property("thread_ready", &motors[1].thread_ready),
Endpoint::make_property("control_deadline", &motors[1].control_deadline),
Endpoint::make_property("last_cpu_time", &motors[1].last_cpu_time),
Endpoint::make_property("loop_counter", &motors[1].loop_counter),
Endpoint::make_object("current_control"),
Endpoint::make_object("config"),
Endpoint::make_property("current_lim", &motors[1].current_control.current_lim),
Endpoint::close_tree(),
Endpoint::make_property("p_gain", &motors[1].current_control.p_gain),
Endpoint::make_property("i_gain", &motors[1].current_control.i_gain),
Endpoint::make_property("v_current_control_integral_d", &motors[1].current_control.v_current_control_integral_d),
Endpoint::make_property("v_current_control_integral_q", &motors[1].current_control.v_current_control_integral_q),
Endpoint::make_property("Iq_setpoint", &motors[1].current_control.Iq_setpoint),
Endpoint::make_property("Iq_measured", &motors[1].current_control.Iq_measured),
Endpoint::make_property("Ibus", const_cast<const float*>(&motors[1].current_control.Ibus)),
Endpoint::close_tree(),
Endpoint::make_object("gate_driver"),
Endpoint::make_property("drv_fault", reinterpret_cast<int32_t*>(&motors[1].drv_fault)),
Endpoint::make_property("status_reg_1", (&motors[1].gate_driver_regs.Stat_Reg_1_Value)),
Endpoint::make_property("status_reg_2", (&motors[1].gate_driver_regs.Stat_Reg_2_Value)),
Endpoint::make_property("ctrl_reg_1", (&motors[1].gate_driver_regs.Ctrl_Reg_1_Value)),
Endpoint::make_property("ctrl_reg_2", (&motors[1].gate_driver_regs.Ctrl_Reg_2_Value)),
Endpoint::close_tree(),
Endpoint::make_object("encoder"),
Endpoint::make_object("config"),
Endpoint::make_property("use_index", &motors[1].encoder.use_index),
Endpoint::make_property("manually_calibrated", &motors[1].encoder.manually_calibrated),
Endpoint::make_property("idx_search_speed", &motors[1].encoder.idx_search_speed),
Endpoint::make_property("cpr", &motors[1].encoder.encoder_cpr),
Endpoint::make_property("offset", &motors[1].encoder.encoder_offset),
Endpoint::make_property("motor_dir", &motors[1].encoder.motor_dir),
Endpoint::close_tree(),
Endpoint::make_property("phase", const_cast<const float*>(&motors[1].encoder.phase)),
Endpoint::make_property("pll_pos", &motors[1].encoder.pll_pos),
Endpoint::make_property("pll_vel", &motors[1].encoder.pll_vel),
Endpoint::make_property("pll_kp", &motors[1].encoder.pll_kp),
Endpoint::make_property("pll_ki", &motors[1].encoder.pll_ki),
Endpoint::make_property("encoder_offset", &motors[1].encoder.encoder_offset),
Endpoint::make_property("encoder_state", &motors[1].encoder.encoder_state),
Endpoint::make_property("motor_dir", &motors[1].encoder.motor_dir),
Endpoint::close_tree(),
Endpoint::make_function("set_pos_setpoint", &motors_1_set_pos_setpoint_func),
Endpoint::make_property("pos_setpoint", &motors[1].set_pos_setpoint_args.pos_setpoint),
Endpoint::make_property("vel_feed_forward", &motors[1].set_pos_setpoint_args.vel_feed_forward),
Endpoint::make_property("current_feed_forward", &motors[1].set_pos_setpoint_args.current_feed_forward),
Endpoint::close_tree(),
Endpoint::make_function("set_vel_setpoint", &motors_1_set_vel_setpoint_func),
Endpoint::make_property("vel_setpoint", &motors[1].set_vel_setpoint_args.vel_setpoint),
Endpoint::make_property("current_feed_forward", &motors[1].set_vel_setpoint_args.current_feed_forward),
Endpoint::close_tree(),
Endpoint::make_function("set_current_setpoint", &motors_1_set_current_setpoint_func),
Endpoint::make_property("current_setpoint", &motors[1].set_current_setpoint_args.current_setpoint),
Endpoint::close_tree(),
Endpoint::make_object("timing_log"),
Endpoint::make_property("TIMING_LOG_GENERAL", &motors[1].timing_log[TIMING_LOG_GENERAL]),
Endpoint::make_property("TIMING_LOG_ADC_CB_M0_I", &motors[1].timing_log[TIMING_LOG_ADC_CB_M0_I]),
Endpoint::make_property("TIMING_LOG_ADC_CB_M0_DC", &motors[1].timing_log[TIMING_LOG_ADC_CB_M0_DC]),
Endpoint::make_property("TIMING_LOG_ADC_CB_M1_I", &motors[1].timing_log[TIMING_LOG_ADC_CB_M1_I]),
Endpoint::make_property("TIMING_LOG_ADC_CB_M1_DC", &motors[1].timing_log[TIMING_LOG_ADC_CB_M1_DC]),
Endpoint::make_property("TIMING_LOG_MEAS_R", &motors[1].timing_log[TIMING_LOG_MEAS_R]),
Endpoint::make_property("TIMING_LOG_MEAS_L", &motors[1].timing_log[TIMING_LOG_MEAS_L]),
Endpoint::make_property("TIMING_LOG_ENC_CALIB", &motors[1].timing_log[TIMING_LOG_ENC_CALIB]),
Endpoint::make_property("TIMING_LOG_IDX_SEARCH", &motors[1].timing_log[TIMING_LOG_IDX_SEARCH]),
Endpoint::make_property("TIMING_LOG_FOC_VOLTAGE", &motors[1].timing_log[TIMING_LOG_FOC_VOLTAGE]),
Endpoint::make_property("TIMING_LOG_FOC_CURRENT", &motors[1].timing_log[TIMING_LOG_FOC_CURRENT]),
Endpoint::close_tree(),
Endpoint::close_tree(), // motor1
Endpoint::make_function("save_configuration", &save_configuration),
// no arguments
Endpoint::close_tree(),
Endpoint::make_function("erase_configuration", &erase_configuration),
// no arguments
Endpoint::close_tree(),
Endpoint::make_function("reboot", &NVIC_SystemReset),
// no arguments
Endpoint::close_tree(),
Endpoint::make_function("enter_dfu_mode", &enter_dfu_mode),
// no arguments
Endpoint::close_tree(),
};
// clang-format on
constexpr size_t NUM_ENDPOINTS = sizeof(endpoints) / sizeof(endpoints[0]);
#if defined(USB_PROTOCOL_NATIVE)
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;
// wait for USB interface to become ready
if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK)
return -1;
// transmit packet
uint8_t status = CDC_Transmit_FS(
const_cast<uint8_t*>(buffer) /* casting this const away is safe because...
well... it's not actually. Stupid STM. */, length);
return (status == USBD_OK) ? 0 : -1;
}
} usb_sender;
BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_sender);
#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED)
class USBSender : public StreamSink {
public:
int process_bytes(const uint8_t* buffer, size_t length) {
// Loop to ensure all bytes get sent
while (length) {
size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE;
// wait for USB interface to become ready
if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK)
return -1;
// transmit chunk
if (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)
return -1;
buffer += chunk;
length -= chunk;
}
return 0;
}
size_t get_free_space() { return SIZE_MAX; }
} usb_sender;
PacketToStreamConverter usb_packet_sender(usb_sender);
BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_packet_sender);
StreamToPacketConverter usb_stream_sink(usb_channel);
#endif
#if defined(UART_PROTOCOL_NATIVE)
class UART4Sender : public StreamSink {
public:
int process_bytes(const uint8_t* buffer, size_t length) {
// Loop to ensure all bytes get sent
while (length) {
size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
// wait for USB interface to become ready
// TODO: implement ring buffer to get a more continuous stream of data
if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
return -1;
// transmit chunk
memcpy(tx_buf_, buffer, chunk);
if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK)
return -1;
buffer += chunk;
length -= chunk;
}
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_channel(endpoints, NUM_ENDPOINTS, uart4_packet_sender);
StreamToPacketConverter UART4_stream_sink(uart4_channel);
#endif
/* Private function prototypes -----------------------------------------------*/
/* Function implementations --------------------------------------------------*/
void init_communication(void) {
switch (gpio_mode) {
case GPIO_MODE_NONE:
break; //do nothing
case GPIO_MODE_UART: {
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
SetGPIO12toUART();
#endif
} break;
case GPIO_MODE_STEP_DIR: {
SetGPIO12toStepDir();
} break;
default:
//TODO: report error unexpected mode
break;
}
}
// 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;
#if !defined(UART_PROTOCOL_NONE)
//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;
#endif
// Re-run state-machine forever
for (;;) {
#if !defined(UART_PROTOCOL_NONE)
// 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 new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
#if defined(UART_PROTOCOL_NATIVE)
// Process bytes in one or two chunks (two in case there was a wrap)
if (new_rcv_idx < last_rcv_idx) {
UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx,
UART_RX_BUFFER_SIZE - last_rcv_idx);
last_rcv_idx = 0;
}
if (new_rcv_idx > last_rcv_idx) {
UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx,
new_rcv_idx - last_rcv_idx);
last_rcv_idx = new_rcv_idx;
}
#elif defined(UART_PROTOCOL_LEGACY)
// Process bytes in one or two chunks (two in case there was a wrap)
if (new_rcv_idx < last_rcv_idx) {
legacy_parse_stream(dma_circ_buffer + last_rcv_idx,
UART_RX_BUFFER_SIZE - last_rcv_idx);
last_rcv_idx = 0;
}
if (new_rcv_idx > last_rcv_idx) {
legacy_parse_stream(dma_circ_buffer + last_rcv_idx,
new_rcv_idx - last_rcv_idx);
last_rcv_idx = new_rcv_idx;
}
#endif
#endif
#if !defined(USB_PROTOCOL_NONE)
// 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.
const uint32_t usb_check_timeout = 1; // ms
osStatus sem_stat = osSemaphoreWait(sem_usb_rx, usb_check_timeout);
if (sem_stat == osOK) {
deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
#if defined(USB_PROTOCOL_NATIVE)
usb_channel.process_packet(usb_buf, usb_len);
#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED)
usb_stream_sink.process_bytes(usb_buf, usb_len);
#elif defined(USB_PROTOCOL_LEGACY)
legacy_parse_cmd(usb_buf, usb_len, USB_RX_DATA_SIZE, SERIAL_PRINTF_IS_USB);
#endif
USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet
}
#endif
}
// If we get here, then this task is done
vTaskDelete(osThreadGetId());
}
// Called from CDC_Receive_FS callback function, this allows motor_parse_cmd to access the
// incoming USB data
void set_cmd_buffer(uint8_t *buf, uint32_t len) {
usb_buf = buf;
usb_len = len;
}
void usb_update_thread() {
for (;;) {
// Wait for signalling from USB interrupt (OTG_FS_IRQHandler)
osStatus semaphore_status = osSemaphoreWait(sem_usb_irq, osWaitForever);
if (semaphore_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);
}
}
vTaskDelete(osThreadGetId());
}
Reference in New Issue View Git Blame Copy Permalink