Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_Control: changed attitude controllers to take angular error not angle

Browse Source 
this makes it easier for ACRO mode
This commit is contained in:
Andrew Tridgell 2013-07-13 20:27:48 +10:00
parent b721bcc129
commit cc778a68ae
4 changed files with 10 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/APM_Control/AP_PitchController.cpp
View File

@ -179,7 +179,7 @@ int32_t AP_PitchController::get_rate_out(float desired_rate, float scaler)
// 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)
int32_t AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool stabilize, int16_t aspd_min, int16_t aspd_max)
{
// 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
@ -208,14 +208,16 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab
// If no airspeed available use average of min and max
aspeed = 0.5f*(float(aspd_min) + float(aspd_max));
}
rate_offset = fabsf(ToDeg((GRAVITY_MSS / max(aspeed , float(aspd_min))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff;
if (abs(_ahrs->pitch_sensor) > 7000) {
// don't do turn coordination handling when at very high pitch angles
rate_offset = 0;
} else {
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;
}
//Calculate pitch angle error in centi-degrees
int32_t angle_err = angle - _ahrs->pitch_sensor;
// Calculate the desired pitch rate (deg/sec) from the angle error
float desired_rate = angle_err * 0.01f / _tau;

2
libraries/APM_Control/AP_PitchController.h
View File

@ -16,7 +16,7 @@ public:
void set_ahrs(AP_AHRS *ahrs) { _ahrs = ahrs; }
int32_t get_rate_out(float desired_rate, float scaler = 1.0);
int32_t get_servo_out(int32_t angle, float scaler = 1.0, bool stabilize = false, int16_t aspd_min = 0, int16_t aspd_max = 0);
int32_t get_servo_out(int32_t angle_err, float scaler = 1.0, bool stabilize = false, int16_t aspd_min = 0, int16_t aspd_max = 0);
void reset_I();

5
libraries/APM_Control/AP_RollController.cpp
View File

@ -169,11 +169,8 @@ int32_t AP_RollController::get_rate_out(float desired_rate, float scaler)
3) boolean which is true when stabilise mode is active
4) minimum FBW airspeed (metres/sec)
*/
int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabilize, int16_t aspd_min)
int32_t AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool stabilize, int16_t aspd_min)
{
// Calculate bank angle error in centi-degrees
int32_t angle_err = angle - _ahrs->roll_sensor;
if (_tau < 0.1) {
_tau = 0.1;
}

2
libraries/APM_Control/AP_RollController.h
View File

@ -16,7 +16,7 @@ public:
void set_ahrs(AP_AHRS *ahrs) { _ahrs = ahrs; }
int32_t get_rate_out(float desired_rate, float scaler=1.0);
int32_t get_servo_out(int32_t angle, float scaler=1.0, bool stabilize=false, int16_t aspd_min = 0);
int32_t get_servo_out(int32_t angle_err, float scaler=1.0, bool stabilize=false, int16_t aspd_min = 0);
void reset_I();