From ef8b97437342401a464bcc844d6c347c33919d73 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 22:58:57 +0400 Subject: fw_att_control: update manual_control_setpoint usage --- src/modules/fw_att_control/fw_att_control_main.cpp | 17 ++++++++++++++--- src/modules/fw_att_control/fw_att_control_params.c | 10 ++++++++++ 2 files changed, 24 insertions(+), 3 deletions(-) (limited to 'src/modules/fw_att_control') 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 f139c25f4..4464b5e01 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -173,6 +173,8 @@ private: float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ + float roll_max; /**< Max Roll in rad */ + float pitch_max; /**< Max Pitch in rad */ } _parameters; /**< local copies of interesting parameters */ @@ -211,6 +213,8 @@ private: param_t trim_yaw; param_t rollsp_offset_deg; param_t pitchsp_offset_deg; + param_t roll_max; + param_t pitch_max; } _parameter_handles; /**< handles for interesting parameters */ @@ -354,6 +358,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF"); _parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF"); + _parameter_handles.roll_max = param_find("FW_R_MAX"); + _parameter_handles.pitch_max = param_find("FW_P_MAX"); + /* fetch initial parameter values */ parameters_update(); } @@ -421,6 +428,10 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg)); _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg); _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg); + param_get(_parameter_handles.roll_max, &(_parameters.roll_max)); + param_get(_parameter_handles.pitch_max, &(_parameters.pitch_max)); + _parameters.roll_max = math::radians(_parameters.roll_max); + _parameters.pitch_max = math::radians(_parameters.pitch_max); /* pitch control parameters */ @@ -700,8 +711,8 @@ FixedwingAttitudeControl::task_main() * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset_rad; - pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset_rad; + roll_sp = (_manual.roll * _parameters.roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; + pitch_sp = (-_manual.pitch * _parameters.pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; @@ -809,7 +820,7 @@ FixedwingAttitudeControl::task_main() } else { /* manual/direct control */ _actuators.control[0] = _manual.roll; - _actuators.control[1] = _manual.pitch; + _actuators.control[1] = -_manual.pitch; _actuators.control[2] = _manual.yaw; _actuators.control[3] = _manual.throttle; _actuators.control[4] = _manual.flaps; 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 c80a44f2a..14815745c 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -186,3 +186,13 @@ PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f); // @Description An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe // @Range -90.0 to 90.0 PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); + +// @DisplayName Max Roll +// @Description Max roll for manual control in attitude stabilized mode +// @Range 0.0 to 90.0 +PARAM_DEFINE_FLOAT(FW_R_MAX, 45.0f); + +// @DisplayName Max Pitch +// @Description Max pitch for manual control in attitude stabilized mode +// @Range 0.0 to 90.0 +PARAM_DEFINE_FLOAT(FW_P_MAX, 45.0f); -- cgit v1.2.3 From 606660d94a0f2b2968c0bd3a8f035a6db97d9e10 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 14:07:16 +0400 Subject: fw_att_control: renamed FW_R_MAX/FW_P_MAX to FW_MAN_R_MAX/FW_MAN_P_MAX --- src/modules/fw_att_control/fw_att_control_main.cpp | 24 +++++++++++----------- src/modules/fw_att_control/fw_att_control_params.c | 8 ++++---- 2 files changed, 16 insertions(+), 16 deletions(-) (limited to 'src/modules/fw_att_control') 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 4ab5c19cf..5276b1c13 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -173,8 +173,8 @@ private: float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ - float roll_max; /**< Max Roll in rad */ - float pitch_max; /**< Max Pitch in rad */ + float man_roll_max; /**< Max Roll in rad */ + float man_pitch_max; /**< Max Pitch in rad */ } _parameters; /**< local copies of interesting parameters */ @@ -213,8 +213,8 @@ private: param_t trim_yaw; param_t rollsp_offset_deg; param_t pitchsp_offset_deg; - param_t roll_max; - param_t pitch_max; + param_t man_roll_max; + param_t man_pitch_max; } _parameter_handles; /**< handles for interesting parameters */ @@ -358,8 +358,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF"); _parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF"); - _parameter_handles.roll_max = param_find("FW_R_MAX"); - _parameter_handles.pitch_max = param_find("FW_P_MAX"); + _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); + _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); /* fetch initial parameter values */ parameters_update(); @@ -428,10 +428,10 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg)); _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg); _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg); - param_get(_parameter_handles.roll_max, &(_parameters.roll_max)); - param_get(_parameter_handles.pitch_max, &(_parameters.pitch_max)); - _parameters.roll_max = math::radians(_parameters.roll_max); - _parameters.pitch_max = math::radians(_parameters.pitch_max); + param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max)); + param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max)); + _parameters.man_roll_max = math::radians(_parameters.man_roll_max); + _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); /* pitch control parameters */ @@ -717,8 +717,8 @@ FixedwingAttitudeControl::task_main() * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.roll * _parameters.roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; - pitch_sp = (-_manual.pitch * _parameters.pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; + roll_sp = (_manual.roll * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; + pitch_sp = (-_manual.pitch * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; 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 14815745c..aa637e207 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -187,12 +187,12 @@ PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f); // @Range -90.0 to 90.0 PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); -// @DisplayName Max Roll +// @DisplayName Max Manual Roll // @Description Max roll for manual control in attitude stabilized mode // @Range 0.0 to 90.0 -PARAM_DEFINE_FLOAT(FW_R_MAX, 45.0f); +PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); -// @DisplayName Max Pitch +// @DisplayName Max Manual Pitch // @Description Max pitch for manual control in attitude stabilized mode // @Range 0.0 to 90.0 -PARAM_DEFINE_FLOAT(FW_P_MAX, 45.0f); +PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f); -- cgit v1.2.3