dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

run code style fixer tool on ecl attitude lib

This commit is contained in:
Thomas Gubler 2015-01-05 07:41:06 +01:00 committed by Lorenz Meier
parent edc5f8a057
commit e5e42650c4
5 changed files with 129 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
View File

@ -61,11 +61,12 @@ ECL_PitchController::~ECL_PitchController()
float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data)
{
float roll = ctl_data.roll;
/* Do not calculate control signal with bad inputs */
if (!(isfinite(ctl_data.pitch_setpoint) &&
isfinite(roll) &&
isfinite(ctl_data.pitch) &&
isfinite(ctl_data.airspeed))) {
isfinite(roll) &&
isfinite(ctl_data.pitch) &&
isfinite(ctl_data.airspeed))) {
perf_count(_nonfinite_input_perf);
warnx("not controlling pitch");
return _rate_setpoint;
@ -78,6 +79,7 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
if (fabsf(roll) < math::radians(90.0f)) {
/* not inverted, but numerically still potentially close to infinity */
roll = math::constrain(roll, math::radians(-80.0f), math::radians(80.0f));
} else {
/* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */
@ -85,6 +87,7 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
if (roll > 0.0f) {
/* right hemisphere */
roll = math::constrain(roll, math::radians(100.0f), math::radians(180.0f));
} else {
/* left hemisphere */
roll = math::constrain(roll, math::radians(-100.0f), math::radians(-180.0f));
@ -94,9 +97,11 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
/* calculate the offset in the rate resulting from rolling */
//xxx needs explanation and conversion to body angular rates or should be removed
float turn_offset = fabsf((CONSTANTS_ONE_G / ctl_data.airspeed) *
tanf(roll) * sinf(roll)) * _roll_ff;
if (inverted)
tanf(roll) * sinf(roll)) * _roll_ff;
if (inverted) {
turn_offset = -turn_offset;
}
/* Calculate the error */
float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
@ -108,9 +113,11 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
_rate_setpoint += turn_offset;
/* limit the rate */ //XXX: move to body angluar rates
if (_max_rate > 0.01f && _max_rate_neg > 0.01f) {
if (_rate_setpoint > 0.0f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
} else {
_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
}
@ -124,13 +131,13 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(ctl_data.roll) &&
isfinite(ctl_data.pitch) &&
isfinite(ctl_data.pitch_rate) &&
isfinite(ctl_data.yaw_rate) &&
isfinite(ctl_data.yaw_rate_setpoint) &&
isfinite(ctl_data.airspeed_min) &&
isfinite(ctl_data.airspeed_max) &&
isfinite(ctl_data.scaler))) {
isfinite(ctl_data.pitch) &&
isfinite(ctl_data.pitch_rate) &&
isfinite(ctl_data.yaw_rate) &&
isfinite(ctl_data.yaw_rate_setpoint) &&
isfinite(ctl_data.airspeed_min) &&
isfinite(ctl_data.airspeed_max) &&
isfinite(ctl_data.scaler))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
@ -142,25 +149,29 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
/* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000)
if (dt_micros > 500000) {
lock_integrator = true;
}
/* input conditioning */
float airspeed = ctl_data.airspeed;
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
} else if (airspeed < ctl_data.airspeed_min) {
airspeed = ctl_data.airspeed_min;
}
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
/* Transform estimation to body angular rates (jacobian) */
float pitch_bodyrate = cosf(ctl_data.roll) * ctl_data.pitch_rate +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate;
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate;
_rate_error = _bodyrate_setpoint - pitch_bodyrate;
@ -174,6 +185,7 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
@ -188,8 +200,8 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
/* Apply PI rate controller and store non-limited output */
_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ integrator_constrained; //scaler is proportional to 1/airspeed
_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ integrator_constrained; //scaler is proportional to 1/airspeed
// warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
// warnx("roll: _last_output %.4f", (double)_last_output);
return math::constrain(_last_output, -1.0f, 1.0f);

9
src/lib/ecl/attitude_fw/ecl_pitch_controller.h
View File

