diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-04-03 22:58:57 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-04-03 22:58:57 +0400 |
commit | ef8b97437342401a464bcc844d6c347c33919d73 (patch) | |
tree | bc776ed9b902fe21b64e1acff737a6e788b48827 | |
parent | e2ac5222d812bdbfaf33fc2d320ee22ab861d433 (diff) | |
download | px4-firmware-ef8b97437342401a464bcc844d6c347c33919d73.tar.gz px4-firmware-ef8b97437342401a464bcc844d6c347c33919d73.tar.bz2 px4-firmware-ef8b97437342401a464bcc844d6c347c33919d73.zip |
fw_att_control: update manual_control_setpoint usage
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 17 | ||||
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_params.c | 10 |
2 files changed, 24 insertions, 3 deletions
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); |