From 202c29154a3b10636af46d10a8e315321330fd43 Mon Sep 17 00:00:00 2001 From: Daniel Agar 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 0e221ed7820c..6e076521d240 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -292,6 +292,8 @@ class Simulator : public control::SuperBlock _gps.writeData(&gps_data); _param_sub = orb_subscribe(ORB_ID(parameter_update)); + + _battery_status.timestamp = hrt_absolute_time(); } ~Simulator() { @@ -343,6 +345,7 @@ class Simulator : public control::SuperBlock // 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 56bcf647a8cf..6bbd77b063e9 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 e644068befff..4e4176316dd5 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 @@ class GPSSIM : public VirtDevObj 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",