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

48 lines
923 B
C
Raw Normal View History

APM_Control: fixed indent-tabs-mode
2013-05-29 20:53:02 -03:00
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
APM_Control: added new APM controllers library See http://www.challinger.us/2012/07/16/tuning-arduplane-roll-and-pitch-controllers/ for details
2012-08-21 23:08:14 -03:00
APM_Control: ported to AP_HAL Unable to test since there are no unit tests.
2012-10-12 14:53:42 -03:00
#ifndef __AP_YAW_CONTROLLER_H__
#define __AP_YAW_CONTROLLER_H__
APM_Control: added new APM controllers library See http://www.challinger.us/2012/07/16/tuning-arduplane-roll-and-pitch-controllers/ for details
2012-08-21 23:08:14 -03:00
#include <AP_AHRS.h>
#include <AP_Common.h>
#include <math.h> // for fabs()
class AP_YawController {
public:
APM_Control: fixed build on ARM
2013-01-01 22:53:26 -04:00
AP_YawController()
{
APM_Control: updates for new AP_Param API
2012-12-12 17:41:12 -04:00
AP_Param::setup_object_defaults(this, var_info);
}
APM_Control: added new APM controllers library See http://www.challinger.us/2012/07/16/tuning-arduplane-roll-and-pitch-controllers/ for details
2012-08-21 23:08:14 -03:00
void set_ahrs(AP_AHRS *ahrs) {
_ahrs = ahrs;
APM_Control: move reliance from IMU to INS
2012-11-05 00:31:19 -04:00
_ins = _ahrs->get_ins();
APM_Control: added new APM controllers library See http://www.challinger.us/2012/07/16/tuning-arduplane-roll-and-pitch-controllers/ for details
2012-08-21 23:08:14 -03:00
}
APM_Control: ROLL and PITCH controllers These changes reduce height variation in turns and improve robustness. the specific changes are: 1) Linked roll and pitch integrator protection to the final output value so that if final output is on upper limit, the integrator is prevented from increasing and vice-versa. This improves wind-up protection. 2) Modified rate feedback in roll and pitch controllers to use body rates rather than Euler or earth rates. 3) Changed the roll to pitch compensation to use measured roll angle and estimated airspeed to calculate the component of turn rate (assuming a level coordinated turn) around the pitch axis. This a mathematically correct calculation and will work over a range of bank angles and aircraft with minimal (if any) tuning required. 4) The integrator in the roll and pitch loop is clamped when the estimated speed is below the minimum FBW speed 5) The noise filter in the pitch and roll loop has been changed to use a FOH discretisation. This gives improved noise rejection and less phase loss when compared to the previous filter that used a ZOH or equivalent discretisation. This has been flown on the rascal in the SITL and on a X-8 with limited flight testing. Initial results have been encouraging with reduced height variation in turns. Compare to standard PIDS, the revised pitch and roll controllers allow the use of rate feedback (effectively the same as the old D term) without beating the servos to death. The bank angle compensation in the pitch loop works effectively over a much larger range of bank angles and requires minimal tuning compared to the old calculation. YAW CONTROLLER Currently testing the a 3-loop acceleration autopilot topology for the yaw loop with feed forward yaw rate for turn compensation. This 3-loop topology is commonly used in tactical skid to to turn missiles and is easy to tune. The following block diagram shows the general signal flow Note that the acceleration measurement has to pass through an integrator before it gets to the actuator. This is a important feature as it eliminates problems of high frequency noise and potential coupling with structural modes associated with direct feedback of measured acceleration to actuator. The high pass filter has been inserted to compensate for airspeed and bank angle measurement errors which will cause steady state errors in the calculation of the turn yaw rate. The yaw controller flies SITL well, but hasn't been flight tested yet. It can be configured either as a simple yaw damper, or the acceleration and integral term can be turned on to allow feedback control of lateral acceleration/sideslip. TO DO: Need to reduce number of tuning parameters and provide consistent naming Need to provide guidance on tuning these loops with definitions for all the gain terms. Need to check signs and units into and out of lateral loops. DESIGN DECISIONS PENDING: 1) Can we remove the noise filters? Provided the mpu6k noise filter is running they are of limited benefit given the 25Hz Nyquist frequency 2) If we do remove them and rely on the mpu6k noise filter, what is the apprporiate default cutoff frequency for plane use. 20Hz is probably OK for most setups, but some noisy/high vibration setups would require as low as 10Hz 3) The inverted flight logic looks like a crash waiting to happen. It's problematic to test and even if implemented correctly would still crash a plane with poor inverted flight capability. We should either implement it properly and fully tested or delete it.
2013-04-23 08:02:18 -03:00
int32_t get_servo_out(float scaler = 1.0, bool stabilize = false, int16_t aspd_min = 0, int16_t aspd_max = 0);
APM_Control: added new APM controllers library See http://www.challinger.us/2012/07/16/tuning-arduplane-roll-and-pitch-controllers/ for details
2012-08-21 23:08:14 -03:00
void reset_I();
static const struct AP_Param::GroupInfo var_info[];
private:
APM_Control: ROLL and PITCH controllers These changes reduce height variation in turns and improve robustness. the specific changes are: 1) Linked roll and pitch integrator protection to the final output value so that if final output is on upper limit, the integrator is prevented from increasing and vice-versa. This improves wind-up protection. 2) Modified rate feedback in roll and pitch controllers to use body rates rather than Euler or earth rates. 3) Changed the roll to pitch compensation to use measured roll angle and estimated airspeed to calculate the component of turn rate (assuming a level coordinated turn) around the pitch axis. This a mathematically correct calculation and will work over a range of bank angles and aircraft with minimal (if any) tuning required. 4) The integrator in the roll and pitch loop is clamped when the estimated speed is below the minimum FBW speed 5) The noise filter in the pitch and roll loop has been changed to use a FOH discretisation. This gives improved noise rejection and less phase loss when compared to the previous filter that used a ZOH or equivalent discretisation. This has been flown on the rascal in the SITL and on a X-8 with limited flight testing. Initial results have been encouraging with reduced height variation in turns. Compare to standard PIDS, the revised pitch and roll controllers allow the use of rate feedback (effectively the same as the old D term) without beating the servos to death. The bank angle compensation in the pitch loop works effectively over a much larger range of bank angles and requires minimal tuning compared to the old calculation. YAW CONTROLLER Currently testing the a 3-loop acceleration autopilot topology for the yaw loop with feed forward yaw rate for turn compensation. This 3-loop topology is commonly used in tactical skid to to turn missiles and is easy to tune. The following block diagram shows the general signal flow Note that the acceleration measurement has to pass through an integrator before it gets to the actuator. This is a important feature as it eliminates problems of high frequency noise and potential coupling with structural modes associated with direct feedback of measured acceleration to actuator. The high pass filter has been inserted to compensate for airspeed and bank angle measurement errors which will cause steady state errors in the calculation of the turn yaw rate. The yaw controller flies SITL well, but hasn't been flight tested yet. It can be configured either as a simple yaw damper, or the acceleration and integral term can be turned on to allow feedback control of lateral acceleration/sideslip. TO DO: Need to reduce number of tuning parameters and provide consistent naming Need to provide guidance on tuning these loops with definitions for all the gain terms. Need to check signs and units into and out of lateral loops. DESIGN DECISIONS PENDING: 1) Can we remove the noise filters? Provided the mpu6k noise filter is running they are of limited benefit given the 25Hz Nyquist frequency 2) If we do remove them and rely on the mpu6k noise filter, what is the apprporiate default cutoff frequency for plane use. 20Hz is probably OK for most setups, but some noisy/high vibration setups would require as low as 10Hz 3) The inverted flight logic looks like a crash waiting to happen. It's problematic to test and even if implemented correctly would still crash a plane with poor inverted flight capability. We should either implement it properly and fully tested or delete it.
2013-04-23 08:02:18 -03:00
AP_Float _K_A;
AP_Float _K_I;
AP_Float _K_D;
AP_Float _K_FF;
APM_Control: added new APM controllers library See http://www.challinger.us/2012/07/16/tuning-arduplane-roll-and-pitch-controllers/ for details
2012-08-21 23:08:14 -03:00
uint32_t _last_t;
float _last_error;
APM_Control : Removed 20Hz low-pass filters on rate gyro inputs. These are not required due to MPU 6000 filtering.
2013-04-23 16:22:17 -03:00
float _last_out;
float _last_rate_hp_out;
float _last_rate_hp_in;
float _K_D_last;
APM_Control: added new APM controllers library See http://www.challinger.us/2012/07/16/tuning-arduplane-roll-and-pitch-controllers/ for details
2012-08-21 23:08:14 -03:00
float _integrator;
AP_AHRS *_ahrs;
APM_Control: move reliance from IMU to INS
2012-11-05 00:31:19 -04:00
AP_InertialSensor *_ins;
APM_Control: added new APM controllers library See http://www.challinger.us/2012/07/16/tuning-arduplane-roll-and-pitch-controllers/ for details
2012-08-21 23:08:14 -03:00
};
APM_Control: ported to AP_HAL Unable to test since there are no unit tests.
2012-10-12 14:53:42 -03:00
#endif // __AP_YAW_CONTROLLER_H__