Enlargeable Config Packet

Firmware/LowLevel/platformio.ini

@@ -23,6 +23,7 @@ lib_deps =
     Wire
     SPI
     FastCRC
+    LittleFS
     bakercp/PacketSerial@^1.4.0
     powerbroker2/FireTimer@^1.0.5
     https://github.com/ClemensElflein/NeoPixelConnect.git

Firmware/LowLevel/src/datatypes.h

@@ -60,7 +60,7 @@ struct ll_status {
     // Bit 0: Initialized (i.e. setup() was a success). If this is 0, all other bits are meaningless.
     // Bit 1: Raspberry Power
     // Bit 2: Charging enabled
-    // Bit 3: don't care
+    // Bit 3: don't care (reserved for ESC shutdown PR)
     // Bit 4: Rain detected
     // Bit 5: Sound available
     // Bit 6: Sound busy
@@ -74,6 +74,7 @@ struct ll_status {
     // Bit 2: Emergency/Hall 4 (Stop2) active
     // Bit 3: Emergency/Hall 1 (Lift1) active
     // Bit 4: Emergency/Hall 2 (Lift2) active
+    // Bit 5: Not an emergency but probably usable for pause via SA Handle?
     uint8_t emergency_bitmask;
     // Charge voltage
     float v_charge;
@@ -135,22 +136,66 @@ struct ll_ui_event {
 } __attribute__((packed));
 #pragma pack(pop)
 
-#define LL_HIGH_LEVEL_CONFIG_MAX_COMMS_VERSION 1           // Max. comms packet version supported by this open_mower LL FW
-#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_EMERGENCY_INVERSE 1 << 1  // Sample, for possible future usage, i.e. for SA-Type emergency
+#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 : unsigned int {
+    OFF = 0,
+    LIFT_TILT,  // Wheel lifted and/or wheels tilted functionality
+    STOP,       // Stop mower
+    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 : 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 : 3;
+    HallLevel level : 1;
+} __attribute__((packed));
+#pragma pack(pop)
+
+#define MAX_HALL_INPUTS 10  // How much Hall-inputs we support. 4 * OM + 6 * Stock-CoverUI + 0 spare (because not yet required to make it fixed)
+
+// LL/HL config packet, bi-directional, flexible-length, with defaults for YF-C500.
 #pragma pack(push, 1)
 struct ll_high_level_config {
-    uint8_t type;
-    uint8_t comms_version = LL_HIGH_LEVEL_CONFIG_MAX_COMMS_VERSION;  // Increasing comms packet-version number for packet compatibility (n > 0)
-    uint8_t config_bitmask = 0;                                      // See LL_HIGH_LEVEL_CONFIG_BIT_*
-    int8_t volume;                                                   // Volume (0-100%) feedback (if directly changed via CoverUI)
-    iso639_1 language;                                               // ISO 639-1 (2-char) language code (en, de, ...)
-    uint16_t spare1 = 0;                                             // Spare for future use
-    uint16_t spare2 = 0;                                             // Spare for future use
-    uint16_t crc;
+    // ATTENTION: This is a flexible length struct. It is allowed to grow independently to HL without loosing compatibility,
+    //    but never change or restructure already published member, except you really know their consequences.
+
+    // 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 = 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::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 default to off
+    };
+    // INFO: Before adding a new member here: Decide if and how much hall_configs spares do we like to have
+
+    // uint16_t crc;  Just for illustration, that it get appended, but only within the wire buffer
 } __attribute__((packed));
 #pragma pack(pop)
 

Firmware/LowLevel/src/main.cpp

@@ -14,14 +14,18 @@
 // SOFTWARE.
 //
 //
-#include <NeoPixelConnect.h>
 #include <Arduino.h>
 #include <FastCRC.h>
+#include <LittleFS.h>
+#include <NeoPixelConnect.h>
 #include <PacketSerial.h>
+
+#include "config_defaults.h"
 #include "datatypes.h"
+#include "debug.h"
+#include "imu.h"
 #include "pins.h"
 #include "ui_board.h"
-#include "imu.h"
 
 #ifdef ENABLE_SOUND_MODULE
 #include <soundsystem.h>
