From a6a88774637c745ee069c6482a79b7d4b3cc8b12 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 30 Oct 2019 20:45:44 +0100 Subject: [PATCH] simulator: don't reset battery when disarmed Before this change, the battery percentage is reset to 100% as soon as the drone is disarmed again. In my opinion it is more realistic if the batteries don't magically fill up again but stay low. --- src/modules/simulator/simulator_mavlink.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 312ead864a..c64da6ac35 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -67,7 +67,6 @@ static int _fd; static unsigned char _buf[2048]; static sockaddr_in _srcaddr; static unsigned _addrlen = sizeof(_srcaddr); -static hrt_abstime batt_sim_start = 0; const unsigned mode_flag_armed = 128; const unsigned mode_flag_custom = 1; @@ -371,6 +370,9 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg) update_sensors(now_us, imu); + static float battery_percentage = 1.0f; + static uint64_t last_integration_us = 0; + // battery simulation (limit update to 100Hz) if (hrt_elapsed_time(&_battery_status.timestamp) >= 10_ms) { @@ -378,15 +380,19 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg) bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); - if (!armed || batt_sim_start == 0 || batt_sim_start > now_us) { - batt_sim_start = now_us; + if (armed) { + if (last_integration_us != 0) { + battery_percentage -= (now_us - last_integration_us) / discharge_interval_us; + } + + last_integration_us = now_us; + + } else { + last_integration_us = 0; } float ibatt = -1.0f; // no current sensor in simulation - /* 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::max(battery_percentage, _battery_min_percentage.get() / 100.f); float vbatt = math::gradual(battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(), _battery.full_cell_voltage()); vbatt *= _battery.cell_count();