Skip to content

Commit

Permalink
simulator optimize GPS and battery
Browse files Browse the repository at this point in the history 
 - GPS and battery were publishing at > 800Hz
  • Loading branch information
@dagar @LorenzMeier
dagar authored and LorenzMeier committed Jan 2, 2018
1 parent 66f6144 commit 202c291
Show file tree
Hide file tree
Showing 3 changed files with 42 additions and 96 deletions.
3 changes: 3 additions & 0 deletions src/modules/simulator/simulator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
{
Expand Down Expand Up @@ -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;
Expand Down
45 changes: 23 additions & 22 deletions src/modules/simulator/simulator_mavlink.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
90 changes: 16 additions & 74 deletions src/platforms/posix/drivers/gpssim/gpssim.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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();
}
}

Expand All @@ -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",
Expand Down

0 comments on commit 202c291

Please sign in to comment.