CORE
logger
Diff: logger.h
- Revision:
- 0:c3e3dc7923c8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/logger.h Mon Dec 09 15:37:51 2019 +0000
@@ -0,0 +1,505 @@
+#include "mbed.h"
+#include "math.h"
+
+#define BMP280_SLAVE_ADDRESS (0x76 << 1)
+#define AK8963_ADDRESS 0x0C << 1
+#define WHO_AM_I_AK8963 0x00
+#define INFO 0x01
+#define AK8963_ST1 0x02
+#define AK8963_XOUT_L 0x03
+#define AK8963_XOUT_H 0x04
+#define AK8963_YOUT_L 0x05
+#define AK8963_YOUT_H 0x06
+#define AK8963_ZOUT_L 0x07
+#define AK8963_ZOUT_H 0x08
+#define AK8963_ST2 0x09
+#define AK8963_CNTL 0x0A
+#define AK8963_ASTC 0x0C
+#define AK8963_I2CDIS 0x0F
+#define AK8963_ASAX 0x10
+#define AK8963_ASAY 0x11
+#define AK8963_ASAZ 0x12
+#define XG_OFFSET_H 0x13
+#define XG_OFFSET_L 0x14
+#define YG_OFFSET_H 0x15
+#define YG_OFFSET_L 0x16
+#define ZG_OFFSET_H 0x17
+#define ZG_OFFSET_L 0x18
+#define SMPLRT_DIV 0x19
+#define CONFIG 0x1A
+#define GYRO_CONFIG 0x1B
+#define ACCEL_CONFIG 0x1C
+#define ACCEL_CONFIG2 0x1D
+#define LP_ACCEL_ODR 0x1E
+#define WOM_THR 0x1F
+#define FIFO_EN 0x23
+#define I2C_MST_CTRL 0x24
+#define I2C_MST_STATUS 0x36
+#define INT_PIN_CFG 0x37
+#define INT_ENABLE 0x38
+#define DMP_INT_STATUS 0x39
+#define INT_STATUS 0x3A
+#define ACCEL_XOUT_H 0x3B
+#define ACCEL_XOUT_L 0x3C
+#define ACCEL_YOUT_H 0x3D
+#define ACCEL_YOUT_L 0x3E
+#define ACCEL_ZOUT_H 0x3F
+#define ACCEL_ZOUT_L 0x40
+#define TEMP_OUT_H 0x41
+#define TEMP_OUT_L 0x42
+#define GYRO_XOUT_H 0x43
+#define GYRO_XOUT_L 0x44
+#define GYRO_YOUT_H 0x45
+#define GYRO_YOUT_L 0x46
+#define GYRO_ZOUT_H 0x47
+#define GYRO_ZOUT_L 0x48
+#define MOT_DETECT_STATUS 0x61
+#define I2C_MST_DELAY_CTRL 0x67
+#define SIGNAL_PATH_RESET 0x68
+#define MOT_DETECT_CTRL 0x69
+#define USER_CTRL 0x6A
+#define PWR_MGMT_1 0x6B
+#define PWR_MGMT_2 0x6C
+#define FIFO_COUNTH 0x72
+#define FIFO_COUNTL 0x73
+#define FIFO_R_W 0x74
+#define WHO_AM_I_MPU9250 0x75
+#define XA_OFFSET_H 0x77
+#define XA_OFFSET_L 0x78
+#define YA_OFFSET_H 0x7A
+#define YA_OFFSET_L 0x7B
+#define ZA_OFFSET_H 0x7D
+#define ZA_OFFSET_L 0x7E
+#define P0 1015.0
+#define PI 3.14159265358979323846f
+#define ADO 0
+#define MPU9250_ADDRESS 0x68 << 1
+#define I2C_SDA PB_7
+#define I2C_SCL PB_6
+I2C i2c(I2C_SDA, I2C_SCL);
+
+enum Ascale
+{
+ AFS_2G = 0,
+ AFS_4G,
+ AFS_8G,
+ AFS_16G
+};
+
+enum Gscale
+{
+ GFS_250DPS = 0,
+ GFS_500DPS,
+ GFS_1000DPS,
+ GFS_2000DPS
+};
+
+enum Mscale
+{
+ MFS_14BITS = 0,
+ MFS_16BITS
+};
+uint8_t Ascale;
+uint8_t Gscale;
+uint8_t Mscale;
+uint8_t Mmode;
+float aRes, gRes, mRes;
+
+int16_t accelCount[3];
+int16_t gyroCount[3];
+int16_t magCount[3];
+float q[4];
+
+float ax, ay, az, gx, gy, gz, mx, my, mz;
+float pitch, yaw, roll;
+float deltat;
+int lastUpdate, firstUpdate, Now;
+int delt_t;
+int count;
+int16_t tempCount;
+float temperature;
+
+class LOGGER
+{
+private:
+ char address;
+ uint16_t dig_T1;
+ int16_t dig_T2, dig_T3;
+ uint16_t dig_P1;
+ int16_t dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9;
+ int32_t t_fine;
+
+public:
+ float pressf;
+ float tempf;
+
+ LOGGER(char slave_adr = BMP280_SLAVE_ADDRESS) : address(slave_adr), t_fine(0)
+ {
+ initialize();
+ }
+
+ void initialize()
+ {
+ char cmd[18];
+ cmd[0] = 0xf2;
+ cmd[1] = 0x01;
+ i2c.write(address, cmd, 2);
+ cmd[0] = 0xf4;
+ cmd[1] = 0x27;
+ i2c.write(address, cmd, 2);
+ cmd[0] = 0xf5;
+ cmd[1] = 0xa0;
+ i2c.write(address, cmd, 2);
+ cmd[0] = 0x88;
+ i2c.write(address, cmd, 1);
+ i2c.read(address, cmd, 6);
+ dig_T1 = (cmd[1] << 8) | cmd[0];
+ dig_T2 = (cmd[3] << 8) | cmd[2];
+ dig_T3 = (cmd[5] << 8) | cmd[4];
+ cmd[0] = 0x8E;
+ i2c.write(address, cmd, 1);
+ i2c.read(address, cmd, 18);
+ dig_P1 = (cmd[1] << 8) | cmd[0];
+ dig_P2 = (cmd[3] << 8) | cmd[2];
+ dig_P3 = (cmd[5] << 8) | cmd[4];
+ dig_P4 = (cmd[7] << 8) | cmd[6];
+ dig_P5 = (cmd[9] << 8) | cmd[8];
+ dig_P6 = (cmd[11] << 8) | cmd[10];
+ dig_P7 = (cmd[13] << 8) | cmd[12];
+ dig_P8 = (cmd[15] << 8) | cmd[14];
+ dig_P9 = (cmd[17] << 8) | cmd[16];
+ }
+ float getTemperature()
+ {
+ uint32_t temp_raw;
+ float tempf;
+ char cmd[4];
+ cmd[0] = 0xfa;
+ i2c.write(address, cmd, 1);
+ i2c.read(address, &cmd[1], 3);
+ temp_raw = (cmd[1] << 12) | (cmd[2] << 4) | (cmd[3] >> 4);
+ int32_t temp;
+ temp =
+ (((((temp_raw >> 3) - (dig_T1 << 1))) * dig_T2) >> 11) +
+ ((((((temp_raw >> 4) - dig_T1) * ((temp_raw >> 4) - dig_T1)) >> 12) * dig_T3) >> 14);
+ t_fine = temp;
+ temp = (temp * 5 + 128) >> 8;
+ tempf = (float)temp;
+ return (tempf / 100.0f);
+ }
+ float getPressure()
+ {
+ uint32_t press_raw;
+ float pressf;
+ char cmd[4];
+ cmd[0] = 0xf7; // press_msb
+ i2c.write(address, cmd, 1);
+ i2c.read(address, &cmd[1], 3);
+ press_raw = (cmd[1] << 12) | (cmd[2] << 4) | (cmd[3] >> 4);
+
+ int32_t var1, var2;
+ uint32_t press;
+
+ var1 = (t_fine >> 1) - 64000;
+ var2 = (((var1 >> 2) * (var1 >> 2)) >> 11) * dig_P6;
+ var2 = var2 + ((var1 * dig_P5) << 1);
+ var2 = (var2 >> 2) + (dig_P4 << 16);
+ var1 = (((dig_P3 * (((var1 >> 2) * (var1 >> 2)) >> 13)) >> 3) + ((dig_P2 * var1) >> 1)) >> 18;
+ var1 = ((32768 + var1) * dig_P1) >> 15;
+ if (var1 == 0)
+ {
+ return 0;
+ }
+ press = (((1048576 - press_raw) - (var2 >> 12))) * 3125;
+ if (press < 0x80000000)
+ {
+ press = (press << 1) / var1;
+ }
+ else
+ {
+ press = (press / var1) * 2;
+ }
+ var1 = ((int32_t)dig_P9 * ((int32_t)(((press >> 3) * (press >> 3)) >> 13))) >> 12;
+ var2 = (((int32_t)(press >> 2)) * (int32_t)dig_P8) >> 13;
+ press = (press + ((var1 + var2 + dig_P7) >> 4));
+ pressf = (float)press;
+ return (pressf / 100.0f);
+ }
+ void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
+ {
+ char data_write[2];
+ data_write[0] = subAddress;
+ data_write[1] = data;
+ i2c.write(address, data_write, 2, 0);
+ }
+
+ char readByte(uint8_t address, uint8_t subAddress)
+ {
+ char data[1];
+ char data_write[1];
+ data_write[0] = subAddress;
+ i2c.write(address, data_write, 1, 1);
+ i2c.read(address, data, 1, 0);
+ return data[0];
+ }
+
+ void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t *dest)
+ {
+ char data[14];
+ char data_write[1];
+ data_write[0] = subAddress;
+ i2c.write(address, data_write, 1, 1);
+ i2c.read(address, data, count, 0);
+ for (int ii = 0; ii < count; ii++)
+ {
+ dest[ii] = data[ii];
+ }
+ }
+
+ void getMres()
+ {
+ switch (Mscale)
+ {
+ case MFS_14BITS:
+ mRes = 10.0 * 4219.0 / 8190.0;
+ break;
+ case MFS_16BITS:
+ mRes = 10.0 * 4219.0 / 32760.0;
+ break;
+ }
+ }
+
+ void getGres()
+ {
+ switch (Gscale)
+ {
+ case GFS_250DPS:
+ gRes = 250.0 / 32768.0;
+ break;
+ case GFS_500DPS:
+ gRes = 500.0 / 32768.0;
+ break;
+ case GFS_1000DPS:
+ gRes = 1000.0 / 32768.0;
+ break;
+ case GFS_2000DPS:
+ gRes = 2000.0 / 32768.0;
+ break;
+ }
+ }
+
+ void getAres()
+ {
+ switch (Ascale)
+ {
+ case AFS_2G:
+ aRes = 2.0 / 32768.0;
+ break;
+ case AFS_4G:
+ aRes = 4.0 / 32768.0;
+ break;
+ case AFS_8G:
+ aRes = 8.0 / 32768.0;
+ break;
+ case AFS_16G:
+ aRes = 16.0 / 32768.0;
+ break;
+ }
+ }
+
+ void readAccelData(int16_t *destination)
+ {
+ uint8_t rawData[6];
+ readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);
+ destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]);
+ destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]);
+ destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]);
+ }
+
+ void readGyroData(int16_t *destination)
+ {
+ uint8_t rawData[6];
+ readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);
+ destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]);
+ destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]);
+ destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]);
+ }
+
+ void readMagData(int16_t *destination)
+ {
+ uint8_t rawData[7];
+ if (readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01)
+ {
+ readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]);
+ uint8_t c = rawData[6];
+ if (!(c & 0x08))
+ {
+ destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]);
+ destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]);
+ destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]);
+ }
+ }
+ }
+
+ int16_t readTempData()
+ {
+ uint8_t rawData[2];
+ readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]);
+ return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]);
+ }
+
+ void resetMPU9250()
+ {
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80);
+ wait(0.1);
+ }
+
+ void initAK8963(float *destination)
+ {
+ uint8_t rawData[3];
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00);
+ wait(0.01);
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F);
+ wait(0.01);
+ readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]);
+ destination[0] = (float)(rawData[0] - 128) / 256.0f + 1.0f;
+ destination[1] = (float)(rawData[1] - 128) / 256.0f + 1.0f;
+ destination[2] = (float)(rawData[2] - 128) / 256.0f + 1.0f;
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00);
+ wait(0.01);
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode);
+ wait(0.01);
+ }
+
+ void initMPU9250()
+ {
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00);
+ wait(0.1);
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
+ writeByte(MPU9250_ADDRESS, CONFIG, 0x03);
+ writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04);
+ uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG);
+ writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0xE0);
+ writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0x18);
+ writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c | Gscale << 3);
+
+ c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG);
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0xE0);
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0x18);
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c | Ascale << 3);
+ c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2);
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c & ~0x0F);
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c | 0x03);
+ writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);
+ writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01);
+ }
+// void calibrateMPU9250(float *dest1, float *dest2)
+// {
+// uint8_t data[12];
+// uint16_t ii, packet_count, fifo_count;
+// int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
+//
+// writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80);
+// wait(0.1);
+// writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
+// writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
+// wait(0.2);
+// writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00);
+// writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00);
+// writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00);
+// writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00);
+// writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00);
+// writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C);
+// wait(0.015);
+// writeByte(MPU9250_ADDRESS, CONFIG, 0x01);
+// writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00);
+// writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00);
+// writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00);
+//
+// uint16_t gyrosensitivity = 131;
+// uint16_t accelsensitivity = 16384;
+// writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40);
+// writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78);
+// wait(0.04);
+// writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00);
+// readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]);
+// fifo_count = ((uint16_t)data[0] << 8) | data[1];
+// packet_count = fifo_count / 12;
+//
+// for (ii = 0; ii < packet_count; ii++)
+// {
+// int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
+// readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]);
+// accel_temp[0] = (int16_t)(((int16_t)data[0] << 8) | data[1]);
+// accel_temp[1] = (int16_t)(((int16_t)data[2] << 8) | data[3]);
+// accel_temp[2] = (int16_t)(((int16_t)data[4] << 8) | data[5]);
+// gyro_temp[0] = (int16_t)(((int16_t)data[6] << 8) | data[7]);
+// gyro_temp[1] = (int16_t)(((int16_t)data[8] << 8) | data[9]);
+// gyro_temp[2] = (int16_t)(((int16_t)data[10] << 8) | data[11]);
+//
+// accel_bias[0] += (int32_t)accel_temp[0];
+// accel_bias[1] += (int32_t)accel_temp[1];
+// accel_bias[2] += (int32_t)accel_temp[2];
+// gyro_bias[0] += (int32_t)gyro_temp[0];
+// gyro_bias[1] += (int32_t)gyro_temp[1];
+// gyro_bias[2] += (int32_t)gyro_temp[2];
+// }
+// accel_bias[0] /= (int32_t)packet_count;
+// accel_bias[1] /= (int32_t)packet_count;
+// accel_bias[2] /= (int32_t)packet_count;
+// gyro_bias[0] /= (int32_t)packet_count;
+// gyro_bias[1] /= (int32_t)packet_count;
+// gyro_bias[2] /= (int32_t)packet_count;
+//
+// if (accel_bias[2] > 0L)
+// {
+// accel_bias[2] -= (int32_t)accelsensitivity;
+// }
+// else
+// {
+// accel_bias[2] += (int32_t)accelsensitivity;
+// }
+// data[0] = (-gyro_bias[0] / 4 >> 8) & 0xFF;
+// data[1] = (-gyro_bias[0] / 4) & 0xFF;
+// data[2] = (-gyro_bias[1] / 4 >> 8) & 0xFF;
+// data[3] = (-gyro_bias[1] / 4) & 0xFF;
+// data[4] = (-gyro_bias[2] / 4 >> 8) & 0xFF;
+// data[5] = (-gyro_bias[2] / 4) & 0xFF;
+//
+// dest1[0] = (float)gyro_bias[0] / (float)gyrosensitivity;
+// dest1[1] = (float)gyro_bias[1] / (float)gyrosensitivity;
+// dest1[2] = (float)gyro_bias[2] / (float)gyrosensitivity;
+//
+// int32_t accel_bias_reg[3] = {0, 0, 0};
+// readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]);
+// accel_bias_reg[0] = (int16_t)((int16_t)data[0] << 8) | data[1];
+// readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
+// accel_bias_reg[1] = (int16_t)((int16_t)data[0] << 8) | data[1];
+// readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
+// accel_bias_reg[2] = (int16_t)((int16_t)data[0] << 8) | data[1];
+//
+// uint32_t mask = 1uL;
+// uint8_t mask_bit[3] = {0, 0, 0};
+//
+// for (ii = 0; ii < 3; ii++)
+// {
+// if (accel_bias_reg[ii] & mask)
+// mask_bit[ii] = 0x01;
+// }
+// accel_bias_reg[0] -= (accel_bias[0] / 8);
+// accel_bias_reg[1] -= (accel_bias[1] / 8);
+// accel_bias_reg[2] -= (accel_bias[2] / 8);
+// data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
+// data[1] = (accel_bias_reg[0]) & 0xFF;
+// data[1] = data[1] | mask_bit[0];
+// data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
+// data[3] = (accel_bias_reg[1]) & 0xFF;
+// data[3] = data[3] | mask_bit[1];
+// data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
+// data[5] = (accel_bias_reg[2]) & 0xFF;
+// data[5] = data[5] | mask_bit[2];
+// dest2[0] = (float)accel_bias[0] / (float)accelsensitivity;
+// dest2[1] = (float)accel_bias[1] / (float)accelsensitivity;
+// dest2[2] = (float)accel_bias[2] / (float)accelsensitivity;
+// }
+};
\ No newline at end of file