![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
9月16日0時35分
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_13 by
MPU9250/MPU9250.h
- Committer:
- taknokolat
- Date:
- 2018-09-15
- Revision:
- 10:652071c20bf6
- Parent:
- 0:17f575135219
File content as of revision 10:652071c20bf6:
#ifndef MPU9250_H #define MPU9250_H #include "mbed.h" #include "math.h" #include "MPU9250_register.h" #define AK8963_ADDRESS 0x0C<<1 // Using the MSENSR-9250 breakout board, ADO is set to 0 // Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 //mbed uses the eight-bit device address, so shift seven-bit addresses left by one! #define ADO 0 #if ADO #define MPU9250_ADDRESS 0x69<<1 // Device address when ADO = 1 #else #define MPU9250_ADDRESS 0x68<<1 // Device address when ADO = 0 #endif #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral #define Ki 0.0f #define IAM_MPU9250 0x71 #define IAM_AK8963 0x48 #define DECLINATION -7.45f //Declination at Tokyo is -7.45 degree 2017/06/14 const float PI = 3.14159265358979323846f; const float LPGAIN_MAG = 0.0; enum Ascale { AFS_2G = 0, AFS_4G, AFS_8G, AFS_16G }; enum Gscale { GFS_250DPS = 0, GFS_500DPS, GFS_1000DPS, GFS_2000DPS }; enum Mscale { MFS_14BITS = 0, // 0.6 mG per LSB MFS_16BITS // 0.15 mG per LSB }; class MPU9250 { public: //MPU9250(); MPU9250(PinName sda, PinName scl, RawSerial* serial_p); ~MPU9250(); bool Initialize(void); bool sensingAcGyMg(void); void calculatePostureAngle(float degree[3]); float calculateYawByMg(void); void MPU9250SelfTest(float * destination); void pickupAccel(float accel[3]); void pickupGyro(float gyro[3]); void pickupMag(float mag[3]); float pickupTemp(void); void displayAccel(void); void displayGyro(void); void displayMag(void); void displayAngle(void); void displayQuaternion(void); void displayTemperature(void); void setMagBias(float bias_x, float bias_y, float bias_z); private: //add I2C *i2c_p; I2C &i2c; RawSerial* pc_p; Timer timer; uint8_t Ascale; // AFS_2G, AFS_4G, AFS_8G, AFS_16G uint8_t Gscale; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS uint8_t Mscale; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution uint8_t Mmode; // Either 8 Hz 0x02) or 100 Hz (0x06) magnetometer data ODR float aRes, gRes, mRes; // scale resolutions per LSB for the sensors int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output float magCalibration[3], magbias[3]; // Factory mag calibration and mag bias float gyroBias[3], accelBias[3]; // Bias corrections for gyro and accelerometer float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius float temperature; float SelfTest[6]; // parameters for 6 DoF sensor fusion calculations float GyroMeasError; // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3 float beta; // compute beta float GyroMeasDrift; // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) float zeta ; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value float pitch, yaw, roll; float deltat; // integration interval for both filter schemes int lastUpdate, firstUpdate, Now; // used to calculate integration interval // used to calculate integration interval float q[4]; // vector to hold quaternion float eInt[3]; // vector to hold integral error for Mahony method float lpmag[3]; void writeByte(uint8_t address, uint8_t subAddress, uint8_t data); char readByte(uint8_t address, uint8_t subAddress); void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest); void initializeValue(void); void initMPU9250(void); void initAK8963(float * destination); void resetMPU9250(void); void calibrateMPU9250(float * dest1, float * dest2); void getMres(void); void getGres(void); void getAres(void); void readAccelData(int16_t * destination); void readGyroData(int16_t * destination); void readMagData(int16_t * destination); int16_t readTempData(void); uint8_t Whoami_MPU9250(void); uint8_t Whoami_AK8963(void); void sensingAccel(void); void sensingGyro(void); void sensingMag(void); void sensingTemp(void); void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); void translateQuaternionToDeg(float quaternion[4]); void calibrateDegree(void); void transformCoordinateFromCompassToMPU(); protected: }; #endif