diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index c7898e2b75535..f8afa254b4cec 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -7678,6 +7678,12 @@ def RichenPower(self): raise NotAchievedException("Did not find expected GEN message") def IE24(self): + '''Test IntelligentEnergy 2.4kWh generator with V1 and V2 telemetry protocols''' + protocol_ver = (1, 2) + for ver in protocol_ver: + self.run_IE24(ver) + + def run_IE24(self, proto_ver): '''Test IntelligentEnergy 2.4kWh generator''' elec_battery_instance = 2 fuel_battery_instance = 1 @@ -7687,14 +7693,14 @@ def IE24(self): "GEN_TYPE": 2, "BATT%u_MONITOR" % (fuel_battery_instance + 1): 18, # fuel-based generator "BATT%u_MONITOR" % (elec_battery_instance + 1): 17, - "SIM_IE24_ENABLE": 1, + "SIM_IE24_ENABLE": proto_ver, "LOG_DISARMED": 1, }) self.customise_SITL_commandline(["--uartF=sim:ie24"]) - self.start_subtest("ensure that BATTERY_STATUS for electrical generator message looks right") - self.start_subsubtest("Checking original voltage (electrical)") + self.start_subtest("Protocol %i: ensure that BATTERY_STATUS for electrical generator message looks right" % proto_ver) + self.start_subsubtest("Protocol %i: Checking original voltage (electrical)" % proto_ver) # ArduPilot spits out essentially uninitialised battery # messages until we read things fromthe battery: self.delay_sim_time(30) @@ -7712,13 +7718,13 @@ def IE24(self): "battery_remaining": original_elec_m.battery_remaining - 1, }, instance=elec_battery_instance) - self.start_subtest("ensure that BATTERY_STATUS for fuel generator message looks right") - self.start_subsubtest("Checking original voltage (fuel)") + self.start_subtest("Protocol %i: ensure that BATTERY_STATUS for fuel generator message looks right" % proto_ver) + self.start_subsubtest("Protocol %i: Checking original voltage (fuel)" % proto_ver) # ArduPilot spits out essentially uninitialised battery # messages until we read things fromthe battery: if original_fuel_m.battery_remaining <= 90: raise NotAchievedException("Bad original percentage (want=>%f got %f" % (90, original_fuel_m.battery_remaining)) - self.start_subsubtest("Ensure percentage is counting down") + self.start_subsubtest("Protocol %i: Ensure percentage is counting down" % proto_ver) self.wait_message_field_values('BATTERY_STATUS', { "battery_remaining": original_fuel_m.battery_remaining - 1, }, instance=fuel_battery_instance) @@ -7728,7 +7734,7 @@ def IE24(self): self.disarm_vehicle() # Test for pre-arm check fail when state is not running - self.start_subtest("If you haven't taken off generator error should cause instant failsafe and disarm") + self.start_subtest("Protocol %i: Without takeoff generator error should cause failsafe and disarm" % proto_ver) self.set_parameter("SIM_IE24_STATE", 8) self.wait_statustext("Status not running", timeout=40) self.try_arm(result=False, @@ -7736,7 +7742,7 @@ def IE24(self): self.set_parameter("SIM_IE24_STATE", 2) # Explicitly set state to running # Test that error code does result in failsafe - self.start_subtest("If you haven't taken off generator error should cause instant failsafe and disarm") + self.start_subtest("Protocol %i: Without taken off generator error should cause failsafe and disarm" % proto_ver) self.change_mode("STABILIZE") self.set_parameter("DISARM_DELAY", 0) self.arm_vehicle() diff --git a/libraries/AP_Generator/AP_Generator_IE_2400.cpp b/libraries/AP_Generator/AP_Generator_IE_2400.cpp index d297261d5ff4f..84d6a497bfe35 100644 --- a/libraries/AP_Generator/AP_Generator_IE_2400.cpp +++ b/libraries/AP_Generator/AP_Generator_IE_2400.cpp @@ -18,6 +18,7 @@ #if AP_GENERATOR_IE_2400_ENABLED #include +#include extern const AP_HAL::HAL& hal; @@ -32,21 +33,86 @@ void AP_Generator_IE_2400::init() _frontend._has_fuel_remaining = true; } -// Update fuel cell, expected to be called at 20hz +// Assigns the unit specific measurements once a valid sentence is obtained void AP_Generator_IE_2400::assign_measurements(const uint32_t now) { - // Successfully decoded a new valid sentence + + if (_type == PacketType::V2_INFO) { + // Got info packet + if (_had_info) { + // Not expecting the version to change + return; + } + _had_info = true; + + // Info tells us the protocol version, so lock on straight away + if (_version == ProtocolVersion::DETECTING) { + if (strcmp(_info.Protocol_version, "4") == 0) { + _version = ProtocolVersion::V2; + } else { + // Got a valid info packet, but don't know this protocol version + // Give up + _version = ProtocolVersion::UNKNOWN; + } + } + + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IE Fuel cell detected, PCM: %s, Ver: %s, SN: %s", _info.PCM_number, _info.Software_version, _info.Serial_number); + + return; + } + + // Try and lock onto version + if (_version == ProtocolVersion::DETECTING) { + ProtocolVersion new_version = ProtocolVersion::DETECTING; + switch (_type) { + case PacketType::NONE: + // Should not get a valid packet of type none + _last_version_packet_count = 0; + return; + + case PacketType::LEGACY_DATA: + new_version = ProtocolVersion::LEGACY; + break; + + case PacketType::V2_DATA: + case PacketType::V2_INFO: + new_version = ProtocolVersion::V2; + break; + } + + if (_last_version == new_version) { + _last_version_packet_count++; + } else { + _last_version_packet_count = 0; + } + _last_version = new_version; + + // If received 20 valid packets for a single protocol version then lock on + if (_last_version_packet_count > 20) { + _version = new_version; + gcs().send_text(MAV_SEVERITY_INFO, "Generator: IE using %s protocol", (_version == ProtocolVersion::V2) ? "V2" : "legacy" ); + + } else { + // Don't record any data during version detection + return; + } + } + + if (_type == PacketType::V2_DATA) { + memcpy(&_valid_V2, &_parsed_V2, sizeof(_valid_V2)); + } + // Update internal fuel cell state _pwr_out = _parsed.pwr_out; _spm_pwr = _parsed.spm_pwr; + _battery_pwr = _parsed.battery_pwr; _state = (State)_parsed.state; + _v2_state = (V2_State)_parsed.state; _err_code = _parsed.err_code; + _sub_err_code = _parsed.sub_err_code; - // Scale tank pressure linearly to a value between 0 and 1 - // Min = 5 bar, max = 300 bar, PRESS_GRAD = 1/295. - const float PRESS_GRAD = 0.003389830508f; - _fuel_remaining = constrain_float((_parsed.tank_bar-5)*PRESS_GRAD,0,1); + _fuel_remaining = _fuel_rem; // Update battery voltage _voltage = _parsed.battery_volt; @@ -55,7 +121,7 @@ void AP_Generator_IE_2400::assign_measurements(const uint32_t now) battery is charging. This is aligned with normal AP behaviour. This is the opposite of IE's convention hence *-1 */ if (_parsed.battery_volt > 0) { - _current = -1 * _parsed.battery_pwr / _parsed.battery_volt; + _current = -1 * _battery_pwr / _parsed.battery_volt; } else { _current = 0; } @@ -73,13 +139,44 @@ void AP_Generator_IE_2400::decode_latest_term() _term[_term_offset] = 0; _term_offset = 0; _term_number++; + _type = PacketType::NONE; + + if (_start_char == '<') { + decode_data_packet(); + + } else if (_start_char == '[') { + decode_info_packet(); + + } else { + _sentence_valid = false; + + } +} + +void AP_Generator_IE_2400::decode_data_packet() +{ + // Try and decode both protocol versions until locked on + if ((_version == ProtocolVersion::LEGACY) || (_version == ProtocolVersion::DETECTING)) { + decode_legacy_data(); + } + if ((_version == ProtocolVersion::V2) || (_version == ProtocolVersion::DETECTING)) { + decode_v2_data(); + } +} +void AP_Generator_IE_2400::decode_legacy_data() +{ switch (_term_number) { - case 1: + case 1: { // Float _parsed.tank_bar = strtof(_term, NULL); - break; + // Scale tank pressure linearly to a value between 0 and 1 + // Min = 5 bar, max = 300 bar, PRESS_GRAD = 1/295. + const float PRESS_GRAD = 0.003389830508f; + _fuel_rem = constrain_float((_parsed.tank_bar-5)*PRESS_GRAD,0,1); + break; + } case 2: // Float _parsed.battery_volt = strtof(_term, NULL); @@ -110,7 +207,80 @@ void AP_Generator_IE_2400::decode_latest_term() _parsed.err_code = strtoul(_term, nullptr, 10); // Sentence only declared valid when we have the expected number of terms _sentence_valid = true; + _type = PacketType::LEGACY_DATA; + break; + + default: + // We have received more terms than, something has gone wrong with telemetry data, mark invalid sentence + _sentence_valid = false; + break; + } +} + +void AP_Generator_IE_2400::decode_v2_data() +{ + switch (_term_number) { + case 1: + _fuel_rem = strtof(_term, NULL) * 0.01; + break; + + case 2: + _parsed_V2.inlet_press = strtof(_term, NULL); + break; + + case 3: + _parsed.battery_volt = strtof(_term, NULL); + break; + + case 4: + _parsed.pwr_out = strtol(_term, nullptr, 10); + break; + + case 5: + _parsed.spm_pwr = strtoul(_term, nullptr, 10); + break; + + case 6: + _parsed_V2.unit_fault = strtoul(_term, nullptr, 10); + break; + + case 7: + _parsed.battery_pwr = strtol(_term, nullptr, 10); + break; + + case 8: + _parsed.state = strtoul(_term, nullptr, 10); + break; + + case 9: + _parsed.err_code = strtoul(_term, nullptr, 10); + break; + + case 10: + _parsed.sub_err_code = strtoul(_term, nullptr, 10); + break; + + case 11: + strncpy(_parsed_V2.info_str, _term, ARRAY_SIZE(_parsed_V2.info_str)); + break; + + case 12: { + // The inverted checksum is sent, un-invert it + uint8_t checksum = ~strtoul(_term, nullptr, 10); + + // Sent checksum only included characters up to the checksum term + // Add on the checksum terms to match our running total + for (uint8_t i = 0; i < ARRAY_SIZE(_term); i++) { + if (_term[i] == 0) { + break; + } + checksum += _term[i]; + } + + _sentence_valid = checksum == _checksum; + _type = PacketType::V2_DATA; break; + } default: // We have received more terms than, something has gone wrong with telemetry data, mark invalid sentence @@ -119,6 +289,57 @@ void AP_Generator_IE_2400::decode_latest_term() } } + +void AP_Generator_IE_2400::decode_info_packet() +{ + + switch (_term_number) { + case 1: + // PCM software number + strncpy(_info.PCM_number, _term, ARRAY_SIZE(_info.PCM_number)); + break; + + case 2: + // Software version + strncpy(_info.Software_version, _term, ARRAY_SIZE(_info.Software_version)); + break; + + case 3: + // protocol version + strncpy(_info.Protocol_version, _term, ARRAY_SIZE(_info.Protocol_version)); + break; + + case 4: + // Hardware serial number + strncpy(_info.Serial_number, _term, ARRAY_SIZE(_info.Serial_number)); + break; + + case 5: { + // The inverted checksum is sent, un-invert it + uint8_t checksum = ~strtoul(_term, nullptr, 10); + + // Sent checksum only included characters up to the checksum term + // Add on the checksum terms to match our running total + for (uint8_t i = 0; i < ARRAY_SIZE(_term); i++) { + if (_term[i] == 0) { + break; + } + checksum += _term[i]; + } + + _sentence_valid = checksum == _checksum; + _type = PacketType::V2_INFO; + break; + } + + default: + // We have received more terms than, something has gone wrong with telemetry data, mark invalid sentence + _sentence_valid = false; + break; + } + +} + // Check for failsafes AP_BattMonitor::Failsafe AP_Generator_IE_2400::update_failsafes() const { @@ -138,6 +359,12 @@ AP_BattMonitor::Failsafe AP_Generator_IE_2400::update_failsafes() const // Check for error codes that are deemed critical bool AP_Generator_IE_2400::is_critical_error(const uint32_t err_in) const { + // V2 protocol + if (_version == ProtocolVersion::V2) { + return err_in >= 30; + } + + // V1 protocol switch ((ErrorCode)err_in) { // Error codes that lead to critical action battery monitor failsafe case ErrorCode::BATTERY_CRITICAL: @@ -154,6 +381,12 @@ bool AP_Generator_IE_2400::is_critical_error(const uint32_t err_in) const // Check for error codes that are deemed severe and would be cause to trigger a battery monitor low failsafe action bool AP_Generator_IE_2400::is_low_error(const uint32_t err_in) const { + // V2 protocol + if (_version == ProtocolVersion::V2) { + return (err_in > 20) && (err_in < 30); + } + + // V1 protocol switch ((ErrorCode)err_in) { // Error codes that lead to critical action battery monitor failsafe case ErrorCode::START_DENIED: @@ -161,7 +394,6 @@ bool AP_Generator_IE_2400::is_low_error(const uint32_t err_in) const case ErrorCode::BATTERY_LOW: case ErrorCode::PRESSURE_LOW: case ErrorCode::SPM_LOST: - case ErrorCode::REDUCED_POWER: return true; default: @@ -178,10 +410,47 @@ bool AP_Generator_IE_2400::check_for_err_code(char* msg_txt, uint8_t msg_len) co return false; } + if (_version == ProtocolVersion::V2) { + hal.util->snprintf(msg_txt, msg_len, "Fuel cell err %u.%u: %s", (unsigned)_err_code, (unsigned)_sub_err_code, _valid_V2.info_str); + return true; + } + hal.util->snprintf(msg_txt, msg_len, "Fuel cell err code <%u>", (unsigned)_err_code); return true; } +bool AP_Generator_IE_2400::check_for_warning_code(char* msg_txt, uint8_t msg_len) const +{ + if (_err_code == 0) { + // No error nothing to do. + return false; + } + if (is_critical_error(_err_code) || is_low_error(_err_code)) { + // Critical or low error are already reported + return false; + } + + switch (_version) { + case ProtocolVersion::DETECTING: + case ProtocolVersion::UNKNOWN: + break; + + case ProtocolVersion::LEGACY: + if ((ErrorCode)_err_code == ErrorCode::REDUCED_POWER) { + hal.util->snprintf(msg_txt, msg_len, "Fuel cell reduced power <%u>", (unsigned)_err_code); + return true; + } + break; + + case ProtocolVersion::V2: + hal.util->snprintf(msg_txt, msg_len, "Fuel cell warning %u.%u: %s", (unsigned)_err_code, (unsigned)_sub_err_code, _valid_V2.info_str); + return true; + } + + hal.util->snprintf(msg_txt, msg_len, "Fuel cell warning code <%u>", (unsigned)_err_code); + return true; +} + #if HAL_LOGGING_ENABLED // log generator status to the onboard log void AP_Generator_IE_2400::log_write() @@ -191,19 +460,106 @@ void AP_Generator_IE_2400::log_write() return; } - AP::logger().WriteStreaming( - "IE24", - "TimeUS,FUEL,SPMPWR,POUT,ERR", - "s%WW-", - "F2---", - "Qfiii", - AP_HAL::micros64(), - _fuel_remaining, - _spm_pwr, - _pwr_out, - _err_code - ); + switch (_version) { + case ProtocolVersion::DETECTING: + case ProtocolVersion::UNKNOWN: + return; + + case ProtocolVersion::LEGACY: + AP::logger().WriteStreaming( + "IE24", + "TimeUS,FUEL,SPMPWR,POUT,ERR", + "s%WW-", + "F2---", + "Qfiii", + AP_HAL::micros64(), + _fuel_remaining, + _spm_pwr, + _pwr_out, + _err_code + ); + break; + + case ProtocolVersion::V2: + AP::logger().WriteStreaming( + "IEFC", + "TimeUS,Tank,Inlet,BattV,OutPwr,SPMPwr,FNo,BPwr,State,F1,F2", + "s%-vWW-W---", + "F----------", + "QfffhHBhBII", + AP_HAL::micros64(), + _fuel_remaining, + _valid_V2.inlet_press, + _voltage, + _pwr_out, + _spm_pwr, + _valid_V2.unit_fault, + _battery_pwr, + uint8_t(_v2_state), + _err_code, + _sub_err_code + ); + break; + } + } #endif // HAL_LOGGING_ENABLED +// Return true is fuel cell is in running state suitable for arming +bool AP_Generator_IE_2400::is_running() const +{ + switch (_version) { + case ProtocolVersion::DETECTING: + case ProtocolVersion::UNKNOWN: + return false; + + case ProtocolVersion::LEGACY: + // Can use the base class method + return AP_Generator_IE_FuelCell::is_running(); + + case ProtocolVersion::V2: + return _v2_state == V2_State::Running; + } + + return false; +} + +// Lookup table for running state. State code is the same for all IE units. +const AP_Generator_IE_2400::Lookup_State_V2 AP_Generator_IE_2400::lookup_state_V2[] = { + { V2_State::FCPM_Off, "FCPM Off"}, + { V2_State::Starting, "Starting"}, + { V2_State::Running, "Running"}, + { V2_State::Stopping, "Stopping"}, + { V2_State::Go_to_Sleep, "Sleep"}, +}; + +// Print msg to user updating on state change +void AP_Generator_IE_2400::update_state_msg() +{ + switch (_version) { + case ProtocolVersion::DETECTING: + case ProtocolVersion::UNKNOWN: + break; + + case ProtocolVersion::LEGACY: + // Can use the base class method + AP_Generator_IE_FuelCell::update_state_msg(); + break; + + case ProtocolVersion::V2: { + // If fuel cell state has changed send gcs message + if (_v2_state != _last_v2_state) { + for (const struct Lookup_State_V2 entry : lookup_state_V2) { + if (_v2_state == entry.option) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Generator: %s", entry.msg_txt); + break; + } + } + _last_v2_state = _v2_state; + } + break; + } + } +} + #endif // AP_GENERATOR_IE_2400_ENABLED diff --git a/libraries/AP_Generator/AP_Generator_IE_2400.h b/libraries/AP_Generator/AP_Generator_IE_2400.h index 65acfa8ef9502..c98238f53f7c5 100644 --- a/libraries/AP_Generator/AP_Generator_IE_2400.h +++ b/libraries/AP_Generator/AP_Generator_IE_2400.h @@ -23,9 +23,20 @@ class AP_Generator_IE_2400 : public AP_Generator_IE_FuelCell // Process characters received and extract terms for IE 2.4kW void decode_latest_term(void) override; + // Decode a data packet + void decode_data_packet(); + void decode_legacy_data(); + void decode_v2_data(); + + // Decode a info packet + void decode_info_packet(); + // Check if we have received an error code and populate message with error code bool check_for_err_code(char* msg_txt, uint8_t msg_len) const override; + // Check if we have received an warning code and populate message with warning code + bool check_for_warning_code(char* msg_txt, uint8_t msg_len) const override; + // Check for error codes that are deemed critical bool is_critical_error(const uint32_t err_in) const; @@ -36,6 +47,12 @@ class AP_Generator_IE_2400 : public AP_Generator_IE_FuelCell void log_write(void) override; #endif + // Return true is fuel cell is in running state suitable for arming + bool is_running() const override; + + // Print msg to user updating on state change + void update_state_msg() override; + // IE 2.4kW failsafes enum class ErrorCode { MINOR_INTERNAL = 1, // Minor internal error is to be ignored @@ -53,6 +70,59 @@ class AP_Generator_IE_2400 : public AP_Generator_IE_FuelCell // These measurements are only available on this unit int16_t _pwr_out; // Output power (Watts) uint16_t _spm_pwr; // Stack Power Module (SPM) power draw (Watts) + float _fuel_rem; // fuel remaining 0 to 1 + int16_t _battery_pwr; // Battery charging power + + // Extra data in the V2 packet + struct V2_data { + float inlet_press; + uint8_t unit_fault; // Unit number with issue + char info_str[33]; + }; + V2_data _parsed_V2; + V2_data _valid_V2; + + // Info packet + struct { + char PCM_number[TERM_BUFFER]; + char Software_version[TERM_BUFFER]; + char Protocol_version[TERM_BUFFER]; + char Serial_number[TERM_BUFFER]; + } _info; + bool _had_info; + + enum class ProtocolVersion { + DETECTING = 0, + LEGACY = 1, + V2 = 2, + UNKNOWN = 3, + } _version; + + ProtocolVersion _last_version; + uint8_t _last_version_packet_count; + + enum class PacketType { + NONE = 0, + LEGACY_DATA = 1, + V2_DATA = 2, + V2_INFO = 3, + } _type; + + enum class V2_State { + FCPM_Off = 0, + Starting = 1, + Running = 2, + Stopping = 3, + Go_to_Sleep = 4, + } _v2_state; + V2_State _last_v2_state; + + // State enum to string lookup + struct Lookup_State_V2 { + V2_State option; + const char *msg_txt; + }; + static const Lookup_State_V2 lookup_state_V2[]; }; #endif // AP_GENERATOR_IE_2400_ENABLED diff --git a/libraries/AP_Generator/AP_Generator_IE_650_800.cpp b/libraries/AP_Generator/AP_Generator_IE_650_800.cpp index 8c5b770e1511f..c0e9aae3a2a95 100644 --- a/libraries/AP_Generator/AP_Generator_IE_650_800.cpp +++ b/libraries/AP_Generator/AP_Generator_IE_650_800.cpp @@ -59,6 +59,11 @@ void AP_Generator_IE_650_800::decode_latest_term() _term_offset = 0; _term_number++; + if (_start_char != '<') { + _sentence_valid = false; + return; + } + switch (_term_number) { case 1: _parsed.tank_pct = strtof(_term, NULL); diff --git a/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp b/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp index b66d59b8ddcd4..1385888435f76 100644 --- a/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp +++ b/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp @@ -77,12 +77,14 @@ void AP_Generator_IE_FuelCell::update() bool AP_Generator_IE_FuelCell::decode(char c) { // Start of a string - if (c == '<') { + if ((c == '<') || (c == '[')) { + _start_char = c; _sentence_valid = false; _data_valid = true; _term_number = 0; _term_offset = 0; _in_string = true; + _checksum = c; return false; } if (!_in_string) { @@ -90,7 +92,8 @@ bool AP_Generator_IE_FuelCell::decode(char c) } // End of a string - if (c == '>') { + const char end_char = (_start_char == '[') ? ']' : '>'; + if (c == end_char) { decode_latest_term(); _in_string = false; @@ -100,11 +103,13 @@ bool AP_Generator_IE_FuelCell::decode(char c) // End of a term in the string if (c == ',') { decode_latest_term(); + _checksum += c; return false; } // Otherwise add the char to the current term _term[_term_offset++] = c; + _checksum += c; // We have overrun the expected sentence if (_term_offset >TERM_BUFFER) { @@ -124,7 +129,7 @@ bool AP_Generator_IE_FuelCell::pre_arm_check(char *failmsg, uint8_t failmsg_len) } // Refuse arming if not in running state - if (_state != State::RUNNING) { + if (!is_running()) { strncpy(failmsg, "Status not running", failmsg_len); return false; } @@ -160,36 +165,53 @@ void AP_Generator_IE_FuelCell::check_status(const uint32_t now) } // If fuel cell state has changed send gcs message - if (_state != _last_state) { - for (const struct Lookup_State entry : lookup_state) { - if (_state == entry.option) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Generator: %s", entry.msg_txt); - break; - } - } - _last_state = _state; - } + update_state_msg(); // Check error codes - char msg_txt[32]; - if (check_for_err_code_if_changed(msg_txt, sizeof(msg_txt))) { - GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "%s", msg_txt); - } + check_for_err_code_if_changed(); } // Check error codes and populate message with error code -bool AP_Generator_IE_FuelCell::check_for_err_code_if_changed(char* msg_txt, uint8_t msg_len) +void AP_Generator_IE_FuelCell::check_for_err_code_if_changed() { // Only check if there has been a change in error code - if (_err_code == _last_err_code) { - return false; + if ((_err_code == _last_err_code) && (_sub_err_code == _last_sub_err_code)) { + return; } - if (check_for_err_code(msg_txt, msg_len)) { - _last_err_code = _err_code; - return true; + char msg_txt[64]; + if (check_for_err_code(msg_txt, sizeof(msg_txt)) || check_for_warning_code(msg_txt, sizeof(msg_txt))) { + GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "%s", msg_txt); + + } else if ((_err_code == 0) && (_sub_err_code == 0)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Fuel cell error cleared"); + } - return false; + _last_err_code = _err_code; + _last_sub_err_code = _sub_err_code; + } + +// Return true is fuel cell is in running state suitable for arming +bool AP_Generator_IE_FuelCell::is_running() const +{ + return _state == State::RUNNING; +} + +// Print msg to user updating on state change +void AP_Generator_IE_FuelCell::update_state_msg() +{ + // If fuel cell state has changed send gcs message + if (_state != _last_state) { + for (const struct Lookup_State entry : lookup_state) { + if (_state == entry.option) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Generator: %s", entry.msg_txt); + break; + } + } + _last_state = _state; + } +} + #endif // AP_GENERATOR_IE_ENABLED diff --git a/libraries/AP_Generator/AP_Generator_IE_FuelCell.h b/libraries/AP_Generator/AP_Generator_IE_FuelCell.h index 84395f1125c91..8eac895249301 100644 --- a/libraries/AP_Generator/AP_Generator_IE_FuelCell.h +++ b/libraries/AP_Generator/AP_Generator_IE_FuelCell.h @@ -49,6 +49,8 @@ class AP_Generator_IE_FuelCell : public AP_Generator_Backend uint32_t _err_code; // The error code from fuel cell uint32_t _last_err_code; // The previous error code from fuel cell + uint32_t _sub_err_code; // The sub error code from fuel cell + uint32_t _last_sub_err_code; // The previous sub error code from fuel cell State _state; // The PSU state State _last_state; // The previous PSU state uint32_t _last_time_ms; // Time we got a reading @@ -66,19 +68,22 @@ class AP_Generator_IE_FuelCell : public AP_Generator_Backend int16_t battery_pwr; uint8_t state; uint32_t err_code; + uint32_t sub_err_code; } _parsed; // Constants - static const uint8_t TERM_BUFFER = 12; // Max length of term we expect + static const uint8_t TERM_BUFFER = 33; // Max length of term we expect static const uint16_t HEALTHY_TIMEOUT_MS = 5000; // Time for driver to be marked un-healthy // Decoding vars + char _start_char; // inital sentence character giving sentence type char _term[TERM_BUFFER]; // Term buffer bool _sentence_valid; // Is current sentence valid bool _data_valid; // Is data within expected limits uint8_t _term_number; // Term index within the current sentence uint8_t _term_offset; // Offset within the _term buffer where the next character should be placed bool _in_string; // True if we should be decoding + uint8_t _checksum; // Basic checksum used by V2 protocol // Assigns the unit specific measurements once a valid sentence is obtained virtual void assign_measurements(const uint32_t now) = 0; @@ -100,8 +105,17 @@ class AP_Generator_IE_FuelCell : public AP_Generator_Backend // Check error codes and populate message with error code virtual bool check_for_err_code(char* msg_txt, uint8_t msg_len) const = 0; + // Check if we have received an warning code and populate message with warning code + virtual bool check_for_warning_code(char* msg_txt, uint8_t msg_len) const { return false; } + // Only check the error code if it has changed since we last checked - bool check_for_err_code_if_changed(char* msg_txt, uint8_t msg_len); + void check_for_err_code_if_changed(); + + // Return true is fuel cell is in running state suitable for arming + virtual bool is_running() const; + + // Print msg to user updating on state change + virtual void update_state_msg(); }; #endif // AP_GENERATOR_IE_ENABLED diff --git a/libraries/SITL/SIM_IntelligentEnergy24.cpp b/libraries/SITL/SIM_IntelligentEnergy24.cpp index 2445df9a863a9..7b2456ee6b583 100644 --- a/libraries/SITL/SIM_IntelligentEnergy24.cpp +++ b/libraries/SITL/SIM_IntelligentEnergy24.cpp @@ -29,25 +29,27 @@ extern const AP_HAL::HAL& hal; using namespace SITL; +#define MAX_TANK_PRESSURE 300 //(bar) + // table of user settable parameters const AP_Param::GroupInfo IntelligentEnergy24::var_info[] = { // @Param: ENABLE // @DisplayName: IntelligentEnergy 2.4kWh FuelCell sim enable/disable // @Description: Allows you to enable (1) or disable (0) the FuelCell simulator - // @Values: 0:Disabled,1:Enabled + // @Values: 0:Disabled,1:V1 Protocol,2:V2 Protocol // @User: Advanced AP_GROUPINFO("ENABLE", 1, IntelligentEnergy24, enabled, 0), // @Param: STATE // @DisplayName: Explicitly set state - // @Description: Explicity specify a state for the generator to be in + // @Description: Explicitly specify a state for the generator to be in // @User: Advanced AP_GROUPINFO("STATE", 2, IntelligentEnergy24, set_state, -1), // @Param: ERROR // @DisplayName: Explicitly set error code - // @Description: Explicity specify an error code to send to the generator + // @Description: Explicitly specify an error code to send to the generator // @User: Advanced AP_GROUPINFO("ERROR", 3, IntelligentEnergy24, err_code, 0), @@ -64,15 +66,14 @@ void IntelligentEnergy24::update(const struct sitl_input &input) if (!enabled.get()) { return; } - // gcs().send_text(MAV_SEVERITY_INFO, "fuelcell update"); update_send(); } void IntelligentEnergy24::update_send() { - // just send a chunk of data at 1Hz: + // just send a chunk of data at 2 Hz: const uint32_t now = AP_HAL::millis(); - if (now - last_sent_ms < 500) { + if (now - last_data_sent_ms < 500) { return; } @@ -80,7 +81,7 @@ void IntelligentEnergy24::update_send() float amps = discharge ? -20.0f : 20.0f; // Update pack capacity remaining - bat_capacity_mAh += amps*(now - last_sent_ms)/3600.0f; + bat_capacity_mAh += amps*(now - last_data_sent_ms)/3600.0f; // From capacity remaining approximate voltage by linear interpolation const float min_bat_vol = 42.0f; @@ -90,7 +91,7 @@ void IntelligentEnergy24::update_send() // Simulate tank pressure // Scale tank pressure linearly to a percentage. // Min = 5 bar, max = 300 bar, PRESS_GRAD = 1/295. - const int16_t tank_bar = linear_interpolate(5, 295, bat_capacity_mAh / max_bat_capactiy_mAh, 0, 1); + const int16_t tank_bar = linear_interpolate(5, MAX_TANK_PRESSURE, bat_capacity_mAh / max_bat_capactiy_mAh, 0, 1); battery_voltage = bat_capacity_mAh / max_bat_capactiy_mAh * (max_bat_vol - min_bat_vol) + min_bat_vol; @@ -112,10 +113,13 @@ void IntelligentEnergy24::update_send() state = 2; // Running } - last_sent_ms = now; + last_data_sent_ms = now; char message[128]; - hal.util->snprintf(message, ARRAY_SIZE(message), "<%i,%.1f,%i,%u,%i,%u,%u>\n", + + if (enabled.get() == 1) { + // V1 Protocol + hal.util->snprintf(message, ARRAY_SIZE(message), "<%i,%.1f,%i,%u,%i,%u,%u>\n", tank_bar, battery_voltage, (signed)pwr_out, @@ -124,7 +128,69 @@ void IntelligentEnergy24::update_send() (unsigned)state, (unsigned)err_code); + } else { + // V2 Protocol + + // version message sent at 0.2 Hz + if (now - last_ver_sent_ms > 5e3) { + // PCM software part number, software version number, protocol number, hardware serial number, check-sum + hal.util->snprintf(message, ARRAY_SIZE(message), "[10011867,2.132,4,IE12160A8040015,7]\n"); + + if ((unsigned)write_to_autopilot(message, strlen(message)) != strlen(message)) { + AP_HAL::panic("Failed to write to autopilot: %s", strerror(errno)); + } + last_ver_sent_ms = now; + } + + // data message + memset(&message, 0, sizeof(message)); + int8_t tank_remaining_pct = (float)tank_bar / MAX_TANK_PRESSURE * 100.0; + + hal.util->snprintf(message, ARRAY_SIZE(message), "<%i,%.2f,%.1f,%i,%u,%i,%i,%u,%u,%i,%s,", // last blank , is for fuel cell to send info string up to 32 char ASCII + tank_remaining_pct, + 0.67f, // inlet pressure (bar) + battery_voltage, + (signed)pwr_out, + (unsigned)spm_pwr, + 0, // unit at fault (0 = no fault) + (signed)battery_pwr, + (unsigned)state, + (unsigned)err_code, + 0, // fault state 2 (0 = no fault) + get_error_string(err_code)); + + // calculate the checksum + uint8_t checksum = 0; + for (uint8_t i = 0; i < ARRAY_SIZE(message); i++) { + if (message[i] == 0) { + break; + } + checksum += message[i]; + } + // checksum is inverted 8-bit + checksum = ~checksum; + + // add the checksum to the end of the message + char data_end[7]; + hal.util->snprintf(data_end, ARRAY_SIZE(data_end), "%u>\n", checksum); + strncat(message, data_end, ARRAY_SIZE(data_end)); + + } + if ((unsigned)write_to_autopilot(message, strlen(message)) != strlen(message)) { AP_HAL::panic("Failed to write to autopilot: %s", strerror(errno)); } } + +const char * IntelligentEnergy24::get_error_string(const uint32_t code) +{ + switch (code) { + case 20: + return "THERMAL MNGMT"; + + default: + break; + } + + return ""; +} diff --git a/libraries/SITL/SIM_IntelligentEnergy24.h b/libraries/SITL/SIM_IntelligentEnergy24.h index eecd0939e27d5..68d8782937f62 100644 --- a/libraries/SITL/SIM_IntelligentEnergy24.h +++ b/libraries/SITL/SIM_IntelligentEnergy24.h @@ -60,6 +60,8 @@ class IntelligentEnergy24 : public IntelligentEnergy { void update_send(); + const char * get_error_string(const uint32_t code); + AP_Int8 enabled; // enable sim AP_Int8 set_state; AP_Int32 err_code; @@ -67,7 +69,8 @@ class IntelligentEnergy24 : public IntelligentEnergy { float battery_voltage = 50.4f; float bat_capacity_mAh = 3300; bool discharge = true; // used to switch between battery charging and discharging - uint32_t last_sent_ms; + uint32_t last_data_sent_ms; + uint32_t last_ver_sent_ms; };