From 6c31421889f5bc4fe1b389a72713cd12274774fe Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 23 Sep 2015 12:30:38 +0200 Subject: [PATCH] extracted heading controller --- .../attitude_fw/ecl_heading_controller.cpp | 163 ++++++++++++++++++ .../ecl/attitude_fw/ecl_heading_controller.h | 70 ++++++++ .../ecl/attitude_fw/ecl_yaw_controller.cpp | 34 ---- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 5 +- src/lib/runway_takeoff/RunwayTakeoff.cpp | 4 +- .../fw_att_control/fw_att_control_main.cpp | 26 ++- .../fw_pos_control_l1_main.cpp | 19 +- 7 files changed, 265 insertions(+), 56 deletions(-) create mode 100644 src/lib/ecl/attitude_fw/ecl_heading_controller.cpp create mode 100644 src/lib/ecl/attitude_fw/ecl_heading_controller.h diff --git a/src/lib/ecl/attitude_fw/ecl_heading_controller.cpp b/src/lib/ecl/attitude_fw/ecl_heading_controller.cpp new file mode 100644 index 0000000000..97dfa2f617 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_heading_controller.cpp @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ecl_yaw_controller.cpp + * Implementation of a simple orthogonal coordinated turn yaw PID controller. + * + * Authors and acknowledgements in header. + */ + +#include "ecl_heading_controller.h" +#include +#include +#include +#include +#include +#include +#include + +ECL_HeadingController::ECL_HeadingController() : + ECL_Controller("heading") +{ +} + +ECL_HeadingController::~ECL_HeadingController() +{ +} + +float ECL_HeadingController::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) && + PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { + perf_count(_nonfinite_input_perf); + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + bool lock_integrator = ctl_data.lock_integrator; + + if (dt_micros > 500000) { + lock_integrator = true; + } + + /* input conditioning */ + float airspeed = ctl_data.airspeed; + + if (!PX4_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 = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint + + cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint; + + /* Calculate body angular rate error */ + _rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //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); + } + + _integrator += id; + } + + /* integrator limit */ + //xxx: until start detection is available: integral part in control signal is limited here + 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 + 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_HeadingController::control_attitude(const struct ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && + PX4_ISFINITE(ctl_data.yaw) && + PX4_ISFINITE(ctl_data.airspeed))) { + perf_count(_nonfinite_input_perf); + warnx("not controlling yaw"); + return _rate_setpoint; + } + + /* Calculate the error */ + float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; + + /* Apply P controller: rate setpoint from current error and time constant */ + _rate_setpoint = yaw_error / _tc; + + /* limit the rate */ + if (_max_rate > 0.01f) { + if (_rate_setpoint > 0.0f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + + } else { + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + } + + return _rate_setpoint; +} diff --git a/src/lib/ecl/attitude_fw/ecl_heading_controller.h b/src/lib/ecl/attitude_fw/ecl_heading_controller.h new file mode 100644 index 0000000000..5dd79cd7d4 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_heading_controller.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ecl_yaw_controller.h + * Definition of a simple orthogonal coordinated turn yaw PID controller. + * + * @author Lorenz Meier + * @author Thomas Gubler + * @author Andreas Antener + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ +#ifndef ECL_HEADING_CONTROLLER_H +#define ECL_HEADING_CONTROLLER_H + +#include +#include + +#include "ecl_controller.h" + +class __EXPORT ECL_HeadingController : + public ECL_Controller +{ +public: + ECL_HeadingController(); + + ~ECL_HeadingController(); + + float control_attitude(const struct ECL_ControlData &ctl_data); + + float control_bodyrate(const struct ECL_ControlData &ctl_data); +}; + +#endif // ECL_HEADING_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 240a426bd0..4849bdad79 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -67,9 +67,6 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data case COORD_METHOD_CLOSEACC: return control_attitude_impl_accclosedloop(ctl_data); - case COORD_METHOD_HEADING: - return control_heading(ctl_data); - default: static hrt_abstime last_print = 0; @@ -241,34 +238,3 @@ float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_Co /* dont set a rate setpoint */ return 0.0f; } - -float ECL_YawController::control_heading(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && - PX4_ISFINITE(ctl_data.yaw) && - PX4_ISFINITE(ctl_data.airspeed))) { - perf_count(_nonfinite_input_perf); - warnx("not controlling yaw"); - return _rate_setpoint; - } - - /* Calculate the error */ - float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; - - /* Apply P controller: rate setpoint from current error and time constant */ - _rate_setpoint = yaw_error / _tc; - - /* limit the rate */ - if (_max_rate > 0.01f) { - if (_rate_setpoint > 0.0f) { - _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; - - } else { - _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; - } - - } - - return _rate_setpoint; -} diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index e3e529b30f..511b5fd36f 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -78,8 +78,7 @@ public: enum { COORD_METHOD_OPEN = 0, - COORD_METHOD_CLOSEACC = 1, - COORD_METHOD_HEADING = 2 + COORD_METHOD_CLOSEACC = 1 }; protected: @@ -94,8 +93,6 @@ protected: float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data); - float control_heading(const struct ECL_ControlData &ctl_data); - }; #endif // ECL_YAW_CONTROLLER_H diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index e1814cecc1..d7c33d7094 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -105,7 +105,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::CLAMPED_TO_RUNWAY: if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { _state = RunwayTakeoffState::TAKEOFF; - mavlink_log_info(mavlink_fd, "#audio: Takeoff airspeed reached"); + mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached"); } break; @@ -113,7 +113,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::TAKEOFF: if (alt_agl > math::max(_runway_takeoff_nav_alt.get(), _climbout_diff.get())) { _state = RunwayTakeoffState::FLY; - mavlink_log_info(mavlink_fd, "#audio: Navigating to waypoint"); + mavlink_log_info(mavlink_fd, "#Navigating to waypoint"); } break; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index de8ee9173e..938347ff95 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -82,6 +82,7 @@ #include #include #include +#include #include /** @@ -248,6 +249,7 @@ private: ECL_RollController _roll_ctrl; ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; + ECL_HeadingController _heading_ctrl; /** @@ -502,6 +504,13 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); + /* heading control parameters */ + _heading_ctrl.set_k_p(_parameters.y_p); + _heading_ctrl.set_k_i(_parameters.y_i); + _heading_ctrl.set_k_ff(_parameters.y_ff); + _heading_ctrl.set_integrator_max(_parameters.y_integrator_max); + _heading_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); + return OK; } @@ -961,18 +970,14 @@ FixedwingAttitudeControl::task_main() control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; - if (_att_sp.fw_control_yaw == true) { - // this method controls heading directly with rudder. Used for auto takeoff on runway - _yaw_ctrl.set_coordinated_method(ECL_YawController::COORD_METHOD_HEADING); - } else { - _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); - } + _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); /* Run attitude controllers */ if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) { _roll_ctrl.control_attitude(control_input); _pitch_ctrl.control_attitude(control_input); _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude + _heading_ctrl.control_attitude(control_input); /* Update input data for rate controllers */ control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); @@ -1012,7 +1017,14 @@ FixedwingAttitudeControl::task_main() } } - float yaw_u = _yaw_ctrl.control_bodyrate(control_input); + float yaw_u = 0.0f; + if (_att_sp.fw_control_yaw == true) { + yaw_u = _heading_ctrl.control_bodyrate(control_input); + } + + else { + yaw_u = _yaw_ctrl.control_bodyrate(control_input); + } _actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; /* add in manual rudder control */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 34f414f0d6..7f7d0b15b3 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -960,7 +960,7 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint * for the whole landing */ if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) { if(!land_useterrain) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate"); + mavlink_log_info(_mavlink_fd, "Landing, using terrain estimate"); land_useterrain = true; } return global_pos.terrain_alt; @@ -1229,7 +1229,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { target_bearing = _yaw; } - mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold"); + mavlink_log_info(_mavlink_fd, "#Landing, heading hold"); } // warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw)); @@ -1290,7 +1290,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; - mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle"); + mavlink_log_info(_mavlink_fd, "#Landing, limiting throttle"); } } @@ -1314,7 +1314,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); if (!land_noreturn_vertical) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); + mavlink_log_info(_mavlink_fd, "#Landing, flaring"); land_noreturn_vertical = true; } //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); @@ -1336,7 +1336,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); + mavlink_log_info(_mavlink_fd, "#Landing, on slope"); land_onslope = true; } } else { @@ -1367,7 +1367,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ _takeoff_ground_alt = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "#audio: Takeoff on runway"); + mavlink_log_info(_mavlink_fd, "#Takeoff on runway"); } // update navigation @@ -1417,7 +1417,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Inform user that launchdetection is running */ static hrt_abstime last_sent = 0; if(hrt_absolute_time() - last_sent > 4e6) { - mavlink_log_critical(_mavlink_fd, "Launchdetection running"); + mavlink_log_critical(_mavlink_fd, "#Launchdetection running"); last_sent = hrt_absolute_time(); } @@ -1509,7 +1509,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* reset takeoff/launch state */ - // FIXME: reset on arm/disarm cycle and mode switch if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { reset_takeoff_state(); } @@ -1750,7 +1749,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi * already (not by tecs) */ if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - launch_detection_state == LAUNCHDETECTION_RES_NONE)) { + (launch_detection_state == LAUNCHDETECTION_RES_NONE || + _runway_takeoff.runwayTakeoffEnabled()) + )) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); }