Merge pull request #3323 from PX4/att_est_q_declination

Attitude estimator Q: fix magnetic declination inducing gyro bias growth
This commit is contained in:
Roman Bapst
2015-12-04 16:46:36 +01:00
2 changed files with 35 additions and 2 deletions
+10
View File
@@ -211,6 +211,16 @@ public:
data[3] = static_cast<float>(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2);
}
/**
* simplified version of the above method to create quaternion representing rotation only by yaw
*/
void from_yaw(float yaw) {
data[0] = cosf(yaw / 2.0f);
data[1] = 0.0f;
data[2] = 0.0f;
data[3] = sinf(yaw / 2.0f);
}
/**
* create Euler angles vector from the quaternion
*/