@@ -32,8 +36,6 @@
 #define UI_GET_VERSION_CYCLETIME 5000 // cycletime for UI Get_Version request (UI available check)
 #define UI_GET_VERSION_TIMEOUT 100    // timeout for UI Get_Version response (UI available check)
 
-#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 BUTTON_EMERGENCY_MILLIS 20 // Time for button emergency to activate. This is to debounce the button.
 
 // Define to stream debugging messages via USB
@@ -62,12 +64,6 @@ SerialPIO uiSerial(PIN_UI_TX, PIN_UI_RX, 250);
 #define R_SHUNT 0.003f
 #define CURRENT_SENSE_GAIN 100.0f
 
-#define BATT_ABS_MAX 28.7f
-#define BATT_ABS_Min 21.7f
-
-#define BATT_FULL BATT_ABS_MAX - 0.3f
-#define BATT_EMPTY BATT_ABS_Min + 0.3f
-
 // Emergency will be engaged, if no heartbeat was received in this time frame.
 #define HEARTBEAT_MILLIS 500
 
@@ -123,15 +119,16 @@ uint16_t ui_version = 0;                   // Last received UI firmware version
 uint8_t ui_topic_bitmask = Topic_set_leds; // UI subscription, default to Set_LEDs
 uint16_t ui_interval = 1000;               // UI send msg (LED/State) interval (ms)
 
-// Some vars related to PACKET_ID_LL_HIGH_LEVEL_CONFIG_*
-uint8_t comms_version = 0;  // comms packet version (>0 if implemented)
-uint8_t config_bitmask = 0; // See LL_HIGH_LEVEL_CONFIG_BIT_*
+struct ll_high_level_config llhl_config;  // LL/HL configuration (is initialized with YF-C500 defaults)
+uint16_t config_crc_in_flash = 0;
 
 void sendMessage(void *message, size_t size);
 void sendUIMessage(void *message, size_t size);
 void onPacketReceived(const uint8_t *buffer, size_t size);
 void onUIPacketReceived(const uint8_t *buffer, size_t size);
 void manageUISubscriptions();
