diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 4e9a084a44..1e57a8a578 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -7,6 +7,7 @@ #include "AC_PrecLand_IRLock.h" #include "AC_PrecLand_SITL_Gazebo.h" #include "AC_PrecLand_SITL.h" +#include #include @@ -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(); +} + +} diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index 0490e65218..056206696c 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -5,6 +5,7 @@ #include #include "PosVelEKF.h" #include +#include // 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(_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(_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(); };