Hochschule München / LSM303D_imu

imu01c

LSM303D.h@0:456611adedf8, 2015-10-31 (annotated)

Committer:
HMFK03LST1
Date:
Sat Oct 31 16:14:13 2015 +0000
Revision:
0:456611adedf8
Child:
1:5d0acf280330
new

Who changed what in which revision?

UserRevisionLine numberNew contents of line
HMFK03LST1 0:456611adedf8 1 #include "mbed.h"
HMFK03LST1 0:456611adedf8 2 #include "vector.h"
HMFK03LST1 0:456611adedf8 3
HMFK03LST1 0:456611adedf8 4 #ifndef M_PI
HMFK03LST1 0:456611adedf8 5 #define M_PI 3.14159265358979323846
HMFK03LST1 0:456611adedf8 6 #endif
HMFK03LST1 0:456611adedf8 7
HMFK03LST1 0:456611adedf8 8 /** Tilt-compensated compass interface Library for the STMicro LSM303D 3-axis magnetometer, 3-axis acceleromter
HMFK03LST1 0:456611adedf8 9 *
HMFK03LST1 0:456611adedf8 10 * Based on
HMFK03LST1 0:456611adedf8 11 *
HMFK03LST1 0:456611adedf8 12 * Michael Shimniok http://bot-thoughts.com
HMFK03LST1 0:456611adedf8 13 *
HMFK03LST1 0:456611adedf8 14 * test program by tosihisa and
HMFK03LST1 0:456611adedf8 15 *
HMFK03LST1 0:456611adedf8 16 * Pololu sample library for LSM303DLH breakout by ryantm:
HMFK03LST1 0:456611adedf8 17 *
HMFK03LST1 0:456611adedf8 18 * Copyright (c) 2011 Pololu Corporation. For more information, see
HMFK03LST1 0:456611adedf8 19 *
HMFK03LST1 0:456611adedf8 20 * http://www.pololu.com/
HMFK03LST1 0:456611adedf8 21 * http://forum.pololu.com/
HMFK03LST1 0:456611adedf8 22 *
HMFK03LST1 0:456611adedf8 23 * Permission is hereby granted, free of charge, to any person
HMFK03LST1 0:456611adedf8 24 * obtaining a copy of this software and associated documentation
HMFK03LST1 0:456611adedf8 25 * files (the "Software"), to deal in the Software without
HMFK03LST1 0:456611adedf8 26 * restriction, including without limitation the rights to use,
HMFK03LST1 0:456611adedf8 27 * copy, modify, merge, publish, distribute, sublicense, and/or sell
HMFK03LST1 0:456611adedf8 28 * copies of the Software, and to permit persons to whom the
HMFK03LST1 0:456611adedf8 29 * Software is furnished to do so, subject to the following
HMFK03LST1 0:456611adedf8 30 * conditions:
HMFK03LST1 0:456611adedf8 31 *
HMFK03LST1 0:456611adedf8 32 * The above copyright notice and this permission notice shall be
HMFK03LST1 0:456611adedf8 33 * included in all copies or substantial portions of the Software.
HMFK03LST1 0:456611adedf8 34 *
HMFK03LST1 0:456611adedf8 35 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
HMFK03LST1 0:456611adedf8 36 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
HMFK03LST1 0:456611adedf8 37 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
HMFK03LST1 0:456611adedf8 38 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
HMFK03LST1 0:456611adedf8 39 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
HMFK03LST1 0:456611adedf8 40 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
HMFK03LST1 0:456611adedf8 41 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
HMFK03LST1 0:456611adedf8 42 * OTHER DEALINGS IN THE SOFTWARE.
HMFK03LST1 0:456611adedf8 43 *
HMFK03LST1 0:456611adedf8 44 * @code
HMFK03LST1 0:456611adedf8 45 * #include "mbed.h"
HMFK03LST1 0:456611adedf8 46 * #include "LSM303D.h"
HMFK03LST1 0:456611adedf8 47 *
HMFK03LST1 0:456611adedf8 48 * Serial debug(USBTX,USBRX);
HMFK03LST1 0:456611adedf8 49 * LSM303D compass(p28, p27);
HMFK03LST1 0:456611adedf8 50 *
HMFK03LST1 0:456611adedf8 51 * int main()
HMFK03LST1 0:456611adedf8 52 * {
HMFK03LST1 0:456611adedf8 53 *
HMFK03LST1 0:456611adedf8 54 * int count;
HMFK03LST1 0:456611adedf8 55 *
HMFK03LST1 0:456611adedf8 56 * debug.baud(115200);
HMFK03LST1 0:456611adedf8 57 * debug.printf("LSM303D Test\x0d\x0a");
HMFK03LST1 0:456611adedf8 58 *
HMFK03LST1 0:456611adedf8 59 * compass.setOffset(460, 610, -290);
HMFK03LST1 0:456611adedf8 60 * compass.setScale(0.93, 0.92, 1.00);
HMFK03LST1 0:456611adedf8 61 *
HMFK03LST1 0:456611adedf8 62 * while(1)
HMFK03LST1 0:456611adedf8 63 * {
HMFK03LST1 0:456611adedf8 64 * compass.read();
HMFK03LST1 0:456611adedf8 65 * count++;
HMFK03LST1 0:456611adedf8 66 * if (count%10==0) debug.printf("Heading: %5.1f\n\r Pitch: %4.1f\n\r",compass.hdg, compass.elev );
HMFK03LST1 0:456611adedf8 67 * wait(0.050);
HMFK03LST1 0:456611adedf8 68 * }
HMFK03LST1 0:456611adedf8 69 * }
HMFK03LST1 0:456611adedf8 70 * @endcode
HMFK03LST1 0:456611adedf8 71 */
HMFK03LST1 0:456611adedf8 72 class LSM303D {
HMFK03LST1 0:456611adedf8 73 public:
HMFK03LST1 0:456611adedf8 74 //! ACC Raw readings with x,y and z axis
HMFK03LST1 0:456611adedf8 75 vector acc_raw;
HMFK03LST1 0:456611adedf8 76
HMFK03LST1 0:456611adedf8 77 //! MAG Raw readings with x,y and z axis
HMFK03LST1 0:456611adedf8 78 vector mag_raw;
HMFK03LST1 0:456611adedf8 79
HMFK03LST1 0:456611adedf8 80 //! ACC Filtert readings with x,y and z axis
HMFK03LST1 0:456611adedf8 81 vector acc;
HMFK03LST1 0:456611adedf8 82
HMFK03LST1 0:456611adedf8 83 //! MAG Normal readings with x,y and z axis
HMFK03LST1 0:456611adedf8 84 vector mag;
HMFK03LST1 0:456611adedf8 85
HMFK03LST1 0:456611adedf8 86 //! Heading
HMFK03LST1 0:456611adedf8 87 float hdg;
HMFK03LST1 0:456611adedf8 88
HMFK03LST1 0:456611adedf8 89 //! Pitch
HMFK03LST1 0:456611adedf8 90 float pitch;
HMFK03LST1 0:456611adedf8 91
HMFK03LST1 0:456611adedf8 92 /** Create a new interface for an LSM303D
HMFK03LST1 0:456611adedf8 93 *
HMFK03LST1 0:456611adedf8 94 * @param sda is the pin for the I2C SDA line
HMFK03LST1 0:456611adedf8 95 * @param scl is the pin for the I2C SCL line
HMFK03LST1 0:456611adedf8 96 */
HMFK03LST1 0:456611adedf8 97 LSM303D(PinName sda, PinName scl);
HMFK03LST1 0:456611adedf8 98
HMFK03LST1 0:456611adedf8 99 /** sets the x, y, and z offset corrections for hard iron calibration
HMFK03LST1 0:456611adedf8 100 *
HMFK03LST1 0:456611adedf8 101 * Calibration details here:
HMFK03LST1 0:456611adedf8 102 * http://mbed.org/users/shimniok/notebook/quick-and-dirty-3d-compass-calibration/
HMFK03LST1 0:456611adedf8 103 *
HMFK03LST1 0:456611adedf8 104 * If you gather raw magnetometer data and find, for example, x is offset
HMFK03LST1 0:456611adedf8 105 * by hard iron by -20 then pass +20 to this member function to correct
HMFK03LST1 0:456611adedf8 106 * for hard iron.
HMFK03LST1 0:456611adedf8 107 *
HMFK03LST1 0:456611adedf8 108 * @param x is the offset correction for the x axis
HMFK03LST1 0:456611adedf8 109 * @param y is the offset correction for the y axis
HMFK03LST1 0:456611adedf8 110 * @param z is the offset correction for the z axis
HMFK03LST1 0:456611adedf8 111 */
HMFK03LST1 0:456611adedf8 112 void setOffset(float x, float y, float z);
HMFK03LST1 0:456611adedf8 113
HMFK03LST1 0:456611adedf8 114 /** sets the scale factor for the x, y, and z axes
HMFK03LST1 0:456611adedf8 115 *
HMFK03LST1 0:456611adedf8 116 * Calibratio details here:
HMFK03LST1 0:456611adedf8 117 * http://mbed.org/users/shimniok/notebook/quick-and-dirty-3d-compass-calibration/
HMFK03LST1 0:456611adedf8 118 *
HMFK03LST1 0:456611adedf8 119 * Sensitivity of the three axes is never perfectly identical and this
HMFK03LST1 0:456611adedf8 120 * function can help to correct differences in sensitivity. You're
HMFK03LST1 0:456611adedf8 121 * supplying a multipler such that x, y and z will be normalized to the
HMFK03LST1 0:456611adedf8 122 * same max/min values
HMFK03LST1 0:456611adedf8 123 */
HMFK03LST1 0:456611adedf8 124 void setScale(float x, float y, float z);
HMFK03LST1 0:456611adedf8 125
HMFK03LST1 0:456611adedf8 126 /** read the raw accelerometer and compass values
HMFK03LST1 0:456611adedf8 127 * calc Heading and Pitch
HMFK03LST1 0:456611adedf8 128 */
HMFK03LST1 0:456611adedf8 129 void read();
HMFK03LST1 0:456611adedf8 130
HMFK03LST1 0:456611adedf8 131 /** sets the I2C bus frequency
HMFK03LST1 0:456611adedf8 132 *
HMFK03LST1 0:456611adedf8 133 * @param frequency is the I2C bus/clock frequency, either standard (100000) or fast (400000)
HMFK03LST1 0:456611adedf8 134 */
HMFK03LST1 0:456611adedf8 135 void frequency(int hz);
HMFK03LST1 0:456611adedf8 136
HMFK03LST1 0:456611adedf8 137 private:
HMFK03LST1 0:456611adedf8 138 I2C _compass;
HMFK03LST1 0:456611adedf8 139 vector _offset;
HMFK03LST1 0:456611adedf8 140 vector _scale;
HMFK03LST1 0:456611adedf8 141 long _filt_ax;
HMFK03LST1 0:456611adedf8 142 long _filt_ay;
HMFK03LST1 0:456611adedf8 143 long _filt_az;
HMFK03LST1 0:456611adedf8 144
HMFK03LST1 0:456611adedf8 145 bool write_reg(int addr_i2c,int addr_reg, char v);
HMFK03LST1 0:456611adedf8 146 bool read_reg(int addr_i2c,int addr_reg, char *v);
HMFK03LST1 0:456611adedf8 147 void calc_pos(void);
HMFK03LST1 0:456611adedf8 148 void set_vectors(void);
HMFK03LST1 0:456611adedf8 149
HMFK03LST1 0:456611adedf8 150 };