Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PrecLand: Add prec landing state machine

This commit is contained in:
Rishabh 2021-08-14 15:28:33 +05:30 committed by Randy Mackay
parent c5b98c3490
commit ae98545202
2 changed files with 364 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

259
libraries/AC_PrecLand/AC_PrecLand_StateMachine.cpp Normal file
View File

@ -0,0 +1,259 @@
#include "AC_PrecLand_StateMachine.h"
#include <AC_PrecLand/AC_PrecLand.h>
#include <AP_AHRS/AP_AHRS.h>
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;
}

105
libraries/AC_PrecLand/AC_PrecLand_StateMachine.h Normal file
View File

@ -0,0 +1,105 @@
#pragma once
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS.h>
// 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
};