dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fw_att_control: update manual_control_setpoint usage

This commit is contained in:
Anton Babushkin 2014-04-03 22:58:57 +04:00
parent e2ac5222d8
commit ef8b974373
2 changed files with 24 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -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;

10
src/modules/fw_att_control/fw_att_control_params.c
View File

@ -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);