Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Magnetometer.cpp@11:d21275e60ebb, 2015-03-12 (annotated)

Committer:
pvaibhav
Date:
Thu Mar 12 10:16:01 2015 +0000
Revision:
11:d21275e60ebb
Parent:
4:e759b8c756da
Child:
12:1632d7391453
Magnetometer driver implemented including hall-compensation. Reading registers for all sensors uses big-endian sequential read.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pvaibhav 4:e759b8c756da 1 #include "Magnetometer.h"
pvaibhav 4:e759b8c756da 2 #define DEBUG "BMX055-Mag"
pvaibhav 4:e759b8c756da 3 #include "Logger.h"
pvaibhav 4:e759b8c756da 4
pvaibhav 11:d21275e60ebb 5 #include "Vector3.h"
pvaibhav 11:d21275e60ebb 6
pvaibhav 11:d21275e60ebb 7 Magnetometer::Magnetometer(I2C &i2c) : I2CPeripheral(i2c, 0x10 << 1 /* address */), sum(0, 0, 0), hardIron(0,0,0), numSamples(0)
pvaibhav 11:d21275e60ebb 8 {
pvaibhav 11:d21275e60ebb 9 if (powerOn()) {
pvaibhav 11:d21275e60ebb 10 readCalibrationData();
pvaibhav 11:d21275e60ebb 11 INFO("Bosch Sensortec BMX055-Magneto found");
pvaibhav 11:d21275e60ebb 12 powerOff();
pvaibhav 4:e759b8c756da 13 } else {
pvaibhav 11:d21275e60ebb 14 WARN("Bosch Sensortec BMX055-Magneto not found");
pvaibhav 4:e759b8c756da 15 }
pvaibhav 4:e759b8c756da 16 }
pvaibhav 4:e759b8c756da 17
pvaibhav 11:d21275e60ebb 18 bool Magnetometer::powerOn()
pvaibhav 11:d21275e60ebb 19 {
pvaibhav 11:d21275e60ebb 20 write_reg(0x4B, 0x82); // softreset with power off
pvaibhav 11:d21275e60ebb 21 wait_ms(3); // page 18
pvaibhav 11:d21275e60ebb 22 write_reg(0x4B, 0x83); // softreset with power on, this causes full POR. page 134.
pvaibhav 11:d21275e60ebb 23 wait_ms(3);
pvaibhav 11:d21275e60ebb 24 return read_reg(0x40) == 0x32; // verify chip ID
pvaibhav 11:d21275e60ebb 25 }
pvaibhav 11:d21275e60ebb 26
pvaibhav 11:d21275e60ebb 27 void Magnetometer::powerOff()
pvaibhav 11:d21275e60ebb 28 {
pvaibhav 11:d21275e60ebb 29 write_reg(0x4B, 0x82); // softreset and stay powered off
pvaibhav 11:d21275e60ebb 30 LOG("powered off");
pvaibhav 11:d21275e60ebb 31 }
pvaibhav 11:d21275e60ebb 32
pvaibhav 11:d21275e60ebb 33 void Magnetometer::start()
pvaibhav 11:d21275e60ebb 34 {
pvaibhav 11:d21275e60ebb 35 // configure for "high accuracy preset" pg. 122
pvaibhav 11:d21275e60ebb 36 write_reg(0x51, 23); // 2x+1 = 47 sample avg for XY-axis
pvaibhav 11:d21275e60ebb 37 write_reg(0x52, 41); // 2x+1 = 83 sample avg for Z-axis
pvaibhav 11:d21275e60ebb 38 write_reg(0x4C, 0x28); // 20 Hz ODR and normal mode start
pvaibhav 11:d21275e60ebb 39 }
pvaibhav 11:d21275e60ebb 40
pvaibhav 11:d21275e60ebb 41 void Magnetometer::stop()
pvaibhav 11:d21275e60ebb 42 {
pvaibhav 11:d21275e60ebb 43 write_reg(0x4C, 0x06); // 10 Hz but sleep mode
pvaibhav 11:d21275e60ebb 44 }
pvaibhav 11:d21275e60ebb 45
pvaibhav 11:d21275e60ebb 46 Vector3 Magnetometer::read()
pvaibhav 11:d21275e60ebb 47 {
pvaibhav 11:d21275e60ebb 48 // Refer to https://github.com/kriswiner/BMX-055/blob/master/BMX055_MS5637_BasicAHRS_t3.ino#L790
pvaibhav 11:d21275e60ebb 49 union {
pvaibhav 11:d21275e60ebb 50 uint8_t bytes[8];
pvaibhav 11:d21275e60ebb 51 struct {
pvaibhav 11:d21275e60ebb 52 uint16_t hall;
pvaibhav 11:d21275e60ebb 53 int16_t z;
pvaibhav 11:d21275e60ebb 54 int16_t y;
pvaibhav 11:d21275e60ebb 55 int16_t x;
pvaibhav 11:d21275e60ebb 56 } val;
pvaibhav 11:d21275e60ebb 57 } buffer;
pvaibhav 11:d21275e60ebb 58
pvaibhav 11:d21275e60ebb 59 size_t i = 8;
pvaibhav 11:d21275e60ebb 60 while (i --> 0) {
pvaibhav 11:d21275e60ebb 61 buffer.bytes[i] = read_reg(0x42 + i);
pvaibhav 11:d21275e60ebb 62 };
pvaibhav 11:d21275e60ebb 63
pvaibhav 11:d21275e60ebb 64 int16_t mdata_x = buffer.val.x / 8;
pvaibhav 11:d21275e60ebb 65 int16_t mdata_y = buffer.val.y / 8;
pvaibhav 11:d21275e60ebb 66 int16_t mdata_z = buffer.val.z / 2;
pvaibhav 11:d21275e60ebb 67 uint16_t data_r = buffer.val.hall / 4;
pvaibhav 11:d21275e60ebb 68 int16_t temp; // temporary
pvaibhav 11:d21275e60ebb 69 Vector3 magData;
pvaibhav 11:d21275e60ebb 70
pvaibhav 11:d21275e60ebb 71 // calculate temperature compensated 16-bit magnetic fields
pvaibhav 11:d21275e60ebb 72 temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000)));
pvaibhav 11:d21275e60ebb 73
pvaibhav 11:d21275e60ebb 74 magData.x = ((int16_t)((((int32_t)mdata_x) *
pvaibhav 11:d21275e60ebb 75 ((((((((int32_t)dig_xy2) * ((((int32_t)temp) * ((int32_t)temp)) >> 7)) +
pvaibhav 11:d21275e60ebb 76 (((int32_t)temp) * ((int32_t)(((int16_t)dig_xy1) << 7)))) >> 9) +
pvaibhav 11:d21275e60ebb 77 ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_x2) + ((int16_t)0xA0)))) >> 12)) >> 13)) +
pvaibhav 11:d21275e60ebb 78 (((int16_t)dig_x1) << 3);
pvaibhav 11:d21275e60ebb 79
pvaibhav 11:d21275e60ebb 80 temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000)));
pvaibhav 11:d21275e60ebb 81 magData.y = ((int16_t)((((int32_t)mdata_y) *
pvaibhav 11:d21275e60ebb 82 ((((((((int32_t)dig_xy2) * ((((int32_t)temp) * ((int32_t)temp)) >> 7)) +
pvaibhav 11:d21275e60ebb 83 (((int32_t)temp) * ((int32_t)(((int16_t)dig_xy1) << 7)))) >> 9) +
pvaibhav 11:d21275e60ebb 84 ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_y2) + ((int16_t)0xA0)))) >> 12)) >> 13)) +
pvaibhav 11:d21275e60ebb 85 (((int16_t)dig_y1) << 3);
pvaibhav 11:d21275e60ebb 86
pvaibhav 11:d21275e60ebb 87 magData.z = (((((int32_t)(mdata_z - dig_z4)) << 15) - ((((int32_t)dig_z3) * ((int32_t)(((int16_t)data_r) -
pvaibhav 11:d21275e60ebb 88 ((int16_t)dig_xyz1))))>>2))/(dig_z2 + ((int16_t)(((((int32_t)dig_z1) * ((((int16_t)data_r) << 1)))+(1<<15))>>16))));
pvaibhav 11:d21275e60ebb 89
pvaibhav 11:d21275e60ebb 90 return magData;
pvaibhav 11:d21275e60ebb 91 }
pvaibhav 11:d21275e60ebb 92
pvaibhav 11:d21275e60ebb 93 void Magnetometer::readCalibrationData()
pvaibhav 11:d21275e60ebb 94 {
pvaibhav 11:d21275e60ebb 95 // trying to read in serial order of address
pvaibhav 11:d21275e60ebb 96 dig_x1 = read_reg(0x5D);
pvaibhav 11:d21275e60ebb 97 dig_y1 = read_reg(0x5E);
pvaibhav 11:d21275e60ebb 98 dig_z4 = ( int16_t) (read_reg(0x62) | ( int16_t)(read_reg(0x63) << 8));
pvaibhav 11:d21275e60ebb 99 dig_x2 = read_reg(0x64);
pvaibhav 11:d21275e60ebb 100 dig_y2 = read_reg(0x65);
pvaibhav 11:d21275e60ebb 101 dig_z2 = (uint16_t) (read_reg(0x68) | (uint16_t)(read_reg(0x69) << 8));
pvaibhav 11:d21275e60ebb 102 dig_z1 = (uint16_t) (read_reg(0x6A) | (uint16_t)(read_reg(0x6B) << 8));
pvaibhav 11:d21275e60ebb 103 dig_xyz1 = (uint16_t) (read_reg(0x6C) | (uint16_t)(read_reg(0x6D) << 8));
pvaibhav 11:d21275e60ebb 104 dig_z3 = ( int16_t) (read_reg(0x6E) | ( int16_t)(read_reg(0x6F) << 8));
pvaibhav 11:d21275e60ebb 105 dig_xy2 = read_reg(0x70);
pvaibhav 11:d21275e60ebb 106 dig_xy1 = read_reg(0x71);
pvaibhav 11:d21275e60ebb 107 LOG("calibration data loaded");
pvaibhav 11:d21275e60ebb 108 }