Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_Common/AP_Common.h>
       4             : #include <AC_PID/AC_PID.h>
       5             : #include <AC_PID/AC_P.h>
       6             : 
       7             : class AR_AttitudeControl {
       8             : public:
       9             : 
      10             :     // constructor
      11             :     AR_AttitudeControl();
      12             : 
      13             :     // do not allow copying
      14             :     CLASS_NO_COPY(AR_AttitudeControl);
      15             : 
      16           0 :     static AR_AttitudeControl *get_singleton() { return _singleton; }
      17             : 
      18             :     //
      19             :     // steering controller
      20             :     //
      21             : 
      22             :     // return a steering servo output given a desired lateral acceleration rate in m/s/s.
      23             :     // positive lateral acceleration is to the right.  dt should normally be the main loop rate.
      24             :     // return value is normally in range -1.0 to +1.0 but can be higher or lower
      25             :     float get_steering_out_lat_accel(float desired_accel, bool motor_limit_left, bool motor_limit_right, float dt);
      26             : 
      27             :     // return a steering servo output given a heading in radians
      28             :     // set rate_max_rads to a non-zero number to apply a limit on the desired turn rate
      29             :     // return value is normally in range -1.0 to +1.0 but can be higher or lower
      30             :     float get_steering_out_heading(float heading_rad, float rate_max_rads, bool motor_limit_left, bool motor_limit_right, float dt);
      31             : 
      32             :     // return a desired turn-rate given a desired heading in radians
      33             :     // normally the results are later passed into get_steering_out_rate
      34             :     float get_turn_rate_from_heading(float heading_rad, float rate_max_rads) const;
      35             : 
      36             :     // return a steering servo output given a desired yaw rate in radians/sec.
      37             :     // positive yaw is to the right
      38             :     // return value is normally in range -1.0 to +1.0 but can be higher or lower
      39             :     // also sets steering_limit_left and steering_limit_right flags
      40             :     float get_steering_out_rate(float desired_rate, bool motor_limit_left, bool motor_limit_right, float dt);
      41             : 
      42             :     // get latest desired turn rate in rad/sec recorded during calls to get_steering_out_rate.  For reporting purposes only
      43             :     float get_desired_turn_rate() const;
      44             : 
      45             :     // get latest desired lateral acceleration in m/s/s recorded during calls to get_steering_out_lat_accel.  For reporting purposes only
      46             :     float get_desired_lat_accel() const;
      47             : 
      48             :     // get actual lateral acceleration in m/s/s.  returns true on success.  For reporting purposes only
      49             :     bool get_lat_accel(float &lat_accel) const;
      50             : 
      51             :     // calculate the turn rate in rad/sec given a lateral acceleration (in m/s/s) and speed (in m/s)
      52             :     float get_turn_rate_from_lat_accel(float lat_accel, float speed) const;
      53             : 
      54             :     // get the lateral acceleration limit (in m/s/s).  Returns at least 0.1G or approximately 1 m/s/s
      55      409476 :     float get_turn_lat_accel_max() const { return MAX(_turn_lateral_G_max, 0.1f) * GRAVITY_MSS; }
      56             : 
      57             :     // returns true if the steering has been limited which can be caused by the physical steering surface
      58             :     // reaching its physical limits (aka motor limits) or acceleration or turn rate limits being applied
      59      178458 :     bool steering_limit_left() const { return _steering_limit_left; }
      60      147958 :     bool steering_limit_right() const { return _steering_limit_right; }
      61             : 
      62             :     //
      63             :     // throttle / speed controller
      64             :     //
      65             : 
      66             :     // set limits used by throttle controller
      67             :     //   forward/back acceleration max in m/s/s
      68             :     //   forward/back deceleartion max in m/s/s
      69             :     void set_throttle_limits(float throttle_accel_max, float throttle_decel_max);
      70             : 
      71             :     // return a throttle output from -1 to +1 given a desired speed in m/s (use negative speeds to travel backwards)
      72             :     //   desired_speed argument should already have been passed through get_desired_speed_accel_limited function
      73             :     //   motor_limit should be true if motors have hit their upper or lower limits
      74             :     //   cruise speed should be in m/s, cruise throttle should be a number from -1 to +1
      75             :     float get_throttle_out_speed(float desired_speed, bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, float dt);
      76             : 
      77             :     // return a throttle output from -1 to +1 to perform a controlled stop.  stopped is set to true once stop has been completed
      78             :     float get_throttle_out_stop(bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, float dt, bool &stopped);
      79             : 
      80             :     // balancebot pitch to throttle controller
      81             :     // returns a throttle output from -1 to +1 given a desired pitch angle (in radians)
      82             :     // pitch_max should be the user defined max pitch angle (in radians)
      83             :     // motor_limit should be true if the motors have hit their upper or lower limit
      84             :     float get_throttle_out_from_pitch(float desired_pitch, float pitch_max, bool motor_limit, float dt);
      85             : 
      86             :     // returns true if the pitch angle has been limited to prevent falling over
      87             :     // pitch limit protection is implemented within get_throttle_out_from_pitch
      88      346875 :     bool pitch_limited() const { return _pitch_limited; }
      89             : 
      90             :     // get latest desired pitch in radians for reporting purposes
      91             :     float get_desired_pitch() const;
      92             : 
      93             :     // Sailboat heel(roll) angle contorller, release sail to keep at maximum heel angle
      94             :     float get_sail_out_from_heel(float desired_heel, float dt);
      95             : 
      96             :     // low level control accessors for reporting and logging
      97             :     AC_P& get_steering_angle_p() { return _steer_angle_p; }
      98      600327 :     AC_PID& get_steering_rate_pid() { return _steer_rate_pid; }
      99       50077 :     AC_PID& get_pitch_to_throttle_pid() { return _pitch_to_throttle_pid; }
     100       20419 :     AC_PID& get_sailboat_heel_pid() { return _sailboat_heel_pid; }
     101      600310 :     const AP_PIDInfo& get_throttle_speed_pid_info() const { return _throttle_speed_pid_info; }
     102             : 
     103             :     // set the PID notch sample rates
     104             :     void set_notch_sample_rate(float sample_rate);
     105             : 
     106             :     // get the slew rate value for speed and steering for oscillation detection in lua scripts
     107             :     void get_srate(float &steering_srate, float &speed_srate);
     108             : 
     109             :     // get forward speed in m/s (earth-frame horizontal velocity but only along vehicle x-axis).  returns true on success
     110             :     bool get_forward_speed(float &speed) const;
     111             : 
     112             :     // get throttle/speed controller maximum acceleration (also used for deceleration)
     113         132 :     float get_accel_max() const { return MAX(_throttle_accel_max, 0.0f); }
     114             : 
     115             :     // get throttle/speed controller maximum deceleration
     116             :     float get_decel_max() const;
     117             : 
     118             :     // check if speed controller active
     119             :     bool speed_control_active() const;
     120             : 
     121             :     // get latest desired speed recorded during call to get_throttle_out_speed.  For reporting purposes only
     122             :     float get_desired_speed() const;
     123             : 
     124             :     // get acceleration limited desired speed
     125             :     float get_desired_speed_accel_limited(float desired_speed, float dt) const;
     126             : 
     127             :     // get minimum stopping distance (in meters) given a speed (in m/s)
     128             :     float get_stopping_distance(float speed) const;
     129             : 
     130             :     // get speed below which vehicle is considered stopped (in m/s)
     131         307 :     float get_stop_speed() const { return MAX(_stop_speed, 0.0f); }
     132             : 
     133             :     // relax I terms of throttle and steering controllers
     134             :     void relax_I();
     135             : 
     136             :     // parameter var table
     137             :     static const struct AP_Param::GroupInfo var_info[];
     138             : 
     139             : private:
     140             : 
     141             :     static AR_AttitudeControl *_singleton;
     142             : 
     143             :     // parameters
     144             :     AC_P     _steer_angle_p;        // steering angle controller
     145             :     AC_PID   _steer_rate_pid;       // steering rate controller
     146             :     AC_PID   _throttle_speed_pid;   // throttle speed controller
     147             :     AC_PID   _pitch_to_throttle_pid;// balancebot pitch controller
     148             :     AP_Float _pitch_to_throttle_ff; // balancebot feed forward from current pitch angle
     149             :     AP_Float _pitch_limit_tc;       // balancebot pitch limit protection time constant
     150             :     AP_Float _pitch_limit_throttle_thresh;  // balancebot pitch limit throttle threshold (in the range 0 to 1.0)
     151             : 
     152             :     AP_Float _throttle_accel_max;   // speed/throttle control acceleration (and deceleration) maximum in m/s/s.  0 to disable limits
     153             :     AP_Float _throttle_decel_max;    // speed/throttle control deceleration maximum in m/s/s. 0 to use ATC_ACCEL_MAX for deceleration
     154             :     AP_Int8  _brake_enable;         // speed control brake enable/disable. if set to 1 a reversed output to the motors to slow the vehicle.
     155             :     AP_Float _stop_speed;           // speed control stop speed.  Motor outputs to zero once vehicle speed falls below this value
     156             :     AP_Float _steer_accel_max;      // steering angle acceleration max in deg/s/s
     157             :     AP_Float _steer_decel_max;      // steering angle deceleration max in deg/s/s
     158             :     AP_Float _steer_rate_max;       // steering rate control maximum rate in deg/s
     159             :     AP_Float _turn_lateral_G_max;   // sterring maximum lateral acceleration limit in 'G'
     160             : 
     161             :     // steering control
     162             :     uint32_t _steer_lat_accel_last_ms;  // system time of last call to lateral acceleration controller (i.e. get_steering_out_lat_accel)
     163             :     uint32_t _steer_turn_last_ms;   // system time of last call to steering rate controller
     164             :     float    _desired_lat_accel;    // desired lateral acceleration (in m/s/s) from latest call to get_steering_out_lat_accel (for reporting purposes)
     165             :     float    _desired_turn_rate;    // desired turn rate (in radians/sec) either from external caller or from lateral acceleration controller
     166             :     bool     _steering_limit_left;  // true when the steering control has reached its left limit (e.g. motor has reached limits or accel or turn rate limits applied)
     167             :     bool     _steering_limit_right; // true when the steering control has reached its right limit (e.g. motor has reached limits or accel or turn rate limits applied)
     168             : 
     169             :     // throttle control
     170             :     uint32_t _speed_last_ms;        // system time of last call to get_throttle_out_speed
     171             :     float    _desired_speed;        // last recorded desired speed
     172             :     uint32_t _stop_last_ms;         // system time the vehicle was at a complete stop
     173             :     bool     _throttle_limit_low;   // throttle output was limited from going too low (used to reduce i-term buildup)
     174             :     bool     _throttle_limit_high;  // throttle output was limited from going too high (used to reduce i-term buildup)
     175             :     AP_PIDInfo _throttle_speed_pid_info;   // local copy of throttle_speed controller's PID info to allow reporting of unusual FF
     176             : 
     177             :     // balancebot pitch control
     178             :     uint32_t _balance_last_ms = 0;  // system time that get_throttle_out_from_pitch was last called
     179             :     float _pitch_limit_low = 0;     // min desired pitch (in radians) used to protect against falling over
     180             :     float _pitch_limit_high = 0;    // max desired pitch (in radians) used to protect against falling over
     181             :     bool _pitch_limited = false;    // true if pitch was limited on last call to get_throttle_out_from_pitch
     182             : 
     183             :     // Sailboat heel control
     184             :     AC_PID   _sailboat_heel_pid;    // Sailboat heel angle pid controller
     185             :     uint32_t _heel_controller_last_ms = 0;
     186             : };