fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 04:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

using jacobians in fw attitude control

Browse Source
This commit is contained in:
Thomas Gubler
2013-10-24 17:39:12 +02:00
parent 8193382ec2
commit 1c57d7de43
5 changed files with 10 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+3 -2
View File
@@ -56,7 +56,7 @@ ECL_PitchController::ECL_PitchController() :
{
}
float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler,
float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float yaw_rate, float scaler,
bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
{
/* get the usual dt estimate */
@@ -108,7 +108,8 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
float pitch_error = pitch_setpoint - pitch;
/* rate setpoint from current error and time constant */
_rate_setpoint = pitch_error / _tc;
float theta_dot_setpoint = pitch_error / _tc;
_rate_setpoint = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian
/* add turn offset */
_rate_setpoint += turn_offset;