SITL: Add V2 telemetry protocol to IE24 fuel cell

libraries/SITL/SIM_IntelligentEnergy24.cpp

@@ -29,13 +29,15 @@ 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),
 
@@ -70,9 +72,9 @@ void IntelligentEnergy24::update(const struct sitl_input &input)
 
 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 +82,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 +92,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,9 +114,12 @@ void IntelligentEnergy24::update_send()
         state = 2; // Running
     }
 
-    last_sent_ms = now;
+    last_data_sent_ms = now;
 
     char message[128];
+
+    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,
@@ -124,6 +129,54 @@ 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,,", // 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)
+
+        // 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));
     }

libraries/SITL/SIM_IntelligentEnergy24.h

@@ -67,7 +67,8 @@ private:
     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;
 
 };