Merge pull request #72 from Apehaenger/adjust-emergency

Fix emergency bit confusion

Author: ClemensElflein

Firmware/LowLevel/src/datatypes.h

@@ -34,6 +34,19 @@ enum HighLevelMode {
     MODE_RECORDING = 3 // ROS connected, Manual mode during recording etc
 };
 
+#define LL_EMERGENCY_BIT_LATCH 0b00000001
+#define LL_EMERGENCY_BIT_HALL1 0b00001000 // Lift1
+#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_LIFT1 LL_EMERGENCY_BIT_HALL1
+#define LL_EMERGENCY_BIT_LIFT2 LL_EMERGENCY_BIT_HALL2
+#define LL_EMERGENCY_BITS_LIFT (LL_EMERGENCY_BIT_LIFT1 | LL_EMERGENCY_BIT_LIFT2)
+#define LL_EMERGENCY_BIT_STOP1 LL_EMERGENCY_BIT_HALL3
+#define LL_EMERGENCY_BIT_STOP2 LL_EMERGENCY_BIT_HALL4
+#define LL_EMERGENCY_BITS_STOP (LL_EMERGENCY_BIT_STOP1 | LL_EMERGENCY_BIT_STOP2)
+
 #define LL_STATUS_BIT_UI_AVAIL 0b10000000
 
 #pragma pack(push, 1)
