Skip to content

Commit

Permalink
Merge pull request #72 from Apehaenger/adjust-emergency
Browse files Browse the repository at this point in the history 
Fix emergency bit confusion
  • Loading branch information
@ClemensElflein
ClemensElflein committed Apr 6, 2024
2 parents 085c9f5 + fe6cbc5 commit 531f7eb
Show file tree
Hide file tree
Showing 2 changed files with 54 additions and 58 deletions.
21 changes: 17 additions & 4 deletions Firmware/LowLevel/src/datatypes.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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;
Expand Down
91 changes: 37 additions & 54 deletions Firmware/LowLevel/src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
Expand All @@ -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();
}
Expand All @@ -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
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit 531f7eb

Please sign in to comment.