![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Where we will test the side ToF sensors
Dependencies: QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic
wheelchairControlSideTof/IMU6050/MPU6050.h@21:d1faccb96146, 2019-07-09 (annotated)
- Committer:
- isagmz
- Date:
- Tue Jul 09 17:52:32 2019 +0000
- Revision:
- 21:d1faccb96146
Finished IMU side sensor code
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
isagmz | 21:d1faccb96146 | 1 | /*Use #define MPU6050_ES before you include this file if you have an engineering sample (older EVBs will have them), to find out if you have one, check your accelerometer output. |
isagmz | 21:d1faccb96146 | 2 | If it is half of what you expected, and you still are on the correct planet, you got an engineering sample |
isagmz | 21:d1faccb96146 | 3 | */ |
isagmz | 21:d1faccb96146 | 4 | |
isagmz | 21:d1faccb96146 | 5 | |
isagmz | 21:d1faccb96146 | 6 | #ifndef MPU6050_H |
isagmz | 21:d1faccb96146 | 7 | #define MPU6050_H |
isagmz | 21:d1faccb96146 | 8 | |
isagmz | 21:d1faccb96146 | 9 | /** |
isagmz | 21:d1faccb96146 | 10 | * Includes |
isagmz | 21:d1faccb96146 | 11 | */ |
isagmz | 21:d1faccb96146 | 12 | #include "mbed.h" |
isagmz | 21:d1faccb96146 | 13 | |
isagmz | 21:d1faccb96146 | 14 | |
isagmz | 21:d1faccb96146 | 15 | /** |
isagmz | 21:d1faccb96146 | 16 | * Defines |
isagmz | 21:d1faccb96146 | 17 | */ |
isagmz | 21:d1faccb96146 | 18 | #ifndef MPU6050_ADDRESS |
isagmz | 21:d1faccb96146 | 19 | #define MPU6050_ADDRESS 0x68 // address pin low (GND), default for InvenSense evaluation board |
isagmz | 21:d1faccb96146 | 20 | #endif |
isagmz | 21:d1faccb96146 | 21 | |
isagmz | 21:d1faccb96146 | 22 | #ifdef MPU6050_ES |
isagmz | 21:d1faccb96146 | 23 | #define DOUBLE_ACCELERO |
isagmz | 21:d1faccb96146 | 24 | #endif |
isagmz | 21:d1faccb96146 | 25 | |
isagmz | 21:d1faccb96146 | 26 | /** |
isagmz | 21:d1faccb96146 | 27 | * Registers |
isagmz | 21:d1faccb96146 | 28 | */ |
isagmz | 21:d1faccb96146 | 29 | #define MPU6050_CONFIG_REG 0x1A |
isagmz | 21:d1faccb96146 | 30 | #define MPU6050_GYRO_CONFIG_REG 0x1B |
isagmz | 21:d1faccb96146 | 31 | #define MPU6050_ACCELERO_CONFIG_REG 0x1C |
isagmz | 21:d1faccb96146 | 32 | |
isagmz | 21:d1faccb96146 | 33 | #define MPU6050_INT_PIN_CFG 0x37 |
isagmz | 21:d1faccb96146 | 34 | |
isagmz | 21:d1faccb96146 | 35 | #define MPU6050_ACCEL_XOUT_H_REG 0x3B |
isagmz | 21:d1faccb96146 | 36 | #define MPU6050_ACCEL_YOUT_H_REG 0x3D |
isagmz | 21:d1faccb96146 | 37 | #define MPU6050_ACCEL_ZOUT_H_REG 0x3F |
isagmz | 21:d1faccb96146 | 38 | |
isagmz | 21:d1faccb96146 | 39 | #define MPU6050_TEMP_H_REG 0x41 |
isagmz | 21:d1faccb96146 | 40 | |
isagmz | 21:d1faccb96146 | 41 | #define MPU6050_GYRO_XOUT_H_REG 0x43 |
isagmz | 21:d1faccb96146 | 42 | #define MPU6050_GYRO_YOUT_H_REG 0x45 |
isagmz | 21:d1faccb96146 | 43 | #define MPU6050_GYRO_ZOUT_H_REG 0x47 |
isagmz | 21:d1faccb96146 | 44 | |
isagmz | 21:d1faccb96146 | 45 | |
isagmz | 21:d1faccb96146 | 46 | |
isagmz | 21:d1faccb96146 | 47 | #define MPU6050_PWR_MGMT_1_REG 0x6B |
isagmz | 21:d1faccb96146 | 48 | #define MPU6050_WHO_AM_I_REG 0x75 |
isagmz | 21:d1faccb96146 | 49 | |
isagmz | 21:d1faccb96146 | 50 | |
isagmz | 21:d1faccb96146 | 51 | |
isagmz | 21:d1faccb96146 | 52 | /** |
isagmz | 21:d1faccb96146 | 53 | * Definitions |
isagmz | 21:d1faccb96146 | 54 | */ |
isagmz | 21:d1faccb96146 | 55 | #define MPU6050_SLP_BIT 6 |
isagmz | 21:d1faccb96146 | 56 | #define MPU6050_BYPASS_BIT 1 |
isagmz | 21:d1faccb96146 | 57 | |
isagmz | 21:d1faccb96146 | 58 | #define MPU6050_BW_256 0 |
isagmz | 21:d1faccb96146 | 59 | #define MPU6050_BW_188 1 |
isagmz | 21:d1faccb96146 | 60 | #define MPU6050_BW_98 2 |
isagmz | 21:d1faccb96146 | 61 | #define MPU6050_BW_42 3 |
isagmz | 21:d1faccb96146 | 62 | #define MPU6050_BW_20 4 |
isagmz | 21:d1faccb96146 | 63 | #define MPU6050_BW_10 5 |
isagmz | 21:d1faccb96146 | 64 | #define MPU6050_BW_5 6 |
isagmz | 21:d1faccb96146 | 65 | |
isagmz | 21:d1faccb96146 | 66 | #define MPU6050_ACCELERO_RANGE_2G 0 |
isagmz | 21:d1faccb96146 | 67 | #define MPU6050_ACCELERO_RANGE_4G 1 |
isagmz | 21:d1faccb96146 | 68 | #define MPU6050_ACCELERO_RANGE_8G 2 |
isagmz | 21:d1faccb96146 | 69 | #define MPU6050_ACCELERO_RANGE_16G 3 |
isagmz | 21:d1faccb96146 | 70 | |
isagmz | 21:d1faccb96146 | 71 | #define MPU6050_GYRO_RANGE_250 0 |
isagmz | 21:d1faccb96146 | 72 | #define MPU6050_GYRO_RANGE_500 1 |
isagmz | 21:d1faccb96146 | 73 | #define MPU6050_GYRO_RANGE_1000 2 |
isagmz | 21:d1faccb96146 | 74 | #define MPU6050_GYRO_RANGE_2000 3 |
isagmz | 21:d1faccb96146 | 75 | |
isagmz | 21:d1faccb96146 | 76 | //interrupt address |
isagmz | 21:d1faccb96146 | 77 | #define MPU6050_RA_INT_ENABLE 0x38 |
isagmz | 21:d1faccb96146 | 78 | //define how the accelerometer is placed on surface |
isagmz | 21:d1faccb96146 | 79 | #define X_AXIS 1 |
isagmz | 21:d1faccb96146 | 80 | #define Y_AXIS 2 |
isagmz | 21:d1faccb96146 | 81 | #define Z_AXIS 0 |
isagmz | 21:d1faccb96146 | 82 | //translation from radians to degrees |
isagmz | 21:d1faccb96146 | 83 | #define RADIANS_TO_DEGREES 180/3.1415926536 |
isagmz | 21:d1faccb96146 | 84 | //constant used for the complementary filter, which ranges usually from 0.9 to 1.0 |
isagmz | 21:d1faccb96146 | 85 | #define ALPHA 0.96 //filter constant |
isagmz | 21:d1faccb96146 | 86 | //scale the gyro |
isagmz | 21:d1faccb96146 | 87 | #define GYRO_SCALE 2.7176 |
isagmz | 21:d1faccb96146 | 88 | |
isagmz | 21:d1faccb96146 | 89 | /** MPU6050 IMU library. |
isagmz | 21:d1faccb96146 | 90 | * |
isagmz | 21:d1faccb96146 | 91 | * Example: |
isagmz | 21:d1faccb96146 | 92 | * @code |
isagmz | 21:d1faccb96146 | 93 | * Later, maybe |
isagmz | 21:d1faccb96146 | 94 | * @endcode |
isagmz | 21:d1faccb96146 | 95 | */ |
isagmz | 21:d1faccb96146 | 96 | class MPU6050 { |
isagmz | 21:d1faccb96146 | 97 | public: |
isagmz | 21:d1faccb96146 | 98 | /** |
isagmz | 21:d1faccb96146 | 99 | * Constructor. |
isagmz | 21:d1faccb96146 | 100 | * |
isagmz | 21:d1faccb96146 | 101 | * Sleep mode of MPU6050 is immediatly disabled |
isagmz | 21:d1faccb96146 | 102 | * |
isagmz | 21:d1faccb96146 | 103 | * @param sda - mbed pin to use for the SDA I2C line. |
isagmz | 21:d1faccb96146 | 104 | * @param scl - mbed pin to use for the SCL I2C line. |
isagmz | 21:d1faccb96146 | 105 | */ |
isagmz | 21:d1faccb96146 | 106 | MPU6050(PinName sda, PinName scl); |
isagmz | 21:d1faccb96146 | 107 | |
isagmz | 21:d1faccb96146 | 108 | |
isagmz | 21:d1faccb96146 | 109 | /** |
isagmz | 21:d1faccb96146 | 110 | * Tests the I2C connection by reading the WHO_AM_I register. |
isagmz | 21:d1faccb96146 | 111 | * |
isagmz | 21:d1faccb96146 | 112 | * @return True for a working connection, false for an error |
isagmz | 21:d1faccb96146 | 113 | */ |
isagmz | 21:d1faccb96146 | 114 | bool testConnection( void ); |
isagmz | 21:d1faccb96146 | 115 | |
isagmz | 21:d1faccb96146 | 116 | /** |
isagmz | 21:d1faccb96146 | 117 | * Sets the bandwidth of the digital low-pass filter |
isagmz | 21:d1faccb96146 | 118 | * |
isagmz | 21:d1faccb96146 | 119 | * Macros: MPU6050_BW_256 - MPU6050_BW_188 - MPU6050_BW_98 - MPU6050_BW_42 - MPU6050_BW_20 - MPU6050_BW_10 - MPU6050_BW_5 |
isagmz | 21:d1faccb96146 | 120 | * Last number is the gyro's BW in Hz (accelero BW is virtually identical) |
isagmz | 21:d1faccb96146 | 121 | * |
isagmz | 21:d1faccb96146 | 122 | * @param BW - The three bits that set the bandwidth (use the predefined macros) |
isagmz | 21:d1faccb96146 | 123 | */ |
isagmz | 21:d1faccb96146 | 124 | void setBW( char BW ); |
isagmz | 21:d1faccb96146 | 125 | |
isagmz | 21:d1faccb96146 | 126 | /** |
isagmz | 21:d1faccb96146 | 127 | * Sets the auxiliary I2C bus in bypass mode to read the sensors behind the MPU6050 (useful for eval board, otherwise just connect them to primary I2C bus) |
isagmz | 21:d1faccb96146 | 128 | * |
isagmz | 21:d1faccb96146 | 129 | * @param state - Enables/disables the I2C bypass mode |
isagmz | 21:d1faccb96146 | 130 | */ |
isagmz | 21:d1faccb96146 | 131 | void setI2CBypass ( bool state ); |
isagmz | 21:d1faccb96146 | 132 | |
isagmz | 21:d1faccb96146 | 133 | /** |
isagmz | 21:d1faccb96146 | 134 | * Sets the Accelero full-scale range |
isagmz | 21:d1faccb96146 | 135 | * |
isagmz | 21:d1faccb96146 | 136 | * Macros: MPU6050_ACCELERO_RANGE_2G - MPU6050_ACCELERO_RANGE_4G - MPU6050_ACCELERO_RANGE_8G - MPU6050_ACCELERO_RANGE_16G |
isagmz | 21:d1faccb96146 | 137 | * |
isagmz | 21:d1faccb96146 | 138 | * @param range - The two bits that set the full-scale range (use the predefined macros) |
isagmz | 21:d1faccb96146 | 139 | */ |
isagmz | 21:d1faccb96146 | 140 | void setAcceleroRange(char range); |
isagmz | 21:d1faccb96146 | 141 | |
isagmz | 21:d1faccb96146 | 142 | /** |
isagmz | 21:d1faccb96146 | 143 | * Reads the accelero x-axis. |
isagmz | 21:d1faccb96146 | 144 | * |
isagmz | 21:d1faccb96146 | 145 | * @return 16-bit signed integer x-axis accelero data |
isagmz | 21:d1faccb96146 | 146 | */ |
isagmz | 21:d1faccb96146 | 147 | int getAcceleroRawX( void ); |
isagmz | 21:d1faccb96146 | 148 | |
isagmz | 21:d1faccb96146 | 149 | /** |
isagmz | 21:d1faccb96146 | 150 | * Reads the accelero y-axis. |
isagmz | 21:d1faccb96146 | 151 | * |
isagmz | 21:d1faccb96146 | 152 | * @return 16-bit signed integer y-axis accelero data |
isagmz | 21:d1faccb96146 | 153 | */ |
isagmz | 21:d1faccb96146 | 154 | int getAcceleroRawY( void ); |
isagmz | 21:d1faccb96146 | 155 | |
isagmz | 21:d1faccb96146 | 156 | /** |
isagmz | 21:d1faccb96146 | 157 | * Reads the accelero z-axis. |
isagmz | 21:d1faccb96146 | 158 | * |
isagmz | 21:d1faccb96146 | 159 | * @return 16-bit signed integer z-axis accelero data |
isagmz | 21:d1faccb96146 | 160 | */ |
isagmz | 21:d1faccb96146 | 161 | int getAcceleroRawZ( void ); |
isagmz | 21:d1faccb96146 | 162 | |
isagmz | 21:d1faccb96146 | 163 | /** |
isagmz | 21:d1faccb96146 | 164 | * Reads all accelero data. |
isagmz | 21:d1faccb96146 | 165 | * |
isagmz | 21:d1faccb96146 | 166 | * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z |
isagmz | 21:d1faccb96146 | 167 | */ |
isagmz | 21:d1faccb96146 | 168 | void getAcceleroRaw( int *data ); |
isagmz | 21:d1faccb96146 | 169 | |
isagmz | 21:d1faccb96146 | 170 | /** |
isagmz | 21:d1faccb96146 | 171 | * Reads all accelero data, gives the acceleration in m/s2 |
isagmz | 21:d1faccb96146 | 172 | * |
isagmz | 21:d1faccb96146 | 173 | * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work. |
isagmz | 21:d1faccb96146 | 174 | * |
isagmz | 21:d1faccb96146 | 175 | * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z |
isagmz | 21:d1faccb96146 | 176 | */ |
isagmz | 21:d1faccb96146 | 177 | void getAccelero( float *data ); |
isagmz | 21:d1faccb96146 | 178 | |
isagmz | 21:d1faccb96146 | 179 | /** |
isagmz | 21:d1faccb96146 | 180 | * Sets the Gyro full-scale range |
isagmz | 21:d1faccb96146 | 181 | * |
isagmz | 21:d1faccb96146 | 182 | * Macros: MPU6050_GYRO_RANGE_250 - MPU6050_GYRO_RANGE_500 - MPU6050_GYRO_RANGE_1000 - MPU6050_GYRO_RANGE_2000 |
isagmz | 21:d1faccb96146 | 183 | * |
isagmz | 21:d1faccb96146 | 184 | * @param range - The two bits that set the full-scale range (use the predefined macros) |
isagmz | 21:d1faccb96146 | 185 | */ |
isagmz | 21:d1faccb96146 | 186 | void setGyroRange(char range); |
isagmz | 21:d1faccb96146 | 187 | |
isagmz | 21:d1faccb96146 | 188 | /** |
isagmz | 21:d1faccb96146 | 189 | * Reads the gyro x-axis. |
isagmz | 21:d1faccb96146 | 190 | * |
isagmz | 21:d1faccb96146 | 191 | * @return 16-bit signed integer x-axis gyro data |
isagmz | 21:d1faccb96146 | 192 | */ |
isagmz | 21:d1faccb96146 | 193 | int getGyroRawX( void ); |
isagmz | 21:d1faccb96146 | 194 | |
isagmz | 21:d1faccb96146 | 195 | /** |
isagmz | 21:d1faccb96146 | 196 | * Reads the gyro y-axis. |
isagmz | 21:d1faccb96146 | 197 | * |
isagmz | 21:d1faccb96146 | 198 | * @return 16-bit signed integer y-axis gyro data |
isagmz | 21:d1faccb96146 | 199 | */ |
isagmz | 21:d1faccb96146 | 200 | int getGyroRawY( void ); |
isagmz | 21:d1faccb96146 | 201 | |
isagmz | 21:d1faccb96146 | 202 | /** |
isagmz | 21:d1faccb96146 | 203 | * Reads the gyro z-axis. |
isagmz | 21:d1faccb96146 | 204 | * |
isagmz | 21:d1faccb96146 | 205 | * @return 16-bit signed integer z-axis gyro data |
isagmz | 21:d1faccb96146 | 206 | */ |
isagmz | 21:d1faccb96146 | 207 | int getGyroRawZ( void ); |
isagmz | 21:d1faccb96146 | 208 | |
isagmz | 21:d1faccb96146 | 209 | /** |
isagmz | 21:d1faccb96146 | 210 | * Reads all gyro data. |
isagmz | 21:d1faccb96146 | 211 | * |
isagmz | 21:d1faccb96146 | 212 | * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z |
isagmz | 21:d1faccb96146 | 213 | */ |
isagmz | 21:d1faccb96146 | 214 | void getGyroRaw( int *data ); |
isagmz | 21:d1faccb96146 | 215 | |
isagmz | 21:d1faccb96146 | 216 | /** |
isagmz | 21:d1faccb96146 | 217 | * Reads all gyro data, gives the gyro in rad/s |
isagmz | 21:d1faccb96146 | 218 | * |
isagmz | 21:d1faccb96146 | 219 | * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work. |
isagmz | 21:d1faccb96146 | 220 | * |
isagmz | 21:d1faccb96146 | 221 | * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z |
isagmz | 21:d1faccb96146 | 222 | */ |
isagmz | 21:d1faccb96146 | 223 | void getGyro( float *data); |
isagmz | 21:d1faccb96146 | 224 | |
isagmz | 21:d1faccb96146 | 225 | /** |
isagmz | 21:d1faccb96146 | 226 | * Reads temperature data. |
isagmz | 21:d1faccb96146 | 227 | * |
isagmz | 21:d1faccb96146 | 228 | * @return 16 bit signed integer with the raw temperature register value |
isagmz | 21:d1faccb96146 | 229 | */ |
isagmz | 21:d1faccb96146 | 230 | int getTempRaw( void ); |
isagmz | 21:d1faccb96146 | 231 | |
isagmz | 21:d1faccb96146 | 232 | /** |
isagmz | 21:d1faccb96146 | 233 | * Returns current temperature |
isagmz | 21:d1faccb96146 | 234 | * |
isagmz | 21:d1faccb96146 | 235 | * @returns float with the current temperature |
isagmz | 21:d1faccb96146 | 236 | */ |
isagmz | 21:d1faccb96146 | 237 | float getTemp( void ); |
isagmz | 21:d1faccb96146 | 238 | |
isagmz | 21:d1faccb96146 | 239 | /** |
isagmz | 21:d1faccb96146 | 240 | * Sets the sleep mode of the MPU6050 |
isagmz | 21:d1faccb96146 | 241 | * |
isagmz | 21:d1faccb96146 | 242 | * @param state - true for sleeping, false for wake up |
isagmz | 21:d1faccb96146 | 243 | */ |
isagmz | 21:d1faccb96146 | 244 | void setSleepMode( bool state ); |
isagmz | 21:d1faccb96146 | 245 | |
isagmz | 21:d1faccb96146 | 246 | |
isagmz | 21:d1faccb96146 | 247 | /** |
isagmz | 21:d1faccb96146 | 248 | * Writes data to the device, could be private, but public is handy so you can transmit directly to the MPU. |
isagmz | 21:d1faccb96146 | 249 | * |
isagmz | 21:d1faccb96146 | 250 | * @param adress - register address to write to |
isagmz | 21:d1faccb96146 | 251 | * @param data - data to write |
isagmz | 21:d1faccb96146 | 252 | */ |
isagmz | 21:d1faccb96146 | 253 | void write( char address, char data); |
isagmz | 21:d1faccb96146 | 254 | |
isagmz | 21:d1faccb96146 | 255 | /** |
isagmz | 21:d1faccb96146 | 256 | * Read data from the device, could be private, but public is handy so you can transmit directly to the MPU. |
isagmz | 21:d1faccb96146 | 257 | * |
isagmz | 21:d1faccb96146 | 258 | * @param adress - register address to write to |
isagmz | 21:d1faccb96146 | 259 | * @return - data from the register specified by RA |
isagmz | 21:d1faccb96146 | 260 | */ |
isagmz | 21:d1faccb96146 | 261 | char read( char adress); |
isagmz | 21:d1faccb96146 | 262 | |
isagmz | 21:d1faccb96146 | 263 | /** |
isagmz | 21:d1faccb96146 | 264 | * Read multtiple regigsters from the device, more efficient than using multiple normal reads. |
isagmz | 21:d1faccb96146 | 265 | * |
isagmz | 21:d1faccb96146 | 266 | * @param adress - register address to write to |
isagmz | 21:d1faccb96146 | 267 | * @param length - number of bytes to read |
isagmz | 21:d1faccb96146 | 268 | * @param data - pointer where the data needs to be written to |
isagmz | 21:d1faccb96146 | 269 | */ |
isagmz | 21:d1faccb96146 | 270 | void read( char adress, char *data, int length); |
isagmz | 21:d1faccb96146 | 271 | |
isagmz | 21:d1faccb96146 | 272 | /** |
isagmz | 21:d1faccb96146 | 273 | * function for calculating the angle from the accelerometer. |
isagmz | 21:d1faccb96146 | 274 | * it takes 3 values which correspond acceleration in X, Y and Z direction and calculates angles in degrees |
isagmz | 21:d1faccb96146 | 275 | * for pitch, roll and one more direction.. (NOT YAW!) |
isagmz | 21:d1faccb96146 | 276 | * |
isagmz | 21:d1faccb96146 | 277 | * @param data - angle calculated using only accelerometer |
isagmz | 21:d1faccb96146 | 278 | * |
isagmz | 21:d1faccb96146 | 279 | */ |
isagmz | 21:d1faccb96146 | 280 | void getAcceleroAngle( float *data ); |
isagmz | 21:d1faccb96146 | 281 | |
isagmz | 21:d1faccb96146 | 282 | |
isagmz | 21:d1faccb96146 | 283 | /**function which allows to produce the offset values for gyro and accelerometer. |
isagmz | 21:d1faccb96146 | 284 | * offset for gyro is simply a value, which needs to be substracted from original gyro rad/sec speed |
isagmz | 21:d1faccb96146 | 285 | * but offset for accelerometer is calculated in angles... later on might change that |
isagmz | 21:d1faccb96146 | 286 | * function simply takes the number of samples to be taken and calculated the average |
isagmz | 21:d1faccb96146 | 287 | * |
isagmz | 21:d1faccb96146 | 288 | * @param accOffset - accelerometer offset in angle |
isagmz | 21:d1faccb96146 | 289 | * @param gyroOffset - gyroscope offset in rad/s |
isagmz | 21:d1faccb96146 | 290 | * @param sampleSize - number of samples to be taken for calculating offsets |
isagmz | 21:d1faccb96146 | 291 | * |
isagmz | 21:d1faccb96146 | 292 | */ |
isagmz | 21:d1faccb96146 | 293 | void getOffset(float *accOffset, float *gyroOffset, int sampleSize); |
isagmz | 21:d1faccb96146 | 294 | |
isagmz | 21:d1faccb96146 | 295 | /** |
isagmz | 21:d1faccb96146 | 296 | * function for computing the angle, when accelerometer angle offset and gyro offset are both known. |
isagmz | 21:d1faccb96146 | 297 | * we also need to know how much time passed from previous calculation to now |
isagmz | 21:d1faccb96146 | 298 | * it produces the angle in degrees. However angles are produced from -90.0 to 90.0 degrees |
isagmz | 21:d1faccb96146 | 299 | * if anyone need smth different, they can update this library... |
isagmz | 21:d1faccb96146 | 300 | * |
isagmz | 21:d1faccb96146 | 301 | * @param angle - calculated accurate angle from complemetary filter |
isagmz | 21:d1faccb96146 | 302 | * @param accOffset - offset in angle for the accelerometer |
isagmz | 21:d1faccb96146 | 303 | * @param gyroOffset - offset in rad/s for the gyroscope |
isagmz | 21:d1faccb96146 | 304 | * @param interval - time before previous angle calculation and now |
isagmz | 21:d1faccb96146 | 305 | * |
isagmz | 21:d1faccb96146 | 306 | */ |
isagmz | 21:d1faccb96146 | 307 | void computeAngle (float *angle, float *accOffset, float *gyroOffset, float interval); |
isagmz | 21:d1faccb96146 | 308 | |
isagmz | 21:d1faccb96146 | 309 | ///function, which enables interrupts on MPU6050 INT pin |
isagmz | 21:d1faccb96146 | 310 | void enableInt( void ); |
isagmz | 21:d1faccb96146 | 311 | |
isagmz | 21:d1faccb96146 | 312 | ///disables interrupts |
isagmz | 21:d1faccb96146 | 313 | void disableInt( void ); |
isagmz | 21:d1faccb96146 | 314 | |
isagmz | 21:d1faccb96146 | 315 | /**function which sets the alpha value - constant for the complementary filter. default alpha = 0.97 |
isagmz | 21:d1faccb96146 | 316 | * |
isagmz | 21:d1faccb96146 | 317 | * @param val - value the alpha (complementary filter constant) should be set to |
isagmz | 21:d1faccb96146 | 318 | * |
isagmz | 21:d1faccb96146 | 319 | */ |
isagmz | 21:d1faccb96146 | 320 | void setAlpha(float val); |
isagmz | 21:d1faccb96146 | 321 | |
isagmz | 21:d1faccb96146 | 322 | private: |
isagmz | 21:d1faccb96146 | 323 | |
isagmz | 21:d1faccb96146 | 324 | I2C connection; |
isagmz | 21:d1faccb96146 | 325 | char currentAcceleroRange; |
isagmz | 21:d1faccb96146 | 326 | char currentGyroRange; |
isagmz | 21:d1faccb96146 | 327 | float alpha; |
isagmz | 21:d1faccb96146 | 328 | |
isagmz | 21:d1faccb96146 | 329 | }; |
isagmz | 21:d1faccb96146 | 330 | |
isagmz | 21:d1faccb96146 | 331 | |
isagmz | 21:d1faccb96146 | 332 | |
isagmz | 21:d1faccb96146 | 333 | #endif |