JB Dean / GR-PeachAHRSWeb

Modified to run on Renesas GR Peach board

Dependencies:   EthernetInterface HTTP-Server Webpage mbed-rpc mbed-src

Fork of HTTPServer_echoback by Takuya Urakawa

Adafruit_10DOF/Adafruit_10DOF.cpp@16:5d102be2566c, 2015-10-07 (annotated)

Committer:
webOnBoard
Date:
Wed Oct 07 20:42:51 2015 +0000
Revision:
16:5d102be2566c
web based AHRS demo Renesas GR Peach board; Using HTTP Server/RPC Adafruit 10DOF

Who changed what in which revision?

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