SITL: add simulator for IntelligentEnergy 2.4kWh

SITL: Added setup note to comment

SITL: IE24: Add Error param and cycle battery pwr

libraries/SITL/SIM_Aircraft.cpp

@@ -886,6 +886,11 @@ void Aircraft::update_external_payload(const struct sitl_input &input)
     }
 
     sitl->shipsim.update();
+
+    // update IntelligentEnergy 2.4kW generator
+    if (ie24) {
+        ie24->update(input);
+    }
 }
 
 void Aircraft::add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel)

libraries/SITL/SIM_Aircraft.h

@@ -143,6 +143,7 @@ public:
     void set_sprayer(Sprayer *_sprayer) { sprayer = _sprayer; }
     void set_parachute(Parachute *_parachute) { parachute = _parachute; }
     void set_richenpower(RichenPower *_richenpower) { richenpower = _richenpower; }
+    void set_ie24(IntelligentEnergy24 *_ie24) { ie24 = _ie24; }
     void set_gripper_servo(Gripper_Servo *_gripper) { gripper = _gripper; }
     void set_gripper_epm(Gripper_EPM *_gripper_epm) { gripper_epm = _gripper_epm; }
     void set_precland(SIM_Precland *_precland);
@@ -322,6 +323,7 @@ private:
     Gripper_EPM *gripper_epm;
     Parachute *parachute;
     RichenPower *richenpower;
+    IntelligentEnergy24 *ie24;
     SIM_Precland *precland;
     class I2C *i2c;
 };

libraries/SITL/SIM_IntelligentEnergy.cpp

@@ -0,0 +1,28 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the RichenPower Hybrid generators
+*/
+
+#include <AP_Math/AP_Math.h>
+
+#include "SIM_IntelligentEnergy.h"
+#include "SITL.h"
+#include <AP_HAL/utility/sparse-endian.h>
+
+#include <stdio.h>
+#include <errno.h>
+
+using namespace SITL;

libraries/SITL/SIM_IntelligentEnergy.h

@@ -0,0 +1,39 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  Base class for simulator for the IntelligentEnergy Hybrid generators
+*/
+
+#pragma once
+
+#include "SITL_Input.h"
+
+#include "SIM_SerialDevice.h"
+
+#include <stdio.h>
+
+namespace SITL {
+
+class IntelligentEnergy : public SerialDevice {
+public:
+    // update state
+    virtual void update(const struct sitl_input &input) = 0;
+
+protected:
+
+    IntelligentEnergy() {}
+};
+
+}

libraries/SITL/SIM_IntelligentEnergy24.cpp

@@ -0,0 +1,128 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the IntelligentEnergy 2.4kWh FuelCell generator
+*/
+
+#include <AP_Math/AP_Math.h>
+
+#include "SIM_IntelligentEnergy24.h"
+#include "SITL.h"
+
+#include <errno.h>
+
+#include <GCS_MAVLink/GCS.h>
+
+extern const AP_HAL::HAL& hal;
+
+using namespace SITL;
+
+// 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
+    // @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
+    // @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
+    // @User: Advanced
+    AP_GROUPINFO("ERROR", 3, IntelligentEnergy24, err_code, 0),
+
+    AP_GROUPEND
+};
+
+IntelligentEnergy24::IntelligentEnergy24() : IntelligentEnergy::IntelligentEnergy()
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+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:
+    const uint32_t now = AP_HAL::millis();
+    if (now - last_sent_ms < 500) {
+        return;
+    }
+
+    // Simulate constant current charge/discharge of the battery
+    float amps = discharge ? -20.0f : 20.0f;
+
+    // Simulate constant tank pressure. This isn't true in reality, but is good enough
+    const int16_t tank_bar = 250;
+
+    // Update pack capacity remaining
+    bat_capacity_mAh += amps*(now - last_sent_ms)/3600.0f;
+
+    // From capacity remaining approximate voltage by linear interpolation
+    const float min_bat_vol = 42.0f;
+    const float max_bat_vol = 50.4f;
+    const float max_bat_capactiy_mAh = 3300;
+
+    battery_voltage = bat_capacity_mAh / max_bat_capactiy_mAh * (max_bat_vol - min_bat_vol) + min_bat_vol;
+
+    // Decide if we need to charge or discharge the battery
+    if (battery_voltage <= min_bat_vol) {
+        discharge = false;
+    } else if (battery_voltage >= max_bat_vol) {
+        discharge = true;
+    }
+
+    int32_t battery_pwr = battery_voltage * amps; // Watts
+
+    // These are non-physical values
+    const int32_t pwr_out = battery_pwr*1.4f;
+    const uint32_t spm_pwr = battery_pwr*0.3f;
+
+    uint32_t state = set_state;
+    if (set_state == -1) {
+        state = 2; // Running
+    }
+
+    last_sent_ms = now;
+
+    char message[128];
+    hal.util->snprintf(message, ARRAY_SIZE(message), "<%i,%.1f,%i,%u,%i,%u,%u>\n",
+             tank_bar,
+             battery_voltage,
+             pwr_out,
+             spm_pwr,
+             battery_pwr,
+             state,
+             (uint32_t)err_code);
+
+    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

