From 5133db4961906bba4af627586ddd84d59316a019 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@rapyuta-robotics.com>
Date: Sat, 19 Nov 2016 21:41:46 +0900
Subject: [PATCH] only push to queue when measuring real current

---
 MotorControl/low_level.c | 86 +++++++++++++++++++++-------------------
 Odrive.ioc               |  2 +-
 2 files changed, 47 insertions(+), 41 deletions(-)

diff --git a/MotorControl/low_level.c b/MotorControl/low_level.c
index d51cd1d8..6acc1b25 100644
--- a/MotorControl/low_level.c
+++ b/MotorControl/low_level.c
@@ -158,60 +158,65 @@ void safe_assert(int arg) {
 //@TODO: Document how the phasing is done
 static void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc) {
 
+    //@TODO get rid of statics when using more than one motor
+    static float phB_DC_calib = 0.0f;
+    static float phC_DC_calib = 0.0f;
+    // static const float calib_filter_k = 
+
     // Check if this trigger was the CC4 channel, used for actual current measurement at SVM vector 0
     // or the update trigger, which is used for DC_CAL measurement at SVM vector 7
     uint32_t trig_src = hadc->Instance->CR2 & ADC_CR2_JEXTSEL;
     if (trig_src == ADC_EXTERNALTRIGINJECCONV_T1_CC4) {
-        //We should be measuring current here
-
+        //We are measuring current here
         //Set up next measurement to be DC_CAL measurement
         hadc->Instance->CR2 &= ~(ADC_CR2_JEXTSEL);
         hadc->Instance->CR2 |=  ADC_EXTERNALTRIGINJECCONV_T1_TRGO;
-    } else if (trig_src == ADC_EXTERNALTRIGINJECCONV_T1_TRGO) {
-        //We should be measuring DC_CAL here
 
+        // ADC2 and ADC3 record the phB and phC currents concurrently,
+        // and their interrupts should arrive on the same clock cycle.
+        // The HAL issues the callbacks in order, so ADC2 will always be processed before ADC3.
+        // Therefore we store the value from ADC2 and push them both into the queue
+        // when ADC3 is ready.
+        // @TODO: don't use statics, will only work for 1 motor chanel
+        static float phB_current;
+
+        //Only one conversion in sequence, so only rank1
+        uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1);
+
+        //Store and return, or fetch and continue
+        float phC_current;
+        if (hadc == &hadc2) {
+            phB_current = phase_current_from_adcval(ADCValue, 0);
+            return;
+        } else if (hadc == &hadc3) {
+            phC_current = phase_current_from_adcval(ADCValue, 0);
+        } else {
+            //hadc is something else, not expected
+            safe_assert(0);
+        }
+
+        //Allocate mail queue storage
+        Iph_BC_queue_item_t* mail_ptr;
+        mail_ptr = (Iph_BC_queue_item_t*) osMailAlloc(M0_Iph_queue, 0);
+        if (mail_ptr == NULL) {
+            return;
+        }
+
+        //Write contents and send mail
+        mail_ptr->current_phB = phB_current - phB_DC_calib;
+        mail_ptr->current_phC = phC_current - phC_DC_calib;
+        osMailPut(M0_Iph_queue, mail_ptr);
+
+    } else if (trig_src == ADC_EXTERNALTRIGINJECCONV_T1_TRGO) {
+        //We are measuring DC_CAL here
         //Set up next measurement to be current measurement
         hadc->Instance->CR2 &= ~(ADC_CR2_JEXTSEL);
         hadc->Instance->CR2 |=  ADC_EXTERNALTRIGINJECCONV_T1_CC4;
+
+
     } else {
         safe_assert(0);
     }
-
-    // ADC2 and ADC3 record the phB and phC currents concurrently,
-    // and their interrupts should arrive on the same clock cycle.
-    // The HAL issues the callbacks in order, so ADC2 will always be processed before ADC3.
-    // Therefore we store the value from ADC2 and push them both into the queue
-    // when ADC3 is ready.
-    // @TODO: don't use statics, will only work for 1 motor chanel
-    static float phB_current;
-
-    //Only one conversion in sequence, so only rank1
-    uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1);
-
-    //Store and return, or fetch and continue
-    float phC_current;
-    if (hadc == &hadc2) {
-        phB_current = phase_current_from_adcval(ADCValue, 0);
-        return;
-    } else if (hadc == &hadc3) {
-        phC_current = phase_current_from_adcval(ADCValue, 0);
-    } else {
-        //hadc is something else, not expected
-        safe_assert(0);
-    }
-
-    //Allocate mail queue storage
-    Iph_BC_queue_item_t* mail_ptr;
-    mail_ptr = (Iph_BC_queue_item_t*) osMailAlloc(M0_Iph_queue, 0);
-    if (mail_ptr == NULL) {
-        return;
-    }
-
-    //Write contents and send mail
-    mail_ptr->current_phB = phB_current;
-    mail_ptr->current_phC = phC_current;
-    osMailPut(M0_Iph_queue, mail_ptr);
-
 }
 
 void mark_timing() {
@@ -249,6 +254,7 @@ void motor_thread(void const * argument) {
                 //Since we wait forever, we do not expect timeouts here.
                 safe_assert(evt.status == osEventMail);
 
+                //Fetch current out of the mail queue
                 Iph_BC_queue_item_t* mail_ptr = evt.value.p;
                 float M0_phB_current = mail_ptr->current_phB;
                 float M0_phC_current = mail_ptr->current_phC;
diff --git a/Odrive.ioc b/Odrive.ioc
index f992c8b3..36f6f5fb 100755
--- a/Odrive.ioc
+++ b/Odrive.ioc
@@ -153,7 +153,7 @@ Mcu.Pin7=PC2
 Mcu.Pin8=PC3
 Mcu.Pin9=PA0-WKUP
 Mcu.PinsNb=55
-Mcu.UserConstants=
+Mcu.UserConstants=TIM_CLOCK_HZ,84000000
 Mcu.UserName=STM32F405RGTx
 MxCube.Version=4.17.0
 MxDb.Version=DB.4.0.170