Line data    Source code

       1             : #include "AC_PrecLand_config.h"
       2             : 
       3             : #if AC_PRECLAND_ENABLED
       4             : 
       5             : #include <AC_PrecLand/AC_PrecLand.h>
       6             : #include "AC_PrecLand_StateMachine.h"
       7             : #include <AP_AHRS/AP_AHRS.h>
       8             : #include <GCS_MAVLink/GCS.h>
       9             : 
      10             : static const float MAX_POS_ERROR_M = 0.75f;  // Maximum position error for retry locations
      11             : 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
      12             : static const float RETRY_OFFSET_ALT_M = 1.5f;  // This gets added to the altitude of the retry location
      13             : 
      14             : // Initialize the state machine. This is called every time vehicle switches mode
      15        1516 : void AC_PrecLand_StateMachine::init()
      16             : {
      17        1516 :     AC_PrecLand *_precland = AP::ac_precland();
      18        1516 :     if (_precland == nullptr) {
      19             :         // precland not enabled
      20           0 :         return;
      21             :     }
      22             : 
      23        1516 :     if (!_precland->enabled()) {
      24             :         // precland is not enabled, prec land state machine methods should not be called!
      25        1509 :         return;
      26             :     }
      27             :     // init is only called ONCE per mode change. So in a particular mode we can retry only a finite times.
      28             :     // The counter will be reset if the statemachine is called from a different mode
      29           7 :     _retry_count = 0;
      30             :     // reset every other statemachine
      31           7 :     reset_failed_landing_statemachine();
      32             : }
      33             : 
      34             : // Reset the landing statemachines. This needs to be called every time the landing target is back in sight.
      35             : // So that if the landing target goes out of sight again, we can start the failed landing procedure back from the beginning stage
      36       46704 : void AC_PrecLand_StateMachine::reset_failed_landing_statemachine()
      37             : {
      38       46704 :     landing_target_lost_action = TargetLostAction::INIT;
      39       46704 :     _retry_state = RetryLanding::INIT;
      40       46704 :     failsafe_initialized = false;
      41       46704 : }
      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: Vector3f "retry_pos_m" is filled with the required location if we need to retry landing.
      49       63857 : AC_PrecLand_StateMachine::Status AC_PrecLand_StateMachine::update(Vector3p &retry_pos_m)
      50             : {
      51             : 
      52             :     // grab the current status of Landing Target
      53       63857 :     AC_PrecLand *_precland = AP::ac_precland();
      54       63857 :     if (_precland == nullptr) {
      55             :         // should never happen
      56           0 :         return Status::ERROR;
      57             :     }
      58             : 
      59       63857 :     if (!_precland->enabled()) {
      60             :         // precland is not enabled, prec land state machine should not be called!
      61           0 :         return Status::ERROR;
      62             :     }
      63             : 
      64       63857 :     AC_PrecLand::TargetState precland_target_state =  _precland->get_target_state();
      65             : 
      66       63857 :     switch (precland_target_state) {
      67           0 :     case AC_PrecLand::TargetState::TARGET_RECENTLY_LOST:
      68             :         // we have lost the target but had it in sight at least once recently
      69             :         // action will depend on what user wants
      70           0 :         return get_target_lost_actions(retry_pos_m);
      71             : 
      72       17160 :     case AC_PrecLand::TargetState::TARGET_NEVER_SEEN:
      73             :         // we have no clue where we are supposed to be landing
      74             :         // let user decide how strict our failsafe actions need to be
      75       17160 :         return Status::FAILSAFE;
      76             : 
      77       46697 :     case AC_PrecLand::TargetState::TARGET_OUT_OF_RANGE:
      78             :         // The target isn't in sight, but we can't run any fail safe measures or do landing retry
      79             :         // Therefore just descend for now, and check again later if retry is allowed
      80             :     case AC_PrecLand::TargetState::TARGET_FOUND:
      81             :         // no action required, target is in sight
      82       46697 :         reset_failed_landing_statemachine();
      83       46697 :         return Status::DESCEND;
      84             :     }
      85             : 
      86             :     // should never reach here, all values are handled above
      87           0 :     return Status::ERROR;
      88             : }
      89             : 
      90             : 
      91             : // Target is lost (i.e we had it in sight some time back), this method helps decide on what needs to be done next
      92             : // The chosen action depends on user set landing strictness and will be returned by this function
      93             : // Parameters: Vector3f "retry_pos_m" is filled with the required location if we need to retry landing.
      94           0 : AC_PrecLand_StateMachine::Status AC_PrecLand_StateMachine::get_target_lost_actions(Vector3p &retry_pos_m)
      95             : {
      96           0 :     AC_PrecLand *_precland = AP::ac_precland();
      97           0 :     if (_precland == nullptr) {
      98             :         // should never happen
      99           0 :         return Status::ERROR;
     100             :     }
     101             : 
     102           0 :     switch (landing_target_lost_action) {
     103           0 :     case TargetLostAction::INIT: {
     104             :         // figure out how strict the user is with the landing
     105           0 :         const RetryStrictness strictness =_precland->get_retry_strictness();
     106             :         switch (strictness) {
     107           0 :             case RetryStrictness::NORMAL:
     108             :             case RetryStrictness::VERY_STRICT:
     109             :                 // We eventually want to retry landing, but lets descend for some time and hope the target gets in sight
     110             :                 // If not, we will retry landing
     111           0 :                 landing_target_lost_action = TargetLostAction::DESCEND;
     112           0 :                 break;
     113           0 :             case RetryStrictness::NOT_STRICT:
     114             :                 // User just wants to land, prec land isn't a priority
     115           0 :                 landing_target_lost_action = TargetLostAction::LAND_VERTICALLY;
     116           0 :                 break;
     117             :         }
     118             :         // at this stage we will be descending no matter what
     119             :         // so no special action required
     120           0 :         return Status::DESCEND;
     121             :     }
     122             : 
     123           0 :     case TargetLostAction::DESCEND:
     124           0 :         if (AP_HAL::millis() - _precland->get_last_valid_target_ms() >=_precland->get_min_retry_time_sec() * 1000) {
     125             :             // we have descended for some time and the target still isn't in sight
     126             :             // lets retry
     127           0 :             landing_target_lost_action = TargetLostAction::RETRY_LANDING;
     128           0 :             _retry_state = RetryLanding::INIT;
     129             :         }
     130             :         // still descending, no other action
     131           0 :         return Status::DESCEND;
     132             : 
     133           0 :     case TargetLostAction::RETRY_LANDING:
     134             :         // retry the landing by going to another position
     135           0 :         return retry_landing(retry_pos_m);
     136             : 
     137           0 :     case TargetLostAction::LAND_VERTICALLY:
     138             :         // Just land vertically
     139             :         // we will not be retrying to any location here on, so return false
     140           0 :         return Status::DESCEND;
     141             :     }
     142             : 
     143             :     // should never reach here, all cases are handled above
     144           0 :     return Status::ERROR;
     145             : }
     146             : 
     147             : // Retry landing based on a previously known location of the landing target
     148             : // Returns the action that should be taken by the vehicle
     149             : // Vector3f "retry_pos_m" is filled with the required location.
     150           0 : AC_PrecLand_StateMachine::Status AC_PrecLand_StateMachine::retry_landing(Vector3p &retry_pos_m)
     151             : {
     152           0 :     AC_PrecLand *_precland = AP::ac_precland();
     153           0 :     if (_precland == nullptr) {
     154             :         // should never happen
     155           0 :         return Status::ERROR;
     156             :     }
     157             : 
     158           0 :     if (_precland->get_max_retry_allowed() == 0) {
     159             :         // user does not want retry
     160           0 :         return Status::FAILSAFE;
     161             :     }
     162             : 
     163           0 :     if (_retry_count > _precland->get_max_retry_allowed()) {
     164             :         // we have exhausted the amount of times vehicle was allowed to retry landing
     165             :         // do failsafe measure so the vehicle isn't stuck in a constant loop
     166           0 :         return Status::FAILSAFE;
     167             :     }
     168             : 
     169             :     // get the retry position. This depends on what retry behavior has been set by user
     170           0 :     Vector3p go_to_pos;
     171           0 :     const RetryAction retry_action = _precland->get_retry_behaviour();
     172           0 :     if (retry_action == RetryAction::GO_TO_TARGET_LOC) {
     173           0 :         _precland->get_last_detected_landing_pos_NED_m(go_to_pos);
     174           0 :     } else if (retry_action == RetryAction::GO_TO_LAST_LOC) {
     175           0 :         _precland->get_last_vehicle_pos_when_target_detected_NED_m(go_to_pos);
     176             :     }
     177             : 
     178             :     // add a little bit offset so the vehicle climbs slightly higher than where it was
     179             :     // remember this is "D" frame and in meters's
     180           0 :     go_to_pos.z -= RETRY_OFFSET_ALT_M;
     181             : 
     182           0 :     switch (_retry_state) {
     183           0 :     case RetryLanding::INIT:
     184             :         // Init the Retry
     185           0 :         _retry_count ++;
     186           0 :         _retry_state = RetryLanding::IN_PROGRESS;
     187             :         // inform the user what we are doing
     188           0 :         if (_retry_count <= _precland->get_max_retry_allowed()) {
     189           0 :             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PrecLand: Retrying");
     190             :         }
     191           0 :         retry_pos_m = go_to_pos;
     192           0 :         return Status::RETRYING;
     193             : 
     194           0 :     case RetryLanding::IN_PROGRESS: {
     195             :         // continue converging towards the target till we are close by
     196           0 :         retry_pos_m = go_to_pos;
     197           0 :         Vector3p pos;
     198           0 :         if (!AP::ahrs().get_relative_position_NED_origin(pos)) {
     199           0 :             return Status::ERROR;
     200             :         }
     201           0 :         const float dist_to_target = (go_to_pos-pos).length();
     202           0 :         if ((dist_to_target < MAX_POS_ERROR_M)) {
     203             :             // we have approx reached landing location previously detected
     204           0 :             _retry_state = RetryLanding::DESCEND;
     205             :         }
     206           0 :         return Status::RETRYING;
     207             :     }
     208             : 
     209           0 :     case RetryLanding::DESCEND: {
     210             :         // descend a little bit before completing the retry
     211             :         // This will descend to the original height of where landing target was first detected
     212           0 :         Vector3p pos;
     213           0 :         if (!AP::ahrs().get_relative_position_NED_origin(pos)) {
     214           0 :             return Status::ERROR;
     215             :         }
     216             :         // z_target is in "D" frame
     217           0 :         const float z_target = go_to_pos.z + RETRY_OFFSET_ALT_M;
     218           0 :         retry_pos_m = Vector3p{pos.x, pos.y, z_target};
     219           0 :         if (fabsf(pos.z - retry_pos_m.z) < MAX_POS_ERROR_M) {
     220             :             // we have descended to the original height where we started the climb from
     221           0 :             _retry_state = RetryLanding::COMPLETE;
     222           0 :             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PrecLand: Retry Completed");
     223             :         }
     224           0 :         return Status::RETRYING;
     225             :     }
     226             : 
     227           0 :     case RetryLanding::COMPLETE:
     228             :         // Vehicle has completed a retry, and most likely the landing location still isn't sight
     229             :         // we have no choice but to force a failsafe action
     230           0 :         return Status::FAILSAFE;
     231             :     }
     232             : 
     233             :     // should never reach here
     234           0 :     return Status::ERROR;
     235             : }
     236             : 
     237             : // This is only called when the current status of the state machine returns "failsafe" and will return the action that the vehicle should do
     238             : // At the moment this method only allows you to stop in air permanently, or land vertically
     239             : // Failsafe will only trigger as a last resort
     240       17160 : AC_PrecLand_StateMachine::FailSafeAction AC_PrecLand_StateMachine::get_failsafe_actions()
     241             : {
     242       17160 :     AC_PrecLand *_precland = AP::ac_precland();
     243       17160 :     if (_precland == nullptr) {
     244             :         // should never happen, just descend
     245           0 :         return FailSafeAction::DESCEND;
     246             :     }
     247             : 
     248       17160 :     if (!failsafe_initialized) {
     249             :         // start the timer
     250           2 :         failsafe_start_ms = AP_HAL::millis();
     251           2 :         failsafe_initialized = true;
     252           2 :         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PrecLand: Failsafe Measures");
     253             :     }
     254             : 
     255             :     // Depending on the strictness we will either land vertically, wait for some time and then land vertically, not land at all
     256       17160 :     const RetryStrictness strictness= _precland->get_retry_strictness();
     257       17160 :     switch (strictness) {
     258           0 :     case RetryStrictness::VERY_STRICT:
     259             :         // user does not want to land on anything but the target
     260             :         // stop landing (hover)
     261           0 :         return FailSafeAction::HOLD_POS;
     262             : 
     263       17160 :     case RetryStrictness::NORMAL:
     264       17160 :         if (AP_HAL::millis() - failsafe_start_ms < FAILSAFE_INIT_TIMEOUT_MS) {
     265             :             // stop the vehicle for at least a few seconds before descending
     266             :             // this might give user the chance to take over
     267             :             // we do not want to be too linent in landing vertically because of the strictness set by the user
     268        5600 :             return FailSafeAction::HOLD_POS;
     269             :         }
     270             :         // land the vehicle vertically
     271       11560 :         return FailSafeAction::DESCEND;
     272             : 
     273           0 :     case RetryStrictness::NOT_STRICT:
     274             :         // User wants to prioritize landing over staying in the air
     275           0 :         return FailSafeAction::DESCEND;
     276             :     }
     277             : 
     278             :     // should never reach here
     279           0 :     return FailSafeAction::DESCEND;
     280             : }
     281             : 
     282             : #endif // AC_PRECLAND_ENABLED