fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 11:13:35 +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;
src/platforms/posix/drivers/gpssim/gpssim.cpp
+19 -77
View File
@@ -72,8 +72,7 @@ using namespace DriverFramework;
#define GPS_DRIVER_MODE_UBX_SIM
#define GPSSIM_DEVICE_PATH "/dev/gpssim"
#define TIMEOUT_5HZ 500
#define TIMEOUT_10MS 10
#define TIMEOUT_100MS 100000
#define RATE_MEASUREMENT_PERIOD 5000000
/* class for dynamic allocation of satellite info data */
@@ -122,7 +121,6 @@ private:
struct satellite_info_s *_p_report_sat_info; ///< pointer to uORB topic for satellite info
orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info
float _rate; ///< position update rate
bool _fake_gps; ///< fake gps output
SyncObj _sync;
int _fix_type;
int _num_sat;
@@ -186,7 +184,6 @@ GPSSIM::GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info,
_p_report_sat_info(nullptr),
_report_sat_info_pub(nullptr),
_rate(0.0f),
_fake_gps(fake_gps),
_fix_type(fix_type),
_num_sat(num_sat),
_noise_multiplier(noise_multiplier)
@@ -328,81 +325,32 @@ GPSSIM::receive(int timeout)
void
GPSSIM::task_main()
{
/* loop handling received serial bytes and also configuring in between */
while (!_task_should_exit) {
if (_fake_gps) {
_report_gps_pos.timestamp = hrt_absolute_time();
_report_gps_pos.lat = (int32_t)47.378301e7f;
_report_gps_pos.lon = (int32_t)8.538777e7f;
_report_gps_pos.alt = (int32_t)1200e3f;
_report_gps_pos.s_variance_m_s = 10.0f;
_report_gps_pos.c_variance_rad = 0.1f;
_report_gps_pos.fix_type = 3;
_report_gps_pos.eph = 0.9f;
_report_gps_pos.epv = 1.8f;
_report_gps_pos.vel_n_m_s = 0.0f;
_report_gps_pos.vel_e_m_s = 0.0f;
_report_gps_pos.vel_d_m_s = 0.0f;
_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s *
_report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
_report_gps_pos.cog_rad = 0.0f;
_report_gps_pos.vel_ned_valid = true;
// GPS is obviously detected successfully, reset statistics
//_Helper->reset_update_rates();
//no time and satellite information simulated
int recv_ret = receive(TIMEOUT_100MS);
if (!(m_pub_blocked)) {
if (_report_gps_pos_pub != nullptr) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
if (recv_ret > 0) {
/* opportunistic publishing - else invalid data would end up on the bus */
if (_report_gps_pos_pub != nullptr) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
if (_p_report_sat_info) {
if (_report_sat_info_pub != nullptr) {
orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
} else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
_report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info);
}
}
usleep(2e5);
} else {
//Publish initial report that we have access to a GPS
//Make sure to clear any stale data in case driver is reset
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
_report_gps_pos.timestamp = hrt_absolute_time();
_report_gps_pos.timestamp_time_relative = 0;
if (!(m_pub_blocked)) {
if (_report_gps_pos_pub != nullptr) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
}
// GPS is obviously detected successfully, reset statistics
//_Helper->reset_update_rates();
int recv_ret = 0;
while ((recv_ret = receive(TIMEOUT_10MS)) >= 0 && !_task_should_exit) {
/* opportunistic publishing - else invalid data would end up on the bus */
if (recv_ret && !(m_pub_blocked)) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
if (_p_report_sat_info) {
if (_report_sat_info_pub != nullptr) {
orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
} else {
_report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info);
}
}
}
}
// FIXME - if ioctl is called then it will deadlock
_sync.lock();
}
}
@@ -423,13 +371,7 @@ void
GPSSIM::print_info()
{
//GPS Mode
if (_fake_gps) {
PX4_INFO("protocol: faked");
}
else {
PX4_INFO("protocol: SIM");
}
PX4_INFO("protocol: SIM");
PX4_INFO("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
PX4_INFO("sat info: %s, noise: %d, jamming detected: %s",