From 53ffde30c257a00f4760e93cfb3b02bac085f682 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 4 Jan 2015 12:29:15 +0100 Subject: fw att: add param to select yawrate control method --- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 36 ++++++++++++++++++++++++-- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 10 +++++++ 2 files changed, 44 insertions(+), 2 deletions(-) (limited to 'src/lib') diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 82ba2c6c7..9c386a3e8 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -45,10 +45,12 @@ #include #include #include +#include ECL_YawController::ECL_YawController() : ECL_Controller("yaw"), - _coordinated_min_speed(1.0f) + _coordinated_min_speed(1.0f), + _coordinated_method(0) { } @@ -57,6 +59,36 @@ ECL_YawController::~ECL_YawController() } float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data) +{ + switch (_coordinated_method) { + case 0: + return control_attitude_impl_openloop(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 0: + 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); +} + +float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ if (!(isfinite(ctl_data.roll) && @@ -109,7 +141,7 @@ 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) +float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ if (!(isfinite(ctl_data.roll) && isfinite(ctl_data.pitch) && isfinite(ctl_data.pitch_rate) && diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 37c03705e..ab7dce2b1 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -62,6 +62,7 @@ public: ~ECL_YawController(); float control_attitude(const struct ECL_ControlData &ctl_data); + float control_bodyrate(const struct ECL_ControlData &ctl_data); /* Additional setters */ @@ -69,8 +70,17 @@ public: _coordinated_min_speed = coordinated_min_speed; } + void set_coordinated_method(int32_t coordinated_method) { + _coordinated_method = coordinated_method; + } + protected: float _coordinated_min_speed; + int32_t _coordinated_method;; + + float control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data); + float control_bodyrate_impl(const struct ECL_ControlData &ctl_data); + }; #endif // ECL_YAW_CONTROLLER_H -- cgit v1.2.3