diff --git a/MotorControl/low_level.c b/MotorControl/low_level.c index ac35040a..de63abff 100644 --- a/MotorControl/low_level.c +++ b/MotorControl/low_level.c @@ -126,6 +126,10 @@ Motor_t motors[] = { }; const int num_motors = sizeof(motors)/sizeof(motors[0]); +//Pending USB buffer +uint8_t pending_usb_buf[64]; +osThreadId usb_mc_thread_id; + /* Private constant data -----------------------------------------------------*/ static const float one_by_sqrt3 = 0.57735026919f; static const float sqrt3_by_2 = 0.86602540378; @@ -178,6 +182,47 @@ 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); + // 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; + sscanf(pending_usb_buf, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, ¤t_feed_forward); + if (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; + sscanf(pending_usb_buf, "v %u %f %f", &motor_number, &vel_feed_forward, ¤t_feed_forward); + if (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; + sscanf(pending_usb_buf, "c %u %f ", &motor_number, ¤t_feed_forward); + if (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 void init_motor_control() { //Init gate drivers @@ -744,7 +789,7 @@ static void control_motor_loop(Motor_t* motor) { if (motor->control_mode >= POSITION_CONTROL) { float pos_err = motor->pos_setpoint - motor->rotor.pll_pos; vel_des += motor->pos_gain * pos_err; - } + } float vel_lim = motor->vel_limit; if (vel_des > vel_lim) vel_des = vel_lim; if (vel_des < -vel_lim) vel_des = -vel_lim; @@ -812,4 +857,3 @@ void motor_thread(void const * argument) { //De-energize motor queue_voltage_timings(motor, 0.0f, 0.0f); } - diff --git a/MotorControl/low_level.h b/MotorControl/low_level.h index c0a88119..8489ae95 100644 --- a/MotorControl/low_level.h +++ b/MotorControl/low_level.h @@ -8,7 +8,8 @@ /* Exported types ------------------------------------------------------------*/ typedef enum { - M_SIGNAL_PH_CURRENT_MEAS = 1u << 0 + M_SIGNAL_PH_CURRENT_MEAS = 1u << 0, + M_SIGNAL_USB_MOTOR_CONTROL = 1u << 1 } Motor_thread_signals_t; typedef enum { @@ -67,6 +68,8 @@ 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 ------------------------------------------------------------*/ @@ -84,5 +87,6 @@ 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); #endif //__LOW_LEVEL_H diff --git a/Src/freertos.c b/Src/freertos.c index 91675755..5998e75b 100644 --- a/Src/freertos.c +++ b/Src/freertos.c @@ -58,6 +58,7 @@ osThreadId defaultTaskHandle; 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, osPriorityNormal, 0, 512); /* USER CODE END Variables */ @@ -127,6 +128,9 @@ void StartDefaultTask(void const * argument) 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); + //If we get to here, then the default task is done. vTaskDelete(defaultTaskHandle); diff --git a/Src/usbd_cdc_if.c b/Src/usbd_cdc_if.c index 65044d9e..e9b8b19e 100644 --- a/Src/usbd_cdc_if.c +++ b/Src/usbd_cdc_if.c @@ -264,31 +264,12 @@ 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); - // check incoming packet type - if (Buf[0] == 'p') { - // position control - uint8_t motor_number; - float pos_setpoint, vel_feed_forward, current_feed_forward; - sscanf(Buf, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, ¤t_feed_forward); - if (motor_number < num_motors) { - set_pos_setpoint(&motors[motor_number], pos_setpoint, vel_feed_forward, current_feed_forward); - } - } else if (Buf[0] == 'v') { - // velocity control - uint8_t motor_number; - float vel_feed_forward, current_feed_forward; - sscanf(Buf, "v %u %f %f", &motor_number, &vel_feed_forward, ¤t_feed_forward); - if (motor_number < num_motors) { - set_vel_setpoint(&motors[motor_number], vel_feed_forward, current_feed_forward); - } - } else if (Buf[0] == 'c') { - // velocity control - uint8_t motor_number; - float current_feed_forward; - sscanf(Buf, "c %u %f ", &motor_number, ¤t_feed_forward); - if (motor_number < num_motors) { - set_current_setpoint(&motors[motor_number], current_feed_forward); - } + // 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); } return (USBD_OK); /* USER CODE END 6 */