From 685e7ef69fd02c202a8cbb9ca783213f0dadbdf8 Mon Sep 17 00:00:00 2001 From: Apehaenger Date: Sat, 6 Apr 2024 22:07:53 +0200 Subject: [PATCH] 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 | 22 +++++- src/mower_comms/src/mower_comms.cpp | 69 +++++++++++++++++++ src/open_mower/config/mower_config.sh.example | 19 +++++ src/open_mower/launch/include/_comms.launch | 3 + 4 files changed, 112 insertions(+), 1 deletion(-) diff --git a/src/mower_comms/src/ll_datatypes.h b/src/mower_comms/src/ll_datatypes.h index 17cdf77b..55418d0d 100644 --- a/src/mower_comms/src/ll_datatypes.h +++ b/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 \ No newline at end of file diff --git a/src/mower_comms/src/mower_comms.cpp b/src/mower_comms/src/mower_comms.cpp index 73c9c81d..847fd556 100644 --- a/src/mower_comms/src/mower_comms.cpp +++ b/src/mower_comms/src/mower_comms.cpp @@ -38,6 +38,8 @@ #include #include "mower_msgs/HighLevelStatus.h" +#include + 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 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; diff --git a/src/open_mower/config/mower_config.sh.example b/src/open_mower/config/mower_config.sh.example index bb737a80..0a05b204 100644 --- a/src/open_mower/config/mower_config.sh.example +++ b/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 ## ################################ diff --git a/src/open_mower/launch/include/_comms.launch b/src/open_mower/launch/include/_comms.launch index 66e54bfb..21354e05 100644 --- a/src/open_mower/launch/include/_comms.launch +++ b/src/open_mower/launch/include/_comms.launch @@ -11,6 +11,9 @@ + + +