CORE / Logger

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?

UserRevisionLine numberNew 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 };