Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AC_PrecLand_config.h"
       4             : 
       5             : #if AC_PRECLAND_ENABLED
       6             : 
       7             : #include <AC_PrecLand/AC_PrecLand.h>
       8             : #include <AP_Math/AP_Math.h>
       9             : 
      10             : // This class constantly monitors what the status of the landing target is
      11             : // If it is not in sight, depending on user parameters, it decides what measures can be taken to bring the target back in sight
      12             : // 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
      13             : // If we have no clue where the target might be, then failsafe measures are activated
      14             : // Failsafe measures can include stopping completely (hover), or landing vertically
      15             : class AC_PrecLand_StateMachine {
      16             : public:
      17             : 
      18             :     // Constructor
      19        1001 :     AC_PrecLand_StateMachine() {
      20        1001 :         init();
      21        1001 :     };
      22             : 
      23             :     // Do not allow copies
      24             :     CLASS_NO_COPY(AC_PrecLand_StateMachine);
      25             : 
      26             :     // Initialize the state machine. This is called every time vehicle switches mode
      27             :     void init();
      28             : 
      29             :     // Current status of the precland state machine
      30             :     enum class Status: uint8_t {
      31             :         ERROR = 0,               // Unknown error
      32             :         DESCEND,                 // No action is required, just descend vertically
      33             :         RETRYING,                // Vehicle is attempting to retry landing
      34             :         FAILSAFE                 // Switch to prec landing failsafe
      35             :     };
      36             : 
      37             :     // FailSafe action needed
      38             :     enum class FailSafeAction: uint8_t {
      39             :         HOLD_POS = 0,            // Hold the current position of the vehicle
      40             :         DESCEND                  // Descend vertically
      41             :     };
      42             : 
      43             :     // Run Prec Land State Machine. During Prec Landing, we might encounter four scenarios:
      44             :     // 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.
      45             :     // 3. We have the target in sight and can continue landing. 4. The sensor is out of range
      46             :     // This method deals with all of these scenarios
      47             :     // Returns the action needed to be done by the vehicle.
      48             :     // Parameters: Vector3p "retry_pos_m" is filled with the required location if we need to retry landing.
      49             :     Status update(Vector3p &retry_pos_m);
      50             : 
      51             :     // This is only called when the current status of the state machine returns "failsafe" and will return the action that the vehicle should do
      52             :     // At the moment this method only allows you to stop in air permanently, or land vertically
      53             :     // Failsafe will only trigger as a last resort
      54             :     FailSafeAction get_failsafe_actions();
      55             : 
      56             :     // Strictness that the user wants for Prec Landing
      57             :     enum class RetryStrictness: uint8_t {
      58             :         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
      59             :         NORMAL,                 // Vehicle will retry a failed prec landing; if the target isn't found, it will land vertically
      60             :         VERY_STRICT             // Same as above, except vehicle will never land if the target isn't found
      61             :     };
      62             : 
      63             :     // which retry action should be done
      64             :     enum class RetryAction: uint8_t {
      65             :         GO_TO_LAST_LOC = 0,     // Go to the last location where landing target was detected
      66             :         GO_TO_TARGET_LOC        // Go towards the location of the detected landing target
      67             :     };
      68             : 
      69             : private:
      70             : 
      71             :     // Target is lost (i.e we had it in sight some time back), this method helps decide on what needs to be done next
      72             :     // The chosen action depends on user set landing strictness and will be returned by this function
      73             :     // Parameters: Vector3f "retry_pos_m" is filled with the required location if we need to retry landing.
      74             :     Status get_target_lost_actions(Vector3p &retry_pos_m);
      75             : 
      76             :     // Retry landing based on a previously known location of the landing target
      77             :     // Returns the action that should be taken by the vehicle
      78             :     // Vector3f "retry_pos_m" is filled with the required location.
      79             :     Status retry_landing(Vector3p &retry_pos_m);
      80             : 
      81             :     // Reset the landing statemachine. This needs to be called every time the landing target is back in sight.
      82             :     // So that if the landing target goes out of sight again, we can start the failed landing procedure back from the beginning stage
      83             :     void reset_failed_landing_statemachine();
      84             : 
      85             :     // State machine for action to do when Landing target is lost (after it was in sight a while back)
      86             :     enum class TargetLostAction: uint8_t {
      87             :         INIT = 0,               // Decide on what action needs to be taken
      88             :         DESCEND,                // Descend for sometime (happens if we have just lost the target)
      89             :         LAND_VERTICALLY,        // Land vertically
      90             :         RETRY_LANDING,          // Retry landing (only possible if we had the landing target in sight sometime during the flight)
      91             :     };
      92             : 
      93             :     TargetLostAction landing_target_lost_action;  // Current action being done in the Lost Landing target state machine
      94             : 
      95             :     // State Machine for landing retry
      96             :     enum class RetryLanding : uint8_t {
      97             :         INIT = 0,               // Init the retry statemachine. This would involve increasing the retry counter (so we how many times we have already retried)
      98             :         IN_PROGRESS,            // Retry in progress, we wait for the vehicle to get close to the target location
      99             :         DESCEND,                // Descend to the original height from where we had started the retry
     100             :         COMPLETE                // Retry completed. We try failsafe measures after this
     101             :     };
     102             :     RetryLanding _retry_state;   // Current action being done in the Landing retry state machine
     103             :     uint8_t _retry_count;       // Total number of retires done in this mode
     104             : 
     105             :     bool failsafe_initialized;  // True if failsafe has been initalized
     106             :     uint32_t failsafe_start_ms; // timestamp of when failsafe was triggered
     107             : 
     108             : };
     109             : 
     110             : #endif // AC_PRECLAND_ENABLED