航空研究会 / Mbed 2 deprecated hassi_10_usb

usb実装中

Dependencies:   mbed MPU6050_2 HMC5883L_2 SDFileSystem3

MPU9250/MPU9250.h@3:c18342e4fddd, 2019-02-06 (annotated)

Committer:
Skykon
Date:
Wed Feb 06 11:49:05 2019 +0000
Revision:
3:c18342e4fddd
a

Who changed what in which revision?

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