Jatin Sharma / Mbed 2 deprecated RaheeNew

RaheeNew

Dependencies:   mbed

Dependents:   RaheeNew

Fork of Adafruit9-DOf by Bruno Manganelli

Source/Adafruit_9DOF.cpp@0:772bf4786416, 2015-03-21 (annotated)

Committer:
bmanga95
Date:
Sat Mar 21 12:33:05 2015 +0000
Revision:
0:772bf4786416
Child:
1:c3381056a1c6
First version

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