Skip to content

Commit

Permalink
Bitwise emergency handling
Browse files Browse the repository at this point in the history
  • Loading branch information
@Apehaenger
Apehaenger committed Mar 12, 2024
1 parent d8598da commit fe6cbc5
Show file tree
Hide file tree
Showing 2 changed files with 38 additions and 51 deletions.
16 changes: 8 additions & 8 deletions Firmware/LowLevel/src/datatypes.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,10 +33,10 @@ enum HighLevelMode {
};

#define LL_EMERGENCY_BIT_LATCH 0b00000001
#define LL_EMERGENCY_BIT_HALL1 0b00001000
#define LL_EMERGENCY_BIT_HALL2 0b00010000
#define LL_EMERGENCY_BIT_HALL3 0b00000010
#define LL_EMERGENCY_BIT_HALL4 0b00000100
#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
Expand Down Expand Up @@ -65,10 +65,10 @@ struct ll_status {
float uss_ranges_m[5];
// Emergency bitmask:
// Bit 0: Emergency latch
// Bit 1: Emergency/Hall 3 active
// Bit 2: Emergency/Hall 4 active
// Bit 3: Emergency/Hall 1 active
// Bit 4: Emergency/Hall 2 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
73 changes: 30 additions & 43 deletions Firmware/LowLevel/src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -143,30 +143,17 @@ void updateEmergency() {
}
uint8_t last_emergency = status_message.emergency_bitmask & LL_EMERGENCY_BIT_LATCH;

// 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);

// 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 @@ -175,15 +162,25 @@ void updateEmergency() {
button_emergency_started = 0;
}

if (LIFT_EMERGENCY_MILLIS > 0 && lift_emergency_started > 0 && (millis() - lift_emergency_started) >= LIFT_EMERGENCY_MILLIS) {
if (emergency1)
emergency_state |= LL_EMERGENCY_BIT_LIFT1;
if (emergency2)
emergency_state |= LL_EMERGENCY_BIT_LIFT2;
if (button_emergency_started > 0 && (millis() - button_emergency_started) >= BUTTON_EMERGENCY_MILLIS)
{
emergency_state |= (emergency_read & LL_EMERGENCY_BITS_STOP);
}

// 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;
}

if (is_tilted) {
// We just tilted, store the timestamp
// 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 @@ -192,18 +189,9 @@ void updateEmergency() {
tilt_emergency_started = 0;
}

if (TILT_EMERGENCY_MILLIS > 0 && tilt_emergency_started > 0 && (millis() - tilt_emergency_started) >= TILT_EMERGENCY_MILLIS) {
if (emergency1)
emergency_state |= LL_EMERGENCY_BIT_LIFT1;
if (emergency2)
emergency_state |= LL_EMERGENCY_BIT_LIFT2;
}
if (button_emergency_started > 0 && (millis() - button_emergency_started) >= BUTTON_EMERGENCY_MILLIS)
{
if (emergency3)
emergency_state |= LL_EMERGENCY_BIT_STOP1;
if (emergency4)
emergency_state |= LL_EMERGENCY_BIT_STOP2;
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) {
Expand All @@ -214,8 +202,7 @@ void updateEmergency() {
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 & LL_EMERGENCY_BIT_LATCH))
{
if (last_emergency != (emergency_state & LL_EMERGENCY_BIT_LATCH)) {
sendMessage(&status_message, sizeof(struct ll_status));

// Update UI instantly
Expand Down

0 comments on commit fe6cbc5

Please sign in to comment.