Shutdown ESCs when IDLE

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

Set ESC shutdown pin to HIGH if:
  - not on a slope (pitch < 15deg) and...
  - ROS is running and...
  - current mode is IDLE

Requires killswitch to be set to ADC2_HIGH in xesc app config

(Based on the work of @Pete_MI632 on discord)

Firmware/LowLevel/platformio.ini

@@ -42,6 +42,8 @@ debug_build_flags = -O0 -g -ggdb
 
 build_src_filter = +<*> -<.git/> -<.svn/> -<imu/*> -<soundsystem.cpp>
 
+build_flags = -DSHUTDOWN_ESC_WHEN_IDLE
+
 [env:0_13_X]
 lib_ignore = JY901_SERIAL,JY901_I2C
 lib_deps = ${env.lib_deps}

Firmware/LowLevel/src/datatypes.h

@@ -57,7 +57,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
+    // Bit 3: ESC power
     // Bit 4: Rain detected
     // Bit 5: Sound available
     // Bit 6: Sound busy

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

@@ -36,6 +36,7 @@
 #define LIFT_EMERGENCY_MILLIS 100  // Time for both wheels to be lifted in order to count as emergency (0 disable). This is to filter uneven ground.
 #define BUTTON_EMERGENCY_MILLIS 20 // Time for button emergency to activate. This is to debounce the button.
 
+#define SHUTDOWN_ESC_MAX_PITCH 15.0 // Do not shutdown ESCs if absolute pitch angle is greater than this
 // Define to stream debugging messages via USB
 // #define USB_DEBUG
 
@@ -118,6 +119,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
@@ -388,6 +390,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);
@@ -671,6 +676,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) {
@@ -686,6 +697,43 @@ void loop() {
 #else
         status_message.charging_current = -1.0f;
 #endif
+
+#ifdef SHUTDOWN_ESC_WHEN_IDLE
+        // ESC power saving when mower is IDLE
+        if(!ROS_running || last_high_level_state.current_mode != HighLevelMode::MODE_IDLE || fabs(pitch_angle) > SHUTDOWN_ESC_MAX_PITCH) {
+            // Enable escs if not idle, or if ROS is not running, or on a slope
+            digitalWrite(PIN_ESC_SHUTDOWN, LOW);
+            status_message.status_bitmask |= 0b1000;
+        } else {
+            digitalWrite(PIN_ESC_SHUTDOWN, HIGH);
+            status_message.status_bitmask &= 0b11110111;
+        }
+#else
+        status_message.status_bitmask |= 0b1000;
+#endif
+
+        // If mowing use charge current ADC to determine adc offset
+        if(
+                ROS_running &&
+                last_high_level_state.current_mode == HighLevelMode::MODE_AUTONOMOUS &&
+                last_high_level_state.gps_quality != 0
+            ) {
+            adc_offset_samples[next_adc_offset_sample++] = (float)analogRead(PIN_ANALOG_CHARGE_VOLTAGE);
+            next_adc_offset_sample %= 20;
+
+            float tmp = 0.0f;
+            for(int i=0; i<20; i++) {
+                tmp += adc_offset_samples[i];
+            }
+            float new_adc_offset = tmp / 20.0f;
+            // Limit maximum offset
+            if(new_adc_offset > 0.0f) {
+                adc_offset = min(new_adc_offset, (4096 * MAX_ADC_OFFSET_PC));
+            } else {
+                adc_offset = max(new_adc_offset, -(4096 * MAX_ADC_OFFSET_PC));
+            }
+        }
+
         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);