Jatin Sharma / Mbed 2 deprecated RaheeNew

RaheeNew

Dependencies:   mbed

Dependents:   RaheeNew

Fork of Adafruit9-DOf by Bruno Manganelli

Source/Adafruit_9DOF.cpp@1:c3381056a1c6, 2015-11-28 (annotated)

Committer:
jatinsha
Date:
Sat Nov 28 10:06:40 2015 +0000
Revision:
1:c3381056a1c6
Parent:
0:772bf4786416 
js

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bmanga95 0:772bf4786416 1 /***************************************************************************
bmanga95 0:772bf4786416 2 This is a library for the Adafruit 9DOF Breakout
bmanga95 0:772bf4786416 3
bmanga95 0:772bf4786416 4 Designed specifically to work with the Adafruit 9DOF Breakout:
bmanga95 0:772bf4786416 5 http://www.adafruit.com/products/1714
bmanga95 0:772bf4786416 6
bmanga95 0:772bf4786416 7 These displays use I2C to communicate, 2 pins are required to interface.
bmanga95 0:772bf4786416 8
bmanga95 0:772bf4786416 9 Adafruit invests time and resources providing this open source code,
bmanga95 0:772bf4786416 10 please support Adafruit andopen-source hardware by purchasing products
bmanga95 0:772bf4786416 11 from Adafruit!
bmanga95 0:772bf4786416 12
bmanga95 0:772bf4786416 13 Written by Kevin Townsend for Adafruit Industries.
bmanga95 0:772bf4786416 14 BSD license, all text above must be included in any redistribution
bmanga95 0:772bf4786416 15 ***************************************************************************/
bmanga95 0:772bf4786416 16
bmanga95 0:772bf4786416 17 #include <math.h>
bmanga95 0:772bf4786416 18 #include "Adafruit_9DOF.h"
bmanga95 0:772bf4786416 19 #define PI (3.14159265F);
bmanga95 0:772bf4786416 20
bmanga95 0:772bf4786416 21 /***************************************************************************
bmanga95 0:772bf4786416 22 PRIVATE FUNCTIONS
bmanga95 0:772bf4786416 23 ***************************************************************************/
bmanga95 0:772bf4786416 24
bmanga95 0:772bf4786416 25
bmanga95 0:772bf4786416 26 /***************************************************************************
bmanga95 0:772bf4786416 27 CONSTRUCTOR
bmanga95 0:772bf4786416 28 ***************************************************************************/
bmanga95 0:772bf4786416 29
bmanga95 0:772bf4786416 30 /**************************************************************************/
bmanga95 0:772bf4786416 31 /*!
bmanga95 0:772bf4786416 32 @brief Instantiates a new Adafruit_9DOF class
bmanga95 0:772bf4786416 33 */
bmanga95 0:772bf4786416 34 /**************************************************************************/
bmanga95 0:772bf4786416 35 Adafruit_9DOF::Adafruit_9DOF(void)
bmanga95 0:772bf4786416 36 {
bmanga95 0:772bf4786416 37 }
bmanga95 0:772bf4786416 38
bmanga95 0:772bf4786416 39 /***************************************************************************
bmanga95 0:772bf4786416 40 PUBLIC FUNCTIONS
bmanga95 0:772bf4786416 41 ***************************************************************************/
bmanga95 0:772bf4786416 42
bmanga95 0:772bf4786416 43 /**************************************************************************/
bmanga95 0:772bf4786416 44 /*!
bmanga95 0:772bf4786416 45 @brief Setups the HW
bmanga95 0:772bf4786416 46 */
bmanga95 0:772bf4786416 47 /**************************************************************************/
jatinsha 1:c3381056a1c6 48 bool Adafruit_9DOF::begin(){
bmanga95 0:772bf4786416 49 // Enable I2C
bmanga95 0:772bf4786416 50 return true;
bmanga95 0:772bf4786416 51 }
bmanga95 0:772bf4786416 52
bmanga95 0:772bf4786416 53 /**************************************************************************/
bmanga95 0:772bf4786416 54 /*!
bmanga95 0:772bf4786416 55 @brief Populates the .pitch/.roll fields in the sensors_vec_t struct
bmanga95 0:772bf4786416 56 with the right angular data (in degree)
bmanga95 0:772bf4786416 57
bmanga95 0:772bf4786416 58 @param event The sensors_event_t variable containing the
bmanga95 0:772bf4786416 59 data from the accelerometer
bmanga95 0:772bf4786416 60 @param orientation The sensors_vec_t object that will have it's
bmanga95 0:772bf4786416 61 .pitch and .roll fields populated
bmanga95 0:772bf4786416 62 @return Returns true if the operation was successful, false if there
bmanga95 0:772bf4786416 63 was an error
bmanga95 0:772bf4786416 64
bmanga95 0:772bf4786416 65 @code
bmanga95 0:772bf4786416 66
bmanga95 0:772bf4786416 67 bool error;
bmanga95 0:772bf4786416 68 sensors_event_t event;
bmanga95 0:772bf4786416 69 sensors_vec_t orientation;
bmanga95 0:772bf4786416 70 ...
bmanga95 0:772bf4786416 71 lsm303accelGetSensorEvent(&event);
bmanga95 0:772bf4786416 72 error = accelGetOrientation(&event, &orientation);
bmanga95 0:772bf4786416 73
bmanga95 0:772bf4786416 74 @endcode
bmanga95 0:772bf4786416 75 */
bmanga95 0:772bf4786416 76 /**************************************************************************/
bmanga95 0:772bf4786416 77 bool Adafruit_9DOF::accelGetOrientation(sensors_event_t *event, sensors_vec_t *orientation)
bmanga95 0:772bf4786416 78 {
bmanga95 0:772bf4786416 79 /* Make sure the input is valid, not null, etc. */
bmanga95 0:772bf4786416 80 if (event == NULL) return false;
bmanga95 0:772bf4786416 81 if (orientation == NULL) return false;
bmanga95 0:772bf4786416 82
bmanga95 0:772bf4786416 83 float t_pitch;
bmanga95 0:772bf4786416 84 float t_roll;
bmanga95 0:772bf4786416 85 float signOfZ = event->acceleration.z >= 0 ? 1.0F : -1.0F;
bmanga95 0:772bf4786416 86
bmanga95 0:772bf4786416 87 /* roll: Rotation around the longitudinal axis (the plane body, 'X axis'). -90<=roll<=90 */
bmanga95 0:772bf4786416 88 /* roll is positive and increasing when moving downward */
bmanga95 0:772bf4786416 89 /* */
bmanga95 0:772bf4786416 90 /* y */
bmanga95 0:772bf4786416 91 /* roll = atan(-----------------) */
bmanga95 0:772bf4786416 92 /* sqrt(x^2 + z^2) */
bmanga95 0:772bf4786416 93 /* where: x, y, z are returned value from accelerometer sensor */
bmanga95 0:772bf4786416 94
bmanga95 0:772bf4786416 95 t_roll = event->acceleration.x * event->acceleration.x + event->acceleration.z * event->acceleration.z;
bmanga95 0:772bf4786416 96 orientation->roll = (float)atan2(event->acceleration.y, sqrt(t_roll)) * 180 / PI;
bmanga95 0:772bf4786416 97
bmanga95 0:772bf4786416 98 /* pitch: Rotation around the lateral axis (the wing span, 'Y axis'). -180<=pitch<=180) */
bmanga95 0:772bf4786416 99 /* pitch is positive and increasing when moving upwards */
bmanga95 0:772bf4786416 100 /* */
bmanga95 0:772bf4786416 101 /* x */
bmanga95 0:772bf4786416 102 /* roll = atan(-----------------) */
bmanga95 0:772bf4786416 103 /* sqrt(y^2 + z^2) */
bmanga95 0:772bf4786416 104 /* where: x, y, z are returned value from accelerometer sensor */
bmanga95 0:772bf4786416 105
bmanga95 0:772bf4786416 106 t_pitch = event->acceleration.y * event->acceleration.y + event->acceleration.z * event->acceleration.z;
bmanga95 0:772bf4786416 107 orientation->pitch = (float)atan2(event->acceleration.x, signOfZ * sqrt(t_pitch)) * 180 / PI;
bmanga95 0:772bf4786416 108
bmanga95 0:772bf4786416 109 return true;
bmanga95 0:772bf4786416 110 }
bmanga95 0:772bf4786416 111
bmanga95 0:772bf4786416 112
bmanga95 0:772bf4786416 113 /**************************************************************************/
bmanga95 0:772bf4786416 114 /*!
bmanga95 0:772bf4786416 115 @brief Utilize the sensor data from an accelerometer to compensate
bmanga95 0:772bf4786416 116 the magnetic sensor measurements when the sensor is tilted
bmanga95 0:772bf4786416 117 (the pitch and roll angles are not equal 0�)
bmanga95 0:772bf4786416 118
bmanga95 0:772bf4786416 119 @param axis The given axis (SENSOR_AXIS_X/Y/Z) that is
bmanga95 0:772bf4786416 120 parallel to the gravity of the Earth
bmanga95 0:772bf4786416 121
bmanga95 0:772bf4786416 122 @param mag_event The raw magnetometer data to adjust for tilt
bmanga95 0:772bf4786416 123
bmanga95 0:772bf4786416 124 @param accel_event The accelerometer event data to use to determine
bmanga95 0:772bf4786416 125 the tilt when compensating the mag_event values
bmanga95 0:772bf4786416 126
bmanga95 0:772bf4786416 127 @code
bmanga95 0:772bf4786416 128
bmanga95 0:772bf4786416 129 // Perform tilt compensation with matching accelerometer data
bmanga95 0:772bf4786416 130 sensors_event_t accel_event;
bmanga95 0:772bf4786416 131 error = lsm303accelGetSensorEvent(&accel_event);
bmanga95 0:772bf4786416 132 if (!error)
bmanga95 0:772bf4786416 133 {
bmanga95 0:772bf4786416 134 magTiltCompensation(SENSOR_AXIS_Z, &mag_event, &accel_event);
bmanga95 0:772bf4786416 135 }
bmanga95 0:772bf4786416 136
bmanga95 0:772bf4786416 137 @endcode
bmanga95 0:772bf4786416 138 */
bmanga95 0:772bf4786416 139 /**************************************************************************/
bmanga95 0:772bf4786416 140 bool Adafruit_9DOF::magTiltCompensation(sensors_axis_t axis, sensors_event_t *mag_event, sensors_event_t *accel_event)
bmanga95 0:772bf4786416 141 {
bmanga95 0:772bf4786416 142 /* Make sure the input is valid, not null, etc. */
bmanga95 0:772bf4786416 143 if (mag_event == NULL) return false;
bmanga95 0:772bf4786416 144 if (accel_event == NULL) return false;
bmanga95 0:772bf4786416 145
bmanga95 0:772bf4786416 146 float accel_X, accel_Y, accel_Z;
bmanga95 0:772bf4786416 147 float *mag_X, *mag_Y, *mag_Z;
bmanga95 0:772bf4786416 148
bmanga95 0:772bf4786416 149 switch (axis)
bmanga95 0:772bf4786416 150 {
bmanga95 0:772bf4786416 151 case SENSOR_AXIS_X:
bmanga95 0:772bf4786416 152 /* The X-axis is parallel to the gravity */
bmanga95 0:772bf4786416 153 accel_X = accel_event->acceleration.y;
bmanga95 0:772bf4786416 154 accel_Y = accel_event->acceleration.z;
bmanga95 0:772bf4786416 155 accel_Z = accel_event->acceleration.x;
bmanga95 0:772bf4786416 156 mag_X = &(mag_event->magnetic.y);
bmanga95 0:772bf4786416 157 mag_Y = &(mag_event->magnetic.z);
bmanga95 0:772bf4786416 158 mag_Z = &(mag_event->magnetic.x);
bmanga95 0:772bf4786416 159 break;
bmanga95 0:772bf4786416 160
bmanga95 0:772bf4786416 161 case SENSOR_AXIS_Y:
bmanga95 0:772bf4786416 162 /* The Y-axis is parallel to the gravity */
bmanga95 0:772bf4786416 163 accel_X = accel_event->acceleration.z;
bmanga95 0:772bf4786416 164 accel_Y = accel_event->acceleration.x;
bmanga95 0:772bf4786416 165 accel_Z = accel_event->acceleration.y;
bmanga95 0:772bf4786416 166 mag_X = &(mag_event->magnetic.z);
bmanga95 0:772bf4786416 167 mag_Y = &(mag_event->magnetic.x);
bmanga95 0:772bf4786416 168 mag_Z = &(mag_event->magnetic.y);
bmanga95 0:772bf4786416 169 break;
bmanga95 0:772bf4786416 170
bmanga95 0:772bf4786416 171 case SENSOR_AXIS_Z:
bmanga95 0:772bf4786416 172 /* The Z-axis is parallel to the gravity */
bmanga95 0:772bf4786416 173 accel_X = accel_event->acceleration.x;
bmanga95 0:772bf4786416 174 accel_Y = accel_event->acceleration.y;
bmanga95 0:772bf4786416 175 accel_Z = accel_event->acceleration.z;
bmanga95 0:772bf4786416 176 mag_X = &(mag_event->magnetic.x);
bmanga95 0:772bf4786416 177 mag_Y = &(mag_event->magnetic.y);
bmanga95 0:772bf4786416 178 mag_Z = &(mag_event->magnetic.z);
bmanga95 0:772bf4786416 179 break;
bmanga95 0:772bf4786416 180
bmanga95 0:772bf4786416 181 default:
bmanga95 0:772bf4786416 182 return false;
bmanga95 0:772bf4786416 183 }
bmanga95 0:772bf4786416 184
bmanga95 0:772bf4786416 185 float t_roll = accel_X * accel_X + accel_Z * accel_Z;
bmanga95 0:772bf4786416 186 float rollRadians = (float)atan2(accel_Y, sqrt(t_roll));
bmanga95 0:772bf4786416 187
bmanga95 0:772bf4786416 188 float t_pitch = accel_Y * accel_Y + accel_Z * accel_Z;
bmanga95 0:772bf4786416 189 float pitchRadians = (float)atan2(accel_X, sqrt(t_pitch));
bmanga95 0:772bf4786416 190
bmanga95 0:772bf4786416 191 float cosRoll = (float)cos(rollRadians);
bmanga95 0:772bf4786416 192 float sinRoll = (float)sin(rollRadians);
bmanga95 0:772bf4786416 193 float cosPitch = (float)cos(-1*pitchRadians);
bmanga95 0:772bf4786416 194 float sinPitch = (float)sin(-1*pitchRadians);
bmanga95 0:772bf4786416 195
bmanga95 0:772bf4786416 196 /* The tilt compensation algorithm */
bmanga95 0:772bf4786416 197 /* Xh = X.cosPitch + Z.sinPitch */
bmanga95 0:772bf4786416 198 /* Yh = X.sinRoll.sinPitch + Y.cosRoll - Z.sinRoll.cosPitch */
bmanga95 0:772bf4786416 199 *mag_X = (*mag_X) * cosPitch + (*mag_Z) * sinPitch;
bmanga95 0:772bf4786416 200 *mag_Y = (*mag_X) * sinRoll * sinPitch + (*mag_Y) * cosRoll - (*mag_Z) * sinRoll * cosPitch;
bmanga95 0:772bf4786416 201
bmanga95 0:772bf4786416 202 return true;
bmanga95 0:772bf4786416 203 }
bmanga95 0:772bf4786416 204
bmanga95 0:772bf4786416 205 /**************************************************************************/
bmanga95 0:772bf4786416 206 /*!
bmanga95 0:772bf4786416 207 @brief Populates the .heading fields in the sensors_vec_t
bmanga95 0:772bf4786416 208 struct with the right angular data (0-359�)
bmanga95 0:772bf4786416 209
bmanga95 0:772bf4786416 210 Heading increases when measuring clockwise
bmanga95 0:772bf4786416 211
bmanga95 0:772bf4786416 212 @param axis The given axis (SENSOR_AXIS_X/Y/Z)
bmanga95 0:772bf4786416 213
bmanga95 0:772bf4786416 214 @param event The raw magnetometer sensor data to use when
bmanga95 0:772bf4786416 215 calculating out heading
bmanga95 0:772bf4786416 216
bmanga95 0:772bf4786416 217 @param orientation The sensors_vec_t object where we will
bmanga95 0:772bf4786416 218 assign an 'orientation.heading' value
bmanga95 0:772bf4786416 219
bmanga95 0:772bf4786416 220 @code
bmanga95 0:772bf4786416 221
bmanga95 0:772bf4786416 222 magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation);
bmanga95 0:772bf4786416 223
bmanga95 0:772bf4786416 224 @endcode
bmanga95 0:772bf4786416 225 */
bmanga95 0:772bf4786416 226 /**************************************************************************/
bmanga95 0:772bf4786416 227 bool Adafruit_9DOF::magGetOrientation(sensors_axis_t axis, sensors_event_t *event, sensors_vec_t *orientation)
bmanga95 0:772bf4786416 228 {
bmanga95 0:772bf4786416 229 /* Make sure the input is valid, not null, etc. */
bmanga95 0:772bf4786416 230 if (event == NULL) return false;
bmanga95 0:772bf4786416 231 if (orientation == NULL) return false;
bmanga95 0:772bf4786416 232
bmanga95 0:772bf4786416 233 switch (axis)
bmanga95 0:772bf4786416 234 {
bmanga95 0:772bf4786416 235 case SENSOR_AXIS_X:
bmanga95 0:772bf4786416 236 /* Sensor rotates around X-axis */
bmanga95 0:772bf4786416 237 /* "heading" is the angle between the 'Y axis' and magnetic north on the horizontal plane (Oyz) */
bmanga95 0:772bf4786416 238 /* heading = atan(Mz / My) */
bmanga95 0:772bf4786416 239 orientation->heading = (float)atan2(event->magnetic.z, event->magnetic.y) * 180 / PI;
bmanga95 0:772bf4786416 240 break;
bmanga95 0:772bf4786416 241
bmanga95 0:772bf4786416 242 case SENSOR_AXIS_Y:
bmanga95 0:772bf4786416 243 /* Sensor rotates around Y-axis */
bmanga95 0:772bf4786416 244 /* "heading" is the angle between the 'Z axis' and magnetic north on the horizontal plane (Ozx) */
bmanga95 0:772bf4786416 245 /* heading = atan(Mx / Mz) */
bmanga95 0:772bf4786416 246 orientation->heading = (float)atan2(event->magnetic.x, event->magnetic.z) * 180 / PI;
bmanga95 0:772bf4786416 247 break;
bmanga95 0:772bf4786416 248
bmanga95 0:772bf4786416 249 case SENSOR_AXIS_Z:
bmanga95 0:772bf4786416 250 /* Sensor rotates around Z-axis */
bmanga95 0:772bf4786416 251 /* "heading" is the angle between the 'X axis' and magnetic north on the horizontal plane (Oxy) */
bmanga95 0:772bf4786416 252 /* heading = atan(My / Mx) */
bmanga95 0:772bf4786416 253 orientation->heading = (float)atan2(event->magnetic.y, event->magnetic.x) * 180 / PI;
bmanga95 0:772bf4786416 254 break;
bmanga95 0:772bf4786416 255
bmanga95 0:772bf4786416 256 default:
bmanga95 0:772bf4786416 257 return false;
bmanga95 0:772bf4786416 258 }
bmanga95 0:772bf4786416 259
bmanga95 0:772bf4786416 260 /* Normalize to 0-359� */
bmanga95 0:772bf4786416 261 if (orientation->heading < 0)
bmanga95 0:772bf4786416 262 {
bmanga95 0:772bf4786416 263 orientation->heading = 360 + orientation->heading;
bmanga95 0:772bf4786416 264 }
bmanga95 0:772bf4786416 265
bmanga95 0:772bf4786416 266 return true;
bmanga95 0:772bf4786416 267 }
bmanga95 0:772bf4786416 268
bmanga95 0:772bf4786416 269 /**************************************************************************/
bmanga95 0:772bf4786416 270 /*!
bmanga95 0:772bf4786416 271 @brief Populates the .roll/.pitch/.heading fields in the sensors_vec_t
bmanga95 0:772bf4786416 272 struct with the right angular data (in degree).
bmanga95 0:772bf4786416 273
bmanga95 0:772bf4786416 274 The starting position is set by placing the object flat and
bmanga95 0:772bf4786416 275 pointing northwards (Z-axis pointing upward and X-axis pointing
bmanga95 0:772bf4786416 276 northwards).
bmanga95 0:772bf4786416 277
bmanga95 0:772bf4786416 278 The orientation of the object can be modeled as resulting from
bmanga95 0:772bf4786416 279 3 consecutive rotations in turn: heading (Z-axis), pitch (Y-axis),
bmanga95 0:772bf4786416 280 and roll (X-axis) applied to the starting position.
bmanga95 0:772bf4786416 281
bmanga95 0:772bf4786416 282
bmanga95 0:772bf4786416 283 @param accel_event The sensors_event_t variable containing the
bmanga95 0:772bf4786416 284 data from the accelerometer
bmanga95 0:772bf4786416 285
bmanga95 0:772bf4786416 286 @param mag_event The sensors_event_t variable containing the
bmanga95 0:772bf4786416 287 data from the magnetometer
bmanga95 0:772bf4786416 288
bmanga95 0:772bf4786416 289 @param orientation The sensors_vec_t object that will have it's
bmanga95 0:772bf4786416 290 .roll, .pitch and .heading fields populated
bmanga95 0:772bf4786416 291 */
bmanga95 0:772bf4786416 292 /**************************************************************************/
bmanga95 0:772bf4786416 293 bool Adafruit_9DOF::fusionGetOrientation(sensors_event_t *accel_event, sensors_event_t *mag_event, sensors_vec_t *orientation)
bmanga95 0:772bf4786416 294 {
bmanga95 0:772bf4786416 295 /* Make sure the input is valid, not null, etc. */
bmanga95 0:772bf4786416 296 if ( accel_event == NULL) return false;
bmanga95 0:772bf4786416 297 if ( mag_event == NULL) return false;
bmanga95 0:772bf4786416 298 if ( orientation == NULL) return false;
bmanga95 0:772bf4786416 299
bmanga95 0:772bf4786416 300 float const PI_F = 3.14159265F;
bmanga95 0:772bf4786416 301
bmanga95 0:772bf4786416 302 /* roll: Rotation around the X-axis. -180 <= roll <= 180 */
bmanga95 0:772bf4786416 303 /* a positive roll angle is defined to be a clockwise rotation about the positive X-axis */
bmanga95 0:772bf4786416 304 /* */
bmanga95 0:772bf4786416 305 /* y */
bmanga95 0:772bf4786416 306 /* roll = atan2(---) */
bmanga95 0:772bf4786416 307 /* z */
bmanga95 0:772bf4786416 308 /* */
bmanga95 0:772bf4786416 309 /* where: y, z are returned value from accelerometer sensor */
bmanga95 0:772bf4786416 310 orientation->roll = (float)atan2(accel_event->acceleration.y, accel_event->acceleration.z);
bmanga95 0:772bf4786416 311
bmanga95 0:772bf4786416 312 /* pitch: Rotation around the Y-axis. -180 <= roll <= 180 */
bmanga95 0:772bf4786416 313 /* a positive pitch angle is defined to be a clockwise rotation about the positive Y-axis */
bmanga95 0:772bf4786416 314 /* */
bmanga95 0:772bf4786416 315 /* -x */
bmanga95 0:772bf4786416 316 /* pitch = atan(-------------------------------) */
bmanga95 0:772bf4786416 317 /* y * sin(roll) + z * cos(roll) */
bmanga95 0:772bf4786416 318 /* */
bmanga95 0:772bf4786416 319 /* where: x, y, z are returned value from accelerometer sensor */
bmanga95 0:772bf4786416 320 if (accel_event->acceleration.y * sin(orientation->roll) + accel_event->acceleration.z * cos(orientation->roll) == 0)
bmanga95 0:772bf4786416 321 orientation->pitch = accel_event->acceleration.x > 0 ? (PI_F / 2) : (-PI_F / 2);
bmanga95 0:772bf4786416 322 else
bmanga95 0:772bf4786416 323 orientation->pitch = (float)atan(-accel_event->acceleration.x / (accel_event->acceleration.y * sin(orientation->roll) + \
bmanga95 0:772bf4786416 324 accel_event->acceleration.z * cos(orientation->roll)));
bmanga95 0:772bf4786416 325
bmanga95 0:772bf4786416 326 /* heading: Rotation around the Z-axis. -180 <= roll <= 180 */
bmanga95 0:772bf4786416 327 /* a positive heading angle is defined to be a clockwise rotation about the positive Z-axis */
bmanga95 0:772bf4786416 328 /* */
bmanga95 0:772bf4786416 329 /* z * sin(roll) - y * cos(roll) */
bmanga95 0:772bf4786416 330 /* heading = atan2(--------------------------------------------------------------------------) */
bmanga95 0:772bf4786416 331 /* x * cos(pitch) + y * sin(pitch) * sin(roll) + z * sin(pitch) * cos(roll)) */
bmanga95 0:772bf4786416 332 /* */
bmanga95 0:772bf4786416 333 /* where: x, y, z are returned value from magnetometer sensor */
bmanga95 0:772bf4786416 334 orientation->heading = (float)atan2(mag_event->magnetic.z * sin(orientation->roll) - mag_event->magnetic.y * cos(orientation->roll), \
bmanga95 0:772bf4786416 335 mag_event->magnetic.x * cos(orientation->pitch) + \
bmanga95 0:772bf4786416 336 mag_event->magnetic.y * sin(orientation->pitch) * sin(orientation->roll) + \
bmanga95 0:772bf4786416 337 mag_event->magnetic.z * sin(orientation->pitch) * cos(orientation->roll));
bmanga95 0:772bf4786416 338
bmanga95 0:772bf4786416 339
bmanga95 0:772bf4786416 340 /* Convert angular data to degree */
bmanga95 0:772bf4786416 341 orientation->roll = orientation->roll * 180 / PI_F;
bmanga95 0:772bf4786416 342 orientation->pitch = orientation->pitch * 180 / PI_F;
bmanga95 0:772bf4786416 343 orientation->heading = orientation->heading * 180 / PI_F;
bmanga95 0:772bf4786416 344
bmanga95 0:772bf4786416 345 return true;
bmanga95 0:772bf4786416 346 }