HL->LL config packet with "DFP is 5V" implementation (#79)

* Initial proposal

* Added comms_version for packet versioning and compact code

* Switch to flexible config req- & response- packet type

src/mower_comms/src/ll_datatypes.h

@@ -23,10 +23,11 @@
 #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
 
-
 #pragma pack(push, 1)
 struct ll_status {
     // Type of this message. Has to be PACKET_ID_LL_STATUS.
@@ -109,4 +110,23 @@ struct ll_high_level_state {
 } __attribute__((packed));
 #pragma pack(pop)
 
+#define LL_HIGH_LEVEL_CONFIG_MAX_COMMS_VERSION 1           // Max. comms packet version supported by this open_mower_ros
+#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 = PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP;               // By default, respond only (for now)
+    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). -1 = don't change
+    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

src/mower_comms/src/mower_comms.cpp

@@ -38,6 +38,8 @@
 #include <xbot_msgs/WheelTick.h>
 #include "mower_msgs/HighLevelStatus.h"
 
+#include <bitset>
+
 ros::Publisher status_pub;
 ros::Publisher wheel_tick_pub;
 
@@ -65,6 +67,10 @@ float speed_l = 0, speed_r = 0, speed_mow = 0;
 double wheel_ticks_per_m = 0.0;
 double wheel_distance_m = 0.0;
 
+bool dfp_is_5v = false;       // DFP is set to 5V Vcc
+std::string language = "en";  // ISO-639-1 (2 char) language code
+int volume = -1;              // -1 = don't change, 0-100 = volume (%)
+
 // Serial port and buffer for the low level connection
 serial::Serial serial_port;
 uint8_t out_buf[1000];
@@ -228,6 +234,37 @@ void publishStatus() {
     wheel_tick_pub.publish(wheel_tick_msg);
 }
 
+void publishLowLevelConfig() {
+    if (!serial_port.isOpen() || !allow_send)
+        return;
+
+    struct ll_high_level_config config_pkt;
+
+    config_pkt.volume = volume;
+    for (unsigned int i = 0; i < sizeof(config_pkt.language) / sizeof(char); i++) {
+        config_pkt.language[i] = language[i];
+    }
+    // Set config_bitmask flags
+    (dfp_is_5v) ? config_pkt.config_bitmask |= LL_HIGH_LEVEL_CONFIG_BIT_DFPIS5V : config_pkt.config_bitmask &= ~LL_HIGH_LEVEL_CONFIG_BIT_DFPIS5V;
+
+    crc.reset();
+    crc.process_bytes(&config_pkt, sizeof(struct ll_high_level_config) - 2);
+    config_pkt.crc = crc.checksum();
+
+    size_t encoded_size = cobs.encode((uint8_t *)&config_pkt, sizeof(struct ll_high_level_config), out_buf);
+    out_buf[encoded_size] = 0;
+    encoded_size++;
+
+    try {
+        ROS_INFO_STREAM("Send ll_high_level_config packet 0x" << std::hex << +config_pkt.type << " with comms_version=" << +config_pkt.comms_version
+                                                              << ", config_bitmask=0b" << std::bitset<8>(config_pkt.config_bitmask)
+                                                              << ", volume=" << std::dec << +config_pkt.volume << ", language='" << config_pkt.language << "'");
+        serial_port.write(out_buf, encoded_size);
+    } catch (std::exception &e) {
+        ROS_ERROR_STREAM("Error writing to serial port");
+    }
+}
+
 void publishActuatorsTimerTask(const ros::TimerEvent &timer_event) {
     publishActuators();
     publishStatus();
@@ -343,6 +380,25 @@ void handleLowLevelUIEvent(struct ll_ui_event *ui_event) {
 
 }
 
+void handleLowLevelConfig(struct ll_high_level_config *config_pkt) {
+    ROS_INFO_STREAM("Received ll_high_level_config packet 0x" << std::hex << +config_pkt->type
+                                                              << " with comms_version=" << +config_pkt->comms_version
+                                                              << ", config_bitmask=0b" << std::bitset<8>(config_pkt->config_bitmask)
+                                                              << ", volume=" << std::dec << +config_pkt->volume << ", language='" << config_pkt->language << "'");
+
+    // TODO: Handle announced comms_version once required
+
+    if (config_pkt->volume >= 0)
+        volume = config_pkt->volume;
+    language = config_pkt->language;
+
+
+    if (config_pkt->type == PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ ||                      // Config requested
+        config_pkt->config_bitmask & LL_HIGH_LEVEL_CONFIG_BIT_DFPIS5V != dfp_is_5v) {  // Our DFP_IS_5V setting is leading
+        publishLowLevelConfig();
+    }
+}
+
 void handleLowLevelStatus(struct ll_status *status) {
     std::unique_lock<std::mutex> lk(ll_status_mutex);
     last_ll_status = *status;
@@ -416,6 +472,10 @@ int main(int argc, char **argv) {
 
     speed_l = speed_r = speed_mow = 0;
 
+    paramNh.getParam("dfp_is_5v", dfp_is_5v);
+    paramNh.getParam("language", language);
+    paramNh.getParam("volume", volume);
+    ROS_INFO_STREAM("DFP is set to 5V [boolean]: " << dfp_is_5v << ", language: '" << language << "', volume: " << volume);
 
     // Setup XESC interfaces
     if(mowerParamNh.hasParam("xesc_type")) {
@@ -526,6 +586,15 @@ int main(int argc, char **argv) {
                                             "Low Level Board sent a valid packet with the wrong size. Type was UI_EVENT");
                                 }
                                 break;
+                            case PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ:
+                            case PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP:
+                                if (data_size == sizeof(struct ll_high_level_config)) {
+                                    handleLowLevelConfig((struct ll_high_level_config *)buffer_decoded);
+                                } else {
+                                    ROS_INFO_STREAM(
+                                        "Low Level Board sent a valid packet with the wrong size. Type was CONFIG_* (0x" << std::hex << buffer_decoded[0] << ")");
+                                }
+                                break;
                             default:
                                 ROS_INFO_STREAM("Got unknown packet from Low Level Board");
                                 break;

src/open_mower/config/mower_config.sh.example

@@ -55,6 +55,25 @@ export OM_MOWER_GAMEPAD="xbox360"
 # Output will be stored in your $HOME
 # export OM_ENABLE_RECORDING_ALL=False
 
+# Full Sound Support - But read on carefully:
+# Up to (and inclusive) OM hardware version 0.13.x, DFPlayer's power supply is set by default to 3.3V.
+# This is done by solder jumper "JP1" whose board track is by default at 3.3V.
+# If you manually opened the 3.3V track and solder a bridge to 5V, then you can indicate it here to get full sound support.
+# DO NOT enable "OM_DFP_IS_5V=True" if you haven't changed it in real. You might risk your "Raspberry Pico"!
+# And even if the Pico isn't expensive, it's a torture to replace it.
+# export OM_DFP_IS_5V=False
+#
+# Language as ISO-639-1 code string (currently only used by sound).
+# Overwrite a previously configure language (i.e. changed by CoverUI)
+# Supported languages: en|de
+# export OM_LANGUAGE="en"
+#
+# Sound volume (%)
+# Supported values:
+# 0-100 = Set sound volume on OM start, ignoring previously set volume level (i.e. by CoverUI)
+#    -1 = Don't change a previusoly set volume level (i.e. by CoverUI)
+# export OM_VOLUME=-1
+
 ################################
 ##        GPS Settings        ##
 ################################

src/open_mower/launch/include/_comms.launch

@@ -11,6 +11,9 @@
         <rosparam if="$(eval env('OM_MOWER')=='CUSTOM')" file="$(env HOME)/mower_params/comms_params.yaml"/>
         <param name="wheel_ticks_per_m" value="$(env OM_WHEEL_TICKS_PER_M)"/>
         <param name="wheel_distance_m" value="$(env OM_WHEEL_DISTANCE_M)"/>
+        <param name="dfp_is_5v" value="$(optenv OM_DFP_IS_5V False)"/>
+        <param name="language" value="$(optenv OM_LANGUAGE en)"/>
+        <param name="volume" value="$(optenv OM_VOLUME -1)"/>
     </node>
 
     <include file="$(find open_mower)/launch/include/_gps.launch" unless="$(optenv OM_NO_GPS False)">