diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-06 11:25:17 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-06 11:25:17 +0100 |
commit | d3fd3d8219179251d10655944992da75abb8932b (patch) | |
tree | 25782ae747ddaacf74d610c9b57627ae46e07f7b /apps/commander/commander.c | |
parent | 0ef1d6d7529e9c84969ed6f512f733772bba34a0 (diff) | |
parent | 8eb8909a3c24c6028e4945e4a057d6d2f27f3d04 (diff) | |
download | px4-firmware-d3fd3d8219179251d10655944992da75abb8932b.tar.gz px4-firmware-d3fd3d8219179251d10655944992da75abb8932b.tar.bz2 px4-firmware-d3fd3d8219179251d10655944992da75abb8932b.zip |
Merged, compiling
Diffstat (limited to 'apps/commander/commander.c')
-rw-r--r-- | apps/commander/commander.c | 31 |
1 files changed, 20 insertions, 11 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index ddbfc19d6..f7e93db6c 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -69,6 +69,7 @@ #include <poll.h> #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> +#include <uORB/topics/battery_status.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/offboard_control_setpoint.h> #include <uORB/topics/vehicle_global_position.h> @@ -1315,8 +1316,6 @@ int commander_thread_main(int argc, char *argv[]) int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); - sensors.battery_voltage_v = 0.0f; - sensors.battery_voltage_valid = false; /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1328,6 +1327,12 @@ int commander_thread_main(int argc, char *argv[]) struct parameter_update_s param_changed; memset(¶m_changed, 0, sizeof(param_changed)); + /* subscribe to battery topic */ + int battery_sub = orb_subscribe(ORB_ID(battery_status)); + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + battery.voltage_v = 0.0f; + // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1413,15 +1418,19 @@ int commander_thread_main(int argc, char *argv[]) last_local_position_time = local_position.timestamp; } - battery_voltage = sensors.battery_voltage_v; - battery_voltage_valid = sensors.battery_voltage_valid; - - /* - * Only update battery voltage estimate if voltage is - * valid and system has been running for two and a half seconds - */ - if (battery_voltage_valid && (hrt_absolute_time() - start_time > 2500000)) { - bat_remain = battery_remaining_estimate_voltage(battery_voltage); + orb_check(battery_sub, &new_data); + if (new_data) { + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + battery_voltage = battery.voltage_v; + battery_voltage_valid = true; + + /* + * Only update battery voltage estimate if system has + * been running for two and a half seconds. + */ + if (hrt_absolute_time() - start_time > 2500000) { + bat_remain = battery_remaining_estimate_voltage(battery_voltage); + } } /* Slow but important 8 Hz checks */ |