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:
Thu Jan 30 02:16:15 2020 -0800
Revision:
4:70d05578f041
Parent:
2:2269b723d16a
Child:
5:c75be9000d75
Fix a couple more minor issues

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