dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
Julian Oes 2019-10-30 20:45:44 +01:00 committed by Daniel Agar
parent 66b3a4269b
commit a6a8877463
1 changed files with 12 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/simulator/simulator_mavlink.cpp
View File

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