@ -66,15 +66,18 @@ public:
float control_bodyrate(const struct ECL_ControlData &ctl_data);
/* Additional Setters */
void set_max_rate_pos(float max_rate_pos) {
void set_max_rate_pos(float max_rate_pos)
{
_max_rate = max_rate_pos;
}
void set_max_rate_neg(float max_rate_neg) {
void set_max_rate_neg(float max_rate_neg)
{
_max_rate_neg = max_rate_neg;
}
void set_roll_ff(float roll_ff) {
void set_roll_ff(float roll_ff)
{
_roll_ff = roll_ff;
}

24
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
View File

@ -71,6 +71,7 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat
_rate_setpoint = roll_error / _tc;
/* limit the rate */ //XXX: move to body angluar rates
if (_max_rate > 0.01f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
@ -83,12 +84,12 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(ctl_data.pitch) &&
isfinite(ctl_data.roll_rate) &&
isfinite(ctl_data.yaw_rate) &&
isfinite(ctl_data.yaw_rate_setpoint) &&
isfinite(ctl_data.airspeed_min) &&
isfinite(ctl_data.airspeed_max) &&
isfinite(ctl_data.scaler))) {
isfinite(ctl_data.roll_rate) &&
isfinite(ctl_data.yaw_rate) &&
isfinite(ctl_data.yaw_rate_setpoint) &&
isfinite(ctl_data.airspeed_min) &&
isfinite(ctl_data.airspeed_max) &&
isfinite(ctl_data.scaler))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
@ -100,14 +101,18 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
/* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000)
if (dt_micros > 500000) {
lock_integrator = true;
}
/* input conditioning */
float airspeed = ctl_data.airspeed;
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
} else if (airspeed < ctl_data.airspeed_min) {
airspeed = ctl_data.airspeed_min;
}
@ -131,6 +136,7 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
@ -146,8 +152,8 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
/* Apply PI rate controller and store non-limited output */
_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ integrator_constrained; //scaler is proportional to 1/airspeed
_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ integrator_constrained; //scaler is proportional to 1/airspeed
return math::constrain(_last_output, -1.0f, 1.0f);
}

134
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
View File

