From cc778a68ae74aaa7f87fa0994b7b3cfbf76e46c2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 13 Jul 2013 20:27:48 +1000 Subject: [PATCH] APM_Control: changed attitude controllers to take angular error not angle this makes it easier for ACRO mode --- libraries/APM_Control/AP_PitchController.cpp | 12 +++++++----- libraries/APM_Control/AP_PitchController.h | 2 +- libraries/APM_Control/AP_RollController.cpp | 5 +---- libraries/APM_Control/AP_RollController.h | 2 +- 4 files changed, 10 insertions(+), 11 deletions(-) diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index e4bbf5b067..e22282bcd7 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -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; diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index 7586fa514c..5433081d13 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -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(); diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index 1051ba7169..a68619d908 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -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; } diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index e24332252c..49a4c50333 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -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();