From ac7fc30ea73988dab0663623bf2c31d74c165f09 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 24 Jan 2017 15:25:21 +0100 Subject: [PATCH] fw attitude controllers: restructuring in order to allow acro flight mode - split rate control into body rate control and euler rate control - this allows standard attitude control but also direct rate control of all three axes - allow activation/deactivation of turn compensation Signed-off-by: Roman --- attitude_fw/ecl_controller.h | 9 +++-- attitude_fw/ecl_pitch_controller.cpp | 46 +++++++++++++---------- attitude_fw/ecl_pitch_controller.h | 1 + attitude_fw/ecl_roll_controller.cpp | 21 +++++++---- attitude_fw/ecl_roll_controller.h | 1 + attitude_fw/ecl_wheel_controller.cpp | 16 ++++---- attitude_fw/ecl_wheel_controller.h | 7 +++- attitude_fw/ecl_yaw_controller.cpp | 56 ++++++++-------------------- attitude_fw/ecl_yaw_controller.h | 3 +- 9 files changed, 77 insertions(+), 83 deletions(-) diff --git a/attitude_fw/ecl_controller.h b/attitude_fw/ecl_controller.h index 2b9d1b6761..2612152950 100644 --- a/attitude_fw/ecl_controller.h +++ b/attitude_fw/ecl_controller.h @@ -56,9 +56,9 @@ struct ECL_ControlData { float roll; float pitch; float yaw; - float roll_rate; - float pitch_rate; - float yaw_rate; + float body_x_rate; + float body_y_rate; + float body_z_rate; float speed_body_u; float speed_body_v; float speed_body_w; @@ -78,6 +78,7 @@ struct ECL_ControlData { float groundspeed; float groundspeed_scaler; bool lock_integrator; + bool do_turn_compensation; }; class __EXPORT ECL_Controller @@ -87,6 +88,7 @@ public: ~ECL_Controller() = default; virtual float control_attitude(const struct ECL_ControlData &ctl_data) = 0; + virtual float control_euler_rate(const struct ECL_ControlData &ctl_data) = 0; virtual float control_bodyrate(const struct ECL_ControlData &ctl_data) = 0; /* Setters */ @@ -96,6 +98,7 @@ public: void set_k_ff(float k_ff); void set_integrator_max(float max); void set_max_rate(float max_rate); + void set_bodyrate_setpoint(float rate) {_bodyrate_setpoint = rate;}; /* Getters */ float get_rate_error(); diff --git a/attitude_fw/ecl_pitch_controller.cpp b/attitude_fw/ecl_pitch_controller.cpp index 30ab2b2e85..5a726f7be7 100644 --- a/attitude_fw/ecl_pitch_controller.cpp +++ b/attitude_fw/ecl_pitch_controller.cpp @@ -91,8 +91,8 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da /* Do not calculate control signal with bad inputs */ if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.pitch_rate) && - PX4_ISFINITE(ctl_data.yaw_rate) && + PX4_ISFINITE(ctl_data.body_y_rate) && + PX4_ISFINITE(ctl_data.body_z_rate) && PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) && @@ -112,10 +112,6 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da lock_integrator = true; } - /* 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; - /* apply turning offset to desired bodyrate setpoint*/ /* flying inverted (wings upside down)*/ bool inverted = false; @@ -144,22 +140,24 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da /* input conditioning */ float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max); - /* Calculate desired body fixed y-axis angular rate needed to compensate for roll angle. - For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175 - Availible on google books 8/11/2015: - https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/ - float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) * - tanf(constrained_roll) * sinf(constrained_roll))); + if (ctl_data.do_turn_compensation) { + /* Calculate desired body fixed y-axis angular rate needed to compensate for roll angle. + For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175 + Availible on google books 8/11/2015: + https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/ + float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) * + tanf(constrained_roll) * sinf(constrained_roll))); - if (inverted) { - body_fixed_turn_offset = -body_fixed_turn_offset; + if (inverted) { + body_fixed_turn_offset = -body_fixed_turn_offset; + } + + /* Finally add the turn offset to your bodyrate setpoint*/ + _bodyrate_setpoint += body_fixed_turn_offset; } - /* Finally add the turn offset to your bodyrate setpoint*/ - _bodyrate_setpoint += body_fixed_turn_offset; - - _rate_error = _bodyrate_setpoint - ctl_data.pitch_rate; + _rate_error = _bodyrate_setpoint - ctl_data.body_y_rate; if (!lock_integrator && _k_i > 0.0f) { @@ -188,7 +186,15 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da _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 -// 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); } + +float ECL_PitchController::control_euler_rate(const struct ECL_ControlData &ctl_data) +{ + /* 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; + + return control_bodyrate(ctl_data); +} diff --git a/attitude_fw/ecl_pitch_controller.h b/attitude_fw/ecl_pitch_controller.h index 820dcdd3a4..d6ae88e66c 100644 --- a/attitude_fw/ecl_pitch_controller.h +++ b/attitude_fw/ecl_pitch_controller.h @@ -62,6 +62,7 @@ public: ~ECL_PitchController() = default; float control_attitude(const struct ECL_ControlData &ctl_data); + float control_euler_rate(const struct ECL_ControlData &ctl_data); float control_bodyrate(const struct ECL_ControlData &ctl_data); /* Additional Setters */ diff --git a/attitude_fw/ecl_roll_controller.cpp b/attitude_fw/ecl_roll_controller.cpp index 26be7c19c7..4d438bad68 100644 --- a/attitude_fw/ecl_roll_controller.cpp +++ b/attitude_fw/ecl_roll_controller.cpp @@ -79,8 +79,8 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat { /* Do not calculate control signal with bad inputs */ if (!(PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.roll_rate) && - PX4_ISFINITE(ctl_data.yaw_rate) && + PX4_ISFINITE(ctl_data.body_x_rate) && + PX4_ISFINITE(ctl_data.body_z_rate) && PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) && @@ -100,11 +100,8 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat lock_integrator = true; } - /* Transform setpoint to body angular rates (jacobian) */ - _bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint; - /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - ctl_data.roll_rate; //body angular rate error + _rate_error = _bodyrate_setpoint - ctl_data.body_x_rate; //body angular rate error if (!lock_integrator && _k_i > 0.0f) { @@ -127,8 +124,7 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat /* integrator limit */ //xxx: until start detection is available: integral part in control signal is limited here - float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max); - //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); + 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 * ctl_data.scaler + @@ -138,3 +134,12 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat return math::constrain(_last_output, -1.0f, 1.0f); } +float ECL_RollController::control_euler_rate(const struct ECL_ControlData &ctl_data) +{ + /* Transform setpoint to body angular rates (jacobian) */ + _bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint; + + return control_bodyrate(ctl_data); + +} + diff --git a/attitude_fw/ecl_roll_controller.h b/attitude_fw/ecl_roll_controller.h index f8681010d9..cb127ef029 100644 --- a/attitude_fw/ecl_roll_controller.h +++ b/attitude_fw/ecl_roll_controller.h @@ -62,6 +62,7 @@ public: ~ECL_RollController() = default; float control_attitude(const struct ECL_ControlData &ctl_data); + float control_euler_rate(const struct ECL_ControlData &ctl_data); float control_bodyrate(const struct ECL_ControlData &ctl_data); }; diff --git a/attitude_fw/ecl_wheel_controller.cpp b/attitude_fw/ecl_wheel_controller.cpp index 1a04d39211..645ffe313b 100644 --- a/attitude_fw/ecl_wheel_controller.cpp +++ b/attitude_fw/ecl_wheel_controller.cpp @@ -54,12 +54,12 @@ ECL_WheelController::ECL_WheelController() : float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data) { - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.yaw_rate) && - PX4_ISFINITE(ctl_data.groundspeed) && - PX4_ISFINITE(ctl_data.groundspeed_scaler))) { - return math::constrain(_last_output, -1.0f, 1.0f); - } + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.body_z_rate) && + PX4_ISFINITE(ctl_data.groundspeed) && + PX4_ISFINITE(ctl_data.groundspeed_scaler))) { + return math::constrain(_last_output, -1.0f, 1.0f); + } /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); @@ -76,8 +76,8 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da /* input conditioning */ float min_speed = 1.0f; - /* Calculate body angular rate error */ - _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error + /* Calculate body angular rate error */ + _rate_error = _rate_setpoint - ctl_data.body_z_rate; //body angular rate error if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) { diff --git a/attitude_fw/ecl_wheel_controller.h b/attitude_fw/ecl_wheel_controller.h index 898728c897..166899bd8f 100644 --- a/attitude_fw/ecl_wheel_controller.h +++ b/attitude_fw/ecl_wheel_controller.h @@ -61,8 +61,11 @@ public: ECL_WheelController(); ~ECL_WheelController() = default; - float control_attitude(const struct ECL_ControlData &ctl_data); - float control_bodyrate(const struct ECL_ControlData &ctl_data); + float control_attitude(const struct ECL_ControlData &ctl_data); + + float control_bodyrate(const struct ECL_ControlData &ctl_data); + + float control_euler_rate(const struct ECL_ControlData &ctl_data) {return 0;}; }; #endif // ECL_HEADING_CONTROLLER_H diff --git a/attitude_fw/ecl_yaw_controller.cpp b/attitude_fw/ecl_yaw_controller.cpp index a98b03bf7b..19ac89764d 100644 --- a/attitude_fw/ecl_yaw_controller.cpp +++ b/attitude_fw/ecl_yaw_controller.cpp @@ -76,25 +76,6 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data 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 (ecl_elapsed_time(&last_print) > 5e6) { - warnx("invalid param setting FW_YCO_METHOD"); - last_print = ecl_absolute_time(); - } - } - - return math::constrain(_last_output, -1.0f, 1.0f); -} - float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ @@ -108,7 +89,6 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control return _rate_setpoint; } -// static int counter = 0; /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ _rate_setpoint = 0.0f; @@ -122,25 +102,17 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control 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); } - -// if(counter % 20 == 0) { -// warnx("denumerator: %.4f, speed_body_u: %.4f, speed_body_w: %.4f, cosf(roll): %.4f, cosf(pitch): %.4f, sinf(pitch): %.4f", (double)denumerator, (double)speed_body_u, (double)speed_body_w, (double)cosf(roll), (double)cosf(pitch), (double)sinf(pitch)); -// } } - /* limit the rate */ //XXX: move to body angluar rates + + /* 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; } - -// counter++; - if (!PX4_ISFINITE(_rate_setpoint)) { warnx("yaw rate sepoint not finite"); _rate_setpoint = 0.0f; @@ -149,11 +121,11 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control return _rate_setpoint; } -float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl_data) +float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) && - PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) && + if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.body_y_rate) && + PX4_ISFINITE(ctl_data.body_z_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) && PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) && PX4_ISFINITE(ctl_data.scaler))) { return math::constrain(_last_output, -1.0f, 1.0f); @@ -182,11 +154,6 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl 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; - /* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */ if (_coordinated_method == COORD_METHOD_CLOSEACC) { //XXX: filtering of acceleration? @@ -194,7 +161,7 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl } /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error + _rate_error = _bodyrate_setpoint - ctl_data.body_z_rate; //body angular rate error if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { @@ -222,7 +189,6 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl /* 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 - //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); @@ -233,3 +199,13 @@ float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_Co /* dont set a rate setpoint */ return 0.0f; } + +float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_data) +{ + /* 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; + + return control_bodyrate(ctl_data); + +} \ No newline at end of file diff --git a/attitude_fw/ecl_yaw_controller.h b/attitude_fw/ecl_yaw_controller.h index 4f5e7d6e74..ef23274b7d 100644 --- a/attitude_fw/ecl_yaw_controller.h +++ b/attitude_fw/ecl_yaw_controller.h @@ -61,6 +61,7 @@ public: ~ECL_YawController() = default; float control_attitude(const struct ECL_ControlData &ctl_data); + float control_euler_rate(const struct ECL_ControlData &ctl_data); float control_bodyrate(const struct ECL_ControlData &ctl_data); /* Additional setters */ @@ -85,8 +86,6 @@ protected: int32_t _coordinated_method; - float control_bodyrate_impl(const struct ECL_ControlData &ctl_data); - float control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data); float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data);