Merge pull request #364 from tinymovr/firmware/pre_cl_check

Pre CL checks
tinymovr · Jul 17, 2024 · 0231297 · 0231297
2 parents 780ecbe + 47bc4d1
commit 0231297
Show file tree

Hide file tree

Showing 8 changed files with 690 additions and 18 deletions.
diff --git a/docs/protocol/reference.rst b/docs/protocol/reference.rst
@@ -324,6 +324,8 @@ Flags:
 
 - CURRENT_LIMIT_EXCEEDED
 
+- PRE_CL_I_SD_EXCEEDED
+
 controller.position.setpoint
 -------------------------------------------------------------------
 

diff --git a/firmware/src/can/can_endpoints.h b/firmware/src/can/can_endpoints.h
@@ -10,7 +10,7 @@
 #include <src/common.h>
 #include <src/tm_enums.h>
 
-static const uint32_t avlos_proto_hash = 3296093882;
+static const uint32_t avlos_proto_hash = 1031937702;
 extern uint8_t (*avlos_endpoints[97])(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 extern uint32_t _avlos_get_proto_hash(void);
 

diff --git a/firmware/src/config.h b/firmware/src/config.h
@@ -38,6 +38,8 @@
 #define VBUS_LOW_THRESHOLD          (10.4f)   // V
 #define VEL_HARD_LIMIT              (600000.0f)  // ticks/s
 #define I_HARD_LIMIT                (60.0f)    // A
+#define MAX_CL_INIT_STEPS           (200)
+#define PRE_CL_I_SD_MAX            (0.4f)
 
 // Encoder rectification lookup table size
 #define ECN_BITS (6)

diff --git a/firmware/src/controller/controller.c b/firmware/src/controller/controller.c
@@ -15,18 +15,20 @@
 //  * You should have received a copy of the GNU General Public License
 //  * along with this program. If not, see <http://www.gnu.org/licenses/>.
 
-#include "src/system/system.h"
+#include <src/system/system.h>
 #include <src/sensor/sensor.h>
 #include <src/observer/observer.h>
-#include "src/adc/adc.h"
-#include "src/motor/motor.h"
+#include <src/adc/adc.h>
+#include <src/motor/motor.h>
 #include <src/gatedriver/gatedriver.h>
 #include <src/utils/utils.h>
 #include <src/scheduler/scheduler.h>
 #include <src/can/can_endpoints.h>
 #include <src/controller/controller.h>
 #include "src/watchdog/watchdog.h"
 
+void CLPreStep(void);
+void CLPreCheck(void);
 void CLControlStep(void);
 static inline bool Controller_LimitVelocity(float min_limit, float max_limit, float vel_estimate,
                                                             float vel_gain, float *I);
@@ -61,7 +63,10 @@ static ControllerState state = {
     .Iq_integrator = 0.0f,
     .Id_integrator = 0.0f,
 
-    .t_plan = 0.0f};
+    .t_plan = 0.0f
+};
+
+Statistics pre_cl_stats = {0};
 
 #if defined BOARD_REV_R32 || BOARD_REV_R33 || defined BOARD_REV_R5
 
@@ -134,6 +139,16 @@ void Controller_ControlLoop(void)
                 controller_set_state(CONTROLLER_STATE_IDLE);
                 Watchdog_reset();
             }
+            else if ((motor_get_is_gimbal() == false) && pre_cl_stats.size < MAX_CL_INIT_STEPS)
+            {
+                CLPreStep();
+            }
+            else if ((motor_get_is_gimbal() == false) && pre_cl_stats.size == MAX_CL_INIT_STEPS)
+            {
+                state.pos_setpoint = observer_get_pos_estimate(&position_observer);
+                CLPreStep();
+                CLPreCheck();
+            }
             else
             {
                 CLControlStep();
@@ -143,6 +158,22 @@ void Controller_ControlLoop(void)
     }
 }
 
