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

rotate attitude for fw mode only if VTOL is a tailsitter

This commit is contained in:
tumbili 2015-06-19 08:46:20 +02:00
parent d320dc8ada
commit 4e9fd5b2a4
1 changed files with 16 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -193,12 +193,14 @@ private:
float trim_roll;
float trim_pitch;
float trim_yaw;
float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */
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 man_roll_max; /**< Max Roll in rad */
float man_pitch_max; /**< Max Pitch in rad */
float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */
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 man_roll_max; /**< Max Roll in rad */
float man_pitch_max; /**< Max Pitch in rad */
int vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */
} _parameters; /**< local copies of interesting parameters */
@ -241,6 +243,8 @@ private:
param_t man_roll_max;
param_t man_pitch_max;
param_t vtol_type;
} _parameter_handles; /**< handles for interesting parameters */
@ -404,6 +408,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
_parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");
_parameter_handles.vtol_type = param_find("VT_TYPE");
/* fetch initial parameter values */
parameters_update();
}
@ -481,6 +487,8 @@ FixedwingAttitudeControl::parameters_update()
_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
param_get(_parameter_handles.vtol_type, &_parameters.vtol_type);
/* pitch control parameters */
_pitch_ctrl.set_time_constant(_parameters.tconst);
_pitch_ctrl.set_k_p(_parameters.p_p);
@ -703,10 +711,8 @@ FixedwingAttitudeControl::task_main()
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
if (_vehicle_status.is_vtol) {
/* vehicle type is VTOL, need to modify attitude!
* The following modification to the attitude is vehicle specific and in this case applies
* to tail-sitter models !!!
if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) {
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
*
* Since the VTOL airframe is initialized as a multicopter we need to
* modify the estimated attitude for the fixed wing operation.