From f7b2856af0868dba8307ff5e514e0e5ee6508158 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 23 Sep 2020 11:03:38 +1000 Subject: [PATCH] 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 | 5 + libraries/SITL/SIM_Aircraft.h | 2 + libraries/SITL/SIM_IntelligentEnergy.cpp | 28 +++++ libraries/SITL/SIM_IntelligentEnergy.h | 39 +++++++ libraries/SITL/SIM_IntelligentEnergy24.cpp | 128 +++++++++++++++++++++ libraries/SITL/SIM_IntelligentEnergy24.h | 72 ++++++++++++ libraries/SITL/SIM_RichenPower.h | 1 + libraries/SITL/SITL.cpp | 3 + libraries/SITL/SITL.h | 2 + 9 files changed, 280 insertions(+) create mode 100644 libraries/SITL/SIM_IntelligentEnergy.cpp create mode 100644 libraries/SITL/SIM_IntelligentEnergy.h create mode 100644 libraries/SITL/SIM_IntelligentEnergy24.cpp create mode 100644 libraries/SITL/SIM_IntelligentEnergy24.h diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 80aaabcf1f..019290a5e2 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/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) diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 670f8ba4eb..07ed075f92 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/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; }; diff --git a/libraries/SITL/SIM_IntelligentEnergy.cpp b/libraries/SITL/SIM_IntelligentEnergy.cpp new file mode 100644 index 0000000000..c14d63eb7c --- /dev/null +++ b/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 . + */ +/* + Simulator for the RichenPower Hybrid generators +*/ + +#include + +#include "SIM_IntelligentEnergy.h" +#include "SITL.h" +#include + +#include +#include + +using namespace SITL; diff --git a/libraries/SITL/SIM_IntelligentEnergy.h b/libraries/SITL/SIM_IntelligentEnergy.h new file mode 100644 index 0000000000..01ebc1a18b --- /dev/null +++ b/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 . + */ +/* + Base class for simulator for the IntelligentEnergy Hybrid generators +*/ + +#pragma once + +#include "SITL_Input.h" + +#include "SIM_SerialDevice.h" + +#include + +namespace SITL { + +class IntelligentEnergy : public SerialDevice { +public: + // update state + virtual void update(const struct sitl_input &input) = 0; + +protected: + + IntelligentEnergy() {} +}; + +} diff --git a/libraries/SITL/SIM_IntelligentEnergy24.cpp b/libraries/SITL/SIM_IntelligentEnergy24.cpp new file mode 100644 index 0000000000..6669e4fa3a --- /dev/null +++ b/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 . + */ +/* + Simulator for the IntelligentEnergy 2.4kWh FuelCell generator +*/ + +#include + +#include "SIM_IntelligentEnergy24.h" +#include "SITL.h" + +#include + +#include + +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)); + } +} diff --git a/libraries/SITL/SIM_IntelligentEnergy24.h b/libraries/SITL/SIM_IntelligentEnergy24.h new file mode 100644 index 0000000000..0c12636ec2 --- /dev/null +++ b/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 . + */ +/* + 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 + +#include "SITL_Input.h" + +#include "SIM_IntelligentEnergy.h" + +#include + +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; + +}; + +} diff --git a/libraries/SITL/SIM_RichenPower.h b/libraries/SITL/SIM_RichenPower.h index 80489a24a5..63c7f4591c 100644 --- a/libraries/SITL/SIM_RichenPower.h +++ b/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 diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 391eca6d32..c6e25ec9ca 100644 --- a/libraries/SITL/SITL.cpp +++ b/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), diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 21467537c6..e3fbcaff57 100644 --- a/libraries/SITL/SITL.h +++ b/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 @@ -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