Skip to content

Commit 531f7eb

Browse files
ClemensElfleinClemensElflein
authored
Merge pull request #72 from Apehaenger/adjust-emergency
Fix emergency bit confusion
2 parents 085c9f5 + fe6cbc5 commit 531f7eb

File tree

2 files changed

+54
-58
lines changed

2 files changed

+54
-58
lines changed

‎Firmware/LowLevel/src/datatypes.h‎

Lines changed: 17 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,19 @@ enum HighLevelMode {
3434
MODE_RECORDING = 3 // ROS connected, Manual mode during recording etc
3535
};
3636

37+
#define LL_EMERGENCY_BIT_LATCH 0b00000001
38+
#define LL_EMERGENCY_BIT_HALL1 0b00001000 // Lift1
39+
#define LL_EMERGENCY_BIT_HALL2 0b00010000 // Lift2
40+
#define LL_EMERGENCY_BIT_HALL3 0b00000010 // Stop1
41+
#define LL_EMERGENCY_BIT_HALL4 0b00000100 // Stop2
42+
43+
#define LL_EMERGENCY_BIT_LIFT1 LL_EMERGENCY_BIT_HALL1
44+
#define LL_EMERGENCY_BIT_LIFT2 LL_EMERGENCY_BIT_HALL2
45+
#define LL_EMERGENCY_BITS_LIFT (LL_EMERGENCY_BIT_LIFT1 | LL_EMERGENCY_BIT_LIFT2)
46+
#define LL_EMERGENCY_BIT_STOP1 LL_EMERGENCY_BIT_HALL3
47+
#define LL_EMERGENCY_BIT_STOP2 LL_EMERGENCY_BIT_HALL4
48+
#define LL_EMERGENCY_BITS_STOP (LL_EMERGENCY_BIT_STOP1 | LL_EMERGENCY_BIT_STOP2)
49+
3750
#define LL_STATUS_BIT_UI_AVAIL 0b10000000
3851

