フェイルセーフ完成版
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_53 by
Diff: MPU9250/MPU9250.h
- Revision:
- 0:17f575135219
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU9250/MPU9250.h Fri Sep 07 04:11:48 2018 +0000 @@ -0,0 +1,159 @@ +#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