Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_Common/AP_Common.h>
       4             : #include "AP_AutoTune.h"
       5             : #include <AC_PID/AC_PID.h>
       6             : 
       7             : class AP_FW_Controller
       8             : {
       9             : public:
      10             :     AP_FW_Controller(const AP_FixedWing &parms, const AC_PID::Defaults &defaults, AP_AutoTune::ATType _autotune_type);
      11             : 
      12             :     /* Do not allow copies */
      13             :     CLASS_NO_COPY(AP_FW_Controller);
      14             : 
      15             :     // Run angle controller
      16             :     float run_angle_control(int32_t desired_angle_cd, float scaler, bool disable_integrator, bool ground_mode);
      17             : 
      18             :     // Run pure rate control
      19             :     float run_rate_control(float desired_rate_degs, float scaler);
      20             : 
      21             :     // setup a one loop FF scale multiplier. This replaces any previous scale applied
      22             :     // so should only be used when only one source of scaling is needed
      23           0 :     void set_ff_scale(float _ff_scale) { ff_scale = _ff_scale; }
      24             : 
      25             :     // Reset I term
      26             :     void reset_I();
      27             : 
      28             :     // Reset controller
      29             :     void reset();
      30             : 
      31             :     /*
      32             :       reduce the integrator, used when we have a low scale factor in a quadplane hover
      33             :     */
      34             :     void decay_I();
      35             : 
      36             :     void autotune_start(void);
      37             :     void autotune_restore(void);
      38             : 
      39     2861199 :     const AP_PIDInfo& get_pid_info(void) const
      40             :     {
      41     2861199 :         return _pid_info;
      42             :     }
      43             : 
      44             :     // set the PID notch sample rates
      45      108714 :     void set_notch_sample_rate(float sample_rate) { rate_pid.set_notch_sample_rate(sample_rate); }
      46             : 
      47           0 :     AP_Float &kP(void) { return rate_pid.kP(); }
      48           0 :     AP_Float &kI(void) { return rate_pid.kI(); }
      49           0 :     AP_Float &kD(void) { return rate_pid.kD(); }
      50           0 :     AP_Float &kFF(void) { return rate_pid.ff(); }
      51           0 :     AP_Float &tau(void) { return gains.tau; }
      52             : 
      53             :     // Get input shaping angle, rate, and accel for logging
      54             :     void get_input_shaping(float &angle_deg, float &rate_degs, float &accel_degss) const;
      55             : 
      56             :     // Get the angle error in degrees
      57     7296677 :     float get_angle_error_deg() const { return angle_err_deg; }
      58             : 
      59             :     // Reset the attitude target to such that a change in attitude due to an ahrs change is smooth
      60             :     void ahrs_reset();
      61             : 
      62             :     // Get angle P gain
      63             :     float get_angle_p() const;
      64             : 
      65             : protected:
      66             :     const AP_FixedWing &aparm;
      67             :     AP_AutoTune::ATGains gains;
      68             :     AP_AutoTune *autotune;
      69             :     bool failed_autotune_alloc;
      70             :     float _last_out;
      71             :     AC_PID rate_pid;
      72             :     float angle_err_deg;
      73             :     float ff_scale = 1.0;
      74             : 
      75             :     AP_PIDInfo _pid_info;
      76             : 
      77             :     virtual float run_axis_rate_control(float desired_rate_degs, float scaler, bool disable_integrator, bool ground_mode) = 0;
      78             : 
      79             :     float run_rate_control(float desired_rate_degs, float scaler, bool disable_integrator, bool ground_mode);
      80             : 
      81             :     // Return true if the airspeed should be considered as under speed
      82             :     virtual bool is_underspeed() const = 0;
      83             : 
      84             :     // Return the airspeed in m/s
      85             :     float get_airspeed() const;
      86             : 
      87             :     // Return the measured angle in degrees
      88             :     virtual float get_measured_angle_deg() const = 0;
      89             : 
      90             :     // Return the measured rate in radians per second
      91             :     virtual float get_measured_rate_rads() const = 0;
      92             : 
      93             :     // Return true if input shaping should be used
      94             :     bool should_apply_input_shaping() const;
      95             : 
      96             :     // Return true if rate limits should be applied
      97             :     virtual bool should_apply_rate_limits() const = 0;
      98             : 
      99             :     // Apply positive and negative rate limits to passed in value
     100             :     float rate_limit_degs(float rate_degs) const;
     101             : 
     102             :     // Return rate target offset in deg per second, this is used in angle control
     103     5347552 :     virtual float get_rate_target_offset_degs() const { return 0.0; }
     104             : 
     105             :     // Return positive rate limit in deg per second, zero if disabled
     106             :     virtual float get_positive_rate_limit_degs() const = 0;
     107             : 
     108             :     // Return negative rate limit in deg per second, zero if disabled
     109             :     virtual float get_negative_rate_limit_degs() const = 0;
     110             : 
     111             :     // Reset input shaping to the given values
     112             :     void reset_input_shaping_deg(const float angle_deg, const float rate_degs);
     113             : 
     114             :     // Set point tracking for input shaping
     115             :     float angle_target_deg;
     116             :     float rate_target_degs;
     117             :     float accel_target_degss;
     118             : 
     119             :     // Input shaping accel limit and angle P gain
     120             :     AP_Float accel_limit;
     121             :     AP_Float angle_p;
     122             : 
     123             :     const AP_AutoTune::ATType autotune_type;
     124             : 
     125             : private:
     126             : 
     127             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     128             :     // Ticks tracking to check that the controller is called once per loop and no more
     129             :     uint32_t last_run_ticks;
     130             : #endif
     131             : };