ardupilot/libraries/AC_PrecLand/AC_PrecLand_StateMachine.cpp

276 lines
11 KiB
C++
Raw Normal View History

#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()
{
AC_PrecLand *_precland = AP::ac_precland();
if (_precland == nullptr) {
// precland not enabled
return;
}
if (!_precland->enabled()) {
// precland is not enabled, prec land state machine methods should not be called!
return;
}
// 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;
}
if (!_precland->enabled()) {
// precland is not enabled, prec land state machine should not be called!
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;
}