SITL: fix SIM_BATT_CAP_AH

ArduPilot · Dec 19, 2024 · 65e5aa8 · 65e5aa8
1 parent 5c9003d
commit 65e5aa8
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 2 deletions.
diff --git 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
@@ -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
@@ -609,6 +609,10 @@ void Frame::current_and_voltage(float &voltage, float &current)
         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<num_motors; i++) {

diff --git 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 (constant) 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).
     // @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
+    // @Description: Simulated battery capacity. Set to 0 for unlimited capacity. Changing this parameter will re-initialize the state of charge of the battery.
     // @Units: Ah
     // @User: Advanced
     AP_GROUPINFO("BATT_CAP_AH",   20, SIM,  batt_capacity_ah,  0),