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

change usb cmd processing to single thread with semaphore signalling

Browse Source
This commit is contained in:
Oskar Weigl
2017-03-01 14:57:39 +01:00
parent 6ea24abb79
commit 46e37f1eb5
6 changed files with 82 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Inc/freertos.h
+13
View File
@@ -0,0 +1,13 @@
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __FREERTOS_H
#define __FREERTOS_H
// List of semaphore
osSemaphoreId sem_usb_irq;
// List of threads
osThreadId thread_motor_0;
osThreadId thread_motor_1;
osThreadId thread_usb_cmd;
#endif /* __FREERTOS_H */
MotorControl/low_level.c
+28 -35
View File
@@ -170,7 +170,7 @@ void set_pos_setpoint(Motor_t* motor, float pos_setpoint, float vel_feed_forward
motor->vel_setpoint = vel_feed_forward;
motor->current_setpoint = current_feed_forward;
motor->control_mode = POSITION_CONTROL;
printf("POSITION_CONTROL %3.3f %3.3f %3.3f\n", motor->pos_setpoint, motor->vel_setpoint, motor->current_setpoint);
printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", motor->pos_setpoint, motor->vel_setpoint, motor->current_setpoint);
}
void set_vel_setpoint(Motor_t* motor, float vel_setpoint, float current_feed_forward) {
@@ -186,45 +186,38 @@ void set_current_setpoint(Motor_t* motor, float current_setpoint) {
printf("CURRENT_CONTROL %3.3f\n", motor->current_setpoint);
}
void usb_mc_thread(void const * argument) {
// store threadId
usb_mc_thread_id = osThreadGetId();
// run processing loop
for(;;) {
// wait for USB motor control packets
osSignalWait(M_SIGNAL_USB_MOTOR_CONTROL, osWaitForever);
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 (pending_usb_buf[0] == 'p') {
// position control
uint8_t motor_number;
float pos_setpoint, vel_feed_forward, current_feed_forward;
int numscan = sscanf(pending_usb_buf, "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);
}
// position control
uint8_t motor_number;
float pos_setpoint, vel_feed_forward, current_feed_forward;
int numscan = sscanf(pending_usb_buf, "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 (pending_usb_buf[0] == 'v') {
// velocity control
uint8_t motor_number;
float vel_feed_forward, current_feed_forward;
int numscan = sscanf(pending_usb_buf, "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);
}
// velocity control
uint8_t motor_number;
float vel_feed_forward, current_feed_forward;
int numscan = sscanf(pending_usb_buf, "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 (pending_usb_buf[0] == 'c') {
// velocity control
uint8_t motor_number;
float current_feed_forward;
int numscan = sscanf(pending_usb_buf, "c %u %f ", &motor_number, &current_feed_forward);
if (numscan == 2 && motor_number < num_motors) {
set_current_setpoint(&motors[motor_number], current_feed_forward);
}
// current control
uint8_t motor_number;
float current_feed_forward;
int numscan = sscanf(pending_usb_buf, "c %u %f ", &motor_number, &current_feed_forward);
if (numscan == 2 && motor_number < num_motors) {
set_current_setpoint(&motors[motor_number], current_feed_forward);
}
}
// clear the buffer
memset(pending_usb_buf, 0, sizeof(pending_usb_buf));
}
// If we get here, then this task is done
vTaskDelete(usb_mc_thread_id);
}
// Initalises the low level motor control and then starts the motor control threads
MotorControl/low_level.h
+4 -5
View File
@@ -8,8 +8,7 @@
/* Exported types ------------------------------------------------------------*/
typedef enum {
M_SIGNAL_PH_CURRENT_MEAS = 1u << 0,
M_SIGNAL_USB_MOTOR_CONTROL = 1u << 1
M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
} Motor_thread_signals_t;
typedef enum {
@@ -72,8 +71,6 @@ typedef struct {
extern float vbus_voltage;
extern Motor_t motors[];
extern const int num_motors;
extern uint8_t pending_usb_buf[64];
extern osThreadId usb_mc_thread_id;
/* Exported variables --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
@@ -91,6 +88,8 @@ void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc);
//@TODO move motor thread to high level file
void motor_thread(void const * argument);
void usb_mc_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
+25 -44
View File
@@ -46,7 +46,8 @@
#include "task.h"
#include "cmsis_os.h"
/* USER CODE BEGIN Includes */
/* USER CODE BEGIN Includes */
#include "freertos.h"
#include "low_level.h"
#include "version.h"
/* USER CODE END Includes */
@@ -55,11 +56,7 @@
osThreadId defaultTaskHandle;
/* USER CODE BEGIN Variables */
osThreadDef(task_motor_0, motor_thread, osPriorityHigh+1, 0, 512);
osThreadDef(task_motor_1, motor_thread, osPriorityHigh, 0, 512);
osThreadDef(task_usb_mc, usb_mc_thread, osPriorityIdle, 0, 512);
extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
/* USER CODE END Variables */
/* Function prototypes -------------------------------------------------------*/
@@ -69,9 +66,7 @@ extern void MX_USB_DEVICE_Init(void);
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
/* USER CODE BEGIN FunctionPrototypes */
void usb_int_thread(void const * argument);
void usb_cdc_thread(void const * argument);
void usb_cmd_thread(void const * argument);
/* USER CODE END FunctionPrototypes */
@@ -89,7 +84,9 @@ void MX_FREERTOS_Init(void) {
/* USER CODE END RTOS_MUTEX */
/* USER CODE BEGIN RTOS_SEMAPHORES */
/* add semaphores, ... */
// Init usb irq semaphore with 0 tolkens.
osSemaphoreDef(sem_usb_irq);
sem_usb_irq = osSemaphoreCreate(osSemaphore(sem_usb_irq), 0);
/* USER CODE END RTOS_SEMAPHORES */
/* USER CODE BEGIN RTOS_TIMERS */
@@ -118,23 +115,18 @@ void StartDefaultTask(void const * argument)
/* USER CODE BEGIN StartDefaultTask */
// Start USB Interrupt thread
osThreadDef(task_usb_int, usb_int_thread, osPriorityIdle, 0, 256);
osThreadCreate(osThread(task_usb_int), NULL);
// Init motor control
init_motor_control();
// Start motor threads
osThreadCreate(osThread(task_motor_0), &motors[0]);
osThreadCreate(osThread(task_motor_1), &motors[1]);
osThreadDef(task_motor_0, motor_thread, osPriorityHigh+1, 0, 512);
osThreadDef(task_motor_1, motor_thread, osPriorityHigh, 0, 512);
thread_motor_0 = osThreadCreate(osThread(task_motor_0), &motors[0]);
thread_motor_1 = osThreadCreate(osThread(task_motor_1), &motors[1]);
// Start USB CDC thread
osThreadDef(task_usb_cdc, usb_cdc_thread, osPriorityIdle, 0, 256);
osThreadCreate(osThread(task_usb_cdc), NULL);
// Start USB motor control thread
osThreadCreate(osThread(task_usb_mc), NULL);
// Start USB command handling thread
osThreadDef(task_usb_cmd, usb_cmd_thread, osPriorityNormal, 0, 512);
thread_usb_cmd = osThreadCreate(osThread(task_usb_cmd), NULL);
//If we get to here, then the default task is done.
vTaskDelete(defaultTaskHandle);
@@ -143,30 +135,19 @@ void StartDefaultTask(void const * argument)
}
/* USER CODE BEGIN Application */
void usb_int_thread(void const * argument) {
for(;;) {
// Periodically process USB OTG FS interrupt
// Thread to handle deffered processing of USB interrupt
void usb_cmd_thread(void const * argument) {
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);
HAL_NVIC_DisableIRQ(OTG_FS_IRQn);
osThreadYield();
}
// If we get here, then this task is done
vTaskDelete(osThreadGetId());
}
void usb_cdc_thread(void const * argument) {
// Wait some time for USB CDC connection and print version
osDelay(5000);
printf("ODrive Firmware v%d.%d.%d\n", ODRIVE_FW_VERSION_MAJOR, ODRIVE_FW_VERSION_MINOR, ODRIVE_FW_VERSION_PATCH);
for(;;) {
// Periodically print SysTick information
//printf("osKernelSysTick: %d\n", osKernelSysTick());
osDelay(1000);
}
// If we get here, then this task is done
Src/stm32f4xx_it.c
+7 -2
View File
@@ -37,7 +37,8 @@
#include "cmsis_os.h"
/* USER CODE BEGIN 0 */
#include <low_level.h>
#include "freertos.h"
#include "low_level.h"
typedef void (*ADC_handler_t)(ADC_HandleTypeDef* hadc);
void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, ADC_handler_t callback);
@@ -199,8 +200,12 @@ void OTG_FS_IRQHandler(void)
{
/* USER CODE BEGIN OTG_FS_IRQn 0 */
// only process one OTG FS interrupt at a time
// Mask interrupt, and signal processing of interrupt by usb_cmd_thread
// The thread will re-enable the interrupt when all pending irqs are clear.
HAL_NVIC_DisableIRQ(OTG_FS_IRQn);
osSemaphoreRelease(sem_usb_irq);
// Bypass interrupt processing here
return;
/* USER CODE END OTG_FS_IRQn 0 */
HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS);
Src/usbd_cdc_if.c
+5 -9
View File
@@ -71,8 +71,8 @@
/* USER CODE BEGIN PRIVATE_DEFINES */
/* Define size for the receive and transmit buffer over CDC */
/* It's up to user to redefine and/or remove those define */
#define APP_RX_DATA_SIZE 4
#define APP_TX_DATA_SIZE 4
#define APP_RX_DATA_SIZE 64
#define APP_TX_DATA_SIZE 64
/* USER CODE END PRIVATE_DEFINES */
/**
* @}
@@ -264,13 +264,9 @@ static int8_t CDC_Receive_FS (uint8_t* Buf, uint32_t *Len)
/* USER CODE BEGIN 6 */
USBD_CDC_SetRxBuffer(&hUsbDeviceFS, &Buf[0]);
USBD_CDC_ReceivePacket(&hUsbDeviceFS);
// if usb_mc_thread exists
if (usb_mc_thread_id) {
// copy to temporary buffer
memcpy(pending_usb_buf, Buf, *Len);
// alert USB motor control
osSignalSet(usb_mc_thread_id, M_SIGNAL_USB_MOTOR_CONTROL);
}
motor_parse_cmd(Buf, *Len);
return (USBD_OK);
/* USER CODE END 6 */
}