From b883b304049293f95ad746398bda955516c5ae3c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 May 2016 13:45:15 +0200 Subject: [PATCH] Simulator: Drain the battery to 15%, reffill it once disarmed to allow for more testing --- src/modules/simulator/simulator_mavlink.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 9e53d9d80a..b7c3f359ac 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -69,6 +69,8 @@ static int _fd; static unsigned char _buf[1024]; sockaddr_in _srcaddr; static socklen_t _addrlen = sizeof(_srcaddr); +static bool actuators_on = false; +static hrt_abstime batt_sim_start = 0; using namespace simulator; @@ -112,6 +114,8 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) memset(out, 0, sizeof(out)); } + actuators_on = (out[3] > 0.1f); + actuator_msg.time_usec = hrt_absolute_time(); actuator_msg.roll_ailerons = out[0]; actuator_msg.pitch_elevator = (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol) ? out[1] : -out[1]; @@ -264,9 +268,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) const float discharge_interval_us = 60 * 1000 * 1000; - static hrt_abstime batt_sim_start = 0; - - if (batt_sim_start == 0 || batt_sim_start > now) { + if (!actuators_on || batt_sim_start == 0 || batt_sim_start > now) { batt_sim_start = now; } @@ -279,8 +281,10 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) vbatt = (_battery.full_cell_voltage() - (discharge_v * ((now - batt_sim_start) / discharge_interval_us))) * cellcount; - if (vbatt < (cellcount * _battery.empty_cell_voltage())) { - vbatt = cellcount * _battery.empty_cell_voltage(); + float batt_voltage_loaded = _battery.empty_cell_voltage() - 0.05f; + + if (!PX4_ISFINITE(vbatt) || (vbatt < (cellcount * batt_voltage_loaded))) { + vbatt = cellcount * batt_voltage_loaded; } battery_status_s battery_status = {};