diff --git a/libraries/SITL/SIM_Battery.cpp b/libraries/SITL/SIM_Battery.cpp index 4c94bd305140b..7618745cf2796 100644 --- a/libraries/SITL/SIM_Battery.cpp +++ b/libraries/SITL/SIM_Battery.cpp @@ -128,6 +128,12 @@ void Battery::init_voltage(float 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) { uint64_t now = AP_HAL::micros64(); diff --git a/libraries/SITL/SIM_Battery.h b/libraries/SITL/SIM_Battery.h index af592c00ebc83..0cf1d7e176777 100644 --- a/libraries/SITL/SIM_Battery.h +++ b/libraries/SITL/SIM_Battery.h @@ -30,9 +30,11 @@ class Battery { void setup(float _capacity_Ah, float _resistance, float _max_voltage); 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; } diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index c3f2fba101295..1569038ed1230 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -609,6 +609,10 @@ void Frame::current_and_voltage(float &voltage, float ¤t) 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)) { + battery->init_capacity(param_capacity); + } voltage = battery->get_voltage(); current = 0; for (uint8_t i=0; i