3952
#pragma pack(push, 1)
@@ -54,10 +67,10 @@ struct ll_status {
5467
float uss_ranges_m[5];
5568
// Emergency bitmask:
5669
// Bit 0: Emergency latch
57-
// Bit 1: Emergency 0 active
58-
// Bit 2: Emergency 1 active
59-
// Bit 3: Emergency 2 active
60-
// Bit 4: Emergency 3 active
70+
// Bit 1: Emergency/Hall 3 (Stop1) active
71+
// Bit 2: Emergency/Hall 4 (Stop2) active
72+
// Bit 3: Emergency/Hall 1 (Lift1) active
73+
// Bit 4: Emergency/Hall 2 (Lift2) active
6174
uint8_t emergency_bitmask;
6275
// Charge voltage
6376
float v_charge;

‎Firmware/LowLevel/src/main.cpp‎

Lines changed: 37 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -145,32 +145,19 @@ void updateEmergency() {
145145
emergency_latch = true;
146146
ROS_running = false;
147147
}
148-
uint8_t last_emergency = status_message.emergency_bitmask & 1;
149-
150-
// Mask the emergency bits. 2x Lift sensor, 2x Emergency Button
151-
bool emergency1 = !gpio_get(PIN_EMERGENCY_1) | (stock_ui_emergency_state & Emergency_state::Emergency_lift1);
152-
bool emergency2 = !gpio_get(PIN_EMERGENCY_2) | (stock_ui_emergency_state & Emergency_state::Emergency_lift2);
153-
bool emergency3 = !gpio_get(PIN_EMERGENCY_3) | (stock_ui_emergency_state & Emergency_state::Emergency_stop1);
154-
bool emergency4 = !gpio_get(PIN_EMERGENCY_4) | (stock_ui_emergency_state & Emergency_state::Emergency_stop2);
155-
148+
uint8_t last_emergency = status_message.emergency_bitmask & LL_EMERGENCY_BIT_LATCH;
149+
150+
// Read & assign emergencies in the same manner as in ll_status.emergency_bitmask
151+
uint8_t emergency_read = !gpio_get(PIN_EMERGENCY_3) << 1 | // Stop1
152+
!gpio_get(PIN_EMERGENCY_4) << 2 | // Stop2
153+
!gpio_get(PIN_EMERGENCY_1) << 3 | // Lift1
154+
!gpio_get(PIN_EMERGENCY_2) << 4 | // Lift2
155+
stock_ui_emergency_state; // OR with StockUI emergency
156156
uint8_t emergency_state = 0;
157157

158-
bool is_tilted = emergency1 || emergency2;
159-
bool is_lifted = emergency1 && emergency2;
160-
bool stop_pressed = emergency3 || emergency4;
161-
162-
if (is_lifted) {
163-
// We just lifted, store the timestamp
164-
if (lift_emergency_started == 0) {
165-
lift_emergency_started = millis();
166-
}
167-
} else {
168-
// Not lifted, reset the time
169-
lift_emergency_started = 0;
170-
}
171-
172-
if (stop_pressed) {
173-
// We just pressed, store the timestamp
158+
// Handle emergency "Stop" buttons
159+
if (emergency_read && LL_EMERGENCY_BITS_STOP) {
160+
// If we just pressed, store the timestamp
174161
if (button_emergency_started == 0) {
175162
button_emergency_started = millis();
176163
}
@@ -179,17 +166,25 @@ void updateEmergency() {
179166
button_emergency_started = 0;
180167
}
181168

182-
if (LIFT_EMERGENCY_MILLIS > 0 && lift_emergency_started > 0 && (millis() - lift_emergency_started) >= LIFT_EMERGENCY_MILLIS) {
183-
// Emergency bit 2 (lift wheel 1)set?
184-
if (emergency1)
185-
emergency_state |= 0b01000;
186-
// Emergency bit 1 (lift wheel 2)set?
187-
if (emergency2)
188-
emergency_state |= 0b10000;
169+
if (button_emergency_started > 0 && (millis() - button_emergency_started) >= BUTTON_EMERGENCY_MILLIS)
170+
{
171+
emergency_state |= (emergency_read & LL_EMERGENCY_BITS_STOP);
189172
}
190173

191-
if (is_tilted) {
192-
// We just tilted, store the timestamp
174+
// Handle lifted (both wheels are lifted)
175+
if ((emergency_read & LL_EMERGENCY_BITS_LIFT) == LL_EMERGENCY_BITS_LIFT) {
176+
// If we just lifted, store the timestamp
177+
if (lift_emergency_started == 0) {
178+
lift_emergency_started = millis();
179+
}
180+
} else {
181+
// Not lifted, reset the time
182+
lift_emergency_started = 0;
183+
}
184+
185+
// Handle tilted (one wheel is lifted)
186+
if (emergency_read & LL_EMERGENCY_BITS_LIFT) {
187+
// If we just tilted, store the timestamp
193188
if (tilt_emergency_started == 0) {
194189
tilt_emergency_started = millis();
195190
}
@@ -198,32 +193,20 @@ void updateEmergency() {
198193
tilt_emergency_started = 0;
199194
}
200195

201-
if (TILT_EMERGENCY_MILLIS > 0 && tilt_emergency_started > 0 && (millis() - tilt_emergency_started) >= TILT_EMERGENCY_MILLIS) {
202-
// Emergency bit 2 (lift wheel 1)set?
203-
if (emergency1)
204-
emergency_state |= 0b01000;
205-
// Emergency bit 1 (lift wheel 2)set?
206-
if (emergency2)
207-
emergency_state |= 0b10000;
208-
}
209-
if (button_emergency_started > 0 && (millis() - button_emergency_started) >= BUTTON_EMERGENCY_MILLIS) {
210-
// Emergency bit 2 (stop button) set?
211-
if (emergency3)
212-
emergency_state |= 0b00010;
213-
// Emergency bit 1 (stop button)set?
214-
if (emergency4)
215-
emergency_state |= 0b00100;
196+
if ((LIFT_EMERGENCY_MILLIS > 0 && lift_emergency_started > 0 && (millis() - lift_emergency_started) >= LIFT_EMERGENCY_MILLIS) ||
197+
(TILT_EMERGENCY_MILLIS > 0 && tilt_emergency_started > 0 && (millis() - tilt_emergency_started) >= TILT_EMERGENCY_MILLIS)) {
198+
emergency_state |= (emergency_read & LL_EMERGENCY_BITS_LIFT);
216199
}
217200

218201
if (emergency_state || emergency_latch) {
219-
emergency_latch |= 1;
220-
emergency_state |= 1;
202+
emergency_latch = true;
203+
emergency_state |= LL_EMERGENCY_BIT_LATCH;
221204
}
222205

223206
status_message.emergency_bitmask = emergency_state;
224207

225208
// If it's a new emergency, instantly send the message. This is to not spam the channel during emergencies.
226-
if (last_emergency != (emergency_state & 1)) {
209+
if (last_emergency != (emergency_state & LL_EMERGENCY_BIT_LATCH)) {
227210
sendMessage(&status_message, sizeof(struct ll_status));
228211

229212
// Update UI instantly
@@ -307,11 +290,11 @@ void manageUILEDS() {
307290
}
308291

309292
// Show Info mower lifted or stop button pressed
310-
if (status_message.emergency_bitmask & 0b00110) {
293+
if (status_message.emergency_bitmask & LL_EMERGENCY_BITS_STOP) {
311294
setLed(leds_message, LED_MOWER_LIFTED, LED_blink_fast);
312-
} else if (status_message.emergency_bitmask & 0b11000) {
295+
} else if (status_message.emergency_bitmask & LL_EMERGENCY_BITS_LIFT) {
313296
setLed(leds_message, LED_MOWER_LIFTED, LED_blink_slow);
314-
} else if (status_message.emergency_bitmask & 0b0000001) {
297+
} else if (status_message.emergency_bitmask & LL_EMERGENCY_BIT_LATCH) {
315298
setLed(leds_message, LED_MOWER_LIFTED, LED_on);
316299
} else {
317300
setLed(leds_message, LED_MOWER_LIFTED, LED_off);

0 commit comments

Comments
 (0)