Added BNO080Wheelchair.h
Dependents: BNO080_program wheelchaircontrol8 Version1-9 BNO080_program
Diff: quaternion.h
- Revision:
- 11:dbe6d8d0ceb1
- Parent:
- 4:8e016f874c42
--- a/quaternion.h Tue Jul 30 18:33:45 2019 +0000 +++ b/quaternion.h Fri Aug 02 23:34:30 2019 +0000 @@ -267,12 +267,12 @@ * @param euler A 3-vector in order: roll-pitch-yaw. */ void euler(const TVector3& euler) { - FloatType c1 = cos(euler[2] * 0.5); - FloatType c2 = cos(euler[1] * 0.5); - FloatType c3 = cos(euler[0] * 0.5); - FloatType s1 = sin(euler[2] * 0.5); - FloatType s2 = sin(euler[1] * 0.5); - FloatType s3 = sin(euler[0] * 0.5); + FloatType c1 = cos(euler[2] * 0.5f); + FloatType c2 = cos(euler[1] * 0.5f); + FloatType c3 = cos(euler[0] * 0.5f); + FloatType s1 = sin(euler[2] * 0.5f); + FloatType s2 = sin(euler[1] * 0.5f); + FloatType s3 = sin(euler[0] * 0.5f); mData[0] = c1*c2*s3 - s1*s2*c3; mData[1] = c1*s2*c3 + s1*c2*s3; @@ -296,11 +296,11 @@ sqy = mData[1]*mData[1]; sqz = mData[2]*mData[2]; - euler[1] = asin(2.0 * (mData[3]*mData[1] - mData[0]*mData[2])); + euler[1] = asin(2.0f * (mData[3]*mData[1] - mData[0]*mData[2])); if (PI_OVER_2 - fabs(euler[1]) > EPSILON) { - euler[2] = atan2(2.0 * (mData[0]*mData[1] + mData[3]*mData[2]), + euler[2] = atan2(2.0f * (mData[0]*mData[1] + mData[3]*mData[2]), sqx - sqy - sqz + sqw); - euler[0] = atan2(2.0 * (mData[3]*mData[0] + mData[1]*mData[2]), + euler[0] = atan2(2.0f * (mData[3]*mData[0] + mData[1]*mData[2]), sqw - sqx - sqy + sqz); } else { // compute heading from local 'down' vector