2016-02-17 21:25:09 -04:00
# pragma once
2015-02-16 00:37:13 -04:00
2023-03-22 05:45:41 -03:00
# include "AC_PrecLand_config.h"
# if AC_PRECLAND_ENABLED
2016-06-16 14:10:48 -03:00
# include <GCS_MAVLink/GCS_MAVLink.h>
2023-03-22 05:45:41 -03:00
# include <AP_Math/AP_Math.h>
2016-06-16 14:10:48 -03:00
# include <stdint.h>
2016-07-15 14:30:21 -03:00
# include "PosVelEKF.h"
2018-07-11 09:58:38 -03:00
# include <AP_HAL/utility/RingBuffer.h>
2021-07-21 16:33:21 -03:00
# include <AC_PrecLand/AC_PrecLand_StateMachine.h>
2015-02-16 00:37:13 -04:00
// declare backend classes
class AC_PrecLand_Backend ;
class AC_PrecLand_Companion ;
class AC_PrecLand_IRLock ;
2016-11-10 07:41:17 -04:00
class AC_PrecLand_SITL_Gazebo ;
2016-11-11 01:10:35 -04:00
class AC_PrecLand_SITL ;
2015-02-16 00:37:13 -04:00
class AC_PrecLand
{
// declare backends as friends
friend class AC_PrecLand_Backend ;
friend class AC_PrecLand_Companion ;
friend class AC_PrecLand_IRLock ;
2016-11-10 07:41:17 -04:00
friend class AC_PrecLand_SITL_Gazebo ;
2016-11-11 01:10:35 -04:00
friend class AC_PrecLand_SITL ;
2015-02-16 00:37:13 -04:00
public :
2018-07-15 21:29:20 -03:00
AC_PrecLand ( ) ;
2017-12-12 21:06:10 -04:00
/* Do not allow copies */
2022-09-30 06:50:43 -03:00
CLASS_NO_COPY ( AC_PrecLand ) ;
2017-12-12 21:06:10 -04:00
2021-07-21 16:33:21 -03:00
// return singleton
static AC_PrecLand * get_singleton ( ) {
return _singleton ;
}
2016-06-16 14:10:48 -03:00
// perform any required initialisation of landing controllers
2018-09-21 04:01:30 -03:00
// update_rate_hz should be the rate at which the update method will be called in hz
void init ( uint16_t update_rate_hz ) ;
2015-02-16 00:37:13 -04:00
2016-06-16 14:10:48 -03:00
// returns true if precision landing is healthy
2016-07-15 14:30:21 -03:00
bool healthy ( ) const { return _backend_state . healthy ; }
2015-02-16 00:37:13 -04:00
2017-03-16 05:01:16 -03:00
// returns true if precision landing is enabled (used only for logging)
bool enabled ( ) const { return _enabled . get ( ) ; }
2016-06-16 14:10:48 -03:00
// returns time of last update
2016-07-15 14:30:21 -03:00
uint32_t last_update_ms ( ) const { return _last_update_ms ; }
2016-06-16 14:10:48 -03:00
2018-08-07 22:23:08 -03:00
// returns time of last time target was seen
uint32_t last_backend_los_meas_ms ( ) const { return _last_backend_los_meas_ms ; }
2021-08-05 16:04:22 -03:00
// vehicle has to be closer than this many cm's to the target before descending towards target
float get_max_xy_error_before_descending_cm ( ) const { return _xy_max_dist_desc * 100.0f ; }
2022-06-17 09:02:02 -03:00
// returns orientation of sensor
Rotation get_orient ( ) const { return _orient ; }
2018-08-07 22:23:08 -03:00
// returns ekf outlier count
uint32_t ekf_outlier_count ( ) const { return _outlier_reject_count ; }
2017-03-16 05:01:16 -03:00
// give chance to driver to get updates from sensor, should be called at 400hz
2016-07-15 14:30:21 -03:00
void update ( float rangefinder_alt_cm , bool rangefinder_alt_valid ) ;
2015-02-16 00:37:13 -04:00
2017-03-16 05:01:16 -03:00
// returns target position relative to the EKF origin
2017-02-27 18:50:04 -04:00
bool get_target_position_cm ( Vector2f & ret ) ;
2016-06-16 14:10:48 -03:00
2018-08-07 22:23:08 -03:00
// returns target relative position as 3D vector
void get_target_position_measurement_cm ( Vector3f & ret ) ;
2016-06-16 14:10:48 -03:00
// returns target position relative to vehicle
2017-02-27 18:50:04 -04:00
bool get_target_position_relative_cm ( Vector2f & ret ) ;
2016-06-16 14:10:48 -03:00
// returns target velocity relative to vehicle
2017-02-27 18:50:04 -04:00
bool get_target_velocity_relative_cms ( Vector2f & ret ) ;
2016-06-16 14:10:48 -03:00
2021-09-05 17:08:56 -03:00
// get the absolute velocity of the vehicle
void get_target_velocity_cms ( const Vector2f & vehicle_velocity_cms , Vector2f & target_vel_cms ) ;
2016-06-16 14:10:48 -03:00
// returns true when the landing target has been detected
2017-02-27 18:50:04 -04:00
bool target_acquired ( ) ;
2016-06-16 14:10:48 -03:00
// process a LANDING_TARGET mavlink message
2021-04-12 23:49:59 -03:00
void handle_msg ( const mavlink_landing_target_t & packet , uint32_t timestamp_ms ) ;
2015-09-11 08:00:18 -03:00
2021-07-21 16:33:21 -03:00
// 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 ( ) ) ; }
2023-01-29 06:08:46 -04:00
bool allow_precland_after_reposition ( ) const { return _options & PLND_OPTION_PRECLAND_AFTER_REPOSITION ; }
2023-09-04 00:11:24 -03:00
bool do_fast_descend ( ) const { return _options & PLND_OPTION_FAST_DESCEND ; }
2023-01-29 06:08:46 -04:00
2015-02-16 00:37:13 -04:00
// parameter var table
static const struct AP_Param : : GroupInfo var_info [ ] ;
private :
2021-04-14 00:02:22 -03:00
enum class EstimatorType : uint8_t {
RAW_SENSOR = 0 ,
KALMAN_FILTER = 1 ,
2017-02-27 18:50:04 -04:00
} ;
2021-04-14 00:16:18 -03:00
// types of precision landing (used for PRECLAND_TYPE parameter)
enum class Type : uint8_t {
NONE = 0 ,
2023-03-26 19:24:41 -03:00
# if AC_PRECLAND_COMPANION_ENABLED
2021-04-14 00:16:18 -03:00
COMPANION = 1 ,
2023-03-26 19:24:41 -03:00
# endif
# if AC_PRECLAND_IRLOCK_ENABLED
2021-04-14 00:16:18 -03:00
IRLOCK = 2 ,
2023-03-26 19:24:41 -03:00
# endif
# if AC_PRECLAND_SITL_GAZEBO_ENABLED
2021-04-14 00:16:18 -03:00
SITL_GAZEBO = 3 ,
2023-03-26 19:24:41 -03:00
# endif
# if AC_PRECLAND_SITL_ENABLED
2021-04-14 00:16:18 -03:00
SITL = 4 ,
2023-03-26 19:24:41 -03:00
# endif
2021-04-14 00:16:18 -03:00
} ;
2022-01-31 07:24:31 -04:00
enum PLndOptions {
PLND_OPTION_DISABLED = 0 ,
PLND_OPTION_MOVING_TARGET = ( 1 < < 0 ) ,
2023-01-29 06:08:46 -04:00
PLND_OPTION_PRECLAND_AFTER_REPOSITION = ( 1 < < 1 ) ,
2023-09-04 00:11:24 -03:00
PLND_OPTION_FAST_DESCEND = ( 1 < < 2 ) ,
2022-01-31 07:24:31 -04:00
} ;
2021-07-21 16:33:21 -03:00
// 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 ;
2021-06-05 06:31:44 -03:00
// 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
void check_ekf_init_timeout ( ) ;
2017-03-16 05:01:16 -03:00
// run target position estimator
2017-02-27 18:50:04 -04:00
void run_estimator ( float rangefinder_alt_m , bool rangefinder_alt_valid ) ;
2017-03-16 05:01:16 -03:00
2019-02-04 14:10:57 -04:00
// If a new measurement was retrieved, sets _target_pos_rel_meas_NED and returns true
2017-03-17 19:37:42 -03:00
bool construct_pos_meas_using_rangefinder ( float rangefinder_alt_m , bool rangefinder_alt_valid ) ;
2017-03-16 05:01:16 -03:00
// get vehicle body frame 3D vector from vehicle to target. returns true on success, false on failure
2017-02-27 18:50:04 -04:00
bool retrieve_los_meas ( Vector3f & target_vec_unit_body ) ;
2017-03-16 05:01:16 -03:00
// calculate target's position and velocity relative to the vehicle (used as input to position controller)
// results are stored in_target_pos_rel_out_NE, _target_vel_rel_out_NE
2017-02-27 18:50:04 -04:00
void run_output_prediction ( ) ;
2015-02-16 00:37:13 -04:00
// parameters
2021-04-14 00:12:31 -03:00
AP_Int8 _enabled ; // enabled/disabled
2021-04-14 00:16:18 -03:00
AP_Enum < Type > _type ; // precision landing sensor type
2017-04-11 21:19:21 -03:00
AP_Int8 _bus ; // which sensor bus
2021-04-14 00:02:22 -03:00
AP_Enum < EstimatorType > _estimator_type ; // precision landing estimator type
2018-09-19 00:34:47 -03:00
AP_Float _lag ; // sensor lag in seconds
2016-07-15 14:30:21 -03:00
AP_Float _yaw_align ; // Yaw angle from body x-axis to sensor x-axis.
AP_Float _land_ofs_cm_x ; // Desired landing position of the camera forward of the target in vehicle body frame
AP_Float _land_ofs_cm_y ; // Desired landing position of the camera right of the target in vehicle body frame
2019-02-04 14:10:57 -04:00
AP_Float _accel_noise ; // accelerometer process noise
2017-03-17 19:37:42 -03:00
AP_Vector3f _cam_offset ; // Position of the camera relative to the CG
2021-08-05 16:04:22 -03:00
AP_Float _xy_max_dist_desc ; // Vehicle doing prec land will only descent vertically when horizontal error (in m) is below this limit
2021-07-21 16:33:21 -03:00
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
2022-01-31 07:24:31 -04:00
AP_Int16 _options ; // Bitmask for extra options
2022-06-17 09:02:02 -03:00
AP_Enum < Rotation > _orient ; // Orientation of camera/sensor
2021-07-21 16:33:21 -03:00
2017-03-16 05:01:16 -03:00
uint32_t _last_update_ms ; // system time in millisecond when update was last called
2021-06-05 06:31:44 -03:00
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
2017-03-16 05:01:16 -03:00
uint32_t _last_backend_los_meas_ms ; // system time target was last seen
2021-07-21 16:33:21 -03:00
uint32_t _last_valid_target_ms ; // last time PrecLand library had a output of the landing target position
2016-06-16 14:10:48 -03:00
2017-03-16 05:01:16 -03:00
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)
2021-07-21 16:33:21 -03:00
2017-03-16 05:01:16 -03:00
Vector3f _target_pos_rel_meas_NED ; // target's relative position as 3D vector
2022-06-17 09:02:02 -03:00
Vector3f _approach_vector_body ; // unit vector in landing approach direction (in body frame)
2017-02-27 18:50:04 -04:00
2021-07-21 16:33:21 -03:00
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
2017-03-17 19:37:42 -03:00
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
2017-02-27 18:50:04 -04:00
2017-03-17 19:37:42 -03:00
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
2017-02-27 18:50:04 -04:00
2021-07-21 16:33:21 -03:00
TargetState _current_target_state ; // Current status of the landing target
2018-07-11 09:58:38 -03:00
// structure and buffer to hold a history of vehicle velocity
2017-02-27 18:50:04 -04:00
struct inertial_data_frame_s {
2017-03-16 05:01:16 -03:00
Matrix3f Tbn ; // dcm rotation matrix to rotate body frame to north
2017-02-27 18:50:04 -04:00
Vector3f correctedVehicleDeltaVelocityNED ;
Vector3f inertialNavVelocity ;
bool inertialNavVelocityValid ;
float dt ;
2018-07-11 09:58:38 -03:00
uint64_t time_usec ;
2017-02-27 18:50:04 -04:00
} ;
2018-07-11 09:58:38 -03:00
ObjectArray < inertial_data_frame_s > * _inertial_history ;
2015-02-16 00:37:13 -04:00
// backend state
struct precland_state {
bool healthy ;
} _backend_state ;
AC_PrecLand_Backend * _backend ; // pointers to backend precision landing driver
2021-04-12 07:40:26 -03:00
// write out PREC message to log:
void Write_Precland ( ) ;
uint32_t last_log_ms ; // last time we logged
2021-07-21 16:33:21 -03:00
static AC_PrecLand * _singleton ; //singleton
} ;
namespace AP {
AC_PrecLand * ac_precland ( ) ;
2015-02-16 00:37:13 -04:00
} ;
2023-03-22 05:45:41 -03:00
# endif // AC_PRECLAND_ENABLED