2013-08-29 02:34:34 -03:00
/*
This program is free software : you can redistribute it and / or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation , either version 3 of the License , or
( at your option ) any later version .
2021-11-25 01:23:42 -04:00
2013-08-29 02:34:34 -03:00
This program is distributed in the hope that it will be useful ,
but WITHOUT ANY WARRANTY ; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
GNU General Public License for more details .
2021-11-25 01:23:42 -04:00
2013-08-29 02:34:34 -03:00
You should have received a copy of the GNU General Public License
along with this program . If not , see < http : //www.gnu.org/licenses/>.
*/
2012-08-21 23:08:14 -03:00
// Code by Jon Challinger
2013-04-23 08:02:18 -03:00
// Modified by Paul Riseborough
2012-08-21 23:08:14 -03:00
//
2015-08-11 03:28:41 -03:00
# include <AP_HAL/AP_HAL.h>
2012-08-21 23:08:14 -03:00
# include "AP_RollController.h"
2021-07-20 08:15:06 -03:00
# include <AP_AHRS/AP_AHRS.h>
2022-09-29 20:10:40 -03:00
# include <AP_Scheduler/AP_Scheduler.h>
# include <GCS_MAVLink/GCS.h>
2012-08-21 23:08:14 -03:00
2012-10-12 14:53:42 -03:00
extern const AP_HAL : : HAL & hal ;
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo AP_RollController : : var_info [ ] = {
2020-09-18 21:29:41 -03:00
// @Param: 2SRV_TCONST
2021-11-25 01:23:42 -04:00
// @DisplayName: Roll Time Constant
// @Description: Time constant in seconds from demanded to achieved roll angle. Most models respond well to 0.5. May be reduced for faster responses, but setting lower than a model can achieve will not help.
// @Range: 0.4 1.0
// @Units: s
// @Increment: 0.1
// @User: Advanced
2020-09-18 21:29:41 -03:00
AP_GROUPINFO ( " 2SRV_TCONST " , 0 , AP_RollController , gains . tau , 0.5f ) ,
2013-05-05 03:25:17 -03:00
2021-02-27 05:56:41 -04:00
// index 1 to 3 reserved for old PID values
2013-05-05 03:25:17 -03:00
2020-09-18 21:29:41 -03:00
// @Param: 2SRV_RMAX
2021-11-25 01:23:42 -04:00
// @DisplayName: Maximum Roll Rate
2022-01-24 13:06:20 -04:00
// @Description: This sets the maximum roll rate that the attitude controller will demand (degrees/sec) in angle stabilized modes. Setting it to zero disables this limit.
2021-11-25 01:23:42 -04:00
// @Range: 0 180
// @Units: deg/s
// @Increment: 1
// @User: Advanced
2021-03-31 20:42:19 -03:00
AP_GROUPINFO ( " 2SRV_RMAX " , 4 , AP_RollController , gains . rmax_pos , 0 ) ,
2013-05-05 03:25:17 -03:00
2021-03-31 20:42:19 -03:00
// index 5, 6 reserved for old IMAX, FF
2020-05-10 04:11:11 -03:00
2020-09-18 21:29:41 -03:00
// @Param: _RATE_P
// @DisplayName: Roll axis rate controller P gain
2022-11-20 20:45:06 -04:00
// @Description: Roll axis rate controller P gain. Corrects in proportion to the difference between the desired roll rate vs actual roll rate
2020-09-18 21:29:41 -03:00
// @Range: 0.08 0.35
// @Increment: 0.005
// @User: Standard
// @Param: _RATE_I
// @DisplayName: Roll axis rate controller I gain
// @Description: Roll axis rate controller I gain. Corrects long-term difference in desired roll rate vs actual roll rate
// @Range: 0.01 0.6
// @Increment: 0.01
// @User: Standard
// @Param: _RATE_IMAX
// @DisplayName: Roll axis rate controller I gain maximum
2022-11-20 20:45:06 -04:00
// @Description: Roll axis rate controller I gain maximum. Constrains the maximum that the I term will output
2020-09-18 21:29:41 -03:00
// @Range: 0 1
// @Increment: 0.01
// @User: Standard
// @Param: _RATE_D
// @DisplayName: Roll axis rate controller D gain
// @Description: Roll axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate
// @Range: 0.001 0.03
// @Increment: 0.001
// @User: Standard
// @Param: _RATE_FF
// @DisplayName: Roll axis rate controller feed forward
// @Description: Roll axis rate controller feed forward
// @Range: 0 3.0
// @Increment: 0.001
// @User: Standard
// @Param: _RATE_FLTT
// @DisplayName: Roll axis rate controller target frequency in Hz
// @Description: Roll axis rate controller target frequency in Hz
// @Range: 2 50
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: _RATE_FLTE
// @DisplayName: Roll axis rate controller error frequency in Hz
// @Description: Roll axis rate controller error frequency in Hz
// @Range: 2 50
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: _RATE_FLTD
// @DisplayName: Roll axis rate controller derivative frequency in Hz
// @Description: Roll axis rate controller derivative frequency in Hz
// @Range: 0 50
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: _RATE_SMAX
// @DisplayName: Roll slew rate limit
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Range: 0 200
// @Increment: 0.5
// @User: Advanced
AP_SUBGROUPINFO ( rate_pid , " _RATE_ " , 9 , AP_RollController , AC_PID ) ,
2021-11-25 01:23:42 -04:00
2020-05-10 04:11:11 -03:00
AP_GROUPEND
2012-08-21 23:08:14 -03:00
} ;
2021-03-31 20:42:19 -03:00
// constructor
2022-09-29 20:10:40 -03:00
AP_RollController : : AP_RollController ( const AP_FixedWing & parms )
2021-11-25 01:23:42 -04:00
: aparm ( parms )
2021-03-31 20:42:19 -03:00
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
rate_pid . set_slew_limit_scale ( 45 ) ;
}
2013-07-10 10:25:06 -03:00
2020-09-18 21:29:41 -03:00
/*
AC_PID based rate controller
*/
2021-10-29 07:37:15 -03:00
float AP_RollController : : _get_rate_out ( float desired_rate , float scaler , bool disable_integrator , bool ground_mode )
2020-09-18 21:29:41 -03:00
{
2021-07-20 08:15:06 -03:00
const AP_AHRS & _ahrs = AP : : ahrs ( ) ;
2020-09-18 21:29:41 -03:00
const float dt = AP : : scheduler ( ) . get_loop_period_s ( ) ;
const float eas2tas = _ahrs . get_EAS2TAS ( ) ;
2021-03-31 20:42:19 -03:00
bool limit_I = fabsf ( _last_out ) > = 45 ;
2020-09-18 21:29:41 -03:00
float rate_x = _ahrs . get_gyro ( ) . x ;
float aspeed ;
float old_I = rate_pid . get_i ( ) ;
rate_pid . set_dt ( dt ) ;
if ( ! _ahrs . airspeed_estimate ( aspeed ) ) {
aspeed = 0 ;
}
bool underspeed = aspeed < = float ( aparm . airspeed_min ) ;
if ( underspeed ) {
limit_I = true ;
}
// the P and I elements are scaled by sq(scaler). To use an
// unmodified AC_PID object we scale the inputs and calculate FF separately
//
// note that we run AC_PID in radians so that the normal scaling
// range for IMAX in AC_PID applies (usually an IMAX value less than 1.0)
rate_pid . update_all ( radians ( desired_rate ) * scaler * scaler , rate_x * scaler * scaler , limit_I ) ;
if ( underspeed ) {
// when underspeed we lock the integrator
rate_pid . set_integrator ( old_I ) ;
}
2021-11-25 01:23:42 -04:00
2020-09-18 21:29:41 -03:00
// FF should be scaled by scaler/eas2tas, but since we have scaled
// the AC_PID target above by scaler*scaler we need to instead
// divide by scaler*eas2tas to get the right scaling
const float ff = degrees ( rate_pid . get_ff ( ) / ( scaler * eas2tas ) ) ;
if ( disable_integrator ) {
rate_pid . reset_I ( ) ;
}
// convert AC_PID info object to same scale as old controller
2021-02-27 05:56:41 -04:00
_pid_info = rate_pid . get_pid_info ( ) ;
auto & pinfo = _pid_info ;
2020-09-18 21:29:41 -03:00
const float deg_scale = degrees ( 1 ) ;
pinfo . FF = ff ;
pinfo . P * = deg_scale ;
pinfo . I * = deg_scale ;
pinfo . D * = deg_scale ;
// fix the logged target and actual values to not have the scalers applied
pinfo . target = desired_rate ;
pinfo . actual = degrees ( rate_x ) ;
// sum components
float out = pinfo . FF + pinfo . P + pinfo . I + pinfo . D ;
2021-10-29 07:37:15 -03:00
if ( ground_mode ) {
// when on ground suppress D term to prevent oscillations
out - = pinfo . D + 0.5 * pinfo . P ;
}
2020-09-18 21:29:41 -03:00
// remember the last output to trigger the I limit
2021-03-31 20:42:19 -03:00
_last_out = out ;
2020-09-18 21:29:41 -03:00
2021-03-31 20:42:19 -03:00
if ( autotune ! = nullptr & & autotune - > running & & aspeed > aparm . airspeed_min ) {
2021-11-25 01:23:42 -04:00
// let autotune have a go at the values
2021-04-12 23:37:37 -03:00
autotune - > update ( pinfo , scaler , angle_err_deg ) ;
2021-03-31 20:42:19 -03:00
}
2021-11-25 01:23:42 -04:00
2020-09-18 21:29:41 -03:00
// output is scaled to notional centidegrees of deflection
2021-09-18 14:58:08 -03:00
return constrain_float ( out * 100 , - 4500 , 4500 ) ;
2020-09-18 21:29:41 -03:00
}
2013-07-10 10:25:06 -03:00
/*
Function returns an equivalent elevator deflection in centi - degrees in the range from - 4500 to 4500
A positive demand is up
2021-11-25 01:23:42 -04:00
Inputs are :
2013-07-10 10:25:06 -03:00
1 ) desired roll rate in degrees / sec
2 ) control gain scaler = scaling_speed / aspeed
*/
2021-09-18 14:58:08 -03:00
float AP_RollController : : get_rate_out ( float desired_rate , float scaler )
2013-07-10 10:25:06 -03:00
{
2021-10-29 07:37:15 -03:00
return _get_rate_out ( desired_rate , scaler , false , false ) ;
2013-07-10 10:25:06 -03:00
}
/*
2018-05-29 05:01:44 -03:00
Function returns an equivalent aileron deflection in centi - degrees in the range from - 4500 to 4500
2013-07-10 10:25:06 -03:00
A positive demand is up
2021-11-25 01:23:42 -04:00
Inputs are :
2013-07-10 10:25:06 -03:00
1 ) demanded bank angle in centi - degrees
2 ) control gain scaler = scaling_speed / aspeed
3 ) boolean which is true when stabilise mode is active
4 ) minimum FBW airspeed ( metres / sec )
*/
2021-10-29 07:37:15 -03:00
float AP_RollController : : get_servo_out ( int32_t angle_err , float scaler , bool disable_integrator , bool ground_mode )
2013-07-10 10:25:06 -03:00
{
2021-04-04 21:23:53 -03:00
if ( gains . tau < 0.05f ) {
gains . tau . set ( 0.05f ) ;
2013-07-10 10:25:06 -03:00
}
2021-11-25 01:23:42 -04:00
2021-04-12 23:37:37 -03:00
// Calculate the desired roll rate (deg/sec) from the angle error
angle_err_deg = angle_err * 0.01 ;
float desired_rate = angle_err_deg / gains . tau ;
2013-07-10 10:25:06 -03:00
2019-05-01 08:54:23 -03:00
// Limit the demanded roll rate
2021-03-31 20:42:19 -03:00
if ( gains . rmax_pos & & desired_rate < - gains . rmax_pos ) {
desired_rate = - gains . rmax_pos ;
} else if ( gains . rmax_pos & & desired_rate > gains . rmax_pos ) {
desired_rate = gains . rmax_pos ;
2019-05-01 08:54:23 -03:00
}
2021-10-29 07:37:15 -03:00
return _get_rate_out ( desired_rate , scaler , disable_integrator , ground_mode ) ;
2013-07-10 10:25:06 -03:00
}
2012-08-21 23:08:14 -03:00
void AP_RollController : : reset_I ( )
{
2021-11-25 01:23:42 -04:00
_pid_info . I = 0 ;
2020-09-18 21:29:41 -03:00
rate_pid . reset_I ( ) ;
}
/*
convert from old to new PIDs
this is a temporary conversion function during development
*/
void AP_RollController : : convert_pid ( )
{
2021-02-27 05:56:41 -04:00
AP_Float & ff = rate_pid . ff ( ) ;
2022-05-11 19:04:36 -03:00
if ( ff . configured ( ) ) {
2020-09-18 21:29:41 -03:00
return ;
}
2021-02-27 05:56:41 -04:00
float old_ff = 0 , old_p = 1.0 , old_i = 0.3 , old_d = 0.08 ;
2021-03-31 20:42:19 -03:00
int16_t old_imax = 3000 ;
2021-02-27 05:56:41 -04:00
bool have_old = AP_Param : : get_param_by_index ( this , 1 , AP_PARAM_FLOAT , & old_p ) ;
have_old | = AP_Param : : get_param_by_index ( this , 3 , AP_PARAM_FLOAT , & old_i ) ;
have_old | = AP_Param : : get_param_by_index ( this , 2 , AP_PARAM_FLOAT , & old_d ) ;
have_old | = AP_Param : : get_param_by_index ( this , 6 , AP_PARAM_FLOAT , & old_ff ) ;
2021-03-31 20:42:19 -03:00
have_old | = AP_Param : : get_param_by_index ( this , 5 , AP_PARAM_INT16 , & old_imax ) ;
2021-02-27 05:56:41 -04:00
if ( ! have_old ) {
// none of the old gains were set
2020-09-18 21:29:41 -03:00
return ;
}
2021-02-27 05:56:41 -04:00
const float kp_ff = MAX ( ( old_p - old_i * gains . tau ) * gains . tau - old_d , 0 ) ;
rate_pid . ff ( ) . set_and_save ( old_ff + kp_ff ) ;
rate_pid . kI ( ) . set_and_save_ifchanged ( old_i * gains . tau ) ;
rate_pid . kP ( ) . set_and_save_ifchanged ( old_d ) ;
2020-09-18 21:29:41 -03:00
rate_pid . kD ( ) . set_and_save_ifchanged ( 0 ) ;
2021-03-31 20:42:19 -03:00
rate_pid . kIMAX ( ) . set_and_save_ifchanged ( old_imax / 4500.0 ) ;
}
/*
start an autotune
*/
void AP_RollController : : autotune_start ( void )
{
if ( autotune = = nullptr ) {
autotune = new AP_AutoTune ( gains , AP_AutoTune : : AUTOTUNE_ROLL , aparm , rate_pid ) ;
if ( autotune = = nullptr ) {
if ( ! failed_autotune_alloc ) {
GCS_SEND_TEXT ( MAV_SEVERITY_ERROR , " AutoTune: failed roll allocation " ) ;
}
failed_autotune_alloc = true ;
}
}
if ( autotune ! = nullptr ) {
autotune - > start ( ) ;
}
}
/*
restore autotune gains
*/
void AP_RollController : : autotune_restore ( void )
{
if ( autotune ! = nullptr ) {
autotune - > stop ( ) ;
}
2012-08-21 23:08:14 -03:00
}