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

fw pitch sp and roll sp offset parameter

This commit is contained in:
Thomas Gubler 2014-03-05 00:23:05 +01:00
parent 5fda6da2f6
commit 87fd89ea48
2 changed files with 25 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -170,6 +170,8 @@ private:
float trim_roll;
float trim_pitch;
float trim_yaw;
float rollsp_offset;
float pitchsp_offset;
} _parameters; /**< local copies of interesting parameters */
@ -206,6 +208,8 @@ private:
param_t trim_roll;
param_t trim_pitch;
param_t trim_yaw;
param_t rollsp_offset;
param_t pitchsp_offset;
} _parameter_handles; /**< handles for interesting parameters */
@ -347,6 +351,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.trim_roll = param_find("TRIM_ROLL");
_parameter_handles.trim_pitch = param_find("TRIM_PITCH");
_parameter_handles.trim_yaw = param_find("TRIM_YAW");
_parameter_handles.rollsp_offset = param_find("FW_RSP_OFF");
_parameter_handles.pitchsp_offset = param_find("FW_PSP_OFF");
/* fetch initial parameter values */
parameters_update();
@ -411,6 +417,9 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll));
param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch));
param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
param_get(_parameter_handles.rollsp_offset, &(_parameters.rollsp_offset));
param_get(_parameter_handles.pitchsp_offset, &(_parameters.pitchsp_offset));
/* pitch control parameters */
_pitch_ctrl.set_time_constant(_parameters.tconst);
@ -665,13 +674,13 @@ FixedwingAttitudeControl::task_main()
float airspeed_scaling = _parameters.airspeed_trim / airspeed;
//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
float roll_sp = 0.0f;
float pitch_sp = 0.0f;
float roll_sp = _parameters.rollsp_offset;
float pitch_sp = _parameters.pitchsp_offset;
float throttle_sp = 0.0f;
if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
roll_sp = _att_sp.roll_body;
pitch_sp = _att_sp.pitch_body;
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
@ -692,8 +701,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;
pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f;
roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset;
pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset;
throttle_sp = _manual.throttle;
_actuators.control[4] = _manual.flaps;

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

@ -176,3 +176,13 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
// @Description If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively
// @Range 0.0 to 30
PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f);
// @DisplayName Roll Setpoint Offset
// @Description An airframe specific offset of the roll setpoint in degrees, the value is added to the roll setpoint and should correspond to the typical cruise speed of the airframe
// @Range -90.0 to 90.0
PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
// @DisplayName Pitch Setpoint Offset
// @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);