diff --git a/config/mower_config.schema.json b/config/mower_config.schema.json index 70e7838a..b8b39ae8 100644 --- a/config/mower_config.schema.json +++ b/config/mower_config.schema.json @@ -58,6 +58,13 @@ "default": "xbox360", "description": "Select your gamepad. The cheap ones are usually xbox360.", "x-environment-variable": "OM_MOWER_GAMEPAD" + }, + "OM_IGNORE_CHARGING_CURRENT": { + "type": "boolean", + "default": false, + "title": "Ignore Charging Current", + "description": "Set to True if you're affected by a wrong IC2 sourcing. Read here https://openmower.de/docs/versions/errata/ic2-is-wrong/ for more details", + "x-environment-variable": "OM_IGNORE_CHARGING_CURRENT" } } }, @@ -506,6 +513,52 @@ "type": "number", "description": "Use perimeter sensor for docking. Set to 1 or 2 depending on the signal selected on the docking station. This is only supported by Mowgli builds for now.", "x-environment-variable": "OM_PERIMETER_SIGNAL" + }, + "OM_EMERGENCY_INPUT_CONFIG": { + "type": "string", + "default": "", + "description": "Comma separated list of emergency inputs (halls or stop), where each sensor can be configured in one of the following modes '[!]gnore|nused|top|Lift'", + "x-environment-variable": "OM_EMERGENCY_INPUT_CONFIG" + }, + "OM_EMERGENCY_LIFT_PERIOD": { + "type": "number", + "default": -1, + "description": "Period (ms) for >=2 wheels to be lifted in order to count as emergency (0 = disable)", + "x-environment-variable": "OM_EMERGENCY_LIFT_PERIOD" + }, + "OM_EMERGENCY_TILT_PERIOD": { + "type": "number", + "default": -1, + "description": "Period (ms) for a single wheel to be lifted in order to count as emergency (0 = disable)", + "x-environment-variable": "OM_EMERGENCY_TILT_PERIOD" + }, + "OM_CU_RAIN_THRESHOLD": { + "type": "number", + "default": -1, + "title": "Stock-CoverUI rain sensor threshold", + "description": "Stock-CoverUI limited rain sensor threshold, below which humidity/dryness value get identified as rain. As higher, as more dry. Default to 700", + "x-environment-variable": "OM_CU_RAIN_THRESHOLD" + }, + "OM_BATTERY_CRITICAL_HIGH_VOLTAGE": { + "type": "number", + "default": -1, + "title": "Max. battery voltage", + "description": "Max. battery voltage before charging get switched off", + "x-environment-variable": "OM_BATTERY_CRITICAL_HIGH_VOLTAGE" + }, + "OM_CHARGE_CRITICAL_HIGH_VOLTAGE": { + "type": "number", + "default": -1, + "title": "Max. charge voltage", + "description": "Max. charge voltage before charging get switched off", + "x-environment-variable": "OM_CHARGE_CRITICAL_HIGH_VOLTAGE" + }, + "OM_CHARGE_CRITICAL_HIGH_CURRENT": { + "type": "number", + "default": -1, + "title": "Max. charge current", + "description": "Max. charge current before charging get switched off", + "x-environment-variable": "OM_CHARGE_CRITICAL_HIGH_CURRENT" } } } diff --git a/config/mower_config.sh.example b/config/mower_config.sh.example index 98d5adfd..379a3b88 100644 --- a/config/mower_config.sh.example +++ b/config/mower_config.sh.example @@ -64,6 +64,11 @@ export OM_MOWER_GAMEPAD="xbox360" # Output will be stored in your $HOME # export OM_ENABLE_RECORDING_ALL=False +# Ignore Charging Current +# If you're affected by a wrong IC2 sourcing, read here https://openmower.de/docs/versions/errata/ic2-is-wrong/ +# for more details, you can disable the wrong charging current reading by disabling the charging protection via: +#export OM_IGNORE_CHARGING_CURRENT=True + # 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. @@ -196,6 +201,14 @@ export OM_BATTERY_EMPTY_VOLTAGE=24.0 # Immediate dock if voltage is critical export OM_BATTERY_CRITICAL_VOLTAGE=23.0 +# Absolute battery and charging limits, before charging get switched off +# Over-voltage battery protection +#export OM_BATTERY_CRITICAL_HIGH_VOLTAGE=29.0 +# Over-voltage charge protection +#export OM_CHARGE_CRITICAL_HIGH_VOLTAGE=30.0 +# Over-current charge protection +#export OM_CHARGE_CRITICAL_HIGH_CURRENT=1.5 + # Mower motor temperatures to stop and start mowing export OM_MOWING_MOTOR_TEMP_HIGH=80.0 export OM_MOWING_MOTOR_TEMP_LOW=40.0 @@ -226,6 +239,49 @@ export OM_RAIN_MODE=0 # How long to wait after rain to resume mowing when mode is "Dock Until Dry" export OM_RAIN_DELAY_MINUTES=30 +# Rain threshold (Stock-CoverUI limited parameter) +# A rain sensor threshold, below which humidity/dryness value get identified as rain. +# As higher, as more dry, defaults to 700. +#export OM_CU_RAIN_THRESHOLD=700 + +# Hall / Emergency input configuration +# +# This is an expert configuration option, use it only for custom builds! +# The normal YardForce Classic 500 user should leave all options in commented state. +# +# We do have 10 emergency inputs. 4 * OM-Hall1 up to OM-Hall4, plus 6 * Stock-CoverUI. +# +# Each emergency input can be configure in one of the following modes: +# I = Ignore this input. Use this mode for inputs which aren't connected to any sensor, or if you wish that this sensor get ignored +# U = Undefined (don't touch this input, leave it as it's by default) +# S = Stop. This input shall be used as "Stop" button input +# L = Lift. Use for an input with lift and tilt support like for wheel sensors +# +# In addition, each mode letter, can be prefixed with a "!" character, which indicate that this sensor shall be handled as "low-active" sensor +# +# All sensors which shall be configured, have to be placed in a comma separated list in the following order: +# OM-Hall1, OM-Hall2, OM-Hall3, OM-Hall4 +# If you've a FW-Moded Stock-CoverUI up to CoverUI FW 2.0x, with attached sensors/buttons, you can append the list with the following inputs: +# CoverUI-LIFT|LIFTX, Ignore, CoverUI-LBUMP|RBUMP, Ignore, CoverUI-Stop1, CoverUI-Stop2 +# For those with a CoverUI FW as of 2.1x the appendable list of inputs is: +# CoverUI-LIFT, CoverUI-LIFTX, CoverUI-LBUMP, CoverUI-RBUMP, CoverUI-Stop1, CoverUI-Stop2 +# +# Here's an example of a standard Classic 500 configuration where the sensors got attached as documented, +# which is Hall1+2 to the front wheel-lift sensors, and Hall3+4 to the top cover stop-button halls: +# export OM_EMERGENCY_INPUT_CONFIG="!L, !L, !S, !S" +# Yes, "!x" (low-active), because Classic 500 models, pull the input to low when they get triggered (whereas all CoverUI signals are "high-active") +# +#export OM_EMERGENCY_INPUT_CONFIG="" +# +# Lift period, to filter uneven ground +# Period (ms) for >=2 wheels to be lifted in order to count as emergency (0 = disable) +#export OM_EMERGENCY_LIFT_PERIOD="100" +# +# Tilt period, to filter uneven ground +# Period (ms) for a single wheel to be lifted in order to count as emergency (0 = disable) +#export OM_EMERGENCY_TILT_PERIOD="2500" + + ################################ ## External MQTT Broker ## ################################ diff --git a/src/mower_comms/CMakeLists.txt b/src/mower_comms/CMakeLists.txt index 426f4259..d9758829 100644 --- a/src/mower_comms/CMakeLists.txt +++ b/src/mower_comms/CMakeLists.txt @@ -8,6 +8,7 @@ project(mower_comms) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS + dynamic_reconfigure mower_msgs sensor_msgs roscpp diff --git a/src/mower_comms/src/ll_datatypes.h b/src/mower_comms/src/ll_datatypes.h index c347c182..21b33caf 100644 --- a/src/mower_comms/src/ll_datatypes.h +++ b/src/mower_comms/src/ll_datatypes.h @@ -24,8 +24,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_HIGH_LEVEL_CONFIG_REQ 0x11 // ll_high_level_config and request config from receiver +#define PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP 0x12 // ll_high_level_config response #define PACKET_ID_LL_HEARTBEAT 0x42 #define PACKET_ID_LL_HIGH_LEVEL_STATE 0x43 @@ -47,10 +47,8 @@ struct ll_status { float uss_ranges_m[5]; // Emergency bitmask: // Bit 0: Emergency latch - // Bit 1: Emergency 0 active - // Bit 2: Emergency 1 active - // Bit 3: Emergency 2 active - // Bit 4: Emergency 3 active + // Bit 1: Emergency/Lift (or tilt) + // Bit 2: Emergency/Stop uint8_t emergency_bitmask; // Charge voltage float v_charge; @@ -111,24 +109,74 @@ 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 +enum class OptionState : unsigned int { + OFF = 0, + ON, + UNDEFINED +}; + +#pragma pack(push, 1) +struct ConfigOptions { + OptionState dfp_is_5v : 2; + OptionState background_sounds : 2; + OptionState ignore_charging_current : 2; + // Need to block/waster the bits now, to be prepared for future enhancements + OptionState reserved_for_future_use1 : 2; + OptionState reserved_for_future_use2 : 2; + OptionState reserved_for_future_use3 : 2; + OptionState reserved_for_future_use4 : 2; + OptionState reserved_for_future_use5 : 2; +} __attribute__((packed)); +#pragma pack(pop) +static_assert(sizeof(ConfigOptions) == 2, "Changing size of ConfigOption != 2 will break packet compatibilty"); typedef char iso639_1[2]; // Two char ISO 639-1 language code +enum class HallMode : unsigned int { + OFF = 0, + LIFT_TILT, // Wheel lifted and wheels tilted functionality + STOP, // Stop mower + UNDEFINED // This is used by foreign side to inform that it doesn't has a configuration for this sensor +}; + +#pragma pack(push, 1) +struct HallConfig { + HallConfig(HallMode t_mode = HallMode::UNDEFINED, bool t_active_low = false) + : mode(t_mode), active_low(t_active_low) {}; + + HallMode mode : 3; // 1 bit reserved + bool active_low : 1; +} __attribute__((packed)); +#pragma pack(pop) + +#define MAX_HALL_INPUTS 10 // How much Hall-inputs we do support. 4 * OM + 6 * Stock-CoverUI + +// LL/HL config packet, bi-directional, flexible-length #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; + // 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 later in wire buffer with type PACKET_ID_LL_HIGH_LEVEL_CONFIG_* + + // clang-format off + ConfigOptions options = {.dfp_is_5v = OptionState::OFF, .background_sounds = OptionState::OFF, .ignore_charging_current = OptionState::OFF}; + uint16_t rain_threshold = 0xffff; // If (stock CoverUI) rain value < rain_threshold then it rains + float v_charge_cutoff = -1; // Protective max. charging voltage before charging get switched off (-1 = unknown) + float i_charge_cutoff = -1; // Protective max. charging current before charging get switched off (-1 = unknown) + float v_battery_cutoff = -1; // Protective max. battery voltage before charging get switched off (-1 = unknown) + float v_battery_empty = -1; // Empty battery voltage used for % calc of capacity (-1 = unknown) + float v_battery_full = -1; // Full battery voltage used for % calc of capacity (-1 = unknown) + uint16_t lift_period = 0xffff; // Period (ms) for >=2 wheels to be lifted in order to count as emergency (0 = disable, 0xFFFF = unknown) + uint16_t tilt_period = 0xffff; // Period (ms) for a single wheel to be lifted in order to count as emergency (0 = disable, 0xFFFF = unknown) + uint8_t shutdown_esc_max_pitch = 0xff; // Do not shutdown ESCs if absolute pitch angle is greater than this (0 = disable, 0xff = unknown) (to be implemented, see OpenMower PR #97) + iso639_1 language = {'e', 'n'}; // ISO 639-1 (2-char) language code (en, de, ...) + uint8_t volume = 0xff; // Volume (0-100%) feedback (if directly changed i.e. via CoverUI) (0xff = do not change) + HallConfig hall_configs[MAX_HALL_INPUTS]; // Set all to UNDEFINED + // 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 later within the wire buffer + // clang-format on } __attribute__((packed)); #pragma pack(pop) diff --git a/src/mower_comms/src/mower_comms.cpp b/src/mower_comms/src/mower_comms.cpp index 2bb53ca6..fc2a5e67 100644 --- a/src/mower_comms/src/mower_comms.cpp +++ b/src/mower_comms/src/mower_comms.cpp @@ -16,6 +16,7 @@ // SOFTWARE. // // +#include #include #include #include @@ -24,11 +25,13 @@ #include #include +#include #include #include "COBS.h" #include "boost/crc.hpp" #include "ll_datatypes.h" +#include "mower_logic/MowerLogicConfig.h" #include "mower_msgs/EmergencyStopSrv.h" #include "mower_msgs/HighLevelControlSrv.h" #include "mower_msgs/HighLevelStatus.h" @@ -66,9 +69,11 @@ float speed_l = 0, speed_r = 0, speed_mow = 0, target_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 (%) +// LL/HL configuration +struct ll_high_level_config llhl_config; + +dynamic_reconfigure::Client *reconfigClient; +mower_logic::MowerLogicConfig mower_logic_config; // Serial port and buffer for the low level connection serial::Serial serial_port; @@ -231,41 +236,108 @@ void publishStatus() { wheel_tick_pub.publish(wheel_tick_msg); } -void publishLowLevelConfig() { +std::string getHallConfigsString(const HallConfig *hall_configs, const size_t size) { + std::string str; + + // Parse hall_configs and build a readable string + for (size_t i = 0; i < size; i++) { + if (str.length()) str.append(", "); + if (hall_configs->active_low) str.append("!"); + switch (hall_configs->mode) { + case HallMode::OFF: str.append("I"); break; + case HallMode::LIFT_TILT: str.append("L"); break; + case HallMode::STOP: str.append("S"); break; + case HallMode::UNDEFINED: str.append("U"); break; + default: break; + } + hall_configs++; + } + + return str; +} + +void publishLowLevelConfig(const uint8_t pkt_type) { if (!serial_port.isOpen() || !allow_send) return; - struct ll_high_level_config config_pkt; + // Prepare the pkt + size_t size = sizeof(struct ll_high_level_config) + 3; // +1 type, +2 crc + uint8_t buf[size]; - 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; + // Send config and request a config answer + buf[0] = pkt_type; + // Copy our live config into the message (behind type) + memcpy(&buf[1], &llhl_config, sizeof(struct ll_high_level_config)); + + // Member access to buffer + struct ll_high_level_config *buf_config = (struct ll_high_level_config *)&buf[1]; + + // CRC crc.reset(); - crc.process_bytes(&config_pkt, sizeof(struct ll_high_level_config) - 2); - config_pkt.crc = crc.checksum(); + crc.process_bytes(buf, sizeof(struct ll_high_level_config) + 1); // + type + buf[size - 1] = (crc.checksum() >> 8) & 0xFF; + buf[size - 2] = crc.checksum() & 0xFF; - size_t encoded_size = cobs.encode((uint8_t *)&config_pkt, sizeof(struct ll_high_level_config), out_buf); + // COBS + size_t encoded_size = cobs.encode(buf, size, out_buf); out_buf[encoded_size] = 0; encoded_size++; + // Send 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 << "'"); + // Let's be verbose for easier follow-up + ROS_INFO( + "Send ll_high_level_config packet %#04x\n" + "\t options{dfp_is_5v=%d, background_sounds=%d, ignore_charging_current=%d},\n" + "\t v_charge_cutoff=%f, i_charge_cutoff=%f,\n" + "\t v_battery_cutoff=%f, v_battery_empty=%f, v_battery_full=%f,\n" + "\t lift_period=%d, tilt_period=%d,\n" + "\t shutdown_esc_max_pitch=%d,\n" + "\t language=\"%.2s\", volume=%d\n" + "\t hall_configs=\"%s\"", + buf[0], (int)buf_config->options.dfp_is_5v, (int)buf_config->options.background_sounds, + (int)buf_config->options.ignore_charging_current, buf_config->v_charge_cutoff, buf_config->i_charge_cutoff, + buf_config->v_battery_cutoff, buf_config->v_battery_empty, buf_config->v_battery_full, buf_config->lift_period, + buf_config->tilt_period, buf_config->shutdown_esc_max_pitch, buf_config->language, buf_config->volume, + getHallConfigsString(buf_config->hall_configs, MAX_HALL_INPUTS).c_str()); + serial_port.write(out_buf, encoded_size); } catch (std::exception &e) { ROS_ERROR_STREAM("Error writing to serial port"); } } +/** + * @brief A simple config tracker (struct-class) for managing lost response packets as well as simpler handling of + * LowLevel reboots or flash period. + */ +struct { + ros::Time last_config_req; // Time when last config request was sent + unsigned int tries_left = 0; // Remaining request tries before giving up + + void ackResponse() { // Call this on receive of a response packet to stop monitoring + tries_left = 0; + }; + void setDirty() { // Call this for indicating that config packet need to be resend, i.e. die to LL-reboot + tries_left = 5; + }; + void check() { + if (!tries_left || // No request tries left (probably old LL-FW) + !serial_port.isOpen() || !allow_send || // Serial not ready + ros::Time::now() - last_config_req < ros::Duration(0.5)) // Timeout waiting for response not reached + return; + publishLowLevelConfig(PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ); + last_config_req = ros::Time::now(); + tries_left--; + ROS_WARN_STREAM_COND( + !tries_left, "Didn't received a config packet from LowLevel in time. Is your LowLevel firmware up-to-date?"); + }; +} configTracker; + void publishActuatorsTimerTask(const ros::TimerEvent &timer_event) { publishActuators(); publishStatus(); + configTracker.check(); } bool setMowEnabled(mower_msgs::MowerControlSrvRequest &req, mower_msgs::MowerControlSrvResponse &res) { @@ -374,27 +446,87 @@ 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 - - // We're not interested in the received langauge setting (yet) - - // We're not interested in the received volume setting (yet) +/** + * @brief getNewSetChanged return t_new and checks if the value changed in comparison to t_cur. + * t_new can't be a reference because the same function is also used for packed structures. + * @param t_cur source value + * @param t_new reference + * @return &bool get set to true if t_cur and t_new differ, otherwise changed doesn't get touched + */ +template +T getNewSetChanged(const T t_cur, const T t_new, bool &changed) { + bool equal; + if (std::is_floating_point::value) + equal = fabs(t_cur - t_new) < std::numeric_limits::epsilon(); + else + equal = t_cur == t_new; + + if (!equal) changed = true; + + //ROS_INFO_STREAM("DEBUG mower_comms comp. member: cur " << t_cur << " ?= " << t_new << " == equal " << equal << ", changed " << changed); + + return t_new; +} - 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(); - } +/** + * Handle config packet on receive from LL (LL->HL config packet response) + */ +void handleLowLevelConfig(const uint8_t *buffer, const size_t size) { + // This is a flexible length packet 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 = std::min(sizeof(ll_high_level_config), size - 3); // exclude type & crc + + // Copy payload to separated ll_config + memcpy(&llhl_config, buffer + 1, payload_size); + + // Let's be verbose for easier follow-up + ROS_INFO( + "Received ll_high_level_config packet %#04x\n" + "\t options{dfp_is_5v=%d, background_sounds=%d, ignore_charging_current=%d},\n" + "\t v_charge_cutoff=%f, i_charge_cutoff=%f,\n" + "\t v_battery_cutoff=%f, v_battery_empty=%f, v_battery_full=%f,\n" + "\t lift_period=%d, tilt_period=%d,\n" + "\t shutdown_esc_max_pitch=%d,\n" + "\t language=\"%.2s\", volume=%d\n" + "\t hall_configs=\"%s\"", + *buffer, (int)llhl_config.options.dfp_is_5v, (int)llhl_config.options.background_sounds, + (int)llhl_config.options.ignore_charging_current, llhl_config.v_charge_cutoff, llhl_config.i_charge_cutoff, + llhl_config.v_battery_cutoff, llhl_config.v_battery_empty, llhl_config.v_battery_full, llhl_config.lift_period, + llhl_config.tilt_period, llhl_config.shutdown_esc_max_pitch, llhl_config.language, llhl_config.volume, + getHallConfigsString(llhl_config.hall_configs, MAX_HALL_INPUTS).c_str()); + + // Inform config packet tracker about the response + configTracker.ackResponse(); + + // Copy received config values from LL to mower_logic's related dynamic reconfigure variables and + // decide if mower_logic's dynamic reconfigure need to be updated with probably changed values + bool dirty = false; + // clang-format off + mower_logic_config.cu_rain_threshold = getNewSetChanged(mower_logic_config.cu_rain_threshold, llhl_config.rain_threshold, dirty); + mower_logic_config.charge_critical_high_voltage = getNewSetChanged(mower_logic_config.charge_critical_high_voltage, llhl_config.v_charge_cutoff, dirty); + mower_logic_config.charge_critical_high_current = getNewSetChanged(mower_logic_config.charge_critical_high_current, llhl_config.i_charge_cutoff, dirty); + mower_logic_config.battery_critical_high_voltage = getNewSetChanged(mower_logic_config.battery_critical_high_voltage, llhl_config.v_battery_cutoff, dirty); + mower_logic_config.battery_empty_voltage = getNewSetChanged(mower_logic_config.battery_empty_voltage, llhl_config.v_battery_empty, dirty); + mower_logic_config.battery_full_voltage = getNewSetChanged(mower_logic_config.battery_full_voltage, llhl_config.v_battery_full, dirty); + mower_logic_config.emergency_lift_period = getNewSetChanged(mower_logic_config.emergency_lift_period, llhl_config.lift_period, dirty); + mower_logic_config.emergency_tilt_period = getNewSetChanged(mower_logic_config.emergency_tilt_period, llhl_config.tilt_period, dirty); + // clang-format on + + if (dirty) reconfigClient->setConfiguration(mower_logic_config); } void handleLowLevelStatus(struct ll_status *status) { + static ros::Time last_ll_status_update(ros::Time::now()); + std::unique_lock lk(ll_status_mutex); last_ll_status = *status; + + // LL status get send at 100ms cycle. If we miss 10 packets, we can assume that it got restarted or flashed with a + // new FW. In either case we should ensure that it has the right config and update/re-align with us. + if (ros::Time::now() - last_ll_status_update > ros::Duration(1.0)) configTracker.setDirty(); + last_ll_status_update = ros::Time::now(); } void handleLowLevelIMU(struct ll_imu *imu) { @@ -431,6 +563,50 @@ void handleLowLevelIMU(struct ll_imu *imu) { sensor_mag_pub.publish(sensor_mag_msg); } +void reconfigCB(const mower_logic::MowerLogicConfig &config) { + ROS_INFO_STREAM("mower_comms received new mower_logic config"); + + mower_logic_config = config; + + // Copy changed mower_config's values to the related llhl_config values and + // decide if LL need to be informed with a new config packet + bool dirty = false; + + // clang-format off + llhl_config.rain_threshold = getNewSetChanged(llhl_config.rain_threshold, mower_logic_config.cu_rain_threshold, dirty); + llhl_config.v_charge_cutoff = getNewSetChanged(llhl_config.v_charge_cutoff, mower_logic_config.charge_critical_high_voltage, dirty); + llhl_config.i_charge_cutoff = getNewSetChanged(llhl_config.i_charge_cutoff, mower_logic_config.charge_critical_high_current, dirty); + llhl_config.v_battery_cutoff = getNewSetChanged(llhl_config.v_battery_cutoff, mower_logic_config.battery_critical_high_voltage, dirty); + llhl_config.v_battery_empty = getNewSetChanged(llhl_config.v_battery_empty, mower_logic_config.battery_empty_voltage, dirty); + llhl_config.v_battery_full = getNewSetChanged(llhl_config.v_battery_full, mower_logic_config.battery_full_voltage, dirty); + llhl_config.lift_period = getNewSetChanged(llhl_config.lift_period, mower_logic_config.emergency_lift_period, dirty); + llhl_config.tilt_period = getNewSetChanged(llhl_config.tilt_period, mower_logic_config.emergency_tilt_period, dirty); + // clang-format on + + // Parse emergency_input_config and set hall_configs + char *token = strtok(strdup(mower_logic_config.emergency_input_config.c_str()), ","); + bool low_active; + unsigned int hall_idx = 0; + while (token != NULL) { + low_active = false; + while (*token != 0) { + switch (std::toupper(*token)) { + case '!': low_active = true; break; + case 'I': llhl_config.hall_configs[hall_idx] = {HallMode::OFF, low_active}; break; + case 'L': llhl_config.hall_configs[hall_idx] = {HallMode::LIFT_TILT, low_active}; break; + case 'S': llhl_config.hall_configs[hall_idx] = {HallMode::STOP, low_active}; break; + case 'U': llhl_config.hall_configs[hall_idx] = {HallMode::UNDEFINED, low_active}; break; + default: break; + } + token++; + } + token = strtok(NULL, ","); + hall_idx++; + } + + if (dirty) configTracker.setDirty(); +} + int main(int argc, char **argv) { ros::init(argc, argv, "mower_comms"); @@ -445,6 +621,9 @@ int main(int argc, char **argv) { highLevelClient = n.serviceClient("mower_service/high_level_control"); + mower_logic_config = mower_logic::MowerLogicConfig::__getDefault__(); + reconfigClient = new dynamic_reconfigure::Client("/mower_logic", reconfigCB); + std::string ll_serial_port_name; if (!paramNh.getParam("ll_serial_port", ll_serial_port_name)) { ROS_ERROR_STREAM("Error getting low level serial port parameter. Quitting."); @@ -459,11 +638,13 @@ int main(int argc, char **argv) { speed_l = speed_r = speed_mow = target_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); + // Some generic settings from param server (non- dynamic) + llhl_config.options.ignore_charging_current = + paramNh.param("/mower_logic/ignore_charging_current", false) ? OptionState::ON : OptionState::OFF; + llhl_config.options.dfp_is_5v = paramNh.param("dfp_is_5v", false) ? OptionState::ON : OptionState::OFF; + llhl_config.volume = paramNh.param("volume", -1); + // ISO-639-1 (2 char) language code + strncpy(llhl_config.language, paramNh.param("language", "en").c_str(), 2); // Setup XESC interfaces if (mowerParamNh.hasParam("xesc_type")) { @@ -572,12 +753,7 @@ int main(int argc, char **argv) { 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] << ")"); - } + handleLowLevelConfig(buffer_decoded, data_size); break; default: ROS_INFO_STREAM("Got unknown packet from Low Level Board"); break; } diff --git a/src/mower_logic/cfg/MowerLogic.cfg b/src/mower_logic/cfg/MowerLogic.cfg index 3c5d7ecf..24fd0b17 100755 --- a/src/mower_logic/cfg/MowerLogic.cfg +++ b/src/mower_logic/cfg/MowerLogic.cfg @@ -1,6 +1,6 @@ #!/usr/bin/env python -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, bool_t +from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, bool_t, str_t gen = ParameterGenerator() @@ -25,10 +25,12 @@ gen.add("mow_angle_increment", double_t, 0, "Mowing angle automatic increment. W gen.add("tool_width", double_t, 0, "Width of the mower", 0.14, 0.1, 2) gen.add("enable_mower", bool_t, 0, "True to enable mow motor", False) gen.add("manual_pause_mowing", bool_t, 0, "True to disable mowing automatically", False) -gen.add("battery_empty_voltage", double_t, 0, "Voltage to return to docking station (over 20s interval)", 24.0, 18.0, 32.0) -gen.add("battery_critical_voltage", double_t, 0, "Voltage to return to docking station (immediate)", 23.0, 18.0, 32.0) -gen.add("battery_critical_high_voltage", double_t, 0, "Charge up to this voltage (hardcoded in Pico-FW)", 29.0, 23.0, 32.0) -gen.add("battery_full_voltage", double_t, 0, "Voltage to start mowing again", 28.0, 22.0, 32.0) +gen.add("battery_critical_voltage", double_t, 0, "Voltage to return to docking station (immediate)", -1) # AH: Optional in mower_config, but get set to empty_voltage if undefined +gen.add("battery_empty_voltage", double_t, 0, "Voltage to return to docking station (over 20s interval)", -1) # AH: Required mower_config parameter, so no default nor min/max needed +gen.add("battery_full_voltage", double_t, 0, "Voltage to start mowing again", -1) # AH: Required mower_config parameter, so no default nor min/max needed +gen.add("battery_critical_high_voltage", double_t, 0, "Charge up to this battery voltage, before charging get switched off", -1) # AH: In my opinions it's save here without defaults +gen.add("charge_critical_high_voltage", double_t, 0, "Max. charge voltage before charging get switched off", -1) # AH: In my opinions it's save here without defaults +gen.add("charge_critical_high_current", double_t, 0, "Max. charge current before charging get switched off", -1) # AH: In my opinions it's save here without defaults gen.add("motor_hot_temperature", double_t, 0, "Motor temperature to pause mowing", 70.0, 20.0, 150.0) gen.add("motor_cold_temperature", double_t, 0, "Motor temperature to allow mowing", 40.0, 20.0, 150.0) gen.add("max_position_accuracy", double_t, 0, "We allow driving as long as our position is better than this value (m)", 0.2, 0.01, 1.0) @@ -41,5 +43,9 @@ gen.add("max_first_point_trim_attempts", int_t, 0, "After gnore|nused|top|Lift'", "") exit(gen.generate("mower_logic", "mower_logic", "MowerLogic")) diff --git a/src/mower_logic/src/monitoring/monitoring.cpp b/src/mower_logic/src/monitoring/monitoring.cpp index 8f074f78..1ddb2d50 100644 --- a/src/mower_logic/src/monitoring/monitoring.cpp +++ b/src/mower_logic/src/monitoring/monitoring.cpp @@ -70,7 +70,7 @@ void set_limits_mow_motor_temp(SensorConfig &sensor_config); std::map sensor_configs{ {"om_v_charge", {"V Charge", "V", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_VOLTAGE, [](StatusPtr msg) { return msg->v_charge; }, &set_limits_charge_v}}, {"om_v_battery", {"V Battery", "V", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_VOLTAGE, [](StatusPtr msg) { return msg->v_battery; }, &set_limits_battery_v}}, - {"om_charge_current", {"Charge Current", "A", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_CURRENT, [](StatusPtr msg) { return msg->charge_current; }, &set_limits_charge_current}}, + {"om_charge_current", {"Charge Current", "A", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_CURRENT, [](StatusPtr msg) { return msg->charge_current; }, &set_limits_charge_current, "", [](){ return !paramNh->param("/mower_logic/ignore_charging_current", false); }}}, {"om_left_esc_temp", {"Left ESC Temp", "deg.C", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_TEMPERATURE, [](StatusPtr msg) { return msg->left_esc_status.temperature_pcb; }, &set_limits_esc_temp, "left_xesc"}}, {"om_right_esc_temp", {"Right ESC Temp", "deg.C", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_TEMPERATURE, [](StatusPtr msg) { return msg->right_esc_status.temperature_pcb; }, &set_limits_esc_temp, "right_xesc"}}, {"om_mow_esc_temp", {"Mow ESC Temp", "deg.C", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_TEMPERATURE, [](StatusPtr msg) { return msg->mow_esc_status.temperature_pcb; }, &set_limits_esc_temp, "mower_xesc"}}, @@ -90,7 +90,7 @@ void status(StatusPtr &msg) { sensor_data.stamp = msg->stamp; for (auto &sc_pair : sensor_configs) { - // Skip if sensor doesn't exists + // Skip if sensor doesn't exists or is disabled if (sc_pair.second.existCB && !sc_pair.second.existCB()) continue; if (sc_pair.second.getStatusSensorValueCB) { @@ -139,16 +139,11 @@ void set_limits_battery_v(SensorConfig &sensor_config) { } void set_limits_charge_v(SensorConfig &sensor_config) { - // FIXME: Shall these better go to mower_logic's dyn-reconfigure (or an own dyn-reconfigure server)? - sensor_config.si.min_value = 24.0f; // Mnimum voltage for before deep-discharge - sensor_config.si.max_value = 29.2f; // Optimal charge voltage - sensor_config.si.upper_critical_value = 30.0f; // Taken from OpenMower FW + sensor_config.si.upper_critical_value = mower_logic_config.charge_critical_high_voltage; } void set_limits_charge_current(SensorConfig &sensor_config) { - // FIXME: Shall these better go to mower_logic's dyn-reconfigure (or an own dyn-reconfigure server)? - sensor_config.si.max_value = 1.0f; // Taken from the docs - sensor_config.si.upper_critical_value = 1.5f; // Taken from OpenMower FW + sensor_config.si.upper_critical_value = mower_logic_config.charge_critical_high_current; } void set_limits_esc_temp(SensorConfig &sensor_config) { diff --git a/src/open_mower/launch/open_mower.launch b/src/open_mower/launch/open_mower.launch index 63273824..8e7baef0 100644 --- a/src/open_mower/launch/open_mower.launch +++ b/src/open_mower/launch/open_mower.launch @@ -9,7 +9,6 @@ - @@ -29,6 +28,9 @@ value="$(eval battery_empty_voltage if battery_critical_voltage=='' else battery_critical_voltage)" /> + + + @@ -42,10 +44,15 @@ + + + + +