aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
9c6f80c42b
Ardupilot2/libraries/APM_Control/AP_PitchController.cpp

194 lines
8.1 KiB
C++
Raw Normal View History

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
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
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
// Initial Code by Jon Challinger
// Modified by Paul Riseborough
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
// This library is free software; you can redistribute it and / or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
Add AVR compatibility header for missing math.h definitions. - Define float versions of math functions to the double versions on AVR (eg. #define sinf sin). - These macros appear to be missing in older versions of avr-libs. - Include AP_Math.h rather than math.h to get these definitions.
2013-01-10 15:22:37 -04:00
#include <AP_Math.h>
APM_Control: ported to AP_HAL Unable to test since there are no unit tests.
2012-10-12 14:53:42 -03:00
#include <AP_HAL.h>
APM_Control: include AP_Common.h
2013-01-01 23:44:02 -04:00
#include <AP_Common.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_PitchController.h"
APM_Control: ported to AP_HAL Unable to test since there are no unit tests.
2012-10-12 14:53:42 -03:00
extern const AP_HAL::HAL& hal;
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
const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = {
APM_Control: added inline docs to pitch controller
2013-05-05 03:07:05 -03:00
// @Param: OMEGA
// @DisplayName: Pitch rate gain
// @Description: This is the gain from pitch angle error to demanded pitch rate. It controls the time constant from demanded to achieved pitch angle. For example if a time constant from demanded to achieved pitch of 0.5 sec was required, this gain would be set to 1/0.5 = 2.0. A value of 1.0 is a good default and will work with nearly all models. Advanced users may want to increase this to obtain a faster response.
APM_Control: adjust recommended ranges
2013-05-05 07:11:46 -03:00
// @Range: 0.8 2.5
APM_Control: added inline docs to pitch controller
2013-05-05 03:07:05 -03:00
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("OMEGA", 0, AP_PitchController, _kp_angle, 1.0),
// @Param: K_P
// @DisplayName: Pitch demand gain
// @Description: This is the gain from demanded pitch rate to demanded elevator. Provided CTL_PTCH_OMEGA is set to 1.0, then this gain works the same way as the P term in the old PID (PTCH2SRV_P) and can be set to the same value.
// @Range: 0.1 2
// @Increment: 0.1
// @User: User
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_GROUPINFO("K_P", 1, AP_PitchController, _kp_ff, 0.4),
APM_Control: added inline docs to pitch controller
2013-05-05 03:07:05 -03:00
// @Param: K_D
// @DisplayName: Pitch derivative gain
// @Description: This is the gain from pitch rate error to demanded elevator. This adjusts the damping of the pitch control loop. It has the same effect as the D term in the old PID (PTCH2SRV_D) but without the large spikes in servo demands. This will be set to 0 as a default. Some airframes such as flying wings that have poor pitch damping can benefit from a small value of up to 0.1 on this gain term. This should be increased in 0.01 increments as to high a value can lead to a high frequency pitch oscillation that could overstress the airframe.
// @Range: 0 0.1
// @Increment: 0.01
// @User: User
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_GROUPINFO("K_D", 2, AP_PitchController, _kp_rate, 0.0),
APM_Control: added inline docs to pitch controller
2013-05-05 03:07:05 -03:00
// @Param: K_I
// @DisplayName: Pitch integration gain
// @Description: This is the gain for integration of the pitch rate error. It has essentially the same effect as the I term in the old PID (PTCH2SRV_I). This can be set to 0 as a default, however users can increment this to make the pitch angle tracking more accurate.
// @Range: 0 0.2
// @Increment: 0.01
// @User: User
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_GROUPINFO("K_I", 3, AP_PitchController, _ki_rate, 0.0),
APM_Control: added inline docs to pitch controller
2013-05-05 03:07:05 -03:00
// @Param: RMAX_U
// @DisplayName: Pitch up max rate
// @Description: This sets the maximum nose up pitch rate that the controller will demand (degrees/sec). Setting it to zero disables the limit.
// @Range: 0 100
// @Units: degrees/second
// @Increment: 1
// @User: User
AP_GROUPINFO("RMAX_U", 4, AP_PitchController, _max_rate_pos, 0.0),
// @Param: RMAX_D
// @DisplayName: Pitch down max rate
// @Description: This sets the maximum nose down pitch rate that the controller will demand (degrees/sec). Setting it to zero disables the limit.
// @Range: 0 100
// @Units: degrees/second
// @Increment: 1
// @User: User
AP_GROUPINFO("RMAX_D", 5, AP_PitchController, _max_rate_neg, 0.0),
// @Param: K_RLL
// @DisplayName: Pitch/roll height control
// @Description: This is the gain term that is applied to the pitch rate offset calculated as required to keep the nose level during turns. The default value is 1 which will work for all models. Advanced users can use it to correct for height variation in turns. If height is lost initially in turns this can be increased in small increments of 0.05 to compensate. If height is gained initially in turns then it can be decreased.
// @Range: 0.5 2
// @Increment: 0.05
// @User: User
AP_GROUPINFO("K_RLL", 6, AP_PitchController, _roll_ff, 1.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
AP_GROUPEND
};
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
// Function returns an equivalent elevator deflection in centi-degrees in the range from -4500 to 4500
// A positive demand is up
// Inputs are:
// 1) demanded pitch angle in centi-degrees
// 2) control gain scaler = scaling_speed / aspeed
// 3) boolean which is true when stabilise mode is active
// 4) minimum FBW airspeed (metres/sec)
// 5) maximum FBW airspeed (metres/sec)
//
int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stabilize, int16_t aspd_min, int16_t aspd_max)
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
uint32_t tnow = hal.scheduler->millis();
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 dt = tnow - _last_t;
if (_last_t == 0 || dt > 1000) {
dt = 0;
}
_last_t = tnow;
if(_ahrs == NULL) return 0;
APM_Control: support inverted flight in AP_PitchController auto-reverse pitch control when inverted. This is useful not just for inverted flight mode, but also for recovering from poor manual flight Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2013-05-05 07:12:27 -03:00
float delta_time = (float)dt * 0.001f;
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: Modified calculation of bank compensation rate offset to prevent climb if speed > max fbw speed The previous calculation constrained the speed used to calculate the bank compensation rate offset between the min and max fbw speeds. This would result in an unwanted climb if flown above the max fbw speed (this could happen in fbw-a mode)
2013-04-29 04:37:34 -03:00
// Calculate offset to pitch rate demand required to maintain pitch angle whilst banking
// Calculate ideal turn rate from bank angle and airspeed assuming a level coordinated turn
// Pitch rate offset is the component of turn rate about the pitch axis
float aspeed;
float rate_offset;
float bank_angle = _ahrs->roll;
APM_Control: support inverted flight in AP_PitchController auto-reverse pitch control when inverted. This is useful not just for inverted flight mode, but also for recovering from poor manual flight Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2013-05-05 07:12:27 -03:00
bool inverted = false;
APM_Control: Modified calculation of bank compensation rate offset to prevent climb if speed > max fbw speed The previous calculation constrained the speed used to calculate the bank compensation rate offset between the min and max fbw speeds. This would result in an unwanted climb if flown above the max fbw speed (this could happen in fbw-a mode)
2013-04-29 04:37:34 -03:00
// limit bank angle between +- 80 deg if right way up
APM_Control: support inverted flight in AP_PitchController auto-reverse pitch control when inverted. This is useful not just for inverted flight mode, but also for recovering from poor manual flight Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2013-05-05 07:12:27 -03:00
if (fabsf(bank_angle) < radians(90)) {
bank_angle = constrain_float(bank_angle,-radians(80),radians(80));
} else {
inverted = true;
if (bank_angle > 0.0f) {
bank_angle = constrain_float(bank_angle,radians(100),radians(180));
} else {
bank_angle = constrain_float(bank_angle,-radians(180),-radians(100));
}
APM_Control: Modified calculation of bank compensation rate offset to prevent climb if speed > max fbw speed The previous calculation constrained the speed used to calculate the bank compensation rate offset between the min and max fbw speeds. This would result in an unwanted climb if flown above the max fbw speed (this could happen in fbw-a mode)
2013-04-29 04:37:34 -03:00
}
if (!_ahrs->airspeed_estimate(&aspeed)) {
// If no airspeed available use average of min and max
aspeed = 0.5f*(float(aspd_min) + float(aspd_max));
}
APM_Control: support inverted flight in AP_PitchController auto-reverse pitch control when inverted. This is useful not just for inverted flight mode, but also for recovering from poor manual flight Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2013-05-05 07:12:27 -03:00
rate_offset = fabsf(ToDeg((GRAVITY_MSS / max(aspeed , float(aspd_min))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff;
if (inverted) {
rate_offset = -rate_offset;
}
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
APM_Control: Modified calculation of bank compensation rate offset to prevent climb if speed > max fbw speed The previous calculation constrained the speed used to calculate the bank compensation rate offset between the min and max fbw speeds. This would result in an unwanted climb if flown above the max fbw speed (this could happen in fbw-a mode)
2013-04-29 04:37:34 -03:00
//Calculate pitch angle error in centi-degrees
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
int32_t angle_err = angle - _ahrs->pitch_sensor;
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
// Calculate the desired pitch rate (deg/sec) from the angle error
float desired_rate = angle_err * 0.01f * _kp_angle;
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: support inverted flight in AP_PitchController auto-reverse pitch control when inverted. This is useful not just for inverted flight mode, but also for recovering from poor manual flight Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2013-05-05 07:12:27 -03:00
// limit the maximum pitch rate demand. Don't apply when inverted
// as the rates will be tuned when upright, and it is common that
// much higher rates are needed inverted
if (!inverted) {
if (_max_rate_neg && desired_rate < -_max_rate_neg) {
desired_rate = -_max_rate_neg;
} else if (_max_rate_pos && desired_rate > _max_rate_pos) {
desired_rate = _max_rate_pos;
}
}
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: support inverted flight in AP_PitchController auto-reverse pitch control when inverted. This is useful not just for inverted flight mode, but also for recovering from poor manual flight Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2013-05-05 07:12:27 -03:00
if (inverted) {
desired_rate = -desired_rate;
}
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
// Apply the turn correction offset
desired_rate = desired_rate + rate_offset;
// Get body rate vector (radians/sec)
float omega_y = _ahrs->get_gyro().y;
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 : 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
// Calculate the pitch rate error (deg/sec) and scale
APM_Control: fixed build and merge errors
2013-05-05 01:34:33 -03:00
float rate_error = (desired_rate - ToDeg(omega_y)) * scaler;
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 : 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
// Multiply pitch rate error by _ki_rate and integrate
// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
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
if (!stabilize) {
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
//only integrate if gain and time step are positive and airspeed above min value.
if ((fabsf(_ki_rate) > 0) && (dt > 0) && (aspeed > float(aspd_min)))
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
float integrator_delta = rate_error * _ki_rate * scaler * delta_time;
APM_Control: support inverted flight in AP_PitchController auto-reverse pitch control when inverted. This is useful not just for inverted flight mode, but also for recovering from poor manual flight Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2013-05-05 07:12:27 -03:00
if (_last_out < -45) {
// prevent the integrator from increasing if surface defln demand is above the upper limit
integrator_delta = max(integrator_delta , 0);
} else if (_last_out > 45) {
// prevent the integrator from decreasing if surface defln demand is below the lower limit
integrator_delta = min(integrator_delta , 0);
}
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
_integrator += integrator_delta;
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
}
} else {
_integrator = 0;
}
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
// Calculate the demanded control surface deflection
// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
// path, but want a 1/speed^2 scaler applied to the rate error path.
// This is because acceleration scales with speed^2, but rate scales with speed.
APM_Control: fixed build and merge errors
2013-05-05 01:34:33 -03:00
_last_out = ( (rate_error * _kp_rate) + _integrator + (desired_rate * _kp_ff) ) * scaler;
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
// Convert to centi-degrees and constrain
APM_Control: fixed build and merge errors
2013-05-05 01:34:33 -03:00
return constrain_float(_last_out * 100, -4500, 4500);
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 AP_PitchController::reset_I()
{
_integrator = 0;
}
Reference in New Issue Copy Permalink