航空研究会 / Mbed 2 deprecated AutoFlight2017_Approach

not include takeoff

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of AutoFlight2017_now2 by 航空研究会

MPU9250/MPU9250.h@1:f31e17341659, 2018-08-28 (annotated)

Committer:
TUATBM
Date:
Tue Aug 28 07:12:16 2018 +0000
Revision:
1:f31e17341659
Parent:
0:92024886c0be 
aaa

Who changed what in which revision?

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