![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Port of Adafruit Arduino code
Fork of Adafruit9-DOf by
Source/Adafruit_9DOF.cpp@2:fe10a40b42e8, 2015-11-29 (annotated)
- Committer:
- jatinsha
- Date:
- Sun Nov 29 18:41:42 2015 +0000
- Revision:
- 2:fe10a40b42e8
- Parent:
- 0:772bf4786416
none
Who changed what in which revision?
User | Revision | Line number | New 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 | } |