![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
着陸スロットル0追加
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_21 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?
User | Revision | Line number | New 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 |