diff options
Diffstat (limited to 'src/lib')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 46 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 9 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 24 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 132 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 6 |
5 files changed, 128 insertions, 89 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index add884146..ca454fa62 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -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); diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 47b5605e9..fa7612cb9 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -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; } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 0606c87cb..5d0846ac3 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -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); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index a87906749..f3379b245 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -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) { + 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)); + ctl_data.speed_body_w * sinf(ctl_data.pitch)); - if(fabsf(denumerator) > FLT_EPSILON) { + 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; - - /* - * 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); - } + 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); - _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; } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index b108ad98d..81d821054 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -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; } |