Fully featured I2C and SPI driver for CEVA (Hilcrest)'s BNO080 and FSM300 Inertial Measurement Units.

Dependents:   BNO080-Examples BNO080-Examples

BNO080 Driver

by Jamie Smith / USC Rocket Propulsion Lab

After lots of development, we are proud to present our driver for the Hilcrest BNO080 IMU! This driver is inspired by SparkFun and Nathan Seidle's Arduino driver for this chip, but has been substantially rewritten and adapted.

It supports the main features of the chip, such as reading rotation and acceleration data, as well as some of its more esoteric functionality, such as counting steps and detecting whether the device is being hand-held.

Features

  • Support for 15 different data reports from the IMU, from acceleration to rotation to tap detection
  • Support for reading of sensor data, and automatic checking of update rate against allowed values in metadata
  • BNO_DEBUG switch enabling verbose, detailed output about communications with the chip for ease of debugging
  • Ability to tare sensor rotation and set mounting orientation
  • Can operate in several execution modes: polling I2C, polling SPI, and threaded SPI (which handles timing-critical functions in a dedicated thread, and automatically activates when the IMU has data available)
    • Also has experimental support for using asynchronous SPI transactions, allowing other threads to execute while communication with the BNO is occurring. Note that this functionality requires a patch to Mbed OS source code due to Mbed bug #13941
  • Calibration function
  • Reasonable code size for what you get: the library uses about 4K of flash and one instance of the object uses about 1700 bytes of RAM.

Documentation

Full Doxygen documentation is available online here

Example Code

Here's a simple example:

BNO080 Rotation Vector and Acceleration

#include <mbed.h>
#include <BNO080.h>

int main()
{
	Serial pc(USBTX, USBRX);

	// Create IMU, passing in output stream, pins, I2C address, and I2C frequency
	// These pin assignments are specific to my dev setup -- you'll need to change them
	BNO080I2C imu(&pc, p28, p27, p16, p30, 0x4a, 100000); 

	pc.baud(115200);
	pc.printf("============================================================\n");

	// Tell the IMU to report rotation every 100ms and acceleration every 200ms
	imu.enableReport(BNO080::ROTATION, 100);
	imu.enableReport(BNO080::TOTAL_ACCELERATION, 200);

	while (true)
	{
		wait(.001f);
		
		// poll the IMU for new data -- this returns true if any packets were received
		if(imu.updateData())
		{
			// now check for the specific type of data that was received (can be multiple at once)
			if (imu.hasNewData(BNO080::ROTATION))
			{
				// convert quaternion to Euler degrees and print
				pc.printf("IMU Rotation Euler: ");
				TVector3 eulerRadians = imu.rotationVector.euler();
				TVector3 eulerDegrees = eulerRadians * (180.0 / M_PI);
				eulerDegrees.print(pc, true);
				pc.printf("\n");
			}
			if (imu.hasNewData(BNO080::TOTAL_ACCELERATION))
			{
				// print the acceleration vector using its builtin print() method
				pc.printf("IMU Total Acceleration: ");
				imu.totalAcceleration.print(pc, true);
				pc.printf("\n");
			}
		}
	}

}


If you want more, a comprehensive, ready-to-run set of examples is available on my BNO080-Examples repository.

Credits

This driver makes use of a lightweight, public-domain library for vectors and quaternions available here.

Changelog

Version 2.1 (Nov 24 2020)

  • Added BNO080Async, which provides a threaded implementation of the SPI driver. This should help get the best performance and remove annoying timing requirements on the code calling the driver
  • Added experimental USE_ASYNC_SPI option
  • Fixed bug in v2.0 causing calibrations to fail

Version 2.0 (Nov 18 2020)

  • Added SPI support
  • Refactored buffer system so that SPI could be implemented as a subclass. Unfortunately this does substantially increase the memory usage of the driver, but I believe that the benefits are worth it.

