diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2013-11-13 11:05:22 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2013-11-13 11:05:22 +0100 |
commit | 38172497c116548320c74696d795f9198e0bf4e4 (patch) | |
tree | a3b446c3c503c18d4d4a25be71a90553bd8b737d | |
parent | 8e92d47de1d29ca3461911483361809697bc236a (diff) | |
download | px4-firmware-38172497c116548320c74696d795f9198e0bf4e4.tar.gz px4-firmware-38172497c116548320c74696d795f9198e0bf4e4.tar.bz2 px4-firmware-38172497c116548320c74696d795f9198e0bf4e4.zip |
reintroduce feedforward
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 3 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 10 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 3 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.h | 10 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 3 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 13 | ||||
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 16 | ||||
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_params.c | 6 |
8 files changed, 59 insertions, 5 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index ccaed14ce..d7dbbebd4 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -53,6 +53,7 @@ ECL_PitchController::ECL_PitchController() : _k_p(0.0f), _k_i(0.0f), _k_d(0.0f), + _k_ff(0.0f), _integrator_max(0.0f), _max_rate_pos(0.0f), _max_rate_neg(0.0f), @@ -173,7 +174,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //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 ba8ed3e6f..2ca0490fd 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -73,21 +73,30 @@ public: void set_k_p(float k_p) { _k_p = k_p; } + void set_k_i(float k_i) { _k_i = k_i; } void set_k_d(float k_d) { _k_d = k_d; } + + void set_k_ff(float k_ff) { + _k_ff = k_ff; + } + void set_integrator_max(float max) { _integrator_max = max; } + void set_max_rate_pos(float max_rate_pos) { _max_rate_pos = max_rate_pos; } + void set_max_rate_neg(float max_rate_neg) { _max_rate_neg = max_rate_neg; } + void set_roll_ff(float roll_ff) { _roll_ff = roll_ff; } @@ -111,6 +120,7 @@ private: float _k_p; float _k_i; float _k_d; + float _k_ff; float _integrator_max; float _max_rate_pos; float _max_rate_neg; diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 4b0bfc6c4..bd6c9da71 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -53,6 +53,7 @@ ECL_RollController::ECL_RollController() : _k_p(0.0f), _k_i(0.0f), _k_d(0.0f), + _k_ff(0.0f), _integrator_max(0.0f), _max_rate(0.0f), _last_output(0.0f), @@ -141,7 +142,7 @@ float ECL_RollController::control_bodyrate(float pitch, //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index dd7d1bf53..efc7b8944 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -71,18 +71,27 @@ public: _tc = time_constant; } } + void set_k_p(float k_p) { _k_p = k_p; } + void set_k_i(float k_i) { _k_i = k_i; } + void set_k_d(float k_d) { _k_d = k_d; } + + void set_k_ff(float k_ff) { + _k_ff = k_ff; + } + void set_integrator_max(float max) { _integrator_max = max; } + void set_max_rate(float max_rate) { _max_rate = max_rate; } @@ -105,6 +114,7 @@ private: float _k_p; float _k_i; float _k_d; + float _k_ff; float _integrator_max; float _max_rate; float _last_output; diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index a4ecc48a2..7c366aaf2 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -51,6 +51,7 @@ ECL_YawController::ECL_YawController() : _k_p(0.0f), _k_i(0.0f), _k_d(0.0f), + _k_ff(0.0f), _integrator_max(0.0f), _max_rate(0.0f), _roll_ff(0.0f), @@ -159,7 +160,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * 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); diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index b5ae1e6af..f15645fcf 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -69,22 +69,32 @@ public: void set_k_p(float k_p) { _k_p = k_p; - } + } + void set_k_i(float k_i) { _k_i = k_i; } + void set_k_d(float k_d) { _k_d = k_d; } + + void set_k_ff(float k_ff) { + _k_ff = k_ff; + } + void set_integrator_max(float max) { _integrator_max = max; } + void set_max_rate(float max_rate) { _max_rate = max_rate; } + void set_k_roll_ff(float roll_ff) { _roll_ff = roll_ff; } + void set_coordinated_min_speed(float coordinated_min_speed) { _coordinated_min_speed = coordinated_min_speed; } @@ -107,6 +117,7 @@ private: float _k_p; float _k_i; float _k_d; + float _k_ff; float _integrator_max; float _max_rate; float _roll_ff; 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 a5f3f1d91..53f89c7c4 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -141,6 +141,7 @@ private: float p_p; float p_d; float p_i; + float p_ff; float p_rmax_pos; float p_rmax_neg; float p_integrator_max; @@ -148,11 +149,13 @@ private: float r_p; float r_d; float r_i; + float r_ff; float r_integrator_max; float r_rmax; float y_p; float y_i; float y_d; + float y_ff; float y_roll_feedforward; float y_integrator_max; float y_coordinated_min_speed; @@ -169,6 +172,7 @@ private: param_t p_p; param_t p_d; param_t p_i; + param_t p_ff; param_t p_rmax_pos; param_t p_rmax_neg; param_t p_integrator_max; @@ -176,11 +180,13 @@ private: param_t r_p; param_t r_d; param_t r_i; + param_t r_ff; param_t r_integrator_max; param_t r_rmax; param_t y_p; param_t y_i; param_t y_d; + param_t y_ff; param_t y_roll_feedforward; param_t y_integrator_max; param_t y_coordinated_min_speed; @@ -302,6 +308,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.p_p = param_find("FW_PR_P"); _parameter_handles.p_d = param_find("FW_PR_D"); _parameter_handles.p_i = param_find("FW_PR_I"); + _parameter_handles.p_ff = param_find("FW_PR_FF"); _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); _parameter_handles.p_integrator_max = param_find("FW_PR_IMAX"); @@ -310,12 +317,14 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.r_p = param_find("FW_RR_P"); _parameter_handles.r_d = param_find("FW_RR_D"); _parameter_handles.r_i = param_find("FW_RR_I"); + _parameter_handles.r_ff = param_find("FW_RR_FF"); _parameter_handles.r_integrator_max = param_find("FW_RR_IMAX"); _parameter_handles.r_rmax = param_find("FW_R_RMAX"); _parameter_handles.y_p = param_find("FW_YR_P"); _parameter_handles.y_i = param_find("FW_YR_I"); _parameter_handles.y_d = param_find("FW_YR_D"); + _parameter_handles.y_ff = param_find("FW_YR_FF"); _parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); _parameter_handles.y_integrator_max = param_find("FW_YR_IMAX"); _parameter_handles.y_rmax = param_find("FW_Y_RMAX"); @@ -363,6 +372,7 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.p_p, &(_parameters.p_p)); param_get(_parameter_handles.p_d, &(_parameters.p_d)); param_get(_parameter_handles.p_i, &(_parameters.p_i)); + param_get(_parameter_handles.p_ff, &(_parameters.p_ff)); param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); @@ -371,12 +381,15 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.r_p, &(_parameters.r_p)); param_get(_parameter_handles.r_d, &(_parameters.r_d)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); + param_get(_parameter_handles.r_ff, &(_parameters.r_ff)); + param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max)); param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax)); param_get(_parameter_handles.y_p, &(_parameters.y_p)); param_get(_parameter_handles.y_i, &(_parameters.y_i)); param_get(_parameter_handles.y_d, &(_parameters.y_d)); + param_get(_parameter_handles.y_ff, &(_parameters.y_ff)); param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward)); param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed)); @@ -391,6 +404,7 @@ FixedwingAttitudeControl::parameters_update() _pitch_ctrl.set_k_p(_parameters.p_p); _pitch_ctrl.set_k_i(_parameters.p_i); _pitch_ctrl.set_k_d(_parameters.p_d); + _pitch_ctrl.set_k_ff(_parameters.p_ff); _pitch_ctrl.set_integrator_max(_parameters.p_integrator_max); _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); @@ -401,6 +415,7 @@ FixedwingAttitudeControl::parameters_update() _roll_ctrl.set_k_p(_parameters.r_p); _roll_ctrl.set_k_i(_parameters.r_i); _roll_ctrl.set_k_d(_parameters.r_d); + _roll_ctrl.set_k_ff(_parameters.r_ff); _roll_ctrl.set_integrator_max(_parameters.r_integrator_max); _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); @@ -408,6 +423,7 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_k_p(_parameters.y_p); _yaw_ctrl.set_k_i(_parameters.y_i); _yaw_ctrl.set_k_d(_parameters.y_d); + _yaw_ctrl.set_k_ff(_parameters.y_ff); _yaw_ctrl.set_k_roll_ff(_parameters.y_roll_feedforward); _yaw_ctrl.set_integrator_max(_parameters.y_integrator_max); _yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed); 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 796ab3f90..240749fed 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. +f * Copyright (c) 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without @@ -138,3 +138,7 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); PARAM_DEFINE_FLOAT(FW_Y_RMAX, 60); PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1.0f); + +PARAM_DEFINE_FLOAT(FW_RR_FF, 0.0f); +PARAM_DEFINE_FLOAT(FW_PR_FF, 0.0f); +PARAM_DEFINE_FLOAT(FW_YR_FF, 0.0f); |