@ -61,33 +61,40 @@ ECL_YawController::~ECL_YawController()
float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data)
{
switch (_coordinated_method) {
case COORD_METHOD_OPEN:
return control_attitude_impl_openloop(ctl_data);
case COORD_METHOD_CLOSEACC:
return control_attitude_impl_accclosedloop(ctl_data);
default:
static hrt_abstime last_print = 0;
if (hrt_elapsed_time(&last_print) > 5e6) {
warnx("invalid param setting FW_YCO_METHOD");
last_print = hrt_absolute_time();
}
case COORD_METHOD_OPEN:
return control_attitude_impl_openloop(ctl_data);
case COORD_METHOD_CLOSEACC:
return control_attitude_impl_accclosedloop(ctl_data);
default:
static hrt_abstime last_print = 0;
if (hrt_elapsed_time(&last_print) > 5e6) {
warnx("invalid param setting FW_YCO_METHOD");
last_print = hrt_absolute_time();
}
}
return _rate_setpoint;
}
float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
switch (_coordinated_method) {
case COORD_METHOD_OPEN:
case COORD_METHOD_CLOSEACC:
return control_bodyrate_impl(ctl_data);
default:
static hrt_abstime last_print = 0;
if (hrt_elapsed_time(&last_print) > 5e6) {
warnx("invalid param setting FW_YCO_METHOD");
last_print = hrt_absolute_time();
}
case COORD_METHOD_OPEN:
case COORD_METHOD_CLOSEACC:
return control_bodyrate_impl(ctl_data);
default:
static hrt_abstime last_print = 0;
if (hrt_elapsed_time(&last_print) > 5e6) {
warnx("invalid param setting FW_YCO_METHOD");
last_print = hrt_absolute_time();
}
}
return math::constrain(_last_output, -1.0f, 1.0f);
}
@ -95,12 +102,12 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(ctl_data.roll) &&
isfinite(ctl_data.pitch) &&
isfinite(ctl_data.speed_body_u) &&
isfinite(ctl_data.speed_body_v) &&
isfinite(ctl_data.speed_body_w) &&
isfinite(ctl_data.roll_rate_setpoint) &&
isfinite(ctl_data.pitch_rate_setpoint))) {
isfinite(ctl_data.pitch) &&
isfinite(ctl_data.speed_body_u) &&
isfinite(ctl_data.speed_body_v) &&
isfinite(ctl_data.speed_body_w) &&
isfinite(ctl_data.roll_rate_setpoint) &&
isfinite(ctl_data.pitch_rate_setpoint))) {
perf_count(_nonfinite_input_perf);
return _rate_setpoint;
}
@ -108,16 +115,17 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
// static int counter = 0;
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_rate_setpoint = 0.0f;
if (sqrtf(ctl_data.speed_body_u * ctl_data.speed_body_u + ctl_data.speed_body_v * ctl_data.speed_body_v +
ctl_data.speed_body_w * ctl_data.speed_body_w) > _coordinated_min_speed) {
float denumerator = (ctl_data.speed_body_u * cosf(ctl_data.roll) * cosf(ctl_data.pitch) +
ctl_data.speed_body_w * sinf(ctl_data.pitch));
if(fabsf(denumerator) > FLT_EPSILON) {
if (sqrtf(ctl_data.speed_body_u * ctl_data.speed_body_u + ctl_data.speed_body_v * ctl_data.speed_body_v +
ctl_data.speed_body_w * ctl_data.speed_body_w) > _coordinated_min_speed) {
float denumerator = (ctl_data.speed_body_u * cosf(ctl_data.roll) * cosf(ctl_data.pitch) +
ctl_data.speed_body_w * sinf(ctl_data.pitch));
if (fabsf(denumerator) > FLT_EPSILON) {
_rate_setpoint = (ctl_data.speed_body_w * ctl_data.roll_rate_setpoint +
9.81f * sinf(ctl_data.roll) * cosf(ctl_data.pitch) +
ctl_data.speed_body_u * ctl_data.pitch_rate_setpoint * sinf(ctl_data.roll)) /
denumerator;
9.81f * sinf(ctl_data.roll) * cosf(ctl_data.pitch) +
ctl_data.speed_body_u * ctl_data.pitch_rate_setpoint * sinf(ctl_data.roll)) /
denumerator;
// warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint);
}
@ -128,15 +136,16 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
}
/* limit the rate */ //XXX: move to body angluar rates
if (_max_rate > 0.01f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
}
// counter++;
if(!isfinite(_rate_setpoint)){
if (!isfinite(_rate_setpoint)) {
warnx("yaw rate sepoint not finite");
_rate_setpoint = 0.0f;
}
@ -148,9 +157,9 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(ctl_data.roll) && isfinite(ctl_data.pitch) && isfinite(ctl_data.pitch_rate) &&
isfinite(ctl_data.yaw_rate) && isfinite(ctl_data.pitch_rate_setpoint) &&
isfinite(ctl_data.airspeed_min) && isfinite(ctl_data.airspeed_max) &&
isfinite(ctl_data.scaler))) {
isfinite(ctl_data.yaw_rate) && isfinite(ctl_data.pitch_rate_setpoint) &&
isfinite(ctl_data.airspeed_min) && isfinite(ctl_data.airspeed_max) &&
isfinite(ctl_data.scaler))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
@ -162,22 +171,26 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl
/* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000)
if (dt_micros > 500000) {
lock_integrator = true;
}
/* input conditioning */
float airspeed = ctl_data.airspeed;
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
} else if (airspeed < ctl_data.airspeed_min) {
airspeed = ctl_data.airspeed_min;
airspeed = ctl_data.airspeed_min;
}
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
cosf(ctl_data.roll)*cosf(ctl_data.pitch) * _rate_setpoint;
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint;
/* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */
if (_coordinated_method == COORD_METHOD_CLOSEACC) {
@ -187,27 +200,28 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl
/* Transform estimation to body angular rates (jacobian) */
float yaw_bodyrate = -sinf(ctl_data.roll) * ctl_data.pitch_rate +
cosf(ctl_data.roll)*cosf(ctl_data.pitch) * ctl_data.yaw_rate;
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * ctl_data.yaw_rate;
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
float id = _rate_error * dt;
float id = _rate_error * dt;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
_integrator += id;
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
_integrator += id;
}
/* integrator limit */
@ -215,14 +229,16 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler * ctl_data.scaler; //scaler is proportional to 1/airspeed
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler *
ctl_data.scaler; //scaler is proportional to 1/airspeed
//warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data) {
float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data)
{
/* dont set a rate setpoint */
return 0.0f;
}

6
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
View File

@ -66,11 +66,13 @@ public:
float control_bodyrate(const struct ECL_ControlData &ctl_data);
/* Additional setters */
void set_coordinated_min_speed(float coordinated_min_speed) {
void set_coordinated_min_speed(float coordinated_min_speed)
{
_coordinated_min_speed = coordinated_min_speed;
}
void set_coordinated_method(int32_t coordinated_method) {
void set_coordinated_method(int32_t coordinated_method)
{
_coordinated_method = coordinated_method;
}