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

AC_Precland: Allow landing retry and failsafe handling

This commit is contained in:
Rishabh 2021-07-22 01:03:21 +05:30 committed by Randy Mackay
parent 7d92ab1fb0
commit d4edd84573
2 changed files with 234 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

181
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -7,6 +7,7 @@
#include "AC_PrecLand_IRLock.h"
#include "AC_PrecLand_SITL_Gazebo.h"
#include "AC_PrecLand_SITL.h"
#include <GCS_MAVLink/GCS.h>
#include <AP_AHRS/AP_AHRS.h>
@ -15,6 +16,8 @@ extern const AP_HAL::HAL& hal;
static const uint32_t EKF_INIT_TIME_MS = 2000; // EKF initialisation requires this many milliseconds of good sensor data
static const uint32_t EKF_INIT_SENSOR_MIN_UPDATE_MS = 500; // Sensor must update within this many ms during EKF init, else init will fail
static const uint32_t LANDING_TARGET_TIMEOUT_MS = 2000; // Sensor must update within this many ms, else prec landing will be switched off
static const uint32_t LANDING_TARGET_LOST_TIMEOUT_MS = 180000; // Target will be considered as "lost" if the last known location of the target is more than this many ms ago
static const float LANDING_TARGET_LOST_DIST_THRESH_M = 30; // If the last known location of the landing target is beyond this many meters, then we will consider it lost
const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
// @Param: ENABLED
@ -121,16 +124,57 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
// @Units: m
// @User: Advanced
AP_GROUPINFO("XY_DIST_MAX", 10, AC_PrecLand, _xy_max_dist_desc, 2.5f),
// @Param: STRICT
// @DisplayName: PrecLand strictness
// @Description: How strictly should the vehicle land on the target if target is lost
// @Values: 0: Land Vertically (Not strict), 1: Retry Landing(Normal Strictness), 2: Do not land (just Hover) (Very Strict)
AP_GROUPINFO("STRICT", 11, AC_PrecLand, _strict, 1),
// @Param: RET_MAX
// @DisplayName: PrecLand Maximum number of retires for a failed landing
// @Description: PrecLand Maximum number of retires for a failed landing. Set to zero to disable landing retry.
// @Range: 0 10
// @Increment: 1
AP_GROUPINFO("RET_MAX", 12, AC_PrecLand, _retry_max, 4),
// @Param: TIMEOUT
// @DisplayName: PrecLand retry timeout
// @Description: Time for which vehicle continues descend even if target is lost. After this time period, vehicle will attemp a landing retry depending on PLND_STRICT parameter.
// @Range: 0 20
// @Units: s
AP_GROUPINFO("TIMEOUT", 13, AC_PrecLand, _retry_timeout_sec, 4),
// @Param: RET_BEHAVE
// @DisplayName: PrecLand retry behaviour
// @Description: Prec Land will do the action selected by this parameter if a retry to a landing is needed
// @Values: 0: Go to the last location where landing target was detected, 1: Go towards the approximate location of the detected landing target
AP_GROUPINFO("RET_BEHAVE", 14, AC_PrecLand, _retry_behave, 0),
// @Param: ALT_MIN
// @DisplayName: PrecLand minimum alt for retry
// @Description: Vehicle will continue landing vertically even if target is lost below this height. This needs a rangefinder to work. Set to zero to disable this.
// @Range: 0 5
// @Units: m
AP_GROUPINFO("ALT_MIN", 15, AC_PrecLand, _sensor_min_alt, 0.75),
// @Param: ALT_MAX
// @DisplayName: PrecLand maximum alt for retry
// @Description: Vehicle will continue landing vertically until this height if target is not found. Below this height if landing target is not found, landing retry/failsafe might be attempted. This needs a rangefinder to work. Set to zero to disable this.
// @Range: 0 50
// @Units: m
AP_GROUPINFO("ALT_MAX", 16, AC_PrecLand, _sensor_max_alt, 8),
AP_GROUPEND
};
// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
AC_PrecLand::AC_PrecLand()
{
if (_singleton != nullptr) {
AP_HAL::panic("AC_PrecLand must be singleton");
}
_singleton = this;
// set parameters to defaults
AP_Param::setup_object_defaults(this, var_info);
}
@ -144,6 +188,9 @@ void AC_PrecLand::init(uint16_t update_rate_hz)
return;
}
// init as target TARGET_NEVER_SEEN, we will update this later
_current_target_state = TargetState::TARGET_NEVER_SEEN;
// default health to false
_backend = nullptr;
_backend_state.healthy = false;
@ -217,12 +264,17 @@ void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid)
inertial_data_newest.time_usec = AP_HAL::micros64();
_inertial_history->push_force(inertial_data_newest);
const float rangefinder_alt_m = rangefinder_alt_cm*0.01f; //cm to meter
// update estimator of target position
if (_backend != nullptr && _enabled) {
_backend->update();
run_estimator(rangefinder_alt_cm*0.01f, rangefinder_alt_valid);
run_estimator(rangefinder_alt_m, rangefinder_alt_valid);
}
// check the status of the landing target location
check_target_status(rangefinder_alt_m, rangefinder_alt_valid);
const uint32_t now = AP_HAL::millis();
if (now - last_log_ms > 40) { // 25Hz
last_log_ms = now;
@ -230,9 +282,92 @@ void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid)
}
}
// check the status of the target
void AC_PrecLand::check_target_status(float rangefinder_alt_m, bool rangefinder_alt_valid)
{
if (target_acquired()) {
// target in sight
_current_target_state = TargetState::TARGET_FOUND;
// early return because we already know the status
return;
}
// target not in sight
if (_current_target_state == TargetState::TARGET_FOUND ||
_current_target_state == TargetState::TARGET_RECENTLY_LOST) {
// we had target in sight, but not any more, i.e we have lost the target
_current_target_state = TargetState::TARGET_RECENTLY_LOST;
} else {
// we never had the target in sight
_current_target_state = TargetState::TARGET_NEVER_SEEN;
}
// We definitely do not have the target in sight
// check if the precision landing sensor is supposed to be in range
// this needs a valid rangefinder to work
if (!check_if_sensor_in_range(rangefinder_alt_m, rangefinder_alt_valid)) {
// Target is not in range (vehicle is either too high or too low). Vehicle will not be attempting any sort of landing retries during this period
_current_target_state = TargetState::TARGET_OUT_OF_RANGE;
return;
}
if (_current_target_state == TargetState::TARGET_RECENTLY_LOST) {
// check if it's nearby/found recently, else the status will be demoted to "TARGET_LOST"
Vector2f curr_pos;
if (AP::ahrs().get_relative_position_NE_origin(curr_pos)) {
const float dist_to_last_target_loc_xy = (curr_pos - Vector2f{_last_target_pos_rel_origin_NED.x, _last_target_pos_rel_origin_NED.y}).length();
const float dist_to_last_loc_xy = (curr_pos - Vector2f{_last_vehicle_pos_NED.x, _last_vehicle_pos_NED.y}).length();
if ((AP_HAL::millis() - _last_valid_target_ms) > LANDING_TARGET_LOST_TIMEOUT_MS) {
// the target has not been seen for a long time
// might as well consider it as "never seen"
_current_target_state = TargetState::TARGET_NEVER_SEEN;
return;
}
if ((dist_to_last_target_loc_xy > LANDING_TARGET_LOST_DIST_THRESH_M) || (dist_to_last_loc_xy > LANDING_TARGET_LOST_DIST_THRESH_M)) {
// the last known location of target is too far away
_current_target_state = TargetState::TARGET_NEVER_SEEN;
return;
}
}
}
}
// Check if the landing target is supposed to be in sight based on the height of the vehicle from the ground
// This needs a valid rangefinder to work, if the min/max parameters are non zero
bool AC_PrecLand::check_if_sensor_in_range(float rangefinder_alt_m, bool rangefinder_alt_valid) const
{
if (is_zero(_sensor_max_alt) && is_zero(_sensor_min_alt)) {
// no sensor limits have been specified, assume sensor is always in range
return true;
}
if (!rangefinder_alt_valid) {
// rangefinder isn't healthy. We might be at a very high altitude
return false;
}
if (rangefinder_alt_m > _sensor_max_alt && !is_zero(_sensor_max_alt)) {
// this prevents triggering a retry when we are too far away from the target
return false;
}
if (rangefinder_alt_m < _sensor_min_alt && !is_zero(_sensor_min_alt)) {
// this prevents triggering a retry when we are very close to the target
return false;
}
// target should be in range
return true;
}
bool AC_PrecLand::target_acquired()
{
if ((AP_HAL::millis()-_last_update_ms) > LANDING_TARGET_TIMEOUT_MS) {
if (_target_acquired) {
// just lost the landing target, inform the user. This message will only be sent once everytime target is lost
gcs().send_text(MAV_SEVERITY_CRITICAL, "PrecLand: Target Lost");
}
// not had a sensor update since a long time
// probably lost the target
_estimator_initialized = false;
@ -317,6 +452,10 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va
// Update if a new Line-Of-Sight measurement is available
if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) {
if (!_estimator_initialized) {
gcs().send_text(MAV_SEVERITY_INFO, "PrecLand: Target Found");
_estimator_initialized = true;
}
_target_pos_rel_est_NE.x = _target_pos_rel_meas_NED.x;
_target_pos_rel_est_NE.y = _target_pos_rel_meas_NED.y;
_target_vel_rel_est_NE.x = -inertial_data_delayed->inertialNavVelocity.x;
@ -334,7 +473,7 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va
}
case EstimatorType::KALMAN_FILTER: {
// Predict
if (target_acquired()) {
if (target_acquired() || _estimator_initialized) {
const float& dt = inertial_data_delayed->dt;
const Vector3f& vehicleDelVel = inertial_data_delayed->correctedVehicleDeltaVelocityNED;
@ -346,6 +485,8 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va
if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) {
float xy_pos_var = sq(_target_pos_rel_meas_NED.z*(0.01f + 0.01f*AP::ahrs().get_gyro().length()) + 0.02f);
if (!_estimator_initialized) {
// Inform the user landing target has been found
gcs().send_text(MAV_SEVERITY_INFO, "PrecLand: Target Found");
// start init of EKF. We will let the filter consume the data for a while before it available for consumption
// reset filter state
if (inertial_data_delayed->inertialNavVelocityValid) {
@ -401,9 +542,11 @@ void AC_PrecLand::check_ekf_init_timeout()
if (AP_HAL::millis()-_last_update_ms > EKF_INIT_SENSOR_MIN_UPDATE_MS) {
// we have lost the target, not enough readings to initialize the EKF
_estimator_initialized = false;
gcs().send_text(MAV_SEVERITY_CRITICAL, "PrecLand: Init Failed");
} else if (AP_HAL::millis()-_estimator_init_ms > EKF_INIT_TIME_MS) {
// the target has been visible for a while, EKF should now have initialized to a good value
_target_acquired = true;
gcs().send_text(MAV_SEVERITY_INFO, "PrecLand: Init Complete");
}
}
}
@ -458,6 +601,14 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m,
// Compute target position relative to IMU
_target_pos_rel_meas_NED = Vector3f{target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt} + cam_pos_ned_rel_imu;
// store the current relative down position so that if we need to retry landing, we know at this height landing target can be found
const AP_AHRS &_ahrs = AP::ahrs();
Vector3f pos_NED;
if (_ahrs.get_relative_position_NED_origin(pos_NED)) {
_last_target_pos_rel_origin_NED.z = pos_NED.z;
_last_vehicle_pos_NED = pos_NED;
}
return true;
}
}
@ -502,6 +653,15 @@ void AC_PrecLand::run_output_prediction()
Vector3f land_ofs_ned_m = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0) * 0.01f;
_target_pos_rel_out_NE.x += land_ofs_ned_m.x;
_target_pos_rel_out_NE.y += land_ofs_ned_m.y;
// store the landing target as a offset from current position. This is used in landing retry
Vector2f last_target_loc_rel_origin_2d;
get_target_position_cm(last_target_loc_rel_origin_2d);
_last_target_pos_rel_origin_NED.x = last_target_loc_rel_origin_2d.x * 0.01f;
_last_target_pos_rel_origin_NED.y = last_target_loc_rel_origin_2d.y * 0.01f;
// record the last time there was a target output
_last_valid_target_ms = AP_HAL::millis();
}
// Write a precision landing entry
@ -538,3 +698,14 @@ void AC_PrecLand::Write_Precland()
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
// singleton instance
AC_PrecLand *AC_PrecLand::_singleton;
namespace AP {
AC_PrecLand *ac_precland()
{
return AC_PrecLand::get_singleton();
}
}

