fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 03:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

simulator optimize GPS and battery

Browse Source 
- GPS and battery were publishing at > 800Hz
This commit is contained in:
Daniel Agar
2018-01-02 01:35:57 +01:00
committed by Lorenz Meier
parent 66f614435f
commit 202c29154a
3 changed files with 53 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator.h
+3
View File
@@ -292,6 +292,8 @@ private:
_gps.writeData(&gps_data);
_param_sub = orb_subscribe(ORB_ID(parameter_update));
_battery_status.timestamp = hrt_absolute_time();
}
~Simulator()
{
@@ -343,6 +345,7 @@ private:
// Lib used to do the battery calculations.
Battery _battery;
battery_status_s _battery_status{};
// For param MAV_TYPE
int32_t _system_type;
src/modules/simulator/simulator_mavlink.cpp
+31 -30
View File
@@ -356,39 +356,40 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
update_sensors(&imu);
// battery simulation
const float discharge_interval_us = _battery_drain_interval_s.get() * 1000 * 1000;
// battery simulation (limit update to 100Hz)
if (hrt_elapsed_time(&_battery_status.timestamp) >= 10000) {
bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
const float discharge_interval_us = _battery_drain_interval_s.get() * 1000 * 1000;
if (!armed || batt_sim_start == 0 || batt_sim_start > now) {
batt_sim_start = now;
bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
if (!armed || batt_sim_start == 0 || batt_sim_start > now) {
batt_sim_start = now;
}
unsigned cellcount = _battery.cell_count();
float vbatt = _battery.full_cell_voltage() ;
float ibatt = -1.0f;
float discharge_v = _battery.full_cell_voltage() - _battery.empty_cell_voltage();
vbatt = (_battery.full_cell_voltage() - (discharge_v * ((now - batt_sim_start) / discharge_interval_us))) * cellcount;
float batt_voltage_loaded = _battery.empty_cell_voltage() - 0.05f;
if (!PX4_ISFINITE(vbatt) || (vbatt < (cellcount * batt_voltage_loaded))) {
vbatt = cellcount * batt_voltage_loaded;
}
// TODO: don't hard-code throttle.
const float throttle = 0.5f;
_battery.updateBatteryStatus(now, vbatt, ibatt, true, true, 0, throttle, armed, &_battery_status);
// publish the battery voltage
int batt_multi;
orb_publish_auto(ORB_ID(battery_status), &_battery_pub, &_battery_status, &batt_multi, ORB_PRIO_HIGH);
}
unsigned cellcount = _battery.cell_count();
float vbatt = _battery.full_cell_voltage() ;
float ibatt = -1.0f;
float discharge_v = _battery.full_cell_voltage() - _battery.empty_cell_voltage();
vbatt = (_battery.full_cell_voltage() - (discharge_v * ((now - batt_sim_start) / discharge_interval_us))) * cellcount;
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 = {};
// TODO: don't hard-code throttle.
const float throttle = 0.5f;
_battery.updateBatteryStatus(now, vbatt, ibatt, true, true, 0, throttle, armed, &battery_status);
// publish the battery voltage
int batt_multi;
orb_publish_auto(ORB_ID(battery_status), &_battery_pub, &battery_status, &batt_multi, ORB_PRIO_HIGH);
}
break;