diff options
author | Julian Oes <joes@student.ethz.ch> | 2013-02-18 13:52:18 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2013-02-18 13:52:18 -0800 |
commit | 5eac78d7645becc486bc6a43852b9631e62465b4 (patch) | |
tree | 600a4e7027238892026127cd5b2ce40918d7b590 | |
parent | 47b05eeb87191fd0b380de008299f85262bc8953 (diff) | |
download | px4-firmware-5eac78d7645becc486bc6a43852b9631e62465b4.tar.gz px4-firmware-5eac78d7645becc486bc6a43852b9631e62465b4.tar.bz2 px4-firmware-5eac78d7645becc486bc6a43852b9631e62465b4.zip |
Checkpoint: Added DESCENT state
-rw-r--r-- | apps/commander/state_machine_helper.c | 51 | ||||
-rw-r--r-- | apps/uORB/topics/vehicle_status.h | 3 |
2 files changed, 34 insertions, 20 deletions
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index a0b786aab..68b4bbe30 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -119,6 +119,16 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ valid_path = true; } break; + case NAVIGATION_STATE_DESCENT: + + if (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { + + mavlink_log_critical(mavlink_fd, "Switched to DESCENT state"); + valid_transition = true; + valid_path = true; + } + break; case NAVIGATION_STATE_LOITER: @@ -146,25 +156,28 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ /* coming from STANDBY pos and home lock are needed */ if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); - valid_transition = true; + if (!current_status->flag_system_armed) { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: disarmed"); + } else if (!current_status->flag_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); + } else { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos and home lock"); + mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); + valid_transition = true; } valid_path = true; /* coming from LAND home lock is needed */ } else if (current_status->navigation_state == NAVIGATION_STATE_LAND) { - if (current_status->flag_valid_launch_position) { + if (!current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); + } else { mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); } valid_path = true; } @@ -183,15 +196,15 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); - valid_transition = true; - } else if (!current_status->flag_global_position_valid) { + if (!current_status->flag_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); + mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); + valid_transition = true; } else { - mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos and home lock"); + } valid_path = true; @@ -219,15 +232,13 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to RTL state"); - valid_transition = true; - } else if (!current_status->flag_global_position_valid) { + if (!current_status->flag_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos lock"); } else if (!current_status->flag_valid_launch_position) { mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); } else { - mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos and home lock"); + mavlink_log_critical(mavlink_fd, "Switched to RTL state"); + valid_transition = true; } valid_path = true; @@ -256,7 +267,8 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ case NAVIGATION_STATE_LAND: if (current_status->navigation_state == NAVIGATION_STATE_RTL - || current_status->navigation_state == NAVIGATION_STATE_LOITER) { + || current_status->navigation_state == NAVIGATION_STATE_LOITER + || current_status->navigation_state == NAVIGATION_STATE_MISSION) { mavlink_log_critical(mavlink_fd, "Switched to LAND state"); valid_transition = true; @@ -315,6 +327,7 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat if (current_status->arming_state == ARMING_STATE_INIT) { if (current_status->flag_system_sensors_initialized) { + current_status->flag_system_armed = false; mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); valid_transition = true; } else { @@ -327,7 +340,7 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat if (current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { - + current_status->flag_system_armed = true; mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); valid_transition = true; } diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 874cf256c..dde978cae 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -63,12 +63,13 @@ typedef enum { NAVIGATION_STATE_STANDBY=0, NAVIGATION_STATE_MANUAL, NAVIGATION_STATE_SEATBELT, + NAVIGATION_STATE_DESCENT, NAVIGATION_STATE_LOITER, NAVIGATION_STATE_AUTO_READY, NAVIGATION_STATE_MISSION, NAVIGATION_STATE_RTL, - NAVIGATION_STATE_TAKEOFF, NAVIGATION_STATE_LAND, + NAVIGATION_STATE_TAKEOFF } navigation_state_t; typedef enum { |