yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

Better transparency in IO mode display, fixes to commander arming, minimum publishing rate for arming

Browse Source
This commit is contained in:
Lorenz Meier
2013-08-18 12:42:24 +02:00
parent e597f97982
commit e5f1a7c2c3
3 changed files with 49 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
src/modules/commander/commander.cpp
View File

@@ -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 */