1 /*
2 * Copyright (c) 2014, Texas Instruments Incorporated
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
7 * are met:
8 *
9 * * Redistributions of source code must retain the above copyright
10 * notice, this list of conditions and the following disclaimer.
11 *
12 * * Redistributions in binary form must reproduce the above copyright
13 * notice, this list of conditions and the following disclaimer in the
14 * documentation and/or other materials provided with the distribution.
15 *
16 * * Neither the name of Texas Instruments Incorporated nor the names of
17 * its contributors may be used to endorse or promote products derived
18 * from this software without specific prior written permission.
19 *
20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
22 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
23 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
24 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
27 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
28 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
29 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
30 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 */
33 #ifndef IMUManager_h
34 #define IMUManager_h
36 #include <Energia.h>
37 #include "LSM303.h"
38 #include "L3G.h"
39 #include "Utilities.h"
41 class IMUManager {
42 public:
43 IMUManager();
45 static bool initGyro();
46 static bool initAccel();
47 static bool initMag();
49 static void enableGyroDefault();
50 static void enableAccelDefault();
51 static void enableMagDefault();
53 static void readGyro();
54 static void readAccel();
55 static void readMag();
57 static void calibrateGyro(int numSeconds);
58 static void calibrateAccelerometer(int numSeconds);
59 static void calibrateMagnetometer(int numSamples);
61 static float getGyroX();
62 static float getGyroY();
63 static float getGyroZ();
65 static float getGyroYaw();
66 static float getGyroPitch();
67 static float getGyroRoll();
69 static float getAccelX();
70 static float getAccelY();
71 static float getAccelZ();
73 static float getMagX();
74 static float getMagY();
75 static float getMagZ();
77 static void zeroGyroXAxis();
78 static void zeroGyroYAxis();
79 static void zeroGyroZAxis();
81 static float getTiltAngle();
82 static float getFilteredTiltAngle();
84 /* raw acceleration values as measured by the accelerometer */
85 static int accel_x;
86 static int accel_y;
87 static int accel_z;
89 /* raw angular velocity values as measured by the gyro */
90 static int gyro_x;
91 static int gyro_y;
92 static int gyro_z;
94 /* raw magnetic field values as measured by the magnetometer */
95 static int mag_x;
96 static int mag_y;
97 static int mag_z;
98 private:
99 static LSM303 accel;
100 static L3G gyro;
102 static Utilities util;
104 /* gyro variables */
105 static float zAngle_gyro;
106 static float yAngle_gyro;
107 static float xAngle_gyro;
109 static float zReferenceAngle_gyro;
110 static float yReferenceAngle_gyro;
111 static float xReferenceAngle_gyro;
113 static int gyro_x_offset;
114 static int gyro_y_offset;
115 static int gyro_z_offset;
117 static float avg_gyro_z_zero_offset;
118 static float avg_gyro_y_zero_offset;
119 static float avg_gyro_x_zero_offset;
121 static float tuningFactor_gyro;
122 static float filteredAngle_gyro;
124 /* accelerometer variables */
125 static float accel_x_offset;
126 static float accel_y_offset;
127 static float accel_z_offset;
129 static bool finishedCalibration_accel;
130 static long initCalibrationTime_accel;
132 /* magnetometer variables */
133 static LSM303::vector<int16_t> mag_min;
134 static LSM303::vector<int16_t> mag_max;
136 /* constants: based on the default setup */
137 const static float GYRO_CONVERSION_FACTOR = (8.75f / 1000.0f); /* deg/s */
138 const static float ACCEL_CONVERSION_FACTOR = (0.061f / 1000.0f); /* g */
139 const static float MAG_CONVERSION_FACTOR = (0.160f / 1000.0f); /* gauss */
141 // const static float GYRO_SAMPLING_RATE
142 // const static float ACCEL_SAMPLING_RATE
143 // const static float MAG_SAMPLING_RATE
145 const static float COMPLEMENTARY_FILTER_WEIGHT = 0.95f;
146 const static int MAG_CALIBRATION_THRESHOLD = 9000;
148 };
149 #endif