Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_Common/AP_Common.h>
       4             : #include <APM_Control/AR_AttitudeControl.h>
       5             : #include <AC_PID/AC_P_2D.h>            // P library (2-axis)
       6             : #include <AC_PID/AC_PID_2D.h>          // PID library (2-axis)
       7             : 
       8             : class AR_PosControl {
       9             : public:
      10             : 
      11             :     // constructor
      12             :     AR_PosControl(AR_AttitudeControl& atc);
      13             : 
      14             :     // do not allow copying
      15             :     CLASS_NO_COPY(AR_PosControl);
      16             : 
      17             :     static AR_PosControl *get_singleton() { return _singleton; }
      18             : 
      19             :     // update navigation
      20             :     void update(float dt);
      21             : 
      22             :     // true if update has been called recently
      23             :     bool is_active() const;
      24             : 
      25             :     // set speed, acceleration and jerk limits
      26             :     void set_limits(float speed_max, float accel_max, float lat_accel_max, float jerk_max);
      27             : 
      28             :     // setter to allow vehicle code to provide turn related param values to this library (should be updated regularly)
      29             :     void set_turn_params(float turn_radius, bool pivot_possible);
      30             : 
      31             :     // set reversed
      32      170374 :     void set_reversed(bool reversed) { _reversed = reversed; }
      33             : 
      34             :     // accessor for _reversed
      35        5346 :     bool get_reversed() { return _reversed; }
      36             : 
      37             :     // get limits
      38      349594 :     float get_speed_max() const { return _speed_max; }
      39      138417 :     float get_accel_max() const { return _accel_max; }
      40      130895 :     float get_lat_accel_max() const { return _lat_accel_max; }
      41      261997 :     float get_jerk_max() const { return _jerk_max; }
      42             : 
      43             :     // initialise the position controller to the current position, velocity, acceleration and attitude
      44             :     // this should be called before the input shaping methods are used
      45             :     // return true on success, false if targets cannot be initialised
      46             :     bool init();
      47             : 
      48             :     // adjust position, velocity and acceleration targets smoothly using input shaping
      49             :     // pos is the target position as an offset from the EKF origin (in meters)
      50             :     // vel is the target velocity in m/s. accel is the target acceleration in m/s/s
      51             :     // dt should be the update rate in seconds
      52             :     // init should be called once before starting to use these methods
      53             :     void input_pos_target(const Vector2p &pos, float dt);
      54             :     void input_pos_vel_target(const Vector2p &pos, const Vector2f &vel, float dt);
      55             :     void input_pos_vel_accel_target(const Vector2p &pos, const Vector2f &vel, const Vector2f &accel, float dt);
      56             : 
      57             :     // set target position, desired velocity and acceleration.  These should be from an externally created path and are not "input shaped"
      58             :     void set_pos_vel_accel_target(const Vector2p &pos, const Vector2f &vel, const Vector2f &accel);
      59             : 
      60             :     // get outputs for forward-back speed (in m/s), lateral speed (in m/s) and turn rate (in rad/sec)
      61      179080 :     float get_desired_speed() const { return _desired_speed; }
      62      179080 :     float get_desired_turn_rate_rads() const { return _desired_turn_rate_rads; }
      63      170767 :     float get_desired_lat_accel() const { return _desired_lat_accel; }
      64             : 
      65             :     // get position target
      66       75253 :     const Vector2p& get_pos_target() const { return _pos_target; }
      67             : 
      68             :     // returns desired velocity vector (i.e. feed forward) in m/s in NE frame
      69             :     Vector2f get_desired_velocity() const;
      70             : 
      71             :     // return desired acceleration vector in m/s in NE frame
      72             :     Vector2f get_desired_accel() const;
      73             : 
      74             :     /// get position error as a vector from the current position to the target position
      75             :     Vector2p get_pos_error() const;
      76             : 
      77             :     // get pid controllers
      78      130659 :     AC_P_2D& get_pos_p() { return _p_pos; }
      79       71522 :     AC_PID_2D& get_vel_pid() { return _pid_vel; }
      80             : 
      81             :     // write PSC logs
      82             :     void write_log();
      83             : 
      84             :     // parameter var table
      85             :     static const struct AP_Param::GroupInfo var_info[];
      86             : 
      87             : private:
      88             : 
      89             :     static AR_PosControl *_singleton;
      90             : 
      91             :     // initialise and check for ekf position resets
      92             :     void init_ekf_xy_reset();
      93             :     void handle_ekf_xy_reset();
      94             : 
      95             :     // references
      96             :     AR_AttitudeControl &_atc;       // rover attitude control library
      97             : 
      98             :     // parameters
      99             :     AC_P_2D   _p_pos;               // position P controller to convert distance error to desired velocity
     100             :     AC_PID_2D _pid_vel;             // velocity PID controller to convert velocity error to desired acceleration
     101             : 
     102             :     // limits
     103             :     float _speed_max;               // maximum forward speed in m/s
     104             :     float _accel_max;               // maximum forward/back acceleration in m/s/s
     105             :     float _lat_accel_max;           // lateral acceleration maximum in m/s/s
     106             :     float _jerk_max;                // maximum jerk in m/s/s/s (used for both forward and lateral input shaping)
     107             :     float _turn_radius;             // vehicle turn radius in meters
     108             : 
     109             :     // position and velocity targets
     110             :     Vector2p _pos_target;           // position target as an offset (in meters) from the EKF origin
     111             :     Vector2f _vel_desired;          // desired velocity in m/s in NE frame.  This is the "feed forward" provided by SCurves
     112             :     Vector2f _vel_target;           // velocity target in m/s in NE frame
     113             :     Vector2f _accel_desired;        // desired accel in m/s/s in NE frame.  This is the "feed forward" provided by SCurves
     114             :     Vector2f _accel_target;         // accel target in m/s/s in NE frame
     115             :     bool _pos_target_valid;         // true if _pos_target is valid
     116             :     bool _vel_desired_valid;        // true if _vel_desired is valid
     117             :     bool _accel_desired_valid;      // true if _accel_desired is valid
     118             : 
     119             :     // variables for navigation
     120             :     uint32_t _last_update_ms;       // system time of last call to update
     121             :     bool _reversed;                 // true if vehicle should move in reverse towards target
     122             : 
     123             :     // main outputs
     124             :     float _desired_speed;           // desired forward_back speed in m/s
     125             :     float _desired_turn_rate_rads;  // desired turn-rate in rad/sec (negative is counter clockwise, positive is clockwise)
     126             :     float _desired_lat_accel;       // desired lateral acceleration (for reporting only)
     127             : 
     128             :     // ekf reset handling
     129             :     uint16_t _ekf_xy_reset_count;      // count of ekf xy position resets
     130             : };