aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Mount: Fix bug in mavlink gimbal attitude control

Browse Source
This commit is contained in:
Paul Riseborough 2015-02-27 16:11:02 +11:00 committed by Andrew Tridgell
parent 103bb2a08d
commit 9c2f1ce869
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_Mount/AP_Mount_MAVLink.cpp
View File

@ -194,17 +194,17 @@ Vector3f AP_Mount_MAVLink::gimbal_update_control1(const Vector3f &ef_target_eule
//divide the demanded quaternion by the estimated to get the error //divide the demanded quaternion by the estimated to get the error
Quaternion quatErr = quatDem / quatEst; Quaternion quatErr = quatDem / quatEst;
// convert the quaternion to an angle error vector // convert the quaternion to an angle error vector using a first order approximation
Vector3f deltaAngErr; Vector3f deltaAngErr;
float scaler = 1.0f-quatErr[0]*quatErr[0]; float scaler;
if (scaler > 1e-12) { if (quatErr[0] >= 0.0f) {
scaler = 1.0f/sqrtf(scaler); scaler = 2.0f;
} else {
scaler = -2.0f;
}
deltaAngErr.x = quatErr[1] * scaler; deltaAngErr.x = quatErr[1] * scaler;
deltaAngErr.y = quatErr[2] * scaler; deltaAngErr.y = quatErr[2] * scaler;
deltaAngErr.z = quatErr[3] * scaler; deltaAngErr.z = quatErr[3] * scaler;
} else {
deltaAngErr.zero();
}
// multiply the angle error vector by a gain to calculate a demanded gimbal rate // multiply the angle error vector by a gain to calculate a demanded gimbal rate
Vector3f rateDemand = deltaAngErr * 1.0f; Vector3f rateDemand = deltaAngErr * 1.0f;