Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AC_PrecLand_config.h"
       4             : 
       5             : #if AC_PRECLAND_ENABLED
       6             : 
       7             : #include <GCS_MAVLink/GCS_MAVLink.h>
       8             : #include <AP_Math/AP_Math.h>
       9             : #include <stdint.h>
      10             : #include "PosVelEKF.h"
      11             : #include <AP_HAL/utility/RingBuffer.h>
      12             : #include <AC_PrecLand/AC_PrecLand_StateMachine.h>
      13             : 
      14             : // declare backend classes
      15             : class AC_PrecLand_Backend;
      16             : class AC_PrecLand_MAVLink;
      17             : class AC_PrecLand_IRLock;
      18             : class AC_PrecLand_SITL_Gazebo;
      19             : class AC_PrecLand_SITL;
      20             : class Location;
      21             : 
      22             : class AC_PrecLand
      23             : {
      24             :     // declare backends as friends
      25             :     friend class AC_PrecLand_Backend;
      26             :     friend class AC_PrecLand_MAVLink;
      27             :     friend class AC_PrecLand_IRLock;
      28             :     friend class AC_PrecLand_SITL_Gazebo;
      29             :     friend class AC_PrecLand_SITL;
      30             : 
      31             : public:
      32             :     AC_PrecLand();
      33             : 
      34             :     /* Do not allow copies */
      35             :     CLASS_NO_COPY(AC_PrecLand);
      36             : 
      37             :     // return singleton
      38       84501 :     static AC_PrecLand *get_singleton() {
      39       84501 :         return _singleton;
      40             :     }
      41             : 
      42             :     // perform any required initialisation of landing controllers
      43             :     // update_rate_hz should be the rate at which the update method will be called in hz
      44             :     void init(uint16_t update_rate_hz);
      45             : 
      46             :     // returns true if precision landing is healthy
      47       28931 :     bool healthy() const { return _backend_state.healthy; }
      48             : 
      49             :     // returns true if precision landing is enabled (used only for logging)
      50     5315060 :     bool enabled() const { return _enabled.get(); }
      51             : 
      52             :     // returns time of last update
      53             :     uint32_t last_update_ms() const { return _last_update_ms; }
      54             : 
      55             :     // returns time of last time target was seen
      56       24679 :     uint32_t last_backend_los_meas_ms() const { return _last_backend_los_meas_ms; }
      57             : 
      58             :     // vehicle has to be closer than this many m's to the target before descending towards target
      59       34267 :     float get_max_xy_error_before_descending_m() const { return _xy_max_dist_desc_m; }
      60             : 
      61             :     // returns orientation of sensor
      62           3 :     Rotation get_orient() const { return _orient; }
      63             : 
      64             :     // returns ekf outlier count
      65       24679 :     uint32_t ekf_outlier_count() const { return _outlier_reject_count; }
      66             : 
      67             :     // give chance to driver to get updates from sensor, should be called at 400hz
      68             :     void update(float rangefinder_alt_m, bool rangefinder_alt_valid);
      69             : 
      70             :     // returns target position relative to the EKF origin
      71             :     bool get_target_position_m(Vector2p& ret);
      72             : 
      73             :     // returns target relative position as 3D vector
      74             :     void get_target_position_measurement_NED_m(Vector3f& ret);
      75             : 
      76             :     // returns target position relative to vehicle
      77             :     bool get_target_position_relative_NE_m(Vector2f& ret);
      78             : 
      79             :     // returns target velocity relative to vehicle
      80             :     bool get_target_velocity_relative_NE_ms(Vector2f& ret);
      81             : 
      82             :     // get the absolute velocity of the vehicle
      83             :     void get_target_velocity_ms(const Vector2f& vehicle_velocity_ms, Vector2f& target_vel_ms);
      84             : 
      85             :     // returns true when the landing target has been detected
      86             :     bool target_acquired();
      87             : 
      88             :     // process a LANDING_TARGET mavlink message
      89             :     void handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms);
      90             : 
      91             :     // State of the Landing Target Location
      92             :     enum class TargetState: uint8_t {
      93             :         TARGET_NEVER_SEEN = 0,
      94             :         TARGET_OUT_OF_RANGE,
      95             :         TARGET_RECENTLY_LOST,
      96             :         TARGET_FOUND
      97             :     };
      98             : 
      99             :     // return the last time PrecLand library had a output of the landing target position
     100           0 :     uint32_t get_last_valid_target_ms() const { return _last_valid_target_ms; }
     101             : 
     102             :     // return the current state of the location of the target
     103       63857 :     TargetState get_target_state() const { return _current_target_state; }
     104             : 
     105             :     // return the last known landing position in Earth Frame NED meters.
     106           0 :     void get_last_detected_landing_pos_NED_m(Vector3p &pos) const { pos = _last_target_pos_rel_origin_ned_m; }
     107             : 
     108             :     // return the last known position of the vehicle when the target was detected in Earth Frame NED meters.
     109           0 :     void get_last_vehicle_pos_when_target_detected_NED_m(Vector3p &pos) const { pos = _last_vehicle_pos_ned_m; }
     110             : 
     111             :     // Parameter getters
     112       17160 :     AC_PrecLand_StateMachine::RetryStrictness get_retry_strictness() const { return static_cast<AC_PrecLand_StateMachine::RetryStrictness>(_strict.get()); }
     113           0 :     uint8_t get_max_retry_allowed() const { return _retry_max; }
     114           0 :     float get_min_retry_time_sec() const { return _retry_timeout_s; }
     115           0 :     AC_PrecLand_StateMachine::RetryAction get_retry_behaviour() const { return static_cast<AC_PrecLand_StateMachine::RetryAction>(_retry_behave.get()); }
     116             : 
     117     4274071 :     bool allow_precland_after_reposition() const { return _options & PLND_OPTION_PRECLAND_AFTER_REPOSITION; }
     118       10320 :     bool do_fast_descend() const { return _options & PLND_OPTION_FAST_DESCEND; }
     119             : 
     120             :     /*
     121             :       get target location lat/lon. Note that altitude in returned
     122             :       location is not reliable
     123             :      */
     124             :     bool get_target_location(Location &loc);
     125             : 
     126             :     /*
     127             :       get the absolute velocity of the target in m/s.
     128             :       return false if we cannot estimate target velocity or if the target is not acquired
     129             :     */
     130             :     bool get_target_velocity(Vector2f& ret);
     131             : 
     132             :     // parameter var table
     133             :     static const struct AP_Param::GroupInfo var_info[];
     134             : 
     135             : private:
     136             :     enum class EstimatorType : uint8_t {
     137             :         RAW_SENSOR = 0,
     138             :         KALMAN_FILTER = 1,
     139             :     };
     140             : 
     141             :     // types of precision landing (used for PRECLAND_TYPE parameter)
     142             :     enum class Type : uint8_t {
     143             :         NONE = 0,
     144             : #if AC_PRECLAND_MAVLINK_ENABLED
     145             :         MAVLINK = 1,
     146             : #endif
     147             : #if AC_PRECLAND_IRLOCK_ENABLED
     148             :         IRLOCK = 2,
     149             : #endif
     150             : #if AC_PRECLAND_SITL_GAZEBO_ENABLED
     151             :         SITL_GAZEBO = 3,
     152             : #endif
     153             : #if AC_PRECLAND_SITL_ENABLED
     154             :         SITL = 4,
     155             : #endif
     156             :     };
     157             : 
     158             :     enum PLndOptions {
     159             :         PLND_OPTION_DISABLED = 0,
     160             :         PLND_OPTION_MOVING_TARGET = (1 << 0),
     161             :         PLND_OPTION_PRECLAND_AFTER_REPOSITION = (1 << 1),
     162             :         PLND_OPTION_FAST_DESCEND = (1 << 2),
     163             :     };
     164             : 
     165             :     // frames for vectors from vehicle to target
     166             :     enum class VectorFrame : uint8_t {
     167             :         BODY_FRD = 0,     // body frame, forward-right-down relative to the vehicle's attitude
     168             :         LOCAL_FRD = 1,    // forward-right-down where forward is aligned with front of the vehicle in the horizontal plane
     169             :     };
     170             : 
     171             :     // check the status of the target
     172             :     void check_target_status(float rangefinder_alt_m, bool rangefinder_alt_valid);
     173             : 
     174             :     // Check if the landing target is supposed to be in sight based on the height of the vehicle from the ground
     175             :     // This needs a valid rangefinder to work, if the min/max parameters are non zero
     176             :     bool check_if_sensor_in_range(float rangefinder_alt_m, bool rangefinder_alt_valid) const;
     177             : 
     178             :     // check if EKF got the time to initialize when the landing target was first detected
     179             :     // Expects sensor to update within EKF_INIT_SENSOR_MIN_UPDATE_MS milliseconds till EKF_INIT_TIME_MS milliseconds have passed
     180             :     // after this period landing target estimates can be used by vehicle
     181             :     void check_ekf_init_timeout();
     182             : 
     183             :     // run target position estimator
     184             :     void run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid);
     185             : 
     186             :     // If a new measurement was retrieved, sets _target_pos_rel_meas_ned_m and returns true
     187             :     bool construct_pos_meas_using_rangefinder(float rangefinder_alt_m, bool rangefinder_alt_valid);
     188             : 
     189             :     // get 3D vector from vehicle to target and frame.  returns true on success, false on failure
     190             :     bool retrieve_los_meas(Vector3f& target_vec_unit, VectorFrame& frame);
     191             : 
     192             :     // calculate target's position and velocity relative to the vehicle (used as input to position controller)
     193             :     // results are stored in_target_pos_rel_out_NE, _target_vel_rel_out_ne_ms
     194             :     void run_output_prediction();
     195             : 
     196             :     // parameters
     197             :     AP_Int8                     _enabled;               // enabled/disabled
     198             :     AP_Enum<Type>               _type;                  // precision landing sensor type
     199             :     AP_Int8                     _bus;                   // which sensor bus
     200             :     AP_Enum<EstimatorType>      _estimator_type;        // precision landing estimator type
     201             :     AP_Float                    _lag_s;                 // sensor lag in seconds
     202             :     AP_Float                    _yaw_align_cd;          // Yaw angle from body x-axis to sensor x-axis.
     203             :     AP_Float                    _land_ofs_cm_x;         // Desired landing position of the camera forward of the target in vehicle body frame
     204             :     AP_Float                    _land_ofs_cm_y;         // Desired landing position of the camera right of the target in vehicle body frame
     205             :     AP_Float                    _accel_noise;           // accelerometer process noise
     206             :     AP_Vector3f                 _cam_offset_m;          // Position of the camera relative to the CG
     207             :     AP_Float                    _xy_max_dist_desc_m;    // Vehicle doing prec land will only descent vertically when horizontal error (in m) is below this limit
     208             :     AP_Int8                     _strict;                // PrecLand strictness
     209             :     AP_Int8                     _retry_max;             // PrecLand Maximum number of retires to a failed landing
     210             :     AP_Float                    _retry_timeout_s;       // Time for which vehicle continues descend even if target is lost. After this time period, vehicle will attempt a landing retry depending on PLND_STRICT param.
     211             :     AP_Int8                     _retry_behave;          // Action to do when trying a landing retry
     212             :     AP_Float                    _sensor_min_alt_m;      // PrecLand minimum height required for detecting target
     213             :     AP_Float                    _sensor_max_alt_m;      // PrecLand maximum height the sensor can detect target
     214             :     AP_Int16                    _options;               // Bitmask for extra options
     215             :     AP_Enum<Rotation>           _orient;                // Orientation of camera/sensor
     216             : 
     217             :     uint32_t                    _last_update_ms;            // system time in millisecond when update was last called
     218             :     bool                        _target_acquired;           // true if target has been seen recently after estimator is initialized
     219             :     bool                        _estimator_initialized;     // true if estimator has been initialized after few seconds of the target being detected by sensor
     220             :     uint32_t                    _estimator_init_ms;         // system time in millisecond when EKF was init
     221             :     uint32_t                    _last_backend_los_meas_ms;  // system time target was last seen
     222             :     uint32_t                    _last_valid_target_ms;      // last time PrecLand library had a output of the landing target position
     223             : 
     224             :     PosVelEKF                   _ekf_x, _ekf_y;             // Kalman Filter for x and y axis
     225             :     uint32_t                    _outlier_reject_count;      // mini-EKF's outlier counter (3 consecutive outliers lead to EKF accepting updates)
     226             : 
     227             :     Vector3f                    _target_pos_rel_meas_ned_m; // target's relative position as 3D vector
     228             :     Vector3f                    _approach_vector_body;      // unit vector in landing approach direction (in body frame)
     229             : 
     230             :     Vector3p                    _last_target_pos_rel_origin_ned_m;  // stores the last known location of the target horizontally, and the height of the vehicle where it detected this target in meters NED
     231             :     Vector3p                    _last_vehicle_pos_ned_m;            // stores the position of the vehicle when landing target was last detected in m and NED
     232             :     Vector2f                    _target_pos_rel_est_ne_m;           // target's position relative to the IMU, not compensated for lag
     233             :     Vector2f                    _target_vel_rel_est_ne_ms;          // target's velocity relative to the IMU, not compensated for lag
     234             : 
     235             :     Vector2f                    _target_pos_rel_out_ne_m;   // target's position relative to the camera, fed into position controller
     236             :     Vector2f                    _target_vel_rel_out_ne_ms;  // target's velocity relative to the CG, fed into position controller
     237             :     Vector3f                    _last_veh_velocity_NED_ms;  // AHRS velocity at last estimate
     238             : 
     239             :     TargetState                 _current_target_state;      // Current status of the landing target
     240             : 
     241             :     // structure and buffer to hold a history of vehicle velocity
     242             :     struct inertial_data_frame_s {
     243             :         Matrix3f Tbn;                               // dcm rotation matrix to rotate body frame to north
     244             :         Vector3f correctedVehicleDeltaVelocityNED;
     245             :         Vector3f inertialNavVelocity;
     246             :         bool inertialNavVelocityValid;
     247             :         float dt;
     248             :         uint64_t time_usec;
     249             :     };
     250             :     ObjectArray<inertial_data_frame_s> *_inertial_history;
     251             :     struct inertial_data_frame_s *_inertial_data_delayed;
     252             : 
     253             :     // backend state
     254             :     struct precland_state {
     255             :         bool    healthy;
     256             :     } _backend_state;
     257             :     AC_PrecLand_Backend         *_backend;  // pointers to backend precision landing driver
     258             : 
     259             :     // write out PREC message to log:
     260             :     void Write_Precland();
     261             :     uint32_t _last_log_ms;  // last time we logged
     262             : 
     263             :     static AC_PrecLand *_singleton; //singleton
     264             : };
     265             : 
     266             : namespace AP {
     267             :     AC_PrecLand *ac_precland();
     268             : };
     269             : 
     270             : #endif // AC_PRECLAND_ENABLED