From 3f0ea5c679fdb027ccd52db84476edc29c7bfbe0 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 ++++++ src/modules/fw_att_control/fw_att_control_main.cpp | 5 +++ src/modules/fw_att_control/fw_att_control_params.c | 13 ++++++++ 4 files changed, 62 insertions(+), 2 deletions(-) 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 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 6f225bb48..14c6c5916 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -178,6 +178,7 @@ private: float y_roll_feedforward; float y_integrator_max; float y_coordinated_min_speed; + int32_t y_coordinated_method; float y_rmax; float airspeed_min; @@ -220,6 +221,7 @@ private: param_t y_roll_feedforward; param_t y_integrator_max; param_t y_coordinated_min_speed; + param_t y_coordinated_method; param_t y_rmax; param_t airspeed_min; @@ -386,6 +388,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); _parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN"); + _parameter_handles.y_coordinated_method = param_find("FW_YCO_METHOD"); _parameter_handles.trim_roll = param_find("TRIM_ROLL"); _parameter_handles.trim_pitch = param_find("TRIM_PITCH"); @@ -454,6 +457,7 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.y_ff, &(_parameters.y_ff)); param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed)); + param_get(_parameter_handles.y_coordinated_method, &(_parameters.y_coordinated_method)); param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); @@ -496,6 +500,7 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_k_ff(_parameters.y_ff); _yaw_ctrl.set_integrator_max(_parameters.y_integrator_max); _yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed); + _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); return OK; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 7cae84678..f48075d71 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -275,6 +275,19 @@ PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); */ PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f); +/** + * Method used for yaw coordination + * + * The param value sets the method used to calculate the yaw rate + * 0: open-loop zero lateral acceleration based on kinematic constraints + * + * @min 0 + * @max 0 + * @unit m/s + * @group FW Attitude Control + */ +PARAM_DEFINE_INT32(FW_YCO_METHOD, 0); + /* Airspeed parameters: * The following parameters about airspeed are used by the attitude and the * position controller. -- cgit v1.2.3