Merge pull request #366 from tinymovr/firmware/tinymovr_r53

Support for Tinymovr R53

.vscode/tasks.json

@@ -96,9 +96,10 @@
                 "R50",
                 "R51",
                 "R52",
+                "R53",
                 "M51"
             ],
-            "default": "R52"
+            "default": "R53"
         }
     ]
 }

firmware/build_all.sh

@@ -7,7 +7,7 @@ DEST_DIR="./release_binaries"
 mkdir -p "$DEST_DIR"
 
 # Board revisions for which the firmware needs to be built
-REVISIONS=("R32" "R33" "R50" "R51" "R52" "M51")
+REVISIONS=("R32" "R33" "R50" "R51" "R52" "R53" "M51")
 
 # Build types
 TYPES=("release" "upgrade")

firmware/src/can/can.c

@@ -49,7 +49,16 @@ const size_t endpoint_count = sizeof(avlos_endpoints) / sizeof(avlos_endpoints[0
 
 void CAN_init(void)
 {
-#if defined(BOARD_REV_R52)
+#if defined(BOARD_REV_R53)
+    // Clearing bits for MUX F this way completely crashed the mcu
+    // no hardfault or anything, just debugger disconnected. 
+    // It was not possible connect via swd afterwards so the
+    // board got bricked. Thus avoid this command. Thankfully
+    // it is not necessary for enabling the gpio.
+    // PAC55XX_SCC->PFMUXSEL.w &= 0xFFFFFF0F;        // Clear bits to select GPIO function
+    PAC55XX_GPIOF->MODE.P5 = IO_PUSH_PULL_OUTPUT; // GPIO configured as an output
+    PAC55XX_GPIOF->OUT.P5 = 0;                    // Set low to force transceiver into normal mode
+#elif defined(BOARD_REV_R52)
     PAC55XX_SCC->PDMUXSEL.w &= 0xFFFFFF0F;        // Clear bits to select GPIO function
     PAC55XX_GPIOD->MODE.P7 = IO_PUSH_PULL_OUTPUT; // GPIO configured as an output
     PAC55XX_GPIOD->OUT.P7 = 0;                    // Set low to force transceiver into normal mode
@@ -84,6 +93,7 @@ void CAN_init(void)
     PAC55XX_GPIOD->MODE.P4 = IO_PUSH_PULL_OUTPUT; // GPIO configured as an output
     PAC55XX_GPIOD->OUT.P4 = 0;                    // Set low to force transceiver into normal mode
 
+
 #endif
 
     can_io_config(CAN_BUS_PINS);

firmware/src/common.h

@@ -92,7 +92,7 @@
 
 #if defined BOARD_REV_R32 || defined BOARD_REV_R33
 #define BOARD_REV_R3
-#elif defined BOARD_REV_R50 || BOARD_REV_R51 || defined BOARD_REV_R52
+#elif defined BOARD_REV_R50 || BOARD_REV_R51 || defined BOARD_REV_R52 || defined BOARD_REV_R53
 #define BOARD_REV_R5
 #elif defined BOARD_REV_M51
 #define BOARD_REV_M5
@@ -118,6 +118,8 @@
 #define BOARD_REV_IDX 11
 #elif defined BOARD_REV_R52
 #define BOARD_REV_IDX 12
+#elif defined BOARD_REV_R53
+#define BOARD_REV_IDX 13
 #elif defined BOARD_REV_M50
 #define BOARD_REV_IDX 20
 #elif defined BOARD_REV_M51

firmware/src/controller/controller.c

@@ -126,8 +126,11 @@ void Controller_ControlLoop(void)
         {
             state.is_calibrating = true;
             system_reset_calibration();
-            (void)(ADC_calibrate_offset() && motor_calibrate_resistance() && motor_calibrate_inductance());
-            (void)(sensors_calibrate());
+            // TODO: sensors_calibrate should also return bool, and be integrated in the calibration sequence
+            if (ADC_calibrate_offset() && motor_calibrate_resistance() && motor_calibrate_inductance())
+            {
+                (void)(sensors_calibrate());
+            }
             state.is_calibrating = false;
             controller_set_state(CONTROLLER_STATE_IDLE); 
         }

firmware/src/motor/motor.c

@@ -79,25 +79,30 @@ bool motor_calibrate_resistance(void)
 {
     if (!motor_get_is_gimbal())
     {
-        float I_cal = motor_get_I_cal();
-        float V_setpoint = 0.0f;
         FloatTriplet I_phase_meas = {0.0f};
         FloatTriplet modulation_values = {0.0f};
+
+		ADC_get_phase_currents(&I_phase_meas);
+
+		float I_meas = I_phase_meas.A;
+		float I_cal = motor_get_I_cal();
+        float V_setpoint = 0.0f;
 
         for (uint32_t i = 0; i < CAL_R_LEN; i++)
         {
             ADC_get_phase_currents(&I_phase_meas);
 
 			// 
-            if (V_setpoint > MAX_CALIBRATION_VOLTAGE && I_phase_meas.A < MIN_CALIBRATION_CURRENT)
+            if (V_setpoint > MAX_CALIBRATION_VOLTAGE && I_meas < MIN_CALIBRATION_CURRENT)
             {
                 uint8_t *error_ptr = motor_get_error_ptr();
                 *error_ptr |= MOTOR_ERRORS_ABNORMAL_CALIBRATION_VOLTAGE;
                 gate_driver_set_duty_cycle(&three_phase_zero);
                 return false;
             }
 
-            V_setpoint += CAL_V_GAIN * (I_cal - I_phase_meas.A);
+            V_setpoint += CAL_V_GAIN * (I_cal - I_meas);
+			I_meas += CAL_I_GAIN * (I_phase_meas.A - I_meas);
             const float pwm_setpoint = V_setpoint / system_get_Vbus();
             SVM(pwm_setpoint, 0.0f, &modulation_values.A, &modulation_values.B, &modulation_values.C);
             gate_driver_set_duty_cycle(&modulation_values);

firmware/src/motor/motor.h

@@ -35,7 +35,7 @@
 #define MIN_PHASE_INDUCTANCE (1e-5f)
 #define MAX_PHASE_INDUCTANCE (1e-2f)
 #define MAX_CALIBRATION_VOLTAGE (5.0f) // V
-#define MIN_CALIBRATION_CURRENT (0.1f) // A
+#define MIN_CALIBRATION_CURRENT (0.05f) // A
 #endif
 
 #define CAL_R_LEN             (2 * PWM_FREQ_HZ)
@@ -44,6 +44,7 @@
 #define CAL_STAY_LEN          (PWM_FREQ_HZ / 2)
 #define CAL_DIR_LEN           (3 * PWM_FREQ_HZ)
 #define CAL_PHASE_TURNS       (8)
+#define CAL_I_GAIN            (0.05f)
 #if defined BOARD_REV_R32 || BOARD_REV_R33 || defined BOARD_REV_R5
 #define CAL_V_GAIN            (0.0005f)
 #define CAL_V_INDUCTANCE      (2.0f)

firmware/src/sensor/sensor.h

@@ -22,14 +22,19 @@
 #include <src/ssp/ssp_func.h>
 #include <src/motor/motor.h>
 
-#if defined BOARD_REV_R3
-#define ONBOARD_SENSOR_SSP_PORT SSPD
-#define ONBOARD_SENSOR_SSP_STRUCT PAC55XX_SSPD
+#if defined(BOARD_REV_R53)
+#define ONBOARD_SENSOR_SSP_PORT SSPC
+#define ONBOARD_SENSOR_SSP_STRUCT PAC55XX_SSPC
+#define EXTERNAL_SENSOR_SSP_PORT SSPD_PD4567
+#define EXTERNAL_SENSOR_SSP_STRUCT PAC55XX_SSPD
 #elif defined BOARD_REV_R5 || defined BOARD_REV_M5
 #define ONBOARD_SENSOR_SSP_PORT SSPC
 #define ONBOARD_SENSOR_SSP_STRUCT PAC55XX_SSPC
 #define EXTERNAL_SENSOR_SSP_PORT SSPD
 #define EXTERNAL_SENSOR_SSP_STRUCT PAC55XX_SSPD
+#elif defined BOARD_REV_R3
+#define ONBOARD_SENSOR_SSP_PORT SSPD
+#define ONBOARD_SENSOR_SSP_STRUCT PAC55XX_SSPD
 #endif
 
 typedef struct Sensor Sensor;

firmware/src/ssp/ssp_func.c

@@ -661,13 +661,17 @@ void ssp_io_config(SSP_TYPE ssp, SSP_MS_TYPE ms_mode)
 
         case SSPD:
             // Select ssp D peripheral choose one!
-//            SSPD_IO_Select_PD4567(ms_mode);
 //            SSPD_IO_Select_PE4567(ms_mode);
             SSPD_IO_Select_PF4567(ms_mode);
 //            SSPD_IO_Select_PG0123(ms_mode);
 //            SSPD_IO_Select_PG4567(ms_mode);
             break;
 
+        case SSPD_PD4567:
+            SSPD_IO_Select_PD4567(ms_mode);
+
+            break;
+
         default:
             break;
     }

firmware/src/ssp/ssp_func.h

@@ -38,7 +38,8 @@ typedef enum
     SSPA = 0,
     SSPB = 1,
     SSPC = 2,
-    SSPD = 3
+    SSPD = 3,
+    SSPD_PD4567 = 4
 } SSP_TYPE;
 
 

studio/Python/tests/test_board.py

@@ -35,6 +35,7 @@
 class TestBoard(TMTestCase):
 
     @pytest.mark.hitl_default
+    @pytest.mark.hitl_mini
     def test_a_sensor_readings(self):
         """
         Test sensor readings
@@ -47,6 +48,7 @@ def test_a_sensor_readings(self):
         self.assertLess(st.pstdev(pos_estimates) * ticks, 5 * ticks)
 
     @pytest.mark.hitl_default
+    @pytest.mark.hitl_mini
     def test_b_invalid_values(self):
         """
         Test rejection of invalid values for limits and gains
@@ -77,6 +79,7 @@ def test_b_invalid_values(self):
         time.sleep(2)
 
     @pytest.mark.hitl_default
+    @pytest.mark.hitl_mini
     def test_c_calibrate(self):
         """
         Test board calibration if not calibrated
@@ -86,6 +89,7 @@ def test_c_calibrate(self):
 
     @pytest.mark.eol
     @pytest.mark.hitl_default
+    @pytest.mark.hitl_mini
     def test_d_position_control(self):
         """
         Test position control
@@ -111,61 +115,78 @@ def test_d_position_control(self):
             )
 
     @pytest.mark.hitl_default
+    @pytest.mark.hitl_mini
     def test_e_velocity_control(self):
         """
         Test velocity control
         """
+        hw_rev = self.tm.hw_revision
         self.check_state(0)
         self.try_calibrate()
         self.tm.controller.velocity_mode()
         self.check_state(2)
 
         R = 15
 
+        if hw_rev > 20:
+            multiplier = 4000 * ticks / s
+            max_delta = 15000 * ticks / s
+        else:
+            multiplier = 20000 * ticks / s
+            max_delta = 30000 * ticks / s
+
         velocity_pairs = []
 
         for i in range(R):
-            target = i * 20000 * ticks / s
+            target = i * multiplier
             self.tm.controller.velocity.setpoint = target
             time.sleep(tsleep)
             velocity_pairs.append((target, self.tm.sensors.user_frame.velocity_estimate))
 
         for i in range(R):
-            target = (R - i) * 20000 * ticks / s
+            target = (R - i) * multiplier
             self.tm.controller.velocity.setpoint = target
             time.sleep(tsleep)
             velocity_pairs.append((target, self.tm.sensors.user_frame.velocity_estimate))
 
         for i in range(R):
-            target = -i * 20000 * ticks / s
+            target = -i * multiplier
             self.tm.controller.velocity.setpoint = target
             time.sleep(tsleep)
             velocity_pairs.append((target, self.tm.sensors.user_frame.velocity_estimate))
 
         for i in range(R):
-            target = (i - R) * 20000 * ticks / s
+            target = (i - R) * multiplier
             self.tm.controller.velocity.setpoint = target
             time.sleep(tsleep)
             velocity_pairs.append((target, self.tm.sensors.user_frame.velocity_estimate))
 
         for target, estimate in velocity_pairs:
-            self.assertAlmostEqual(target, estimate, delta=30000 * ticks / s)
+            self.assertAlmostEqual(target, estimate, delta=max_delta)
 
     @pytest.mark.hitl_default
+    @pytest.mark.hitl_mini
     def test_f_random_pos_control(self):
         """
         Test random positions
         """
+        hw_rev = self.tm.hw_revision
         self.check_state(0)
         self.try_calibrate()
         self.tm.controller.position_mode()
         self.check_state(2)
 
-        self.tm.controller.velocity.limit = 200000 * ticks / s
+        if hw_rev > 20:
+            self.tm.controller.velocity.limit = 100000 * ticks / s
+            tick_range = 10000
+        else:
+            self.tm.controller.velocity.limit = 200000 * ticks / s
+            tick_range = 24000
 
         for _ in range(15):
-            new_pos = random.uniform(-24000, 24000)
+            new_pos = random.uniform(-tick_range, tick_range)
             self.tm.controller.position.setpoint = new_pos * ticks
+
             time.sleep(0.35)
             self.assertAlmostEqual(
                 self.tm.sensors.user_frame.position_estimate,