From 6bb0361479c47a10596d010c82f098577e4da8f5 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@rapyuta-robotics.com>
Date: Wed, 1 Mar 2017 21:22:15 +0100
Subject: [PATCH] testing and cleanup

---
 MotorControl/low_level.c | 16 ++++++----------
 Src/freertos.c           |  9 +++++----
 Src/stm32f4xx_it.c       |  4 +---
 3 files changed, 12 insertions(+), 17 deletions(-)

diff --git a/MotorControl/low_level.c b/MotorControl/low_level.c
index 89030fbe..870318d2 100644
--- a/MotorControl/low_level.c
+++ b/MotorControl/low_level.c
@@ -135,10 +135,6 @@ 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;
@@ -193,27 +189,27 @@ void motor_parse_cmd(uint8_t* buffer, int len) {
     buffer[len] = 0;
 
     // check incoming packet type
-    if (pending_usb_buf[0] == 'p') {
+    if (buffer[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);
+        int numscan = sscanf(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 (pending_usb_buf[0] == 'v') {
+    } else if (buffer[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);
+        int numscan = sscanf(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 (pending_usb_buf[0] == 'c') {
+    } else if (buffer[0] == 'c') {
         // current control
         uint8_t motor_number;
         float current_feed_forward;
-        int numscan = sscanf(pending_usb_buf, "c %u %f ", &motor_number, &current_feed_forward);
+        int numscan = sscanf(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);
         }
diff --git a/Src/freertos.c b/Src/freertos.c
index de35cebd..a57c8d9a 100644
--- a/Src/freertos.c
+++ b/Src/freertos.c
@@ -84,9 +84,10 @@ void MX_FREERTOS_Init(void) {
   /* USER CODE END RTOS_MUTEX */
 
   /* USER CODE BEGIN RTOS_SEMAPHORES */
-  // Init usb irq semaphore with 0 tolkens.
+  // Init usb irq binary semaphore, and start with no tolkens by removing the starting one.
   osSemaphoreDef(sem_usb_irq);
-  sem_usb_irq = osSemaphoreCreate(osSemaphore(sem_usb_irq), 0);
+  sem_usb_irq = osSemaphoreCreate(osSemaphore(sem_usb_irq), 1);
+  osSemaphoreWait(sem_usb_irq, 0);
   /* USER CODE END RTOS_SEMAPHORES */
 
   /* USER CODE BEGIN RTOS_TIMERS */
@@ -143,9 +144,9 @@ void usb_cmd_thread(void const * argument) {
     // Wait for signalling from USB interrupt (OTG_FS_IRQHandler)
     osSemaphoreWait(sem_usb_irq, osWaitForever);
     // Irq processing loop
-    while(HAL_NVIC_GetActive(OTG_FS_IRQn)) {
+    //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);
   }
diff --git a/Src/stm32f4xx_it.c b/Src/stm32f4xx_it.c
index 76a9d20e..ead1bf2a 100644
--- a/Src/stm32f4xx_it.c
+++ b/Src/stm32f4xx_it.c
@@ -203,9 +203,7 @@ void OTG_FS_IRQHandler(void)
   // 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);
-  if (sem_usb_irq) {
-    osSemaphoreRelease(sem_usb_irq);
-  }
+  osSemaphoreRelease(sem_usb_irq);
   // Bypass interrupt processing here
   return;