Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-08-30 08:06:33 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Move command parsing to own file

Browse Source
This commit is contained in:
Oskar Weigl
2017-10-10 20:36:07 -07:00
parent 41473f2bfb
commit 19893ee0c0
8 changed files with 346 additions and 314 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Makefile
+2 -1
View File
@@ -82,7 +82,8 @@ C_SOURCES = \
Src/usbd_cdc_if.c \
Src/syscalls.c \
MotorControl/utils.c \
MotorControl/low_level.c
MotorControl/commands.c \
MotorControl/low_level.c
ASM_SOURCES = \
startup/startup_stm32f405xx.s
MotorControl/commands.c
+314
View File
@@ -0,0 +1,314 @@
/* Includes ------------------------------------------------------------------*/
#include <commands.h>
#include <usart.h>
#include <freertos_vars.h>
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
/* Private constant data -----------------------------------------------------*/
// variables exposed to usb/serial interface via set/get/monitor
// Note: this will be depricated soon
static float* const exposed_floats[] = {
&vbus_voltage, // ro
NULL, //&elec_rad_per_enc, // ro
&motors[0].pos_setpoint, // rw
&motors[0].pos_gain, // rw
&motors[0].vel_setpoint, // rw
&motors[0].vel_gain, // rw
&motors[0].vel_integrator_gain, // rw
&motors[0].vel_integrator_current, // rw
&motors[0].vel_limit, // rw
&motors[0].current_setpoint, // rw
&motors[0].calibration_current, // rw
&motors[0].phase_inductance, // ro
&motors[0].phase_resistance, // ro
&motors[0].current_meas.phB, // ro
&motors[0].current_meas.phC, // ro
&motors[0].DC_calib.phB, // rw
&motors[0].DC_calib.phC, // rw
&motors[0].shunt_conductance, // rw
&motors[0].phase_current_rev_gain, // rw
&motors[0].current_control.current_lim, // rw
&motors[0].current_control.p_gain, // rw
&motors[0].current_control.i_gain, // rw
&motors[0].current_control.v_current_control_integral_d, // rw
&motors[0].current_control.v_current_control_integral_q, // rw
&motors[0].current_control.Ibus, // ro
&motors[0].encoder.phase, // ro
&motors[0].encoder.pll_pos, // rw
&motors[0].encoder.pll_vel, // rw
&motors[0].encoder.pll_kp, // rw
&motors[0].encoder.pll_ki, // rw
&motors[1].pos_setpoint, // rw
&motors[1].pos_gain, // rw
&motors[1].vel_setpoint, // rw
&motors[1].vel_gain, // rw
&motors[1].vel_integrator_gain, // rw
&motors[1].vel_integrator_current, // rw
&motors[1].vel_limit, // rw
&motors[1].current_setpoint, // rw
&motors[1].calibration_current, // rw
&motors[1].phase_inductance, // ro
&motors[1].phase_resistance, // ro
&motors[1].current_meas.phB, // ro
&motors[1].current_meas.phC, // ro
&motors[1].DC_calib.phB, // rw
&motors[1].DC_calib.phC, // rw
&motors[1].shunt_conductance, // rw
&motors[1].phase_current_rev_gain, // rw
&motors[1].current_control.current_lim, // rw
&motors[1].current_control.p_gain, // rw
&motors[1].current_control.i_gain, // rw
&motors[1].current_control.v_current_control_integral_d, // rw
&motors[1].current_control.v_current_control_integral_q, // rw
&motors[1].current_control.Ibus, // ro
&motors[1].encoder.phase, // ro
&motors[1].encoder.pll_pos, // rw
&motors[1].encoder.pll_vel, // rw
&motors[1].encoder.pll_kp, // rw
&motors[1].encoder.pll_ki, // rw
};
static int* const exposed_ints[] = {
(int*)&motors[0].control_mode, // rw
&motors[0].encoder.encoder_offset, // rw
&motors[0].encoder.encoder_state, // ro
&motors[0].error, // rw
(int*)&motors[1].control_mode, // rw
&motors[1].encoder.encoder_offset, // rw
&motors[1].encoder.encoder_state, // ro
&motors[1].error, // rw
};
static bool* const exposed_bools[] = {
&motors[0].thread_ready, // ro
&motors[0].enable_control, // rw
&motors[0].do_calibration, // rw
&motors[0].calibration_ok, // ro
&motors[1].thread_ready, // ro
&motors[1].enable_control, // rw
&motors[1].do_calibration, // rw
&motors[1].calibration_ok, // ro
};
static uint16_t* const exposed_uint16[] = {
&motors[0].control_deadline, // rw
&motors[0].last_cpu_time, // ro
&motors[1].control_deadline, // rw
&motors[1].last_cpu_time, // ro
};
/* Private variables ---------------------------------------------------------*/
monitoring_slot monitoring_slots[20] = {0};
/* Private function prototypes -----------------------------------------------*/
static void print_monitoring(int limit);
/* Function implementations --------------------------------------------------*/
void motor_parse_cmd(uint8_t* buffer, int len) {
// TODO very hacky way of terminating sscanf at end of buffer:
// We should do some proper struct packing instead of using sscanf altogether
buffer[len] = 0;
// check incoming packet type
if (buffer[0] == 'p') {
// position control
unsigned motor_number;
float pos_setpoint, vel_feed_forward, current_feed_forward;
int numscan = sscanf((const char*)buffer, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &current_feed_forward);
if (numscan == 4 && motor_number < num_motors) {
set_pos_setpoint(&motors[motor_number], pos_setpoint, vel_feed_forward, current_feed_forward);
}
} else if (buffer[0] == 'v') {
// velocity control
unsigned motor_number;
float vel_feed_forward, current_feed_forward;
int numscan = sscanf((const char*)buffer, "v %u %f %f", &motor_number, &vel_feed_forward, &current_feed_forward);
if (numscan == 3 && motor_number < num_motors) {
set_vel_setpoint(&motors[motor_number], vel_feed_forward, current_feed_forward);
}
} else if (buffer[0] == 'c') {
// current control
unsigned motor_number;
float current_feed_forward;
int numscan = sscanf((const char*)buffer, "c %u %f", &motor_number, &current_feed_forward);
if (numscan == 2 && motor_number < num_motors) {
set_current_setpoint(&motors[motor_number], current_feed_forward);
}
} else if (buffer[0] == 'g') { // GET
// g <0:float,1:int,2:bool,3:uint16> index
int type = 0;
int index = 0;
int numscan = sscanf((const char*)buffer, "g %u %u", &type, &index);
if (numscan == 2) {
switch(type){
case 0: {
printf("%f\n",*exposed_floats[index]);
break;
};
case 1: {
printf("%d\n",*exposed_ints[index]);
break;
};
case 2: {
printf("%d\n",*exposed_bools[index]);
break;
};
case 3: {
printf("%hu\n",*exposed_uint16[index]);
break;
};
}
}
} else if (buffer[0] == 's') { // SET
// s <0:float,1:int,2:bool,3:uint16> index value
int type = 0;
int index = 0;
int numscan = sscanf((const char*)buffer, "s %u %u", &type, &index);
if (numscan == 2) {
switch(type) {
case 0: {
sscanf((const char*)buffer, "s %u %u %f", &type, &index, exposed_floats[index]);
break;
};
case 1: {
sscanf((const char*)buffer, "s %u %u %d", &type, &index, exposed_ints[index]);
break;
};
case 2: {
int btmp = 0;
sscanf((const char*)buffer, "s %u %u %d", &type, &index, &btmp);
*exposed_bools[index] = btmp ? true : false;
break;
};
case 3: {
sscanf((const char*)buffer, "s %u %u %hu", &type, &index, exposed_uint16[index]);
break;
};
}
}
} else if (buffer[0] == 'm') { // Setup Monitor
// m <0:float,1:int,2:bool,3:uint16> index monitoring_slot
int type = 0;
int index = 0;
int slot = 0;
int numscan = sscanf((const char*)buffer, "m %u %u %u", &type, &index, &slot);
if (numscan == 3) {
monitoring_slots[slot].type = type;
monitoring_slots[slot].index = index;
}
} else if (buffer[0] == 'o') { // Output Monitor
int limit = 0;
int numscan = sscanf((const char*)buffer, "o %u", &limit);
if (numscan == 1) {
print_monitoring(limit);
}
}
}
static void print_monitoring(int limit) {
for (int i=0;i<limit;i++) {
switch (monitoring_slots[i].type) {
case 0:
printf("%f\t",*exposed_floats[monitoring_slots[i].index]);
break;
case 1:
printf("%d\t",*exposed_ints[monitoring_slots[i].index]);
break;
case 2:
printf("%d\t",*exposed_bools[monitoring_slots[i].index]);
break;
case 3:
printf("%hu\t",*exposed_uint16[monitoring_slots[i].index]);
break;
default:
i=100;
}
}
printf("\n");
}
// Thread to handle deffered processing of USB interrupt, and
// read commands out of the UART DMA circular buffer
void usb_cmd_thread(void const * argument) {
//DMA open loop continous circular buffer
//1ms delay periodic, chase DMA ptr around, on new data:
// Check for start char
// copy into parse-buffer
// check for end-char
// checksum, etc.
#define UART_BUFFER_SIZE 64
static uint8_t dma_circ_buffer[UART_BUFFER_SIZE];
static uint8_t parse_buffer[UART_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_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
// Re-run state-machine forever
for (;;) {
//Inialize recieve state machine
bool reset_read_state = false;
bool read_active = false;
uint32_t parse_buffer_idx = 0;
//Run state machine until reset
do {
// Fetch the circular buffer "write pointer", where it would write next
uint32_t rcv_idx = UART_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_BUFFER_SIZE)
last_rcv_idx = 0;
// Look for start character
if (c == '$') {
read_active = true;
continue; // do not record start char
}
// Record into parse buffer when actively reading
if (read_active) {
parse_buffer[parse_buffer_idx++] = c;
if (c == '\r' || c == '\n' || c == '!') {
// End of command string: exchange end char with terminating null
parse_buffer[parse_buffer_idx-1] = '\0';
motor_parse_cmd(parse_buffer, parse_buffer_idx);
// Reset receieve state machine
reset_read_state = true;
break;
} else if (parse_buffer_idx == UART_BUFFER_SIZE - 1) {
// We are not at end of command, and receiving another character after this
// would go into the last slot, which is reserved for terminating null.
// We have effectively overflowed parse buffer: abort.
reset_read_state = true;
break;
}
}
}
// When we reach here, we are out of immediate characters to fetch out of buffer
// So we sleep for a bit.
osDelay(1);
} while (!reset_read_state);
}
for (;;) {
// Wait for signalling from USB interrupt (OTG_FS_IRQHandler)
osSemaphoreWait(sem_usb_irq, osWaitForever);
// Irq processing loop
//while(HAL_NVIC_GetActive(OTG_FS_IRQn)) {
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());
}
MotorControl/commands.h
+10
View File
@@ -0,0 +1,10 @@
#ifndef COMMANDS_H
#define COMMANDS_H
#include <low_level.h>
/* Exported functions --------------------------------------------------------*/
void cmd_parse_thread(void const * argument);
void motor_parse_cmd(uint8_t* buffer, int len);
#endif /* COMMANDS_H */
MotorControl/low_level.c
+9 -223
View File
@@ -34,6 +34,11 @@
// Arbitrary non-zero inital value to avoid division by zero if ADC reading is late
float vbus_voltage = 12.0f;
// For now, this automatically updates to the interface that most
// recently recieved a command. In the future we may want to separate
// debug printf and the main serial comms.
SerialPrintf_t serial_printf_select = SERIAL_PRINTF_IS_NONE;
// TODO stick parameter into struct
#define ENCODER_CPR (600*4)
#define POLE_PAIRS 7
@@ -221,107 +226,9 @@ static const int current_meas_hz = CURRENT_MEAS_HZ;
/* Private variables ---------------------------------------------------------*/
static float brake_resistance = 0.47f; // [ohm]
/* Monitoring */
monitoring_slot monitoring_slots[20] = {0};
/* variables exposed to usb interface via set/get/monitor
* If you change something here, don't forget to regenerate the python interface with generate_api.py
* ro/rw : read only/read write -> ro prevents the code generator from generating setter
* */
float* exposed_floats[] = {
&vbus_voltage, // ro
&elec_rad_per_enc, // ro
&motors[0].pos_setpoint, // rw
&motors[0].pos_gain, // rw
&motors[0].vel_setpoint, // rw
&motors[0].vel_gain, // rw
&motors[0].vel_integrator_gain, // rw
&motors[0].vel_integrator_current, // rw
&motors[0].vel_limit, // rw
&motors[0].current_setpoint, // rw
&motors[0].calibration_current, // rw
&motors[0].phase_inductance, // ro
&motors[0].phase_resistance, // ro
&motors[0].current_meas.phB, // ro
&motors[0].current_meas.phC, // ro
&motors[0].DC_calib.phB, // rw
&motors[0].DC_calib.phC, // rw
&motors[0].shunt_conductance, // rw
&motors[0].phase_current_rev_gain, // rw
&motors[0].current_control.current_lim, // rw
&motors[0].current_control.p_gain, // rw
&motors[0].current_control.i_gain, // rw
&motors[0].current_control.v_current_control_integral_d, // rw
&motors[0].current_control.v_current_control_integral_q, // rw
&motors[0].current_control.Ibus, // ro
&motors[0].encoder.phase, // ro
&motors[0].encoder.pll_pos, // rw
&motors[0].encoder.pll_vel, // rw
&motors[0].encoder.pll_kp, // rw
&motors[0].encoder.pll_ki, // rw
&motors[1].pos_setpoint, // rw
&motors[1].pos_gain, // rw
&motors[1].vel_setpoint, // rw
&motors[1].vel_gain, // rw
&motors[1].vel_integrator_gain, // rw
&motors[1].vel_integrator_current, // rw
&motors[1].vel_limit, // rw
&motors[1].current_setpoint, // rw
&motors[1].calibration_current, // rw
&motors[1].phase_inductance, // ro
&motors[1].phase_resistance, // ro
&motors[1].current_meas.phB, // ro
&motors[1].current_meas.phC, // ro
&motors[1].DC_calib.phB, // rw
&motors[1].DC_calib.phC, // rw
&motors[1].shunt_conductance, // rw
&motors[1].phase_current_rev_gain, // rw
&motors[1].current_control.current_lim, // rw
&motors[1].current_control.p_gain, // rw
&motors[1].current_control.i_gain, // rw
&motors[1].current_control.v_current_control_integral_d, // rw
&motors[1].current_control.v_current_control_integral_q, // rw
&motors[1].current_control.Ibus, // ro
&motors[1].encoder.phase, // ro
&motors[1].encoder.pll_pos, // rw
&motors[1].encoder.pll_vel, // rw
&motors[1].encoder.pll_kp, // rw
&motors[1].encoder.pll_ki, // rw
};
int* exposed_ints[] = {
(int*)&motors[0].control_mode, // rw
&motors[0].encoder.encoder_offset, // rw
&motors[0].encoder.encoder_state, // ro
&motors[0].error, // rw
(int*)&motors[1].control_mode, // rw
&motors[1].encoder.encoder_offset, // rw
&motors[1].encoder.encoder_state, // ro
&motors[1].error, // rw
};
bool* exposed_bools[] = {
&motors[0].thread_ready, // ro
&motors[0].enable_control, // rw
&motors[0].do_calibration, // rw
&motors[0].calibration_ok, // ro
&motors[1].thread_ready, // ro
&motors[1].enable_control, // rw
&motors[1].do_calibration, // rw
&motors[1].calibration_ok, // ro
};
uint16_t* exposed_uint16[] = {
&motors[0].control_deadline, // rw
&motors[0].last_cpu_time, // ro
&motors[1].control_deadline, // rw
&motors[1].last_cpu_time, // ro
};
/* Private function prototypes -----------------------------------------------*/
// Command Handling
static void print_monitoring(int limit);
// Utility
static uint16_t check_timing(Motor_t* motor);
static void global_fault(int error);
@@ -361,27 +268,7 @@ static void control_motor_loop(Motor_t* motor);
// TODO move to different file
//--------------------------------
static void print_monitoring(int limit) {
for (int i=0;i<limit;i++) {
switch (monitoring_slots[i].type) {
case 0:
printf("%f\t",*exposed_floats[monitoring_slots[i].index]);
break;
case 1:
printf("%d\t",*exposed_ints[monitoring_slots[i].index]);
break;
case 2:
printf("%d\t",*exposed_bools[monitoring_slots[i].index]);
break;
case 3:
printf("%hu\t",*exposed_uint16[monitoring_slots[i].index]);
break;
default:
i=100;
}
}
printf("\n");
}
void set_pos_setpoint(Motor_t* motor, float pos_setpoint, float vel_feed_forward, float current_feed_forward) {
motor->pos_setpoint = pos_setpoint;
@@ -410,109 +297,6 @@ void set_current_setpoint(Motor_t* motor, float current_setpoint) {
#endif
}
void motor_parse_cmd(uint8_t* buffer, int len) {
// TODO very hacky way of terminating sscanf at end of buffer:
// We should do some proper struct packing instead of using sscanf altogether
buffer[len] = 0;
// check incoming packet type
if (buffer[0] == 'p') {
// position control
unsigned motor_number;
float pos_setpoint, vel_feed_forward, current_feed_forward;
int numscan = sscanf((const char*)buffer, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &current_feed_forward);
if (numscan == 4 && motor_number < num_motors) {
set_pos_setpoint(&motors[motor_number], pos_setpoint, vel_feed_forward, current_feed_forward);
}
} else if (buffer[0] == 'v') {
// velocity control
unsigned motor_number;
float vel_feed_forward, current_feed_forward;
int numscan = sscanf((const char*)buffer, "v %u %f %f", &motor_number, &vel_feed_forward, &current_feed_forward);
if (numscan == 3 && motor_number < num_motors) {
set_vel_setpoint(&motors[motor_number], vel_feed_forward, current_feed_forward);
}
} else if (buffer[0] == 'c') {
// current control
unsigned motor_number;
float current_feed_forward;
int numscan = sscanf((const char*)buffer, "c %u %f", &motor_number, &current_feed_forward);
if (numscan == 2 && motor_number < num_motors) {
set_current_setpoint(&motors[motor_number], current_feed_forward);
}
} else if (buffer[0] == 'g') { // GET
// g <0:float,1:int,2:bool,3:uint16> index
int type = 0;
int index = 0;
int numscan = sscanf((const char*)buffer, "g %u %u", &type, &index);
if (numscan == 2) {
switch(type){
case 0: {
printf("%f\n",*exposed_floats[index]);
break;
};
case 1: {
printf("%d\n",*exposed_ints[index]);
break;
};
case 2: {
printf("%d\n",*exposed_bools[index]);
break;
};
case 3: {
printf("%hu\n",*exposed_uint16[index]);
break;
};
}
}
} else if (buffer[0] == 's') { // SET
// s <0:float,1:int,2:bool,3:uint16> index value
int type = 0;
int index = 0;
int numscan = sscanf((const char*)buffer, "s %u %u", &type, &index);
if (numscan == 2) {
switch(type) {
case 0: {
sscanf((const char*)buffer, "s %u %u %f", &type, &index, exposed_floats[index]);
break;
};
case 1: {
sscanf((const char*)buffer, "s %u %u %d", &type, &index, exposed_ints[index]);
break;
};
case 2: {
int btmp = 0;
sscanf((const char*)buffer, "s %u %u %d", &type, &index, &btmp);
*exposed_bools[index] = btmp ? true : false;
break;
};
case 3: {
sscanf((const char*)buffer, "s %u %u %hu", &type, &index, exposed_uint16[index]);
break;
};
}
}
} else if (buffer[0] == 'm') { // Setup Monitor
// m <0:float,1:int,2:bool,3:uint16> index monitoring_slot
int type = 0;
int index = 0;
int slot = 0;
int numscan = sscanf((const char*)buffer, "m %u %u %u", &type, &index, &slot);
if (numscan == 3) {
monitoring_slots[slot].type = type;
monitoring_slots[slot].index = index;
}
} else if (buffer[0] == 'o') { // Output Monitor
int limit = 0;
int numscan = sscanf((const char*)buffer, "o %u", &limit);
if (numscan == 1) {
print_monitoring(limit);
}
}
}
//--------------------------------
// Utility
//--------------------------------
@@ -1050,6 +834,7 @@ static bool motor_calibration(Motor_t* motor){
// Test functions
//--------------------------------
__attribute__((unused))
static void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude) {
for (;;) {
for (float ph = 0.0f; ph < 2.0f * M_PI; ph += omega * current_meas_period) {
@@ -1069,6 +854,7 @@ static void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude
}
//TODO integrate as mode in main control loop
__attribute__((unused))
static void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q) {
for (;;) {
osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever);
MotorControl/low_level.h
+7 -3
View File
@@ -142,10 +142,17 @@ typedef struct{
int index;
} monitoring_slot;
typedef enum {
SERIAL_PRINTF_IS_NONE,
SERIAL_PRINTF_IS_USB,
SERIAL_PRINTF_IS_UART,
} SerialPrintf_t;
/* Exported constants --------------------------------------------------------*/
extern float vbus_voltage;
extern Motor_t motors[];
extern const int num_motors;
extern SerialPrintf_t serial_printf_select;
/* Exported variables --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
@@ -165,7 +172,4 @@ void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected);
//@TODO move motor thread to high level file
void motor_thread(void const * argument);
//@TODO move cmd parsing to high level file
void motor_parse_cmd(uint8_t* buffer, int len);
#endif //__LOW_LEVEL_H
Src/freertos.c
+2 -86
View File
@@ -54,15 +54,14 @@
/* USER CODE BEGIN Includes */
#include "freertos_vars.h"
#include "low_level.h"
#include "usart.h"
#include "version.h"
#include "commands.h"
/* USER CODE END Includes */
/* Variables -----------------------------------------------------------------*/
osThreadId defaultTaskHandle;
/* USER CODE BEGIN Variables */
extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
/* USER CODE END Variables */
/* Function prototypes -------------------------------------------------------*/
@@ -143,89 +142,6 @@ void StartDefaultTask(void const * argument)
/* USER CODE BEGIN Application */
//TODO move this to a different file
// Thread to handle deffered processing of USB interrupt
void usb_cmd_thread(void const * argument) {
//DMA open loop continous circular buffer
//1ms delay periodic, chase DMA ptr around, on new data:
// Check for start char
// copy into parse-buffer
// check for end-char
// checksum, etc.
#define UART_BUFFER_SIZE 64
static uint8_t dma_circ_buffer[UART_BUFFER_SIZE];
static uint8_t parse_buffer[UART_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_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
// Re-run state-machine forever
for (;;) {
//Inialize recieve state machine
bool reset_read_state = false;
bool read_active = false;
uint32_t parse_buffer_idx = 0;
//Run state machine until reset
do {
// Fetch the circular buffer "write pointer", where it would write next
uint32_t rcv_idx = UART_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_BUFFER_SIZE)
last_rcv_idx = 0;
// Look for start character
if (c == '$') {
read_active = true;
continue; // do not record start char
}
// Record into parse buffer when actively reading
if (read_active) {
parse_buffer[parse_buffer_idx++] = c;
if (c == '\r' || c == '\n' || c == '!') {
// End of command string: exchange end char with terminating null
parse_buffer[parse_buffer_idx-1] = '\0';
motor_parse_cmd(parse_buffer, parse_buffer_idx);
// Reset receieve state machine
reset_read_state = true;
break;
} else if (parse_buffer_idx == UART_BUFFER_SIZE - 1) {
// We are not at end of command, and receiving another character after this
// would go into the last slot, which is reserved for terminating null.
// We have effectively overflowed parse buffer: abort.
reset_read_state = true;
break;
}
}
}
// When we reach here, we are out of immediate characters to fetch out of buffer
// So we sleep for a bit.
osDelay(1);
} while (!reset_read_state);
}
for (;;) {
// Wait for signalling from USB interrupt (OTG_FS_IRQHandler)
osSemaphoreWait(sem_usb_irq, osWaitForever);
// Irq processing loop
//while(HAL_NVIC_GetActive(OTG_FS_IRQn)) {
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());
}
/* USER CODE END Application */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Src/syscalls.c
+1
View File
@@ -7,6 +7,7 @@
#include <sys/unistd.h>
#include "usbd_cdc_if.h"
#include <commands.h>
//int _read(int file, char *data, int len) {}
//int _close(int file) {}
Src/usbd_cdc_if.c
+1 -1
View File
@@ -50,7 +50,7 @@
#include "usbd_cdc_if.h"
/* USER CODE BEGIN INCLUDE */
#include "utils.h"
#include "low_level.h"
#include "commands.h"
/* USER CODE END INCLUDE */
/** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY