From 254c8244799aa9955c534058cf7c5dee0c4a400a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=B6rg=20Ebeling?= Date: Sun, 18 Aug 2024 13:23:33 +0200 Subject: [PATCH 1/2] Simplify status bit handling of ESC_POWER and add roll-, pitch- & tilt_angle debug output --- Firmware/LowLevel/src/datatypes.h | 3 ++- Firmware/LowLevel/src/main.cpp | 14 ++++++++++---- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/Firmware/LowLevel/src/datatypes.h b/Firmware/LowLevel/src/datatypes.h index 96494114..5df7987f 100644 --- a/Firmware/LowLevel/src/datatypes.h +++ b/Firmware/LowLevel/src/datatypes.h @@ -47,7 +47,8 @@ enum HighLevelMode { #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 +#define LL_STATUS_BIT_ESC_POWER 0b1 << 3 +#define LL_STATUS_BIT_UI_AVAIL 0b1 << 7 #pragma pack(push, 1) struct ll_status { diff --git a/Firmware/LowLevel/src/main.cpp b/Firmware/LowLevel/src/main.cpp index b4e02088..bfbb84ea 100644 --- a/Firmware/LowLevel/src/main.cpp +++ b/Firmware/LowLevel/src/main.cpp @@ -38,7 +38,7 @@ #define SHUTDOWN_ESC_MAX_PITCH 15.0 // Do not shutdown ESCs if absolute pitch angle is greater than this // Define to stream debugging messages via USB -// #define USB_DEBUG +#define USB_DEBUG // Only define DEBUG_SERIAL if USB_DEBUG is actually enabled. // This enforces compile errors if it's used incorrectly. @@ -703,13 +703,13 @@ void loop() { if(!ROS_running || last_high_level_state.current_mode != HighLevelMode::MODE_IDLE || fabs(pitch_angle) > SHUTDOWN_ESC_MAX_PITCH) { // Enable escs if not idle, or if ROS is not running, or on a slope digitalWrite(PIN_ESC_SHUTDOWN, LOW); - status_message.status_bitmask |= 0b1000; + status_message.status_bitmask |= LL_STATUS_BIT_ESC_POWER; } else { digitalWrite(PIN_ESC_SHUTDOWN, HIGH); - status_message.status_bitmask &= 0b11110111; + status_message.status_bitmask &= ~LL_STATUS_BIT_ESC_POWER; } #else - status_message.status_bitmask |= 0b1000; + status_message.status_bitmask |= LL_STATUS_BIT_ESC_POWER; #endif status_message.status_bitmask = (status_message.status_bitmask & 0b11111011) | ((charging_allowed & 0b1) << 2); @@ -743,6 +743,12 @@ void loop() { DEBUG_SERIAL.print(" A\t"); DEBUG_SERIAL.print("emergency: 0b"); DEBUG_SERIAL.print(status_message.emergency_bitmask, BIN); + DEBUG_SERIAL.print("\troll: "); + DEBUG_SERIAL.print(roll_angle, 3); + DEBUG_SERIAL.print("\tpitch: "); + DEBUG_SERIAL.print(pitch_angle, 3); + DEBUG_SERIAL.print("\ttilt: "); + DEBUG_SERIAL.print(tilt_angle, 3); DEBUG_SERIAL.println(); #endif } From 90adad4ba79df897f2e1b167483f59ce391db105 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=B6rg=20Ebeling?= Date: Sun, 18 Aug 2024 14:02:10 +0200 Subject: [PATCH 2/2] Switch from pitch- to tilt_angle and change shutdown logic --- Firmware/LowLevel/src/main.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/Firmware/LowLevel/src/main.cpp b/Firmware/LowLevel/src/main.cpp index bfbb84ea..7ce2cf04 100644 --- a/Firmware/LowLevel/src/main.cpp +++ b/Firmware/LowLevel/src/main.cpp @@ -36,9 +36,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 SHUTDOWN_ESC_MAX_PITCH 15.0 // Do not shutdown ESCs if absolute pitch angle is greater than this +#define SHUTDOWN_ESC_MAX_TILT 15.0f // Shutdown ESCs if tilt angle is greater than this // Define to stream debugging messages via USB -#define USB_DEBUG +// #define USB_DEBUG // Only define DEBUG_SERIAL if USB_DEBUG is actually enabled. // This enforces compile errors if it's used incorrectly. @@ -699,9 +699,9 @@ void loop() { #endif #ifdef SHUTDOWN_ESC_WHEN_IDLE - // ESC power saving when mower is IDLE - if(!ROS_running || last_high_level_state.current_mode != HighLevelMode::MODE_IDLE || fabs(pitch_angle) > SHUTDOWN_ESC_MAX_PITCH) { - // Enable escs if not idle, or if ROS is not running, or on a slope + // ESC power saving and safety shutdown + if ((!ROS_running || last_high_level_state.current_mode != HighLevelMode::MODE_IDLE) && tilt_angle <= SHUTDOWN_ESC_MAX_TILT) { + // Enable ESCs if ROS is not running, or not idle, but in any case only if not tilted digitalWrite(PIN_ESC_SHUTDOWN, LOW); status_message.status_bitmask |= LL_STATUS_BIT_ESC_POWER; } else {