From 0075f68f2e3895b81aa5c7ab4c170bb7a42971e8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=B6rg=20Ebeling?= Date: Mon, 14 Oct 2024 22:19:31 +0200 Subject: [PATCH] Intermediate result after protocol implementation with ROS --- Firmware/LowLevel/include/config_defaults.h | 28 ------------ Firmware/LowLevel/include/debug.h | 42 ------------------ Firmware/LowLevel/src/datatypes.h | 47 +++++++++++---------- 3 files changed, 25 insertions(+), 92 deletions(-) delete mode 100644 Firmware/LowLevel/include/config_defaults.h delete mode 100644 Firmware/LowLevel/include/debug.h diff --git a/Firmware/LowLevel/include/config_defaults.h b/Firmware/LowLevel/include/config_defaults.h deleted file mode 100644 index 9e6044fb..00000000 --- a/Firmware/LowLevel/include/config_defaults.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef _CONFIG_H -#define _CONFIG_H - -#define BATT_ABS_MAX 28.7f -#define BATT_ABS_MIN 21.7f - -#define BATT_EMPTY BATT_ABS_MIN + 0.3f -#define BATT_FULL BATT_ABS_MAX - 0.3f - -#define V_BATT_CUTOFF 29.0f - -#define TILT_EMERGENCY_MILLIS 2500 // Time for a single wheel to be lifted in order to count as emergency (0 disable). This is to filter uneven ground. -#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 V_CHARGE_MAX 30.0f // Default YF-C500 max. charge voltage -#define I_CHARGE_MAX 1.5f // Default YF-C500 max. charge current - -#define V_CHARGE_ABS_MAX 40.0f // Absolute max. limited by D2/D3 Schottky -#define I_CHARGE_ABS_MAX 5.0f // Absolute max. limited by D2/D3 Schottky - -#define RAIN_THRESHOLD 700U // (Yet) Stock-CoverUI exclusive raw ADC value. Is the measured value is lower, it rains - -#define SOUND_VOLUME 80 // Volume (0-100%) -#define LANGUAGE 'e', 'n' // ISO 639-1 (2-char) language code (en, de, ...) - -#define CONFIG_FILENAME "/openmower.cfg" // Where our llhl_config get saved in LittleFS (flash) - -#endif // _CONFIG_H \ No newline at end of file diff --git a/Firmware/LowLevel/include/debug.h b/Firmware/LowLevel/include/debug.h deleted file mode 100644 index 407a0052..00000000 --- a/Firmware/LowLevel/include/debug.h +++ /dev/null @@ -1,42 +0,0 @@ -// Created by Apehaenger on 02/02/23. -// -// This work is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License. -// -// Feel free to use the design in your private/educational projects, but don't try to sell the design or products based on it without getting my consent first. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -// -// -#ifndef _DEBUG_H_ -#define _DEBUG_H_ - -// Define to stream debugging messages via USB -//#define USB_DEBUG -#define DEBUG_PREFIX "" // You may define a debug prefix string - -#ifdef USB_DEBUG -#define DEBUG_SERIAL Serial - -// Some bloody simple debug macros which superfluous '#ifdef USB_DEBUG' ... -#define DEBUG_BEGIN(b) \ - DEBUG_SERIAL.begin(b); \ - while (!DEBUG_SERIAL) \ - ; -#define DEBUG_PRINTF(fmt, ...) \ - do \ - { \ - DEBUG_SERIAL.printf(DEBUG_PREFIX fmt, ##__VA_ARGS__); \ - } while (0) -#else -#define DEBUG_BEGIN(b) -#define DEBUG_PRINTF(fmt, ...) -#endif - - -#endif // _DEBUG_H_ diff --git a/Firmware/LowLevel/src/datatypes.h b/Firmware/LowLevel/src/datatypes.h index a985c5c6..440b26e6 100644 --- a/Firmware/LowLevel/src/datatypes.h +++ b/Firmware/LowLevel/src/datatypes.h @@ -19,7 +19,6 @@ #define _DATATYPES_H #include -#include "config_defaults.h" #define PACKET_ID_LL_STATUS 1 #define PACKET_ID_LL_IMU 2 @@ -137,27 +136,30 @@ struct ll_ui_event { } __attribute__((packed)); #pragma pack(pop) -#define LL_HIGH_LEVEL_CONFIG_BIT_DFPIS5V 1 << 0 // Enable full sound via mower_config env var "OM_DFP_IS_5V" -#define LL_HIGH_LEVEL_CONFIG_BIT_BACKGROUND_SOUNDS 1 << 1 // Enable background sounds +#define LL_HIGH_LEVEL_CONFIG_BIT_DFPIS5V (1 << 0) // Enable full sound via mower_config env var "OM_DFP_IS_5V" +#define LL_HIGH_LEVEL_CONFIG_BIT_BACKGROUND_SOUNDS (1 << 1) // Enable background sounds + +#define LL_HIGH_LEVEL_CONFIG_BIT_HL_IS_LEADING ((LL_HIGH_LEVEL_CONFIG_BIT_DFPIS5V) | (LL_HIGH_LEVEL_CONFIG_BIT_BACKGROUND_SOUNDS)) // Config bits where HL is leading typedef char iso639_1[2]; // Two char ISO 639-1 language code -enum class HallMode : uint8_t { +enum class HallMode : unsigned int { OFF = 0, - EMERGENCY, // Triggers emergency with lift & tilt functionality + LIFT_TILT, // Wheel lifted and/or wheels tilted functionality STOP, // Stop mower - PAUSE // Pause the mower (not yet implemented in ROS) + PAUSE, // Pause the mower (not yet implemented in ROS) + UNDEFINED // This is used by foreign side to inform that it doesn't has a configuration for this sensor }; // FIXME: Decide later which is more comfortable, activeLow = 0 | 1 -enum class HallLevel : uint8_t { +enum class HallLevel : unsigned int { ACTIVE_LOW = 0, // If Hall-Sensor (or button) is active/triggered we've this level on our GPIO ACTIVE_HIGH }; #pragma pack(push, 1) struct HallConfig { - HallMode mode : 4; + HallMode mode : 3; HallLevel level : 1; } __attribute__((packed)); #pragma pack(pop) @@ -172,23 +174,24 @@ struct ll_high_level_config { // uint8_t type; Just for illustration. Get set in wire buffer with type PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ or PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP - uint8_t config_bitmask = 0; // See LL_HIGH_LEVEL_CONFIG_BIT_* - uint16_t rain_threshold = RAIN_THRESHOLD; // If (stock CoverUI) rain value < rain_threshold then it rains. Expected to differ between C500, SA and SC types - float v_charge_cutoff = V_CHARGE_MAX; // Protective max. charging voltage before charging get switched off - float i_charge_cutoff = I_CHARGE_MAX; // Protective max. charging current before charging get switched off - float v_battery_cutoff = V_BATT_CUTOFF; // Protective max. battery voltage before charging get switched off - float v_battery_empty = BATT_EMPTY; // Empty battery voltage used for % calc of capacity - float v_battery_full = BATT_FULL; // Full battery voltage used for % calc of capacity - uint16_t lift_period = LIFT_EMERGENCY_MILLIS; // Period (ms) for both wheels to be lifted in order to count as emergency (0 disable, 0xFFFF do not change). This is to filter uneven ground - uint16_t tilt_period = TILT_EMERGENCY_MILLIS; // Period (ms) for a single wheel to be lifted in order to count as emergency (0 disable, 0xFFFF do not change). This is to filter uneven ground - iso639_1 language = {LANGUAGE}; // ISO 639-1 (2-char) language code (en, de, ...) - int8_t volume = SOUND_VOLUME; // Volume (0-100%) feedback (if directly changed i.e. via CoverUI or WebApp) + uint8_t config_bitmask = 0; // See LL_HIGH_LEVEL_CONFIG_BIT_* + uint16_t rain_threshold = 700; // If (stock CoverUI) rain value < rain_threshold then it rains. Expected to differ between C500, SA and SC types (0xFFFF = unknown) + float v_charge_cutoff = 30.0f; // Protective max. charging voltage before charging get switched off (NAN = unknown) + float i_charge_cutoff = 1.5f; // Protective max. charging current before charging get switched off (NAN = unknown) + float v_battery_cutoff = 29.0f; // Protective max. battery voltage before charging get switched off (NAN = unknown) + float v_battery_empty = 21.7f + 0.3f; // Empty battery voltage used for % calc of capacity (NAN = unknown) + float v_battery_full = 28.7f - 0.3f; // Full battery voltage used for % calc of capacity (NAN = unknown) + uint16_t lift_period = 100; // Period (ms) for both wheels to be lifted in order to count as emergency (0 = disable, 0xFFFF = unknown). This is to filter uneven ground + uint16_t tilt_period = 2500; // Period (ms) for a single wheel to be lifted in order to count as emergency (0 = disable, 0xFFFF = unknown). This is to filter uneven ground + float shutdown_esc_max_pitch = 15.0f; // Do not shutdown ESCs if absolute pitch angle is greater than this (to be implemented) + iso639_1 language = {'e', 'n'}; // ISO 639-1 (2-char) language code (en, de, ...) + uint8_t volume = 80; // Volume (0-100%) feedback (if directly changed i.e. via CoverUI or WebApp) (0xff = do not change) HallConfig hall_configs[MAX_HALL_INPUTS] = { - {HallMode::EMERGENCY, HallLevel::ACTIVE_LOW}, // [0] OM Hall-1 input (Lift1) - {HallMode::EMERGENCY, HallLevel::ACTIVE_LOW}, // [1] OM Hall-2 input (Lift2) + {HallMode::LIFT_TILT, HallLevel::ACTIVE_LOW}, // [0] OM Hall-1 input (Lift1) + {HallMode::LIFT_TILT, HallLevel::ACTIVE_LOW}, // [1] OM Hall-2 input (Lift2) {HallMode::STOP, HallLevel::ACTIVE_LOW}, // [2] OM Hall-3 input (Stop1) {HallMode::STOP, HallLevel::ACTIVE_LOW}, // [3] OM Hall-4 input (Stop2) - // [4] Stock-CoverUI-1 ... [9] Stock-CoverUI-6 defaults to off + // [4] Stock-CoverUI-1 ... [9] Stock-CoverUI-6 default to off }; // INFO: Before adding a new member here: Decide if and how much hall_configs spares do we like to have