diff --git a/libraries/AP_Generator/AP_Generator_IE_2400.cpp b/libraries/AP_Generator/AP_Generator_IE_2400.cpp index d297261d5f..8a4e01eade 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,6 +207,7 @@ 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: @@ -119,6 +217,129 @@ void AP_Generator_IE_2400::decode_latest_term() } } +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 + _sentence_valid = false; + break; + } +} + + +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 { @@ -173,6 +394,11 @@ bool AP_Generator_IE_2400::is_low_error(const uint32_t err_in) const // Check error codes and populate message with error code bool AP_Generator_IE_2400::check_for_err_code(char* msg_txt, uint8_t msg_len) const { + if ((_version == ProtocolVersion::V2) && (strlen(_valid_V2.info_str) > 0)) { + hal.util->snprintf(msg_txt, msg_len, "Fuel cell err %s", _valid_V2.info_str); + return true; + } + // Check if we have received an error code if (!is_critical_error(_err_code) && !is_low_error(_err_code)) { return false; @@ -191,19 +417,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 65acfa8ef9..637f95fb1b 100644 --- a/libraries/AP_Generator/AP_Generator_IE_2400.h +++ b/libraries/AP_Generator/AP_Generator_IE_2400.h @@ -23,6 +23,14 @@ private: // 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; @@ -36,6 +44,12 @@ private: 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 +67,59 @@ private: // 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 8c5b770e15..c0e9aae3a2 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 b66d59b8dd..e25a382368 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,15 +165,7 @@ 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]; @@ -181,15 +178,38 @@ void AP_Generator_IE_FuelCell::check_status(const uint32_t now) bool AP_Generator_IE_FuelCell::check_for_err_code_if_changed(char* msg_txt, uint8_t msg_len) { // Only check if there has been a change in error code - if (_err_code == _last_err_code) { + if ((_err_code == _last_err_code) && (_sub_err_code == _last_sub_err_code)) { return false; } if (check_for_err_code(msg_txt, msg_len)) { _last_err_code = _err_code; + _last_sub_err_code = _sub_err_code; return true; } return false; } + +// 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 84395f1125..dfa22cd2a9 100644 --- a/libraries/AP_Generator/AP_Generator_IE_FuelCell.h +++ b/libraries/AP_Generator/AP_Generator_IE_FuelCell.h @@ -49,6 +49,8 @@ protected: 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 @@ protected: 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; @@ -103,5 +108,11 @@ protected: // 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); + // 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