diff --git a/Firmware/LowLevel/src/datatypes.h b/Firmware/LowLevel/src/datatypes.h index 440b26e6..0222c954 100644 --- a/Firmware/LowLevel/src/datatypes.h +++ b/Firmware/LowLevel/src/datatypes.h @@ -136,10 +136,11 @@ 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_IGNORE_CHARGING_CURRENT (1 << 2) // Ignore charging current -#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 +#define LL_HIGH_LEVEL_CONFIG_BIT_HL_IS_LEADING (LL_HIGH_LEVEL_CONFIG_BIT_DFPIS5V | LL_HIGH_LEVEL_CONFIG_BIT_BACKGROUND_SOUNDS | LL_HIGH_LEVEL_CONFIG_BIT_IGNORE_CHARGING_CURRENT) typedef char iso639_1[2]; // Two char ISO 639-1 language code @@ -176,15 +177,15 @@ struct ll_high_level_config { 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) + float v_charge_cutoff = 30.0f; // Protective max. charging voltage before charging get switched off (-1 = unknown) + float i_charge_cutoff = 1.5f; // Protective max. charging current before charging get switched off (-1 = unknown) + float v_battery_cutoff = 29.0f; // Protective max. battery voltage before charging get switched off (-1 = unknown) + float v_battery_empty = 21.7f + 0.3f; // Empty battery voltage used for % calc of capacity (-1 = unknown) + float v_battery_full = 28.7f - 0.3f; // Full battery voltage used for % calc of capacity (-1 = 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, ...) + float shutdown_esc_max_pitch = 0.0f; // Do not shutdown ESCs if absolute pitch angle is greater than this (0 = disable, 0xffff = unknown) (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::LIFT_TILT, HallLevel::ACTIVE_LOW}, // [0] OM Hall-1 input (Lift1) diff --git a/Firmware/LowLevel/src/main.cpp b/Firmware/LowLevel/src/main.cpp index 4c43eb84..8295bd1b 100644 --- a/Firmware/LowLevel/src/main.cpp +++ b/Firmware/LowLevel/src/main.cpp @@ -572,32 +572,29 @@ void applyConfig(const uint8_t *buffer, const size_t size) { // Copy payload to temporary config memcpy(&hl_config, buffer, payload_size); - // Takeover of HL leading config values + // Takeover HL leading config_bitmask values. This is only to illustrate that we may mix HL as well as LL leading bits here (if required some when). + // FIXME: Check if bitfields would look better llhl_config.config_bitmask = (llhl_config.config_bitmask & ~LL_HIGH_LEVEL_CONFIG_BIT_HL_IS_LEADING) | (hl_config.config_bitmask & LL_HIGH_LEVEL_CONFIG_BIT_HL_IS_LEADING); - // Set HL leading member if they're not "undefined" - // TODO: Not that nice :-/ Is there no more clever way? Even with a templated function, I do see similar 8 lines here - + // Take over HL members if they're not "undefined" if (hl_config.rain_threshold != 0xffff) llhl_config.rain_threshold = hl_config.rain_threshold; - if (!std::isnan(hl_config.v_charge_cutoff)) llhl_config.v_charge_cutoff = min(hl_config.v_charge_cutoff, 40.0f); // Absolute max. limited by D2/D3 Schottky - if (!std::isnan(hl_config.i_charge_cutoff)) llhl_config.i_charge_cutoff = min(hl_config.i_charge_cutoff, 5.0f); // Absolute max. limited by D2/D3 Schottky - if (!std::isnan(hl_config.v_battery_cutoff)) llhl_config.v_battery_cutoff = hl_config.v_battery_cutoff; - if (!std::isnan(hl_config.v_battery_empty)) llhl_config.v_battery_empty = hl_config.v_battery_empty; - if (!std::isnan(hl_config.v_battery_full)) llhl_config.v_battery_full = hl_config.v_battery_full; - if (hl_config.lift_period != 0xffff) hl_config.lift_period = hl_config.lift_period; - if (hl_config.tilt_period != 0xffff) hl_config.tilt_period = hl_config.tilt_period; - - // HL is always leading for language - strncpy(llhl_config.language, hl_config.language, 2); + if (hl_config.v_charge_cutoff >= 0) llhl_config.v_charge_cutoff = min(hl_config.v_charge_cutoff, 40.0f); // Absolute max. limited by D2/D3 Schottky + if (hl_config.i_charge_cutoff >= 0) llhl_config.i_charge_cutoff = min(hl_config.i_charge_cutoff, 5.0f); // Absolute max. limited by D2/D3 Schottky + if (hl_config.v_battery_cutoff >= 0) llhl_config.v_battery_cutoff = hl_config.v_battery_cutoff; + if (hl_config.v_battery_empty >= 0) llhl_config.v_battery_empty = hl_config.v_battery_empty; + if (hl_config.v_battery_full >= 0) llhl_config.v_battery_full = hl_config.v_battery_full; + if (hl_config.lift_period != 0xffff) llhl_config.lift_period = hl_config.lift_period; + if (hl_config.tilt_period != 0xffff) llhl_config.tilt_period = hl_config.tilt_period; + if (hl_config.volume != 0xff) llhl_config.volume = hl_config.volume; - // HL send 0xff if it doesn't provide a volume - if (hl_config.volume != 0xff) llhl_config.volume = llhl_config.volume; + strncpy(llhl_config.language, hl_config.language, 2); // HL always force the language // Take over those hall inputs which are not undefined for (size_t i = 0; i < MAX_HALL_INPUTS; i++) { if (hl_config.hall_configs[i].mode != HallMode::UNDEFINED) llhl_config.hall_configs[i] = hl_config.hall_configs[i]; } + // FIXME: TODO: Hall/emergency implementation } void onPacketReceived(const uint8_t *buffer, size_t size) { @@ -627,21 +624,18 @@ void onPacketReceived(const uint8_t *buffer, size_t size) { if (!ROS_running) { // ROS is running (again (i.e. due to restart after reconfiguration)) ROS_running = true; - - // Send current LL config (and request HL config response) - sendConfigMessage(PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ); } } else if (buffer[0] == PACKET_ID_LL_HIGH_LEVEL_STATE && size == sizeof(struct ll_high_level_state)) { // copy the state last_high_level_state = *((struct ll_high_level_state *) buffer); } else if (buffer[0] == PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ || buffer[0] == PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP) { - applyConfig(buffer + 1, size - 1); // Skip type + applyConfig(buffer + 1, size - 1); // Skip type // Store in flash saveConfigToFlash(); if (buffer[0] == PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ) - sendConfigMessage(PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP); + sendConfigMessage(PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP); // Other side requested a config response } }