Implement flexible config req- & response- packages. See PR79 of open_mower_ros

Author: Apehaenger

Firmware/LowLevel/src/datatypes.h

@@ -23,6 +23,8 @@
 #define PACKET_ID_LL_STATUS 1
 #define PACKET_ID_LL_IMU 2
 #define PACKET_ID_LL_UI_EVENT 3
+#define PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ 0x21 // ll_high_level_config and request config from receiver
+#define PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP 0x22 // ll_high_level_config response
 #define PACKET_ID_LL_HEARTBEAT 0x42
 #define PACKET_ID_LL_HIGH_LEVEL_STATE 0x43
 
@@ -117,4 +119,23 @@ 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
+
+typedef char iso639_1[2]; // Two char ISO 639-1 language code
+
+#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;
+} __attribute__((packed));
+#pragma pack(pop)
+
 #endif

Firmware/LowLevel/src/main.cpp

@@ -123,6 +123,10 @@ 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_*
+
 void sendMessage(void *message, size_t size);
 void sendUIMessage(void *message, size_t size);
 void onPacketReceived(const uint8_t *buffer, size_t size);
@@ -556,6 +560,15 @@ 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
+    strcpy(ll_config.language, "en"); // FIXME: Adapt once nv_config or improve-sound got merged
+    sendMessage(&ll_config, sizeof(struct ll_high_level_config));
+}
+
 void onPacketReceived(const uint8_t *buffer, size_t size) {
     // sanity check for CRC to work (1 type, 1 data, 2 CRC)
     if (size < 4)
@@ -572,7 +585,6 @@ void onPacketReceived(const uint8_t *buffer, size_t size) {
 
         // CRC and packet is OK, reset watchdog
         last_heartbeat_millis = millis();
-        ROS_running = true;
         struct ll_heartbeat *heartbeat = (struct ll_heartbeat *) buffer;
         if (heartbeat->emergency_release_requested) {
             emergency_latch = false;
@@ -581,10 +593,31 @@ void onPacketReceived(const uint8_t *buffer, size_t size) {
         if (heartbeat->emergency_requested) {
             emergency_latch = true;
         }
+        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) && 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
+
+        if (buffer[0] == PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ)
+            sendConfigMessage(PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP);
+    }
 }
 
 // returns true, if it's a good idea to charge the battery (current, voltages, ...)