logger
logger.h@0:c3e3dc7923c8, 2019-12-09 (annotated)
- Committer:
- takuto003
- Date:
- Mon Dec 09 15:37:51 2019 +0000
- Revision:
- 0:c3e3dc7923c8
Logger apd
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
takuto003 | 0:c3e3dc7923c8 | 1 | #include "mbed.h" |
takuto003 | 0:c3e3dc7923c8 | 2 | #include "math.h" |
takuto003 | 0:c3e3dc7923c8 | 3 | |
takuto003 | 0:c3e3dc7923c8 | 4 | #define BMP280_SLAVE_ADDRESS (0x76 << 1) |
takuto003 | 0:c3e3dc7923c8 | 5 | #define AK8963_ADDRESS 0x0C << 1 |
takuto003 | 0:c3e3dc7923c8 | 6 | #define WHO_AM_I_AK8963 0x00 |
takuto003 | 0:c3e3dc7923c8 | 7 | #define INFO 0x01 |
takuto003 | 0:c3e3dc7923c8 | 8 | #define AK8963_ST1 0x02 |
takuto003 | 0:c3e3dc7923c8 | 9 | #define AK8963_XOUT_L 0x03 |
takuto003 | 0:c3e3dc7923c8 | 10 | #define AK8963_XOUT_H 0x04 |
takuto003 | 0:c3e3dc7923c8 | 11 | #define AK8963_YOUT_L 0x05 |
takuto003 | 0:c3e3dc7923c8 | 12 | #define AK8963_YOUT_H 0x06 |
takuto003 | 0:c3e3dc7923c8 | 13 | #define AK8963_ZOUT_L 0x07 |
takuto003 | 0:c3e3dc7923c8 | 14 | #define AK8963_ZOUT_H 0x08 |
takuto003 | 0:c3e3dc7923c8 | 15 | #define AK8963_ST2 0x09 |
takuto003 | 0:c3e3dc7923c8 | 16 | #define AK8963_CNTL 0x0A |
takuto003 | 0:c3e3dc7923c8 | 17 | #define AK8963_ASTC 0x0C |
takuto003 | 0:c3e3dc7923c8 | 18 | #define AK8963_I2CDIS 0x0F |
takuto003 | 0:c3e3dc7923c8 | 19 | #define AK8963_ASAX 0x10 |
takuto003 | 0:c3e3dc7923c8 | 20 | #define AK8963_ASAY 0x11 |
takuto003 | 0:c3e3dc7923c8 | 21 | #define AK8963_ASAZ 0x12 |
takuto003 | 0:c3e3dc7923c8 | 22 | #define XG_OFFSET_H 0x13 |
takuto003 | 0:c3e3dc7923c8 | 23 | #define XG_OFFSET_L 0x14 |
takuto003 | 0:c3e3dc7923c8 | 24 | #define YG_OFFSET_H 0x15 |
takuto003 | 0:c3e3dc7923c8 | 25 | #define YG_OFFSET_L 0x16 |
takuto003 | 0:c3e3dc7923c8 | 26 | #define ZG_OFFSET_H 0x17 |
takuto003 | 0:c3e3dc7923c8 | 27 | #define ZG_OFFSET_L 0x18 |
takuto003 | 0:c3e3dc7923c8 | 28 | #define SMPLRT_DIV 0x19 |
takuto003 | 0:c3e3dc7923c8 | 29 | #define CONFIG 0x1A |
takuto003 | 0:c3e3dc7923c8 | 30 | #define GYRO_CONFIG 0x1B |
takuto003 | 0:c3e3dc7923c8 | 31 | #define ACCEL_CONFIG 0x1C |
takuto003 | 0:c3e3dc7923c8 | 32 | #define ACCEL_CONFIG2 0x1D |
takuto003 | 0:c3e3dc7923c8 | 33 | #define LP_ACCEL_ODR 0x1E |
takuto003 | 0:c3e3dc7923c8 | 34 | #define WOM_THR 0x1F |
takuto003 | 0:c3e3dc7923c8 | 35 | #define FIFO_EN 0x23 |
takuto003 | 0:c3e3dc7923c8 | 36 | #define I2C_MST_CTRL 0x24 |
takuto003 | 0:c3e3dc7923c8 | 37 | #define I2C_MST_STATUS 0x36 |
takuto003 | 0:c3e3dc7923c8 | 38 | #define INT_PIN_CFG 0x37 |
takuto003 | 0:c3e3dc7923c8 | 39 | #define INT_ENABLE 0x38 |
takuto003 | 0:c3e3dc7923c8 | 40 | #define DMP_INT_STATUS 0x39 |
takuto003 | 0:c3e3dc7923c8 | 41 | #define INT_STATUS 0x3A |
takuto003 | 0:c3e3dc7923c8 | 42 | #define ACCEL_XOUT_H 0x3B |
takuto003 | 0:c3e3dc7923c8 | 43 | #define ACCEL_XOUT_L 0x3C |
takuto003 | 0:c3e3dc7923c8 | 44 | #define ACCEL_YOUT_H 0x3D |
takuto003 | 0:c3e3dc7923c8 | 45 | #define ACCEL_YOUT_L 0x3E |
takuto003 | 0:c3e3dc7923c8 | 46 | #define ACCEL_ZOUT_H 0x3F |
takuto003 | 0:c3e3dc7923c8 | 47 | #define ACCEL_ZOUT_L 0x40 |
takuto003 | 0:c3e3dc7923c8 | 48 | #define TEMP_OUT_H 0x41 |
takuto003 | 0:c3e3dc7923c8 | 49 | #define TEMP_OUT_L 0x42 |
takuto003 | 0:c3e3dc7923c8 | 50 | #define GYRO_XOUT_H 0x43 |
takuto003 | 0:c3e3dc7923c8 | 51 | #define GYRO_XOUT_L 0x44 |
takuto003 | 0:c3e3dc7923c8 | 52 | #define GYRO_YOUT_H 0x45 |
takuto003 | 0:c3e3dc7923c8 | 53 | #define GYRO_YOUT_L 0x46 |
takuto003 | 0:c3e3dc7923c8 | 54 | #define GYRO_ZOUT_H 0x47 |
takuto003 | 0:c3e3dc7923c8 | 55 | #define GYRO_ZOUT_L 0x48 |
takuto003 | 0:c3e3dc7923c8 | 56 | #define MOT_DETECT_STATUS 0x61 |
takuto003 | 0:c3e3dc7923c8 | 57 | #define I2C_MST_DELAY_CTRL 0x67 |
takuto003 | 0:c3e3dc7923c8 | 58 | #define SIGNAL_PATH_RESET 0x68 |
takuto003 | 0:c3e3dc7923c8 | 59 | #define MOT_DETECT_CTRL 0x69 |
takuto003 | 0:c3e3dc7923c8 | 60 | #define USER_CTRL 0x6A |
takuto003 | 0:c3e3dc7923c8 | 61 | #define PWR_MGMT_1 0x6B |
takuto003 | 0:c3e3dc7923c8 | 62 | #define PWR_MGMT_2 0x6C |
takuto003 | 0:c3e3dc7923c8 | 63 | #define FIFO_COUNTH 0x72 |
takuto003 | 0:c3e3dc7923c8 | 64 | #define FIFO_COUNTL 0x73 |
takuto003 | 0:c3e3dc7923c8 | 65 | #define FIFO_R_W 0x74 |
takuto003 | 0:c3e3dc7923c8 | 66 | #define WHO_AM_I_MPU9250 0x75 |
takuto003 | 0:c3e3dc7923c8 | 67 | #define XA_OFFSET_H 0x77 |
takuto003 | 0:c3e3dc7923c8 | 68 | #define XA_OFFSET_L 0x78 |
takuto003 | 0:c3e3dc7923c8 | 69 | #define YA_OFFSET_H 0x7A |
takuto003 | 0:c3e3dc7923c8 | 70 | #define YA_OFFSET_L 0x7B |
takuto003 | 0:c3e3dc7923c8 | 71 | #define ZA_OFFSET_H 0x7D |
takuto003 | 0:c3e3dc7923c8 | 72 | #define ZA_OFFSET_L 0x7E |
takuto003 | 0:c3e3dc7923c8 | 73 | #define P0 1015.0 |
takuto003 | 0:c3e3dc7923c8 | 74 | #define PI 3.14159265358979323846f |
takuto003 | 0:c3e3dc7923c8 | 75 | #define ADO 0 |
takuto003 | 0:c3e3dc7923c8 | 76 | #define MPU9250_ADDRESS 0x68 << 1 |
takuto003 | 0:c3e3dc7923c8 | 77 | #define I2C_SDA PB_7 |
takuto003 | 0:c3e3dc7923c8 | 78 | #define I2C_SCL PB_6 |
takuto003 | 0:c3e3dc7923c8 | 79 | I2C i2c(I2C_SDA, I2C_SCL); |
takuto003 | 0:c3e3dc7923c8 | 80 | |
takuto003 | 0:c3e3dc7923c8 | 81 | enum Ascale |
takuto003 | 0:c3e3dc7923c8 | 82 | { |
takuto003 | 0:c3e3dc7923c8 | 83 | AFS_2G = 0, |
takuto003 | 0:c3e3dc7923c8 | 84 | AFS_4G, |
takuto003 | 0:c3e3dc7923c8 | 85 | AFS_8G, |
takuto003 | 0:c3e3dc7923c8 | 86 | AFS_16G |
takuto003 | 0:c3e3dc7923c8 | 87 | }; |
takuto003 | 0:c3e3dc7923c8 | 88 | |
takuto003 | 0:c3e3dc7923c8 | 89 | enum Gscale |
takuto003 | 0:c3e3dc7923c8 | 90 | { |
takuto003 | 0:c3e3dc7923c8 | 91 | GFS_250DPS = 0, |
takuto003 | 0:c3e3dc7923c8 | 92 | GFS_500DPS, |
takuto003 | 0:c3e3dc7923c8 | 93 | GFS_1000DPS, |
takuto003 | 0:c3e3dc7923c8 | 94 | GFS_2000DPS |
takuto003 | 0:c3e3dc7923c8 | 95 | }; |
takuto003 | 0:c3e3dc7923c8 | 96 | |
takuto003 | 0:c3e3dc7923c8 | 97 | enum Mscale |
takuto003 | 0:c3e3dc7923c8 | 98 | { |
takuto003 | 0:c3e3dc7923c8 | 99 | MFS_14BITS = 0, |
takuto003 | 0:c3e3dc7923c8 | 100 | MFS_16BITS |
takuto003 | 0:c3e3dc7923c8 | 101 | }; |
takuto003 | 0:c3e3dc7923c8 | 102 | uint8_t Ascale; |
takuto003 | 0:c3e3dc7923c8 | 103 | uint8_t Gscale; |
takuto003 | 0:c3e3dc7923c8 | 104 | uint8_t Mscale; |
takuto003 | 0:c3e3dc7923c8 | 105 | uint8_t Mmode; |
takuto003 | 0:c3e3dc7923c8 | 106 | float aRes, gRes, mRes; |
takuto003 | 0:c3e3dc7923c8 | 107 | |
takuto003 | 0:c3e3dc7923c8 | 108 | int16_t accelCount[3]; |
takuto003 | 0:c3e3dc7923c8 | 109 | int16_t gyroCount[3]; |
takuto003 | 0:c3e3dc7923c8 | 110 | int16_t magCount[3]; |
takuto003 | 0:c3e3dc7923c8 | 111 | float q[4]; |
takuto003 | 0:c3e3dc7923c8 | 112 | |
takuto003 | 0:c3e3dc7923c8 | 113 | float ax, ay, az, gx, gy, gz, mx, my, mz; |
takuto003 | 0:c3e3dc7923c8 | 114 | float pitch, yaw, roll; |
takuto003 | 0:c3e3dc7923c8 | 115 | float deltat; |
takuto003 | 0:c3e3dc7923c8 | 116 | int lastUpdate, firstUpdate, Now; |
takuto003 | 0:c3e3dc7923c8 | 117 | int delt_t; |
takuto003 | 0:c3e3dc7923c8 | 118 | int count; |
takuto003 | 0:c3e3dc7923c8 | 119 | int16_t tempCount; |
takuto003 | 0:c3e3dc7923c8 | 120 | float temperature; |
takuto003 | 0:c3e3dc7923c8 | 121 | |
takuto003 | 0:c3e3dc7923c8 | 122 | class LOGGER |
takuto003 | 0:c3e3dc7923c8 | 123 | { |
takuto003 | 0:c3e3dc7923c8 | 124 | private: |
takuto003 | 0:c3e3dc7923c8 | 125 | char address; |
takuto003 | 0:c3e3dc7923c8 | 126 | uint16_t dig_T1; |
takuto003 | 0:c3e3dc7923c8 | 127 | int16_t dig_T2, dig_T3; |
takuto003 | 0:c3e3dc7923c8 | 128 | uint16_t dig_P1; |
takuto003 | 0:c3e3dc7923c8 | 129 | int16_t dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9; |
takuto003 | 0:c3e3dc7923c8 | 130 | int32_t t_fine; |
takuto003 | 0:c3e3dc7923c8 | 131 | |
takuto003 | 0:c3e3dc7923c8 | 132 | public: |
takuto003 | 0:c3e3dc7923c8 | 133 | float pressf; |
takuto003 | 0:c3e3dc7923c8 | 134 | float tempf; |
takuto003 | 0:c3e3dc7923c8 | 135 | |
takuto003 | 0:c3e3dc7923c8 | 136 | LOGGER(char slave_adr = BMP280_SLAVE_ADDRESS) : address(slave_adr), t_fine(0) |
takuto003 | 0:c3e3dc7923c8 | 137 | { |
takuto003 | 0:c3e3dc7923c8 | 138 | initialize(); |
takuto003 | 0:c3e3dc7923c8 | 139 | } |
takuto003 | 0:c3e3dc7923c8 | 140 | |
takuto003 | 0:c3e3dc7923c8 | 141 | void initialize() |
takuto003 | 0:c3e3dc7923c8 | 142 | { |
takuto003 | 0:c3e3dc7923c8 | 143 | char cmd[18]; |
takuto003 | 0:c3e3dc7923c8 | 144 | cmd[0] = 0xf2; |
takuto003 | 0:c3e3dc7923c8 | 145 | cmd[1] = 0x01; |
takuto003 | 0:c3e3dc7923c8 | 146 | i2c.write(address, cmd, 2); |
takuto003 | 0:c3e3dc7923c8 | 147 | cmd[0] = 0xf4; |
takuto003 | 0:c3e3dc7923c8 | 148 | cmd[1] = 0x27; |
takuto003 | 0:c3e3dc7923c8 | 149 | i2c.write(address, cmd, 2); |
takuto003 | 0:c3e3dc7923c8 | 150 | cmd[0] = 0xf5; |
takuto003 | 0:c3e3dc7923c8 | 151 | cmd[1] = 0xa0; |
takuto003 | 0:c3e3dc7923c8 | 152 | i2c.write(address, cmd, 2); |
takuto003 | 0:c3e3dc7923c8 | 153 | cmd[0] = 0x88; |
takuto003 | 0:c3e3dc7923c8 | 154 | i2c.write(address, cmd, 1); |
takuto003 | 0:c3e3dc7923c8 | 155 | i2c.read(address, cmd, 6); |
takuto003 | 0:c3e3dc7923c8 | 156 | dig_T1 = (cmd[1] << 8) | cmd[0]; |
takuto003 | 0:c3e3dc7923c8 | 157 | dig_T2 = (cmd[3] << 8) | cmd[2]; |
takuto003 | 0:c3e3dc7923c8 | 158 | dig_T3 = (cmd[5] << 8) | cmd[4]; |
takuto003 | 0:c3e3dc7923c8 | 159 | cmd[0] = 0x8E; |
takuto003 | 0:c3e3dc7923c8 | 160 | i2c.write(address, cmd, 1); |
takuto003 | 0:c3e3dc7923c8 | 161 | i2c.read(address, cmd, 18); |
takuto003 | 0:c3e3dc7923c8 | 162 | dig_P1 = (cmd[1] << 8) | cmd[0]; |
takuto003 | 0:c3e3dc7923c8 | 163 | dig_P2 = (cmd[3] << 8) | cmd[2]; |
takuto003 | 0:c3e3dc7923c8 | 164 | dig_P3 = (cmd[5] << 8) | cmd[4]; |
takuto003 | 0:c3e3dc7923c8 | 165 | dig_P4 = (cmd[7] << 8) | cmd[6]; |
takuto003 | 0:c3e3dc7923c8 | 166 | dig_P5 = (cmd[9] << 8) | cmd[8]; |
takuto003 | 0:c3e3dc7923c8 | 167 | dig_P6 = (cmd[11] << 8) | cmd[10]; |
takuto003 | 0:c3e3dc7923c8 | 168 | dig_P7 = (cmd[13] << 8) | cmd[12]; |
takuto003 | 0:c3e3dc7923c8 | 169 | dig_P8 = (cmd[15] << 8) | cmd[14]; |
takuto003 | 0:c3e3dc7923c8 | 170 | dig_P9 = (cmd[17] << 8) | cmd[16]; |
takuto003 | 0:c3e3dc7923c8 | 171 | } |
takuto003 | 0:c3e3dc7923c8 | 172 | float getTemperature() |
takuto003 | 0:c3e3dc7923c8 | 173 | { |
takuto003 | 0:c3e3dc7923c8 | 174 | uint32_t temp_raw; |
takuto003 | 0:c3e3dc7923c8 | 175 | float tempf; |
takuto003 | 0:c3e3dc7923c8 | 176 | char cmd[4]; |
takuto003 | 0:c3e3dc7923c8 | 177 | cmd[0] = 0xfa; |
takuto003 | 0:c3e3dc7923c8 | 178 | i2c.write(address, cmd, 1); |
takuto003 | 0:c3e3dc7923c8 | 179 | i2c.read(address, &cmd[1], 3); |
takuto003 | 0:c3e3dc7923c8 | 180 | temp_raw = (cmd[1] << 12) | (cmd[2] << 4) | (cmd[3] >> 4); |
takuto003 | 0:c3e3dc7923c8 | 181 | int32_t temp; |
takuto003 | 0:c3e3dc7923c8 | 182 | temp = |
takuto003 | 0:c3e3dc7923c8 | 183 | (((((temp_raw >> 3) - (dig_T1 << 1))) * dig_T2) >> 11) + |
takuto003 | 0:c3e3dc7923c8 | 184 | ((((((temp_raw >> 4) - dig_T1) * ((temp_raw >> 4) - dig_T1)) >> 12) * dig_T3) >> 14); |
takuto003 | 0:c3e3dc7923c8 | 185 | t_fine = temp; |
takuto003 | 0:c3e3dc7923c8 | 186 | temp = (temp * 5 + 128) >> 8; |
takuto003 | 0:c3e3dc7923c8 | 187 | tempf = (float)temp; |
takuto003 | 0:c3e3dc7923c8 | 188 | return (tempf / 100.0f); |
takuto003 | 0:c3e3dc7923c8 | 189 | } |
takuto003 | 0:c3e3dc7923c8 | 190 | float getPressure() |
takuto003 | 0:c3e3dc7923c8 | 191 | { |
takuto003 | 0:c3e3dc7923c8 | 192 | uint32_t press_raw; |
takuto003 | 0:c3e3dc7923c8 | 193 | float pressf; |
takuto003 | 0:c3e3dc7923c8 | 194 | char cmd[4]; |
takuto003 | 0:c3e3dc7923c8 | 195 | cmd[0] = 0xf7; // press_msb |
takuto003 | 0:c3e3dc7923c8 | 196 | i2c.write(address, cmd, 1); |
takuto003 | 0:c3e3dc7923c8 | 197 | i2c.read(address, &cmd[1], 3); |
takuto003 | 0:c3e3dc7923c8 | 198 | press_raw = (cmd[1] << 12) | (cmd[2] << 4) | (cmd[3] >> 4); |
takuto003 | 0:c3e3dc7923c8 | 199 | |
takuto003 | 0:c3e3dc7923c8 | 200 | int32_t var1, var2; |
takuto003 | 0:c3e3dc7923c8 | 201 | uint32_t press; |
takuto003 | 0:c3e3dc7923c8 | 202 | |
takuto003 | 0:c3e3dc7923c8 | 203 | var1 = (t_fine >> 1) - 64000; |
takuto003 | 0:c3e3dc7923c8 | 204 | var2 = (((var1 >> 2) * (var1 >> 2)) >> 11) * dig_P6; |
takuto003 | 0:c3e3dc7923c8 | 205 | var2 = var2 + ((var1 * dig_P5) << 1); |
takuto003 | 0:c3e3dc7923c8 | 206 | var2 = (var2 >> 2) + (dig_P4 << 16); |
takuto003 | 0:c3e3dc7923c8 | 207 | var1 = (((dig_P3 * (((var1 >> 2) * (var1 >> 2)) >> 13)) >> 3) + ((dig_P2 * var1) >> 1)) >> 18; |
takuto003 | 0:c3e3dc7923c8 | 208 | var1 = ((32768 + var1) * dig_P1) >> 15; |
takuto003 | 0:c3e3dc7923c8 | 209 | if (var1 == 0) |
takuto003 | 0:c3e3dc7923c8 | 210 | { |
takuto003 | 0:c3e3dc7923c8 | 211 | return 0; |
takuto003 | 0:c3e3dc7923c8 | 212 | } |
takuto003 | 0:c3e3dc7923c8 | 213 | press = (((1048576 - press_raw) - (var2 >> 12))) * 3125; |
takuto003 | 0:c3e3dc7923c8 | 214 | if (press < 0x80000000) |
takuto003 | 0:c3e3dc7923c8 | 215 | { |
takuto003 | 0:c3e3dc7923c8 | 216 | press = (press << 1) / var1; |
takuto003 | 0:c3e3dc7923c8 | 217 | } |
takuto003 | 0:c3e3dc7923c8 | 218 | else |
takuto003 | 0:c3e3dc7923c8 | 219 | { |
takuto003 | 0:c3e3dc7923c8 | 220 | press = (press / var1) * 2; |
takuto003 | 0:c3e3dc7923c8 | 221 | } |
takuto003 | 0:c3e3dc7923c8 | 222 | var1 = ((int32_t)dig_P9 * ((int32_t)(((press >> 3) * (press >> 3)) >> 13))) >> 12; |
takuto003 | 0:c3e3dc7923c8 | 223 | var2 = (((int32_t)(press >> 2)) * (int32_t)dig_P8) >> 13; |
takuto003 | 0:c3e3dc7923c8 | 224 | press = (press + ((var1 + var2 + dig_P7) >> 4)); |
takuto003 | 0:c3e3dc7923c8 | 225 | pressf = (float)press; |
takuto003 | 0:c3e3dc7923c8 | 226 | return (pressf / 100.0f); |
takuto003 | 0:c3e3dc7923c8 | 227 | } |
takuto003 | 0:c3e3dc7923c8 | 228 | void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) |
takuto003 | 0:c3e3dc7923c8 | 229 | { |
takuto003 | 0:c3e3dc7923c8 | 230 | char data_write[2]; |
takuto003 | 0:c3e3dc7923c8 | 231 | data_write[0] = subAddress; |
takuto003 | 0:c3e3dc7923c8 | 232 | data_write[1] = data; |
takuto003 | 0:c3e3dc7923c8 | 233 | i2c.write(address, data_write, 2, 0); |
takuto003 | 0:c3e3dc7923c8 | 234 | } |
takuto003 | 0:c3e3dc7923c8 | 235 | |
takuto003 | 0:c3e3dc7923c8 | 236 | char readByte(uint8_t address, uint8_t subAddress) |
takuto003 | 0:c3e3dc7923c8 | 237 | { |
takuto003 | 0:c3e3dc7923c8 | 238 | char data[1]; |
takuto003 | 0:c3e3dc7923c8 | 239 | char data_write[1]; |
takuto003 | 0:c3e3dc7923c8 | 240 | data_write[0] = subAddress; |
takuto003 | 0:c3e3dc7923c8 | 241 | i2c.write(address, data_write, 1, 1); |
takuto003 | 0:c3e3dc7923c8 | 242 | i2c.read(address, data, 1, 0); |
takuto003 | 0:c3e3dc7923c8 | 243 | return data[0]; |
takuto003 | 0:c3e3dc7923c8 | 244 | } |
takuto003 | 0:c3e3dc7923c8 | 245 | |
takuto003 | 0:c3e3dc7923c8 | 246 | void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t *dest) |
takuto003 | 0:c3e3dc7923c8 | 247 | { |
takuto003 | 0:c3e3dc7923c8 | 248 | char data[14]; |
takuto003 | 0:c3e3dc7923c8 | 249 | char data_write[1]; |
takuto003 | 0:c3e3dc7923c8 | 250 | data_write[0] = subAddress; |
takuto003 | 0:c3e3dc7923c8 | 251 | i2c.write(address, data_write, 1, 1); |
takuto003 | 0:c3e3dc7923c8 | 252 | i2c.read(address, data, count, 0); |
takuto003 | 0:c3e3dc7923c8 | 253 | for (int ii = 0; ii < count; ii++) |
takuto003 | 0:c3e3dc7923c8 | 254 | { |
takuto003 | 0:c3e3dc7923c8 | 255 | dest[ii] = data[ii]; |
takuto003 | 0:c3e3dc7923c8 | 256 | } |
takuto003 | 0:c3e3dc7923c8 | 257 | } |
takuto003 | 0:c3e3dc7923c8 | 258 | |
takuto003 | 0:c3e3dc7923c8 | 259 | void getMres() |
takuto003 | 0:c3e3dc7923c8 | 260 | { |
takuto003 | 0:c3e3dc7923c8 | 261 | switch (Mscale) |
takuto003 | 0:c3e3dc7923c8 | 262 | { |
takuto003 | 0:c3e3dc7923c8 | 263 | case MFS_14BITS: |
takuto003 | 0:c3e3dc7923c8 | 264 | mRes = 10.0 * 4219.0 / 8190.0; |
takuto003 | 0:c3e3dc7923c8 | 265 | break; |
takuto003 | 0:c3e3dc7923c8 | 266 | case MFS_16BITS: |
takuto003 | 0:c3e3dc7923c8 | 267 | mRes = 10.0 * 4219.0 / 32760.0; |
takuto003 | 0:c3e3dc7923c8 | 268 | break; |
takuto003 | 0:c3e3dc7923c8 | 269 | } |
takuto003 | 0:c3e3dc7923c8 | 270 | } |
takuto003 | 0:c3e3dc7923c8 | 271 | |
takuto003 | 0:c3e3dc7923c8 | 272 | void getGres() |
takuto003 | 0:c3e3dc7923c8 | 273 | { |
takuto003 | 0:c3e3dc7923c8 | 274 | switch (Gscale) |
takuto003 | 0:c3e3dc7923c8 | 275 | { |
takuto003 | 0:c3e3dc7923c8 | 276 | case GFS_250DPS: |
takuto003 | 0:c3e3dc7923c8 | 277 | gRes = 250.0 / 32768.0; |
takuto003 | 0:c3e3dc7923c8 | 278 | break; |
takuto003 | 0:c3e3dc7923c8 | 279 | case GFS_500DPS: |
takuto003 | 0:c3e3dc7923c8 | 280 | gRes = 500.0 / 32768.0; |
takuto003 | 0:c3e3dc7923c8 | 281 | break; |
takuto003 | 0:c3e3dc7923c8 | 282 | case GFS_1000DPS: |
takuto003 | 0:c3e3dc7923c8 | 283 | gRes = 1000.0 / 32768.0; |
takuto003 | 0:c3e3dc7923c8 | 284 | break; |
takuto003 | 0:c3e3dc7923c8 | 285 | case GFS_2000DPS: |
takuto003 | 0:c3e3dc7923c8 | 286 | gRes = 2000.0 / 32768.0; |
takuto003 | 0:c3e3dc7923c8 | 287 | break; |
takuto003 | 0:c3e3dc7923c8 | 288 | } |
takuto003 | 0:c3e3dc7923c8 | 289 | } |
takuto003 | 0:c3e3dc7923c8 | 290 | |
takuto003 | 0:c3e3dc7923c8 | 291 | void getAres() |
takuto003 | 0:c3e3dc7923c8 | 292 | { |
takuto003 | 0:c3e3dc7923c8 | 293 | switch (Ascale) |
takuto003 | 0:c3e3dc7923c8 | 294 | { |
takuto003 | 0:c3e3dc7923c8 | 295 | case AFS_2G: |
takuto003 | 0:c3e3dc7923c8 | 296 | aRes = 2.0 / 32768.0; |
takuto003 | 0:c3e3dc7923c8 | 297 | break; |
takuto003 | 0:c3e3dc7923c8 | 298 | case AFS_4G: |
takuto003 | 0:c3e3dc7923c8 | 299 | aRes = 4.0 / 32768.0; |
takuto003 | 0:c3e3dc7923c8 | 300 | break; |
takuto003 | 0:c3e3dc7923c8 | 301 | case AFS_8G: |
takuto003 | 0:c3e3dc7923c8 | 302 | aRes = 8.0 / 32768.0; |
takuto003 | 0:c3e3dc7923c8 | 303 | break; |
takuto003 | 0:c3e3dc7923c8 | 304 | case AFS_16G: |
takuto003 | 0:c3e3dc7923c8 | 305 | aRes = 16.0 / 32768.0; |
takuto003 | 0:c3e3dc7923c8 | 306 | break; |
takuto003 | 0:c3e3dc7923c8 | 307 | } |
takuto003 | 0:c3e3dc7923c8 | 308 | } |
takuto003 | 0:c3e3dc7923c8 | 309 | |
takuto003 | 0:c3e3dc7923c8 | 310 | void readAccelData(int16_t *destination) |
takuto003 | 0:c3e3dc7923c8 | 311 | { |
takuto003 | 0:c3e3dc7923c8 | 312 | uint8_t rawData[6]; |
takuto003 | 0:c3e3dc7923c8 | 313 | readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); |
takuto003 | 0:c3e3dc7923c8 | 314 | destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]); |
takuto003 | 0:c3e3dc7923c8 | 315 | destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]); |
takuto003 | 0:c3e3dc7923c8 | 316 | destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]); |
takuto003 | 0:c3e3dc7923c8 | 317 | } |
takuto003 | 0:c3e3dc7923c8 | 318 | |
takuto003 | 0:c3e3dc7923c8 | 319 | void readGyroData(int16_t *destination) |
takuto003 | 0:c3e3dc7923c8 | 320 | { |
takuto003 | 0:c3e3dc7923c8 | 321 | uint8_t rawData[6]; |
takuto003 | 0:c3e3dc7923c8 | 322 | readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); |
takuto003 | 0:c3e3dc7923c8 | 323 | destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]); |
takuto003 | 0:c3e3dc7923c8 | 324 | destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]); |
takuto003 | 0:c3e3dc7923c8 | 325 | destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]); |
takuto003 | 0:c3e3dc7923c8 | 326 | } |
takuto003 | 0:c3e3dc7923c8 | 327 | |
takuto003 | 0:c3e3dc7923c8 | 328 | void readMagData(int16_t *destination) |
takuto003 | 0:c3e3dc7923c8 | 329 | { |
takuto003 | 0:c3e3dc7923c8 | 330 | uint8_t rawData[7]; |
takuto003 | 0:c3e3dc7923c8 | 331 | if (readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) |
takuto003 | 0:c3e3dc7923c8 | 332 | { |
takuto003 | 0:c3e3dc7923c8 | 333 | readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); |
takuto003 | 0:c3e3dc7923c8 | 334 | uint8_t c = rawData[6]; |
takuto003 | 0:c3e3dc7923c8 | 335 | if (!(c & 0x08)) |
takuto003 | 0:c3e3dc7923c8 | 336 | { |
takuto003 | 0:c3e3dc7923c8 | 337 | destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]); |
takuto003 | 0:c3e3dc7923c8 | 338 | destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]); |
takuto003 | 0:c3e3dc7923c8 | 339 | destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]); |
takuto003 | 0:c3e3dc7923c8 | 340 | } |
takuto003 | 0:c3e3dc7923c8 | 341 | } |
takuto003 | 0:c3e3dc7923c8 | 342 | } |
takuto003 | 0:c3e3dc7923c8 | 343 | |
takuto003 | 0:c3e3dc7923c8 | 344 | int16_t readTempData() |
takuto003 | 0:c3e3dc7923c8 | 345 | { |
takuto003 | 0:c3e3dc7923c8 | 346 | uint8_t rawData[2]; |
takuto003 | 0:c3e3dc7923c8 | 347 | readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); |
takuto003 | 0:c3e3dc7923c8 | 348 | return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]); |
takuto003 | 0:c3e3dc7923c8 | 349 | } |
takuto003 | 0:c3e3dc7923c8 | 350 | |
takuto003 | 0:c3e3dc7923c8 | 351 | void resetMPU9250() |
takuto003 | 0:c3e3dc7923c8 | 352 | { |
takuto003 | 0:c3e3dc7923c8 | 353 | writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); |
takuto003 | 0:c3e3dc7923c8 | 354 | wait(0.1); |
takuto003 | 0:c3e3dc7923c8 | 355 | } |
takuto003 | 0:c3e3dc7923c8 | 356 | |
takuto003 | 0:c3e3dc7923c8 | 357 | void initAK8963(float *destination) |
takuto003 | 0:c3e3dc7923c8 | 358 | { |
takuto003 | 0:c3e3dc7923c8 | 359 | uint8_t rawData[3]; |
takuto003 | 0:c3e3dc7923c8 | 360 | writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); |
takuto003 | 0:c3e3dc7923c8 | 361 | wait(0.01); |
takuto003 | 0:c3e3dc7923c8 | 362 | writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); |
takuto003 | 0:c3e3dc7923c8 | 363 | wait(0.01); |
takuto003 | 0:c3e3dc7923c8 | 364 | readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); |
takuto003 | 0:c3e3dc7923c8 | 365 | destination[0] = (float)(rawData[0] - 128) / 256.0f + 1.0f; |
takuto003 | 0:c3e3dc7923c8 | 366 | destination[1] = (float)(rawData[1] - 128) / 256.0f + 1.0f; |
takuto003 | 0:c3e3dc7923c8 | 367 | destination[2] = (float)(rawData[2] - 128) / 256.0f + 1.0f; |
takuto003 | 0:c3e3dc7923c8 | 368 | writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); |
takuto003 | 0:c3e3dc7923c8 | 369 | wait(0.01); |
takuto003 | 0:c3e3dc7923c8 | 370 | writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); |
takuto003 | 0:c3e3dc7923c8 | 371 | wait(0.01); |
takuto003 | 0:c3e3dc7923c8 | 372 | } |
takuto003 | 0:c3e3dc7923c8 | 373 | |
takuto003 | 0:c3e3dc7923c8 | 374 | void initMPU9250() |
takuto003 | 0:c3e3dc7923c8 | 375 | { |
takuto003 | 0:c3e3dc7923c8 | 376 | writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); |
takuto003 | 0:c3e3dc7923c8 | 377 | wait(0.1); |
takuto003 | 0:c3e3dc7923c8 | 378 | writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); |
takuto003 | 0:c3e3dc7923c8 | 379 | writeByte(MPU9250_ADDRESS, CONFIG, 0x03); |
takuto003 | 0:c3e3dc7923c8 | 380 | writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); |
takuto003 | 0:c3e3dc7923c8 | 381 | uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); |
takuto003 | 0:c3e3dc7923c8 | 382 | writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0xE0); |
takuto003 | 0:c3e3dc7923c8 | 383 | writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0x18); |
takuto003 | 0:c3e3dc7923c8 | 384 | writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c | Gscale << 3); |
takuto003 | 0:c3e3dc7923c8 | 385 | |
takuto003 | 0:c3e3dc7923c8 | 386 | c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); |
takuto003 | 0:c3e3dc7923c8 | 387 | writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0xE0); |
takuto003 | 0:c3e3dc7923c8 | 388 | writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0x18); |
takuto003 | 0:c3e3dc7923c8 | 389 | writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c | Ascale << 3); |
takuto003 | 0:c3e3dc7923c8 | 390 | c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); |
takuto003 | 0:c3e3dc7923c8 | 391 | writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c & ~0x0F); |
takuto003 | 0:c3e3dc7923c8 | 392 | writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c | 0x03); |
takuto003 | 0:c3e3dc7923c8 | 393 | writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22); |
takuto003 | 0:c3e3dc7923c8 | 394 | writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); |
takuto003 | 0:c3e3dc7923c8 | 395 | } |
takuto003 | 0:c3e3dc7923c8 | 396 | // void calibrateMPU9250(float *dest1, float *dest2) |
takuto003 | 0:c3e3dc7923c8 | 397 | // { |
takuto003 | 0:c3e3dc7923c8 | 398 | // uint8_t data[12]; |
takuto003 | 0:c3e3dc7923c8 | 399 | // uint16_t ii, packet_count, fifo_count; |
takuto003 | 0:c3e3dc7923c8 | 400 | // int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; |
takuto003 | 0:c3e3dc7923c8 | 401 | // |
takuto003 | 0:c3e3dc7923c8 | 402 | // writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); |
takuto003 | 0:c3e3dc7923c8 | 403 | // wait(0.1); |
takuto003 | 0:c3e3dc7923c8 | 404 | // writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); |
takuto003 | 0:c3e3dc7923c8 | 405 | // writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); |
takuto003 | 0:c3e3dc7923c8 | 406 | // wait(0.2); |
takuto003 | 0:c3e3dc7923c8 | 407 | // writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); |
takuto003 | 0:c3e3dc7923c8 | 408 | // writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); |
takuto003 | 0:c3e3dc7923c8 | 409 | // writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); |
takuto003 | 0:c3e3dc7923c8 | 410 | // writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); |
takuto003 | 0:c3e3dc7923c8 | 411 | // writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); |
takuto003 | 0:c3e3dc7923c8 | 412 | // writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); |
takuto003 | 0:c3e3dc7923c8 | 413 | // wait(0.015); |
takuto003 | 0:c3e3dc7923c8 | 414 | // writeByte(MPU9250_ADDRESS, CONFIG, 0x01); |
takuto003 | 0:c3e3dc7923c8 | 415 | // writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); |
takuto003 | 0:c3e3dc7923c8 | 416 | // writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); |
takuto003 | 0:c3e3dc7923c8 | 417 | // writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); |
takuto003 | 0:c3e3dc7923c8 | 418 | // |
takuto003 | 0:c3e3dc7923c8 | 419 | // uint16_t gyrosensitivity = 131; |
takuto003 | 0:c3e3dc7923c8 | 420 | // uint16_t accelsensitivity = 16384; |
takuto003 | 0:c3e3dc7923c8 | 421 | // writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); |
takuto003 | 0:c3e3dc7923c8 | 422 | // writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); |
takuto003 | 0:c3e3dc7923c8 | 423 | // wait(0.04); |
takuto003 | 0:c3e3dc7923c8 | 424 | // writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); |
takuto003 | 0:c3e3dc7923c8 | 425 | // readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); |
takuto003 | 0:c3e3dc7923c8 | 426 | // fifo_count = ((uint16_t)data[0] << 8) | data[1]; |
takuto003 | 0:c3e3dc7923c8 | 427 | // packet_count = fifo_count / 12; |
takuto003 | 0:c3e3dc7923c8 | 428 | // |
takuto003 | 0:c3e3dc7923c8 | 429 | // for (ii = 0; ii < packet_count; ii++) |
takuto003 | 0:c3e3dc7923c8 | 430 | // { |
takuto003 | 0:c3e3dc7923c8 | 431 | // int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; |
takuto003 | 0:c3e3dc7923c8 | 432 | // readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); |
takuto003 | 0:c3e3dc7923c8 | 433 | // accel_temp[0] = (int16_t)(((int16_t)data[0] << 8) | data[1]); |
takuto003 | 0:c3e3dc7923c8 | 434 | // accel_temp[1] = (int16_t)(((int16_t)data[2] << 8) | data[3]); |
takuto003 | 0:c3e3dc7923c8 | 435 | // accel_temp[2] = (int16_t)(((int16_t)data[4] << 8) | data[5]); |
takuto003 | 0:c3e3dc7923c8 | 436 | // gyro_temp[0] = (int16_t)(((int16_t)data[6] << 8) | data[7]); |
takuto003 | 0:c3e3dc7923c8 | 437 | // gyro_temp[1] = (int16_t)(((int16_t)data[8] << 8) | data[9]); |
takuto003 | 0:c3e3dc7923c8 | 438 | // gyro_temp[2] = (int16_t)(((int16_t)data[10] << 8) | data[11]); |
takuto003 | 0:c3e3dc7923c8 | 439 | // |
takuto003 | 0:c3e3dc7923c8 | 440 | // accel_bias[0] += (int32_t)accel_temp[0]; |
takuto003 | 0:c3e3dc7923c8 | 441 | // accel_bias[1] += (int32_t)accel_temp[1]; |
takuto003 | 0:c3e3dc7923c8 | 442 | // accel_bias[2] += (int32_t)accel_temp[2]; |
takuto003 | 0:c3e3dc7923c8 | 443 | // gyro_bias[0] += (int32_t)gyro_temp[0]; |
takuto003 | 0:c3e3dc7923c8 | 444 | // gyro_bias[1] += (int32_t)gyro_temp[1]; |
takuto003 | 0:c3e3dc7923c8 | 445 | // gyro_bias[2] += (int32_t)gyro_temp[2]; |
takuto003 | 0:c3e3dc7923c8 | 446 | // } |
takuto003 | 0:c3e3dc7923c8 | 447 | // accel_bias[0] /= (int32_t)packet_count; |
takuto003 | 0:c3e3dc7923c8 | 448 | // accel_bias[1] /= (int32_t)packet_count; |
takuto003 | 0:c3e3dc7923c8 | 449 | // accel_bias[2] /= (int32_t)packet_count; |
takuto003 | 0:c3e3dc7923c8 | 450 | // gyro_bias[0] /= (int32_t)packet_count; |
takuto003 | 0:c3e3dc7923c8 | 451 | // gyro_bias[1] /= (int32_t)packet_count; |
takuto003 | 0:c3e3dc7923c8 | 452 | // gyro_bias[2] /= (int32_t)packet_count; |
takuto003 | 0:c3e3dc7923c8 | 453 | // |
takuto003 | 0:c3e3dc7923c8 | 454 | // if (accel_bias[2] > 0L) |
takuto003 | 0:c3e3dc7923c8 | 455 | // { |
takuto003 | 0:c3e3dc7923c8 | 456 | // accel_bias[2] -= (int32_t)accelsensitivity; |
takuto003 | 0:c3e3dc7923c8 | 457 | // } |
takuto003 | 0:c3e3dc7923c8 | 458 | // else |
takuto003 | 0:c3e3dc7923c8 | 459 | // { |
takuto003 | 0:c3e3dc7923c8 | 460 | // accel_bias[2] += (int32_t)accelsensitivity; |
takuto003 | 0:c3e3dc7923c8 | 461 | // } |
takuto003 | 0:c3e3dc7923c8 | 462 | // data[0] = (-gyro_bias[0] / 4 >> 8) & 0xFF; |
takuto003 | 0:c3e3dc7923c8 | 463 | // data[1] = (-gyro_bias[0] / 4) & 0xFF; |
takuto003 | 0:c3e3dc7923c8 | 464 | // data[2] = (-gyro_bias[1] / 4 >> 8) & 0xFF; |
takuto003 | 0:c3e3dc7923c8 | 465 | // data[3] = (-gyro_bias[1] / 4) & 0xFF; |
takuto003 | 0:c3e3dc7923c8 | 466 | // data[4] = (-gyro_bias[2] / 4 >> 8) & 0xFF; |
takuto003 | 0:c3e3dc7923c8 | 467 | // data[5] = (-gyro_bias[2] / 4) & 0xFF; |
takuto003 | 0:c3e3dc7923c8 | 468 | // |
takuto003 | 0:c3e3dc7923c8 | 469 | // dest1[0] = (float)gyro_bias[0] / (float)gyrosensitivity; |
takuto003 | 0:c3e3dc7923c8 | 470 | // dest1[1] = (float)gyro_bias[1] / (float)gyrosensitivity; |
takuto003 | 0:c3e3dc7923c8 | 471 | // dest1[2] = (float)gyro_bias[2] / (float)gyrosensitivity; |
takuto003 | 0:c3e3dc7923c8 | 472 | // |
takuto003 | 0:c3e3dc7923c8 | 473 | // int32_t accel_bias_reg[3] = {0, 0, 0}; |
takuto003 | 0:c3e3dc7923c8 | 474 | // readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); |
takuto003 | 0:c3e3dc7923c8 | 475 | // accel_bias_reg[0] = (int16_t)((int16_t)data[0] << 8) | data[1]; |
takuto003 | 0:c3e3dc7923c8 | 476 | // readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); |
takuto003 | 0:c3e3dc7923c8 | 477 | // accel_bias_reg[1] = (int16_t)((int16_t)data[0] << 8) | data[1]; |
takuto003 | 0:c3e3dc7923c8 | 478 | // readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]); |
takuto003 | 0:c3e3dc7923c8 | 479 | // accel_bias_reg[2] = (int16_t)((int16_t)data[0] << 8) | data[1]; |
takuto003 | 0:c3e3dc7923c8 | 480 | // |
takuto003 | 0:c3e3dc7923c8 | 481 | // uint32_t mask = 1uL; |
takuto003 | 0:c3e3dc7923c8 | 482 | // uint8_t mask_bit[3] = {0, 0, 0}; |
takuto003 | 0:c3e3dc7923c8 | 483 | // |
takuto003 | 0:c3e3dc7923c8 | 484 | // for (ii = 0; ii < 3; ii++) |
takuto003 | 0:c3e3dc7923c8 | 485 | // { |
takuto003 | 0:c3e3dc7923c8 | 486 | // if (accel_bias_reg[ii] & mask) |
takuto003 | 0:c3e3dc7923c8 | 487 | // mask_bit[ii] = 0x01; |
takuto003 | 0:c3e3dc7923c8 | 488 | // } |
takuto003 | 0:c3e3dc7923c8 | 489 | // accel_bias_reg[0] -= (accel_bias[0] / 8); |
takuto003 | 0:c3e3dc7923c8 | 490 | // accel_bias_reg[1] -= (accel_bias[1] / 8); |
takuto003 | 0:c3e3dc7923c8 | 491 | // accel_bias_reg[2] -= (accel_bias[2] / 8); |
takuto003 | 0:c3e3dc7923c8 | 492 | // data[0] = (accel_bias_reg[0] >> 8) & 0xFF; |
takuto003 | 0:c3e3dc7923c8 | 493 | // data[1] = (accel_bias_reg[0]) & 0xFF; |
takuto003 | 0:c3e3dc7923c8 | 494 | // data[1] = data[1] | mask_bit[0]; |
takuto003 | 0:c3e3dc7923c8 | 495 | // data[2] = (accel_bias_reg[1] >> 8) & 0xFF; |
takuto003 | 0:c3e3dc7923c8 | 496 | // data[3] = (accel_bias_reg[1]) & 0xFF; |
takuto003 | 0:c3e3dc7923c8 | 497 | // data[3] = data[3] | mask_bit[1]; |
takuto003 | 0:c3e3dc7923c8 | 498 | // data[4] = (accel_bias_reg[2] >> 8) & 0xFF; |
takuto003 | 0:c3e3dc7923c8 | 499 | // data[5] = (accel_bias_reg[2]) & 0xFF; |
takuto003 | 0:c3e3dc7923c8 | 500 | // data[5] = data[5] | mask_bit[2]; |
takuto003 | 0:c3e3dc7923c8 | 501 | // dest2[0] = (float)accel_bias[0] / (float)accelsensitivity; |
takuto003 | 0:c3e3dc7923c8 | 502 | // dest2[1] = (float)accel_bias[1] / (float)accelsensitivity; |
takuto003 | 0:c3e3dc7923c8 | 503 | // dest2[2] = (float)accel_bias[2] / (float)accelsensitivity; |
takuto003 | 0:c3e3dc7923c8 | 504 | // } |
takuto003 | 0:c3e3dc7923c8 | 505 | }; |