Merge pull request #348 from tinymovr/firmware/amt22_fixes

Improve AMT22 reliability

firmware/src/sensor/amt22.c

@@ -60,11 +60,12 @@ bool amt22_init_with_config(Sensor *s, const AMT22SensorConfig *c)
 bool amt22_init(Sensor *s)
 {
     AMT22Sensor *as = (AMT22Sensor *)s;
-    ssp_init(as->config.ssp_port, SSP_MS_MASTER, 16, SSP_DATA_SIZE_8, 0, 0);
-    delay_us(10000); 
-
+    ssp_init(as->config.ssp_port, SSP_MS_MASTER, 16, SSP_DATA_SIZE_8, SWSEL_SW, 0, 0);
+    delay_us(50000); 
+    
     amt22_send_angle_cmd(s); 
     amt22_update(s, false); 
+
     s->initialized = true;
     return true;
 }

firmware/src/sensor/amt22.h

@@ -46,6 +46,9 @@ typedef struct
     AMT22SensorConfig config;
     uint8_t errors;
     int32_t angle;
+    uint8_t val_l;
+    uint8_t val_h;
+
 } AMT22Sensor;
 
 void amt22_make_blank_sensor(Sensor *s);
@@ -69,13 +72,19 @@ static inline uint8_t amt22_get_errors(const Sensor *s)
 static inline void amt22_send_angle_cmd(const Sensor *s)
 {
     // AMT22 has specific timing requirements with respect to reading
-    // the first and second bytes of the angle. For this reason we
-    // insert the 6us delay that corresponds to the datasheet-specified
-    // 3us + an experimentally defined value to account for transmission
-    // delay.
-    ssp_write_one(((const AMT22Sensor *)s)->config.ssp_struct, AMT22_CMD_READ_ANGLE);
-    delay_us(5);
-    ssp_write_one(((const AMT22Sensor *)s)->config.ssp_struct, AMT22_CMD_READ_ANGLE);
+    // the first and second bytes of the angle.
+    // TODO: These delays make reading of the sensor really slow
+    // refactor using interrupts
+    AMT22Sensor *as = ((AMT22Sensor *)s);
+    as->config.ssp_struct->SSCR.SWSS = 0;
+    delay_us(3);
+    ssp_write_one(as->config.ssp_struct, AMT22_CMD_READ_ANGLE);
+    as->val_h = ssp_read_one(as->config.ssp_struct);
+    delay_us(3);
+    ssp_write_one(as->config.ssp_struct, AMT22_CMD_READ_ANGLE);
+    as->val_l = ssp_read_one(as->config.ssp_struct);
+    delay_us(3);
+    as->config.ssp_struct->SSCR.SWSS = 1;
 }
 
 static inline int32_t amt22_get_raw_angle(const Sensor *s)
@@ -86,21 +95,35 @@ static inline int32_t amt22_get_raw_angle(const Sensor *s)
 static inline void amt22_update(Sensor *s, bool check_error)
 {
     AMT22Sensor *as = (AMT22Sensor *)s;
-    volatile uint8_t val_l = ssp_read_one(as->config.ssp_struct);
-    volatile uint8_t val_h = ssp_read_one(as->config.ssp_struct);
 
-    const int32_t angle = (((val_h & 0xff) << 8) | (val_l & 0xff)) & 0x3FFF; 
-    if (check_error)
+    const int32_t value = (((as->val_h & 0xff) << 8) | (as->val_l & 0xff)); 
+
+    // TODO: This checksum calculation is sub-optimal
+    bool binaryArray[16];
+
+    for(int i = 0; i < 16; i++) binaryArray[i] = (0x01) & (value >> (i));
+
+    if ((binaryArray[15] == !(binaryArray[13] ^ binaryArray[11] ^ binaryArray[9] ^ binaryArray[7] ^ binaryArray[5] ^ binaryArray[3] ^ binaryArray[1]))
+          && (binaryArray[14] == !(binaryArray[12] ^ binaryArray[10] ^ binaryArray[8] ^ binaryArray[6] ^ binaryArray[4] ^ binaryArray[2] ^ binaryArray[0])))
     {
-        const int32_t delta = as->angle - angle;
-        if ( ((delta > AMT22_MAX_ALLOWED_DELTA) || (delta < -AMT22_MAX_ALLOWED_DELTA)) &&
-            ((delta > AMT22_MAX_ALLOWED_DELTA_ADD) || (delta < AMT22_MIN_ALLOWED_DELTA_ADD)) &&
-            ((delta > AMT22_MAX_ALLOWED_DELTA_SUB) || (delta < AMT22_MIN_ALLOWED_DELTA_SUB)) )
+        const int32_t angle = value & 0x3FFF; 
+        if (check_error)
         {
-            as->errors |= SENSORS_SETUP_ONBOARD_ERRORS_READING_UNSTABLE;
+            const int32_t delta = as->angle - angle;
+            if ( ((delta > AMT22_MAX_ALLOWED_DELTA) || (delta < -AMT22_MAX_ALLOWED_DELTA)) &&
+                ((delta > AMT22_MAX_ALLOWED_DELTA_ADD) || (delta < AMT22_MIN_ALLOWED_DELTA_ADD)) &&
+                ((delta > AMT22_MAX_ALLOWED_DELTA_SUB) || (delta < AMT22_MIN_ALLOWED_DELTA_SUB)) )
+            {
+
+            }
+            else
+            {
+                as->angle = value & 0x3FFF;
+            }
         }
+
     }
-    as->angle = angle;
+
 }
 
 