@@ -54,10 +67,10 @@ struct ll_status {
     float uss_ranges_m[5];
     // Emergency bitmask:
     // Bit 0: Emergency latch
-    // Bit 1: Emergency 0 active
-    // Bit 2: Emergency 1 active
-    // Bit 3: Emergency 2 active
-    // Bit 4: Emergency 3 active
+    // Bit 1: Emergency/Hall 3 (Stop1) active
+    // Bit 2: Emergency/Hall 4 (Stop2) active
+    // Bit 3: Emergency/Hall 1 (Lift1) active
+    // Bit 4: Emergency/Hall 2 (Lift2) active
     uint8_t emergency_bitmask;
     // Charge voltage
     float v_charge;

Firmware/LowLevel/src/main.cpp

@@ -145,32 +145,19 @@ void updateEmergency() {
         emergency_latch = true;
         ROS_running = false;
     }
-    uint8_t last_emergency = status_message.emergency_bitmask & 1;
-
-    // Mask the emergency bits. 2x Lift sensor, 2x Emergency Button
-    bool emergency1 = !gpio_get(PIN_EMERGENCY_1) | (stock_ui_emergency_state & Emergency_state::Emergency_lift1);
-    bool emergency2 = !gpio_get(PIN_EMERGENCY_2) | (stock_ui_emergency_state & Emergency_state::Emergency_lift2);
-    bool emergency3 = !gpio_get(PIN_EMERGENCY_3) | (stock_ui_emergency_state & Emergency_state::Emergency_stop1);
-    bool emergency4 = !gpio_get(PIN_EMERGENCY_4) | (stock_ui_emergency_state & Emergency_state::Emergency_stop2);
-
+    uint8_t last_emergency = status_message.emergency_bitmask & LL_EMERGENCY_BIT_LATCH;
+
+    // Read & assign emergencies in the same manner as in ll_status.emergency_bitmask
+    uint8_t emergency_read = !gpio_get(PIN_EMERGENCY_3) << 1 | // Stop1
+                             !gpio_get(PIN_EMERGENCY_4) << 2 | // Stop2
+                             !gpio_get(PIN_EMERGENCY_1) << 3 | // Lift1
+                             !gpio_get(PIN_EMERGENCY_2) << 4 | // Lift2
+                             stock_ui_emergency_state; // OR with StockUI emergency
     uint8_t emergency_state = 0;
 
-    bool is_tilted = emergency1 || emergency2;
-    bool is_lifted = emergency1 && emergency2;
-    bool stop_pressed = emergency3 || emergency4;
-
-    if (is_lifted) {
-        // We just lifted, store the timestamp
-        if (lift_emergency_started == 0) {
-            lift_emergency_started = millis();
-        }
-    } else {
-        // Not lifted, reset the time
-        lift_emergency_started = 0;
-    }
-
-    if (stop_pressed) {
-        // We just pressed, store the timestamp
+    // Handle emergency "Stop" buttons
+    if (emergency_read && LL_EMERGENCY_BITS_STOP) {
+        // If we just pressed, store the timestamp
         if (button_emergency_started == 0) {
             button_emergency_started = millis();
         }
@@ -179,17 +166,25 @@ void updateEmergency() {
         button_emergency_started = 0;
     }
 
-    if (LIFT_EMERGENCY_MILLIS > 0 && lift_emergency_started > 0 && (millis() - lift_emergency_started) >= LIFT_EMERGENCY_MILLIS) {
-        // Emergency bit 2 (lift wheel 1)set?
-        if (emergency1)
-            emergency_state |= 0b01000;
-        // Emergency bit 1 (lift wheel 2)set?
-        if (emergency2)
-            emergency_state |= 0b10000;
+    if (button_emergency_started > 0 && (millis() - button_emergency_started) >= BUTTON_EMERGENCY_MILLIS)
+    {
+        emergency_state |= (emergency_read & LL_EMERGENCY_BITS_STOP);
     }
 
-    if (is_tilted) {
-        // We just tilted, store the timestamp
+    // Handle lifted (both wheels are lifted)
+    if ((emergency_read & LL_EMERGENCY_BITS_LIFT) == LL_EMERGENCY_BITS_LIFT) {
+        // If we just lifted, store the timestamp
+        if (lift_emergency_started == 0) {
+            lift_emergency_started = millis();
+        }
+    } else {
+        // Not lifted, reset the time
+        lift_emergency_started = 0;
+    }
+
+    // Handle tilted (one wheel is lifted)
+    if (emergency_read & LL_EMERGENCY_BITS_LIFT) {
+        // If we just tilted, store the timestamp
         if (tilt_emergency_started == 0) {
             tilt_emergency_started = millis();
         }
@@ -198,32 +193,20 @@ void updateEmergency() {
         tilt_emergency_started = 0;
     }
 
-    if (TILT_EMERGENCY_MILLIS > 0 && tilt_emergency_started > 0 && (millis() - tilt_emergency_started) >= TILT_EMERGENCY_MILLIS) {
-        // Emergency bit 2 (lift wheel 1)set?
-        if (emergency1)
-            emergency_state |= 0b01000;
-        // Emergency bit 1 (lift wheel 2)set?
-        if (emergency2)
-            emergency_state |= 0b10000;
-    }
-    if (button_emergency_started > 0 && (millis() - button_emergency_started) >= BUTTON_EMERGENCY_MILLIS) {
-        // Emergency bit 2 (stop button) set?
-        if (emergency3)
-            emergency_state |= 0b00010;
-        // Emergency bit 1 (stop button)set?
-        if (emergency4)
-            emergency_state |= 0b00100;
+    if ((LIFT_EMERGENCY_MILLIS > 0 && lift_emergency_started > 0 && (millis() - lift_emergency_started) >= LIFT_EMERGENCY_MILLIS) ||
+        (TILT_EMERGENCY_MILLIS > 0 && tilt_emergency_started > 0 && (millis() - tilt_emergency_started) >= TILT_EMERGENCY_MILLIS)) {
+        emergency_state |= (emergency_read & LL_EMERGENCY_BITS_LIFT);
     }
 
     if (emergency_state || emergency_latch) {
-        emergency_latch |= 1;
-        emergency_state |= 1;
+        emergency_latch = true;
+        emergency_state |= LL_EMERGENCY_BIT_LATCH;
     }
 
     status_message.emergency_bitmask = emergency_state;
 
     // If it's a new emergency, instantly send the message. This is to not spam the channel during emergencies.
-    if (last_emergency != (emergency_state & 1)) {
+    if (last_emergency != (emergency_state & LL_EMERGENCY_BIT_LATCH)) {
         sendMessage(&status_message, sizeof(struct ll_status));
 
         // Update UI instantly
@@ -307,11 +290,11 @@ void manageUILEDS() {
     }
 
     // Show Info mower lifted or stop button pressed
-    if (status_message.emergency_bitmask & 0b00110) {
+    if (status_message.emergency_bitmask & LL_EMERGENCY_BITS_STOP) {
         setLed(leds_message, LED_MOWER_LIFTED, LED_blink_fast);
-    } else if (status_message.emergency_bitmask & 0b11000) {
+    } else if (status_message.emergency_bitmask & LL_EMERGENCY_BITS_LIFT) {
         setLed(leds_message, LED_MOWER_LIFTED, LED_blink_slow);
-    } else if (status_message.emergency_bitmask & 0b0000001) {
+    } else if (status_message.emergency_bitmask & LL_EMERGENCY_BIT_LATCH) {
         setLed(leds_message, LED_MOWER_LIFTED, LED_on);
     } else {
         setLed(leds_message, LED_MOWER_LIFTED, LED_off);