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

add simple trim parameter for fw attitude

This commit is contained in:
Thomas Gubler 2014-03-03 22:09:01 +01:00
parent b76e26c5e5
commit 5707118a97
1 changed files with 28 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -166,6 +166,11 @@ private:
float airspeed_min; float airspeed_min;
float airspeed_trim; float airspeed_trim;
float airspeed_max; float airspeed_max;
float trim_roll;
float trim_pitch;
float trim_yaw;
} _parameters; /**< local copies of interesting parameters */ } _parameters; /**< local copies of interesting parameters */
struct { struct {
@ -197,6 +202,10 @@ private:
param_t airspeed_min; param_t airspeed_min;
param_t airspeed_trim; param_t airspeed_trim;
param_t airspeed_max; param_t airspeed_max;
param_t trim_roll;
param_t trim_pitch;
param_t trim_yaw;
} _parameter_handles; /**< handles for interesting parameters */ } _parameter_handles; /**< handles for interesting parameters */
@ -335,6 +344,10 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN"); _parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN");
_parameter_handles.trim_roll = param_find("TRIM_ROLL");
_parameter_handles.trim_pitch = param_find("TRIM_PITCH");
_parameter_handles.trim_yaw = param_find("TRIM_YAW");
/* fetch initial parameter values */ /* fetch initial parameter values */
parameters_update(); parameters_update();
} }
@ -395,6 +408,10 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
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));
/* pitch control parameters */ /* pitch control parameters */
_pitch_ctrl.set_time_constant(_parameters.tconst); _pitch_ctrl.set_time_constant(_parameters.tconst);
_pitch_ctrl.set_k_p(_parameters.p_p); _pitch_ctrl.set_k_p(_parameters.p_p);
@ -670,9 +687,13 @@ FixedwingAttitudeControl::task_main()
* With this mapping the stick angle is a 1:1 representation of * With this mapping the stick angle is a 1:1 representation of
* the commanded attitude. If more than 45 degrees are desired, * the commanded attitude. If more than 45 degrees are desired,
* a scaling parameter can be applied to the remote. * a scaling parameter can be applied to the remote.
*
* The trim gets subtracted here from the manual setpoint to get
* the intended attitude setpoint. Later, after the rate control step the
* trim is added again to the control signal.
*/ */
roll_sp = _manual.roll * 0.75f; roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f;
pitch_sp = _manual.pitch * 0.75f; pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f;
throttle_sp = _manual.throttle; throttle_sp = _manual.throttle;
_actuators.control[4] = _manual.flaps; _actuators.control[4] = _manual.flaps;
@ -685,7 +706,7 @@ FixedwingAttitudeControl::task_main()
att_sp.timestamp = hrt_absolute_time(); att_sp.timestamp = hrt_absolute_time();
att_sp.roll_body = roll_sp; att_sp.roll_body = roll_sp;
att_sp.pitch_body = pitch_sp; att_sp.pitch_body = pitch_sp;
att_sp.yaw_body = 0.0f; att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
att_sp.thrust = throttle_sp; att_sp.thrust = throttle_sp;
/* lazily publish the setpoint only once available */ /* lazily publish the setpoint only once available */
@ -719,12 +740,12 @@ FixedwingAttitudeControl::task_main()
speed_body_u, speed_body_v, speed_body_w, speed_body_u, speed_body_v, speed_body_w,
_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
/* Run attitude RATE controllers which need the desired attitudes from above */ /* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
_att.rollspeed, _att.yawspeed, _att.rollspeed, _att.yawspeed,
_yaw_ctrl.get_desired_rate(), _yaw_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f; _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
if (!isfinite(roll_u)) { if (!isfinite(roll_u)) {
warnx("roll_u %.4f", roll_u); warnx("roll_u %.4f", roll_u);
} }
@ -733,7 +754,7 @@ FixedwingAttitudeControl::task_main()
_att.pitchspeed, _att.yawspeed, _att.pitchspeed, _att.yawspeed,
_yaw_ctrl.get_desired_rate(), _yaw_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f; _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
if (!isfinite(pitch_u)) { if (!isfinite(pitch_u)) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f", warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f",
pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body); pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body);
@ -743,7 +764,7 @@ FixedwingAttitudeControl::task_main()
_att.pitchspeed, _att.yawspeed, _att.pitchspeed, _att.yawspeed,
_pitch_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f; _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
if (!isfinite(yaw_u)) { if (!isfinite(yaw_u)) {
warnx("yaw_u %.4f", yaw_u); warnx("yaw_u %.4f", yaw_u);
} }