diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-18 12:42:24 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-18 12:42:24 +0200 |
commit | e5f1a7c2c3a67648829ec0dff5bf290dddc25847 (patch) | |
tree | bd4ea16fa97f6aa4c4fe446d0e7bf87cc0a64397 /src/modules/commander/commander.cpp | |
parent | e597f979824074e971aef19814951d87c8b6a8b5 (diff) | |
download | px4-firmware-e5f1a7c2c3a67648829ec0dff5bf290dddc25847.tar.gz px4-firmware-e5f1a7c2c3a67648829ec0dff5bf290dddc25847.tar.bz2 px4-firmware-e5f1a7c2c3a67648829ec0dff5bf290dddc25847.zip |
Better transparency in IO mode display, fixes to commander arming, minimum publishing rate for arming
Diffstat (limited to 'src/modules/commander/commander.cpp')
-rw-r--r-- | src/modules/commander/commander.cpp | 51 |
1 files changed, 46 insertions, 5 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 30906034e..12543800e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -273,6 +273,43 @@ void usage(const char *reason) void print_status() { warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); + + /* read all relevant states */ + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + struct vehicle_status_s state; + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + + const char* armed_str; + + switch (state.arming_state) { + case ARMING_STATE_INIT: + armed_str = "INIT"; + break; + case ARMING_STATE_STANDBY: + armed_str = "STANDBY"; + break; + case ARMING_STATE_ARMED: + armed_str = "ARMED"; + break; + case ARMING_STATE_ARMED_ERROR: + armed_str = "ARMED_ERROR"; + break; + case ARMING_STATE_STANDBY_ERROR: + armed_str = "STANDBY_ERROR"; + break; + case ARMING_STATE_REBOOT: + armed_str = "REBOOT"; + break; + case ARMING_STATE_IN_AIR_RESTORE: + armed_str = "IN_AIR_RESTORE"; + break; + default: + armed_str = "ERR: UNKNOWN STATE"; + break; + } + + + warnx("arming: %s", armed_str); } void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) @@ -945,9 +982,9 @@ int commander_thread_main(int argc, char *argv[]) /* store current state to reason later about a state change */ // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = status.condition_global_position_valid; - bool local_pos_valid = status.condition_local_position_valid; - bool airspeed_valid = status.condition_airspeed_valid; + // bool global_pos_valid = status.condition_global_position_valid; + // bool local_pos_valid = status.condition_local_position_valid; + // bool airspeed_valid = status.condition_airspeed_valid; /* check for global or local position updates, set a timeout of 2s */ @@ -1274,11 +1311,15 @@ int commander_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); } - /* publish vehicle status at least with 1 Hz */ - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { + /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ + if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { status.counter++; status.timestamp = t; orb_publish(ORB_ID(vehicle_status), status_pub, &status); + control_mode.timestamp = t; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + armed.timestamp = t; + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } /* play arming and battery warning tunes */ |