Version 1.3 (Jul 21 2020)

  • Fix deprecation warnings and compile errors in Mbed 6
  • Fix compile errors in Arm Compiler (why doesn't it have M_PI????)

Version 1.2 (Jan 30 2020)

  • Removed accidental IRQ change
  • Fixed hard iron offset reading incorrectly due to missing cast

Version 1.1 (Jun 14 2019)

  • Added support for changing permanent orientation
  • Add FRS writing functions
  • Removed some errant printfs

Version 1.0 (Dec 29 2018)

  • Initial Mbed OS release
Committer:
Jamie Smith
Date:
Tue Nov 24 15:06:05 2020 -0800
Revision:
9:430f5302f9e1
Parent:
5:c75be9000d75
Implement BNO080Async

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jamie Smith 1:aac28ffd63ed 1 #ifndef QUATERNION_H
Jamie Smith 1:aac28ffd63ed 2 #define QUATERNION_H
Jamie Smith 1:aac28ffd63ed 3
Jamie Smith 1:aac28ffd63ed 4 #include <cmath>
Jamie Smith 1:aac28ffd63ed 5 #include <mbed.h>
Jamie Smith 1:aac28ffd63ed 6 #include "tmatrix.h"
Jamie Smith 1:aac28ffd63ed 7
Jamie Smith 5:c75be9000d75 8 // Arm Compiler has no M_PI
Jamie Smith 5:c75be9000d75 9 #ifndef M_PI
Jamie Smith 5:c75be9000d75 10 #define M_PI 3.14159265358979323846
Jamie Smith 5:c75be9000d75 11 #endif
Jamie Smith 5:c75be9000d75 12
Jamie Smith 1:aac28ffd63ed 13 class Quaternion
Jamie Smith 1:aac28ffd63ed 14 {
Jamie Smith 1:aac28ffd63ed 15 public:
Jamie Smith 1:aac28ffd63ed 16 typedef float FloatType;
Jamie Smith 1:aac28ffd63ed 17
Jamie Smith 1:aac28ffd63ed 18 private:
Jamie Smith 1:aac28ffd63ed 19 FloatType mData[4];
Jamie Smith 1:aac28ffd63ed 20
Jamie Smith 1:aac28ffd63ed 21 public:
Jamie Smith 1:aac28ffd63ed 22
Jamie Smith 1:aac28ffd63ed 23 Quaternion() {
Jamie Smith 1:aac28ffd63ed 24 mData[0] = mData[1] = mData[2] = 0;
Jamie Smith 1:aac28ffd63ed 25 mData[3] = 1;
Jamie Smith 1:aac28ffd63ed 26 }
Jamie Smith 1:aac28ffd63ed 27
Jamie Smith 1:aac28ffd63ed 28 Quaternion(const TVector3& v, FloatType w) {
Jamie Smith 1:aac28ffd63ed 29 mData[0] = v.element(0,0);
Jamie Smith 1:aac28ffd63ed 30 mData[1] = v.element(1,0);
Jamie Smith 1:aac28ffd63ed 31 mData[2] = v.element(2,0);
Jamie Smith 1:aac28ffd63ed 32 mData[3] = w;
Jamie Smith 1:aac28ffd63ed 33 }
Jamie Smith 1:aac28ffd63ed 34
Jamie Smith 1:aac28ffd63ed 35 Quaternion(const TVector4& v) {
Jamie Smith 1:aac28ffd63ed 36 mData[0] = v.element(0,0);
Jamie Smith 1:aac28ffd63ed 37 mData[1] = v.element(1,0);
Jamie Smith 1:aac28ffd63ed 38 mData[2] = v.element(2,0);
Jamie Smith 1:aac28ffd63ed 39 mData[3] = v.element(3,0);
Jamie Smith 1:aac28ffd63ed 40 }
Jamie Smith 1:aac28ffd63ed 41
Jamie Smith 1:aac28ffd63ed 42 Quaternion(const FloatType* array) {
Jamie Smith 2:2269b723d16a 43 MBED_ASSERT(array != NULL);
Jamie Smith 1:aac28ffd63ed 44 for (uint32_t i = 0; i < 4; i++) {
Jamie Smith 1:aac28ffd63ed 45 mData[i] = array[i];
Jamie Smith 1:aac28ffd63ed 46 }
Jamie Smith 1:aac28ffd63ed 47 }
Jamie Smith 1:aac28ffd63ed 48
Jamie Smith 1:aac28ffd63ed 49 Quaternion(FloatType x, FloatType y, FloatType z, FloatType w) {
Jamie Smith 1:aac28ffd63ed 50 mData[0] = x;
Jamie Smith 1:aac28ffd63ed 51 mData[1] = y;
Jamie Smith 1:aac28ffd63ed 52 mData[2] = z;
Jamie Smith 1:aac28ffd63ed 53 mData[3] = w;
Jamie Smith 1:aac28ffd63ed 54 }
Jamie Smith 1:aac28ffd63ed 55
Jamie Smith 1:aac28ffd63ed 56 FloatType x() const { return mData[0]; }
Jamie Smith 1:aac28ffd63ed 57 FloatType y() const { return mData[1]; }
Jamie Smith 1:aac28ffd63ed 58 FloatType z() const { return mData[2]; }
Jamie Smith 1:aac28ffd63ed 59 FloatType w() const { return real(); }
Jamie Smith 1:aac28ffd63ed 60
Jamie Smith 1:aac28ffd63ed 61 TVector3 complex() const { return TVector3(mData); }
Jamie Smith 1:aac28ffd63ed 62 void complex(const TVector3& c) { mData[0] = c[0]; mData[1] = c[1]; mData[2] = c[2]; }
Jamie Smith 1:aac28ffd63ed 63
Jamie Smith 1:aac28ffd63ed 64 FloatType real() const { return mData[3]; }
Jamie Smith 1:aac28ffd63ed 65 void real(FloatType r) { mData[3] = r; }
Jamie Smith 1:aac28ffd63ed 66
Jamie Smith 1:aac28ffd63ed 67 Quaternion conjugate(void) const {
Jamie Smith 1:aac28ffd63ed 68 return Quaternion(-complex(), real());
Jamie Smith 1:aac28ffd63ed 69 }
Jamie Smith 1:aac28ffd63ed 70
Jamie Smith 1:aac28ffd63ed 71 /**
Jamie Smith 1:aac28ffd63ed 72 * @brief Computes the inverse of this quaternion.
Jamie Smith 1:aac28ffd63ed 73 *
Jamie Smith 1:aac28ffd63ed 74 * @note This is a general inverse. If you know a priori
Jamie Smith 1:aac28ffd63ed 75 * that you're using a unit quaternion (i.e., norm() == 1),
Jamie Smith 1:aac28ffd63ed 76 * it will be significantly faster to use conjugate() instead.
Jamie Smith 1:aac28ffd63ed 77 *
Jamie Smith 1:aac28ffd63ed 78 * @return The quaternion q such that q * (*this) == (*this) * q
Jamie Smith 1:aac28ffd63ed 79 * == [ 0 0 0 1 ]<sup>T</sup>.
Jamie Smith 1:aac28ffd63ed 80 */
Jamie Smith 1:aac28ffd63ed 81 Quaternion inverse(void) const {
Jamie Smith 1:aac28ffd63ed 82 return conjugate() / norm();
Jamie Smith 1:aac28ffd63ed 83 }
Jamie Smith 1:aac28ffd63ed 84
Jamie Smith 1:aac28ffd63ed 85
Jamie Smith 1:aac28ffd63ed 86 /**
Jamie Smith 1:aac28ffd63ed 87 * @brief Computes the product of this quaternion with the
Jamie Smith 1:aac28ffd63ed 88 * quaternion 'rhs'.
Jamie Smith 1:aac28ffd63ed 89 *
Jamie Smith 1:aac28ffd63ed 90 * @param rhs The right-hand-side of the product operation.
Jamie Smith 1:aac28ffd63ed 91 *
Jamie Smith 1:aac28ffd63ed 92 * @return The quaternion product (*this) x @p rhs.
Jamie Smith 1:aac28ffd63ed 93 */
Jamie Smith 1:aac28ffd63ed 94 Quaternion product(const Quaternion& rhs) const {
Jamie Smith 1:aac28ffd63ed 95 return Quaternion(y()*rhs.z() - z()*rhs.y() + x()*rhs.w() + w()*rhs.x(),
Jamie Smith 1:aac28ffd63ed 96 z()*rhs.x() - x()*rhs.z() + y()*rhs.w() + w()*rhs.y(),
Jamie Smith 1:aac28ffd63ed 97 x()*rhs.y() - y()*rhs.x() + z()*rhs.w() + w()*rhs.z(),
Jamie Smith 1:aac28ffd63ed 98 w()*rhs.w() - x()*rhs.x() - y()*rhs.y() - z()*rhs.z());
Jamie Smith 1:aac28ffd63ed 99 }
Jamie Smith 1:aac28ffd63ed 100
Jamie Smith 1:aac28ffd63ed 101 /**
Jamie Smith 1:aac28ffd63ed 102 * @brief Quaternion product operator.
Jamie Smith 1:aac28ffd63ed 103 *
Jamie Smith 1:aac28ffd63ed 104 * The result is a quaternion such that:
Jamie Smith 1:aac28ffd63ed 105 *
Jamie Smith 1:aac28ffd63ed 106 * result.real() = (*this).real() * rhs.real() -
Jamie Smith 1:aac28ffd63ed 107 * (*this).complex().dot(rhs.complex());
Jamie Smith 1:aac28ffd63ed 108 *
Jamie Smith 1:aac28ffd63ed 109 * and:
Jamie Smith 1:aac28ffd63ed 110 *
Jamie Smith 1:aac28ffd63ed 111 * result.complex() = rhs.complex() * (*this).real
Jamie Smith 1:aac28ffd63ed 112 * + (*this).complex() * rhs.real()
Jamie Smith 1:aac28ffd63ed 113 * - (*this).complex().cross(rhs.complex());
Jamie Smith 1:aac28ffd63ed 114 *
Jamie Smith 1:aac28ffd63ed 115 * @return The quaternion product (*this) x rhs.
Jamie Smith 1:aac28ffd63ed 116 */
Jamie Smith 1:aac28ffd63ed 117 Quaternion operator*(const Quaternion& rhs) const {
Jamie Smith 1:aac28ffd63ed 118 return product(rhs);
Jamie Smith 1:aac28ffd63ed 119 }
Jamie Smith 1:aac28ffd63ed 120
Jamie Smith 1:aac28ffd63ed 121 /**
Jamie Smith 1:aac28ffd63ed 122 * @brief Quaternion scalar product operator.
Jamie Smith 1:aac28ffd63ed 123 * @param s A scalar by which to multiply all components
Jamie Smith 1:aac28ffd63ed 124 * of this quaternion.
Jamie Smith 1:aac28ffd63ed 125 * @return The quaternion (*this) * s.
Jamie Smith 1:aac28ffd63ed 126 */
Jamie Smith 1:aac28ffd63ed 127 Quaternion operator*(FloatType s) const {
Jamie Smith 1:aac28ffd63ed 128 return Quaternion(complex()*s, real()*s);
Jamie Smith 1:aac28ffd63ed 129 }
Jamie Smith 1:aac28ffd63ed 130
Jamie Smith 1:aac28ffd63ed 131 /**
Jamie Smith 1:aac28ffd63ed 132 * @brief Produces the sum of this quaternion and rhs.
Jamie Smith 1:aac28ffd63ed 133 */
Jamie Smith 1:aac28ffd63ed 134 Quaternion operator+(const Quaternion& rhs) const {
Jamie Smith 1:aac28ffd63ed 135 return Quaternion(x()+rhs.x(), y()+rhs.y(), z()+rhs.z(), w()+rhs.w());
Jamie Smith 1:aac28ffd63ed 136 }
Jamie Smith 1:aac28ffd63ed 137
Jamie Smith 1:aac28ffd63ed 138 /**
Jamie Smith 1:aac28ffd63ed 139 * @brief Produces the difference of this quaternion and rhs.
Jamie Smith 1:aac28ffd63ed 140 */
Jamie Smith 1:aac28ffd63ed 141 Quaternion operator-(const Quaternion& rhs) const {
Jamie Smith 1:aac28ffd63ed 142 return Quaternion(x()-rhs.x(), y()-rhs.y(), z()-rhs.z(), w()-rhs.w());
Jamie Smith 1:aac28ffd63ed 143 }
Jamie Smith 1:aac28ffd63ed 144
Jamie Smith 1:aac28ffd63ed 145 /**
Jamie Smith 1:aac28ffd63ed 146 * @brief Unary negation.
Jamie Smith 1:aac28ffd63ed 147 */
Jamie Smith 1:aac28ffd63ed 148 Quaternion operator-() const {
Jamie Smith 1:aac28ffd63ed 149 return Quaternion(-x(), -y(), -z(), -w());
Jamie Smith 1:aac28ffd63ed 150 }
Jamie Smith 1:aac28ffd63ed 151
Jamie Smith 1:aac28ffd63ed 152 /**
Jamie Smith 1:aac28ffd63ed 153 * @brief Quaternion scalar division operator.
Jamie Smith 1:aac28ffd63ed 154 * @param s A scalar by which to divide all components
Jamie Smith 1:aac28ffd63ed 155 * of this quaternion.
Jamie Smith 1:aac28ffd63ed 156 * @return The quaternion (*this) / s.
Jamie Smith 1:aac28ffd63ed 157 */
Jamie Smith 1:aac28ffd63ed 158 Quaternion operator/(FloatType s) const {
Jamie Smith 1:aac28ffd63ed 159 MBED_ASSERT(s != 0);
Jamie Smith 1:aac28ffd63ed 160 return Quaternion(complex()/s, real()/s);
Jamie Smith 1:aac28ffd63ed 161 }
Jamie Smith 1:aac28ffd63ed 162
Jamie Smith 1:aac28ffd63ed 163 /**
Jamie Smith 1:aac28ffd63ed 164 * @brief Returns a matrix representation of this
Jamie Smith 1:aac28ffd63ed 165 * quaternion.
Jamie Smith 1:aac28ffd63ed 166 *
Jamie Smith 1:aac28ffd63ed 167 * Specifically this is the matrix such that:
Jamie Smith 1:aac28ffd63ed 168 *
Jamie Smith 1:aac28ffd63ed 169 * this->matrix() * q.vector() = (*this) * q for any quaternion q.
Jamie Smith 1:aac28ffd63ed 170 *
Jamie Smith 1:aac28ffd63ed 171 * Note that this is @e NOT the rotation matrix that may be
Jamie Smith 1:aac28ffd63ed 172 * represented by a unit quaternion.
Jamie Smith 1:aac28ffd63ed 173 */
Jamie Smith 1:aac28ffd63ed 174 TMatrix4 matrix() const {
Jamie Smith 1:aac28ffd63ed 175 FloatType m[16] = {
Jamie Smith 1:aac28ffd63ed 176 w(), -z(), y(), x(),
Jamie Smith 1:aac28ffd63ed 177 z(), w(), -x(), y(),
Jamie Smith 1:aac28ffd63ed 178 -y(), x(), w(), z(),
Jamie Smith 1:aac28ffd63ed 179 -x(), -y(), -z(), w()
Jamie Smith 1:aac28ffd63ed 180 };
Jamie Smith 1:aac28ffd63ed 181 return TMatrix4(m);
Jamie Smith 1:aac28ffd63ed 182 }
Jamie Smith 1:aac28ffd63ed 183
Jamie Smith 1:aac28ffd63ed 184 /**
Jamie Smith 1:aac28ffd63ed 185 * @brief Returns a matrix representation of this
Jamie Smith 1:aac28ffd63ed 186 * quaternion for right multiplication.
Jamie Smith 1:aac28ffd63ed 187 *
Jamie Smith 1:aac28ffd63ed 188 * Specifically this is the matrix such that:
Jamie Smith 1:aac28ffd63ed 189 *
Jamie Smith 1:aac28ffd63ed 190 * q.vector().transpose() * this->matrix() = (q *
Jamie Smith 1:aac28ffd63ed 191 * (*this)).vector().transpose() for any quaternion q.
Jamie Smith 1:aac28ffd63ed 192 *
Jamie Smith 1:aac28ffd63ed 193 * Note that this is @e NOT the rotation matrix that may be
Jamie Smith 1:aac28ffd63ed 194 * represented by a unit quaternion.
Jamie Smith 1:aac28ffd63ed 195 */
Jamie Smith 1:aac28ffd63ed 196 TMatrix4 rightMatrix() const {
Jamie Smith 1:aac28ffd63ed 197 FloatType m[16] = {
Jamie Smith 1:aac28ffd63ed 198 +w(), -z(), y(), -x(),
Jamie Smith 1:aac28ffd63ed 199 +z(), w(), -x(), -y(),
Jamie Smith 1:aac28ffd63ed 200 -y(), x(), w(), -z(),
Jamie Smith 1:aac28ffd63ed 201 +x(), y(), z(), w()
Jamie Smith 1:aac28ffd63ed 202 };
Jamie Smith 1:aac28ffd63ed 203 return TMatrix4(m);
Jamie Smith 1:aac28ffd63ed 204 }
Jamie Smith 1:aac28ffd63ed 205
Jamie Smith 1:aac28ffd63ed 206 /**
Jamie Smith 1:aac28ffd63ed 207 * @brief Returns this quaternion as a 4-vector.
Jamie Smith 1:aac28ffd63ed 208 *
Jamie Smith 1:aac28ffd63ed 209 * This is simply the vector [x y z w]<sup>T</sup>
Jamie Smith 1:aac28ffd63ed 210 */
Jamie Smith 1:aac28ffd63ed 211 TVector4 vector() const { return TVector4(mData); }
Jamie Smith 1:aac28ffd63ed 212
Jamie Smith 1:aac28ffd63ed 213 /**
Jamie Smith 1:aac28ffd63ed 214 * @brief Returns the norm ("magnitude") of the quaternion.
Jamie Smith 1:aac28ffd63ed 215 * @return The 2-norm of [ w(), x(), y(), z() ]<sup>T</sup>.
Jamie Smith 1:aac28ffd63ed 216 */
Jamie Smith 1:aac28ffd63ed 217 FloatType norm() const { return sqrt(mData[0]*mData[0]+mData[1]*mData[1]+
Jamie Smith 1:aac28ffd63ed 218 mData[2]*mData[2]+mData[3]*mData[3]); }
Jamie Smith 1:aac28ffd63ed 219
Jamie Smith 1:aac28ffd63ed 220 /**
Jamie Smith 1:aac28ffd63ed 221 * @brief Computes the rotation matrix represented by a unit
Jamie Smith 1:aac28ffd63ed 222 * quaternion.
Jamie Smith 1:aac28ffd63ed 223 *
Jamie Smith 1:aac28ffd63ed 224 * @note This does not check that this quaternion is normalized.
Jamie Smith 1:aac28ffd63ed 225 * It formulaically returns the matrix, which will not be a
Jamie Smith 1:aac28ffd63ed 226 * rotation if the quaternion is non-unit.
Jamie Smith 1:aac28ffd63ed 227 */
Jamie Smith 1:aac28ffd63ed 228 TMatrix3 rotationMatrix() const {
Jamie Smith 1:aac28ffd63ed 229 FloatType m[9] = {
Jamie Smith 1:aac28ffd63ed 230 1-2*y()*y()-2*z()*z(), 2*x()*y() - 2*z()*w(), 2*x()*z() + 2*y()*w(),
Jamie Smith 1:aac28ffd63ed 231 2*x()*y() + 2*z()*w(), 1-2*x()*x()-2*z()*z(), 2*y()*z() - 2*x()*w(),
Jamie Smith 1:aac28ffd63ed 232 2*x()*z() - 2*y()*w(), 2*y()*z() + 2*x()*w(), 1-2*x()*x()-2*y()*y()
Jamie Smith 1:aac28ffd63ed 233 };
Jamie Smith 1:aac28ffd63ed 234 return TMatrix3(m);
Jamie Smith 1:aac28ffd63ed 235 }
Jamie Smith 1:aac28ffd63ed 236 /**
Jamie Smith 1:aac28ffd63ed 237 * @brief Sets quaternion to be same as rotation by scaled axis w.
Jamie Smith 1:aac28ffd63ed 238 */
Jamie Smith 1:aac28ffd63ed 239 void scaledAxis(const TVector3& w) {
Jamie Smith 1:aac28ffd63ed 240 FloatType theta = w.norm();
Jamie Smith 1:aac28ffd63ed 241 if (theta > 0.0001) {
Jamie Smith 1:aac28ffd63ed 242 FloatType s = sin(theta / 2.0);
Jamie Smith 1:aac28ffd63ed 243 TVector3 W(w / theta * s);
Jamie Smith 1:aac28ffd63ed 244 mData[0] = W[0];
Jamie Smith 1:aac28ffd63ed 245 mData[1] = W[1];
Jamie Smith 1:aac28ffd63ed 246 mData[2] = W[2];
Jamie Smith 1:aac28ffd63ed 247 mData[3] = cos(theta / 2.0);
Jamie Smith 1:aac28ffd63ed 248 } else {
Jamie Smith 1:aac28ffd63ed 249 mData[0]=mData[1]=mData[2]=0;
Jamie Smith 1:aac28ffd63ed 250 mData[3]=1.0;
Jamie Smith 1:aac28ffd63ed 251 }
Jamie Smith 1:aac28ffd63ed 252 }
Jamie Smith 1:aac28ffd63ed 253
Jamie Smith 1:aac28ffd63ed 254 /**
Jamie Smith 1:aac28ffd63ed 255 * @brief Returns a vector rotated by this quaternion.
Jamie Smith 1:aac28ffd63ed 256 *
Jamie Smith 1:aac28ffd63ed 257 * Functionally equivalent to: (rotationMatrix() * v)
Jamie Smith 1:aac28ffd63ed 258 * or (q * Quaternion(0, v) * q.inverse()).
Jamie Smith 1:aac28ffd63ed 259 *
Jamie Smith 1:aac28ffd63ed 260 * @warning conjugate() is used instead of inverse() for better
Jamie Smith 1:aac28ffd63ed 261 * performance, when this quaternion must be normalized.
Jamie Smith 1:aac28ffd63ed 262 */
Jamie Smith 1:aac28ffd63ed 263 TVector3 rotatedVector(const TVector3& v) const {
Jamie Smith 1:aac28ffd63ed 264 return (((*this) * Quaternion(v, 0)) * conjugate()).complex();
Jamie Smith 1:aac28ffd63ed 265 }
Jamie Smith 1:aac28ffd63ed 266
Jamie Smith 1:aac28ffd63ed 267
Jamie Smith 1:aac28ffd63ed 268
Jamie Smith 1:aac28ffd63ed 269 /**
Jamie Smith 1:aac28ffd63ed 270 * @brief Computes the quaternion that is equivalent to a given
Jamie Smith 1:aac28ffd63ed 271 * euler angle rotation.
Jamie Smith 1:aac28ffd63ed 272 * @param euler A 3-vector in order: roll-pitch-yaw.
Jamie Smith 1:aac28ffd63ed 273 */
Jamie Smith 1:aac28ffd63ed 274 void euler(const TVector3& euler) {
Jamie Smith 1:aac28ffd63ed 275 FloatType c1 = cos(euler[2] * 0.5);
Jamie Smith 1:aac28ffd63ed 276 FloatType c2 = cos(euler[1] * 0.5);
Jamie Smith 1:aac28ffd63ed 277 FloatType c3 = cos(euler[0] * 0.5);
Jamie Smith 1:aac28ffd63ed 278 FloatType s1 = sin(euler[2] * 0.5);
Jamie Smith 1:aac28ffd63ed 279 FloatType s2 = sin(euler[1] * 0.5);
Jamie Smith 1:aac28ffd63ed 280 FloatType s3 = sin(euler[0] * 0.5);
Jamie Smith 1:aac28ffd63ed 281
Jamie Smith 1:aac28ffd63ed 282 mData[0] = c1*c2*s3 - s1*s2*c3;
Jamie Smith 1:aac28ffd63ed 283 mData[1] = c1*s2*c3 + s1*c2*s3;
Jamie Smith 1:aac28ffd63ed 284 mData[2] = s1*c2*c3 - c1*s2*s3;
Jamie Smith 1:aac28ffd63ed 285 mData[3] = c1*c2*c3 + s1*s2*s3;
Jamie Smith 1:aac28ffd63ed 286 }
Jamie Smith 1:aac28ffd63ed 287
Jamie Smith 1:aac28ffd63ed 288 /** @brief Returns an equivalent euler angle representation of
Jamie Smith 1:aac28ffd63ed 289 * this quaternion.
Jamie Smith 1:aac28ffd63ed 290 * @return Euler angles in roll-pitch-yaw order.
Jamie Smith 1:aac28ffd63ed 291 */
Jamie Smith 1:aac28ffd63ed 292 TVector3 euler(void) const {
Jamie Smith 1:aac28ffd63ed 293 TVector3 euler;
Jamie Smith 1:aac28ffd63ed 294 const static FloatType PI_OVER_2 = M_PI * 0.5;
Jamie Smith 1:aac28ffd63ed 295 const static FloatType EPSILON = 1e-10;
Jamie Smith 1:aac28ffd63ed 296 FloatType sqw, sqx, sqy, sqz;
Jamie Smith 1:aac28ffd63ed 297
Jamie Smith 1:aac28ffd63ed 298 // quick conversion to Euler angles to give tilt to user
Jamie Smith 1:aac28ffd63ed 299 sqw = mData[3]*mData[3];
Jamie Smith 1:aac28ffd63ed 300 sqx = mData[0]*mData[0];
Jamie Smith 1:aac28ffd63ed 301 sqy = mData[1]*mData[1];
Jamie Smith 1:aac28ffd63ed 302 sqz = mData[2]*mData[2];
Jamie Smith 1:aac28ffd63ed 303
Jamie Smith 1:aac28ffd63ed 304 euler[1] = asin(2.0 * (mData[3]*mData[1] - mData[0]*mData[2]));
Jamie Smith 1:aac28ffd63ed 305 if (PI_OVER_2 - fabs(euler[1]) > EPSILON) {
Jamie Smith 5:c75be9000d75 306 euler[2] = atan2(static_cast<FloatType>(2.0) * (mData[0]*mData[1] + mData[3]*mData[2]),
Jamie Smith 1:aac28ffd63ed 307 sqx - sqy - sqz + sqw);
Jamie Smith 5:c75be9000d75 308 euler[0] = atan2(static_cast<FloatType>(2.0) * (mData[3]*mData[0] + mData[1]*mData[2]),
Jamie Smith 1:aac28ffd63ed 309 sqw - sqx - sqy + sqz);
Jamie Smith 1:aac28ffd63ed 310 } else {
Jamie Smith 1:aac28ffd63ed 311 // compute heading from local 'down' vector
Jamie Smith 1:aac28ffd63ed 312 euler[2] = atan2(2*mData[1]*mData[2] - 2*mData[0]*mData[3],
Jamie Smith 1:aac28ffd63ed 313 2*mData[0]*mData[2] + 2*mData[1]*mData[3]);
Jamie Smith 1:aac28ffd63ed 314 euler[0] = 0.0;
Jamie Smith 1:aac28ffd63ed 315
Jamie Smith 1:aac28ffd63ed 316 // If facing down, reverse yaw
Jamie Smith 1:aac28ffd63ed 317 if (euler[1] < 0)
Jamie Smith 1:aac28ffd63ed 318 euler[2] = M_PI - euler[2];
Jamie Smith 1:aac28ffd63ed 319 }
Jamie Smith 1:aac28ffd63ed 320 return euler;
Jamie Smith 1:aac28ffd63ed 321 }
Jamie Smith 1:aac28ffd63ed 322
Jamie Smith 1:aac28ffd63ed 323 /**
Jamie Smith 1:aac28ffd63ed 324 * @brief Returns pointer to the internal array.
Jamie Smith 1:aac28ffd63ed 325 *
Jamie Smith 1:aac28ffd63ed 326 * Array is in order x,y,z,w.
Jamie Smith 1:aac28ffd63ed 327 */
Jamie Smith 1:aac28ffd63ed 328 FloatType* row(uint32_t i) { return mData + i; }
Jamie Smith 1:aac28ffd63ed 329 // Const version of the above.
Jamie Smith 1:aac28ffd63ed 330 const FloatType* row(uint32_t i) const { return mData + i; }
Jamie Smith 1:aac28ffd63ed 331 };
Jamie Smith 1:aac28ffd63ed 332
Jamie Smith 1:aac28ffd63ed 333 /**
Jamie Smith 1:aac28ffd63ed 334 * @brief Global operator allowing left-multiply by scalar.
Jamie Smith 1:aac28ffd63ed 335 */
Jamie Smith 1:aac28ffd63ed 336 Quaternion operator*(Quaternion::FloatType s, const Quaternion& q);
Jamie Smith 1:aac28ffd63ed 337
Jamie Smith 1:aac28ffd63ed 338
Jamie Smith 1:aac28ffd63ed 339 #endif /* QUATERNION_H */