@@ -0,0 +1,72 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the IntelligentEnergy 2.4kW FuelCell
+
+./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:ie24 --speedup=1 --console
+
+param set SERIAL5_PROTOCOL 30  # Generator
+param set SERIAL5_BAUD 115200
+param set GEN_TYPE 2  # IE24
+param set BATT2_MONITOR 17  # electrical
+param set SIM_IE24_ENABLE 1
+param fetch
+
+graph BATTERY_STATUS.voltages[0]
+
+reboot
+
+# TODO: ./Tools/autotest/autotest.py --gdb --debug build.ArduCopter fly.ArduCopter.IntelligentEnergy24
+
+*/
+
+#pragma once
+
+#include <AP_Param/AP_Param.h>
+
+#include "SITL_Input.h"
+
+#include "SIM_IntelligentEnergy.h"
+
+#include <stdio.h>
+
+namespace SITL {
+
+class IntelligentEnergy24 : public IntelligentEnergy {
+public:
+
+    IntelligentEnergy24();
+
+    // update state
+    void update(const struct sitl_input &input) override;
+
+    static const AP_Param::GroupInfo var_info[];
+
+private:
+
+    void update_send();
+
+    AP_Int8 enabled;  // enable sim
+    AP_Int8 set_state;
+    AP_Int32 err_code;
+
+    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;
+
+};
+
+}

libraries/SITL/SIM_RichenPower.h

@@ -24,6 +24,7 @@ param set SERVO8_FUNCTION 42
 param fetch
 param set SIM_RICH_CTRL 8
 param set RC9_OPTION 85
+param set GEN_TYPE 3
 
 reboot
 

libraries/SITL/SITL.cpp

@@ -226,6 +226,9 @@ const AP_Param::GroupInfo SITL::var_info3[] = {
     // @Path: ./SIM_RichenPower.cpp
     AP_SUBGROUPINFO(richenpower_sim, "RICH_", 31, SITL, RichenPower),
 
+    // @Path: ./SIM_IntelligentEnergy24.cpp
+    AP_SUBGROUPINFO(ie24_sim, "IE24_", 32, SITL, IntelligentEnergy24),
+
     // user settable parameters for the 1st barometer
     AP_GROUPINFO("BARO_RND",      35, SITL,  baro_noise[0], 0.2f),
     AP_GROUPINFO("BARO_DRIFT",    36, SITL,  baro_drift[0], 0),

libraries/SITL/SITL.h

@@ -19,6 +19,7 @@
 #include "SIM_ToneAlarm.h"
 #include "SIM_EFI_MegaSquirt.h"
 #include "SIM_RichenPower.h"
+#include "SIM_IntelligentEnergy24.h"
 #include "SIM_Ship.h"
 #include <AP_RangeFinder/AP_RangeFinder.h>
 
@@ -397,6 +398,7 @@ public:
     ToneAlarm tonealarm_sim;
     SIM_Precland precland_sim;
     RichenPower richenpower_sim;
+    IntelligentEnergy24 ie24_sim;
 
     struct {
         // LED state, for serial LED emulation