Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_Common/AP_Common.h>
       4             : #include <AC_PID/AP_PIDInfo.h>
       5             : 
       6             : class AP_SteerController {
       7             : public:
       8         692 :     AP_SteerController()
       9         692 :     {
      10         692 :         AP_Param::setup_object_defaults(this, var_info);
      11         692 :     }
      12             : 
      13             :     /* Do not allow copies */
      14             :     CLASS_NO_COPY(AP_SteerController);
      15             : 
      16             :     /*
      17             :       return a steering servo output from -4500 to 4500 given a
      18             :       desired lateral acceleration rate in m/s/s. Positive lateral
      19             :       acceleration is to the right.
      20             :      */
      21             :         int32_t get_steering_out_lat_accel(float desired_accel);
      22             : 
      23             :     /*
      24             :       return a steering servo output from -4500 to 4500 given a
      25             :       desired yaw rate in degrees/sec. Positive yaw is to the right.
      26             :      */
      27             :         int32_t get_steering_out_rate(float desired_rate);
      28             : 
      29             :     /*
      30             :       return a steering servo output from -4500 to 4500 given a
      31             :       yaw error in centi-degrees
      32             :      */
      33             :         int32_t get_steering_out_angle_error(int32_t angle_err);
      34             : 
      35             :     /*
      36             :       return the steering radius (half diameter). Assumed to be half
      37             :       the P value.
      38             :      */
      39             :     float get_turn_radius(void) const { return _K_P * 0.5f; }
      40             : 
      41             :         void reset_I();
      42             : 
      43             :         static const struct AP_Param::GroupInfo var_info[];
      44             : 
      45       60722 :     const class AP_PIDInfo& get_pid_info(void) const { return _pid_info; }
      46             : 
      47             :     void set_reverse(bool reverse) {
      48             :         _reverse = reverse;
      49             :     }
      50             : 
      51             :     // Returns true if controller has been run recently
      52             :     bool active() const;
      53             : 
      54             : private:
      55             :     AP_Float _tau;
      56             :         AP_Float _K_FF;
      57             :         AP_Float _K_P;
      58             :         AP_Float _K_I;
      59             :         AP_Float _K_D;
      60             :         AP_Float _minspeed;
      61             :     AP_Int16  _imax;
      62             :         uint32_t _last_t;
      63             :         float _last_out;
      64             : 
      65             :         AP_Float _deratespeed;
      66             :         AP_Float _deratefactor;
      67             :         AP_Float _mindegree;
      68             : 
      69             :     AP_PIDInfo _pid_info {};
      70             : 
      71             :     bool _reverse;
      72             : };