From 12a587f778dc3e670a7e11860bc1769fba7c937b Mon Sep 17 00:00:00 2001 From: Hunter McClelland Date: Thu, 5 Feb 2026 13:53:11 -0500 Subject: [PATCH 01/11] SITL: split Frame's battery management into smaller parts --- libraries/SITL/SIM_Frame.cpp | 19 ++++++++++++++++--- libraries/SITL/SIM_Frame.h | 2 ++ 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index b9db8a731086b..859ea9cad1fcd 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -728,8 +728,8 @@ void Frame::calculate_forces(const Aircraft &aircraft, } -// calculate current and voltage -void Frame::current_and_voltage(float &voltage, float ¤t) +// reset battery if its governing params have changed +void Frame::reset_battery_if_requested(void) { float param_voltage = AP::sitl()->batt_voltage; if (!is_equal(last_param_voltage,param_voltage)) { @@ -740,10 +740,23 @@ void Frame::current_and_voltage(float &voltage, float ¤t) if (!is_equal(battery->get_capacity(), param_capacity)) { battery->init_capacity(param_capacity); } +} + +// calculate current and voltage +void Frame::current_and_voltage(float &voltage, float ¤t) +{ + reset_battery_if_requested(); voltage = battery->get_voltage(); - current = 0; + current = get_current_amp(); +} + +// computes (total) instantaneous current +float Frame::get_current_amp(void) +{ + float current = 0; for (uint8_t i=0; i Date: Thu, 5 Feb 2026 13:56:59 -0500 Subject: [PATCH 02/11] SITL: move battery managagement out of Frame to Aircraft-children Specifically, the children of Aircraft which have a Frame, naturally. --- libraries/SITL/SIM_Frame.cpp | 8 -------- libraries/SITL/SIM_Frame.h | 2 -- libraries/SITL/SIM_Multicopter.cpp | 4 +++- libraries/SITL/SIM_QuadPlane.cpp | 4 +++- 4 files changed, 6 insertions(+), 12 deletions(-) diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index 859ea9cad1fcd..de09023a1d89c 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -742,14 +742,6 @@ void Frame::reset_battery_if_requested(void) } } -// calculate current and voltage -void Frame::current_and_voltage(float &voltage, float ¤t) -{ - reset_battery_if_requested(); - voltage = battery->get_voltage(); - current = get_current_amp(); -} - // computes (total) instantaneous current float Frame::get_current_amp(void) { diff --git a/libraries/SITL/SIM_Frame.h b/libraries/SITL/SIM_Frame.h index f8f722451cd28..87ac2713c2123 100644 --- a/libraries/SITL/SIM_Frame.h +++ b/libraries/SITL/SIM_Frame.h @@ -63,9 +63,7 @@ class Frame { uint8_t motor_offset; void reset_battery_if_requested(void); - // calculate current and voltage float get_current_amp(void); - void current_and_voltage(float &voltage, float ¤t); // get mass in kg float get_mass(void) const { diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 30d0cfef7ed62..549a0d249d9fe 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -71,7 +71,9 @@ void MultiCopter::update(const struct sitl_input &input) } // estimate voltage and current - frame->current_and_voltage(battery_voltage, battery_current); + frame->reset_battery_if_requested(); + battery_voltage = battery.get_voltage(); + battery_current = frame->get_current_amp(); battery.set_current(battery_current); diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp index 924e1af4be6b4..8d9e69f03e07e 100644 --- a/libraries/SITL/SIM_QuadPlane.cpp +++ b/libraries/SITL/SIM_QuadPlane.cpp @@ -136,7 +136,9 @@ void QuadPlane::update(const struct sitl_input &input) } // estimate voltage and current - frame->current_and_voltage(battery_voltage, battery_current); + frame->reset_battery_if_requested(); + battery_voltage = battery.get_voltage(); + battery_current = frame->get_current_amp(); battery.set_current(battery_current); From 275e975bb414578aa498aa1d908126e0288e344f Mon Sep 17 00:00:00 2001 From: Hunter McClelland Date: Fri, 6 Feb 2026 11:09:22 -0500 Subject: [PATCH 03/11] SITL: refactor SITL::Plane's battery consumption up one level --- libraries/SITL/SIM_Plane.cpp | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp index 6a0322c84a999..f3b18fb457cae 100644 --- a/libraries/SITL/SIM_Plane.cpp +++ b/libraries/SITL/SIM_Plane.cpp @@ -401,7 +401,6 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel float elevator = filtered_servo_angle(input, 1); float rudder = filtered_servo_angle(input, 3); bool launch_triggered = input.servos[6] > 1700; - float throttle; if (reverse_elevator_rudder) { elevator = -elevator; rudder = -rudder; @@ -444,16 +443,7 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel } //printf("Aileron: %.1f elevator: %.1f rudder: %.1f\n", aileron, elevator, rudder); - if (reverse_thrust) { - throttle = filtered_servo_angle(input, 2); - } else { - throttle = filtered_servo_range(input, 2); - } - - float thrust = throttle; - - battery_voltage = sitl->batt_voltage - 0.7*throttle; - battery_current = (battery_voltage/sitl->batt_voltage)*50.0f*sq(throttle); + float thrust = reverse_thrust ? filtered_servo_angle(input, 2) : filtered_servo_range(input, 2); if (ice_engine) { thrust = icengine.update(input); @@ -526,7 +516,11 @@ void Plane::update(const struct sitl_input &input) update_wind(input); calculate_forces(input, rot_accel); - + + float throttle = reverse_thrust ? filtered_servo_angle(input, 2) : filtered_servo_range(input, 2); + battery_voltage = sitl->batt_voltage - 0.7*throttle; + battery_current = (battery_voltage/sitl->batt_voltage)*50.0f*sq(throttle); + update_dynamics(rot_accel); /* From c1e993813368bf317c1e3294fad10ecbce687826 Mon Sep 17 00:00:00 2001 From: Hunter McClelland Date: Tue, 3 Feb 2026 08:20:36 -0500 Subject: [PATCH 04/11] SITL: SIM::Battery manages its own initial voltage --- libraries/SITL/SIM_Battery.cpp | 2 ++ libraries/SITL/SIM_Battery.h | 2 ++ libraries/SITL/SIM_Frame.cpp | 6 +++--- libraries/SITL/SIM_Frame.h | 1 - 4 files changed, 7 insertions(+), 4 deletions(-) diff --git a/libraries/SITL/SIM_Battery.cpp b/libraries/SITL/SIM_Battery.cpp index 7618745cf2796..de9d804e143bb 100644 --- a/libraries/SITL/SIM_Battery.cpp +++ b/libraries/SITL/SIM_Battery.cpp @@ -119,6 +119,8 @@ void Battery::setup(float _capacity_Ah, float _resistance, float _max_voltage) capacity_Ah = _capacity_Ah; resistance = _resistance; max_voltage = _max_voltage; + // Arbitrarily setup battery initially as depleted + init_voltage(0.0f); } void Battery::init_voltage(float voltage) diff --git a/libraries/SITL/SIM_Battery.h b/libraries/SITL/SIM_Battery.h index 46a686eaaf02a..2a53bef52db89 100644 --- a/libraries/SITL/SIM_Battery.h +++ b/libraries/SITL/SIM_Battery.h @@ -35,6 +35,8 @@ class Battery { // return battery temperature in Kelvin: float get_temperature(void) const { return temperature.kelvin; } + // useful for detecting if param-controlled batt voltage has been changed + float get_init_voltage(void) const { return voltage_set; } private: float capacity_Ah; diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index de09023a1d89c..ad7735568d129 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -731,10 +731,10 @@ void Frame::calculate_forces(const Aircraft &aircraft, // reset battery if its governing params have changed void Frame::reset_battery_if_requested(void) { - float param_voltage = AP::sitl()->batt_voltage; - if (!is_equal(last_param_voltage,param_voltage)) { + // Treat a param-change as a forced re-init of battery + const float param_voltage = AP::sitl()->batt_voltage; + if (!is_equal(battery->get_init_voltage(), param_voltage)) { battery->init_voltage(param_voltage); - last_param_voltage = param_voltage; } const float param_capacity = AP::sitl()->batt_capacity_ah; if (!is_equal(battery->get_capacity(), param_capacity)) { diff --git a/libraries/SITL/SIM_Frame.h b/libraries/SITL/SIM_Frame.h index 87ac2713c2123..e64f3513aaf3f 100644 --- a/libraries/SITL/SIM_Frame.h +++ b/libraries/SITL/SIM_Frame.h @@ -158,7 +158,6 @@ class Frame { // exposed area times coefficient of drag float areaCd; float mass; - float last_param_voltage; #if AP_SIM_ENABLED Battery *battery; #endif From f6c2ede9c8d1c0c39a9b0c52fa44fdff5d87c5d7 Mon Sep 17 00:00:00 2001 From: Hunter McClelland Date: Thu, 5 Feb 2026 14:07:18 -0500 Subject: [PATCH 05/11] SITL: make Aircraft responsible for managing battery resets --- libraries/SITL/SIM_Aircraft.cpp | 16 ++++++++++++++++ libraries/SITL/SIM_Aircraft.h | 2 ++ libraries/SITL/SIM_Frame.cpp | 15 --------------- libraries/SITL/SIM_Frame.h | 1 - libraries/SITL/SIM_Multicopter.cpp | 3 +-- libraries/SITL/SIM_QuadPlane.cpp | 3 +-- 6 files changed, 20 insertions(+), 20 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 28be7e2baeebb..a4ed13cad8f49 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -1415,6 +1415,22 @@ bool Aircraft::set_pose(uint8_t instance, const Location &loc, const Quaternion return true; } +/* + Detect if the sim params controlling the battery have changed (e.g. by user) & respond +*/ +void Aircraft::reinitialize_battery_if_param_has_changed(void) +{ + // Treat a param-change as a forced re-init of battery + const float param_voltage = sitl->batt_voltage; + if (!is_equal(battery.get_init_voltage(), param_voltage)) { + battery.init_voltage(param_voltage); + } + const float param_capacity = sitl->batt_capacity_ah; + if (!is_equal(battery.get_capacity(), param_capacity)) { + battery.init_capacity(param_capacity); + } +} + /* wrapper for scripting access */ diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 13992dfb2b35b..3b01adf920a11 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -359,6 +359,8 @@ class Aircraft { // update EAS speeds void update_eas_airspeed(); + void reinitialize_battery_if_param_has_changed(void); + // clamp support class Clamp { public: diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index ad7735568d129..ddc58465784ac 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -727,21 +727,6 @@ void Frame::calculate_forces(const Aircraft &aircraft, body_accel = thrust/aircraft.gross_mass(); } - -// reset battery if its governing params have changed -void Frame::reset_battery_if_requested(void) -{ - // Treat a param-change as a forced re-init of battery - const float param_voltage = AP::sitl()->batt_voltage; - if (!is_equal(battery->get_init_voltage(), param_voltage)) { - battery->init_voltage(param_voltage); - } - const float param_capacity = AP::sitl()->batt_capacity_ah; - if (!is_equal(battery->get_capacity(), param_capacity)) { - battery->init_capacity(param_capacity); - } -} - // computes (total) instantaneous current float Frame::get_current_amp(void) { diff --git a/libraries/SITL/SIM_Frame.h b/libraries/SITL/SIM_Frame.h index e64f3513aaf3f..a8b007bcd76d2 100644 --- a/libraries/SITL/SIM_Frame.h +++ b/libraries/SITL/SIM_Frame.h @@ -62,7 +62,6 @@ class Frame { float terminal_rotation_rate; uint8_t motor_offset; - void reset_battery_if_requested(void); float get_current_amp(void); // get mass in kg diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 549a0d249d9fe..2bc4413d73b1b 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -70,8 +70,7 @@ void MultiCopter::update(const struct sitl_input &input) accel_body.zero(); } - // estimate voltage and current - frame->reset_battery_if_requested(); + reinitialize_battery_if_param_has_changed(); battery_voltage = battery.get_voltage(); battery_current = frame->get_current_amp(); diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp index 8d9e69f03e07e..b7d656d16821b 100644 --- a/libraries/SITL/SIM_QuadPlane.cpp +++ b/libraries/SITL/SIM_QuadPlane.cpp @@ -135,8 +135,7 @@ void QuadPlane::update(const struct sitl_input &input) quad_accel_body.rotate(ROTATION_PITCH_270); } - // estimate voltage and current - frame->reset_battery_if_requested(); + reinitialize_battery_if_param_has_changed(); battery_voltage = battery.get_voltage(); battery_current = frame->get_current_amp(); From ac72c9a02a7224d99c80cc44fc4bbafc531726a9 Mon Sep 17 00:00:00 2001 From: Hunter McClelland Date: Thu, 5 Feb 2026 14:25:43 -0500 Subject: [PATCH 06/11] SITL: Frame now does not use/have a SIM::Battery It uses the batt owned by Aircraft-children which also have a Frame. --- libraries/SITL/SIM_Frame.cpp | 8 +++----- libraries/SITL/SIM_Frame.h | 13 ++++++++----- libraries/SITL/SIM_Multicopter.cpp | 5 ++++- libraries/SITL/SIM_QuadPlane.cpp | 5 ++++- 4 files changed, 19 insertions(+), 12 deletions(-) diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index ddc58465784ac..02db5af0835aa 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -577,10 +577,9 @@ void Frame::parse_vector3(AP_JSON::value val, const char* label, Vector3f ¶m /* initialise the frame */ -void Frame::init(const char *frame_str, Battery *_battery) +void Frame::init(const char *frame_str) { model = default_model; - battery = _battery; const char *colon = strchr(frame_str, ':'); size_t slen = strlen(frame_str); @@ -620,8 +619,6 @@ void Frame::init(const char *frame_str, Battery *_battery) // power_factor is ratio of power consumed per newton of thrust float power_factor = hover_power / hover_thrust; - battery->setup(model.battCapacityAh, model.refBatRes, model.maxVoltage); - if (uint8_t(model.num_motors) != num_motors) { ::printf("Warning model expected %u motors and got %u\n", uint8_t(model.num_motors), num_motors); } @@ -682,7 +679,8 @@ void Frame::calculate_forces(const Aircraft &aircraft, const auto *_sitl = AP::sitl(); for (uint8_t i=0; iget_voltage(), use_drag); + motors[i].calculate_forces(input, motor_offset, mtorque, mthrust, vel_air_bf, + gyro, air_density, aircraft.get_battery_voltage(), use_drag); torque += mtorque; thrust += mthrust; // simulate motor rpm diff --git a/libraries/SITL/SIM_Frame.h b/libraries/SITL/SIM_Frame.h index a8b007bcd76d2..9172ce64952ae 100644 --- a/libraries/SITL/SIM_Frame.h +++ b/libraries/SITL/SIM_Frame.h @@ -49,7 +49,7 @@ class Frame { static Frame *create_frame(const char *name); // initialise frame - void init(const char *frame_str, Battery *_battery); + void init(const char *frame_str); // calculate rotational and linear accelerations void calculate_forces(const Aircraft &aircraft, @@ -73,6 +73,10 @@ class Frame { void set_mass(float new_mass) { mass = new_mass; } + + float get_model_batt_max_voltage(void) const { return model.maxVoltage; } + float get_model_batt_capacity_ah(void) const { return model.battCapacityAh; } + float get_model_batt_resistance_ohm(void) const { return model.refBatRes; } private: /* @@ -95,7 +99,6 @@ class Frame { float refCurrent = 29.3; // Amps float refAlt = 593; // altitude AMSL float refTempC = 25; // temperature C - float refBatRes = 0.01; // BAT.Res // full pack voltage float maxVoltage = 4.2*3; @@ -103,6 +106,9 @@ class Frame { // battery capacity in Ah. Use zero for unlimited float battCapacityAh = 0.0; + // battery resistance reference value in Ohms + float refBatRes = 0.01; + // CTUN.ThO at hover at refAlt float hoverThrOut = 0.39; @@ -157,9 +163,6 @@ class Frame { // exposed area times coefficient of drag float areaCd; float mass; -#if AP_SIM_ENABLED - Battery *battery; -#endif // json parsing helpers void parse_float(AP_JSON::value val, const char* label, float ¶m); diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 2bc4413d73b1b..37f2e02189a51 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -32,7 +32,10 @@ MultiCopter::MultiCopter(const char *frame_str) : exit(1); } - frame->init(frame_str, &battery); + frame->init(frame_str); + battery.setup(frame->get_model_batt_capacity_ah(), + frame->get_model_batt_resistance_ohm(), + frame->get_model_batt_max_voltage()); mass = frame->get_mass(); frame_height = 0.1; diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp index b7d656d16821b..752f32aca9631 100644 --- a/libraries/SITL/SIM_QuadPlane.cpp +++ b/libraries/SITL/SIM_QuadPlane.cpp @@ -101,7 +101,10 @@ QuadPlane::QuadPlane(const char *frame_str) : frame->motor_offset = motor_offset; // we use zero terminal velocity to let the plane model handle the drag - frame->init(frame_str, &battery); + frame->init(frame_str); + battery.setup(frame->get_model_batt_capacity_ah(), + frame->get_model_batt_resistance_ohm(), + frame->get_model_batt_max_voltage()); // increase mass for plane components mass = frame->get_mass() * 1.5; From ea555a70ae4c6c14867607fb5d80316b538ef657 Mon Sep 17 00:00:00 2001 From: Hunter McClelland Date: Thu, 5 Feb 2026 19:26:11 -0500 Subject: [PATCH 07/11] SITL: put battery maybe-reset logic into SITL::Battery --- libraries/SITL/SIM_Aircraft.cpp | 16 ---------------- libraries/SITL/SIM_Aircraft.h | 2 -- libraries/SITL/SIM_Battery.cpp | 22 ++++++++++++++++++++-- libraries/SITL/SIM_Battery.h | 7 ++++--- libraries/SITL/SIM_Multicopter.cpp | 2 +- libraries/SITL/SIM_QuadPlane.cpp | 2 +- 6 files changed, 26 insertions(+), 25 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index a4ed13cad8f49..28be7e2baeebb 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -1415,22 +1415,6 @@ bool Aircraft::set_pose(uint8_t instance, const Location &loc, const Quaternion return true; } -/* - Detect if the sim params controlling the battery have changed (e.g. by user) & respond -*/ -void Aircraft::reinitialize_battery_if_param_has_changed(void) -{ - // Treat a param-change as a forced re-init of battery - const float param_voltage = sitl->batt_voltage; - if (!is_equal(battery.get_init_voltage(), param_voltage)) { - battery.init_voltage(param_voltage); - } - const float param_capacity = sitl->batt_capacity_ah; - if (!is_equal(battery.get_capacity(), param_capacity)) { - battery.init_capacity(param_capacity); - } -} - /* wrapper for scripting access */ diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 3b01adf920a11..13992dfb2b35b 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -359,8 +359,6 @@ class Aircraft { // update EAS speeds void update_eas_airspeed(); - void reinitialize_battery_if_param_has_changed(void); - // clamp support class Clamp { public: diff --git a/libraries/SITL/SIM_Battery.cpp b/libraries/SITL/SIM_Battery.cpp index de9d804e143bb..65b7b22815bc3 100644 --- a/libraries/SITL/SIM_Battery.cpp +++ b/libraries/SITL/SIM_Battery.cpp @@ -17,6 +17,7 @@ */ #include "SIM_Battery.h" +#include using namespace SITL; @@ -119,8 +120,25 @@ void Battery::setup(float _capacity_Ah, float _resistance, float _max_voltage) capacity_Ah = _capacity_Ah; resistance = _resistance; max_voltage = _max_voltage; - // Arbitrarily setup battery initially as depleted - init_voltage(0.0f); + + voltage_set = max_voltage; + voltage_filter.reset(voltage_set); + set_initial_SoC(voltage_set); +} + +void Battery::maybe_reset(float desired_voltage, float desired_capacity_Ah) +{ + const bool reset_not_needed = (is_equal(voltage_set, desired_voltage) + && is_equal(capacity_Ah, desired_capacity_Ah)); + if (reset_not_needed) { + return; + } + + capacity_Ah = desired_capacity_Ah; + // a negative desired voltage is unexpected, but not problematic + voltage_set = MIN(desired_voltage, max_voltage); + voltage_filter.reset(voltage_set); + set_initial_SoC(voltage_set); } void Battery::init_voltage(float voltage) diff --git a/libraries/SITL/SIM_Battery.h b/libraries/SITL/SIM_Battery.h index 2a53bef52db89..3336530b6f16c 100644 --- a/libraries/SITL/SIM_Battery.h +++ b/libraries/SITL/SIM_Battery.h @@ -26,17 +26,18 @@ class Battery { public: void setup(float _capacity_Ah, float _resistance, float _max_voltage); + // Implements the (re)setting behavior described by SIM_BATT_* parameters + // (reminder: if the desired values are different than previous choices, reset) + void maybe_reset(float desired_voltage, float desired_capacity_Ah); + void init_voltage(float voltage); void init_capacity(float capacity); void set_current(float current_amps); float get_voltage(void) const; - float get_capacity(void) const { return capacity_Ah; } // return battery temperature in Kelvin: float get_temperature(void) const { return temperature.kelvin; } - // useful for detecting if param-controlled batt voltage has been changed - float get_init_voltage(void) const { return voltage_set; } private: float capacity_Ah; diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 37f2e02189a51..2d08273d7b7d0 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -73,7 +73,7 @@ void MultiCopter::update(const struct sitl_input &input) accel_body.zero(); } - reinitialize_battery_if_param_has_changed(); + battery.maybe_reset(sitl->batt_voltage, sitl->batt_capacity_ah); battery_voltage = battery.get_voltage(); battery_current = frame->get_current_amp(); diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp index 752f32aca9631..1a9991b630877 100644 --- a/libraries/SITL/SIM_QuadPlane.cpp +++ b/libraries/SITL/SIM_QuadPlane.cpp @@ -138,7 +138,7 @@ void QuadPlane::update(const struct sitl_input &input) quad_accel_body.rotate(ROTATION_PITCH_270); } - reinitialize_battery_if_param_has_changed(); + battery.maybe_reset(sitl->batt_voltage, sitl->batt_capacity_ah); battery_voltage = battery.get_voltage(); battery_current = frame->get_current_amp(); From dc4ab144a43e87898eb52a86baa465c28cabe961 Mon Sep 17 00:00:00 2001 From: Hunter McClelland Date: Thu, 5 Feb 2026 14:58:38 -0500 Subject: [PATCH 08/11] SITL: [minor] remove an unused string, mark it unused in ctor --- libraries/SITL/SIM_Aircraft.cpp | 3 +-- libraries/SITL/SIM_Aircraft.h | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 28be7e2baeebb..73a38bf5d69e5 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -50,8 +50,7 @@ Aircraft *Aircraft::instances[MAX_SIM_INSTANCES]; parent class for all simulator types */ -Aircraft::Aircraft(const char *frame_str) : - frame(frame_str) +Aircraft::Aircraft(const char *unused_frame_str) { // make the SIM_* variables available to simulator backends sitl = AP::sitl(); diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 13992dfb2b35b..55ff2744bd3de 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -53,7 +53,7 @@ namespace SITL { */ class Aircraft { public: - Aircraft(const char *frame_str); + Aircraft(const char *unused_frame_str); // called directly after constructor: virtual void set_start_location(const Location &start_loc, const float start_yaw); @@ -274,7 +274,6 @@ class Aircraft { uint32_t last_frame_count; uint8_t instance; const char *autotest_dir; - const char *frame; bool use_time_sync = true; float last_speedup = -1.0f; const char *config_ = ""; From e8a13136f01bbc1a80ecdd38cbb91b1f3a3a1ca2 Mon Sep 17 00:00:00 2001 From: Hunter McClelland Date: Thu, 5 Feb 2026 18:13:10 -0500 Subject: [PATCH 09/11] SITL: improve SITL::Battery in multiple ways --- libraries/SITL/SIM_Battery.cpp | 95 ++++++++++--------- libraries/SITL/SIM_Battery.h | 27 +++--- libraries/SITL/SIM_Multicopter.cpp | 2 +- libraries/SITL/SIM_QuadPlane.cpp | 2 +- libraries/SITL/SITL.cpp | 4 +- .../EvaluateBatteryModel.cpp | 3 +- 6 files changed, 69 insertions(+), 64 deletions(-) diff --git a/libraries/SITL/SIM_Battery.cpp b/libraries/SITL/SIM_Battery.cpp index 65b7b22815bc3..7881aa2c97064 100644 --- a/libraries/SITL/SIM_Battery.cpp +++ b/libraries/SITL/SIM_Battery.cpp @@ -17,6 +17,7 @@ */ #include "SIM_Battery.h" +#include #include using namespace SITL; @@ -72,9 +73,14 @@ static const struct { /* use table to get resting voltage from remaining capacity */ -float Battery::get_resting_voltage(float charge_pct) const +float Battery::get_resting_voltage(void) const { + if (capacity_is_unlimited()) { + return voltage_set; + } + float charge_pct = 100 * remaining_Ah / capacity_Ah; const float max_cell_voltage = soc_table[0].volt_per_cell; + const float min_cell_voltage = soc_table[ARRAY_SIZE(soc_table) - 1].volt_per_cell; for (uint8_t i=1; i= soc_table[i].soc_pct) { // linear interpolation between table rows @@ -86,15 +92,21 @@ float Battery::get_resting_voltage(float charge_pct) const return (cell_volt / max_cell_voltage) * max_voltage; } } - // off the bottom of the table, return a small non-zero to prevent math errors - return 0.001; + // off the bottom of the table + return min_cell_voltage; } /* - use table to set initial state of charge from voltage + return remaining Amp-hours (aka "charge", "state of charge") corresponding to a voltage + + this is const for readability: the remaining_ah computation has no "side effects" */ -void Battery::set_initial_SoC(float voltage) +float Battery::compute_remaining_ah(float voltage) const { + if (capacity_is_unlimited()) { + return FLT_MAX; + } + const float max_cell_voltage = soc_table[0].volt_per_cell; float cell_volt = (voltage / max_voltage) * max_cell_voltage; @@ -106,24 +118,28 @@ void Battery::set_initial_SoC(float voltage) float soc1 = soc_table[i].soc_pct; float soc2 = soc_table[i-1].soc_pct; float soc = soc1 + (dv1 / dv2) * (soc2 - soc1); - remaining_Ah = capacity_Ah * soc * 0.01; - return; + return capacity_Ah * soc * 0.01; } } - // off the bottom of the table - remaining_Ah = 0; + return 0.0f; } -void Battery::setup(float _capacity_Ah, float _resistance, float _max_voltage) +void Battery::set_remaining_ah(void) +{ + remaining_Ah = compute_remaining_ah(voltage_set); +} + +// Reminder: capacity <= 0 means **unlimited** +void Battery::setup(float _capacity_Ah, float _resistance_ohm, float _max_voltage) { capacity_Ah = _capacity_Ah; - resistance = _resistance; + resistance_ohm = _resistance_ohm; max_voltage = _max_voltage; voltage_set = max_voltage; voltage_filter.reset(voltage_set); - set_initial_SoC(voltage_set); + set_remaining_ah(); } void Battery::maybe_reset(float desired_voltage, float desired_capacity_Ah) @@ -138,23 +154,10 @@ void Battery::maybe_reset(float desired_voltage, float desired_capacity_Ah) // a negative desired voltage is unexpected, but not problematic voltage_set = MIN(desired_voltage, max_voltage); voltage_filter.reset(voltage_set); - set_initial_SoC(voltage_set); + set_remaining_ah(); } -void Battery::init_voltage(float voltage) -{ - voltage_filter.reset(voltage); - voltage_set = voltage; - set_initial_SoC(voltage); -} - -void Battery::init_capacity(float capacity) -{ - capacity_Ah = capacity; - set_initial_SoC(voltage_set); -} - -void Battery::set_current(float current) +void Battery::consume_energy(float current_amps) { uint64_t now = AP_HAL::micros64(); float dt = (now - last_us) * 1.0e-6; @@ -163,31 +166,29 @@ void Battery::set_current(float current) dt = 0; } last_us = now; - float delta_Ah = current * dt / 3600; + float delta_Ah = current_amps * dt / 3600; remaining_Ah -= delta_Ah; remaining_Ah = MAX(0, remaining_Ah); - float voltage_delta = current * resistance; - float voltage; - if (!is_positive(capacity_Ah)) { - voltage = voltage_set; - } else { - voltage = get_resting_voltage(100 * remaining_Ah / capacity_Ah) - voltage_delta; - } - - voltage_filter.apply(voltage, dt); + float voltage_delta = current_amps * resistance_ohm; + float sagged_voltage = get_resting_voltage() - voltage_delta; + voltage_filter.apply(sagged_voltage, dt); - { - const uint64_t temperature_dt = now - temperature.last_update_micros; - temperature.last_update_micros = now; - // 1 amp*1 second == 0.1 degrees of energy. Did those units hurt? - temperature.kelvin += 0.1 * current * temperature_dt * 0.000001; - // decay temperature at some %second towards ambient - temperature.kelvin -= (temperature.kelvin - 273) * 0.10 * temperature_dt * 0.000001; - } + update_temperature(current_amps, now); } -float Battery::get_voltage(void) const +void Battery::update_temperature(float current_amps, uint64_t now) { - return voltage_filter.get(); + const float dt = (now - temperature.last_update_micros) * 1.0e-6; + temperature.last_update_micros = now; + + // temperature growth model + + // thermal_capacity value chosen to match previous steady-state behavior at 28amps + // (reminder: thermal_capacity = mass * specific_heat) + const float inverse_of_thermal_capacity = 0.36f; // use inverse so we can multiply, not divide + temperature.kelvin += (current_amps * current_amps) * resistance_ohm * dt * inverse_of_thermal_capacity; + + // temperature decay model: first-order + temperature.kelvin -= (temperature.kelvin - (ambient_temperature + 273.15f)) * 0.10f * dt; } diff --git a/libraries/SITL/SIM_Battery.h b/libraries/SITL/SIM_Battery.h index 3336530b6f16c..bf0a46a9412d3 100644 --- a/libraries/SITL/SIM_Battery.h +++ b/libraries/SITL/SIM_Battery.h @@ -22,40 +22,45 @@ namespace SITL { +constexpr float ambient_temperature = 0.0f; // degC + class Battery { public: - void setup(float _capacity_Ah, float _resistance, float _max_voltage); + // Note: capacity<=0 means **unlimited** capacity. + void setup(float _capacity_Ah, float _resistance_ohm, float _max_voltage); // Implements the (re)setting behavior described by SIM_BATT_* parameters // (reminder: if the desired values are different than previous choices, reset) void maybe_reset(float desired_voltage, float desired_capacity_Ah); - void init_voltage(float voltage); - void init_capacity(float capacity); + // Call this periodically to "step" the battery forward in time + void consume_energy(float current_amps); - void set_current(float current_amps); - float get_voltage(void) const; + float get_voltage(void) const { return voltage_filter.get(); } // return battery temperature in Kelvin: float get_temperature(void) const { return temperature.kelvin; } private: - float capacity_Ah; - float resistance; + float capacity_Ah; // set <= 0 for unlmited capacity + float resistance_ohm; float max_voltage; float voltage_set; - float remaining_Ah; + float remaining_Ah; // if capacity is unlimited, set this to FLT_MAX uint64_t last_us; struct { - float kelvin = 273; + float kelvin = ambient_temperature + 273.15f; uint64_t last_update_micros; } temperature; + void update_temperature(float current_amps, uint64_t now); // 10Hz filter for battery voltage LowPassFilterFloat voltage_filter{10}; - float get_resting_voltage(float charge_pct) const; - void set_initial_SoC(float voltage); + float get_resting_voltage(void) const; + float compute_remaining_ah(float voltage) const; // const shows this has no side effects + void set_remaining_ah(void); + bool capacity_is_unlimited(void) const { return !(is_positive(capacity_Ah)); } // for readability }; } diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 2d08273d7b7d0..de0af251fb8b7 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -77,7 +77,7 @@ void MultiCopter::update(const struct sitl_input &input) battery_voltage = battery.get_voltage(); battery_current = frame->get_current_amp(); - battery.set_current(battery_current); + battery.consume_energy(battery_current); update_dynamics(rot_accel); update_external_payload(input); diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp index 1a9991b630877..6f2408ae24411 100644 --- a/libraries/SITL/SIM_QuadPlane.cpp +++ b/libraries/SITL/SIM_QuadPlane.cpp @@ -142,7 +142,7 @@ void QuadPlane::update(const struct sitl_input &input) battery_voltage = battery.get_voltage(); battery_current = frame->get_current_amp(); - battery.set_current(battery_current); + battery.consume_energy(battery_current); float throttle; if (reverse_thrust) { diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index b12b1c872784c..71d4618de9098 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -117,13 +117,13 @@ const AP_Param::GroupInfo SIM::var_info[] = { AP_GROUPINFO("SONAR_ROT", 17, SIM, sonar_rot, Rotation::ROTATION_PITCH_270), // @Param: BATT_VOLTAGE // @DisplayName: Simulated battery voltage - // @Description: Simulated battery voltage. Constant voltage when SIM_BATT_CAP_AH is 0, otherwise changing this parameter will re-initialize the state of charge of the battery based on this voltage versus the battery's maximum voltage (default is max voltage). + // @Description: Simulated battery resting voltage. This value (re)sets the battery's resting voltage (before load sag). Simultaneously, it (re)sets the remaining charge: a value less than the max voltage (re)sets to a partially-charged battery. The maximum voltage of the internal battery model is the default value of this parameter. (Do not confuse the default value with the actual value. A few uncommon ways exist to set or modify the default.) A value greater than the max voltage is treated as equal to the max voltage. (This makes it easy to reset to fully charged.) For the special case of unlimited capacity, see `SIM_BATT_CAP_AH`. For the special case where an external source provides battery state, this parameter has no effect. // @Units: V // @User: Advanced AP_GROUPINFO("BATT_VOLTAGE", 19, SIM, batt_voltage, 12.6f), // @Param: BATT_CAP_AH // @DisplayName: Simulated battery capacity - // @Description: Simulated battery capacity. Set to 0 for unlimited capacity. Changing this parameter will re-initialize the state of charge of the battery. + // @Description: Simulated battery capacity. Changing this value (re)sets the battery's charge percentage, like `SIM_BATT_VOLTAGE` does. Set to 0 for unlimited capacity. An unlimited-capacity battery's instantaneous voltage may sag under load, but charge percentage (and thus resting voltage) remain constant. For the special case where an external source provides battery state, this parameter has no effect. // @Units: Ah // @User: Advanced AP_GROUPINFO("BATT_CAP_AH", 20, SIM, batt_capacity_ah, 0), diff --git a/libraries/SITL/examples/EvaluateBatteryModel/EvaluateBatteryModel.cpp b/libraries/SITL/examples/EvaluateBatteryModel/EvaluateBatteryModel.cpp index 1e131f758a794..bcbe8e2fac396 100644 --- a/libraries/SITL/examples/EvaluateBatteryModel/EvaluateBatteryModel.cpp +++ b/libraries/SITL/examples/EvaluateBatteryModel/EvaluateBatteryModel.cpp @@ -62,7 +62,6 @@ void setup(void) // setup battery model battery.setup(amp_hour_capacity, resistance, max_voltage); - battery.init_voltage(max_voltage); uint64_t time = 0; hal.scheduler->stop_clock(time); @@ -70,7 +69,7 @@ void setup(void) ::printf("time, voltage\n"); while (battery.get_voltage() >= min_voltage) { - battery.set_current(current); + battery.consume_energy(current); ::printf("%0.2f, %0.2f\n", time * 1.0e-6, battery.get_voltage()); From b305e3c429b8933418183697d212955561f5eace Mon Sep 17 00:00:00 2001 From: Hunter McClelland Date: Tue, 24 Mar 2026 13:23:20 -0400 Subject: [PATCH 10/11] SITL: Create SITL::Aircraft::get_battery_current() --- libraries/SITL/SIM_Aircraft.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 55ff2744bd3de..932026596185c 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -175,6 +175,7 @@ class Aircraft { #endif float get_battery_voltage() const { return battery_voltage; } float get_battery_temperature() const { return battery.get_temperature(); } + float get_battery_current() const { return battery_current; } float ambient_temperature_degC() const; From adba6945261abcf4881bc93ada0f0b86ed1a9e5d Mon Sep 17 00:00:00 2001 From: Hunter McClelland Date: Tue, 3 Feb 2026 09:51:09 -0500 Subject: [PATCH 11/11] SITL: children of STIL::Aircraft explicitly manage batt temperature --- libraries/SITL/SIM_Aircraft.h | 6 ++++-- libraries/SITL/SIM_FlightAxis.cpp | 2 ++ libraries/SITL/SIM_JSON.cpp | 2 ++ libraries/SITL/SIM_Multicopter.cpp | 1 + libraries/SITL/SIM_Plane.cpp | 2 ++ libraries/SITL/SIM_QuadPlane.cpp | 1 + libraries/SITL/SIM_Scrimmage.cpp | 1 + 7 files changed, 13 insertions(+), 2 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 932026596185c..fb3f355d1f88e 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -174,7 +174,7 @@ class Aircraft { void set_dronecan_device(DroneCANDevice *_dronecan) { dronecan = _dronecan; } #endif float get_battery_voltage() const { return battery_voltage; } - float get_battery_temperature() const { return battery.get_temperature(); } + float get_battery_temperature() const { return battery_temperature; } float get_battery_current() const { return battery_current; } float ambient_temperature_degC() const; @@ -223,12 +223,14 @@ class Aircraft { float airspeed_pitot; // m/s, EAS airspeed, as seen by fwd pitot tube float battery_voltage; float battery_current; + float battery_temperature; float local_ground_level; // ground level at local position bool lock_step_scheduled; bool flightaxis_sync_imus_to_frames; // causes the frame counter to be incremented on each timestep, IMUs will then update at the same rate uint32_t last_one_hz_ms; - // battery model + // OPTIONAL battery model + // ("OPTIONAL" because a child can ignore it and directly set/get battery_* members.) Battery battery; uint32_t motor_mask; diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index 28720caec05fb..f1c8c32ef7dad 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -656,6 +656,8 @@ void FlightAxis::update(const struct sitl_input &input) battery_voltage = MAX(state.m_batteryVoltage_VOLTS, 0); battery_current = MAX(state.m_batteryCurrentDraw_AMPS, 0); + // this aircraft has a constant battery temperature + battery_temperature = 273.0f; // kelvin rpm[0] = state.m_heliMainRotorRPM; rpm[1] = state.m_propRPM; motor_mask = 3; diff --git a/libraries/SITL/SIM_JSON.cpp b/libraries/SITL/SIM_JSON.cpp index dfa9223e0e01f..e3ed232d34514 100644 --- a/libraries/SITL/SIM_JSON.cpp +++ b/libraries/SITL/SIM_JSON.cpp @@ -495,6 +495,8 @@ void JSON::recv_fdm(const struct sitl_input &input) if ((received_bitmask & BAT_AMP) != 0) { battery_current = state.bat_amp; } + // this aircraft has a constant battery temperature + battery_temperature = 273.0f; // kelvin double deltat; if (state.timestamp_s < last_timestamp_s) { diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index de0af251fb8b7..ce50f8b9d0b79 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -78,6 +78,7 @@ void MultiCopter::update(const struct sitl_input &input) battery_current = frame->get_current_amp(); battery.consume_energy(battery_current); + battery_temperature = battery.get_temperature(); update_dynamics(rot_accel); update_external_payload(input); diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp index f3b18fb457cae..ad9fffc61313e 100644 --- a/libraries/SITL/SIM_Plane.cpp +++ b/libraries/SITL/SIM_Plane.cpp @@ -520,6 +520,8 @@ void Plane::update(const struct sitl_input &input) float throttle = reverse_thrust ? filtered_servo_angle(input, 2) : filtered_servo_range(input, 2); battery_voltage = sitl->batt_voltage - 0.7*throttle; battery_current = (battery_voltage/sitl->batt_voltage)*50.0f*sq(throttle); + // this aircraft has a constant battery temperature + battery_temperature = 273.0f; // kelvin update_dynamics(rot_accel); diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp index 6f2408ae24411..8ebbd71024016 100644 --- a/libraries/SITL/SIM_QuadPlane.cpp +++ b/libraries/SITL/SIM_QuadPlane.cpp @@ -143,6 +143,7 @@ void QuadPlane::update(const struct sitl_input &input) battery_current = frame->get_current_amp(); battery.consume_energy(battery_current); + battery_temperature = battery.get_temperature(); float throttle; if (reverse_thrust) { diff --git a/libraries/SITL/SIM_Scrimmage.cpp b/libraries/SITL/SIM_Scrimmage.cpp index 709e3e4afaac9..0aa3ed9d3da8a 100644 --- a/libraries/SITL/SIM_Scrimmage.cpp +++ b/libraries/SITL/SIM_Scrimmage.cpp @@ -117,6 +117,7 @@ void Scrimmage::recv_fdm(const struct sitl_input &input) battery_voltage = 0; battery_current = 0; + battery_temperature = 273.0f; rpm[0] = 0; rpm[1] = 0;