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

AP_Gimbal: update getGimbalRateDemVecTilt to use quaternions

Browse Source
This commit is contained in:
Arthur Benemann 2015-03-26 18:05:24 -07:00 committed by Randy Mackay
parent 4bdae02cfd
commit 0189f80462
1 changed files with 23 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_Gimbal/AP_Gimbal.cpp
View File

@ -113,13 +113,30 @@ Vector3f AP_Gimbal::getGimbalRateDemVecTilt(Quaternion quatEst)
Vector3f eulerEst;
quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z);
//TODO receive target from AP_Mount
Vector3f vectorError;
vectorError.x = eulerEst.x;
vectorError.y = eulerEst.y - _angle_ef_target_rad.y;
vectorError.z = 0;
// Calculate a demanded quaternion using the demanded roll and pitch and estimated yaw (yaw is slaved to the vehicle)
Quaternion quatDem;
quatDem.from_euler(_angle_ef_target_rad.x,
_angle_ef_target_rad.y,
eulerEst.z);
Vector3f gimbalRateDemVecTilt = - vectorError * K_gimbalRate;
//divide the demanded quaternion by the estimated to get the error
Quaternion quatErr = quatDem / quatEst;
// Convert to a delta rotation using a small angle appoximation
quatErr.normalize();
Vector3f deltaAngErr;
float scaler;
if (quatErr[0] >= 0.0f) {
scaler = 2.0f;
} else {
scaler = -2.0f;
}
deltaAngErr.x = scaler * quatErr[1];
deltaAngErr.y = scaler * quatErr[2];
deltaAngErr.z = scaler * quatErr[3];
// multiply the angle error vector by a gain to calculate a demanded gimbal rate required to control tilt
Vector3f gimbalRateDemVecTilt = deltaAngErr * K_gimbalRate;
return gimbalRateDemVecTilt;
}