diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 79d9a8a67fad..aadc006f42e6 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -327,13 +327,13 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) } float ibatt = -1.0f; // no current sensor in simulation - const float minimum_percentage = 0.5f; // change this value if you want to simulate low battery reaction + const float minimum_percentage = 0.499f; // change this value if you want to simulate low battery reaction /* Simulate the voltage of a linearly draining battery but stop at the minimum percentage */ float battery_percentage = 1.0f - (now_us - batt_sim_start) / discharge_interval_us; - battery_percentage = math::min(battery_percentage, minimum_percentage); - float vbatt = math::gradual(battery_percentage, 0.f, 1.f, _battery.full_cell_voltage(), _battery.empty_cell_voltage()); + battery_percentage = math::max(battery_percentage, minimum_percentage); + float vbatt = math::gradual(battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(), _battery.full_cell_voltage()); vbatt *= _battery.cell_count(); const float throttle = 0.0f; // simulate no throttle compensation to make the estimate predictable