diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 649d2890f8..28389cce89 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -232,7 +232,7 @@ static struct commander_state_s internal_state = {}; static struct mission_result_s _mission_result = {}; static uint8_t main_state_before_rtl = commander_state_s::MAIN_STATE_MAX; -static unsigned _last_mission_instance = 0; + static manual_control_setpoint_s sp_man = {}; ///< the current manual control setpoint static manual_control_setpoint_s _last_sp_man = {}; ///< the manual control setpoint valid at the last mode switch static uint8_t _last_sp_man_arm_switch = 0; @@ -810,7 +810,7 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state); break; case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: - if (_mission_result.valid) { + if (status_flags.condition_auto_mission_available) { main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state); } else { main_ret = TRANSITION_DENIED; @@ -1116,7 +1116,7 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; // check if current mission and first item are valid - if (_mission_result.valid) { + if (status_flags.condition_auto_mission_available) { // requested first mission item valid if (PX4_ISFINITE(cmd->param1) && (cmd->param1 >= -1) && (cmd->param1 < _mission_result.seq_total)) { @@ -2443,17 +2443,44 @@ Commander::run() orb_check(mission_result_sub, &updated); if (updated) { - orb_copy(ORB_ID(mission_result), mission_result_sub, &_mission_result); + mission_result_s new_mission_result = {}; - status_flags.condition_auto_mission_available = _mission_result.valid && !_mission_result.finished; + if (orb_copy(ORB_ID(mission_result), mission_result_sub, &new_mission_result) == PX4_OK) { - if (status.mission_failure != _mission_result.failure) { - status.mission_failure = _mission_result.failure; - status_changed = true; + // if mission_result is valid for the current mission + const bool mission_result_ok = (new_mission_result.timestamp > commander_boot_timestamp) && (new_mission_result.instance_count > 0); - if (status.mission_failure) { - mavlink_log_critical(&mavlink_log_pub, "mission cannot be completed"); + status_flags.condition_auto_mission_available = mission_result_ok && new_mission_result.valid; + + if (mission_result_ok) { + + if (status.mission_failure != new_mission_result.failure) { + status.mission_failure = new_mission_result.failure; + status_changed = true; + + if (status.mission_failure) { + mavlink_log_critical(&mavlink_log_pub, "Mission cannot be completed"); + } + } + + /* Only evaluate mission state if home is set */ + if (status_flags.condition_home_position_valid && + (_mission_result.instance_count != new_mission_result.instance_count)) { + + if (!status_flags.condition_auto_mission_available) { + /* the mission is invalid */ + tune_mission_fail(true); + } else if (new_mission_result.warning) { + /* the mission has a warning */ + tune_mission_fail(true); + } else { + /* the mission is valid */ + tune_mission_ok(true); + } + } } + + _mission_result = new_mission_result; } } @@ -2587,30 +2614,6 @@ Commander::run() } } - /* Only evaluate mission state if home is set, - * this prevents false positives for the mission - * rejection. Back off 3 seconds to not overlay - * home tune. - */ - if (status_flags.condition_home_position_valid && - (hrt_elapsed_time(&_home.timestamp) > 3000000) && - _last_mission_instance != _mission_result.instance_count) { - - if (!_mission_result.valid) { - /* the mission is invalid */ - tune_mission_fail(true); - } else if (_mission_result.warning) { - /* the mission has a warning */ - tune_mission_fail(true); - } else { - /* the mission is valid */ - tune_mission_ok(true); - } - - /* prevent further feedback until the mission changes */ - _last_mission_instance = _mission_result.instance_count; - } - /* RC input check */ if (!status_flags.rc_input_blocked && sp_man.timestamp != 0 && (hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f))) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 763335f74f..3a14c983b8 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -454,9 +454,8 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case commander_state_s::MAIN_STATE_AUTO_MISSION: /* need global position, home position, and a valid mission */ - // TODO: require mission? condition_auto_mission_available if (status_flags->condition_global_position_valid && - status_flags->condition_home_position_valid) { + status_flags->condition_auto_mission_available) { ret = TRANSITION_CHANGED; } @@ -1128,7 +1127,6 @@ int prearm_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, // mission required if ((arm_requirements & ARM_REQ_MISSION_BIT) && (!status_flags->condition_auto_mission_available || - !status_flags->condition_home_position_valid || !status_flags->condition_global_position_valid)) { prearm_ok = false;