CansatB_2021 / Mbed 2 deprecated cansat

地磁気センサーのプログラム

Dependencies:   mbed

gm_sensor/MPU9250.h@0:af969b92a87c, 2021-10-20 (annotated)

Committer:
minanao
Date:
Wed Oct 20 08:15:44 2021 +0000
Revision:
0:af969b92a87c
gm_sensor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
minanao 0:af969b92a87c 1 #include "mbed.h"
minanao 0:af969b92a87c 2 #include "math.h"
minanao 0:af969b92a87c 3
minanao 0:af969b92a87c 4 // See also MPU-9250 Register Map and Descriptions, Revision 4.0, RM-MPU-9250A-00, Rev. 1.4, 9/9/2013 for registers not listed in
minanao 0:af969b92a87c 5 // above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map
minanao 0:af969b92a87c 6 //
minanao 0:af969b92a87c 7 //Magnetometer Registers
minanao 0:af969b92a87c 8 #define AK8963_ADDRESS 0x0C<<1
minanao 0:af969b92a87c 9 #define WHO_AM_I_AK8963 0x00 // should return 0x48
minanao 0:af969b92a87c 10 #define INFO 0x01
minanao 0:af969b92a87c 11 #define AK8963_ST1 0x02 // data ready status bit 0
minanao 0:af969b92a87c 12 #define AK8963_XOUT_L 0x03 // data
minanao 0:af969b92a87c 13 #define AK8963_XOUT_H 0x04
minanao 0:af969b92a87c 14 #define AK8963_YOUT_L 0x05
minanao 0:af969b92a87c 15 #define AK8963_YOUT_H 0x06
minanao 0:af969b92a87c 16 #define AK8963_ZOUT_L 0x07
minanao 0:af969b92a87c 17 #define AK8963_ZOUT_H 0x08
minanao 0:af969b92a87c 18 #define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2
minanao 0:af969b92a87c 19 #define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
minanao 0:af969b92a87c 20 #define AK8963_ASTC 0x0C // Self test control
minanao 0:af969b92a87c 21 #define AK8963_I2CDIS 0x0F // I2C disable
minanao 0:af969b92a87c 22 #define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
minanao 0:af969b92a87c 23 #define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
minanao 0:af969b92a87c 24 #define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
minanao 0:af969b92a87c 25
minanao 0:af969b92a87c 26 #define SELF_TEST_X_GYRO 0x00
minanao 0:af969b92a87c 27 #define SELF_TEST_Y_GYRO 0x01
minanao 0:af969b92a87c 28 #define SELF_TEST_Z_GYRO 0x02
minanao 0:af969b92a87c 29
minanao 0:af969b92a87c 30 /*#define X_FINE_GAIN 0x03 // [7:0] fine gain
minanao 0:af969b92a87c 31 #define Y_FINE_GAIN 0x04
minanao 0:af969b92a87c 32 #define Z_FINE_GAIN 0x05
minanao 0:af969b92a87c 33 #define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer
minanao 0:af969b92a87c 34 #define XA_OFFSET_L_TC 0x07
minanao 0:af969b92a87c 35 #define YA_OFFSET_H 0x08
minanao 0:af969b92a87c 36 #define YA_OFFSET_L_TC 0x09
minanao 0:af969b92a87c 37 #define ZA_OFFSET_H 0x0A
minanao 0:af969b92a87c 38 #define ZA_OFFSET_L_TC 0x0B */
minanao 0:af969b92a87c 39
minanao 0:af969b92a87c 40 #define SELF_TEST_X_ACCEL 0x0D
minanao 0:af969b92a87c 41 #define SELF_TEST_Y_ACCEL 0x0E
minanao 0:af969b92a87c 42 #define SELF_TEST_Z_ACCEL 0x0F
minanao 0:af969b92a87c 43
minanao 0:af969b92a87c 44 #define SELF_TEST_A 0x10
minanao 0:af969b92a87c 45
minanao 0:af969b92a87c 46 #define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope
minanao 0:af969b92a87c 47 #define XG_OFFSET_L 0x14
minanao 0:af969b92a87c 48 #define YG_OFFSET_H 0x15
minanao 0:af969b92a87c 49 #define YG_OFFSET_L 0x16
minanao 0:af969b92a87c 50 #define ZG_OFFSET_H 0x17
minanao 0:af969b92a87c 51 #define ZG_OFFSET_L 0x18
minanao 0:af969b92a87c 52 #define SMPLRT_DIV 0x19
minanao 0:af969b92a87c 53 #define CONFIG 0x1A
minanao 0:af969b92a87c 54 #define GYRO_CONFIG 0x1B
minanao 0:af969b92a87c 55 #define ACCEL_CONFIG 0x1C
minanao 0:af969b92a87c 56 #define ACCEL_CONFIG2 0x1D
minanao 0:af969b92a87c 57 #define LP_ACCEL_ODR 0x1E
minanao 0:af969b92a87c 58 #define WOM_THR 0x1F
minanao 0:af969b92a87c 59
minanao 0:af969b92a87c 60 #define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
minanao 0:af969b92a87c 61 #define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0]
minanao 0:af969b92a87c 62 #define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
minanao 0:af969b92a87c 63
minanao 0:af969b92a87c 64 #define FIFO_EN 0x23
minanao 0:af969b92a87c 65 #define I2C_MST_CTRL 0x24
minanao 0:af969b92a87c 66 #define I2C_SLV0_ADDR 0x25
minanao 0:af969b92a87c 67 #define I2C_SLV0_REG 0x26
minanao 0:af969b92a87c 68 #define I2C_SLV0_CTRL 0x27
minanao 0:af969b92a87c 69 #define I2C_SLV1_ADDR 0x28
minanao 0:af969b92a87c 70 #define I2C_SLV1_REG 0x29
minanao 0:af969b92a87c 71 #define I2C_SLV1_CTRL 0x2A
minanao 0:af969b92a87c 72 #define I2C_SLV2_ADDR 0x2B
minanao 0:af969b92a87c 73 #define I2C_SLV2_REG 0x2C
minanao 0:af969b92a87c 74 #define I2C_SLV2_CTRL 0x2D
minanao 0:af969b92a87c 75 #define I2C_SLV3_ADDR 0x2E
minanao 0:af969b92a87c 76 #define I2C_SLV3_REG 0x2F
minanao 0:af969b92a87c 77 #define I2C_SLV3_CTRL 0x30
minanao 0:af969b92a87c 78 #define I2C_SLV4_ADDR 0x31
minanao 0:af969b92a87c 79 #define I2C_SLV4_REG 0x32
minanao 0:af969b92a87c 80 #define I2C_SLV4_DO 0x33
minanao 0:af969b92a87c 81 #define I2C_SLV4_CTRL 0x34
minanao 0:af969b92a87c 82 #define I2C_SLV4_DI 0x35
minanao 0:af969b92a87c 83 #define I2C_MST_STATUS 0x36
minanao 0:af969b92a87c 84 #define INT_PIN_CFG 0x37
minanao 0:af969b92a87c 85 #define INT_ENABLE 0x38
minanao 0:af969b92a87c 86 #define DMP_INT_STATUS 0x39 // Check DMP interrupt
minanao 0:af969b92a87c 87 #define INT_STATUS 0x3A
minanao 0:af969b92a87c 88 #define ACCEL_XOUT_H 0x3B
minanao 0:af969b92a87c 89 #define ACCEL_XOUT_L 0x3C
minanao 0:af969b92a87c 90 #define ACCEL_YOUT_H 0x3D
minanao 0:af969b92a87c 91 #define ACCEL_YOUT_L 0x3E
minanao 0:af969b92a87c 92 #define ACCEL_ZOUT_H 0x3F
minanao 0:af969b92a87c 93 #define ACCEL_ZOUT_L 0x40
minanao 0:af969b92a87c 94 #define TEMP_OUT_H 0x41
minanao 0:af969b92a87c 95 #define TEMP_OUT_L 0x42
minanao 0:af969b92a87c 96 #define GYRO_XOUT_H 0x43
minanao 0:af969b92a87c 97 #define GYRO_XOUT_L 0x44
minanao 0:af969b92a87c 98 #define GYRO_YOUT_H 0x45
minanao 0:af969b92a87c 99 #define GYRO_YOUT_L 0x46
minanao 0:af969b92a87c 100 #define GYRO_ZOUT_H 0x47
minanao 0:af969b92a87c 101 #define GYRO_ZOUT_L 0x48
minanao 0:af969b92a87c 102 #define EXT_SENS_DATA_00 0x49
minanao 0:af969b92a87c 103 #define EXT_SENS_DATA_01 0x4A
minanao 0:af969b92a87c 104 #define EXT_SENS_DATA_02 0x4B
minanao 0:af969b92a87c 105 #define EXT_SENS_DATA_03 0x4C
minanao 0:af969b92a87c 106 #define EXT_SENS_DATA_04 0x4D
minanao 0:af969b92a87c 107 #define EXT_SENS_DATA_05 0x4E
minanao 0:af969b92a87c 108 #define EXT_SENS_DATA_06 0x4F
minanao 0:af969b92a87c 109 #define EXT_SENS_DATA_07 0x50
minanao 0:af969b92a87c 110 #define EXT_SENS_DATA_08 0x51
minanao 0:af969b92a87c 111 #define EXT_SENS_DATA_09 0x52
minanao 0:af969b92a87c 112 #define EXT_SENS_DATA_10 0x53
minanao 0:af969b92a87c 113 #define EXT_SENS_DATA_11 0x54
minanao 0:af969b92a87c 114 #define EXT_SENS_DATA_12 0x55
minanao 0:af969b92a87c 115 #define EXT_SENS_DATA_13 0x56
minanao 0:af969b92a87c 116 #define EXT_SENS_DATA_14 0x57
minanao 0:af969b92a87c 117 #define EXT_SENS_DATA_15 0x58
minanao 0:af969b92a87c 118 #define EXT_SENS_DATA_16 0x59
minanao 0:af969b92a87c 119 #define EXT_SENS_DATA_17 0x5A
minanao 0:af969b92a87c 120 #define EXT_SENS_DATA_18 0x5B
minanao 0:af969b92a87c 121 #define EXT_SENS_DATA_19 0x5C
minanao 0:af969b92a87c 122 #define EXT_SENS_DATA_20 0x5D
minanao 0:af969b92a87c 123 #define EXT_SENS_DATA_21 0x5E
minanao 0:af969b92a87c 124 #define EXT_SENS_DATA_22 0x5F
minanao 0:af969b92a87c 125 #define EXT_SENS_DATA_23 0x60
minanao 0:af969b92a87c 126 #define MOT_DETECT_STATUS 0x61
minanao 0:af969b92a87c 127 #define I2C_SLV0_DO 0x63
minanao 0:af969b92a87c 128 #define I2C_SLV1_DO 0x64
minanao 0:af969b92a87c 129 #define I2C_SLV2_DO 0x65
minanao 0:af969b92a87c 130 #define I2C_SLV3_DO 0x66
minanao 0:af969b92a87c 131 #define I2C_MST_DELAY_CTRL 0x67
minanao 0:af969b92a87c 132 #define SIGNAL_PATH_RESET 0x68
minanao 0:af969b92a87c 133 #define MOT_DETECT_CTRL 0x69
minanao 0:af969b92a87c 134 #define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP
minanao 0:af969b92a87c 135 #define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode
minanao 0:af969b92a87c 136 #define PWR_MGMT_2 0x6C
minanao 0:af969b92a87c 137 #define DMP_BANK 0x6D // Activates a specific bank in the DMP
minanao 0:af969b92a87c 138 #define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank
minanao 0:af969b92a87c 139 #define DMP_REG 0x6F // Register in DMP from which to read or to which to write
minanao 0:af969b92a87c 140 #define DMP_REG_1 0x70
minanao 0:af969b92a87c 141 #define DMP_REG_2 0x71
minanao 0:af969b92a87c 142 #define FIFO_COUNTH 0x72
minanao 0:af969b92a87c 143 #define FIFO_COUNTL 0x73
minanao 0:af969b92a87c 144 #define FIFO_R_W 0x74
minanao 0:af969b92a87c 145 #define WHO_AM_I_MPU9250 0x75 // Should return 0x71
minanao 0:af969b92a87c 146 #define XA_OFFSET_H 0x77
minanao 0:af969b92a87c 147 #define XA_OFFSET_L 0x78
minanao 0:af969b92a87c 148 #define YA_OFFSET_H 0x7A
minanao 0:af969b92a87c 149 #define YA_OFFSET_L 0x7B
minanao 0:af969b92a87c 150 #define ZA_OFFSET_H 0x7D
minanao 0:af969b92a87c 151 #define ZA_OFFSET_L 0x7E
minanao 0:af969b92a87c 152
minanao 0:af969b92a87c 153 #define PI 3.14159265358979323846f
minanao 0:af969b92a87c 154 // Using the MSENSR-9250 breakout board, ADO is set to 0
minanao 0:af969b92a87c 155 // Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
minanao 0:af969b92a87c 156 //mbed uses the eight-bit device address, so shift seven-bit addresses left by one!
minanao 0:af969b92a87c 157 #define ADO 0
minanao 0:af969b92a87c 158 #if ADO
minanao 0:af969b92a87c 159 #define MPU9250_ADDRESS 0x69<<1 // Device address when ADO = 1
minanao 0:af969b92a87c 160 #else
minanao 0:af969b92a87c 161 #define MPU9250_ADDRESS 0x68<<1 // Device address when ADO = 0
minanao 0:af969b92a87c 162 #endif
minanao 0:af969b92a87c 163
minanao 0:af969b92a87c 164 // Set initial input parameters
minanao 0:af969b92a87c 165 enum Ascale {
minanao 0:af969b92a87c 166 AFS_2G = 0,
minanao 0:af969b92a87c 167 AFS_4G,
minanao 0:af969b92a87c 168 AFS_8G,
minanao 0:af969b92a87c 169 AFS_16G
minanao 0:af969b92a87c 170 };
minanao 0:af969b92a87c 171
minanao 0:af969b92a87c 172 enum Gscale {
minanao 0:af969b92a87c 173 GFS_250DPS = 0,
minanao 0:af969b92a87c 174 GFS_500DPS,
minanao 0:af969b92a87c 175 GFS_1000DPS,
minanao 0:af969b92a87c 176 GFS_2000DPS
minanao 0:af969b92a87c 177 };
minanao 0:af969b92a87c 178
minanao 0:af969b92a87c 179 enum Mscale {
minanao 0:af969b92a87c 180 MFS_14BITS = 0, // 0.6 mG per LSB
minanao 0:af969b92a87c 181 MFS_16BITS // 0.15 mG per LSB
minanao 0:af969b92a87c 182 };
minanao 0:af969b92a87c 183
minanao 0:af969b92a87c 184 class MPU9250 {
minanao 0:af969b92a87c 185
minanao 0:af969b92a87c 186 protected:
minanao 0:af969b92a87c 187
minanao 0:af969b92a87c 188 public:
minanao 0:af969b92a87c 189
minanao 0:af969b92a87c 190 MPU9250(PinName sda, PinName scl);
minanao 0:af969b92a87c 191 MPU9250(I2C *i2c);
minanao 0:af969b92a87c 192 ~MPU9250();
minanao 0:af969b92a87c 193
minanao 0:af969b92a87c 194 void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
minanao 0:af969b92a87c 195 char readByte(uint8_t address, uint8_t subAddress);
minanao 0:af969b92a87c 196 void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
minanao 0:af969b92a87c 197 float getMres();
minanao 0:af969b92a87c 198 float getGres();
minanao 0:af969b92a87c 199 float getAres();
minanao 0:af969b92a87c 200 void readAccelData(int16_t * destination);
minanao 0:af969b92a87c 201 void readGyroData(int16_t * destination);
minanao 0:af969b92a87c 202 void readMagData(int16_t * destination);
minanao 0:af969b92a87c 203 int16_t readTempData();
minanao 0:af969b92a87c 204 void resetMPU9250();
minanao 0:af969b92a87c 205 void initAK8963(float * destination);
minanao 0:af969b92a87c 206 void initMPU9250();
minanao 0:af969b92a87c 207 void calibrateMPU9250(float * dest1, float * dest2);
minanao 0:af969b92a87c 208 void MPU9250SelfTest(float * destination);
minanao 0:af969b92a87c 209 void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
minanao 0:af969b92a87c 210 void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
minanao 0:af969b92a87c 211 void setWakeOnMotion();
minanao 0:af969b92a87c 212
minanao 0:af969b92a87c 213 float SelfTest[6];
minanao 0:af969b92a87c 214 float gyroBias[3],accelBias[3]; // Bias corrections for gyro and accelerometer
minanao 0:af969b92a87c 215 float magCalibration[3], magbias[3]; // Factory mag calibration and mag bias
minanao 0:af969b92a87c 216
minanao 0:af969b92a87c 217 uint8_t Ascale; // AFS_2G, AFS_4G, AFS_8G, AFS_16G
minanao 0:af969b92a87c 218 uint8_t Gscale; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS
minanao 0:af969b92a87c 219 uint8_t Mscale; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution
minanao 0:af969b92a87c 220 uint8_t Mmode; // Either 8 Hz 0x02) or 100 Hz (0x06) magnetometer data ODR
minanao 0:af969b92a87c 221 float aRes, gRes, mRes; // scale resolutions per LSB for the sensors
minanao 0:af969b92a87c 222
minanao 0:af969b92a87c 223 int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output
minanao 0:af969b92a87c 224 int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output
minanao 0:af969b92a87c 225 int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output
minanao 0:af969b92a87c 226 float q[4]; // vector to hold quaternion
minanao 0:af969b92a87c 227
minanao 0:af969b92a87c 228 float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values
minanao 0:af969b92a87c 229 float pitch, yaw, roll;
minanao 0:af969b92a87c 230 float deltat; // integration interval for both filter schemes
minanao 0:af969b92a87c 231 int lastUpdate, firstUpdate, Now; // used to calculate integration interval
minanao 0:af969b92a87c 232 int delt_t; // used to control display output rate
minanao 0:af969b92a87c 233 int count; // used to control display output rate
minanao 0:af969b92a87c 234 int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius
minanao 0:af969b92a87c 235 float temperature;
minanao 0:af969b92a87c 236 private:
minanao 0:af969b92a87c 237 I2C *i2c_;
minanao 0:af969b92a87c 238 };