diff --git a/Inc/freertos.h b/Inc/freertos.h new file mode 100644 index 00000000..a0bc6d2b --- /dev/null +++ b/Inc/freertos.h @@ -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 */ \ No newline at end of file diff --git a/MotorControl/low_level.c b/MotorControl/low_level.c index 5c34bdf1..89030fbe 100644 --- a/MotorControl/low_level.c +++ b/MotorControl/low_level.c @@ -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, ¤t_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, ¤t_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, ¤t_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, ¤t_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, ¤t_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, ¤t_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 diff --git a/MotorControl/low_level.h b/MotorControl/low_level.h index c688ab5e..887ea4b2 100644 --- a/MotorControl/low_level.h +++ b/MotorControl/low_level.h @@ -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 diff --git a/Src/freertos.c b/Src/freertos.c index 473ba3c2..de35cebd 100644 --- a/Src/freertos.c +++ b/Src/freertos.c @@ -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 diff --git a/Src/stm32f4xx_it.c b/Src/stm32f4xx_it.c index a9f2a8ea..ead1bf2a 100644 --- a/Src/stm32f4xx_it.c +++ b/Src/stm32f4xx_it.c @@ -37,7 +37,8 @@ #include "cmsis_os.h" /* USER CODE BEGIN 0 */ -#include +#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); diff --git a/Src/usbd_cdc_if.c b/Src/usbd_cdc_if.c index e9b8b19e..6b0d57c4 100644 --- a/Src/usbd_cdc_if.c +++ b/Src/usbd_cdc_if.c @@ -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 */ }