diff --git a/libraries/AC_PrecLand/AC_PrecLand_StateMachine.cpp b/libraries/AC_PrecLand/AC_PrecLand_StateMachine.cpp new file mode 100644 index 0000000000..5443117019 --- /dev/null +++ b/libraries/AC_PrecLand/AC_PrecLand_StateMachine.cpp @@ -0,0 +1,259 @@ +#include "AC_PrecLand_StateMachine.h" +#include +#include + +static const float MAX_POS_ERROR_M = 0.75f; // Maximum possition error for retry locations +static const uint32_t FAILSAFE_INIT_TIMEOUT_MS = 7000; // Timeout in ms before failsafe measures are started. During this period vehicle is completely stopped to give user the time to take over +static const float RETRY_OFFSET_ALT_M = 1.5f; // This gets added to the altitude of the retry location + +// Initialize the state machine. This is called everytime vehicle switches mode +void AC_PrecLand_StateMachine::init() +{ + // init is only called ONCE per mode change. So in a particuar mode we can retry only a finite times. + // The counter will be reset if the statemachine is called from a different mode + _retry_count = 0; + // reset every other statemachine + reset_failed_landing_statemachine(); +} + +// Reset the landing statemachines. This needs to be called everytime the landing target is back in sight. +// So that if the landing target goes out of sight again, we can start the failed landing procedure back from the beginning stage +void AC_PrecLand_StateMachine::reset_failed_landing_statemachine() +{ + landing_target_lost_action = TargetLostAction::INIT; + _retry_state = RetryLanding::INIT; + failsafe_initialized = false; +} + +// Run Prec Land State Machine. During Prec Landing, we might encounter four scenarios: +// 1. We had the target in sight, but have lost it now. 2. We never had the target in sight and user wants to land. +// 3. We have the target in sight and can continue landing. 4. The sensor is out of range +// This method deals with all of these scenarios +// Returns the action needed to be done by the vehicle. +// Parameters: Vector3f "retry_pos_m" is filled with the required location if we need to retry landing. +AC_PrecLand_StateMachine::Status AC_PrecLand_StateMachine::update(Vector3f &retry_pos_m) +{ + + // grab the current status of Landing Target + AC_PrecLand *_precland = AP::ac_precland(); + if (_precland == nullptr) { + // should never happen + return Status::ERROR; + } + AC_PrecLand::TargetState precland_target_state = _precland->get_target_state(); + + switch (precland_target_state) { + case AC_PrecLand::TargetState::TARGET_RECENTLY_LOST: + // we have lost the target but had it in sight at least once recently + // action will depend on what user wants + return get_target_lost_actions(retry_pos_m); + + case AC_PrecLand::TargetState::TARGET_NEVER_SEEN: + // we have no clue where we are supposed to be landing + // let user decide how strict our failsafe actions need to be + return Status::FAILSAFE; + + case AC_PrecLand::TargetState::TARGET_OUT_OF_RANGE: + // The target isn't in sight, but we can't run any fail safe measures or do landing retry + // Therefore just descend for now, and check again later if retry is allowed + case AC_PrecLand::TargetState::TARGET_FOUND: + // no action required, target is in sight + reset_failed_landing_statemachine(); + return Status::DESCEND; + } + + // should never reach here, all values are handled above + return Status::ERROR; +} + + +// Target is lost (i.e we had it in sight some time back), this method helps decide on what needs to be done next +// The chosen action depends on user set landing strictness and will be returned by this function +// Parameters: Vector3f "retry_pos_m" is filled with the required location if we need to retry landing. +AC_PrecLand_StateMachine::Status AC_PrecLand_StateMachine::get_target_lost_actions(Vector3f &retry_pos_m) +{ + AC_PrecLand *_precland = AP::ac_precland(); + if (_precland == nullptr) { + // should never happen + return Status::ERROR; + } + + switch (landing_target_lost_action) { + case TargetLostAction::INIT: { + // figure out how strict the user is with the landing + const RetryStrictness strictness =_precland->get_retry_strictness(); + switch (strictness) { + case RetryStrictness::NORMAL: + case RetryStrictness::VERY_STRICT: + // We eventually want to retry landing, but lets descend for some time and hope the target gets in sight + // If not, we will retry landing + landing_target_lost_action = TargetLostAction::DESCEND; + break; + case RetryStrictness::NOT_STRICT: + // User just wants to land, prec land isn't a priority + landing_target_lost_action = TargetLostAction::LAND_VERTICALLY; + break; + } + // at this stage we will be descending no matter what + // so no special action required + return Status::DESCEND; + } + + case TargetLostAction::DESCEND: + if (AP_HAL::millis() - _precland->get_last_valid_target_ms() >=_precland->get_min_retry_time_sec() * 1000) { + // we have descended for some time and the target still isn't in sight + // lets retry + landing_target_lost_action = TargetLostAction::RETRY_LANDING; + _retry_state = RetryLanding::INIT; + } + // still descending, no other action + return Status::DESCEND; + + case TargetLostAction::RETRY_LANDING: + // retry the landing by going to another position + return retry_landing(retry_pos_m); + + case TargetLostAction::LAND_VERTICALLY: + // Just land vertically + // we will not be retrying to any location here on, so return false + return Status::DESCEND; + } + + // should never reach here, all cases are handled above + return Status::ERROR; +} + +// Retry landing based on a previously known location of the landing target +// Returns the action that should be taken by the vehicle +// Vector3f "retry_pos_m" is filled with the required location. +AC_PrecLand_StateMachine::Status AC_PrecLand_StateMachine::retry_landing(Vector3f &retry_pos_m) +{ + AC_PrecLand *_precland = AP::ac_precland(); + if (_precland == nullptr) { + // should never happen + return Status::ERROR; + } + + if (_precland->get_max_retry_allowed() == 0) { + // user does not want retry + return Status::FAILSAFE; + } + + if (_retry_count > _precland->get_max_retry_allowed()) { + // we have exhausted the amount of times vehicle was allowed to retry landing + // do failsafe measure so the vehicle isn't stuck in a constant loop + return Status::FAILSAFE; + } + + // get the retry position. This depends on what retry behavior has been set by user + Vector3f go_to_pos; + const RetryAction retry_action = _precland->get_retry_behaviour(); + if (retry_action == RetryAction::GO_TO_TARGET_LOC) { + _precland->get_last_detected_landing_pos(go_to_pos); + } else if (retry_action == RetryAction::GO_TO_LAST_LOC) { + _precland->get_last_vehicle_pos_when_target_detected(go_to_pos); + } + + // add a little bit offset so the vehicle climbs slightly higher than where it was + // remember this is "D" frame and in meters's + go_to_pos.z -= RETRY_OFFSET_ALT_M; + + switch (_retry_state) { + case RetryLanding::INIT: + // Init the Retry + _retry_count ++; + _retry_state = RetryLanding::IN_PROGRESS; + // inform the user what we are doing + if (_retry_count <= _precland->get_max_retry_allowed()) { + gcs().send_text(MAV_SEVERITY_INFO, "PrecLand: Retrying"); + } + retry_pos_m = go_to_pos; + return Status::RETRYING; + + case RetryLanding::IN_PROGRESS: { + // continue converging towards the target till we are close by + retry_pos_m = go_to_pos; + Vector3f pos; + if (!AP::ahrs().get_relative_position_NED_origin(pos)) { + return Status::ERROR; + } + const float dist_to_target = (go_to_pos-pos).length(); + if ((dist_to_target < MAX_POS_ERROR_M)) { + // we have approx reached landing location previously detected + _retry_state = RetryLanding::DESCEND; + } + return Status::RETRYING; + } + + case RetryLanding::DESCEND: { + // descend a little bit before completing the retry + // This will descend to the original height of where landing target was first detected + Vector3f pos; + if (!AP::ahrs().get_relative_position_NED_origin(pos)) { + return Status::ERROR; + } + // z_target is in "D" frame + const float z_target = go_to_pos.z + RETRY_OFFSET_ALT_M; + retry_pos_m = Vector3f{pos.x, pos.y, z_target}; + if (fabsf(pos.z - retry_pos_m.z) < MAX_POS_ERROR_M) { + // we have descended to the original height where we started the climb from + _retry_state = RetryLanding::COMPLETE; + gcs().send_text(MAV_SEVERITY_INFO, "PrecLand: Retry Completed"); + } + return Status::RETRYING; + } + + case RetryLanding::COMPLETE: + // Vehicle has completed a retry, and most likely the landing location still isn't sight + // we have no choice but to force a failsafe action + return Status::FAILSAFE; + } + + // should never reach here + return Status::ERROR; +} + +// This is only called when the current status of the state machine returns "failsafe" and will return the action that the vehicle should do +// At the moment this method only allows you to stop in air permanently, or land vertically +// Failsafe will only trigger as a last resort +AC_PrecLand_StateMachine::FailSafeAction AC_PrecLand_StateMachine::get_failsafe_actions() +{ + AC_PrecLand *_precland = AP::ac_precland(); + if (_precland == nullptr) { + // should never happen, just descend + return FailSafeAction::DESCEND; + } + + if (!failsafe_initialized) { + // start the timer + failsafe_start_ms = AP_HAL::millis(); + failsafe_initialized = true; + gcs().send_text(MAV_SEVERITY_INFO, "PrecLand: Failsafe Measures"); + } + + // Depending on the strictness we will either land vertically, wait for some time and then land vertically, not land at all + const RetryStrictness strictness= _precland->get_retry_strictness(); + switch (strictness) { + case RetryStrictness::VERY_STRICT: + // user does not want to land on anything but the target + // stop landing (hover) + return FailSafeAction::HOLD_POS; + + case RetryStrictness::NORMAL: + if (AP_HAL::millis() - failsafe_start_ms < FAILSAFE_INIT_TIMEOUT_MS) { + // stop the vehicle for at least a few seconds before descending + // this might give user the chance to take over + // we do not want to be too linent in landing vertically because of the strictness set by the user + return FailSafeAction::HOLD_POS; + } + // land the vehicle vertically + return FailSafeAction::DESCEND; + + case RetryStrictness::NOT_STRICT: + // User wants to prioritize landing over staying in the air + return FailSafeAction::DESCEND; + } + + // should never reach here + return FailSafeAction::DESCEND; +} diff --git a/libraries/AC_PrecLand/AC_PrecLand_StateMachine.h b/libraries/AC_PrecLand/AC_PrecLand_StateMachine.h new file mode 100644 index 0000000000..7993202dbe --- /dev/null +++ b/libraries/AC_PrecLand/AC_PrecLand_StateMachine.h @@ -0,0 +1,105 @@ +#pragma once + +#include +#include + +// This class constantly monitors what the status of the landing target is +// If it is not in sight, depending on user parameters, it decides what measures can be taken to bring the target back in sight +// If the target has been lost recently, the vehicle might try to retry the landing by going to the last known location of the target/going to the location where the target was last detected +// If we have no clue where the target might be, then failsafe measures are activated +// Failsafe measures can include stopping completely (hover), or landing vertically +class AC_PrecLand_StateMachine { +public: + + // Constructor + AC_PrecLand_StateMachine() { + init(); + }; + + // Do not allow copies + AC_PrecLand_StateMachine(const AC_PrecLand_StateMachine &other) = delete; + AC_PrecLand_StateMachine &operator=(const AC_PrecLand_StateMachine&) = delete; + + // Initialize the state machine. This is called everytime vehicle switches mode + void init(); + + // Current status of the precland state machine + enum class Status: uint8_t { + ERROR = 0, // Unknown error + DESCEND, // No action is required, just descend vertically + RETRYING, // Vehicle is attempting to retry landing + FAILSAFE // Switch to prec landing failsafe + }; + + // FailSafe action needed + enum class FailSafeAction: uint8_t { + HOLD_POS = 0, // Hold the current position of the vehicle + DESCEND // Descend vertically + }; + + // Run Prec Land State Machine. During Prec Landing, we might encounter four scenarios: + // 1. We had the target in sight, but have lost it now. 2. We never had the target in sight and user wants to land. + // 3. We have the target in sight and can continue landing. 4. The sensor is out of range + // This method deals with all of these scenarios + // Returns the action needed to be done by the vehicle. + // Parameters: Vector3f "retry_pos_m" is filled with the required location if we need to retry landing. + Status update(Vector3f &retry_pos_m); + + // This is only called when the current status of the state machine returns "failsafe" and will return the action that the vehicle should do + // At the moment this method only allows you to stop in air permanently, or land vertically + // Failsafe will only trigger as a last resort + FailSafeAction get_failsafe_actions(); + + // Strictness that the user wants for Prec Landing + enum class RetryStrictness: uint8_t { + NOT_STRICT = 0, // This is the behaviour on Copter 4.1 and below. The vehicle will land ASAP irrespective of target in sight or not + NORMAL, // Vehicle will retry a failed prec landing; if the target isn't found, it will land vertically + VERY_STRICT // Same as above, except vehicle will never land if the target isn't found + }; + + // which retry action should be done + enum class RetryAction: uint8_t { + GO_TO_LAST_LOC = 0, // Go to the last location where landing target was detected + GO_TO_TARGET_LOC // Go towards the location of the detected landing target + }; + +private: + + // Target is lost (i.e we had it in sight some time back), this method helps decide on what needs to be done next + // The chosen action depends on user set landing strictness and will be returned by this function + // Parameters: Vector3f "retry_pos_m" is filled with the required location if we need to retry landing. + Status get_target_lost_actions(Vector3f &retry_pos_m); + + // Retry landing based on a previously known location of the landing target + // Returns the action that should be taken by the vehicle + // Vector3f "retry_pos_m" is filled with the required location. + Status retry_landing(Vector3f &retry_pos_m); + + // Reset the landing statemachine. This needs to be called everytime the landing target is back in sight. + // So that if the landing target goes out of sight again, we can start the failed landing procedure back from the beginning stage + void reset_failed_landing_statemachine(); + + // State machine for action to do when Landing target is lost (after it was in sight a while back) + enum class TargetLostAction: uint8_t { + INIT = 0, // Decide on what action needs to be taken + DESCEND, // Descend for sometime (happens if we have just lost the target) + LAND_VERTICALLY, // Land vertically + RETRY_LANDING, // Retry landing (only possible if we had the landing target in sight sometime during the flight) + }; + + TargetLostAction landing_target_lost_action; // Current action being done in the Lost Landing target state machine + + // State Machine for landing retry + enum class RetryLanding : uint8_t { + INIT = 0, // Init the retry statemachine. This would involve increasing the retry counter (so we how many times we have already retried) + IN_PROGRESS, // Retry in progress, we wait for the vehicle to get close to the target location + DESCEND, // Descend to the original height from where we had started the retry + COMPLETE // Retry completed. We try failsafe measures after this + }; + RetryLanding _retry_state; // Current action being done in the Landing retry state machine + uint8_t _retry_count; // Total number of retires done in this mode + + bool failsafe_initialized; // True if failsafe has been initalized + uint32_t failsafe_start_ms; // timestamp of when failsafe was triggered + +};