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

APM_Control: tweak code layout and param descriptions

This commit is contained in:
Andrew Tridgell 2013-05-31 17:21:20 +10:00
parent d875e95da3
commit 14c8c1bdfc
2 changed files with 23 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/APM_Control/AP_PitchController.cpp
View File

@ -18,7 +18,7 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = {
// @Param: TCONST // @Param: TCONST
// @DisplayName: Pitch Time Constant // @DisplayName: Pitch Time Constant
// @Description: This controls the time constant in seconds from demanded to achieved pitch angle. A value of 0.7 is a good default and will work with nearly all models. Advanced users may want to reduce this time to obtain a faster response but there is no point setting a time less than the aircraft can achieve. // @Description: This controls the time constant in seconds from demanded to achieved pitch angle. A value of 0.5 is a good default and will work with nearly all models. Advanced users may want to reduce this time to obtain a faster response but there is no point setting a time less than the aircraft can achieve.
// @Range: 0.4 1.0 // @Range: 0.4 1.0
// @Units: seconds // @Units: seconds
// @Increment: 0.1 // @Increment: 0.1
@ -167,8 +167,7 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab
// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs // Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
if (!stabilize) { if (!stabilize) {
//only integrate if gain and time step are positive and airspeed above min value. //only integrate if gain and time step are positive and airspeed above min value.
if ((ki_rate > 0) && (dt > 0) && (aspeed > float(aspd_min))) if ((ki_rate > 0) && (dt > 0) && (aspeed > float(aspd_min))) {
{
float integrator_delta = rate_error * ki_rate * scaler * delta_time; float integrator_delta = rate_error * ki_rate * scaler * delta_time;
if (_last_out < -45) { if (_last_out < -45) {
// prevent the integrator from increasing if surface defln demand is above the upper limit // prevent the integrator from increasing if surface defln demand is above the upper limit

32
libraries/APM_Control/AP_RollController.cpp
View File

@ -17,7 +17,7 @@ extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = {
// @Param: T_CONST // @Param: T_CONST
// @DisplayName: Pitch Time Constant // @DisplayName: Pitch Time Constant
// @Description: This controls the time constant in seconds from demanded to achieved bank angle. A value of 0.7 is a good default and will work with nearly all models. Advanced users may want to reduce this time to obtain a faster response but there is no point setting a time less than the aircraft can achieve. // @Description: This controls the time constant in seconds from demanded to achieved bank angle. A value of 0.5 is a good default and will work with nearly all models. Advanced users may want to reduce this time to obtain a faster response but there is no point setting a time less than the aircraft can achieve.
// @Range: 0.4 1.0 // @Range: 0.4 1.0
// @Units: seconds // @Units: seconds
// @Increment: 0.1 // @Increment: 0.1
@ -82,8 +82,11 @@ int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabi
float kp_ff = max((_K_P - _K_I * _tau) * _tau - _K_D , 0); float kp_ff = max((_K_P - _K_I * _tau) * _tau - _K_D , 0);
float ki_rate = _K_I * _tau; float ki_rate = _K_I * _tau;
if(_ahrs == NULL) return 0; // can't control without a reference if (_ahrs == NULL) {
float delta_time = (float)dt / 1000.0f; // can't control without a reference
return 0;
}
float delta_time = (float)dt * 0.001f;
// Calculate bank angle error in centi-degrees // Calculate bank angle error in centi-degrees
int32_t angle_err = angle - _ahrs->roll_sensor; int32_t angle_err = angle - _ahrs->roll_sensor;
@ -92,8 +95,11 @@ int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabi
float desired_rate = angle_err * 0.01f / _tau; float desired_rate = angle_err * 0.01f / _tau;
// Limit the demanded roll rate // Limit the demanded roll rate
if (_max_rate && desired_rate < -_max_rate) desired_rate = -_max_rate; if (_max_rate && desired_rate < -_max_rate) {
else if (_max_rate && desired_rate > _max_rate) desired_rate = _max_rate; desired_rate = -_max_rate;
} else if (_max_rate && desired_rate > _max_rate) {
desired_rate = _max_rate;
}
// Get body rate vector (radians/sec) // Get body rate vector (radians/sec)
float omega_x = _ahrs->get_gyro().x; float omega_x = _ahrs->get_gyro().x;
@ -103,19 +109,23 @@ int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabi
// Get an airspeed estimate - default to zero if none available // Get an airspeed estimate - default to zero if none available
float aspeed; float aspeed;
if (!_ahrs->airspeed_estimate(&aspeed)) aspeed = 0.0f; if (!_ahrs->airspeed_estimate(&aspeed)) {
aspeed = 0.0f;
}
// Multiply roll rate error by _ki_rate and integrate // Multiply roll rate error by _ki_rate and integrate
// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs // Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
if (!stabilize) { if (!stabilize) {
//only integrate if gain and time step are positive and airspeed above min value. //only integrate if gain and time step are positive and airspeed above min value.
if ((ki_rate > 0) && (dt > 0) && (aspeed > float(aspd_min))) if ((ki_rate > 0) && (dt > 0) && (aspeed > float(aspd_min))) {
{
float integrator_delta = rate_error * ki_rate * scaler * delta_time; float integrator_delta = rate_error * ki_rate * scaler * delta_time;
// prevent the integrator from increasing if surface defln demand is above the upper limit // prevent the integrator from increasing if surface defln demand is above the upper limit
if (_last_out < -45) integrator_delta = max(integrator_delta , 0); if (_last_out < -45) {
// prevent the integrator from decreasing if surface defln demand is below the lower limit integrator_delta = max(integrator_delta , 0);
else if (_last_out > 45) integrator_delta = min(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);
}
_integrator += integrator_delta; _integrator += integrator_delta;
} }
} else { } else {