59
libraries/AC_PrecLand/AC_PrecLand.h
View File

@ -5,6 +5,7 @@
#include <stdint.h>
#include "PosVelEKF.h"
#include <AP_HAL/utility/RingBuffer.h>
#include <AC_PrecLand/AC_PrecLand_StateMachine.h>
// declare backend classes
class AC_PrecLand_Backend;
@ -29,6 +30,11 @@ public:
AC_PrecLand(const AC_PrecLand &other) = delete;
AC_PrecLand &operator=(const AC_PrecLand&) = delete;
// return singleton
static AC_PrecLand *get_singleton() {
return _singleton;
}
// perform any required initialisation of landing controllers
// update_rate_hz should be the rate at which the update method will be called in hz
void init(uint16_t update_rate_hz);
@ -72,6 +78,32 @@ public:
// process a LANDING_TARGET mavlink message
void handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms);
// State of the Landing Target Location
enum class TargetState: uint8_t {
TARGET_NEVER_SEEN = 0,
TARGET_OUT_OF_RANGE,
TARGET_RECENTLY_LOST,
TARGET_FOUND
};
// return the last time PrecLand library had a output of the landing target position
uint32_t get_last_valid_target_ms() const { return _last_valid_target_ms; }
// return the current state of the location of the target
TargetState get_target_state() const { return _current_target_state; }
// return the last known landing position in Earth Frame NED meters.
void get_last_detected_landing_pos(Vector3f &pos) const { pos = _last_target_pos_rel_origin_NED; }
// return the last known postion of the vehicle when the target was detected in Earth Frame NED meters.
void get_last_vehicle_pos_when_target_detected(Vector3f &pos) const { pos = _last_vehicle_pos_NED; }
// Parameter getters
AC_PrecLand_StateMachine::RetryStrictness get_retry_strictness() const { return static_cast<AC_PrecLand_StateMachine::RetryStrictness>(_strict.get()); }
uint8_t get_max_retry_allowed() const { return _retry_max; }
float get_min_retry_time_sec() const { return _retry_timeout_sec; }
AC_PrecLand_StateMachine::RetryAction get_retry_behaviour() const { return static_cast<AC_PrecLand_StateMachine::RetryAction>(_retry_behave.get()); }
// parameter var table
static const struct AP_Param::GroupInfo var_info[];
@ -90,6 +122,13 @@ private:
SITL = 4,
};
// check the status of the target
void check_target_status(float rangefinder_alt_m, bool rangefinder_alt_valid);
// Check if the landing target is supposed to be in sight based on the height of the vehicle from the ground
// This needs a valid rangefinder to work, if the min/max parameters are non zero
bool check_if_sensor_in_range(float rangefinder_alt_m, bool rangefinder_alt_valid) const;
// check if EKF got the time to initialize when the landing target was first detected
// Expects sensor to update within EKF_INIT_SENSOR_MIN_UPDATE_MS milliseconds till EKF_INIT_TIME_MS milliseconds have passed
// after this period landing target estimates can be used by vehicle
@ -120,23 +159,35 @@ private:
AP_Float _accel_noise; // accelerometer process noise
AP_Vector3f _cam_offset; // Position of the camera relative to the CG
AP_Float _xy_max_dist_desc; // Vehicle doing prec land will only descent vertically when horizontal error (in m) is below this limit
AP_Int8 _strict; // PrecLand strictness
AP_Int8 _retry_max; // PrecLand Maximum number of retires to a failed landing
AP_Float _retry_timeout_sec; // Time for which vehicle continues descend even if target is lost. After this time period, vehicle will attemp a landing retry depending on PLND_STRICT param.
AP_Int8 _retry_behave; // Action to do when trying a landing retry
AP_Float _sensor_min_alt; // PrecLand minimum height required for detecting target
AP_Float _sensor_max_alt; // PrecLand maximum height the sensor can detect target
uint32_t _last_update_ms; // system time in millisecond when update was last called
bool _target_acquired; // true if target has been seen recently after estimator is initialized
bool _estimator_initialized; // true if estimator has been initialized after few seconds of the target being detected by sensor
uint32_t _estimator_init_ms; // system time in millisecond when EKF was init
uint32_t _last_backend_los_meas_ms; // system time target was last seen
uint32_t _last_valid_target_ms; // last time PrecLand library had a output of the landing target position
PosVelEKF _ekf_x, _ekf_y; // Kalman Filter for x and y axis
uint32_t _outlier_reject_count; // mini-EKF's outlier counter (3 consecutive outliers lead to EKF accepting updates)
Vector3f _target_pos_rel_meas_NED; // target's relative position as 3D vector
Vector3f _last_target_pos_rel_origin_NED; // stores the last known location of the target horizontally, and the height of the vehicle where it detected this target in meters NED
Vector3f _last_vehicle_pos_NED; // stores the position of the vehicle when landing target was last detected in m and NED
Vector2f _target_pos_rel_est_NE; // target's position relative to the IMU, not compensated for lag
Vector2f _target_vel_rel_est_NE; // target's velocity relative to the IMU, not compensated for lag
Vector2f _target_pos_rel_out_NE; // target's position relative to the camera, fed into position controller
Vector2f _target_vel_rel_out_NE; // target's velocity relative to the CG, fed into position controller
TargetState _current_target_state; // Current status of the landing target
// structure and buffer to hold a history of vehicle velocity
struct inertial_data_frame_s {
Matrix3f Tbn; // dcm rotation matrix to rotate body frame to north
@ -157,4 +208,10 @@ private:
// write out PREC message to log:
void Write_Precland();
uint32_t last_log_ms; // last time we logged
static AC_PrecLand *_singleton; //singleton
};
namespace AP {
AC_PrecLand *ac_precland();
};