+TM_RAMFUNC void CLPreStep(void)
+{
+    gate_driver_set_duty_cycle(&three_phase_zero);
+    // Should approximate zero as from Kirchoff
+    float Iphase_meas_sum = state.I_phase_meas.A + state.I_phase_meas.B + state.I_phase_meas.C; 
+    update_statistics(&pre_cl_stats, Iphase_meas_sum);
+}
+
+TM_RAMFUNC void CLPreCheck(void)
+{
+    if (calculate_standard_deviation(&pre_cl_stats) > PRE_CL_I_SD_MAX)
+    {
+        state.errors |= CONTROLLER_ERRORS_PRE_CL_I_SD_EXCEEDED;
+    }
+}
+
 TM_RAMFUNC void CLControlStep(void)
 {
     switch (state.mode)
@@ -312,7 +343,6 @@ TM_RAMFUNC void controller_set_state(controller_state_options new_state)
     {
         if ((new_state == CONTROLLER_STATE_CL_CONTROL) && (state.state == CONTROLLER_STATE_IDLE) && (!errors_exist()) && motor_get_calibrated())
         {
-            state.pos_setpoint = observer_get_pos_estimate(&position_observer);
             gate_driver_enable();
             state.state = CONTROLLER_STATE_CL_CONTROL;
         }
@@ -325,6 +355,7 @@ TM_RAMFUNC void controller_set_state(controller_state_options new_state)
         {
             gate_driver_set_duty_cycle(&three_phase_zero);
             gate_driver_disable();
+            memset(&pre_cl_stats, 0, sizeof(pre_cl_stats));
             state.state = CONTROLLER_STATE_IDLE;
         }
     }

diff --git a/firmware/src/tm_enums.h b/firmware/src/tm_enums.h
@@ -56,7 +56,8 @@ typedef enum
 typedef enum
 {
     CONTROLLER_ERRORS_NONE = 0,
-    CONTROLLER_ERRORS_CURRENT_LIMIT_EXCEEDED = (1 << 0)
+    CONTROLLER_ERRORS_CURRENT_LIMIT_EXCEEDED = (1 << 0), 
+    CONTROLLER_ERRORS_PRE_CL_I_SD_EXCEEDED = (1 << 1)
 } controller_errors_flags;
 
 typedef enum

diff --git a/firmware/src/utils/utils.h b/firmware/src/utils/utils.h
@@ -115,7 +115,6 @@ static inline float our_floorf(float x)
 	return (float)((int)x - 1);
 }
 
-
 // based on https://github.com/divideconcept/FastTrigo/blob/master/fasttrigo.cpp
 static inline float cos_32s(float x)
 {
@@ -144,14 +143,24 @@ static inline float fast_sin(float angle)
     return fast_cos(halfpi-angle);
 }
 
-static inline int calculate_parity(int x, int mask)
+typedef struct {
+    float sum_current;
+    float sum_current_squared;
+    uint32_t size;
+} Statistics;
+
+static inline void update_statistics(Statistics *s, float new_current)
+{
+    s->sum_current += new_current;
+    s->sum_current_squared += new_current * new_current;
+    s->size++;
+}
+
+static inline float calculate_standard_deviation(Statistics *s)
 {
-    x &= mask;
-    x ^= x >> 8;
-    x ^= x >> 4;
-    x ^= x >> 2;
-    x ^= x >> 1;
-    return x & 1;
+    float mean = s->sum_current / s->size;
+    float mean_squares = s->sum_current_squared / s->size;
+    return fast_sqrt(mean_squares - mean * mean);
 }
 
 // https://github.com/madcowswe/ODrive/blob/3113aedf081cf40e942d25d3b0b36c8806f11f23/Firmware/MotorControl/utils.c

diff --git a/studio/Python/tests/test_board.py b/studio/Python/tests/test_board.py
@@ -97,6 +97,9 @@ def test_d_position_control(self):
         self.tm.controller.position_mode()
         self.check_state(2)
 
+        self.tm.controller.position.setpoint = 0
+        time.sleep(0.4)
+
         for i in range(5):
             self.tm.controller.position.setpoint = i * 3000 * ticks
             if hw_rev > 20:
@@ -158,6 +161,8 @@ def test_f_random_pos_control(self):
         self.tm.controller.position_mode()
         self.check_state(2)
 
+        self.tm.controller.velocity.limit = 200000 * ticks / s
+
         for _ in range(15):
             new_pos = random.uniform(-24000, 24000)
             self.tm.controller.position.setpoint = new_pos * ticks
@@ -259,11 +264,11 @@ def test_j_gimbal_mode(self):
         self.tm.controller.position_mode()
         self.check_state(2)
 
-        for i in range(10):
-            self.tm.controller.position.setpoint = i * 1000 * ticks
+        for i in range(5):
+            self.tm.controller.position.setpoint = i * 2000 * ticks
             time.sleep(0.3)
             self.assertAlmostEqual(
-                i * 1000 * ticks, self.tm.sensors.user_frame.position_estimate, delta=1000 * ticks
+                i * 2000 * ticks, self.tm.sensors.user_frame.position_estimate, delta=2000 * ticks
             )
 
     @pytest.mark.hitl_default