diff options
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/commander.cpp | 132 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 89 |
2 files changed, 116 insertions, 105 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6181dafab..872939d6d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -122,7 +122,7 @@ extern struct system_load_s system_load; #define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 #define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ +#define POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 2000000 @@ -353,27 +353,42 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - case VEHICLE_CMD_COMPONENT_ARM_DISARM: + case VEHICLE_CMD_NAV_TAKEOFF: { + transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); + if (nav_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); + } + + if (nav_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } break; + } case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(status, safety, armed)) { + if (is_safe(status, safety, armed)) { - if (((int)(cmd->param1)) == 1) { - /* reboot */ - up_systemreset(); - } else if (((int)(cmd->param1)) == 3) { - /* reboot to bootloader */ + if (((int)(cmd->param1)) == 1) { + /* reboot */ + up_systemreset(); + + } else if (((int)(cmd->param1)) == 3) { + /* reboot to bootloader */ + + // XXX implement + result = VEHICLE_CMD_RESULT_UNSUPPORTED; - // XXX implement - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { result = VEHICLE_CMD_RESULT_DENIED; } + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + break; case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { @@ -519,6 +534,7 @@ int commander_thread_main(int argc, char *argv[]) orb_advert_t status_pub; /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); + status.condition_landed = true; // initialize to safe value /* armed topic */ struct actuator_armed_s armed; @@ -760,6 +776,14 @@ int commander_thread_main(int argc, char *argv[]) last_diff_pres_time = diff_pres.timestamp; } + /* Check for valid airspeed/differential pressure measurements */ + if (t - last_diff_pres_time < 2000000 && t > 2000000) { + status.condition_airspeed_valid = true; + + } else { + status.condition_airspeed_valid = false; + } + orb_check(cmd_sub, &updated); if (updated) { @@ -785,6 +809,20 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); } + /* set the condition to valid if there has recently been a global position estimate */ + if (t - global_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && global_position.valid) { + if (!status.condition_global_position_valid) { + status.condition_global_position_valid = true; + status_changed = true; + } + + } else { + if (status.condition_global_position_valid) { + status.condition_global_position_valid = false; + status_changed = true; + } + } + /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -794,7 +832,7 @@ int commander_thread_main(int argc, char *argv[]) } /* set the condition to valid if there has recently been a local position estimate */ - if (t - local_position.timestamp < LOCAL_POSITION_TIMEOUT) { + if (t - local_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && local_position.valid) { if (!status.condition_local_position_valid) { status.condition_local_position_valid = true; status_changed = true; @@ -920,36 +958,6 @@ int commander_thread_main(int argc, char *argv[]) * set of position measurements is available. */ - /* 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; - - - /* check for global or local position updates, set a timeout of 2s */ - if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) { - status.condition_global_position_valid = true; - - } else { - status.condition_global_position_valid = false; - } - - if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) { - status.condition_local_position_valid = true; - - } else { - status.condition_local_position_valid = false; - } - - /* Check for valid airspeed/differential pressure measurements */ - if (t - last_diff_pres_time < 2000000 && t > 2000000) { - status.condition_airspeed_valid = true; - - } else { - status.condition_airspeed_valid = false; - } - orb_check(gps_sub, &updated); if (updated) { @@ -1083,6 +1091,7 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } + } else if (res == TRANSITION_DENIED) { /* DENIED here indicates safety switch not pressed */ @@ -1536,8 +1545,10 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v break; case MAIN_STATE_AUTO: - if (current_status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF) { + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't act while taking off */ + res = TRANSITION_NOT_CHANGED; + } else { if (current_status->condition_landed) { /* if landed: transitions only to AUTO_READY are allowed */ res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); @@ -1545,20 +1556,27 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v break; } else { - /* if not landed: act depending on switches */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } } + + } else { + /* always switch to loiter in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 163aceed2..f313adce4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -109,9 +109,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow arming from STANDBY and IN-AIR-RESTORE */ if ((status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) - && (!safety->safety_switch_available || safety->safety_off)) /* only allow arming if safety is off */ - { + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) + && (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */ ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = false; @@ -182,6 +181,7 @@ bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s // 3) Safety switch is present AND engaged -> actuators locked if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) { return true; + } else { return false; } @@ -284,6 +284,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = false; break; @@ -294,6 +295,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; break; @@ -304,6 +306,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; break; @@ -314,6 +317,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; break; @@ -324,80 +328,68 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; break; case NAVIGATION_STATE_AUTO_READY: ret = TRANSITION_CHANGED; - 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 = true; - control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_TAKEOFF: /* only transitions from AUTO_READY */ - if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = TRANSITION_CHANGED; 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 = true; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; } break; case NAVIGATION_STATE_AUTO_LOITER: - - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - 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 = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - + ret = TRANSITION_CHANGED; + 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 = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_MISSION: - - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - 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 = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - + ret = TRANSITION_CHANGED; + 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 = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_RTL: - - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - 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 = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - + ret = TRANSITION_CHANGED; + 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 = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_LAND: @@ -411,6 +403,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; } |