Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : //      Code by Jon Challinger
      16             : //  Modified by Paul Riseborough
      17             : //
      18             : 
      19             : 
      20             : #include "AP_FW_Controller.h"
      21             : #include <AP_AHRS/AP_AHRS.h>
      22             : #include <AP_Scheduler/AP_Scheduler.h>
      23             : #include <GCS_MAVLink/GCS.h>
      24             : #include <AP_Math/AP_Math.h>
      25             : 
      26        1384 : AP_FW_Controller::AP_FW_Controller(const AP_FixedWing &parms, const AC_PID::Defaults &defaults, AP_AutoTune::ATType _autotune_type)
      27        1384 :     : aparm(parms),
      28        1384 :       rate_pid(defaults),
      29        2768 :       autotune_type(_autotune_type)
      30             : {
      31        1384 :     rate_pid.set_slew_limit_scale(45);
      32        1384 : }
      33             : 
      34             : // Return true if input shaping should be used
      35    15559752 : bool AP_FW_Controller::should_apply_input_shaping() const
      36             : {
      37             :     // Must be using rate limits
      38    15559752 :     if (!should_apply_rate_limits()) {
      39      254025 :         return false;
      40             :     }
      41             : 
      42             :     // Accel limit must be set
      43    15305727 :     if (!is_positive(accel_limit.get())) {
      44      762064 :         return false;
      45             :     }
      46             : 
      47             :     // auto-tune must not be running
      48    14543663 :     if ((autotune != nullptr) && autotune->running) {
      49       39490 :         return false;
      50             :     }
      51             : 
      52    14504173 :     return true;
      53             : }
      54             : 
      55             : // Run angle controller
      56    10693292 : float AP_FW_Controller::run_angle_control(int32_t desired_angle_cd, float scaler, bool disable_integrator, bool ground_mode)
      57             : {
      58             :     // Ensure tau is valid
      59    10693292 :     if (gains.tau < 0.05f) {
      60           0 :         gains.tau.set(0.05f);
      61             :     }
      62    10693292 :     const float desired_angle_deg = wrap_180(desired_angle_cd * 0.01);
      63             : 
      64    10693292 :     if (!should_apply_input_shaping()) {
      65             :         // Calculate rate directly from angle error with no input shaping
      66      822909 :         angle_err_deg = wrap_180(desired_angle_deg - get_measured_angle_deg());
      67      822909 :         float desired_rate_degs = angle_err_deg / gains.tau;
      68             : 
      69             :         // Reset input shaping set points
      70      822909 :         reset_input_shaping_deg(desired_angle_deg, desired_rate_degs);
      71             : 
      72             :         // Add coordination offset
      73      822909 :         desired_rate_degs += get_rate_target_offset_degs();
      74             : 
      75             :         // Apply rate limits if enabled
      76      822909 :         if (should_apply_rate_limits()) {
      77      618145 :             desired_rate_degs = rate_limit_degs(desired_rate_degs);
      78             :         }
      79             : 
      80             :         // Run rate controller
      81      822909 :         return run_axis_rate_control(desired_rate_degs, scaler, disable_integrator, ground_mode);
      82             :     }
      83             : 
      84             :     // Apply input shaping to desired angle
      85     9870383 :     const float dt = AP::scheduler().get_loop_period_s();
      86             : 
      87     9870383 :     const float accel_max = accel_limit.get();
      88     9870383 :     const float jerk_limit = accel_max / MAX(gains.tau.get(), 0.1);
      89             : 
      90             :     // Ensure the shortest path is taken
      91     9870383 :     const float angle_error = wrap_180(desired_angle_deg - angle_target_deg);
      92             : 
      93             :     // Apply input shaping updating the accel target
      94    19740766 :     shape_pos_vel_accel(
      95             :         angle_error, 0.0, 0.0, // desired pos, vel and accel
      96     9870383 :         0.0, rate_target_degs, accel_target_degss, // current shaped target
      97     9870383 :         -get_negative_rate_limit_degs(), get_positive_rate_limit_degs(), // velocity limits
      98             :         -accel_max, accel_max, // accel limits
      99             :         jerk_limit, // jerk limit
     100             :         dt, true
     101             :     );
     102             : 
     103             :     // Integrate pos and vel from updated accel target
     104     9870383 :     angle_target_deg += rate_target_degs * dt + accel_target_degss * 0.5 * sq(dt);
     105     9870383 :     rate_target_degs += accel_target_degss * dt;
     106             : 
     107             :     // Make sure target remains in the range +-180
     108     9870383 :     angle_target_deg = wrap_180(angle_target_deg);
     109             : 
     110             :     // Calculate angle error
     111     9870383 :     angle_err_deg = wrap_180(angle_target_deg - get_measured_angle_deg());
     112             : 
     113             :     // Use 1 / tau if angle gain is not set
     114     9870383 :     float angle_gain = 1.0 / gains.tau.get();
     115     9870383 :     if (is_positive(angle_p.get())) {
     116           0 :         angle_gain = angle_p.get();
     117             :     }
     118             : 
     119             :     // Apply gain using sqrt controller
     120     9870383 :     float desired_rate_degs = sqrt_controller(angle_err_deg, angle_gain, accel_max * 0.5, dt);
     121             : 
     122             :     // Add feed forward rate demand and offset then constrain to rate limit
     123     9870383 :     desired_rate_degs = rate_limit_degs(desired_rate_degs + rate_target_degs + get_rate_target_offset_degs());
     124             : 
     125             :     // Run rate controller
     126     9870383 :     return run_axis_rate_control(desired_rate_degs, scaler, disable_integrator, ground_mode);
     127             : }
     128             : 
     129             : /*
     130             :   AC_PID based rate controller
     131             : */
     132    15125334 : float AP_FW_Controller::run_rate_control(float desired_rate_degs, float scaler, bool disable_integrator, bool ground_mode)
     133             : {
     134             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     135             :     // Check that the controller is called once per loop and no more
     136    15125334 :     const uint32_t ticks = AP::scheduler().ticks32();
     137    15125334 :     if (last_run_ticks != 0) {
     138    15124480 :         if (last_run_ticks == ticks) {
     139           0 :             AP_HAL::panic("FW rate control must be run only once per loop");
     140             :         }
     141    15124480 :         if ((last_run_ticks + 1) != ticks) {
     142           0 :             AP_HAL::panic("FW rate control must be run or reset every loop");
     143             :         }
     144             :     }
     145    15125334 :     last_run_ticks = ticks;
     146             : #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
     147             : 
     148    15125334 :     const float dt = AP::scheduler().get_loop_period_s();
     149             : 
     150    15125334 :     const float eas2tas = AP::ahrs().get_EAS2TAS();
     151    15125334 :     bool limit_I = fabsf(_last_out) >= 45;
     152    15125334 :     const float rate_rads = get_measured_rate_rads();
     153    15125334 :     const float old_I = rate_pid.get_i();
     154             : 
     155    15125334 :     const bool underspeed = is_underspeed();
     156    15125334 :     if (underspeed) {
     157     7001082 :         limit_I = true;
     158             :     }
     159             : 
     160             :     // the PID elements are scaled by sq(scaler). To use an
     161             :     // unmodified AC_PID object we scale the inputs (target and measurement)
     162             :     //
     163             :     // note that we run AC_PID in radians so that the normal scaling
     164             :     // range for IMAX in AC_PID applies (usually an IMAX value less than 1.0)
     165    15125334 :     rate_pid.update_all(radians(desired_rate_degs) * scaler * scaler, rate_rads * scaler * scaler, dt, limit_I);
     166             : 
     167    15125334 :     if (underspeed) {
     168             :         // when underspeed we lock the integrator
     169     7001082 :         rate_pid.set_integrator(old_I);
     170             :     }
     171             : 
     172             :     // FF and DFF should be scaled by scaler/eas2tas, but since we have scaled
     173             :     // the AC_PID target above by scaler*scaler we need to instead
     174             :     // divide by scaler*eas2tas to get the right scaling
     175    15125334 :     const float ff = degrees(ff_scale * rate_pid.get_ff_component() / (scaler * eas2tas));
     176    15125334 :     const float dff = degrees(ff_scale * rate_pid.get_dff_component() / (scaler * eas2tas));
     177    15125334 :     ff_scale = 1.0;
     178             : 
     179    15125334 :     if (disable_integrator) {
     180         978 :         rate_pid.reset_I();
     181             :     }
     182             : 
     183             :     // convert AC_PID info object to same scale as old controller
     184    15125334 :     _pid_info = rate_pid.get_pid_info();
     185    15125334 :     auto &pinfo = _pid_info;
     186             : 
     187    15125334 :     const float deg_scale = degrees(1);
     188    15125334 :     pinfo.FF = ff;
     189    15125334 :     pinfo.P *= deg_scale;
     190    15125334 :     pinfo.I *= deg_scale;
     191    15125334 :     pinfo.D *= deg_scale;
     192    15125334 :     pinfo.DFF = dff;
     193             : 
     194             :     // fix the logged target and actual values to not have the scalers applied
     195    15125334 :     pinfo.target = desired_rate_degs;
     196    15125334 :     pinfo.actual = degrees(rate_rads);
     197             : 
     198             :     // sum components
     199    15125334 :     float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D + pinfo.DFF;
     200    15125334 :     if (ground_mode) {
     201             :         // when on ground suppress D and half P term to prevent oscillations
     202     2600584 :         out -= pinfo.D + 0.5*pinfo.P;
     203             :     }
     204             : 
     205             :     // remember the last output to trigger the I limit
     206    15125334 :     _last_out = out;
     207             : 
     208    15125334 :     if (autotune != nullptr && autotune->running && get_airspeed() > aparm.airspeed_min) {
     209             :         // let autotune have a go at the values
     210       37088 :         autotune->update(pinfo, scaler, angle_err_deg);
     211             :     }
     212             : 
     213             :     // output is scaled to notional centidegrees of deflection
     214    15125334 :     return constrain_float(out * 100, -4500, 4500);
     215             : }
     216             : 
     217             : /*
     218             :  Function returns an equivalent control surface deflection in centi-degrees in the range from -4500 to 4500
     219             : */
     220     4432042 : float AP_FW_Controller::run_rate_control(float desired_rate_degs, float scaler)
     221             : {
     222             :     // Zero angle error in pure rate control
     223     4432042 :     angle_err_deg = 0.0;
     224             : 
     225     4432042 :     if (!should_apply_input_shaping()) {
     226             :         // Reset input shaping set points
     227      225410 :         reset_input_shaping_deg(get_measured_angle_deg(), desired_rate_degs);
     228             : 
     229             :         // run rate control with no input shaping
     230      225410 :         return run_rate_control(desired_rate_degs, scaler, false, false);
     231             :     }
     232             : 
     233             :     // Apply input shaping to desired rate
     234     4206632 :     const float dt = AP::scheduler().get_loop_period_s();
     235             : 
     236             :     // Reset the input shaping target angle
     237     4206632 :     angle_target_deg = get_measured_angle_deg();
     238             : 
     239     4206632 :     const float accel_max = accel_limit.get();
     240     4206632 :     const float jerk_limit = accel_max / MAX(gains.tau.get(), 0.1);
     241             : 
     242             :     // Apply input shaping updating the accel target
     243     8413264 :     shape_pos_vel_accel(
     244             :         0.0, desired_rate_degs, 0.0, // desired pos, vel and accel
     245     4206632 :         0.0, rate_target_degs, accel_target_degss, // current shaped target
     246     4206632 :         -get_negative_rate_limit_degs(), get_positive_rate_limit_degs(), // velocity limits
     247             :         -accel_max, accel_max, // accel limits
     248             :         jerk_limit, // jerk limit
     249             :         dt, true
     250             :     );
     251             : 
     252     4206632 :     rate_target_degs += accel_target_degss * dt;
     253             : 
     254             :     // Run rate controller
     255     4206632 :     return run_rate_control(rate_target_degs, scaler, false, false);
     256             : }
     257             : 
     258             : // Reset I term
     259     4025984 : void AP_FW_Controller::reset_I()
     260             : {
     261     4025984 :     rate_pid.reset_I();
     262     4025984 :     _last_out = 0.0;
     263     4025984 : }
     264             : 
     265             : /*
     266             :     reduce the integrator, used when we have a low scale factor in a quadplane hover
     267             : */
     268     4526724 : void AP_FW_Controller::decay_I()
     269             : {
     270             :     // this reduces integrator by 95% over 2s
     271     4526724 :     _pid_info.I *= 0.995f;
     272     4526724 :     rate_pid.set_integrator(rate_pid.get_i() * 0.995);
     273     4526724 : }
     274             : 
     275             : /*
     276             :   restore autotune gains
     277             :  */
     278        3848 : void AP_FW_Controller::autotune_restore(void)
     279             : {
     280        3848 :     if (autotune != nullptr) {
     281          64 :         autotune->stop();
     282             :     }
     283        3848 : }
     284             : 
     285             : /*
     286             :   start an autotune
     287             :  */
     288           8 : void AP_FW_Controller::autotune_start(void)
     289             : {
     290           8 :     if (autotune == nullptr) {
     291           6 :         autotune = NEW_NOTHROW AP_AutoTune(gains, autotune_type, aparm, rate_pid);
     292           6 :         if (autotune == nullptr) {
     293           0 :             if (!failed_autotune_alloc) {
     294           0 :                 GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AutoTune: failed %s allocation", AP_AutoTune::axis_string(autotune_type));
     295             :             }
     296           0 :             failed_autotune_alloc = true;
     297             :         }
     298             :     }
     299           8 :     if (autotune != nullptr) {
     300           8 :         autotune->start();
     301             :     }
     302           8 : }
     303             : 
     304             : // Return the airspeed in m/s
     305    20412851 : float AP_FW_Controller::get_airspeed() const
     306             : {
     307             :     float aspeed;
     308    20412851 :     if (!AP::ahrs().airspeed_EAS(aspeed)) {
     309             :         // If no airspeed available use average of min and max
     310      101024 :         aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_max));
     311             :     }
     312    20412851 :     return aspeed;
     313             : }
     314             : 
     315             : // Reset controller
     316     1628664 : void AP_FW_Controller::reset()
     317             : {
     318             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     319             :     // Reset tick tracking
     320     1628664 :     last_run_ticks = 0;
     321             : #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
     322             : 
     323             :     // Reset PID
     324     1628664 :     rate_pid.reset_I();
     325     1628664 :     rate_pid.reset_filter();
     326     1628664 :     _last_out = 0.0;
     327             : 
     328             :     // Reset input shaping
     329     1628664 :     reset_input_shaping_deg(get_measured_angle_deg(), degrees(get_measured_rate_rads()));
     330     1628664 : }
     331             : 
     332             : // Apply positive and negative rate limits to passed in value
     333    13165511 : float AP_FW_Controller::rate_limit_degs(float rate_degs) const
     334             : {
     335    13165511 :     const float pos_rate_limit = get_positive_rate_limit_degs();
     336    13165511 :     if (is_positive(pos_rate_limit)) {
     337     4073984 :         rate_degs = MIN(rate_degs, pos_rate_limit);
     338             :     }
     339             : 
     340    13165511 :     const float neg_rate_limit = get_negative_rate_limit_degs();
     341    13165511 :     if (is_positive(neg_rate_limit)) {
     342     4073984 :         rate_degs = MAX(rate_degs, -neg_rate_limit);
     343             :     }
     344             : 
     345    13165511 :     return rate_degs;
     346             : }
     347             : 
     348             : // Reset input shaping applying rate limits
     349     2676983 : void AP_FW_Controller::reset_input_shaping_deg(const float angle_deg, const float rate_degs)
     350             : {
     351             :     // No angle limits at the controller level, reset to the passed in angle
     352     2676983 :     angle_target_deg = angle_deg;
     353             : 
     354             :     // Reset to passed in rate and apply rate limits
     355     2676983 :     rate_target_degs = rate_limit_degs(rate_degs);
     356             : 
     357             :     // Reset accel to zero
     358     2676983 :     accel_target_degss = 0.0;
     359     2676983 : }
     360             : 
     361             : // Get input shaping angle, rate and accel for logging
     362     2033082 : void AP_FW_Controller::get_input_shaping(float &angle_deg, float &rate_degs, float &accel_degss) const
     363             : {
     364     2033082 :     angle_deg = angle_target_deg;
     365     2033082 :     rate_degs = rate_target_degs;
     366     2033082 :     accel_degss = accel_target_degss;
     367     2033082 : }
     368             : 
     369             : // Reset the attitude target to such that a change in attitude due to an ahrs change is smooth
     370         848 : void AP_FW_Controller::ahrs_reset()
     371             : {
     372             :     // Update the target angle such that the angle error remains the same before and after the change in measured angle from the ahrs
     373         848 :     angle_target_deg = wrap_180(get_measured_angle_deg() + angle_err_deg);
     374         848 : }
     375             : 
     376             : // Get angle P gain
     377      434418 : float AP_FW_Controller::get_angle_p() const
     378             : {
     379      434418 :     if (should_apply_input_shaping() && is_positive(angle_p.get())) {
     380             :         // Angle gain is in use and configured
     381           0 :         return angle_p.get();
     382             :     }
     383             : 
     384             :     // angle gain = 1 / tau
     385      434418 :     return 1.0 / gains.tau.get();
     386             : }