fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08: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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/drivers/px4io/px4io.cpp
View File

@ -1519,8 +1519,8 @@ PX4IO::print_status()
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
printf("arming 0x%04x%s%s%s%s%s%s\n",
arming,
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""),
((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"),
((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"),
((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""),
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""),

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

2
src/modules/commander/state_machine_helper.cpp
View File

@ -119,7 +119,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
{
ret = TRANSITION_CHANGED;
armed->armed = true;
armed->ready_to_arm = false;
armed->ready_to_arm = true;
}
break;