航空研究会 / Mbed 2 deprecated sbus_test_2018

a

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of AutoFlight2018_Control by 航空研究会

MPU9250/MPU9250.h@1:09a162f4f6ce, 2018-09-07 (annotated)

Committer:
TUATBM
Date:
Fri Sep 07 03:42:49 2018 +0000
Revision:
1:09a162f4f6ce
Parent:
0:813f5cd20cf1 
a

Who changed what in which revision?

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