Elecia White / Mbed 2 deprecated freedom_inertial_hello_world

A hello world for inertial sensors

Dependencies:   FXAS21000 FXOS8700Q mbed

Fork of FRDM-STBC-AGM01 by angus taggart

Adafruit_9DOF.cpp@4:5ab2bb2f062b, 2015-06-30 (annotated)

Committer:
Elecia
Date:
Tue Jun 30 16:23:32 2015 +0000
Revision:
4:5ab2bb2f062b
Parent:
3:123b546e4a5c 
Added element14 link

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Elecia 3:123b546e4a5c 1 /***************************************************************************
Elecia 3:123b546e4a5c 2 ** https://github.com/adafruit/Adafruit_9DOF/blob/master/Adafruit_9DOF.cpp
Elecia 3:123b546e4a5c 3 ** Modifed Adafruit's orientation code to get only the tilt compensated heading
Elecia 3:123b546e4a5c 4 ***************************************************************************/
Elecia 3:123b546e4a5c 5 /***************************************************************************
Elecia 3:123b546e4a5c 6 This is a library for the Adafruit 9DOF Breakout
Elecia 3:123b546e4a5c 7 Designed specifically to work with the Adafruit 9DOF Breakout:
Elecia 3:123b546e4a5c 8 http://www.adafruit.com/products/1714
Elecia 3:123b546e4a5c 9 These displays use I2C to communicate, 2 pins are required to interface.
Elecia 3:123b546e4a5c 10 Adafruit invests time and resources providing this open source code,
Elecia 3:123b546e4a5c 11 please support Adafruit andopen-source hardware by purchasing products
Elecia 3:123b546e4a5c 12 from Adafruit!
Elecia 3:123b546e4a5c 13 Written by Kevin Townsend for Adafruit Industries.
Elecia 3:123b546e4a5c 14 BSD license, all text above must be included in any redistribution
Elecia 3:123b546e4a5c 15 ***************************************************************************/
Elecia 3:123b546e4a5c 16
Elecia 3:123b546e4a5c 17 #include <math.h>
Elecia 3:123b546e4a5c 18 #include "Adafruit_9DOF.h"
Elecia 3:123b546e4a5c 19
Elecia 3:123b546e4a5c 20 #define SENSORS_GRAVITY_EARTH (9.80665F)
Elecia 3:123b546e4a5c 21 #define MILLIG_TO_M_PER_S2 (SENSORS_GRAVITY_EARTH/1000.0F)
Elecia 3:123b546e4a5c 22 #define MILLIGAUSS_TO_MICROTESLA (10.0F)
Elecia 3:123b546e4a5c 23 /**************************************************************************/
Elecia 3:123b546e4a5c 24 /*!
Elecia 3:123b546e4a5c 25 @brief Populates the .roll/.pitch/.heading fields in the sensors_vec_t
Elecia 3:123b546e4a5c 26 struct with the right angular data (in degree).
Elecia 3:123b546e4a5c 27 The starting position is set by placing the object flat and
Elecia 3:123b546e4a5c 28 pointing northwards (Z-axis pointing upward and X-axis pointing
Elecia 3:123b546e4a5c 29 northwards).
Elecia 3:123b546e4a5c 30 The orientation of the object can be modeled as resulting from
Elecia 3:123b546e4a5c 31 3 consecutive rotations in turn: heading (Z-axis), pitch (Y-axis),
Elecia 3:123b546e4a5c 32 and roll (X-axis) applied to the starting position.
Elecia 3:123b546e4a5c 33 @param accel The vector variable containing the
Elecia 3:123b546e4a5c 34 data from the accelerometer
Elecia 3:123b546e4a5c 35 @param mag The vector variable containing the
Elecia 3:123b546e4a5c 36 data from the magnetometer
Elecia 3:123b546e4a5c 37 @param orientation The sensors_vec_t object that will have it's
Elecia 3:123b546e4a5c 38 .roll, .pitch and .heading fields populated
Elecia 3:123b546e4a5c 39 */
Elecia 3:123b546e4a5c 40 /**************************************************************************/
Elecia 3:123b546e4a5c 41 bool fusionGetOrientation(int16_t *accel, int16_t *mag, tOrientation *orientation)
Elecia 3:123b546e4a5c 42 {
Elecia 3:123b546e4a5c 43 /* Make sure the input is valid, not null, etc. */
Elecia 3:123b546e4a5c 44 if ( accel == NULL) return false;
Elecia 3:123b546e4a5c 45 if ( mag == NULL) return false;
Elecia 3:123b546e4a5c 46 if ( orientation == NULL) return false;
Elecia 3:123b546e4a5c 47
Elecia 3:123b546e4a5c 48 float const PI_F = 3.14159265F;
Elecia 3:123b546e4a5c 49
Elecia 3:123b546e4a5c 50 double aX = ((double) accel[X]) * MILLIG_TO_M_PER_S2;
Elecia 3:123b546e4a5c 51 double aY = ((double) accel[Y]) * MILLIG_TO_M_PER_S2;
Elecia 3:123b546e4a5c 52 double aZ = ((double) accel[Z]) * MILLIG_TO_M_PER_S2;
Elecia 3:123b546e4a5c 53
Elecia 3:123b546e4a5c 54 double mX = ((double) mag[X]) * MILLIGAUSS_TO_MICROTESLA;
Elecia 3:123b546e4a5c 55 double mY = ((double) mag[Y]) * MILLIGAUSS_TO_MICROTESLA;
Elecia 3:123b546e4a5c 56 double mZ = ((double) mag[Z]) * MILLIGAUSS_TO_MICROTESLA;
Elecia 3:123b546e4a5c 57
Elecia 3:123b546e4a5c 58
Elecia 3:123b546e4a5c 59 /* roll: Rotation around the X-axis. -180 <= roll <= 180 */
Elecia 3:123b546e4a5c 60 /* a positive roll angle is defined to be a clockwise rotation about the positive X-axis */
Elecia 3:123b546e4a5c 61 /* */
Elecia 3:123b546e4a5c 62 /* y */
Elecia 3:123b546e4a5c 63 /* roll = atan2(---) */
Elecia 3:123b546e4a5c 64 /* z */
Elecia 3:123b546e4a5c 65 /* */
Elecia 3:123b546e4a5c 66 /* where: y, z are returned value from accelerometer sensor */
Elecia 3:123b546e4a5c 67 orientation->roll = (float)atan2(aY, aZ);
Elecia 3:123b546e4a5c 68
Elecia 3:123b546e4a5c 69 /* pitch: Rotation around the Y-axis. -180 <= roll <= 180 */
Elecia 3:123b546e4a5c 70 /* a positive pitch angle is defined to be a clockwise rotation about the positive Y-axis */
Elecia 3:123b546e4a5c 71 /* */
Elecia 3:123b546e4a5c 72 /* x */
Elecia 3:123b546e4a5c 73 /* roll = atan(-----------------) */
Elecia 3:123b546e4a5c 74 /* sqrt(y^2 + z^2) */
Elecia 3:123b546e4a5c 75 /* where: x, y, z are returned value from accelerometer sensor */
Elecia 3:123b546e4a5c 76
Elecia 3:123b546e4a5c 77 double t_pitch = (aY*aY) + (aZ*aZ);
Elecia 3:123b546e4a5c 78 float signOfZ = aZ >= 0 ? 1.0F : -1.0F;
Elecia 3:123b546e4a5c 79 orientation->pitch = (float)atan2(aX, signOfZ * sqrt(t_pitch));
Elecia 3:123b546e4a5c 80
Elecia 3:123b546e4a5c 81
Elecia 3:123b546e4a5c 82 /* heading: Rotation around the Z-axis. -180 <= roll <= 180 */
Elecia 3:123b546e4a5c 83 /* a positive heading angle is defined to be a clockwise rotation about the positive Z-axis */
Elecia 3:123b546e4a5c 84 /* */
Elecia 3:123b546e4a5c 85 /* z * sin(roll) - y * cos(roll) */
Elecia 3:123b546e4a5c 86 /* heading = atan2(--------------------------------------------------------------------------) */
Elecia 3:123b546e4a5c 87 /* x * cos(pitch) + y * sin(pitch) * sin(roll) + z * sin(pitch) * cos(roll)) */
Elecia 3:123b546e4a5c 88 /* */
Elecia 3:123b546e4a5c 89 /* where: x, y, z are returned value from magnetometer sensor */
Elecia 3:123b546e4a5c 90 orientation->heading = (float)atan2(mZ * sin(orientation->roll) - mY * cos(orientation->roll), \
Elecia 3:123b546e4a5c 91 mX * cos(orientation->pitch) + \
Elecia 3:123b546e4a5c 92 mY * sin(orientation->pitch) * sin(orientation->roll) + \
Elecia 3:123b546e4a5c 93 mZ * sin(orientation->pitch) * cos(orientation->roll));
Elecia 3:123b546e4a5c 94
Elecia 3:123b546e4a5c 95
Elecia 3:123b546e4a5c 96 orientation->heading = (float)atan2(mY, mX);
Elecia 3:123b546e4a5c 97
Elecia 3:123b546e4a5c 98 /* Convert angular data to degree */
Elecia 3:123b546e4a5c 99 orientation->roll = orientation->roll * 180 / PI_F;
Elecia 3:123b546e4a5c 100 orientation->pitch = orientation->pitch * 180 / PI_F;
Elecia 3:123b546e4a5c 101 orientation->heading = orientation->heading * 180 / PI_F;
Elecia 3:123b546e4a5c 102
Elecia 3:123b546e4a5c 103 return true;
Elecia 3:123b546e4a5c 104 }