usb実装中
Dependencies: mbed MPU6050_2 HMC5883L_2 SDFileSystem3
Diff: MPU9250/MPU9250.h
- Revision:
- 6:166746820555
- Parent:
- 5:8bfe95431ec0
- Child:
- 7:8989a4b84695
--- a/MPU9250/MPU9250.h Fri Feb 08 09:11:27 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,159 +0,0 @@ -#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 \ No newline at end of file