Shutdown ESCs when IDLE/EMERGENCY (#97)

Align IMU accelerometer axes.
Calculate pitch, roll, and overall tilt angles.

Set ESC shutdown pin to HIGH if:
  - shutdown_esc_max_pitch is set
  - not on a slope (pitch < shutdown_esc_max_pitch) and...
  - ROS is running and...
  - current mode is IDLE or emergency latched

Requires killswitch to be set to ADC2_HIGH in xesc app config

(Based on the work of @Pete_MI632 on discord)

Author: olliewalsh

Firmware/LowLevel/src/datatypes.h

@@ -101,7 +101,7 @@ struct ll_status {
     // Bit 0: Initialized (i.e. setup() was a success). If this is 0, all other bits are meaningless.
     // Bit 1: Raspberry Power
     // Bit 2: Charging enabled
-    // Bit 3: don't care (reserved for ESC shutdown PR)
+    // Bit 3: ESC power
     // Bit 4: Rain detected
     // Bit 5: Sound available
     // Bit 6: Sound busy
@@ -168,7 +168,7 @@ struct ll_high_level_state {
 struct ll_ui_event {
     // Type of this message. Has to be PACKET_ID_LL_UI_EVENT
     uint8_t type;
-    uint8_t button_id; 
+    uint8_t button_id;
     uint8_t press_duration;   // 0 for single press, 1 for long, 2 for very long press
     uint16_t crc;
 } __attribute__((packed));
@@ -236,7 +236,7 @@ struct ll_high_level_config {
     float v_battery_full = 28.7f - 0.3f;   // Full battery voltage used for % calc of capacity (-1 = unknown)
     uint16_t lift_period = 100;            // Period (ms) for >=2 wheels to be lifted in order to count as emergency (0 = disable, 0xFFFF = unknown). This is to filter uneven ground
     uint16_t tilt_period = 2500;           // Period (ms) for a single wheel to be lifted in order to count as emergency (0 = disable, 0xFFFF = unknown). This is to filter uneven ground
-    uint8_t shutdown_esc_max_pitch = 0;    // Do not shutdown ESCs if absolute pitch angle is greater than this (0 = disable, 0xffff = unknown) (to be implemented, see PR #97)
+    uint8_t shutdown_esc_max_pitch = 0;    // Do not shutdown ESCs if absolute pitch angle is greater than this (0 = disable, 0xffff = unknown)
     iso639_1 language = {'e', 'n'};        // ISO 639-1 (2-char) language code (en, de, ...)
     uint8_t volume = 80;                   // Volume (0-100%) feedback (if directly changed i.e. via CoverUI or WebApp) (0xff = do not change)
     HallConfig hall_configs[MAX_HALL_INPUTS] = {

Firmware/LowLevel/src/imu/LSM6DSO/imu.cpp

@@ -49,12 +49,16 @@ bool imu_read(float *acceleration_mss, float *gyro_rads, float *mag_uT) {
     success &= IMU.Get_X_Axes(accelerometer) == 0;
     success &= IMU.Get_G_Axes(gyroscope) == 0;
 
-    acceleration_mss[0] = accelerometer[0] * 9.81 / 1000.0;
-    acceleration_mss[1] = accelerometer[1] * 9.81 / 1000.0;
+    // Left down: Y = -g
+    acceleration_mss[1] = accelerometer[0] * 9.81 / 1000.0;
+    // Nose down: X = -g
+    acceleration_mss[0] = -accelerometer[1] * 9.81 / 1000.0;
+    // Flat: Z = +g
     acceleration_mss[2] = accelerometer[2] * 9.81 / 1000.0;
 
-    gyro_rads[0] = gyroscope[0] * (PI / 180.0) / 1000.0;
-    gyro_rads[1] = gyroscope[1] * (PI / 180.0) / 1000.0;
+    // Datasheet shows gyro and acceleromter axes are aligned
+    gyro_rads[1] = -gyroscope[0] * (PI / 180.0) / 1000.0;
+    gyro_rads[0] = gyroscope[1] * (PI / 180.0) / 1000.0;
     gyro_rads[2] = gyroscope[2] * (PI / 180.0) / 1000.0;
 
     mag_uT[0] = 0;

Firmware/LowLevel/src/imu/MPU9250/imu.cpp

@@ -20,14 +20,20 @@ bool imu_read(float *acceleration_mss, float *gyro_rads, float *mag_uT)
 {
     IMU.readSensor();
 
-    acceleration_mss[0] = IMU.getAccelX_mss();
-    acceleration_mss[1] = IMU.getAccelY_mss();
+    // TODO: test this
+    // Left down: Y = -g
+    acceleration_mss[1] = -IMU.getAccelX_mss();
+    // Nose down: X = -g
+    acceleration_mss[0] = IMU.getAccelY_mss();
+    // Flat: Z = +g
     acceleration_mss[2] = IMU.getAccelZ_mss();
 
-    gyro_rads[0] = IMU.getGyroX_rads();
-    gyro_rads[1] = IMU.getGyroY_rads();
+    // MPU-9250 Datasheet shows gyro and acceleromter axes are aligned
+    gyro_rads[1] = -IMU.getGyroX_rads();
+    gyro_rads[0] = IMU.getGyroY_rads();
     gyro_rads[2] = -IMU.getGyroZ_rads();
 
+    // Mag is NED coordinates
     mag_uT[0] = IMU.getMagX_uT();
     mag_uT[1] = IMU.getMagY_uT();
     mag_uT[2] = IMU.getMagZ_uT();

Firmware/LowLevel/src/imu/WT901_I2C/imu.cpp

@@ -23,6 +23,7 @@ bool imu_read(float *acceleration_mss, float *gyro_rads, float *mag_uT)
     IMU.GetMag();
     IMU.GetGyro();
 
+    // WT901 axes seem to be corrected?
     acceleration_mss[0] = (float)IMU.stcAcc.a[0] / 32768.0f * 16.0f * 9.81f;
     acceleration_mss[1] = (float)IMU.stcAcc.a[1] / 32768.0f * 16.0f * 9.81f;
     acceleration_mss[2] = (float)IMU.stcAcc.a[2] / 32768.0f * 16.0f * 9.81f;

Firmware/LowLevel/src/imu/WT901_SERIAL/imu.cpp

@@ -23,6 +23,7 @@ bool init_imu()
 
 bool imu_read(float *acceleration_mss, float *gyro_rads, float *mag_uT)
 {
+    // WT901 axes seem to be corrected?
     acceleration_mss[0] = (float)IMU.stcAcc.a[0] / 32768.0f * 16.0f * 9.81f;
     acceleration_mss[1] = (float)IMU.stcAcc.a[1] / 32768.0f * 16.0f * 9.81f;
     acceleration_mss[2] = (float)IMU.stcAcc.a[2] / 32768.0f * 16.0f * 9.81f;

Firmware/LowLevel/src/main.cpp

@@ -102,6 +102,7 @@ bool ROS_running = false;
 unsigned long charging_disabled_time = 0;
 
 float imu_temp[9];
+float pitch_angle = 0, roll_angle = 0, tilt_angle = 0;
 
 uint16_t ui_version = 0;                   // Last received UI firmware version
 uint8_t ui_topic_bitmask = Topic_set_leds; // UI subscription, default to Set_LEDs
@@ -396,6 +397,9 @@ void setup() {
     pinMode(PIN_ENABLE_CHARGE, OUTPUT);
     digitalWrite(PIN_ENABLE_CHARGE, HIGH);
 
+    pinMode(PIN_ESC_SHUTDOWN, OUTPUT);
+    digitalWrite(PIN_ESC_SHUTDOWN, LOW);
+
     gpio_init(PIN_RASPI_POWER);
     gpio_put(PIN_RASPI_POWER, true);
     gpio_set_dir(PIN_RASPI_POWER, true);
@@ -751,6 +755,12 @@ void loop() {
         sendMessage(&imu_message, sizeof(struct ll_imu));
 
         last_imu_millis = now;
+
+        // Update pitch, roll, tilt
+        pitch_angle = atan2f(imu_temp[0], imu_temp[2]) * 180.0f / M_PI;
+        roll_angle = atan2f(imu_temp[1], imu_temp[2]) * 180.0f / M_PI;
+        float accXY = sqrtf((imu_temp[0]*imu_temp[0]) + (imu_temp[1]*imu_temp[1]));
+        tilt_angle = atan2f(accXY, imu_temp[2]) * 180.0f / M_PI;
     }
 
     if (now - last_status_update_millis > STATUS_CYCLETIME) {
@@ -762,6 +772,25 @@ void loop() {
                 (float) analogRead(PIN_ANALOG_CHARGE_VOLTAGE) * (3.3f / 4096.0f) * ((VIN_R1 + VIN_R2) / VIN_R2);
         status_message.charging_current = llhl_config.options.ignore_charging_current == OptionState::ON ? -1.0f : (float)analogRead(PIN_ANALOG_CHARGE_CURRENT) * (3.3f / 4096.0f) / (CURRENT_SENSE_GAIN * R_SHUNT);
 
+
+        // ESC power saving
+        bool shutdown_escs = false;
+        if (llhl_config.shutdown_esc_max_pitch != 0) {
+            bool emergency_or_idle = emergency_latch || HighLevelState::getMode(last_high_level_state.current_mode) == HighLevelState::Mode::IDLE;
+            bool on_slope = fabs(pitch_angle) > llhl_config.shutdown_esc_max_pitch;
+            if(ROS_running && emergency_or_idle && !on_slope) {
+                shutdown_escs = true;
+            }
+        }
+        if (shutdown_escs){
+            digitalWrite(PIN_ESC_SHUTDOWN, HIGH);
+            status_message.status_bitmask &= 0b11110111;
+        } else {
+            digitalWrite(PIN_ESC_SHUTDOWN, LOW);
+            status_message.status_bitmask |= 0b1000;
+        }
+
+
         status_message.status_bitmask = (status_message.status_bitmask & 0b11111011) | ((charging_allowed & 0b1) << 2);
         status_message.status_bitmask = (status_message.status_bitmask & 0b11011111) | ((sound_available & 0b1) << 5);
 

configs/xESC/YardForce_Classic_Drive_App.xml

@@ -15,7 +15,7 @@
     <uavcan_raw_mode>0</uavcan_raw_mode>
     <uavcan_raw_rpm_max>50000</uavcan_raw_rpm_max>
     <servo_out_enable>0</servo_out_enable>
-    <kill_sw_mode>0</kill_sw_mode>
+    <kill_sw_mode>4</kill_sw_mode>
     <app_to_use>3</app_to_use>
     <app_ppm_conf.ctrl_type>0</app_ppm_conf.ctrl_type>
     <app_ppm_conf.pid_max_erpm>15000</app_ppm_conf.pid_max_erpm>

configs/xESC/YardForce_Classic_Mower_App.xml

@@ -15,7 +15,7 @@
     <uavcan_raw_mode>0</uavcan_raw_mode>
     <uavcan_raw_rpm_max>50000</uavcan_raw_rpm_max>
     <servo_out_enable>0</servo_out_enable>
-    <kill_sw_mode>0</kill_sw_mode>
+    <kill_sw_mode>4</kill_sw_mode>
     <app_to_use>3</app_to_use>
     <app_ppm_conf.ctrl_type>0</app_ppm_conf.ctrl_type>
     <app_ppm_conf.pid_max_erpm>15000</app_ppm_conf.pid_max_erpm>