航空研究会 / Mbed 2 deprecated Autoflight2018_9

21:34

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of Autoflight2018_8 by 航空研究会

MPU9250/MPU9250.h@0:17f575135219, 2018-09-07 (annotated)

Committer:
HARUKIDELTA
Date:
Fri Sep 07 04:11:48 2018 +0000
Revision:
0:17f575135219
SBUS_READ

Who changed what in which revision?

UserRevisionLine numberNew contents of line
HARUKIDELTA 0:17f575135219 1 #ifndef MPU9250_H
HARUKIDELTA 0:17f575135219 2 #define MPU9250_H
HARUKIDELTA 0:17f575135219 3
HARUKIDELTA 0:17f575135219 4 #include "mbed.h"
HARUKIDELTA 0:17f575135219 5 #include "math.h"
HARUKIDELTA 0:17f575135219 6 #include "MPU9250_register.h"
HARUKIDELTA 0:17f575135219 7
HARUKIDELTA 0:17f575135219 8
HARUKIDELTA 0:17f575135219 9 #define AK8963_ADDRESS 0x0C<<1
HARUKIDELTA 0:17f575135219 10
HARUKIDELTA 0:17f575135219 11 // Using the MSENSR-9250 breakout board, ADO is set to 0
HARUKIDELTA 0:17f575135219 12 // Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
HARUKIDELTA 0:17f575135219 13 //mbed uses the eight-bit device address, so shift seven-bit addresses left by one!
HARUKIDELTA 0:17f575135219 14 #define ADO 0
HARUKIDELTA 0:17f575135219 15 #if ADO
HARUKIDELTA 0:17f575135219 16 #define MPU9250_ADDRESS 0x69<<1 // Device address when ADO = 1
HARUKIDELTA 0:17f575135219 17 #else
HARUKIDELTA 0:17f575135219 18 #define MPU9250_ADDRESS 0x68<<1 // Device address when ADO = 0
HARUKIDELTA 0:17f575135219 19 #endif
HARUKIDELTA 0:17f575135219 20
HARUKIDELTA 0:17f575135219 21 #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
HARUKIDELTA 0:17f575135219 22 #define Ki 0.0f
HARUKIDELTA 0:17f575135219 23
HARUKIDELTA 0:17f575135219 24 #define IAM_MPU9250 0x71
HARUKIDELTA 0:17f575135219 25 #define IAM_AK8963 0x48
HARUKIDELTA 0:17f575135219 26
HARUKIDELTA 0:17f575135219 27 #define DECLINATION -7.45f //Declination at Tokyo is -7.45 degree 2017/06/14
HARUKIDELTA 0:17f575135219 28
HARUKIDELTA 0:17f575135219 29 const float PI = 3.14159265358979323846f;
HARUKIDELTA 0:17f575135219 30 const float LPGAIN_MAG = 0.0;
HARUKIDELTA 0:17f575135219 31
HARUKIDELTA 0:17f575135219 32
HARUKIDELTA 0:17f575135219 33 enum Ascale {
HARUKIDELTA 0:17f575135219 34 AFS_2G = 0,
HARUKIDELTA 0:17f575135219 35 AFS_4G,
HARUKIDELTA 0:17f575135219 36 AFS_8G,
HARUKIDELTA 0:17f575135219 37 AFS_16G
HARUKIDELTA 0:17f575135219 38 };
HARUKIDELTA 0:17f575135219 39
HARUKIDELTA 0:17f575135219 40 enum Gscale {
HARUKIDELTA 0:17f575135219 41 GFS_250DPS = 0,
HARUKIDELTA 0:17f575135219 42 GFS_500DPS,
HARUKIDELTA 0:17f575135219 43 GFS_1000DPS,
HARUKIDELTA 0:17f575135219 44 GFS_2000DPS
HARUKIDELTA 0:17f575135219 45 };
HARUKIDELTA 0:17f575135219 46
HARUKIDELTA 0:17f575135219 47 enum Mscale {
HARUKIDELTA 0:17f575135219 48 MFS_14BITS = 0, // 0.6 mG per LSB
HARUKIDELTA 0:17f575135219 49 MFS_16BITS // 0.15 mG per LSB
HARUKIDELTA 0:17f575135219 50 };
HARUKIDELTA 0:17f575135219 51
HARUKIDELTA 0:17f575135219 52
HARUKIDELTA 0:17f575135219 53
HARUKIDELTA 0:17f575135219 54 class MPU9250 {
HARUKIDELTA 0:17f575135219 55
HARUKIDELTA 0:17f575135219 56 public:
HARUKIDELTA 0:17f575135219 57 //MPU9250();
HARUKIDELTA 0:17f575135219 58 MPU9250(PinName sda, PinName scl, RawSerial* serial_p);
HARUKIDELTA 0:17f575135219 59 ~MPU9250();
HARUKIDELTA 0:17f575135219 60
HARUKIDELTA 0:17f575135219 61 bool Initialize(void);
HARUKIDELTA 0:17f575135219 62 bool sensingAcGyMg(void);
HARUKIDELTA 0:17f575135219 63 void calculatePostureAngle(float degree[3]);
HARUKIDELTA 0:17f575135219 64 float calculateYawByMg(void);
HARUKIDELTA 0:17f575135219 65
HARUKIDELTA 0:17f575135219 66 void MPU9250SelfTest(float * destination);
HARUKIDELTA 0:17f575135219 67
HARUKIDELTA 0:17f575135219 68 void pickupAccel(float accel[3]);
HARUKIDELTA 0:17f575135219 69 void pickupGyro(float gyro[3]);
HARUKIDELTA 0:17f575135219 70 void pickupMag(float mag[3]);
HARUKIDELTA 0:17f575135219 71 float pickupTemp(void);
HARUKIDELTA 0:17f575135219 72
HARUKIDELTA 0:17f575135219 73 void displayAccel(void);
HARUKIDELTA 0:17f575135219 74 void displayGyro(void);
HARUKIDELTA 0:17f575135219 75 void displayMag(void);
HARUKIDELTA 0:17f575135219 76 void displayAngle(void);
HARUKIDELTA 0:17f575135219 77 void displayQuaternion(void);
HARUKIDELTA 0:17f575135219 78 void displayTemperature(void);
HARUKIDELTA 0:17f575135219 79
HARUKIDELTA 0:17f575135219 80 void setMagBias(float bias_x, float bias_y, float bias_z);
HARUKIDELTA 0:17f575135219 81
HARUKIDELTA 0:17f575135219 82 private:
HARUKIDELTA 0:17f575135219 83 //add
HARUKIDELTA 0:17f575135219 84 I2C *i2c_p;
HARUKIDELTA 0:17f575135219 85 I2C &i2c;
HARUKIDELTA 0:17f575135219 86
HARUKIDELTA 0:17f575135219 87 RawSerial* pc_p;
HARUKIDELTA 0:17f575135219 88
HARUKIDELTA 0:17f575135219 89 Timer timer;
HARUKIDELTA 0:17f575135219 90
HARUKIDELTA 0:17f575135219 91 uint8_t Ascale; // AFS_2G, AFS_4G, AFS_8G, AFS_16G
HARUKIDELTA 0:17f575135219 92 uint8_t Gscale; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS
HARUKIDELTA 0:17f575135219 93 uint8_t Mscale; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution
HARUKIDELTA 0:17f575135219 94 uint8_t Mmode; // Either 8 Hz 0x02) or 100 Hz (0x06) magnetometer data ODR
HARUKIDELTA 0:17f575135219 95 float aRes, gRes, mRes; // scale resolutions per LSB for the sensors
HARUKIDELTA 0:17f575135219 96
HARUKIDELTA 0:17f575135219 97 int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output
HARUKIDELTA 0:17f575135219 98 int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output
HARUKIDELTA 0:17f575135219 99 int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output
HARUKIDELTA 0:17f575135219 100 float magCalibration[3], magbias[3]; // Factory mag calibration and mag bias
HARUKIDELTA 0:17f575135219 101 float gyroBias[3], accelBias[3]; // Bias corrections for gyro and accelerometer
HARUKIDELTA 0:17f575135219 102 float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values
HARUKIDELTA 0:17f575135219 103 int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius
HARUKIDELTA 0:17f575135219 104 float temperature;
HARUKIDELTA 0:17f575135219 105 float SelfTest[6];
HARUKIDELTA 0:17f575135219 106
HARUKIDELTA 0:17f575135219 107 // parameters for 6 DoF sensor fusion calculations
HARUKIDELTA 0:17f575135219 108 float GyroMeasError; // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
HARUKIDELTA 0:17f575135219 109 float beta; // compute beta
HARUKIDELTA 0:17f575135219 110 float GyroMeasDrift; // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
HARUKIDELTA 0:17f575135219 111 float zeta ; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
HARUKIDELTA 0:17f575135219 112
HARUKIDELTA 0:17f575135219 113 float pitch, yaw, roll;
HARUKIDELTA 0:17f575135219 114 float deltat; // integration interval for both filter schemes
HARUKIDELTA 0:17f575135219 115 int lastUpdate, firstUpdate, Now; // used to calculate integration interval // used to calculate integration interval
HARUKIDELTA 0:17f575135219 116 float q[4]; // vector to hold quaternion
HARUKIDELTA 0:17f575135219 117 float eInt[3]; // vector to hold integral error for Mahony method
HARUKIDELTA 0:17f575135219 118
HARUKIDELTA 0:17f575135219 119 float lpmag[3];
HARUKIDELTA 0:17f575135219 120
HARUKIDELTA 0:17f575135219 121 void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
HARUKIDELTA 0:17f575135219 122 char readByte(uint8_t address, uint8_t subAddress);
HARUKIDELTA 0:17f575135219 123 void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
HARUKIDELTA 0:17f575135219 124
HARUKIDELTA 0:17f575135219 125 void initializeValue(void);
HARUKIDELTA 0:17f575135219 126
HARUKIDELTA 0:17f575135219 127 void initMPU9250(void);
HARUKIDELTA 0:17f575135219 128 void initAK8963(float * destination);
HARUKIDELTA 0:17f575135219 129 void resetMPU9250(void);
HARUKIDELTA 0:17f575135219 130 void calibrateMPU9250(float * dest1, float * dest2);
HARUKIDELTA 0:17f575135219 131
HARUKIDELTA 0:17f575135219 132 void getMres(void);
HARUKIDELTA 0:17f575135219 133 void getGres(void);
HARUKIDELTA 0:17f575135219 134 void getAres(void);
HARUKIDELTA 0:17f575135219 135
HARUKIDELTA 0:17f575135219 136 void readAccelData(int16_t * destination);
HARUKIDELTA 0:17f575135219 137 void readGyroData(int16_t * destination);
HARUKIDELTA 0:17f575135219 138 void readMagData(int16_t * destination);
HARUKIDELTA 0:17f575135219 139 int16_t readTempData(void);
HARUKIDELTA 0:17f575135219 140
HARUKIDELTA 0:17f575135219 141 uint8_t Whoami_MPU9250(void);
HARUKIDELTA 0:17f575135219 142 uint8_t Whoami_AK8963(void);
HARUKIDELTA 0:17f575135219 143
HARUKIDELTA 0:17f575135219 144 void sensingAccel(void);
HARUKIDELTA 0:17f575135219 145 void sensingGyro(void);
HARUKIDELTA 0:17f575135219 146 void sensingMag(void);
HARUKIDELTA 0:17f575135219 147 void sensingTemp(void);
HARUKIDELTA 0:17f575135219 148
HARUKIDELTA 0:17f575135219 149 void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
HARUKIDELTA 0:17f575135219 150 void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
HARUKIDELTA 0:17f575135219 151 void translateQuaternionToDeg(float quaternion[4]);
HARUKIDELTA 0:17f575135219 152 void calibrateDegree(void);
HARUKIDELTA 0:17f575135219 153
HARUKIDELTA 0:17f575135219 154 void transformCoordinateFromCompassToMPU();
HARUKIDELTA 0:17f575135219 155 protected:
HARUKIDELTA 0:17f575135219 156
HARUKIDELTA 0:17f575135219 157 };
HARUKIDELTA 0:17f575135219 158
HARUKIDELTA 0:17f575135219 159 #endif