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();