Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/APM_Control/AR_AttitudeControl.cpp

854 lines
33 KiB
C++
Raw Normal View History

AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -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.
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.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
APM_Control: stop taking references to ahrs in APM_Control
2021-07-20 08:15:06 -03:00
#include <AP_AHRS/AP_AHRS.h>
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h>
#include "AR_AttitudeControl.h"
APM_Control: Fix AP_GPS include
2019-06-14 00:15:29 -03:00
#include <AP_GPS/AP_GPS.h>
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = {
AR_AttitudeControl: fix parameter docs
2017-12-09 03:48:58 -04:00
// @Param: _STR_RAT_P
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @DisplayName: Steering control rate P gain
// @Description: Steering control rate P gain. Converts the turn rate error (in radians/sec) to a steering control output (in the range -1 to +1)
AR_AttitudeControl: parameter desc range increase new steering controller can use higher FF and zero P
2018-01-10 22:00:34 -04:00
// @Range: 0.000 2.000
AR_AttitudeControl: update param desc increment and range
2020-09-15 22:25:12 -03:00
// @Increment: 0.001
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @User: Standard
AR_AttitudeControl: fix parameter docs
2017-12-09 03:48:58 -04:00
// @Param: _STR_RAT_I
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @DisplayName: Steering control I gain
// @Description: Steering control I gain. Corrects long term error between the desired turn rate (in rad/s) and actual
// @Range: 0.000 2.000
AR_AttitudeControl: update param desc increment and range
2020-09-15 22:25:12 -03:00
// @Increment: 0.001
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @User: Standard
AR_AttitudeControl: fix parameter docs
2017-12-09 03:48:58 -04:00
// @Param: _STR_RAT_IMAX
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @DisplayName: Steering control I gain maximum
AR_AttitudeControl: correct spelling error in param docs
2020-03-15 21:09:06 -03:00
// @Description: Steering control I gain maximum. Constrains the steering output (range -1 to +1) that the I term will generate
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @Range: 0.000 1.000
AR_AttitudeControl: add parameter description increment
2017-12-09 02:20:41 -04:00
// @Increment: 0.01
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @User: Standard
AR_AttitudeControl: fix parameter descriptions
2017-12-11 04:01:18 -04:00
// @Param: _STR_RAT_D
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @DisplayName: Steering control D gain
// @Description: Steering control D gain. Compensates for short-term change in desired turn rate vs actual
// @Range: 0.000 0.400
AR_AttitudeControl: add parameter description increment
2017-12-09 02:20:41 -04:00
// @Increment: 0.001
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @User: Standard
AR_AttitudeControl: fix parameter descriptions
2017-12-11 04:01:18 -04:00
// @Param: _STR_RAT_FF
// @DisplayName: Steering control feed forward
// @Description: Steering control feed forward
AR_AttitudeControl: parameter desc range increase new steering controller can use higher FF and zero P
2018-01-10 22:00:34 -04:00
// @Range: 0.000 3.000
AR_AttitudeControl: fix parameter descriptions
2017-12-11 04:01:18 -04:00
// @Increment: 0.001
// @User: Standard
AR_AttitudeControl: fix parameter docs
2017-12-09 03:48:58 -04:00
// @Param: _STR_RAT_FILT
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @DisplayName: Steering control filter frequency
// @Description: Steering control input filter. Lower values reduce noise but add delay.
AR_AttitudeControl: allow filter to be zero Also increase default filter to 50hz
2018-01-02 02:10:02 -04:00
// @Range: 0.000 100.000
AR_AttitudeControl: add parameter description increment
2017-12-09 02:20:41 -04:00
// @Increment: 0.1
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @Units: Hz
// @User: Standard
APM_Control: AR_AttitudeControl: add description for new PID params
2019-09-20 18:09:47 -03:00
// @Param: _STR_RAT_FLTT
// @DisplayName: Steering control Target filter frequency in Hz
// @Description: Target filter frequency in Hz
AR_AttitudeControl: update param desc increment and range
2020-09-15 22:25:12 -03:00
// @Range: 0.000 100.000
// @Increment: 0.1
APM_Control: AR_AttitudeControl: add description for new PID params
2019-09-20 18:09:47 -03:00
// @Units: Hz
// @User: Standard
// @Param: _STR_RAT_FLTE
// @DisplayName: Steering control Error filter frequency in Hz
// @Description: Error filter frequency in Hz
AR_AttitudeControl: update param desc increment and range
2020-09-15 22:25:12 -03:00
// @Range: 0.000 100.000
// @Increment: 0.1
APM_Control: AR_AttitudeControl: add description for new PID params
2019-09-20 18:09:47 -03:00
// @Units: Hz
// @User: Standard
// @Param: _STR_RAT_FLTD
// @DisplayName: Steering control Derivative term filter frequency in Hz
// @Description: Derivative filter frequency in Hz
AR_AttitudeControl: update param desc increment and range
2020-09-15 22:25:12 -03:00
// @Range: 0.000 100.000
// @Increment: 0.1
APM_Control: AR_AttitudeControl: add description for new PID params
2019-09-20 18:09:47 -03:00
// @Units: Hz
// @User: Standard
APM_Control: added SMAX param docs
2020-09-24 04:26:05 -03:00
// @Param: _STR_RAT_SMAX
// @DisplayName: Steering 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
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
AP_SUBGROUPINFO(_steer_rate_pid, "_STR_RAT_", 1, AR_AttitudeControl, AC_PID),
// @Param: _SPEED_P
// @DisplayName: Speed control P gain
// @Description: Speed control P gain. Converts the error between the desired speed (in m/s) and actual speed to a motor output (in the range -1 to +1)
// @Range: 0.010 2.000
AR_AttitudeControl: add parameter description increment
2017-12-09 02:20:41 -04:00
// @Increment: 0.01
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @User: Standard
// @Param: _SPEED_I
// @DisplayName: Speed control I gain
// @Description: Speed control I gain. Corrects long term error between the desired speed (in m/s) and actual speed
// @Range: 0.000 2.000
AR_AttitudeControl: update param desc increment and range
2020-09-15 22:25:12 -03:00
// @Increment: 0.01
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @User: Standard
// @Param: _SPEED_IMAX
// @DisplayName: Speed control I gain maximum
AR_AttitudeControl: correct spelling error in param docs
2020-03-15 21:09:06 -03:00
// @Description: Speed control I gain maximum. Constrains the maximum motor output (range -1 to +1) that the I term will generate
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @Range: 0.000 1.000
AR_AttitudeControl: add parameter description increment
2017-12-09 02:20:41 -04:00
// @Increment: 0.01
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @User: Standard
// @Param: _SPEED_D
// @DisplayName: Speed control D gain
// @Description: Speed control D gain. Compensates for short-term change in desired speed vs actual
// @Range: 0.000 0.400
AR_AttitudeControl: add parameter description increment
2017-12-09 02:20:41 -04:00
// @Increment: 0.001
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @User: Standard
AR_AttitudeControl: fix parameter descriptions
2017-12-11 04:01:18 -04:00
// @Param: _SPEED_FF
// @DisplayName: Speed control feed forward
// @Description: Speed control feed forward
AR_AttitudeControl: parameter desc range increase new steering controller can use higher FF and zero P
2018-01-10 22:00:34 -04:00
// @Range: 0.000 0.500
AR_AttitudeControl: fix parameter descriptions
2017-12-11 04:01:18 -04:00
// @Increment: 0.001
// @User: Standard
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @Param: _SPEED_FILT
// @DisplayName: Speed control filter frequency
// @Description: Speed control input filter. Lower values reduce noise but add delay.
AR_AttitudeControl: allow filter to be zero Also increase default filter to 50hz
2018-01-02 02:10:02 -04:00
// @Range: 0.000 100.000
AR_AttitudeControl: add parameter description increment
2017-12-09 02:20:41 -04:00
// @Increment: 0.1
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @Units: Hz
// @User: Standard
APM_Control: AR_AttitudeControl: add description for new PID params
2019-09-20 18:09:47 -03:00
// @Param: _SPEED_FLTT
// @DisplayName: Speed control Target filter frequency in Hz
// @Description: Target filter frequency in Hz
AR_AttitudeControl: update param desc increment and range
2020-09-15 22:25:12 -03:00
// @Range: 0.000 100.000
// @Increment: 0.1
APM_Control: AR_AttitudeControl: add description for new PID params
2019-09-20 18:09:47 -03:00
// @Units: Hz
// @User: Standard
// @Param: _SPEED_FLTE
// @DisplayName: Speed control Error filter frequency in Hz
// @Description: Error filter frequency in Hz
AR_AttitudeControl: update param desc increment and range
2020-09-15 22:25:12 -03:00
// @Range: 0.000 100.000
// @Increment: 0.1
APM_Control: AR_AttitudeControl: add description for new PID params
2019-09-20 18:09:47 -03:00
// @Units: Hz
// @User: Standard
// @Param: _SPEED_FLTD
// @DisplayName: Speed control Derivative term filter frequency in Hz
// @Description: Derivative filter frequency in Hz
AR_AttitudeControl: update param desc increment and range
2020-09-15 22:25:12 -03:00
// @Range: 0.000 100.000
// @Increment: 0.1
APM_Control: AR_AttitudeControl: add description for new PID params
2019-09-20 18:09:47 -03:00
// @Units: Hz
// @User: Standard
APM_Control: added SMAX param docs
2020-09-24 04:26:05 -03:00
// @Param: _SPEED_SMAX
// @DisplayName: Speed control 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
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
AP_SUBGROUPINFO(_throttle_speed_pid, "_SPEED_", 2, AR_AttitudeControl, AC_PID),
// @Param: _ACCEL_MAX
// @DisplayName: Speed control acceleration (and deceleration) maximum in m/s/s
// @Description: Speed control acceleration (and deceleration) maximum in m/s/s. 0 to disable acceleration limiting
AR_AttitudeControl: parameter desc range increase new steering controller can use higher FF and zero P
2018-01-10 22:00:34 -04:00
// @Range: 0.0 10.0
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @Increment: 0.1
// @Units: m/s/s
// @User: Standard
AP_GROUPINFO("_ACCEL_MAX", 3, AR_AttitudeControl, _throttle_accel_max, AR_ATTCONTROL_THR_ACCEL_MAX),
// @Param: _BRAKE
// @DisplayName: Speed control brake enable/disable
// @Description: Speed control brake enable/disable. Allows sending a reversed output to the motors to slow the vehicle.
// @Values: 0:Disable,1:Enable
// @User: Standard
APM_Control: BRAKE param default is true
2018-07-18 23:28:27 -03:00
AP_GROUPINFO("_BRAKE", 4, AR_AttitudeControl, _brake_enable, 1),
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @Param: _STOP_SPEED
// @DisplayName: Speed control stop speed
// @Description: Speed control stop speed. Motor outputs to zero once vehicle speed falls below this value
AR_AttitudeControl: parameter desc range increase new steering controller can use higher FF and zero P
2018-01-10 22:00:34 -04:00
// @Range: 0.00 0.50
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// @Increment: 0.01
// @Units: m/s
// @User: Standard
AP_GROUPINFO("_STOP_SPEED", 5, AR_AttitudeControl, _stop_speed, AR_ATTCONTROL_STOP_SPEED_DEFAULT),
APM_Control: Fixed the subparam indexs to start at 1 AFAIK there is still a bug in ArduPilot where subparams should not start at index 0. This is due to the way the index math and bit shifing works as it incorrectly offsets all 0 index params to 0. We allow 2 levels of sub params - 3 levels in total. So params, sub params and sub sub params. The 0 parameter in all those is unfortunately always references param[0]. So param[0] and subparam[0] and subsubparam[0] will reference the same parameter value. Its why we always say start the index from 1 as the math and bitshifting then works correctly. Yeah this is worded badly - hard to explain.
2017-11-27 17:13:53 -04:00
// @Param: _STR_ANG_P
// @DisplayName: Steering control angle P gain
// @Description: Steering control angle P gain. Converts the error between the desired heading/yaw (in radians) and actual heading/yaw to a desired turn rate (in rad/sec)
// @Range: 1.000 10.000
AR_AttitudeControl: add parameter description increment
2017-12-09 02:20:41 -04:00
// @Increment: 0.1
APM_Control: Fixed the subparam indexs to start at 1 AFAIK there is still a bug in ArduPilot where subparams should not start at index 0. This is due to the way the index math and bit shifing works as it incorrectly offsets all 0 index params to 0. We allow 2 levels of sub params - 3 levels in total. So params, sub params and sub sub params. The 0 parameter in all those is unfortunately always references param[0]. So param[0] and subparam[0] and subsubparam[0] will reference the same parameter value. Its why we always say start the index from 1 as the math and bitshifting then works correctly. Yeah this is worded badly - hard to explain.
2017-11-27 17:13:53 -04:00
// @User: Standard
AP_SUBGROUPINFO(_steer_angle_p, "_STR_ANG_", 6, AR_AttitudeControl, AC_P),
AR_AttitudeControl: add angular accel and rate limits This limits the change in desired turn rate to reduce impossible requests which should help avoid overshoot Also add rotation rate limit to turn-rate controller
2017-12-25 00:28:07 -04:00
// @Param: _STR_ACC_MAX
// @DisplayName: Steering control angular acceleration maximum
AR_AttitudeControl: minor param desc fix
2019-04-29 03:31:07 -03:00
// @Description: Steering control angular acceleration maximum (in deg/s/s). 0 to disable acceleration limiting
AR_AttitudeControl: add angular accel and rate limits This limits the change in desired turn rate to reduce impossible requests which should help avoid overshoot Also add rotation rate limit to turn-rate controller
2017-12-25 00:28:07 -04:00
// @Range: 0 1000
// @Increment: 0.1
// @Units: deg/s/s
// @User: Standard
AP_GROUPINFO("_STR_ACC_MAX", 7, AR_AttitudeControl, _steer_accel_max, AR_ATTCONTROL_STEER_ACCEL_MAX),
// @Param: _STR_RAT_MAX
// @DisplayName: Steering control rotation rate maximum
// @Description: Steering control rotation rate maximum in deg/s. 0 to remove rate limiting
// @Range: 0 1000
// @Increment: 0.1
// @Units: deg/s
// @User: Standard
AP_GROUPINFO("_STR_RAT_MAX", 8, AR_AttitudeControl, _steer_rate_max, AR_ATTCONTROL_STEER_RATE_MAX),
AR_AttitudeControl: add separate decel limit
2018-05-18 01:16:42 -03:00
// @Param: _DECEL_MAX
// @DisplayName: Speed control deceleration maximum in m/s/s
AR_AttitudeControl: disable limits when _ACCEL_MAX is zero
2018-06-08 23:04:35 -03:00
// @Description: Speed control and deceleration maximum in m/s/s. 0 to use ATC_ACCEL_MAX for deceleration
AR_AttitudeControl: add separate decel limit
2018-05-18 01:16:42 -03:00
// @Range: 0.0 10.0
// @Increment: 0.1
// @Units: m/s/s
// @User: Standard
AP_GROUPINFO("_DECEL_MAX", 9, AR_AttitudeControl, _throttle_decel_max, 0.00f),
APM_Control: added balancing function for BalanceBot
2018-06-18 08:14:28 -03:00
// @Param: _BAL_P
// @DisplayName: Pitch control P gain
// @Description: Pitch control P gain for BalanceBots. Converts the error between the desired pitch (in radians) and actual pitch to a motor output (in the range -1 to +1)
// @Range: 0.000 2.000
// @Increment: 0.01
// @User: Standard
// @Param: _BAL_I
// @DisplayName: Pitch control I gain
// @Description: Pitch control I gain for BalanceBots. Corrects long term error between the desired pitch (in radians) and actual pitch
// @Range: 0.000 2.000
AR_AttitudeControl: update param desc increment and range
2020-09-15 22:25:12 -03:00
// @Increment: 0.01
APM_Control: added balancing function for BalanceBot
2018-06-18 08:14:28 -03:00
// @User: Standard
// @Param: _BAL_IMAX
// @DisplayName: Pitch control I gain maximum
// @Description: Pitch control I gain maximum. Constrains the maximum motor output (range -1 to +1) that the I term will generate
// @Range: 0.000 1.000
// @Increment: 0.01
// @User: Standard
// @Param: _BAL_D
// @DisplayName: Pitch control D gain
// @Description: Pitch control D gain. Compensates for short-term change in desired pitch vs actual
// @Range: 0.000 0.100
// @Increment: 0.001
// @User: Standard
// @Param: _BAL_FF
// @DisplayName: Pitch control feed forward
// @Description: Pitch control feed forward
// @Range: 0.000 0.500
// @Increment: 0.001
// @User: Standard
// @Param: _BAL_FILT
// @DisplayName: Pitch control filter frequency
// @Description: Pitch control input filter. Lower values reduce noise but add delay.
// @Range: 0.000 100.000
// @Increment: 0.1
// @Units: Hz
// @User: Standard
APM_Control: AR_AttitudeControl: add description for new PID params
2019-09-20 18:09:47 -03:00
// @Param: _BAL_FLTT
// @DisplayName: Pitch control Target filter frequency in Hz
// @Description: Target filter frequency in Hz
AR_AttitudeControl: update param desc increment and range
2020-09-15 22:25:12 -03:00
// @Range: 0.000 100.000
// @Increment: 0.1
APM_Control: AR_AttitudeControl: add description for new PID params
2019-09-20 18:09:47 -03:00
// @Units: Hz
// @User: Standard
// @Param: _BAL_FLTE
// @DisplayName: Pitch control Error filter frequency in Hz
// @Description: Error filter frequency in Hz
AR_AttitudeControl: update param desc increment and range
2020-09-15 22:25:12 -03:00
// @Range: 0.000 100.000
// @Increment: 0.1
APM_Control: AR_AttitudeControl: add description for new PID params
2019-09-20 18:09:47 -03:00
// @Units: Hz
// @User: Standard
// @Param: _BAL_FLTD
// @DisplayName: Pitch control Derivative term filter frequency in Hz
// @Description: Derivative filter frequency in Hz
AR_AttitudeControl: update param desc increment and range
2020-09-15 22:25:12 -03:00
// @Range: 0.000 100.000
// @Increment: 0.1
APM_Control: AR_AttitudeControl: add description for new PID params
2019-09-20 18:09:47 -03:00
// @Units: Hz
// @User: Standard
APM_Control: added SMAX param docs
2020-09-24 04:26:05 -03:00
// @Param: _BAL_SMAX
// @DisplayName: Pitch control 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
APM_Control: added balancing function for BalanceBot
2018-06-18 08:14:28 -03:00
AP_SUBGROUPINFO(_pitch_to_throttle_pid, "_BAL_", 10, AR_AttitudeControl, AC_PID),
AR_AttitudeControl: get_throttle_out_from_pitch accepts vehicle speed
2018-08-08 02:42:09 -03:00
// @Param: _BAL_SPD_FF
// @DisplayName: Pitch control feed forward from speed
// @Description: Pitch control feed forward from speed
// @Range: 0.0 10.0
// @Increment: 0.01
// @User: Standard
AP_GROUPINFO("_BAL_SPD_FF", 11, AR_AttitudeControl, _pitch_to_throttle_speed_ff, AR_ATTCONTROL_BAL_SPEED_FF),
AR_AttitudeControl: add sail heel PID
2018-09-14 03:52:02 -03:00
// @Param: _SAIL_P
// @DisplayName: Sail Heel control P gain
// @Description: Sail Heel control P gain for sailboats. Converts the error between the desired heel angle (in radians) and actual heel to a main sail output (in the range -1 to +1)
// @Range: 0.000 2.000
// @Increment: 0.01
// @User: Standard
// @Param: _SAIL_I
// @DisplayName: Sail Heel control I gain
// @Description: Sail Heel control I gain for sailboats. Corrects long term error between the desired heel angle (in radians) and actual
// @Range: 0.000 2.000
AR_AttitudeControl: update param desc increment and range
2020-09-15 22:25:12 -03:00
// @Increment: 0.01
AR_AttitudeControl: add sail heel PID
2018-09-14 03:52:02 -03:00
// @User: Standard
// @Param: _SAIL_IMAX
// @DisplayName: Sail Heel control I gain maximum
// @Description: Sail Heel control I gain maximum. Constrains the maximum I term contribution to the main sail output (range -1 to +1)
// @Range: 0.000 1.000
// @Increment: 0.01
// @User: Standard
// @Param: _SAIL_D
// @DisplayName: Sail Heel control D gain
// @Description: Sail Heel control D gain. Compensates for short-term change in desired heel angle vs actual
// @Range: 0.000 0.100
// @Increment: 0.001
// @User: Standard
// @Param: _SAIL_FF
// @DisplayName: Sail Heel control feed forward
// @Description: Sail Heel control feed forward
// @Range: 0.000 0.500
// @Increment: 0.001
// @User: Standard
// @Param: _SAIL_FILT
// @DisplayName: Sail Heel control filter frequency
// @Description: Sail Heel control input filter. Lower values reduce noise but add delay.
// @Range: 0.000 100.000
// @Increment: 0.1
// @Units: Hz
// @User: Standard
APM_Control: AR_AttitudeControl: add description for new PID params
2019-09-20 18:09:47 -03:00
// @Param: _SAIL_FLTT
// @DisplayName: Sail Heel Target filter frequency in Hz
// @Description: Target filter frequency in Hz
AR_AttitudeControl: update param desc increment and range
2020-09-15 22:25:12 -03:00
// @Range: 0.000 100.000
// @Increment: 0.1
APM_Control: AR_AttitudeControl: add description for new PID params
2019-09-20 18:09:47 -03:00
// @Units: Hz
// @User: Standard
// @Param: _SAIL_FLTE
// @DisplayName: Sail Heel Error filter frequency in Hz
// @Description: Error filter frequency in Hz
AR_AttitudeControl: update param desc increment and range
2020-09-15 22:25:12 -03:00
// @Range: 0.000 100.000
// @Increment: 0.1
APM_Control: AR_AttitudeControl: add description for new PID params
2019-09-20 18:09:47 -03:00
// @Units: Hz
// @User: Standard
// @Param: _SAIL_FLTD
// @DisplayName: Sail Heel Derivative term filter frequency in Hz
// @Description: Derivative filter frequency in Hz
AR_AttitudeControl: update param desc increment and range
2020-09-15 22:25:12 -03:00
// @Range: 0.000 100.000
// @Increment: 0.1
APM_Control: AR_AttitudeControl: add description for new PID params
2019-09-20 18:09:47 -03:00
// @Units: Hz
// @User: Standard
APM_Control: added SMAX param docs
2020-09-24 04:26:05 -03:00
// @Param: _SAIL_SMAX
// @DisplayName: Sail heel 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
AR_AttitudeControl: add sail heel PID
2018-09-14 03:52:02 -03:00
AP_SUBGROUPINFO(_sailboat_heel_pid, "_SAIL_", 12, AR_AttitudeControl, AC_PID),
APM_Control: G limit all turn rates
2021-05-02 13:36:32 -03:00
// @Param: _TURN_MAX_G
// @DisplayName: Turning maximum G force
// @Description: The maximum turning acceleration (in units of gravities) that the rover can handle while remaining stable. The navigation code will keep the lateral acceleration below this level to avoid rolling over or slipping the wheels in turns
// @Units: gravities
// @Range: 0.1 10
// @Increment: 0.01
// @User: Standard
AP_GROUPINFO("_TURN_MAX_G", 13, AR_AttitudeControl, _turn_lateral_G_max, 0.6f),
AP_AttitudeControl: remove tabs non-functional change
2017-11-08 02:05:33 -04:00
AP_GROUPEND
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
};
APM_Control: stop taking references to ahrs in APM_Control
2021-07-20 08:15:06 -03:00
AR_AttitudeControl::AR_AttitudeControl() :
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
_steer_angle_p(AR_ATTCONTROL_STEER_ANG_P),
APM_Control: support for upgrade to PID object
2019-06-27 06:36:12 -03:00
_steer_rate_pid(AR_ATTCONTROL_STEER_RATE_P, AR_ATTCONTROL_STEER_RATE_I, AR_ATTCONTROL_STEER_RATE_D, AR_ATTCONTROL_STEER_RATE_FF, AR_ATTCONTROL_STEER_RATE_IMAX, 0.0f, AR_ATTCONTROL_STEER_RATE_FILT, 0.0f, AR_ATTCONTROL_DT),
_throttle_speed_pid(AR_ATTCONTROL_THR_SPEED_P, AR_ATTCONTROL_THR_SPEED_I, AR_ATTCONTROL_THR_SPEED_D, 0.0f, AR_ATTCONTROL_THR_SPEED_IMAX, 0.0f, AR_ATTCONTROL_THR_SPEED_FILT, 0.0f, AR_ATTCONTROL_DT),
_pitch_to_throttle_pid(AR_ATTCONTROL_PITCH_THR_P, AR_ATTCONTROL_PITCH_THR_I, AR_ATTCONTROL_PITCH_THR_D, 0.0f, AR_ATTCONTROL_PITCH_THR_IMAX, 0.0f, AR_ATTCONTROL_PITCH_THR_FILT, 0.0f, AR_ATTCONTROL_DT),
_sailboat_heel_pid(AR_ATTCONTROL_HEEL_SAIL_P, AR_ATTCONTROL_HEEL_SAIL_I, AR_ATTCONTROL_HEEL_SAIL_D, 0.0f, AR_ATTCONTROL_HEEL_SAIL_IMAX, 0.0f, AR_ATTCONTROL_HEEL_SAIL_FILT, 0.0f, AR_ATTCONTROL_DT)
AR_AttitudeControl: add sail heel PID
2018-09-14 03:52:02 -03:00
{
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
AP_Param::setup_object_defaults(this, var_info);
}
// return a steering servo output from -1.0 to +1.0 given a desired lateral acceleration rate in m/s/s.
// positive lateral acceleration is to the right.
AR_AttitudeControl: caller provides dt instead of calculated internally This allows the vehicle's main loop rate to be used instead of an internally calculated dt which suffers from jitter
2018-05-21 22:04:53 -03:00
float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool motor_limit_left, bool motor_limit_right, float dt)
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
{
AR_AttitudeControl: add get_desired_turn_rate lat_accel and speed for reporting use only
2017-12-07 08:19:32 -04:00
// record desired accel for reporting purposes
_steer_lat_accel_last_ms = AP_HAL::millis();
_desired_lat_accel = desired_accel;
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// get speed forward
float speed;
if (!get_forward_speed(speed)) {
// we expect caller will not try to control heading using rate control without a valid speed estimate
// on failure to get speed we do not attempt to steer
return 0.0f;
}
AR_AttitudeControl: add get_turn_rate_from_lat_accel
2019-04-30 21:30:00 -03:00
const float desired_rate = get_turn_rate_from_lat_accel(desired_accel, speed);
AR_AttitudeControl: get_steering_out methods accept reverse argument
2017-11-08 01:35:52 -04:00
AR_AttitudeControl: caller provides dt instead of calculated internally This allows the vehicle's main loop rate to be used instead of an internally calculated dt which suffers from jitter
2018-05-21 22:04:53 -03:00
return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right, dt);
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
}
AR_AttitudeControl: replace get_steering_out_angle_error with heading internally uses AHRS's heading to determine angle error so overally a non-functional change
2018-03-16 06:39:52 -03:00
// return a steering servo output from -1 to +1 given a heading in radians
AR_AttitudeControl: clarify units of get_steering_out_heading argument also update comments
2018-09-10 04:19:24 -03:00
// set rate_max_rads to a non-zero number to apply a limit on the desired turn rate
// return value is normally in range -1.0 to +1.0 but can be higher or lower
float AR_AttitudeControl::get_steering_out_heading(float heading_rad, float rate_max_rads, bool motor_limit_left, bool motor_limit_right, float dt)
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
{
AR_AttitudeControl: add get_turn_rate_from_heading
2019-05-02 03:35:46 -03:00
// calculate the desired turn rate (in radians) from the angle error (also in radians)
float desired_rate = get_turn_rate_from_heading(heading_rad, rate_max_rads);
return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right, dt);
}
// return a desired turn-rate given a desired heading in radians
float AR_AttitudeControl::get_turn_rate_from_heading(float heading_rad, float rate_max_rads) const
{
APM_Control: stop taking references to ahrs in APM_Control
2021-07-20 08:15:06 -03:00
const float yaw_error = wrap_PI(heading_rad - AP::ahrs().yaw);
AR_AttitudeControl: replace get_steering_out_angle_error with heading internally uses AHRS's heading to determine angle error so overally a non-functional change
2018-03-16 06:39:52 -03:00
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// Calculate the desired turn rate (in radians) from the angle error (also in radians)
AR_AttitudeControl: limit desired steering rate
2018-05-18 04:04:17 -03:00
float desired_rate = _steer_angle_p.get_p(yaw_error);
AR_AttitudeControl: add get_turn_rate_from_heading
2019-05-02 03:35:46 -03:00
AR_AttitudeControl: limit desired steering rate
2018-05-18 04:04:17 -03:00
// limit desired_rate if a custom pivot turn rate is selected, otherwise use ATC_STR_RAT_MAX
AR_AttitudeControl: clarify units of get_steering_out_heading argument also update comments
2018-09-10 04:19:24 -03:00
if (is_positive(rate_max_rads)) {
desired_rate = constrain_float(desired_rate, -rate_max_rads, rate_max_rads);
AR_AttitudeControl: limit desired steering rate
2018-05-18 04:04:17 -03:00
}
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
AR_AttitudeControl: add get_turn_rate_from_heading
2019-05-02 03:35:46 -03:00
return desired_rate;
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
}
// return a steering servo output from -1 to +1 given a
// desired yaw rate in radians/sec. Positive yaw is to the right.
AR_AttitudeControl: caller provides dt instead of calculated internally This allows the vehicle's main loop rate to be used instead of an internally calculated dt which suffers from jitter
2018-05-21 22:04:53 -03:00
float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool motor_limit_left, bool motor_limit_right, float dt)
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
{
AR_AttitudeControl: caller provides dt instead of calculated internally This allows the vehicle's main loop rate to be used instead of an internally calculated dt which suffers from jitter
2018-05-21 22:04:53 -03:00
// sanity check dt
dt = constrain_float(dt, 0.0f, 1.0f);
// if not called recently, reset input filter and desired turn rate to actual turn rate (used for accel limiting)
AR_AttitudeControl: const local variables
2017-11-08 20:42:09 -04:00
const uint32_t now = AP_HAL::millis();
AR_AttitudeControl: caller provides dt instead of calculated internally This allows the vehicle's main loop rate to be used instead of an internally calculated dt which suffers from jitter
2018-05-21 22:04:53 -03:00
if ((_steer_turn_last_ms == 0) || ((now - _steer_turn_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) {
AR_AttitudeControl: reset input filter for steering and throttle controllers Also only set dt to non-zero value
2017-10-26 02:58:10 -03:00
_steer_rate_pid.reset_filter();
AR_AttitudeControl: reset turn-rate I term if not run for 0.2sec
2018-05-31 22:24:50 -03:00
_steer_rate_pid.reset_I();
APM_Control: stop taking references to ahrs in APM_Control
2021-07-20 08:15:06 -03:00
_desired_turn_rate = AP::ahrs().get_yaw_rate_earth();
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
}
_steer_turn_last_ms = now;
AR_AttitudeControl: add angular accel and rate limits This limits the change in desired turn rate to reduce impossible requests which should help avoid overshoot Also add rotation rate limit to turn-rate controller
2017-12-25 00:28:07 -04:00
// acceleration limit desired turn rate
AR_AttitudeControl: caller provides dt instead of calculated internally This allows the vehicle's main loop rate to be used instead of an internally calculated dt which suffers from jitter
2018-05-21 22:04:53 -03:00
if (is_positive(_steer_accel_max)) {
const float change_max = radians(_steer_accel_max) * dt;
AR_AttitudeControl: add angular accel and rate limits This limits the change in desired turn rate to reduce impossible requests which should help avoid overshoot Also add rotation rate limit to turn-rate controller
2017-12-25 00:28:07 -04:00
desired_rate = constrain_float(desired_rate, _desired_turn_rate - change_max, _desired_turn_rate + change_max);
}
_desired_turn_rate = desired_rate;
// rate limit desired turn rate
if (is_positive(_steer_rate_max)) {
AR_AttitudeControl: fix get_steering_out_rate use of max turn rate and acceleration
2018-05-10 01:54:40 -03:00
const float steer_rate_max_rad = radians(_steer_rate_max);
_desired_turn_rate = constrain_float(_desired_turn_rate, -steer_rate_max_rad, steer_rate_max_rad);
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
}
APM_Control: G limit all turn rates
2021-05-02 13:36:32 -03:00
// G limit based on speed
float speed;
if (get_forward_speed(speed)) {
// do not limit to less than 1 deg/s
AR_AttitudeControl: get_steering_out_rate use abs speed for G limit
2021-05-19 07:26:49 -03:00
const float turn_rate_max = MAX(get_turn_rate_from_lat_accel(get_turn_lat_accel_max(), fabsf(speed)), radians(1.0f));
APM_Control: G limit all turn rates
2021-05-02 13:36:32 -03:00
_desired_turn_rate = constrain_float(_desired_turn_rate, -turn_rate_max, turn_rate_max);
}
AR_AttitudeControl: caller provides dt instead of calculated internally This allows the vehicle's main loop rate to be used instead of an internally calculated dt which suffers from jitter
2018-05-21 22:04:53 -03:00
// set PID's dt
_steer_rate_pid.set_dt(dt);
APM_Control: stop taking references to ahrs in APM_Control
2021-07-20 08:15:06 -03:00
float output = _steer_rate_pid.update_all(_desired_turn_rate, AP::ahrs().get_yaw_rate_earth(), (motor_limit_left || motor_limit_right));
APM_Control: support for upgrade to PID object
2019-06-27 06:36:12 -03:00
output += _steer_rate_pid.get_ff();
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// constrain and return final output
APM_Control: support for upgrade to PID object
2019-06-27 06:36:12 -03:00
return output;
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
}
AR_AttitudeControl: add get_desired_turn_rate lat_accel and speed for reporting use only
2017-12-07 08:19:32 -04:00
// get latest desired turn rate in rad/sec (recorded during calls to get_steering_out_rate)
float AR_AttitudeControl::get_desired_turn_rate() const
{
// return zero if no recent calls to turn rate controller
AR_AttitudeControl: fix get_desired_speed timeout
2017-12-09 02:20:11 -04:00
if ((_steer_turn_last_ms == 0) || ((AP_HAL::millis() - _steer_turn_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) {
AR_AttitudeControl: add get_desired_turn_rate lat_accel and speed for reporting use only
2017-12-07 08:19:32 -04:00
return 0.0f;
}
return _desired_turn_rate;
}
// get latest desired lateral acceleration in m/s/s (recorded during calls to get_steering_out_lat_accel)
float AR_AttitudeControl::get_desired_lat_accel() const
{
// return zero if no recent calls to lateral acceleration controller
AR_AttitudeControl: fix get_desired_speed timeout
2017-12-09 02:20:11 -04:00
if ((_steer_lat_accel_last_ms == 0) || ((AP_HAL::millis() - _steer_lat_accel_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) {
AR_AttitudeControl: add get_desired_turn_rate lat_accel and speed for reporting use only
2017-12-07 08:19:32 -04:00
return 0.0f;
}
return _desired_lat_accel;
}
// get actual lateral acceleration in m/s/s. returns true on success
bool AR_AttitudeControl::get_lat_accel(float &lat_accel) const
{
float speed;
if (!get_forward_speed(speed)) {
return false;
}
APM_Control: stop taking references to ahrs in APM_Control
2021-07-20 08:15:06 -03:00
lat_accel = speed * AP::ahrs().get_yaw_rate_earth();
AR_AttitudeControl: add get_desired_turn_rate lat_accel and speed for reporting use only
2017-12-07 08:19:32 -04:00
return true;
}
AR_AttitudeControl: add get_turn_rate_from_lat_accel
2019-04-30 21:30:00 -03:00
// calculate the turn rate in rad/sec given a lateral acceleration (in m/s/s) and speed (in m/s)
float AR_AttitudeControl::get_turn_rate_from_lat_accel(float lat_accel, float speed) const
{
// enforce minimum speed to stop oscillations when first starting to move
if (fabsf(speed) < AR_ATTCONTROL_STEER_SPEED_MIN) {
if (is_negative(speed)) {
speed = -AR_ATTCONTROL_STEER_SPEED_MIN;
} else {
speed = AR_ATTCONTROL_STEER_SPEED_MIN;
}
}
return lat_accel / speed;
}
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// return a throttle output from -1 to +1 given a desired speed in m/s (use negative speeds to travel backwards)
// motor_limit should be true if motors have hit their upper or lower limits
// cruise speed should be in m/s, cruise throttle should be a number from -1 to +1
AR_AttitudeControl: caller provides dt instead of calculated internally This allows the vehicle's main loop rate to be used instead of an internally calculated dt which suffers from jitter
2018-05-21 22:04:53 -03:00
float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, float dt)
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
{
AR_AttitudeControl: caller provides dt instead of calculated internally This allows the vehicle's main loop rate to be used instead of an internally calculated dt which suffers from jitter
2018-05-21 22:04:53 -03:00
// sanity check dt
dt = constrain_float(dt, 0.0f, 1.0f);
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// get speed forward
float speed;
if (!get_forward_speed(speed)) {
// we expect caller will not try to control heading using rate control without a valid speed estimate
// on failure to get speed we do not attempt to steer
return 0.0f;
}
AR_AttitudeControl: caller provides dt instead of calculated internally This allows the vehicle's main loop rate to be used instead of an internally calculated dt which suffers from jitter
2018-05-21 22:04:53 -03:00
// if not called recently, reset input filter and desired speed to actual speed (used for accel limiting)
AR_AttitudeControl: add speed-control-active method allows caller check if speed controller if active also consolidated other methods to use this check to reduce a little bit of duplicate code
2018-05-28 03:43:53 -03:00
if (!speed_control_active()) {
AR_AttitudeControl: reset input filter for steering and throttle controllers Also only set dt to non-zero value
2017-10-26 02:58:10 -03:00
_throttle_speed_pid.reset_filter();
AR_AttitudeControl: reset I if speed controller not called recently
2018-09-04 02:38:02 -03:00
_throttle_speed_pid.reset_I();
AR_AttitudeControl: caller provides dt instead of calculated internally This allows the vehicle's main loop rate to be used instead of an internally calculated dt which suffers from jitter
2018-05-21 22:04:53 -03:00
_desired_speed = speed;
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
}
AR_AttitudeControl: use speed_control_active in get_desired_speed_accel_limited also minor formatting fixes this is a non-functional change
2019-05-04 00:10:55 -03:00
_speed_last_ms = AP_HAL::millis();
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
AR_AttitudeControl: caller provides dt instead of calculated internally This allows the vehicle's main loop rate to be used instead of an internally calculated dt which suffers from jitter
2018-05-21 22:04:53 -03:00
// acceleration limit desired speed
_desired_speed = get_desired_speed_accel_limited(desired_speed, dt);
// set PID's dt
_throttle_speed_pid.set_dt(dt);
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// calculate base throttle (protect against divide by zero)
float throttle_base = 0.0f;
if (is_positive(cruise_speed) && is_positive(cruise_throttle)) {
throttle_base = desired_speed * (cruise_throttle / cruise_speed);
}
// calculate final output
APM_Control: support for upgrade to PID object
2019-06-27 06:36:12 -03:00
float throttle_out = _throttle_speed_pid.update_all(desired_speed, speed, (_throttle_limit_low || _throttle_limit_high));
throttle_out += _throttle_speed_pid.get_ff();
throttle_out += throttle_base;
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// clear local limit flags used to stop i-term build-up as we stop reversed outputs going to motors
_throttle_limit_low = false;
_throttle_limit_high = false;
// protect against reverse output being sent to the motors unless braking has been enabled
if (!_brake_enable) {
// if both desired speed and actual speed are positive, do not allow negative values
AR_AttitudeControl: fix braking this fix ensures the output throttle is never in the opposite direction from the desired-speed there is a possibility that this could lead to rougher throttle response when the vehicle is transitioning from forward to backwards motion because the throttle response will immediately go to zero when the desired speed cross over zero
2017-10-25 10:21:52 -03:00
if ((desired_speed >= 0.0f) && (throttle_out <= 0.0f)) {
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
throttle_out = 0.0f;
_throttle_limit_low = true;
AR_AttitudeControl: brake fix to set limit in only one direction
2018-07-18 20:55:26 -03:00
} else if ((desired_speed <= 0.0f) && (throttle_out >= 0.0f)) {
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
throttle_out = 0.0f;
_throttle_limit_high = true;
}
}
// final output throttle in range -1 to 1
return throttle_out;
}
// return a throttle output from -1 to +1 to perform a controlled stop. returns true once the vehicle has stopped
AR_AttitudeControl: caller provides dt instead of calculated internally This allows the vehicle's main loop rate to be used instead of an internally calculated dt which suffers from jitter
2018-05-21 22:04:53 -03:00
float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, float dt, bool &stopped)
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
{
// get current system time
AR_AttitudeControl: const local variables
2017-11-08 20:42:09 -04:00
const uint32_t now = AP_HAL::millis();
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// if we were stopped in the last 300ms, assume we are still stopped
bool _stopped = (_stop_last_ms != 0) && (now - _stop_last_ms) < 300;
AR_AttitudeControl: accel limit stops
2018-05-21 08:39:01 -03:00
// get deceleration limited speed
AR_AttitudeControl: caller provides dt instead of calculated internally This allows the vehicle's main loop rate to be used instead of an internally calculated dt which suffers from jitter
2018-05-21 22:04:53 -03:00
float desired_speed_limited = get_desired_speed_accel_limited(0.0f, dt);
AR_AttitudeControl: accel limit stops
2018-05-21 08:39:01 -03:00
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// get speed forward
float speed;
if (!get_forward_speed(speed)) {
// could not get speed so assume stopped
_stopped = true;
} else {
AR_AttitudeControl: get_throttle_out_stop only stops once desired speed reaches zero this reduces the final jerk to a when transitioning from forward to reverse and in cases where ATC_ACCEL_MAX is small
2018-04-21 03:44:09 -03:00
// if desired speed is zero and vehicle drops below _stop_speed consider it stopped
AR_AttitudeControl: accel limit stops
2018-05-21 08:39:01 -03:00
if (is_zero(desired_speed_limited) && fabsf(speed) <= fabsf(_stop_speed)) {
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
_stopped = true;
}
}
// set stopped status for caller
stopped = _stopped;
// if stopped return zero
if (stopped) {
// update last time we thought we were stopped
_stop_last_ms = now;
AR_AttitudeControl: set-throttle-out-stop considered same as running speed controller update the speed-last-ms time when set-throttle-out-stop runs or else the desired throttle will not be acceleration limited when the desired speed is next increased above zero
2019-06-04 09:10:59 -03:00
// set last time speed controller was run so accelerations are limited
_speed_last_ms = now;
AR_AttitudeControl: reset speed filter and I term when stopped
2021-03-02 22:41:30 -04:00
// reset filters and I-term
_throttle_speed_pid.reset_filter();
_throttle_speed_pid.reset_I();
// ensure desired speed is zero
_desired_speed = 0.0f;
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
return 0.0f;
}
AR_AttitudeControl: move unnecessary else
2018-08-05 21:58:13 -03:00
// clear stopped system time
_stop_last_ms = 0;
// run speed controller to bring vehicle to stop
return get_throttle_out_speed(desired_speed_limited, motor_limit_low, motor_limit_high, cruise_speed, cruise_throttle, dt);
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
}
AR_AttitudeControl: get_throttle_out_from_pitch accepts vehicle speed
2018-08-08 02:42:09 -03:00
// balancebot pitch to throttle controller
// returns a throttle output from -100 to +100 given a desired pitch angle and vehicle's current speed (from wheel encoders)
// desired_pitch is in radians, veh_speed_pct is supplied as a percentage (-100 to +100) of vehicle's top speed
float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, float vehicle_speed_pct, bool motor_limit_low, bool motor_limit_high, float dt)
APM_Control: added balancing function for BalanceBot
2018-06-18 08:14:28 -03:00
{
APM_Control: get_throttle_out_from_pitch() accepts dt as argument
2018-06-27 10:14:55 -03:00
// sanity check dt
dt = constrain_float(dt, 0.0f, 1.0f);
APM_Control: added balancing function for BalanceBot
2018-06-18 08:14:28 -03:00
// if not called recently, reset input filter
AR_AttitudeControl: re-order pitch control to match other similar methods
2018-08-04 03:52:38 -03:00
const uint32_t now = AP_HAL::millis();
AR_AttitudeControl: get_throttle_out_from_pitch uses motor limits This allows removing I-term build up from throttle hitting 100%
2018-08-04 04:07:45 -03:00
if ((_balance_last_ms == 0) || ((now - _balance_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) {
APM_Control: added balancing function for BalanceBot
2018-06-18 08:14:28 -03:00
_pitch_to_throttle_pid.reset_filter();
AR_AttitudeControl: re-order pitch control to match other similar methods
2018-08-04 03:52:38 -03:00
_pitch_to_throttle_pid.reset_I();
APM_Control: added balancing function for BalanceBot
2018-06-18 08:14:28 -03:00
}
_balance_last_ms = now;
AR_AttitudeControl: re-order pitch control to match other similar methods
2018-08-04 03:52:38 -03:00
// set PID's dt
_pitch_to_throttle_pid.set_dt(dt);
APM_Control: added balancing function for BalanceBot
2018-06-18 08:14:28 -03:00
AR_AttitudeControl: get_throttle_out_from_pitch accepts vehicle speed
2018-08-08 02:42:09 -03:00
// add feed forward from speed
APM_Control: support for upgrade to PID object
2019-06-27 06:36:12 -03:00
float output = vehicle_speed_pct * 0.01f * _pitch_to_throttle_speed_ff;
APM_Control: stop taking references to ahrs in APM_Control
2021-07-20 08:15:06 -03:00
output += _pitch_to_throttle_pid.update_all(desired_pitch, AP::ahrs().pitch, (motor_limit_low || motor_limit_high));
APM_Control: support for upgrade to PID object
2019-06-27 06:36:12 -03:00
output += _pitch_to_throttle_pid.get_ff();
AR_AttitudeControl: get_throttle_out_from_pitch accepts vehicle speed
2018-08-08 02:42:09 -03:00
AR_AttitudeControl: get_throttle_out_from_pitch uses motor limits This allows removing I-term build up from throttle hitting 100%
2018-08-04 04:07:45 -03:00
// constrain and return final output
APM_Control: support for upgrade to PID object
2019-06-27 06:36:12 -03:00
return output;
APM_Control: added balancing function for BalanceBot
2018-06-18 08:14:28 -03:00
}
AR_AttitudeControl: add get_desired_pitch accessor
2018-08-04 03:22:23 -03:00
// get latest desired pitch in radians for reporting purposes
float AR_AttitudeControl::get_desired_pitch() const
{
// if not called recently, return 0
if ((_balance_last_ms == 0) || ((AP_HAL::millis() - _balance_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) {
return 0.0f;
}
Global: rename desired to target in PID info
2019-06-27 06:31:52 -03:00
return _pitch_to_throttle_pid.get_pid_info().target;
AR_AttitudeControl: add get_desired_pitch accessor
2018-08-04 03:22:23 -03:00
}
AR_AttitudeControl: minor comment fixes
2019-08-06 04:58:17 -03:00
// Sailboat heel(roll) angle controller releases sail to keep at maximum heel angle
// but does not attempt to reach maximum heel angle, ie only lets sails out, does not pull them in
AR_AttitudeControl: add sail heel PID
2018-09-14 03:52:02 -03:00
float AR_AttitudeControl::get_sail_out_from_heel(float desired_heel, float dt)
{
// sanity check dt
dt = constrain_float(dt, 0.0f, 1.0f);
// if not called recently, reset input filter
const uint32_t now = AP_HAL::millis();
if ((_heel_controller_last_ms == 0) || ((now - _heel_controller_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) {
_sailboat_heel_pid.reset_filter();
_sailboat_heel_pid.reset_I();
}
_heel_controller_last_ms = now;
// set PID's dt
_sailboat_heel_pid.set_dt(dt);
APM_Control: stop taking references to ahrs in APM_Control
2021-07-20 08:15:06 -03:00
_sailboat_heel_pid.update_all(desired_heel, fabsf(AP::ahrs().roll));
AR_AttitudeControl: add sail heel PID
2018-09-14 03:52:02 -03:00
// get feed-forward
APM_Control: support for upgrade to PID object
2019-06-27 06:36:12 -03:00
const float ff = _sailboat_heel_pid.get_ff();
AR_AttitudeControl: add sail heel PID
2018-09-14 03:52:02 -03:00
AR_AttitudeControl: minor comment fixes
2019-08-06 04:58:17 -03:00
// get p, constrain to be zero or negative
AR_AttitudeControl: add sail heel PID
2018-09-14 03:52:02 -03:00
float p = _sailboat_heel_pid.get_p();
AR_AttitudeControl: fix sailboat heel PID
2019-07-30 13:21:37 -03:00
if (is_positive(p)) {
AR_AttitudeControl: add sail heel PID
2018-09-14 03:52:02 -03:00
p = 0.0f;
}
AR_AttitudeControl: minor comment fixes
2019-08-06 04:58:17 -03:00
// get i, constrain to be zero or negative
APM_Control: support for upgrade to PID object
2019-06-27 06:36:12 -03:00
float i = _sailboat_heel_pid.get_i();
AR_AttitudeControl: fix sailboat heel PID
2019-07-30 13:21:37 -03:00
if (is_positive(i)) {
AR_AttitudeControl: add sail heel PID
2018-09-14 03:52:02 -03:00
i = 0.0f;
_sailboat_heel_pid.reset_I();
}
// get d
const float d = _sailboat_heel_pid.get_d();
// constrain and return final output
AR_AttitudeControl: fix sailboat heel PID
2019-07-30 13:21:37 -03:00
return (ff + p + i + d) * -1.0f;
AR_AttitudeControl: add sail heel PID
2018-09-14 03:52:02 -03:00
}
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
// get forward speed in m/s (earth-frame horizontal velocity but only along vehicle x-axis). returns true on success
bool AR_AttitudeControl::get_forward_speed(float &speed) const
{
Vector3f velocity;
APM_Control: stop taking references to ahrs in APM_Control
2021-07-20 08:15:06 -03:00
const AP_AHRS &_ahrs = AP::ahrs();
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
if (!_ahrs.get_velocity_NED(velocity)) {
// use less accurate GPS, assuming entire length is along forward/back axis of vehicle
APM_Control: use GPS singleton
2017-12-01 19:44:39 -04:00
if (AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
APM_Control: wrap_180_cd no longer solely returns floats
2019-09-09 05:50:34 -03:00
if (abs(wrap_180_cd(_ahrs.yaw_sensor - AP::gps().ground_course_cd())) <= 9000) {
APM_Control: use GPS singleton
2017-12-01 19:44:39 -04:00
speed = AP::gps().ground_speed();
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
} else {
APM_Control: use GPS singleton
2017-12-01 19:44:39 -04:00
speed = -AP::gps().ground_speed();
AR_AttitudeControl: throttle and steering control library Throttle controller has these advantages over existing controller: based on velocity in vehicle's forward-back axis rather than ground-speed straight-forward PID controller using speed error as input speed control acceleration limts stop control slows vehicle smoothly configurable filtering Steer controller has these advantages over existing controller: output scaled properly for skid-steering vehicles layered P and PID controller for angular error and rate control configurable filtering
2017-08-08 02:36:29 -03:00
}
return true;
} else {
return false;
}
}
// calculate forward speed velocity into body frame
speed = velocity.x*_ahrs.cos_yaw() + velocity.y*_ahrs.sin_yaw();
return true;
}
AR_AttitudeControl: add get_desired_turn_rate lat_accel and speed for reporting use only
2017-12-07 08:19:32 -04:00
AR_AttitudeControl: const get_decel_max and get_stopping_distance
2018-05-28 03:22:21 -03:00
float AR_AttitudeControl::get_decel_max() const
AR_AttitudeControl: add separate decel limit
2018-05-18 01:16:42 -03:00
{
if (is_positive(_throttle_decel_max)) {
return _throttle_decel_max;
} else {
return _throttle_accel_max;
}
}
AR_AttitudeControl: add speed-control-active method allows caller check if speed controller if active also consolidated other methods to use this check to reduce a little bit of duplicate code
2018-05-28 03:43:53 -03:00
// check if speed controller active
bool AR_AttitudeControl::speed_control_active() const
{
// active if there have been recent calls to speed controller
if ((_speed_last_ms == 0) || ((AP_HAL::millis() - _speed_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) {
return false;
}
return true;
}
AR_AttitudeControl: add get_desired_turn_rate lat_accel and speed for reporting use only
2017-12-07 08:19:32 -04:00
// get latest desired speed recorded during call to get_throttle_out_speed. For reporting purposes only
float AR_AttitudeControl::get_desired_speed() const
{
// return zero if no recent calls to speed controller
AR_AttitudeControl: add speed-control-active method allows caller check if speed controller if active also consolidated other methods to use this check to reduce a little bit of duplicate code
2018-05-28 03:43:53 -03:00
if (!speed_control_active()) {
AR_AttitudeControl: add get_desired_turn_rate lat_accel and speed for reporting use only
2017-12-07 08:19:32 -04:00
return 0.0f;
}
return _desired_speed;
}
AR_AttitudeControl: add get_stopping_distance
2018-03-13 09:05:47 -03:00
AR_AttitudeControl: separate speed limiting to new method This allows the desired speed limiting (by acceleration) to be done before the speed-to-throttle PID controller is run. This is required so the avoidance calls (which work on the desired speed) can be run after limiting but before the PID controllers
2018-04-21 03:39:49 -03:00
// get acceleration limited desired speed
AR_AttitudeControl: caller provides dt instead of calculated internally This allows the vehicle's main loop rate to be used instead of an internally calculated dt which suffers from jitter
2018-05-21 22:04:53 -03:00
float AR_AttitudeControl::get_desired_speed_accel_limited(float desired_speed, float dt) const
AR_AttitudeControl: separate speed limiting to new method This allows the desired speed limiting (by acceleration) to be done before the speed-to-throttle PID controller is run. This is required so the avoidance calls (which work on the desired speed) can be run after limiting but before the PID controllers
2018-04-21 03:39:49 -03:00
{
AR_AttitudeControl: disable limits when _ACCEL_MAX is zero
2018-06-08 23:04:35 -03:00
// return input value if no recent calls to speed controller
AR_AttitudeControl: use speed_control_active in get_desired_speed_accel_limited also minor formatting fixes this is a non-functional change
2019-05-04 00:10:55 -03:00
// apply no limiting when ATC_ACCEL_MAX is set to zero
if (!speed_control_active() || !is_positive(_throttle_accel_max)) {
AR_AttitudeControl: disable limits when _ACCEL_MAX is zero
2018-06-08 23:04:35 -03:00
return desired_speed;
}
AR_AttitudeControl: caller provides dt instead of calculated internally This allows the vehicle's main loop rate to be used instead of an internally calculated dt which suffers from jitter
2018-05-21 22:04:53 -03:00
// sanity check dt
dt = constrain_float(dt, 0.0f, 1.0f);
AR_AttitudeControl: separate speed limiting to new method This allows the desired speed limiting (by acceleration) to be done before the speed-to-throttle PID controller is run. This is required so the avoidance calls (which work on the desired speed) can be run after limiting but before the PID controllers
2018-04-21 03:39:49 -03:00
AR_AttitudeControl: fix accel limiting
2018-05-18 01:16:42 -03:00
// use previous desired speed as basis for accel limiting
float speed_prev = _desired_speed;
// if no recent calls to speed controller limit based on current speed
AR_AttitudeControl: add speed-control-active method allows caller check if speed controller if active also consolidated other methods to use this check to reduce a little bit of duplicate code
2018-05-28 03:43:53 -03:00
if (!speed_control_active()) {
AR_AttitudeControl: fix accel limiting
2018-05-18 01:16:42 -03:00
get_forward_speed(speed_prev);
}
AR_AttitudeControl: separate speed limiting to new method This allows the desired speed limiting (by acceleration) to be done before the speed-to-throttle PID controller is run. This is required so the avoidance calls (which work on the desired speed) can be run after limiting but before the PID controllers
2018-04-21 03:39:49 -03:00
// acceleration limit desired speed
AR_AttitudeControl: add separate decel limit
2018-05-18 01:16:42 -03:00
float speed_change_max;
if (fabsf(desired_speed) < fabsf(_desired_speed) && is_positive(_throttle_decel_max)) {
speed_change_max = _throttle_decel_max * dt;
} else {
speed_change_max = _throttle_accel_max * dt;
}
AR_AttitudeControl: fix accel limiting
2018-05-18 01:16:42 -03:00
return constrain_float(desired_speed, speed_prev - speed_change_max, speed_prev + speed_change_max);
AR_AttitudeControl: separate speed limiting to new method This allows the desired speed limiting (by acceleration) to be done before the speed-to-throttle PID controller is run. This is required so the avoidance calls (which work on the desired speed) can be run after limiting but before the PID controllers
2018-04-21 03:39:49 -03:00
}
AR_AttitudeControl: add get_stopping_distance
2018-03-13 09:05:47 -03:00
// get minimum stopping distance (in meters) given a speed (in m/s)
AR_AttitudeControl: const get_decel_max and get_stopping_distance
2018-05-28 03:22:21 -03:00
float AR_AttitudeControl::get_stopping_distance(float speed) const
AR_AttitudeControl: add get_stopping_distance
2018-03-13 09:05:47 -03:00
{
// get maximum vehicle deceleration
const float accel_max = get_accel_max();
// avoid divide by zero
if ((accel_max <= 0.0f) || is_zero(speed)) {
return 0.0f;
}
// assume linear deceleration
return 0.5f * sq(speed) / accel_max;
}
APM_Control: add function to relax rover i terms
2019-01-25 14:58:13 -04:00
// relax I terms of throttle and steering controllers
void AR_AttitudeControl::relax_I()
{
_steer_rate_pid.reset_I();
_throttle_speed_pid.reset_I();
_pitch_to_throttle_pid.reset_I();
}