Line data    Source code

       1             : #pragma once
       2             : 
       3             : /// @file    AP_L1_Control.h
       4             : /// @brief   L1 Control algorithm. This is a instance of an
       5             : /// AP_Navigation class
       6             : 
       7             : /*
       8             :  * Originally written by Brandon Jones 2013
       9             :  *
      10             :  *  Modified by Paul Riseborough 2013 to provide:
      11             :  *  - Explicit control over frequency and damping
      12             :  *  - Explicit control over track capture angle
      13             :  *  - Ability to use loiter radius smaller than L1 length
      14             :  */
      15             : 
      16             : #include <AP_Math/AP_Math.h>
      17             : #include <AP_Param/AP_Param.h>
      18             : #include <AP_Navigation/AP_Navigation.h>
      19             : #include <AP_TECS/AP_TECS.h>
      20             : #include <AP_Common/Location.h>
      21             : 
      22             : class AP_L1_Control : public AP_Navigation {
      23             : public:
      24         692 :     AP_L1_Control(AP_AHRS &ahrs, const AP_TECS *tecs)
      25        1384 :         : _ahrs(ahrs)
      26         692 :         , _tecs(tecs)
      27             :     {
      28         692 :         AP_Param::setup_object_defaults(this, var_info);
      29         692 :     }
      30             : 
      31             :     /* Do not allow copies */
      32             :     CLASS_NO_COPY(AP_L1_Control);
      33             : 
      34             :     /* see AP_Navigation.h for the definitions and units of these
      35             :      * functions */
      36             :     int32_t nav_roll_cd(void) const override;
      37             :     float lateral_acceleration(void) const override;
      38             : 
      39             :     // return the desired track heading angle(centi-degrees)
      40             :     int32_t nav_bearing_cd(void) const override;
      41             : 
      42             :     // return the heading error angle (centi-degrees) +ve to left of track
      43             :     int32_t bearing_error_cd(void) const override;
      44             : 
      45     1795692 :     float crosstrack_error_m(void) const override { return _crosstrack_error; }
      46     1310707 :     float crosstrack_error_integrator(void) const override { return _L1_xtrack_i; }
      47             : 
      48             :     int32_t target_bearing_cd(void) const override;
      49             :     float turn_distance(float wp_radius) const override;
      50             :     float turn_distance(float wp_radius, float turn_angle) const override;
      51             :     float loiter_radius (const float loiter_radius) const override;
      52             :     void update_waypoint(const class Location &prev_WP, const class Location &next_WP, float dist_min = 0.0f) override;
      53             :     void update_loiter(const class Location &center_WP, float radius, int8_t loiter_direction) override;
      54             :     void update_heading_hold(int32_t navigation_heading_cd) override;
      55             :     void update_level_flight(void) override;
      56             :     bool reached_loiter_target(void) override;
      57             : 
      58             :     // set the default NAVL1_PERIOD
      59             :     void set_default_period(float period) {
      60             :         _L1_period.set_default(period);
      61             :     }
      62             : 
      63        3174 :     void set_data_is_stale(void) override {
      64        3174 :         _data_is_stale = true;
      65        3174 :     }
      66         914 :     bool data_is_stale(void) const override {
      67         914 :         return _data_is_stale;
      68             :     }
      69             : 
      70             :     // this supports the NAVl1_* user settable parameters
      71             :     static const struct AP_Param::GroupInfo var_info[];
      72             : 
      73           0 :     void set_reverse(bool reverse) override {
      74           0 :         _reverse = reverse;
      75           0 :     }
      76             : 
      77             : private:
      78             :     // reference to the AHRS object
      79             :     AP_AHRS &_ahrs;
      80             : 
      81             :     // pointer to the SpdHgtControl object
      82             :     const AP_TECS *_tecs;
      83             : 
      84             :     // lateral acceration in m/s required to fly to the
      85             :     // L1 reference point (+ve to right)
      86             :     float _latAccDem;
      87             : 
      88             :     // L1 tracking distance in meters which is dynamically updated
      89             :     float _L1_dist;
      90             : 
      91             :     // Status which is true when the vehicle has started circling the WP
      92             :     bool _WPcircle;
      93             : 
      94             :     // bearing angle (radians) to L1 point
      95             :     float _nav_bearing;
      96             : 
      97             :     // bearing error angle (radians) +ve to left of track
      98             :     float _bearing_error;
      99             : 
     100             :     // crosstrack error in meters
     101             :     float _crosstrack_error;
     102             : 
     103             :     // target bearing in centi-degrees from last update
     104             :     int32_t _target_bearing_cd;
     105             : 
     106             :     // L1 tracking loop period (sec)
     107             :     AP_Float _L1_period;
     108             :     // L1 tracking loop damping ratio
     109             :     AP_Float _L1_damping;
     110             : 
     111             :     // previous value of cross-track velocity
     112             :     float _last_Nu;
     113             : 
     114             :     // prevent indecision in waypoint tracking
     115             :     void _prevent_indecision(float &Nu);
     116             : 
     117             :     // integral feedback to correct crosstrack error. Used to ensure xtrack converges to zero.
     118             :     // For tuning purposes it's helpful to clear the integrator when it changes so a _prev is used
     119             :     float _L1_xtrack_i = 0;
     120             :     AP_Float _L1_xtrack_i_gain;
     121             :     float _L1_xtrack_i_gain_prev = 0;
     122             :     uint32_t _last_update_waypoint_us;
     123             :     bool _data_is_stale = true;
     124             : 
     125             :     AP_Float _loiter_bank_limit;
     126             : 
     127             :     // remember reached_loiter_target decision
     128             :     struct {
     129             :         uint32_t reached_loiter_target_ms;
     130             :         float radius;
     131             :         int8_t direction;
     132             :         Location center_WP;
     133             :     } _last_loiter;
     134             : 
     135             :     bool _reverse = false;
     136             :     float get_yaw() const;
     137             :     int32_t get_yaw_sensor() const;
     138             : };