tilt angle set so meanvalue, errorhandling integrated

Firmware/LowLevel/src/datatypes.h

@@ -39,6 +39,7 @@ enum HighLevelMode {
 #define LL_EMERGENCY_BIT_HALL2 0b00010000 // Lift2
 #define LL_EMERGENCY_BIT_HALL3 0b00000010 // Stop1
 #define LL_EMERGENCY_BIT_HALL4 0b00000100 // Stop2
+#define LL_EMERGENCY_BIT_TILT  0b00100000 // TILT
 
 #define LL_EMERGENCY_BIT_LIFT1 LL_EMERGENCY_BIT_HALL1
 #define LL_EMERGENCY_BIT_LIFT2 LL_EMERGENCY_BIT_HALL2
@@ -47,6 +48,7 @@ enum HighLevelMode {
 #define LL_EMERGENCY_BIT_STOP2 LL_EMERGENCY_BIT_HALL4
 #define LL_EMERGENCY_BITS_STOP (LL_EMERGENCY_BIT_STOP1 | LL_EMERGENCY_BIT_STOP2)
 
+// ll_status
 #define LL_STATUS_BIT_INITIALIZED   0b00000001
 #define LL_STATUS_BIT_RASPI_POWER   0b00000010
 #define LL_STATUS_BIT_CHARGING      0b00000100
@@ -56,6 +58,9 @@ enum HighLevelMode {
 #define LL_STATUS_BIT_SOUND_BUSY    0b01000000
 #define LL_STATUS_BIT_UI_AVAIL      0b10000000
 
+// ll_status_extend
+#define LL_STATUS_EXT_BIT_ESC_KILLSWITCH   0b00000001
+
 #pragma pack(push, 1)
 struct ll_status {
     // Type of this message. Has to be PACKET_ID_LL_STATUS.
@@ -78,6 +83,7 @@ struct ll_status {
     // Bit 2: Emergency/Hall 4 (Stop2) active
     // Bit 3: Emergency/Hall 1 (Lift1) active
     // Bit 4: Emergency/Hall 2 (Lift2) active
+    // Bit 5: Emergency/TILT   mower not in a horizontal position
     uint8_t emergency_bitmask;
     // Charge voltage
     float v_charge;
@@ -90,6 +96,28 @@ struct ll_status {
 } __attribute__((packed));
 #pragma pack(pop)
 
+
+#pragma pack(push, 1)
+struct ll_status_extend {
+    // Type of this message. Has to be PACKET_ID_LL_STATUS.
+    uint8_t type;
+    uint8_t state;
+    // Bitmask for addon states
+    // Bit 0: ESCs Power shut down
+    // Bit 1: 
+    // Bit 2: 
+    // Bit 3: 
+    // Bit 4: 
+    // Bit 5: 
+    // Bit 6: 
+    // Bit 7: 
+    uint8_t state1;
+    // Reserved floats
+    float floatvalue[5];
+    uint16_t crc;
+} __attribute__((packed));
+#pragma pack(pop)
+
 #pragma pack(push, 1)
 struct ll_imu {
     // Type of this message. Has to be PACKET_ID_LL_IMU.

Firmware/LowLevel/src/main.cpp

@@ -48,8 +48,9 @@
 #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_MEAN_COUNT   20            // size of array for calculation TILT-values   
 
-#define SHUTDOWN_ESC_MAX_PITCH 15.0     // Do not shutdown ESCs if absolute pitch angle is greater than this
+#define SHUTDOWN_ESC_MAX_PITCH 25.0     // Do not shutdown ESCs if absolute pitch angle is greater than this
 
 // Define to stream debugging messages via USB
 // #define USB_DEBUG
@@ -100,6 +101,7 @@ FastCRC16 CRC16;
 FloatingAverage Vcharge_Mean;
 FloatingAverage VBatt_Mean;
 FloatingAverage Icharge_Mean;
+FloatingAverage Tilt_Mean;
 
 unsigned long last_imu_millis = 0;
 unsigned long last_status_update_millis = 0;
@@ -120,6 +122,7 @@ bool stock_ui_rain = false;           // Get set by received Get_Rain packet
 // Predefined message buffers, so that we don't need to allocate new ones later.
 struct ll_imu imu_message = {0};
 struct ll_status status_message = {0};
+struct ll_status_extend status_message_extend = {0};
 // current high level state
 struct ll_high_level_state last_high_level_state = {0};
 
@@ -230,6 +233,13 @@ void updateEmergency() {
         emergency_state |= (emergency_read & LL_EMERGENCY_BITS_LIFT);
     }
 
+
+    // emergency if mower tilt angle to high 
+    if (fabs(Tilt_Mean.GetFloatAvg() > SHUTDOWN_ESC_MAX_PITCH)) {
+        emergency_state |= (emergency_read & LL_EMERGENCY_BIT_TILT);
+        }
+
+
     if (emergency_state || emergency_latch) {
         emergency_latch = true;
         emergency_state |= LL_EMERGENCY_BIT_LATCH;
@@ -539,6 +549,7 @@ void setup() {
     Vcharge_Mean.begin(ANALOG_MEAN_COUNT);
     VBatt_Mean.begin(ANALOG_MEAN_COUNT);
     Icharge_Mean.begin(ANALOG_MEAN_COUNT);
+    Tilt_Mean.begin(TILT_MEAN_COUNT);
 
     rp2040.resumeOtherCore();
 
@@ -795,6 +806,7 @@ void loop() {
         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;
+        Tilt_Mean.AddToFloatAvg(tilt_angle);
 
        last_imu_millis = now;
     }
@@ -821,21 +833,26 @@ void loop() {
 
 
 #ifdef SHUTDOWN_ESC_WHEN_IDLE
+        uint8_t last_emergency = status_message.emergency_bitmask;
+
         // 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))   {
+        float tmp =Tilt_Mean.GetFloatAvg();
+        if((ROS_running) && (fabs(Tilt_Mean.GetFloatAvg()) <= 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 {
+            status_message_extend.state &= !LL_STATUS_EXT_BIT_ESC_KILLSWITCH;
+
+        } 
+        else {
             digitalWrite(PIN_ESC_SHUTDOWN, HIGH);
-            // Disable ESCs
-            //status_message.status_bitmask &= ~LL_STATUS_BIT_CHARGE_ERROR;
+            status_message_extend.state |= LL_STATUS_EXT_BIT_ESC_KILLSWITCH;
+
         }
-#else
-        //status_message.status_bitmask |= 0b1000;  // ToDo Collision with charging error bit
+
+
+
+
+
 #endif