From 1050ced7f6518b6d01cd240f9f41cb5c0baae5fb Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Mon, 10 Jul 2017 22:04:42 -0700
Subject: [PATCH] track motor control CPU time

---
 MotorControl/low_level.c | 28 +++++++++++++++++++---------
 MotorControl/low_level.h |  1 +
 2 files changed, 20 insertions(+), 9 deletions(-)

diff --git a/MotorControl/low_level.c b/MotorControl/low_level.c
index fcc7a3e0..aa30c406 100755
--- a/MotorControl/low_level.c
+++ b/MotorControl/low_level.c
@@ -65,6 +65,7 @@ Motor_t motors[] = {
         .motor_timer = &htim1,
         .next_timings = {TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2},
         .control_deadline = TIM_1_8_PERIOD_CLOCKS,
+        .last_cpu_time = 0,
         .current_meas = {0.0f, 0.0f},
         .DC_calib = {0.0f, 0.0f},
         .gate_driver = {
@@ -125,6 +126,7 @@ Motor_t motors[] = {
         .motor_timer = &htim8,
         .next_timings = {TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2},
         .control_deadline = (3*TIM_1_8_PERIOD_CLOCKS)/2,
+        .last_cpu_time = 0,
         .current_meas = {0.0f, 0.0f},
         .DC_calib = {0.0f, 0.0f},
         .gate_driver = {
@@ -244,10 +246,14 @@ float * exposed_floats [] = {
 
 int * exposed_ints [] = {
         (int*)&motors[0].control_mode, // rw
+        (int*)&motors[0].control_deadline, // rw
+        (int*)&motors[0].last_cpu_time, // ro
         &motors[0].rotor.encoder_offset, // rw
         &motors[0].rotor.encoder_state, // ro
         &motors[0].error, // rw
         (int*)&motors[1].control_mode, // rw
+        (int*)&motors[1].control_deadline, // rw
+        (int*)&motors[1].last_cpu_time, // ro
         &motors[1].rotor.encoder_offset, // rw
         &motors[1].rotor.encoder_state, // ro
         &motors[1].error, // rw
@@ -857,7 +863,8 @@ static bool measure_phase_inductance(Motor_t* motor, float voltage_low, float vo
             queue_voltage_timings(motor, test_voltages[i], 0.0f);
 
             // Check we meet deadlines after queueing
-            if (!(check_timing(motor) < motor->control_deadline)) {
+            motor->last_cpu_time = check_timing(motor);
+            if(!(motor->last_cpu_time < motor->control_deadline)){
                 motor->error = ERROR_PHASE_INDUCTANCE_TIMING;
                 return false;
             }
@@ -996,7 +1003,8 @@ static void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude
             queue_voltage_timings(motor, v_alpha, v_beta);
 
             // Check we meet deadlines after queueing
-            if(!(check_timing(motor) < motor->control_deadline)){
+            motor->last_cpu_time = check_timing(motor);
+            if(!(motor->last_cpu_time < motor->control_deadline)){
                 motor->error = ERROR_SCAN_MOTOR_TIMING;
                 return;
             }
@@ -1016,7 +1024,8 @@ static void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q) {
         queue_voltage_timings(motor, v_alpha, v_beta);
 
         // Check we meet deadlines after queueing
-        if (!(check_timing(motor) < motor->control_deadline)) {
+        motor->last_cpu_time = check_timing(motor);
+        if(!(motor->last_cpu_time < motor->control_deadline)){
             motor->error = ERROR_FOC_VOLTAGE_TIMING;
             return;
         }
@@ -1151,7 +1160,8 @@ static bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) {
     queue_modulation_timings(motor, mod_alpha, mod_beta);
 
     // Check we meet deadlines after queueing
-    if(!(check_timing(motor) < motor->control_deadline)){
+    motor->last_cpu_time = check_timing(motor);
+    if(!(motor->last_cpu_time < motor->control_deadline)){
         motor->error = ERROR_FOC_TIMING;
         return false;
     }
@@ -1240,11 +1250,11 @@ void motor_thread(void const * argument) {
 
 #ifdef STANDALONE_MODE
     //Only run tests on M0 for now
-    if (motor == &motors[1]) {
-        // TODO: figure out why M1 MOE must be enabled to run M0 correctly
-        __HAL_TIM_MOE_ENABLE(motor->motor_timer);
-        FOC_voltage_loop(motor, 0.0f, 0.0f);
-    }
+    // if (motor == &motors[1]) {
+    //     // TODO: figure out why M1 MOE must be enabled to run M0 correctly
+    //     __HAL_TIM_MOE_ENABLE(motor->motor_timer);
+    //     FOC_voltage_loop(motor, 0.0f, 0.0f);
+    // }
 
     motor->do_calibration = true;
     motor->enable_control = true;
diff --git a/MotorControl/low_level.h b/MotorControl/low_level.h
index c43c6846..cdc45cf1 100644
--- a/MotorControl/low_level.h
+++ b/MotorControl/low_level.h
@@ -92,6 +92,7 @@ typedef struct {
     TIM_HandleTypeDef* motor_timer;
     uint16_t next_timings[3];
     uint16_t control_deadline;
+    uint16_t last_cpu_time;
     Iph_BC_t current_meas;
     Iph_BC_t DC_calib;
     DRV8301_Obj gate_driver;