diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-07-22 21:48:21 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-07-22 21:48:21 +0400 |
commit | 963abd66badf71a925f80e12312c429d64999424 (patch) | |
tree | fd51db36605969fd864d9a271f768dad97a5a081 /src/modules/commander/state_machine_helper.cpp | |
parent | eb52eae153bc83144ddb5d6d03fdbb22aa0c9203 (diff) | |
download | px4-firmware-963abd66badf71a925f80e12312c429d64999424.tar.gz px4-firmware-963abd66badf71a925f80e12312c429d64999424.tar.bz2 px4-firmware-963abd66badf71a925f80e12312c429d64999424.zip |
commander: WIP, SEATBELT renamed to ASSISTED, added SIMPLE mode, added ASSISTED switch, some cleanup and reorganizing
Diffstat (limited to 'src/modules/commander/state_machine_helper.cpp')
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 66 |
1 files changed, 52 insertions, 14 deletions
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3cf60a99a..60ab01536 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -184,7 +184,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* transitions back to INIT are possible for calibration */ if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; @@ -200,7 +200,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { @@ -235,14 +235,15 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current } break; - case NAVIGATION_STATE_SEATBELT_STANDBY: + case NAVIGATION_STATE_ASSISTED_STANDBY: /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ if (current_state->navigation_state == NAVIGATION_STATE_INIT || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT) { /* need to be disarmed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_STANDBY) { @@ -262,11 +263,12 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current } break; - case NAVIGATION_STATE_SEATBELT: + case NAVIGATION_STATE_ASSISTED_SEATBELT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT || current_state->navigation_state == NAVIGATION_STATE_MANUAL || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER @@ -293,10 +295,43 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current } break; - case NAVIGATION_STATE_SEATBELT_DESCENT: + case NAVIGATION_STATE_ASSISTED_SIMPLE: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ + if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); + tune_negative(); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_ASSISTED_DESCENT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ - if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT + if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER @@ -328,7 +363,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* transitions from INIT or from other STANDBY modes or from AUTO READY */ if (current_state->navigation_state == NAVIGATION_STATE_INIT || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { /* need to be disarmed and have a position and home lock */ @@ -395,7 +430,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a position and home lock */ @@ -422,7 +458,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a mission ready */ @@ -446,7 +483,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a position and home lock */ |