diff --git a/src/modules/fw_att_control_vector/fw_att_control_vector_main.cpp b/src/modules/fw_att_control_vector/fw_att_control_vector_main.cpp index a06bc9cb62..c64f82e544 100644 --- a/src/modules/fw_att_control_vector/fw_att_control_vector_main.cpp +++ b/src/modules/fw_att_control_vector/fw_att_control_vector_main.cpp @@ -133,7 +133,7 @@ private: struct { - float p_tconst; + float tconst; float p_p; float p_d; float p_i; @@ -141,7 +141,6 @@ private: float p_rmax_dn; float p_imax; float p_rll; - float r_tconst; float r_p; float r_d; float r_i; @@ -160,7 +159,7 @@ private: struct { - param_t p_tconst; + param_t tconst; param_t p_p; param_t p_d; param_t p_i; @@ -168,7 +167,6 @@ private: param_t p_rmax_dn; param_t p_imax; param_t p_rll; - param_t r_tconst; param_t r_p; param_t r_d; param_t r_i; @@ -274,7 +272,7 @@ FixedwingAttitudeControlVector::FixedwingAttitudeControlVector() : { // _parameter_handles.roll_p = param_find("FW_ROLL_P"); // _parameter_handles.pitch_p = param_find("FW_PITCH_P"); - _parameter_handles.p_tconst = param_find("FW_P_TCONST"); + _parameter_handles.tconst = param_find("FW_TCONST"); _parameter_handles.p_p = param_find("FW_P_P"); _parameter_handles.p_d = param_find("FW_P_D"); _parameter_handles.p_i = param_find("FW_P_I"); @@ -283,7 +281,6 @@ FixedwingAttitudeControlVector::FixedwingAttitudeControlVector() : _parameter_handles.p_imax = param_find("FW_P_IMAX"); _parameter_handles.p_rll = param_find("FW_P_RLL"); - _parameter_handles.r_tconst = param_find("FW_R_TCONST"); _parameter_handles.r_p = param_find("FW_R_P"); _parameter_handles.r_d = param_find("FW_R_D"); _parameter_handles.r_i = param_find("FW_R_I"); @@ -333,7 +330,7 @@ int FixedwingAttitudeControlVector::parameters_update() { - param_get(_parameter_handles.p_tconst, &(_parameters.p_tconst)); + param_get(_parameter_handles.tconst, &(_parameters.tconst)); 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)); @@ -342,7 +339,6 @@ FixedwingAttitudeControlVector::parameters_update() param_get(_parameter_handles.p_imax, &(_parameters.p_imax)); param_get(_parameter_handles.p_rll, &(_parameters.p_rll)); - param_get(_parameter_handles.r_tconst, &(_parameters.r_tconst)); 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)); @@ -360,29 +356,14 @@ FixedwingAttitudeControlVector::parameters_update() param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); /* pitch control parameters */ - _att_control.set_tconst(_parameters.p_tconst); - // _pitch_ctrl.set_k_p(math::radians(_parameters.p_p)); - // _pitch_ctrl.set_k_i(math::radians(_parameters.p_i)); - // _pitch_ctrl.set_k_d(math::radians(_parameters.p_d)); - // _pitch_ctrl.set_imax(math::radians(_parameters.p_imax)); - // _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_up)); - // _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_dn)); - // _pitch_ctrl.set_roll_ff(math::radians(_parameters.p_rll)); - - // /* roll control parameters */ - // _roll_ctrl.set_tau(_parameters.r_tconst); - // _roll_ctrl.set_k_p(math::radians(_parameters.r_p)); - // _roll_ctrl.set_k_i(math::radians(_parameters.r_i)); - // _roll_ctrl.set_k_d(math::radians(_parameters.r_d)); - // _roll_ctrl.set_imax(math::radians(_parameters.r_imax)); - // _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); - - // /* yaw control parameters */ - // _yaw_ctrl.set_k_a(math::radians(_parameters.y_slip)); - // _yaw_ctrl.set_k_i(math::radians(_parameters.y_int)); - // _yaw_ctrl.set_k_d(math::radians(_parameters.y_damp)); - // _yaw_ctrl.set_k_ff(math::radians(_parameters.y_rll)); - // _yaw_ctrl.set_imax(math::radians(_parameters.y_imax)); + _att_control.set_tconst(_parameters.tconst); + _att_control.set_k_p(math::radians(_parameters.r_p), + math::radians(_parameters.p_p), 0.0f); + _att_control.set_k_d(math::radians(_parameters.r_p), + math::radians(_parameters.p_p), 0.0f); + _att_control.set_k_i(math::radians(_parameters.r_i), + math::radians(_parameters.p_i), + math::radians(_parameters.y_int)); return OK; } diff --git a/src/modules/fw_att_control_vector/fw_att_control_vector_params.c b/src/modules/fw_att_control_vector/fw_att_control_vector_params.c new file mode 100644 index 0000000000..7e434c164b --- /dev/null +++ b/src/modules/fw_att_control_vector/fw_att_control_vector_params.c @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 PX4 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 fw_pos_control_l1_params.c + * + * Parameters defined by the L1 position control task + * + * @author Lorenz Meier + */ + +#include + +#include + +/* + * Controller parameters, accessible via MAVLink + * + */ + +PARAM_DEFINE_FLOAT(FW_TCONST, 0.5f); +PARAM_DEFINE_FLOAT(FW_P_P, 40.0f); +PARAM_DEFINE_FLOAT(FW_P_D, 0.0f); +PARAM_DEFINE_FLOAT(FW_P_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_P_RMAX_UP, 0.0f); +PARAM_DEFINE_FLOAT(FW_P_RMAX_DN, 0.0f); +PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f); +PARAM_DEFINE_FLOAT(FW_P_RLL, 1.0f); +PARAM_DEFINE_FLOAT(FW_R_P, 40.0f); +PARAM_DEFINE_FLOAT(FW_R_D, 0.0f); +PARAM_DEFINE_FLOAT(FW_R_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f); +PARAM_DEFINE_FLOAT(FW_R_RMAX, 60); +PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); +PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); +PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); diff --git a/src/modules/fw_att_control_vector/module.mk b/src/modules/fw_att_control_vector/module.mk index 4def85cd31..e78a715954 100644 --- a/src/modules/fw_att_control_vector/module.mk +++ b/src/modules/fw_att_control_vector/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = fw_att_control_vector SRCS = fw_att_control_vector_main.cpp \ - ecl_fw_att_control_vector.cpp + ecl_fw_att_control_vector.cpp \ + fw_att_control_vector_params.c