diff options
author | Julian Oes <joes@student.ethz.ch> | 2013-02-17 15:04:01 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2013-02-17 15:04:01 -0800 |
commit | 3bc385c789f2b39cda066551ff1d5b767ab26aec (patch) | |
tree | f6912e76eb408546cb8b808f08d7f9051e8b8da9 /apps | |
parent | 0e2db0beb9228720a40bd19a7bd8891e5a8fdaba (diff) | |
download | px4-firmware-3bc385c789f2b39cda066551ff1d5b767ab26aec.tar.gz px4-firmware-3bc385c789f2b39cda066551ff1d5b767ab26aec.tar.bz2 px4-firmware-3bc385c789f2b39cda066551ff1d5b767ab26aec.zip |
Checkpoint: Added arming state check
Diffstat (limited to 'apps')
-rw-r--r-- | apps/commander/commander.c | 6 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.c | 22 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.h | 2 | ||||
-rw-r--r-- | apps/sensors/sensor_params.c | 10 | ||||
-rw-r--r-- | apps/sensors/sensors.cpp | 2 | ||||
-rw-r--r-- | apps/uORB/topics/vehicle_status.h | 2 |
6 files changed, 36 insertions, 8 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 8716caef7..1bfdd5660 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -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, ¤t_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); diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index e1ec73110..d89be781a 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -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 */ diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index bf9296caf..ed18bfbd2 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -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); diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index 9f23ebbba..df7f66116 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -158,13 +158,11 @@ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); +PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 28579bc70..ce404ee7e 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -438,7 +438,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ - _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSION_SW"); + _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index f9c4a5fff..ba9b9793b 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -170,6 +170,8 @@ struct vehicle_status_s bool flag_system_armed; /**< true is motors / actuators are armed */ bool flag_system_emergency; + bool flag_system_sensors_ok; + bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ |