diff --git a/libraries/SITL/SIM_IntelligentEnergy24.cpp b/libraries/SITL/SIM_IntelligentEnergy24.cpp index 2445df9a86..4c87684978 100644 --- a/libraries/SITL/SIM_IntelligentEnergy24.cpp +++ b/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,10 +114,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,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)); } diff --git a/libraries/SITL/SIM_IntelligentEnergy24.h b/libraries/SITL/SIM_IntelligentEnergy24.h index eecd0939e2..99434758a4 100644 --- a/libraries/SITL/SIM_IntelligentEnergy24.h +++ b/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; };