AP_Generator: Add support for IE V2 protocol

libraries/AP_Generator/AP_Generator_IE_2400.cpp

@@ -18,6 +18,7 @@
 #if AP_GENERATOR_IE_2400_ENABLED
 
 #include <AP_Logger/AP_Logger.h>
+#include <GCS_MAVLink/GCS.h>
 
 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

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

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);

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

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