fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 07:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Checkpoint: Added arming state check

Browse Source
This commit is contained in:
Julian Oes
2013-02-17 15:04:01 -08:00
parent 0e2db0beb9
commit 3bc385c789
6 changed files with 36 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+5 -1
View File
@@ -2054,9 +2054,13 @@ int commander_thread_main(int argc, char *argv[])
state_changed = false;
}
/* make changes in state machine if needed */
update_state_machine(stat_pub, &current_status, mavlink_fd);
/* Store old modes to detect and act on state transitions */
voltage_previous = current_status.voltage_battery;
fflush(stdout);
counter++;
usleep(COMMANDER_MONITORING_INTERVAL);
apps/commander/state_machine_helper.c
+22
View File
@@ -53,6 +53,28 @@
#include "state_machine_helper.h"
void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
/* check arming first */
if (current_status->flag_system_armed && current_status->flag_system_emergency) {
do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT);
} else if(current_status->flag_system_armed && !current_status->flag_system_emergency) {
do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ARMED);
} else if(!current_status->flag_system_armed && current_status->flag_system_emergency) {
do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR);
} else if(!current_status->flag_system_armed && !current_status->flag_system_emergency) {
do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY);
} else if (current_status->flag_system_sensors_ok) {
do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY);
} else if (!current_status->flag_system_sensors_ok && current_status->flag_system_armed) {
do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT);
} else if (!current_status->flag_system_sensors_ok && !current_status->flag_system_armed) {
do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR);
}
/* now determine the navigation state */
}
/**
* Transition from one navigation state to another
*/
apps/commander/state_machine_helper.h
+2
View File
@@ -47,6 +47,8 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state);
int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state);