firmware/src/sensor/as5047.c

@@ -60,7 +60,7 @@ bool as5047p_init_with_config(Sensor *s, const AS5047PSensorConfig *c)
 bool as5047p_init(Sensor *s)
 {
     AS5047PSensor *as = (AS5047PSensor *)s;
-    ssp_init(as->config.ssp_port, SSP_MS_MASTER, 16, SSP_DATA_SIZE_16, 1, 0);
+    ssp_init(as->config.ssp_port, SSP_MS_MASTER, 16, SSP_DATA_SIZE_16, SWSEL_SPI, 1, 0);
     delay_us(10000); // Example delay, adjust based on AS5047P datasheet
 
     as5047p_send_angle_cmd(s); 

firmware/src/sensor/ma7xx.c

@@ -60,7 +60,7 @@ bool ma7xx_init_with_port_and_rate(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_T
  bool ma7xx_init(Sensor *s)
 {
     MA7xxSensor *ms = (MA7xxSensor *)s;
-    ssp_init(ms->config.ssp_port, SSP_MS_MASTER, 4, SSP_DATA_SIZE_16, 0, 0);
+    ssp_init(ms->config.ssp_port, SSP_MS_MASTER, 4, SSP_DATA_SIZE_16, SWSEL_SPI, 0, 0);
     delay_us(16000); // ensure 16ms sensor startup time as per the datasheet
     ma7xx_send_angle_cmd(s);
     ma7xx_update(s, false);

firmware/src/ssp/ssp_func.c

@@ -759,7 +759,7 @@ void ssp_interrupt_disable(SSP_TYPE ssp)
     }
 }
 
-void ssp_init(SSP_TYPE ssp, SSP_MS_TYPE ms_mode, uint8_t clkn_div, uint32_t data_size, uint8_t cph, uint8_t cpol)
+void ssp_init(SSP_TYPE ssp, SSP_MS_TYPE ms_mode, uint8_t clkn_div, uint32_t data_size, SSP_SWSEL_TYPE swsel_type, uint8_t cph, uint8_t cpol)
 {
     PAC55XX_SSP_TYPEDEF *ssp_ptr;
 
@@ -806,6 +806,7 @@ void ssp_init(SSP_TYPE ssp, SSP_MS_TYPE ms_mode, uint8_t clkn_div, uint32_t data
 	ssp_ptr->CON.CPO = cpol;                                 // Clock Out Polarity
     ssp_ptr->CON.DSS = data_size;                            // Data Size Select
     ssp_ptr->CON.SOD = SSP_OUTPUT_NOT_DRIVE;                 // Slave Output Disable
+    ssp_ptr->SSCR.SWSEL = swsel_type;                             // Device Select Pin to be controlled by SPI (0) or software (1)
 
     ssp_io_config(ssp, ms_mode);
     ssp_interrupt_enable(ssp);

firmware/src/ssp/ssp_func.h

@@ -25,6 +25,13 @@
 
 #define DF_SSP_BUSY_TICK (25000u)    // use to check the busy tick
 
+// Device Select Enumeration Type
+typedef enum
+{
+    SWSEL_SPI = 0,
+    SWSEL_SW = 1
+} SSP_SWSEL_TYPE;
+
 // Interrupt Enable Enumeration Type
 typedef enum
 {
@@ -38,7 +45,7 @@ typedef enum
 volatile uint16_t ssp_data[10];
 volatile uint16_t data_num;
 
-extern void ssp_init(SSP_TYPE ssp, SSP_MS_TYPE ms_mode, uint8_t clkn_div, uint32_t data_size, uint8_t cph, uint8_t cpol);
+extern void ssp_init(SSP_TYPE ssp, SSP_MS_TYPE ms_mode, uint8_t clkn_div, uint32_t data_size, uint8_t swsel, uint8_t cph, uint8_t cpol);
 extern void ssp_deinit(SSP_TYPE ssp);
 extern uint32_t ssp_write_one(PAC55XX_SSP_TYPEDEF *ssp_ptr, uint16_t data);
 extern uint32_t ssp_write_multi(PAC55XX_SSP_TYPEDEF *ssp_ptr, uint16_t *data, uint32_t byte_num);

studio/Python/tests/test_amt22.py

@@ -57,8 +57,6 @@ def test_a_position_control_before_after_save_and_load_config(self):
         self.configure_sensors(self.tm.sensors.setup.external_spi.type.AMT22)
         self.select_sensors(self.tm.sensors.select.position_sensor.connection.ONBOARD, self.tm.sensors.select.position_sensor.connection.EXTERNAL_SPI)
 
-        self.tm.motor.I_cal = 1.0
-
         self.try_calibrate()
 
         self.tm.controller.position.p_gain = 9
@@ -118,7 +116,6 @@ def test_b_position_control_following_sensor_change(self):
         # Initially configure with external SPI sensor
         self.configure_sensors(self.tm.sensors.setup.external_spi.type.AMT22)
         self.select_sensors(self.tm.sensors.select.position_sensor.connection.ONBOARD, self.tm.sensors.select.position_sensor.connection.EXTERNAL_SPI)
-        self.tm.motor.I_cal = 1.0
         self.try_calibrate()
 
         # Set initial controller gains

studio/Python/tests/test_as5047.py

@@ -57,8 +57,6 @@ def test_a_position_control_before_after_save_and_load_config(self):
         self.configure_sensors(self.tm.sensors.setup.external_spi.type.AS5047)
         self.select_sensors(self.tm.sensors.select.position_sensor.connection.ONBOARD, self.tm.sensors.select.position_sensor.connection.EXTERNAL_SPI)
 
-        self.tm.motor.I_cal = 0.8
-
         self.try_calibrate()
 
         self.tm.controller.position.p_gain = 9
@@ -117,7 +115,6 @@ def test_b_position_control_following_sensor_change(self):
         # Initially configure with external SPI sensor
         self.configure_sensors(self.tm.sensors.setup.external_spi.type.AS5047)
         self.select_sensors(self.tm.sensors.select.position_sensor.connection.ONBOARD, self.tm.sensors.select.position_sensor.connection.EXTERNAL_SPI)
-        self.tm.motor.I_cal = 0.8
         self.try_calibrate()
 
         # Set initial controller gains
@@ -179,8 +176,6 @@ def test_c_position_control_change_baud_rates(self):
         self.configure_sensors(self.tm.sensors.setup.external_spi.type.AS5047)
         self.select_sensors(self.tm.sensors.select.position_sensor.connection.ONBOARD, self.tm.sensors.select.position_sensor.connection.EXTERNAL_SPI)
 
-        self.tm.motor.I_cal = 0.8
-
         self.try_calibrate()
 
         self.tm.controller.position.p_gain = 9

studio/Python/tests/test_base_function.py

@@ -23,10 +23,6 @@ def test_position_control(self):
         """
         hw_rev = self.tm.hw_revision
         self.check_state(0)
-        if hw_rev > 20:
-            self.tm.motor.I_cal = 0.8
-        else:
-            self.tm.motor.I_cal = 5
         self.tm.controller.current.Iq_limit = 5
         self.try_calibrate()
         self.tm.controller.position_mode()

studio/Python/tests/test_board.py

@@ -235,7 +235,6 @@ def test_j_gimbal_mode(self):
         # Ensure we're idle
         self.check_state(0)
         self.tm.motor.type = 1  # gimbal
-        self.tm.motor.I_cal = 5.0
         self.tm.motor.R = 0.2
         self.tm.motor.L = 20 * 1e-5
         self.try_calibrate()

studio/Python/tests/tm_test_case.py

@@ -61,8 +61,20 @@ def reset_and_wait(cls, timeout=0.5):
         cls.tm.reset()
         time.sleep(timeout)
 
-    def try_calibrate(self, force=False, precheck_callback=None, *args, **kwargs):
+    def try_calibrate(self, I_cal=None, force=False, precheck_callback=None,*args, **kwargs):
         if True == force or not self.tm.calibrated:
+            hw_rev = self.tm.hw_revision
+            self.check_state(0)
+            if I_cal and I_cal > 0:
+                self.tm.motor.I_cal = I_cal
+            elif hw_rev > 20:
+                self.tm.motor.I_cal = 0.8
+            else:
+                self.tm.motor.I_cal = 5
+
+            if hw_rev > 20:
+                self.tm.controller.velocity.P_gain = 2e-05
+
             self.tm.controller.calibrate()
             self.wait_for_calibration(*args, **kwargs)
             if precheck_callback: