From 202c29154a3b10636af46d10a8e315321330fd43 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 31 Dec 2017 21:09:39 -0500
Subject: [PATCH] simulator optimize GPS and battery
- GPS and battery were publishing at > 800Hz
---
src/modules/simulator/simulator.h | 3 +
src/modules/simulator/simulator_mavlink.cpp | 45 +++++-----
src/platforms/posix/drivers/gpssim/gpssim.cpp | 90 ++++---------------
3 files changed, 42 insertions(+), 96 deletions(-)
diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index 0e221ed782..6e076521d2 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -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;
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 56bcf647a8..6bbd77b063 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -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);
- unsigned cellcount = _battery.cell_count();
+ if (!armed || batt_sim_start == 0 || batt_sim_start > now) {
+ batt_sim_start = now;
+ }
- float vbatt = _battery.full_cell_voltage() ;
- float ibatt = -1.0f;
+ unsigned cellcount = _battery.cell_count();
- float discharge_v = _battery.full_cell_voltage() - _battery.empty_cell_voltage();
+ float vbatt = _battery.full_cell_voltage() ;
+ float ibatt = -1.0f;
- vbatt = (_battery.full_cell_voltage() - (discharge_v * ((now - batt_sim_start) / discharge_interval_us))) * cellcount;
+ float discharge_v = _battery.full_cell_voltage() - _battery.empty_cell_voltage();
- float batt_voltage_loaded = _battery.empty_cell_voltage() - 0.05f;
+ vbatt = (_battery.full_cell_voltage() - (discharge_v * ((now - batt_sim_start) / discharge_interval_us))) * cellcount;
- if (!PX4_ISFINITE(vbatt) || (vbatt < (cellcount * batt_voltage_loaded))) {
- vbatt = cellcount * batt_voltage_loaded;
- }
+ float batt_voltage_loaded = _battery.empty_cell_voltage() - 0.05f;
- battery_status_s battery_status = {};
+ 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);
+ // 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);
+ // publish the battery voltage
+ int batt_multi;
+ orb_publish_auto(ORB_ID(battery_status), &_battery_pub, &_battery_status, &batt_multi, ORB_PRIO_HIGH);
+ }
}
break;
diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp
index e644068bef..4e4176316d 100644
--- a/src/platforms/posix/drivers/gpssim/gpssim.cpp
+++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp
@@ -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;
-
- //no time and satellite information simulated
-
- 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();
- usleep(2e5);
+ int recv_ret = receive(TIMEOUT_100MS);
- } 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 (recv_ret > 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);
+ /* 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);
- }
+ } 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;
+ 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);
- 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);
- }
- }
+ } 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",
--
GitLab