merge pollrequest Shutdown ESCs when IDLE ClemensElflein#97

Firmware/LowLevel/platformio.ini

@@ -42,6 +42,10 @@ debug_build_flags = -O0 -g -ggdb
 
 build_src_filter = +<*> -<.git/> -<.svn/> -<imu/*> -<soundsystem.cpp>
 
+;  shut down ESC when not in use or pitch > 15 degree for all configurations
+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

@@ -30,7 +30,7 @@
 
 enum HighLevelMode {
     MODE_IDLE = 1, // ROS connected, idle mode
-    MODE_AUTONOMOUS = 2, // ROS connected, Autonomous mode, either mowing or docking or undocking
+    MODE_AUTONOMOUS = 2, // ROS connected, Autonomous mode, either mowing or docking or undocking or paused
     MODE_RECORDING = 3 // ROS connected, Manual mode during recording etc
 };
 

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

@@ -49,12 +49,23 @@ 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;
+   // acceleration_mss[0] = accelerometer[0] * 9.81 / 1000.0;
+   // acceleration_mss[1] = accelerometer[1] * 9.81 / 1000.0;
+
+
+    // Nose down: X = -g
+    acceleration_mss[0] = -accelerometer[1] * 9.81 / 1000.0;
+    // Left down: Y = -g
+    acceleration_mss[1] = accelerometer[0] * 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;
+    //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[0] =  gyroscope[1] * (PI / 180.0) / 1000.0;
+    gyro_rads[1] = -gyroscope[0] * (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,12 +20,13 @@ 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();
-    acceleration_mss[2] = IMU.getAccelZ_mss();
-
-    gyro_rads[0] = IMU.getGyroX_rads();
-    gyro_rads[1] = IMU.getGyroY_rads();
+
+    acceleration_mss[0] =  IMU.getAccelY_mss();
+    acceleration_mss[1] =  -IMU.getAccelX_mss();
+    acceleration_mss[2] =  -IMU.getAccelZ_mss();
+
+    gyro_rads[0] = IMU.getGyroY_rads();
+    gyro_rads[1] = -IMU.getGyroX_rads();
     gyro_rads[2] = -IMU.getGyroZ_rads();
 
     mag_uT[0] = IMU.getMagX_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

@@ -14,10 +14,14 @@
 // SOFTWARE.
 //
 //
-// Changes 18.04.2024 Elmar
-
+// Changes 18.04.2024 ene9ba
+// Merge optimized Soundsystem by Apehaenger pull request #80
+// 
 // floating average integrated for UBat, UCharge and ICharge Hardware added 3,9 kohm between F2 and GND to prevent voltage injection without loading voltage
 // added offset for analog voltage
+//  
+// 27.07.2024 Merge Shutdown ESCs whe Idle (pull request#97) from olliewalsh
+//            Merge Bugfix/wt901 via SerialPIO (#96) from Apehaenger
 
 #include <NeoPixelConnect.h>
 #include <Arduino.h>
@@ -35,15 +39,17 @@
 #include <soundsystem.h>
 #endif
 
-#define IMU_CYCLETIME 20              // cycletime for refresh IMU data
-#define STATUS_CYCLETIME 100          // cycletime for refresh analog and digital Statusvalues
-#define UI_GET_VERSION_CYCLETIME 5000 // cycletime for UI Get_Version request (UI available check)
-#define UI_GET_VERSION_TIMEOUT 100    // timeout for UI Get_Version response (UI available check)
+#define IMU_CYCLETIME 20                // cycletime for refresh IMU data
+#define STATUS_CYCLETIME 100            // cycletime for refresh analog and digital Statusvalues
+#define UI_GET_VERSION_CYCLETIME 5000   // cycletime for UI Get_Version request (UI available check)
+#define UI_GET_VERSION_TIMEOUT 100      // timeout for UI Get_Version response (UI available check)
+
+#define TILT_EMERGENCY_MILLIS 2500      // Time for a single wheel to be lifted in order to count as emergency (0 disable). This is to filter uneven ground.
+#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 ANALOG_MEAN_COUNT 20            // size of array for calculation meanvalues
 
-#define TILT_EMERGENCY_MILLIS 2500  // Time for a single wheel to be lifted in order to count as emergency (0 disable). This is to filter uneven ground.
-#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 ANALOG_MEAN_COUNT 20       // size of array for calculation meanvalues
+#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
@@ -70,16 +76,16 @@ SerialPIO uiSerial(PIN_UI_TX, PIN_UI_RX, 250);
 #define ANALOG_VOLTAGE_OFFSET 21  // not investigated, but a/d shows an Offset 
 #define CURRENT_SENSE_GAIN 100.0f
 
-#define BATT_ABS_MAX 28.7f
-#define BATT_ABS_Min 21.7f
+#define BATT_ABS_MAX 28.0f
+#define BATT_ABS_Min 24.0f
 
 #define BATT_FULL BATT_ABS_MAX - 0.3f
 #define BATT_EMPTY BATT_ABS_Min + 0.3f
 
 //Values for chargecontrol set limits for overurrent and overvoltage
 #define CHARGE_MAX_CURRENT               1.5f  // max allowed loading current
-#define CHARGE_MAX_BATTERY_VOLTAGE      29.0f  // max allowed battery voltage
-#define CHARGE_MAX_CHARGE_VOLTAGE       30.0f  // max allowed charge voltage
+#define CHARGE_MAX_BATTERY_VOLTAGE      28.0f  // max allowed battery voltage
+#define CHARGE_MAX_CHARGE_VOLTAGE       29.0f  // max allowed charge voltage
 
 // Emergency will be engaged, if no heartbeat was received in this time frame.
 #define HEARTBEAT_MILLIS 500
@@ -130,7 +136,9 @@ bool charging_allowed = false;
 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
@@ -412,6 +420,12 @@ void setup() {
     pinMode(PIN_ENABLE_CHARGE, OUTPUT);
     digitalWrite(PIN_ENABLE_CHARGE, HIGH);
 
+    // set output ESC shutdown
+    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);
@@ -766,17 +780,23 @@ void loop() {
         imu_message.acceleration_mss[0] = imu_temp[0];
         imu_message.acceleration_mss[1] = imu_temp[1];
         imu_message.acceleration_mss[2] = imu_temp[2];
-        imu_message.gyro_rads[0] = imu_temp[3];
-        imu_message.gyro_rads[1] = imu_temp[4];
-        imu_message.gyro_rads[2] = imu_temp[5];
-        imu_message.mag_uT[0] = imu_temp[6];
-        imu_message.mag_uT[1] = imu_temp[7];
-        imu_message.mag_uT[2] = imu_temp[8];
+        imu_message.gyro_rads[0]        = imu_temp[3];
+        imu_message.gyro_rads[1]        = imu_temp[4];
+        imu_message.gyro_rads[2]        = imu_temp[5];
+        imu_message.mag_uT[0]           = imu_temp[6];
+        imu_message.mag_uT[1]           = imu_temp[7];
+        imu_message.mag_uT[2]           = imu_temp[8];
 
         imu_message.dt_millis = now - last_imu_millis;
         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;
+
+       last_imu_millis = now;
     }
 
     if (now - last_status_update_millis > STATUS_CYCLETIME) {
@@ -796,9 +816,34 @@ void loop() {
 #else
         ad_value = -1.0f;
 #endif
+
+
+
+
+#ifdef SHUTDOWN_ESC_WHEN_IDLE
+        // ESC power saving when mower is IDLE
+
+
+        if((ROS_running) && (fabs(pitch_angle) <= SHUTDOWN_ESC_MAX_PITCH) && (last_high_level_state.current_mode != HighLevelMode::MODE_IDLE))   {
+            // Enable escs if not idle, or if ROS is running, or on TILT and Mode is not idle
+            digitalWrite(PIN_ESC_SHUTDOWN, LOW);
+            //ToDo set this message state to a new statusbyte
+            //status_message.status_bitmask |= LL_STATUS_BIT_CHARGE_ERROR;
+        } else {
+            digitalWrite(PIN_ESC_SHUTDOWN, HIGH);
+            // Disable ESCs
+            //status_message.status_bitmask &= ~LL_STATUS_BIT_CHARGE_ERROR;
+        }
+#else
+        //status_message.status_bitmask |= 0b1000;  // ToDo Collision with charging error bit
+#endif
+
+
         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);
 
+
+
         // calculate percent value accu filling
         float delta = BATT_FULL - BATT_EMPTY;
         float vo = status_message.v_battery - BATT_EMPTY;

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_Drive_Motor.xml

@@ -1,14 +1,13 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <MCConfiguration>
-    <ConfigVersion>2</ConfigVersion>
     <pwm_mode>1</pwm_mode>
     <comm_mode>0</comm_mode>
     <motor_type>2</motor_type>
     <sensor_mode>0</sensor_mode>
     <l_current_max>2</l_current_max>
     <l_current_min>-2</l_current_min>
-    <l_in_current_max>5</l_in_current_max>
-    <l_in_current_min>-5</l_in_current_min>
+    <l_in_current_max>1</l_in_current_max>
+    <l_in_current_min>-1</l_in_current_min>
     <l_abs_current_max>15</l_abs_current_max>
     <l_min_erpm>-100000</l_min_erpm>
     <l_max_erpm>100000</l_max_erpm>
@@ -24,7 +23,7 @@
     <l_temp_fet_end>90</l_temp_fet_end>
     <l_temp_motor_start>85</l_temp_motor_start>
     <l_temp_motor_end>100</l_temp_motor_end>
-    <l_temp_accel_dec>0.15</l_temp_accel_dec>
+    <l_temp_accel_dec>0</l_temp_accel_dec>
     <l_min_duty>0.005</l_min_duty>
     <l_max_duty>0.95</l_max_duty>
     <l_watt_max>1.5e+06</l_watt_max>
@@ -187,7 +186,7 @@ p, li { white-space: pre-wrap; }
 p, li { white-space: pre-wrap; }
 &lt;/style&gt;&lt;/head&gt;&lt;body style=&quot; font-family:'Roboto'; font-size:12pt; font-weight:400; font-style:normal;&quot;&gt;
 &lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;Some comments about the motor quality. Images can be added as well.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</motor_quality_description>
-    <bms.type>1</bms.type>
+    <bms.type>0</bms.type>
     <bms.t_limit_start>45</bms.t_limit_start>
     <bms.t_limit_end>65</bms.t_limit_end>
     <bms.soc_limit_start>0.05</bms.soc_limit_start>

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>