CIA / Mbed 2 deprecated AttitudeEstimation_usingTicker

MPU6050/9250で姿勢を推定するプログラム ・ジャイロ積算のみ（update()） ・ジャイロ積算後，加速度で補正（update_correction()） の2パターンの関数がある．

Dependencies:   Eigen mbed

MPU9250/MPU9250.h@0:29dce55dbcfe, 2018-03-20 (annotated)

Committer:
daqn
Date:
Tue Mar 20 13:46:54 2018 +0000
Revision:
0:29dce55dbcfe
??????????????; ??????????????; ?????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daqn 0:29dce55dbcfe 1 /** Daqn change log
daqn 0:29dce55dbcfe 2 * 2018/02/16 : Replace words "MPU6050" with "MPU9250".
daqn 0:29dce55dbcfe 3 * 2018/02/21 :
daqn 0:29dce55dbcfe 4 */
daqn 0:29dce55dbcfe 5
daqn 0:29dce55dbcfe 6 /*Use #define MPU9250_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.
daqn 0:29dce55dbcfe 7 If it is half of what you expected, and you still are on the correct planet, you got an engineering sample
daqn 0:29dce55dbcfe 8 */
daqn 0:29dce55dbcfe 9
daqn 0:29dce55dbcfe 10
daqn 0:29dce55dbcfe 11 #ifndef MPU9250_H
daqn 0:29dce55dbcfe 12 #define MPU9250_H
daqn 0:29dce55dbcfe 13
daqn 0:29dce55dbcfe 14 /**
daqn 0:29dce55dbcfe 15 * Includes
daqn 0:29dce55dbcfe 16 */
daqn 0:29dce55dbcfe 17 #include "mbed.h"
daqn 0:29dce55dbcfe 18
daqn 0:29dce55dbcfe 19
daqn 0:29dce55dbcfe 20 /**
daqn 0:29dce55dbcfe 21 * Defines
daqn 0:29dce55dbcfe 22 */
daqn 0:29dce55dbcfe 23 #ifndef MPU9250_ADDRESS
daqn 0:29dce55dbcfe 24 #define MPU9250_ADDRESS 0x68 // address pin low (GND), default for InvenSense evaluation board
daqn 0:29dce55dbcfe 25 #endif
daqn 0:29dce55dbcfe 26
daqn 0:29dce55dbcfe 27 #ifdef MPU9250_ES
daqn 0:29dce55dbcfe 28 #define DOUBLE_ACCELERO
daqn 0:29dce55dbcfe 29 #endif
daqn 0:29dce55dbcfe 30
daqn 0:29dce55dbcfe 31 /**
daqn 0:29dce55dbcfe 32 * Registers
daqn 0:29dce55dbcfe 33 */
daqn 0:29dce55dbcfe 34 #define MPU9250_CONFIG_REG 0x1A
daqn 0:29dce55dbcfe 35 #define MPU9250_GYRO_CONFIG_REG 0x1B
daqn 0:29dce55dbcfe 36 #define MPU9250_ACCELERO_CONFIG_REG 0x1C
daqn 0:29dce55dbcfe 37
daqn 0:29dce55dbcfe 38 #define MPU9250_INT_PIN_CFG 0x37
daqn 0:29dce55dbcfe 39
daqn 0:29dce55dbcfe 40 #define MPU9250_ACCEL_XOUT_H_REG 0x3B
daqn 0:29dce55dbcfe 41 #define MPU9250_ACCEL_YOUT_H_REG 0x3D
daqn 0:29dce55dbcfe 42 #define MPU9250_ACCEL_ZOUT_H_REG 0x3F
daqn 0:29dce55dbcfe 43
daqn 0:29dce55dbcfe 44 #define MPU9250_TEMP_H_REG 0x41
daqn 0:29dce55dbcfe 45
daqn 0:29dce55dbcfe 46 #define MPU9250_GYRO_XOUT_H_REG 0x43
daqn 0:29dce55dbcfe 47 #define MPU9250_GYRO_YOUT_H_REG 0x45
daqn 0:29dce55dbcfe 48 #define MPU9250_GYRO_ZOUT_H_REG 0x47
daqn 0:29dce55dbcfe 49
daqn 0:29dce55dbcfe 50 #define MPU9250_MAG_XOUT_H_REG 0x04
daqn 0:29dce55dbcfe 51 #define MPU9250_MAG_YOUT_H_REG 0x06
daqn 0:29dce55dbcfe 52 #define MPU9250_MAG_ZOUT_H_REG 0x08
daqn 0:29dce55dbcfe 53
daqn 0:29dce55dbcfe 54
daqn 0:29dce55dbcfe 55
daqn 0:29dce55dbcfe 56 #define MPU9250_PWR_MGMT_1_REG 0x6B
daqn 0:29dce55dbcfe 57 #define MPU9250_WHO_AM_I_REG 0x75
daqn 0:29dce55dbcfe 58
daqn 0:29dce55dbcfe 59
daqn 0:29dce55dbcfe 60
daqn 0:29dce55dbcfe 61 /**
daqn 0:29dce55dbcfe 62 * Definitions
daqn 0:29dce55dbcfe 63 */
daqn 0:29dce55dbcfe 64 #define MPU9250_SLP_BIT 6
daqn 0:29dce55dbcfe 65 #define MPU9250_BYPASS_BIT 1
daqn 0:29dce55dbcfe 66
daqn 0:29dce55dbcfe 67 #define MPU9250_BW_256 0
daqn 0:29dce55dbcfe 68 #define MPU9250_BW_188 1
daqn 0:29dce55dbcfe 69 #define MPU9250_BW_98 2
daqn 0:29dce55dbcfe 70 #define MPU9250_BW_42 3
daqn 0:29dce55dbcfe 71 #define MPU9250_BW_20 4
daqn 0:29dce55dbcfe 72 #define MPU9250_BW_10 5
daqn 0:29dce55dbcfe 73 #define MPU9250_BW_5 6
daqn 0:29dce55dbcfe 74
daqn 0:29dce55dbcfe 75 #define MPU9250_ACCELERO_RANGE_2G 0
daqn 0:29dce55dbcfe 76 #define MPU9250_ACCELERO_RANGE_4G 1
daqn 0:29dce55dbcfe 77 #define MPU9250_ACCELERO_RANGE_8G 2
daqn 0:29dce55dbcfe 78 #define MPU9250_ACCELERO_RANGE_16G 3
daqn 0:29dce55dbcfe 79
daqn 0:29dce55dbcfe 80 #define MPU9250_GYRO_RANGE_250 0
daqn 0:29dce55dbcfe 81 #define MPU9250_GYRO_RANGE_500 1
daqn 0:29dce55dbcfe 82 #define MPU9250_GYRO_RANGE_1000 2
daqn 0:29dce55dbcfe 83 #define MPU9250_GYRO_RANGE_2000 3
daqn 0:29dce55dbcfe 84
daqn 0:29dce55dbcfe 85 //interrupt address
daqn 0:29dce55dbcfe 86 #define MPU9250_RA_INT_ENABLE 0x38
daqn 0:29dce55dbcfe 87 //define how the accelerometer is placed on surface
daqn 0:29dce55dbcfe 88 #define X_AXIS 1
daqn 0:29dce55dbcfe 89 #define Y_AXIS 2
daqn 0:29dce55dbcfe 90 #define Z_AXIS 0
daqn 0:29dce55dbcfe 91 //translation from radians to degrees
daqn 0:29dce55dbcfe 92 #define RADIANS_TO_DEGREES 180/3.1415926536
daqn 0:29dce55dbcfe 93 //constant used for the complementary filter, which ranges usually from 0.9 to 1.0
daqn 0:29dce55dbcfe 94 #define ALPHA 0.96 //filter constant
daqn 0:29dce55dbcfe 95 //scale the gyro
daqn 0:29dce55dbcfe 96 #define GYRO_SCALE 2.7176
daqn 0:29dce55dbcfe 97
daqn 0:29dce55dbcfe 98
daqn 0:29dce55dbcfe 99 /**
daqn 0:29dce55dbcfe 100 * Magnetometer
daqn 0:29dce55dbcfe 101 */
daqn 0:29dce55dbcfe 102 #define AK8963_ADDRESS 0x0C<<1
daqn 0:29dce55dbcfe 103 #define WHO_AM_I_AK8963 0x00 // should return 0x48
daqn 0:29dce55dbcfe 104 #define INFO 0x01
daqn 0:29dce55dbcfe 105 #define AK8963_ST1 0x02 // data ready status bit 0
daqn 0:29dce55dbcfe 106 #define AK8963_XOUT_L 0x03 // data
daqn 0:29dce55dbcfe 107 #define AK8963_XOUT_H 0x04
daqn 0:29dce55dbcfe 108 #define AK8963_YOUT_L 0x05
daqn 0:29dce55dbcfe 109 #define AK8963_YOUT_H 0x06
daqn 0:29dce55dbcfe 110 #define AK8963_ZOUT_L 0x07
daqn 0:29dce55dbcfe 111 #define AK8963_ZOUT_H 0x08
daqn 0:29dce55dbcfe 112 #define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2
daqn 0:29dce55dbcfe 113 #define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
daqn 0:29dce55dbcfe 114 #define AK8963_ASTC 0x0C // Self test control
daqn 0:29dce55dbcfe 115 #define AK8963_I2CDIS 0x0F // I2C disable
daqn 0:29dce55dbcfe 116 #define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
daqn 0:29dce55dbcfe 117 #define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
daqn 0:29dce55dbcfe 118 #define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
daqn 0:29dce55dbcfe 119
daqn 0:29dce55dbcfe 120
daqn 0:29dce55dbcfe 121 /** MPU9250 IMU library.
daqn 0:29dce55dbcfe 122 *
daqn 0:29dce55dbcfe 123 * Example:
daqn 0:29dce55dbcfe 124 * @code
daqn 0:29dce55dbcfe 125 * Later, maybe
daqn 0:29dce55dbcfe 126 * @endcode
daqn 0:29dce55dbcfe 127 */
daqn 0:29dce55dbcfe 128 class MPU9250
daqn 0:29dce55dbcfe 129 {
daqn 0:29dce55dbcfe 130 public:
daqn 0:29dce55dbcfe 131 /**
daqn 0:29dce55dbcfe 132 * Constructor.
daqn 0:29dce55dbcfe 133 *
daqn 0:29dce55dbcfe 134 * Sleep mode of MPU9250 is immediatly disabled
daqn 0:29dce55dbcfe 135 *
daqn 0:29dce55dbcfe 136 * @param sda - mbed pin to use for the SDA I2C line.
daqn 0:29dce55dbcfe 137 * @param scl - mbed pin to use for the SCL I2C line.
daqn 0:29dce55dbcfe 138 */
daqn 0:29dce55dbcfe 139 MPU9250(PinName sda, PinName scl);
daqn 0:29dce55dbcfe 140
daqn 0:29dce55dbcfe 141
daqn 0:29dce55dbcfe 142 /**
daqn 0:29dce55dbcfe 143 * Tests the I2C connection by reading the WHO_AM_I register.
daqn 0:29dce55dbcfe 144 *
daqn 0:29dce55dbcfe 145 * @return True for a working connection, false for an error
daqn 0:29dce55dbcfe 146 */
daqn 0:29dce55dbcfe 147 bool testConnection( void );
daqn 0:29dce55dbcfe 148
daqn 0:29dce55dbcfe 149 /**
daqn 0:29dce55dbcfe 150 * Sets the bandwidth of the digital low-pass filter
daqn 0:29dce55dbcfe 151 *
daqn 0:29dce55dbcfe 152 * Macros: MPU9250_BW_256 - MPU9250_BW_188 - MPU9250_BW_98 - MPU9250_BW_42 - MPU9250_BW_20 - MPU9250_BW_10 - MPU9250_BW_5
daqn 0:29dce55dbcfe 153 * Last number is the gyro's BW in Hz (accelero BW is virtually identical)
daqn 0:29dce55dbcfe 154 *
daqn 0:29dce55dbcfe 155 * @param BW - The three bits that set the bandwidth (use the predefined macros)
daqn 0:29dce55dbcfe 156 */
daqn 0:29dce55dbcfe 157 void setBW( char BW );
daqn 0:29dce55dbcfe 158
daqn 0:29dce55dbcfe 159 /**
daqn 0:29dce55dbcfe 160 * Sets the auxiliary I2C bus in bypass mode to read the sensors behind the MPU9250 (useful for eval board, otherwise just connect them to primary I2C bus)
daqn 0:29dce55dbcfe 161 *
daqn 0:29dce55dbcfe 162 * @param state - Enables/disables the I2C bypass mode
daqn 0:29dce55dbcfe 163 */
daqn 0:29dce55dbcfe 164 void setI2CBypass ( bool state );
daqn 0:29dce55dbcfe 165
daqn 0:29dce55dbcfe 166 /**
daqn 0:29dce55dbcfe 167 * Sets the Accelero full-scale range
daqn 0:29dce55dbcfe 168 *
daqn 0:29dce55dbcfe 169 * Macros: MPU9250_ACCELERO_RANGE_2G - MPU9250_ACCELERO_RANGE_4G - MPU9250_ACCELERO_RANGE_8G - MPU9250_ACCELERO_RANGE_16G
daqn 0:29dce55dbcfe 170 *
daqn 0:29dce55dbcfe 171 * @param range - The two bits that set the full-scale range (use the predefined macros)
daqn 0:29dce55dbcfe 172 */
daqn 0:29dce55dbcfe 173 void setAcceleroRange(char range);
daqn 0:29dce55dbcfe 174
daqn 0:29dce55dbcfe 175 /**
daqn 0:29dce55dbcfe 176 * Reads the accelero x-axis.
daqn 0:29dce55dbcfe 177 *
daqn 0:29dce55dbcfe 178 * @return 16-bit signed integer x-axis accelero data
daqn 0:29dce55dbcfe 179 */
daqn 0:29dce55dbcfe 180 int getAcceleroRawX( void );
daqn 0:29dce55dbcfe 181
daqn 0:29dce55dbcfe 182 /**
daqn 0:29dce55dbcfe 183 * Reads the accelero y-axis.
daqn 0:29dce55dbcfe 184 *
daqn 0:29dce55dbcfe 185 * @return 16-bit signed integer y-axis accelero data
daqn 0:29dce55dbcfe 186 */
daqn 0:29dce55dbcfe 187 int getAcceleroRawY( void );
daqn 0:29dce55dbcfe 188
daqn 0:29dce55dbcfe 189 /**
daqn 0:29dce55dbcfe 190 * Reads the accelero z-axis.
daqn 0:29dce55dbcfe 191 *
daqn 0:29dce55dbcfe 192 * @return 16-bit signed integer z-axis accelero data
daqn 0:29dce55dbcfe 193 */
daqn 0:29dce55dbcfe 194 int getAcceleroRawZ( void );
daqn 0:29dce55dbcfe 195
daqn 0:29dce55dbcfe 196 /**
daqn 0:29dce55dbcfe 197 * Reads all accelero data.
daqn 0:29dce55dbcfe 198 *
daqn 0:29dce55dbcfe 199 * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z
daqn 0:29dce55dbcfe 200 */
daqn 0:29dce55dbcfe 201 void getAcceleroRaw( int *data );
daqn 0:29dce55dbcfe 202
daqn 0:29dce55dbcfe 203 /**
daqn 0:29dce55dbcfe 204 * Reads all accelero data, gives the acceleration in m/s2
daqn 0:29dce55dbcfe 205 *
daqn 0:29dce55dbcfe 206 * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work.
daqn 0:29dce55dbcfe 207 *
daqn 0:29dce55dbcfe 208 * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z
daqn 0:29dce55dbcfe 209 */
daqn 0:29dce55dbcfe 210 void getAccelero( float *data );
daqn 0:29dce55dbcfe 211
daqn 0:29dce55dbcfe 212 /**
daqn 0:29dce55dbcfe 213 * Sets the Gyro full-scale range
daqn 0:29dce55dbcfe 214 *
daqn 0:29dce55dbcfe 215 * Macros: MPU9250_GYRO_RANGE_250 - MPU9250_GYRO_RANGE_500 - MPU9250_GYRO_RANGE_1000 - MPU9250_GYRO_RANGE_2000
daqn 0:29dce55dbcfe 216 *
daqn 0:29dce55dbcfe 217 * @param range - The two bits that set the full-scale range (use the predefined macros)
daqn 0:29dce55dbcfe 218 */
daqn 0:29dce55dbcfe 219 void setGyroRange(char range);
daqn 0:29dce55dbcfe 220
daqn 0:29dce55dbcfe 221 /**
daqn 0:29dce55dbcfe 222 * Reads the gyro x-axis.
daqn 0:29dce55dbcfe 223 *
daqn 0:29dce55dbcfe 224 * @return 16-bit signed integer x-axis gyro data
daqn 0:29dce55dbcfe 225 */
daqn 0:29dce55dbcfe 226 int getGyroRawX( void );
daqn 0:29dce55dbcfe 227
daqn 0:29dce55dbcfe 228 /**
daqn 0:29dce55dbcfe 229 * Reads the gyro y-axis.
daqn 0:29dce55dbcfe 230 *
daqn 0:29dce55dbcfe 231 * @return 16-bit signed integer y-axis gyro data
daqn 0:29dce55dbcfe 232 */
daqn 0:29dce55dbcfe 233 int getGyroRawY( void );
daqn 0:29dce55dbcfe 234
daqn 0:29dce55dbcfe 235 /**
daqn 0:29dce55dbcfe 236 * Reads the gyro z-axis.
daqn 0:29dce55dbcfe 237 *
daqn 0:29dce55dbcfe 238 * @return 16-bit signed integer z-axis gyro data
daqn 0:29dce55dbcfe 239 */
daqn 0:29dce55dbcfe 240 int getGyroRawZ( void );
daqn 0:29dce55dbcfe 241
daqn 0:29dce55dbcfe 242 /**
daqn 0:29dce55dbcfe 243 * Reads all gyro data.
daqn 0:29dce55dbcfe 244 *
daqn 0:29dce55dbcfe 245 * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z
daqn 0:29dce55dbcfe 246 */
daqn 0:29dce55dbcfe 247 void getGyroRaw( int *data );
daqn 0:29dce55dbcfe 248
daqn 0:29dce55dbcfe 249 /**
daqn 0:29dce55dbcfe 250 * Reads all gyro data, gives the gyro in rad/s
daqn 0:29dce55dbcfe 251 *
daqn 0:29dce55dbcfe 252 * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work.
daqn 0:29dce55dbcfe 253 *
daqn 0:29dce55dbcfe 254 * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z
daqn 0:29dce55dbcfe 255 */
daqn 0:29dce55dbcfe 256 void getGyro( float *data);
daqn 0:29dce55dbcfe 257
daqn 0:29dce55dbcfe 258 /**
daqn 0:29dce55dbcfe 259 * Reads temperature data.
daqn 0:29dce55dbcfe 260 *
daqn 0:29dce55dbcfe 261 * @return 16 bit signed integer with the raw temperature register value
daqn 0:29dce55dbcfe 262 */
daqn 0:29dce55dbcfe 263 int getTempRaw( void );
daqn 0:29dce55dbcfe 264
daqn 0:29dce55dbcfe 265 /**
daqn 0:29dce55dbcfe 266 * Returns current temperature
daqn 0:29dce55dbcfe 267 *
daqn 0:29dce55dbcfe 268 * @returns float with the current temperature
daqn 0:29dce55dbcfe 269 */
daqn 0:29dce55dbcfe 270 float getTemp( void );
daqn 0:29dce55dbcfe 271
daqn 0:29dce55dbcfe 272 /**
daqn 0:29dce55dbcfe 273 * Sets the sleep mode of the MPU9250
daqn 0:29dce55dbcfe 274 *
daqn 0:29dce55dbcfe 275 * @param state - true for sleeping, false for wake up
daqn 0:29dce55dbcfe 276 */
daqn 0:29dce55dbcfe 277 void setSleepMode( bool state );
daqn 0:29dce55dbcfe 278
daqn 0:29dce55dbcfe 279
daqn 0:29dce55dbcfe 280 /**
daqn 0:29dce55dbcfe 281 * Writes data to the device, could be private, but public is handy so you can transmit directly to the MPU.
daqn 0:29dce55dbcfe 282 *
daqn 0:29dce55dbcfe 283 * @param adress - register address to write to
daqn 0:29dce55dbcfe 284 * @param data - data to write
daqn 0:29dce55dbcfe 285 */
daqn 0:29dce55dbcfe 286 void write( char address, char data);
daqn 0:29dce55dbcfe 287
daqn 0:29dce55dbcfe 288 /**
daqn 0:29dce55dbcfe 289 * Read data from the device, could be private, but public is handy so you can transmit directly to the MPU.
daqn 0:29dce55dbcfe 290 *
daqn 0:29dce55dbcfe 291 * @param adress - register address to write to
daqn 0:29dce55dbcfe 292 * @return - data from the register specified by RA
daqn 0:29dce55dbcfe 293 */
daqn 0:29dce55dbcfe 294 char read( char adress);
daqn 0:29dce55dbcfe 295
daqn 0:29dce55dbcfe 296 /**
daqn 0:29dce55dbcfe 297 * Read multtiple regigsters from the device, more efficient than using multiple normal reads.
daqn 0:29dce55dbcfe 298 *
daqn 0:29dce55dbcfe 299 * @param adress - register address to write to
daqn 0:29dce55dbcfe 300 * @param length - number of bytes to read
daqn 0:29dce55dbcfe 301 * @param data - pointer where the data needs to be written to
daqn 0:29dce55dbcfe 302 */
daqn 0:29dce55dbcfe 303 void read( char adress, char *data, int length);
daqn 0:29dce55dbcfe 304
daqn 0:29dce55dbcfe 305 /**
daqn 0:29dce55dbcfe 306 * function for calculating the angle from the accelerometer.
daqn 0:29dce55dbcfe 307 * it takes 3 values which correspond acceleration in X, Y and Z direction and calculates angles in degrees
daqn 0:29dce55dbcfe 308 * for pitch, roll and one more direction.. (NOT YAW!)
daqn 0:29dce55dbcfe 309 *
daqn 0:29dce55dbcfe 310 * @param data - angle calculated using only accelerometer
daqn 0:29dce55dbcfe 311 *
daqn 0:29dce55dbcfe 312 */
daqn 0:29dce55dbcfe 313 void getAcceleroAngle( float *data );
daqn 0:29dce55dbcfe 314
daqn 0:29dce55dbcfe 315
daqn 0:29dce55dbcfe 316 /**function which allows to produce the offset values for gyro and accelerometer.
daqn 0:29dce55dbcfe 317 * offset for gyro is simply a value, which needs to be substracted from original gyro rad/sec speed
daqn 0:29dce55dbcfe 318 * but offset for accelerometer is calculated in angles... later on might change that
daqn 0:29dce55dbcfe 319 * function simply takes the number of samples to be taken and calculated the average
daqn 0:29dce55dbcfe 320 *
daqn 0:29dce55dbcfe 321 * @param accOffset - accelerometer offset in angle
daqn 0:29dce55dbcfe 322 * @param gyroOffset - gyroscope offset in rad/s
daqn 0:29dce55dbcfe 323 * @param sampleSize - number of samples to be taken for calculating offsets
daqn 0:29dce55dbcfe 324 *
daqn 0:29dce55dbcfe 325 */
daqn 0:29dce55dbcfe 326 void getOffset(float *accOffset, float *gyroOffset, int sampleSize);
daqn 0:29dce55dbcfe 327
daqn 0:29dce55dbcfe 328 /**
daqn 0:29dce55dbcfe 329 * function for computing the angle, when accelerometer angle offset and gyro offset are both known.
daqn 0:29dce55dbcfe 330 * we also need to know how much time passed from previous calculation to now
daqn 0:29dce55dbcfe 331 * it produces the angle in degrees. However angles are produced from -90.0 to 90.0 degrees
daqn 0:29dce55dbcfe 332 * if anyone need smth different, they can update this library...
daqn 0:29dce55dbcfe 333 *
daqn 0:29dce55dbcfe 334 * @param angle - calculated accurate angle from complemetary filter
daqn 0:29dce55dbcfe 335 * @param accOffset - offset in angle for the accelerometer
daqn 0:29dce55dbcfe 336 * @param gyroOffset - offset in rad/s for the gyroscope
daqn 0:29dce55dbcfe 337 * @param interval - time before previous angle calculation and now
daqn 0:29dce55dbcfe 338 *
daqn 0:29dce55dbcfe 339 */
daqn 0:29dce55dbcfe 340 void computeAngle (float *angle, float *accOffset, float *gyroOffset, float interval);
daqn 0:29dce55dbcfe 341
daqn 0:29dce55dbcfe 342 ///function, which enables interrupts on MPU9250 INT pin
daqn 0:29dce55dbcfe 343 void enableInt( void );
daqn 0:29dce55dbcfe 344
daqn 0:29dce55dbcfe 345 ///disables interrupts
daqn 0:29dce55dbcfe 346 void disableInt( void );
daqn 0:29dce55dbcfe 347
daqn 0:29dce55dbcfe 348 /**function which sets the alpha value - constant for the complementary filter. default alpha = 0.97
daqn 0:29dce55dbcfe 349 *
daqn 0:29dce55dbcfe 350 * @param val - value the alpha (complementary filter constant) should be set to
daqn 0:29dce55dbcfe 351 *
daqn 0:29dce55dbcfe 352 */
daqn 0:29dce55dbcfe 353 void setAlpha(float val);
daqn 0:29dce55dbcfe 354
daqn 0:29dce55dbcfe 355 ////////////////////////////////////////////////////////////////////////////
daqn 0:29dce55dbcfe 356 ////////////////////////////////////////////////////////////////////////////
daqn 0:29dce55dbcfe 357 ////////////////////////////////////////////////////////////////////////////
daqn 0:29dce55dbcfe 358
daqn 0:29dce55dbcfe 359 /** Daqn added
daqn 0:29dce55dbcfe 360 * read bytes on specific address and subaddress.
daqn 0:29dce55dbcfe 361 *
daqn 0:29dce55dbcfe 362 * @param adress - register address to write to
daqn 0:29dce55dbcfe 363 * @param subadress - register address to write to
daqn 0:29dce55dbcfe 364 * @param length - number of bytes to read
daqn 0:29dce55dbcfe 365 * @param data - pointer where the data needs to be written to
daqn 0:29dce55dbcfe 366 */
daqn 0:29dce55dbcfe 367 void read(char address, char subaddress, char *data, int length);
daqn 0:29dce55dbcfe 368
daqn 0:29dce55dbcfe 369 /** Daqn added
daqn 0:29dce55dbcfe 370 * Sets the Magneto full-scale range
daqn 0:29dce55dbcfe 371 *
daqn 0:29dce55dbcfe 372 * Macros: MPU9250_MAGNETO_RANGE_250 - MPU9250_MAGNETO_RANGE_500 - MPU9250_MAGNETO_RANGE_1000 - MPU9250_MAGNETO_RANGE_2000
daqn 0:29dce55dbcfe 373 *
daqn 0:29dce55dbcfe 374 * @param range - The two bits that set the full-scale range (use the predefined macros)
daqn 0:29dce55dbcfe 375 */
daqn 0:29dce55dbcfe 376 // void setMagnetoRange(char range);
daqn 0:29dce55dbcfe 377
daqn 0:29dce55dbcfe 378 /** Daqn added
daqn 0:29dce55dbcfe 379 * Reads the magnetometer x-axis.
daqn 0:29dce55dbcfe 380 *
daqn 0:29dce55dbcfe 381 * @return 16-bit signed integer x-axis magnetometer data
daqn 0:29dce55dbcfe 382 */
daqn 0:29dce55dbcfe 383 int getMagnetoRawX( void );
daqn 0:29dce55dbcfe 384
daqn 0:29dce55dbcfe 385 /** Daqn added
daqn 0:29dce55dbcfe 386 * Reads the magnetometer y-axis.
daqn 0:29dce55dbcfe 387 *
daqn 0:29dce55dbcfe 388 * @return 16-bit signed integer y-axis magnetometer data
daqn 0:29dce55dbcfe 389 */
daqn 0:29dce55dbcfe 390 int getMagnetoRawY( void );
daqn 0:29dce55dbcfe 391
daqn 0:29dce55dbcfe 392 /** Daqn added
daqn 0:29dce55dbcfe 393 * Reads the magnetometer z-axis.
daqn 0:29dce55dbcfe 394 *
daqn 0:29dce55dbcfe 395 * @return 16-bit signed integer z-axis magnetometer data
daqn 0:29dce55dbcfe 396 */
daqn 0:29dce55dbcfe 397 int getMagnetoRawZ( void );
daqn 0:29dce55dbcfe 398
daqn 0:29dce55dbcfe 399 /** Daqn added
daqn 0:29dce55dbcfe 400 * Reads all magnetometer data.
daqn 0:29dce55dbcfe 401 *
daqn 0:29dce55dbcfe 402 * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, data[2] = Z
daqn 0:29dce55dbcfe 403 */
daqn 0:29dce55dbcfe 404 void getMagnetoRaw( int *data );
daqn 0:29dce55dbcfe 405
daqn 0:29dce55dbcfe 406 /** Daqn added
daqn 0:29dce55dbcfe 407 * Reads all magnetometer data, gives the magnetometer in [[[TBD]]]
daqn 0:29dce55dbcfe 408 *
daqn 0:29dce55dbcfe 409 * Function uses the last setup value of the full scale range, if you manually set in another range, this won't work.
daqn 0:29dce55dbcfe 410 *
daqn 0:29dce55dbcfe 411 * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z
daqn 0:29dce55dbcfe 412 */
daqn 0:29dce55dbcfe 413 void getMagneto( float *data);
daqn 0:29dce55dbcfe 414
daqn 0:29dce55dbcfe 415 private:
daqn 0:29dce55dbcfe 416
daqn 0:29dce55dbcfe 417 I2C connection;
daqn 0:29dce55dbcfe 418 char currentAcceleroRange;
daqn 0:29dce55dbcfe 419 char currentGyroRange;
daqn 0:29dce55dbcfe 420 float alpha;
daqn 0:29dce55dbcfe 421
daqn 0:29dce55dbcfe 422 };
daqn 0:29dce55dbcfe 423
daqn 0:29dce55dbcfe 424
daqn 0:29dce55dbcfe 425
daqn 0:29dce55dbcfe 426 #endif