Hochschule München / LSM303D_imu

imu01c

LSM303D.h@3:a55edecc96e2, 2016-07-20 (annotated)

Committer:
HMFK03LST1
Date:
Wed Jul 20 14:13:23 2016 +0000
Revision:
3:a55edecc96e2
Parent:
2:4bfc36c368c2 
first

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 2:4bfc36c368c2 66 * if (count%10==0) debug.printf("Heading: %5.1f\n\r Pitch: %4.1f\n\r",compass.hdg, compass.pitch );
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 3:a55edecc96e2 92 //! MAG Minimal readings
HMFK03LST1 2:4bfc36c368c2 93 vector min;
HMFK03LST1 2:4bfc36c368c2 94
HMFK03LST1 3:a55edecc96e2 95 //! MAG Maximal readings
HMFK03LST1 3:a55edecc96e2 96 vector max;
HMFK03LST1 3:a55edecc96e2 97
HMFK03LST1 3:a55edecc96e2 98 //! MAG Min-Max estimated range
HMFK03LST1 3:a55edecc96e2 99 vector spreed;
HMFK03LST1 2:4bfc36c368c2 100
HMFK03LST1 0:456611adedf8 101 /** Create a new interface for an LSM303D
HMFK03LST1 0:456611adedf8 102 *
HMFK03LST1 0:456611adedf8 103 * @param sda is the pin for the I2C SDA line
HMFK03LST1 0:456611adedf8 104 * @param scl is the pin for the I2C SCL line
HMFK03LST1 0:456611adedf8 105 */
HMFK03LST1 0:456611adedf8 106 LSM303D(PinName sda, PinName scl);
HMFK03LST1 0:456611adedf8 107
HMFK03LST1 0:456611adedf8 108 /** sets the x, y, and z offset corrections for hard iron calibration
HMFK03LST1 0:456611adedf8 109 *
HMFK03LST1 0:456611adedf8 110 * Calibration details here:
HMFK03LST1 0:456611adedf8 111 * http://mbed.org/users/shimniok/notebook/quick-and-dirty-3d-compass-calibration/
HMFK03LST1 0:456611adedf8 112 *
HMFK03LST1 0:456611adedf8 113 * If you gather raw magnetometer data and find, for example, x is offset
HMFK03LST1 0:456611adedf8 114 * by hard iron by -20 then pass +20 to this member function to correct
HMFK03LST1 0:456611adedf8 115 * for hard iron.
HMFK03LST1 0:456611adedf8 116 *
HMFK03LST1 0:456611adedf8 117 * @param x is the offset correction for the x axis
HMFK03LST1 0:456611adedf8 118 * @param y is the offset correction for the y axis
HMFK03LST1 0:456611adedf8 119 * @param z is the offset correction for the z axis
HMFK03LST1 0:456611adedf8 120 */
HMFK03LST1 0:456611adedf8 121 void setOffset(float x, float y, float z);
HMFK03LST1 0:456611adedf8 122
HMFK03LST1 0:456611adedf8 123 /** sets the scale factor for the x, y, and z axes
HMFK03LST1 0:456611adedf8 124 *
HMFK03LST1 0:456611adedf8 125 * Calibratio details here:
HMFK03LST1 0:456611adedf8 126 * http://mbed.org/users/shimniok/notebook/quick-and-dirty-3d-compass-calibration/
HMFK03LST1 0:456611adedf8 127 *
HMFK03LST1 0:456611adedf8 128 * Sensitivity of the three axes is never perfectly identical and this
HMFK03LST1 0:456611adedf8 129 * function can help to correct differences in sensitivity. You're
HMFK03LST1 0:456611adedf8 130 * supplying a multipler such that x, y and z will be normalized to the
HMFK03LST1 0:456611adedf8 131 * same max/min values
HMFK03LST1 0:456611adedf8 132 */
HMFK03LST1 0:456611adedf8 133 void setScale(float x, float y, float z);
HMFK03LST1 0:456611adedf8 134
HMFK03LST1 0:456611adedf8 135 /** read the raw accelerometer and compass values
HMFK03LST1 0:456611adedf8 136 * calc Heading and Pitch
HMFK03LST1 0:456611adedf8 137 */
HMFK03LST1 0:456611adedf8 138 void read();
HMFK03LST1 0:456611adedf8 139
HMFK03LST1 2:4bfc36c368c2 140
HMFK03LST1 2:4bfc36c368c2 141 /** Set Mag Limits
HMFK03LST1 2:4bfc36c368c2 142 */
HMFK03LST1 3:a55edecc96e2 143 void set_limits(int mode);
HMFK03LST1 2:4bfc36c368c2 144
HMFK03LST1 2:4bfc36c368c2 145
HMFK03LST1 0:456611adedf8 146 /** sets the I2C bus frequency
HMFK03LST1 0:456611adedf8 147 *
HMFK03LST1 0:456611adedf8 148 * @param frequency is the I2C bus/clock frequency, either standard (100000) or fast (400000)
HMFK03LST1 0:456611adedf8 149 */
HMFK03LST1 0:456611adedf8 150 void frequency(int hz);
HMFK03LST1 0:456611adedf8 151
HMFK03LST1 0:456611adedf8 152 private:
HMFK03LST1 0:456611adedf8 153 I2C _compass;
HMFK03LST1 1:5d0acf280330 154 float _offset_x;
HMFK03LST1 1:5d0acf280330 155 float _offset_y;
HMFK03LST1 1:5d0acf280330 156 float _offset_z;
HMFK03LST1 1:5d0acf280330 157 float _scale_x;
HMFK03LST1 1:5d0acf280330 158 float _scale_y;
HMFK03LST1 1:5d0acf280330 159 float _scale_z;
HMFK03LST1 0:456611adedf8 160 long _filt_ax;
HMFK03LST1 0:456611adedf8 161 long _filt_ay;
HMFK03LST1 0:456611adedf8 162 long _filt_az;
HMFK03LST1 2:4bfc36c368c2 163
HMFK03LST1 0:456611adedf8 164
HMFK03LST1 0:456611adedf8 165 bool write_reg(int addr_i2c,int addr_reg, char v);
HMFK03LST1 0:456611adedf8 166 bool read_reg(int addr_i2c,int addr_reg, char *v);
HMFK03LST1 0:456611adedf8 167 void calc_pos(void);
HMFK03LST1 0:456611adedf8 168 void set_vectors(void);
HMFK03LST1 0:456611adedf8 169
HMFK03LST1 0:456611adedf8 170 };