From b84e01ba24af21ed5f185a3485356cc91a4808ea Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sun, 13 Jan 2019 16:52:04 +0100 Subject: [PATCH 1/2] simulator_mavlink: switch battery max and min to fix simultion In 3e6e1f5c2b042accca72346273fc1755dd7670fa the simulated battery percentage was reversed. I'm assuming because of the possibly missleading variable name. Now I'm fixing it by switching the maximum and minimum voltage such that the name is not misleading anymore but it still works as expected. --- src/modules/simulator/simulator_mavlink.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 79d9a8a67fad..55ad7c001aa0 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -332,8 +332,8 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) /* 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 From d3a03111cd2d69fd60945f8761ecad26fc410457 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sun, 13 Jan 2019 16:59:10 +0100 Subject: [PATCH 2/2] simulator_mavlink: take care of battery percentage rounding errors In 5bb9babc206068e9ee67e1489433be94482a402f I made MAVLink send a rounded up integer instead of rounded down. This makes sense in practice because the low battery reactions happen exactly when the reported number switches in the UI. But here we want to provoke an exact 50% in the UI so we stop counting at 49.9%, it get's rounded up and we see the expected result. --- src/modules/simulator/simulator_mavlink.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 55ad7c001aa0..aadc006f42e6 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -327,7 +327,7 @@ 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;