+void saveConfigToFlash();
+void readConfigFromFlash();
 
 void setRaspiPower(bool power) {
     // Update status bits in the status message
@@ -372,9 +369,7 @@ void setup() {
     //  Therefore, we pause the other core until setup() was a success
     rp2040.idleOtherCore();
 
-#ifdef USB_DEBUG
-    DEBUG_SERIAL.begin(9600);
-#endif
+    DEBUG_BEGIN(9600);
 
     emergency_latch = true;
     ROS_running = false;
@@ -424,6 +419,10 @@ void setup() {
     UISerial.setStream(&UI1_SERIAL);
     UISerial.setPacketHandler(&onUIPacketReceived);
 
+    // Initialize flash and try to read config
+    LittleFS.begin();
+    readConfigFromFlash();
+
     /*
      * IMU INITIALIZATION
      */
@@ -548,12 +547,37 @@ void onUIPacketReceived(const uint8_t *buffer, size_t size) {
 }
 
 void sendConfigMessage(uint8_t pkt_type) {
-    struct ll_high_level_config ll_config;
-    ll_config.type = pkt_type;
-    ll_config.config_bitmask = config_bitmask;
-    ll_config.volume = 80;            // FIXME: Adapt once nv_config or improve-sound got merged
-    strncpy(ll_config.language, "en", 2); // FIXME: Adapt once nv_config or improve-sound got merged
-    sendMessage(&ll_config, sizeof(struct ll_high_level_config));
+    size_t msg_size = sizeof(struct ll_high_level_config) + 3;  // + 1 type + 2 crc
+    uint8_t *msg = (uint8_t *)malloc(msg_size);
+    if (msg == NULL)
+        return;
+    *msg = pkt_type;
+    memcpy(msg + 1, &llhl_config, sizeof(struct ll_high_level_config));  // Copy our live config into the message, behind type
+    sendMessage(msg, msg_size);
+    free(msg);
+}
+
+void applyConfig(const uint8_t *buffer, size_t size) {
+    // This is a flexible length package where the size may vary when ll_high_level_config struct got enhanced only on one side.
+    // If payload size is larger than our struct size, ensure that we only copy those we know of = our struct size.
+    // If payload size is smaller than our struct size, copy only the payload we got, but ensure that the unsent member(s) have reasonable defaults.
+    size_t payload_size = min(sizeof(ll_high_level_config), size - 2);  // exclude crc
+
+    // Use a temporary config for easier sanity adaption and copy our live config, which has at least reasonable defaults.
+    // The live config copy ensures that we've reasonable values for the case that HL config struct is older (smaller) than ours.
+    auto tmp_config = llhl_config;
+
+    // Copy payload to temporary config
+    memcpy(&tmp_config, buffer, payload_size);
+
+    // Sanity
+    tmp_config.v_charge_cutoff = min(tmp_config.v_charge_cutoff, V_CHARGE_ABS_MAX);  // Fix exceed of hardware limits
+    tmp_config.i_charge_cutoff = min(tmp_config.i_charge_cutoff, I_CHARGE_ABS_MAX);  // Fix exceed of hardware limits
+
+    // Make config live
+    llhl_config = tmp_config;
+
+    // PR-80: Assign volume & language if not already stored in flash-config
 }
 
 void onPacketReceived(const uint8_t *buffer, size_t size) {
@@ -590,17 +614,11 @@ void onPacketReceived(const uint8_t *buffer, size_t size) {
     } 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) && size == sizeof(struct ll_high_level_config))
-    {
-        // Read and handle received config
-        struct ll_high_level_config *pkt = (struct ll_high_level_config *)buffer;
-        if (pkt->comms_version <= LL_HIGH_LEVEL_CONFIG_MAX_COMMS_VERSION)
-            comms_version = pkt->comms_version;
-        else
-            comms_version = LL_HIGH_LEVEL_CONFIG_MAX_COMMS_VERSION;
-        config_bitmask = pkt->config_bitmask; // Take over as sent. HL is leading (for now)
-        // FIXME: Assign volume & language if not already stored in flash-config
+    } 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
+
+        // Store in flash
+        saveConfigToFlash();
 
         if (buffer[0] == PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ)
             sendConfigMessage(PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP);
@@ -609,7 +627,7 @@ void onPacketReceived(const uint8_t *buffer, size_t size) {
 
 // returns true, if it's a good idea to charge the battery (current, voltages, ...)
 bool checkShouldCharge() {
-    return status_message.v_charge < 30.0 && status_message.charging_current < 1.5 && status_message.v_battery < 29.0;
+    return status_message.v_charge < llhl_config.v_charge_cutoff && status_message.charging_current < llhl_config.i_charge_cutoff && status_message.v_battery < llhl_config.v_battery_cutoff;
 }
 
 void updateChargingEnabled() {
@@ -790,3 +808,43 @@ void sendUIMessage(void *message, size_t size) {
 
     UISerial.send((uint8_t *) message, size);
 }
+
+void saveConfigToFlash() {
+    uint16_t crc = CRC16.ccitt((const uint8_t*) &llhl_config, sizeof(llhl_config));
+    if (crc == config_crc_in_flash) return;
+
+    File f = LittleFS.open(CONFIG_FILENAME, "w");
+    f.write((const uint8_t*) &llhl_config, sizeof(llhl_config));
+    f.write((const uint8_t*) &crc, 2);
+    f.close();
+}
+
+void readConfigFromFlash() {
+    File f = LittleFS.open(CONFIG_FILENAME, "r");
+    if (!f) return;
+
+    // sanity check for CRC to work (1 data, 2 CRC)
+    const size_t size = f.size();
+    if (size < 3) {
+        f.close();
+        return;
+    }
+
+    // read config
+    uint8_t *buffer = (uint8_t *)malloc(f.size());
+    f.read(buffer, size);
+    if (buffer == NULL)
+        return;
+    f.close();
+
+    // check the CRC
+    uint16_t crc = CRC16.ccitt(buffer, size - 2);
+
+    if (buffer[size - 1] != ((crc >> 8) & 0xFF) ||
+        buffer[size - 2] != (crc & 0xFF))
+        return;
+
+    config_crc_in_flash = crc;
+    applyConfig(buffer, size);
+    free(buffer);
+}