From 8c58c7e645eb4931768ed9c3df89098148c4e7ef Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@rapyuta-robotics.com>
Date: Sun, 6 Nov 2016 17:10:10 +0900
Subject: [PATCH] current conversion in adc callback

---
 MotorControl/test.c | 80 ++++++++++++++++++++++++++++++++-------------
 1 file changed, 58 insertions(+), 22 deletions(-)

diff --git a/MotorControl/test.c b/MotorControl/test.c
index 55605cc8..9bb1398c 100644
--- a/MotorControl/test.c
+++ b/MotorControl/test.c
@@ -44,29 +44,36 @@ void start_adc_pwm(){
 
 }
 
-DRV8301_Obj gate_drivers[] = {
-    {
-        //M0
+typedef struct Motor_s {
+    DRV8301_Obj gate_driver;
+    float shunt_conductance;
+} Motor_t;
 
-        .spiHandle = &hspi3,
-        //Note: this board has the EN_Gate pin shared!
-        .EngpioHandle = EN_GATE_GPIO_Port,
-        .EngpioNumber = EN_GATE_Pin,
-        .nCSgpioHandle = M0_nCS_GPIO_Port,
-        .nCSgpioNumber = M0_nCS_Pin,
-        .RxTimeOut = false,
-        .enableTimeOut = false
+Motor_t motor_configs[] = {
+    { //M0
+        .gate_driver = {
+            .spiHandle = &hspi3,
+            //Note: this board has the EN_Gate pin shared!
+            .EngpioHandle = EN_GATE_GPIO_Port,
+            .EngpioNumber = EN_GATE_Pin,
+            .nCSgpioHandle = M0_nCS_GPIO_Port,
+            .nCSgpioNumber = M0_nCS_Pin,
+            .RxTimeOut = false,
+            .enableTimeOut = false
+        },
+        .shunt_conductance = 1.0f/0.0005f
     }
 };
-static const int num_motors = sizeof(gate_drivers)/sizeof(gate_drivers[0]);
+
+static const int num_motors = sizeof(motor_configs)/sizeof(motor_configs[0]);
 
 //Local view of DRV registers
 static DRV_SPI_8301_Vars_t gate_driver_regs[1/*num_motors*/];
 
 void test_DRV8301_setup() {
     for (int i = 0; i < num_motors; ++i) {
-        DRV8301_enable(&gate_drivers[i]);
-        DRV8301_setupSpi(&gate_drivers[i], &gate_driver_regs[i]);
+        DRV8301_enable(&motor_configs[i].gate_driver);
+        DRV8301_setupSpi(&motor_configs[i].gate_driver, &gate_driver_regs[i]);
 
         //@TODO we can use reporting only if we actually wire up the nOCTW pin
         gate_driver_regs[i].Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown;
@@ -76,9 +83,9 @@ void test_DRV8301_setup() {
         gate_driver_regs[i].Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_20VpV;
 
         gate_driver_regs[i].SndCmd = true;
-        DRV8301_writeData(&gate_drivers[i], &gate_driver_regs[i]);
+        DRV8301_writeData(&motor_configs[i].gate_driver, &gate_driver_regs[i]);
         gate_driver_regs[i].RcvCmd = true;
-        DRV8301_readData(&gate_drivers[i], &gate_driver_regs[i]);
+        DRV8301_readData(&motor_configs[i].gate_driver, &gate_driver_regs[i]);
     }
 }
 
@@ -158,7 +165,36 @@ void test_adc_hist_cb(ADC_HandleTypeDef* hadc) {
 }
 /////////////////////////////////////////////////
 
+float phase_current_from_adcval(uint32_t ADCValue, int motornum) {
+    float rev_gain;
+    switch (gate_driver_regs[motornum].Ctrl_Reg_2.GAIN) {
+        case DRV8301_ShuntAmpGain_10VpV:
+            rev_gain = 1.0f/10.0f;
+            break;
+        case DRV8301_ShuntAmpGain_20VpV:
+            rev_gain = 1.0f/20.0f;
+            break;
+        case DRV8301_ShuntAmpGain_40VpV:
+            rev_gain = 1.0f/40.0f;
+            break;
+        case DRV8301_ShuntAmpGain_80VpV:
+            rev_gain = 1.0f/80.0f;
+            break;
+    }
+
+    int adcval_bal = (int)ADCValue - (1<<11);
+    float amp_out_volt = (3.3f/(float)(1<<12)) * (float)adcval_bal;
+    float shunt_volt = amp_out_volt * rev_gain;
+    float current = shunt_volt * motor_configs[motornum].shunt_conductance;
+    return current;
+}
+
 void test_pwm_from_adc_cb(ADC_HandleTypeDef* hadc) {
+
+    //Only one conversion in sequence, so only rank1
+    uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1);
+    float M0_phB_current = phase_current_from_adcval(ADCValue, 0);
+
     int half_load = htim1.Instance->ARR/2;
     htim1.Instance->CCR1 = half_load - 400;
     htim1.Instance->CCR2 = half_load + 400;
@@ -169,11 +205,11 @@ void test_pwm_from_adc_cb(ADC_HandleTypeDef* hadc) {
 
 
 void test_motor_thread(void const * argument) {
-	int test = 0;
-	while(1) {
-		++test;
-		osDelay(10);
-	}
+    int test = 0;
+    while(1) {
+        ++test;
+        osDelay(10);
+    }
 }
 
 
@@ -183,7 +219,7 @@ void test_main(void) {
     //test_adc_trigger();
 
     test_DRV8301_setup();
-    osDelay(10000);
+    osDelay(1000);
     start_adc_pwm();
 }