fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 15:38:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Simulator: Drain the battery to 15%, reffill it once disarmed to allow for more testing

Browse Source
This commit is contained in:
Lorenz Meier
2016-05-03 13:45:15 +02:00
parent e1e15add01
commit b883b30404
1 changed files with 9 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator_mavlink.cpp
+9 -5
View File
@@ -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 = {};