You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
244 lines
7.2 KiB
244 lines
7.2 KiB
/* 09/23/2017 Copyright Tlera Corporation |
|
|
|
Created by Kris Winer |
|
|
|
This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board. |
|
The LIS2MDL is a low power magnetometer, here used as 3 DoF in a 9 DoF absolute orientation solution. |
|
|
|
Library may be used freely and without limit with attribution. |
|
|
|
*/ |
|
|
|
#include "LIS2MDL.h" |
|
|
|
LIS2MDL::LIS2MDL(uint8_t intPin) |
|
{ |
|
pinMode(intPin, INPUT); |
|
_intPin = intPin; |
|
} |
|
|
|
|
|
uint8_t LIS2MDL::getChipID() |
|
{ |
|
uint8_t c = readByte(LIS2MDL_ADDRESS, LIS2MDL_WHO_AM_I); |
|
return c; |
|
} |
|
|
|
|
|
void LIS2MDL::reset() |
|
{ |
|
// reset device |
|
uint8_t temp = readByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A); |
|
writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL |
|
delay(1); |
|
writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x40); // Set bit 6 to 1 to boot LIS2MDL |
|
delay(100); // Wait for all registers to reset |
|
} |
|
|
|
void LIS2MDL::init(uint8_t MODR) |
|
{ |
|
|
|
// enable temperature compensation (bit 7 == 1), continuous mode (bits 0:1 == 00) |
|
writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, 0x80 | MODR<<2); |
|
|
|
// enable low pass filter (bit 0 == 1), set to ODR/4 |
|
writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_B, 0x01); |
|
|
|
// enable data ready on interrupt pin (bit 0 == 1), enable block data read (bit 4 == 1) |
|
writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, 0x01 | 0x10); |
|
|
|
} |
|
|
|
|
|
uint8_t LIS2MDL::status() |
|
{ |
|
// Read the status register of the altimeter |
|
uint8_t temp = readByte(LIS2MDL_ADDRESS, LIS2MDL_STATUS_REG); |
|
return temp; |
|
} |
|
|
|
|
|
void LIS2MDL::readData(int16_t * destination) |
|
{ |
|
uint8_t rawData[6]; // x/y/z mag register data stored here |
|
readBytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_OUTX_L_REG), 8, &rawData[0]); // Read the 6 raw data registers into data array |
|
|
|
destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value |
|
destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; |
|
destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; |
|
} |
|
|
|
|
|
int16_t LIS2MDL::readTemperature() |
|
{ |
|
uint8_t rawData[2]; // x/y/z mag register data stored here |
|
readBytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_TEMP_OUT_L_REG), 2, &rawData[0]); // Read the 8 raw data registers into data array |
|
|
|
int16_t temp = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value |
|
return temp; |
|
} |
|
|
|
|
|
void LIS2MDL::offsetBias(float * dest1, float * dest2) |
|
{ |
|
int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0}; |
|
int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0}; |
|
float _mRes = 0.0015f; |
|
|
|
Serial.println("Calculate mag offset bias: move all around to sample the complete response surface!"); |
|
delay(4000); |
|
|
|
for (int ii = 0; ii < 4000; ii++) |
|
{ |
|
readData(mag_temp); |
|
for (int jj = 0; jj < 3; jj++) { |
|
if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj]; |
|
if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj]; |
|
} |
|
delay(12); |
|
} |
|
|
|
_mRes = 0.0015f; // fixed sensitivity |
|
// Get hard iron correction |
|
mag_bias[0] = (mag_max[0] + mag_min[0])/2; // get average x mag bias in counts |
|
mag_bias[1] = (mag_max[1] + mag_min[1])/2; // get average y mag bias in counts |
|
mag_bias[2] = (mag_max[2] + mag_min[2])/2; // get average z mag bias in counts |
|
|
|
dest1[0] = (float) mag_bias[0] * _mRes; // save mag biases in G for main program |
|
dest1[1] = (float) mag_bias[1] * _mRes; |
|
dest1[2] = (float) mag_bias[2] * _mRes; |
|
|
|
// Get soft iron correction estimate |
|
mag_scale[0] = (mag_max[0] - mag_min[0])/2; // get average x axis max chord length in counts |
|
mag_scale[1] = (mag_max[1] - mag_min[1])/2; // get average y axis max chord length in counts |
|
mag_scale[2] = (mag_max[2] - mag_min[2])/2; // get average z axis max chord length in counts |
|
|
|
float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2]; |
|
avg_rad /= 3.0f; |
|
|
|
dest2[0] = avg_rad/((float)mag_scale[0]); |
|
dest2[1] = avg_rad/((float)mag_scale[1]); |
|
dest2[2] = avg_rad/((float)mag_scale[2]); |
|
|
|
Serial.println("Mag Calibration done!"); |
|
} |
|
|
|
void LIS2MDL::selfTest() |
|
{ |
|
int16_t temp[3] = {0, 0, 0}; |
|
float magTest[3] = {0., 0., 0.}; |
|
float magNom[3] = {0., 0., 0.}; |
|
int32_t sum[3] = {0, 0, 0}; |
|
float _mRes = 0.0015f; |
|
|
|
// first, get average response with self test disabled |
|
for (int ii = 0; ii < 50; ii++) |
|
{ |
|
readData(temp); |
|
sum[0] += temp[0]; |
|
sum[1] += temp[1]; |
|
sum[2] += temp[2]; |
|
delay(50); |
|
} |
|
|
|
magNom[0] = (float) sum[0] / 50.0f; |
|
magNom[1] = (float) sum[1] / 50.0f; |
|
magNom[2] = (float) sum[2] / 50.0f; |
|
|
|
uint8_t c = readByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C); |
|
writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c | 0x02); // enable self test |
|
delay(100); // let mag respond |
|
|
|
sum[0] = 0; |
|
sum[1] = 0; |
|
sum[2] = 0; |
|
for (int ii = 0; ii < 50; ii++) |
|
{ |
|
readData(temp); |
|
sum[0] += temp[0]; |
|
sum[1] += temp[1]; |
|
sum[2] += temp[2]; |
|
delay(50); |
|
} |
|
|
|
magTest[0] = (float) sum[0] / 50.0f; |
|
magTest[1] = (float) sum[1] / 50.0f; |
|
magTest[2] = (float) sum[2] / 50.0f; |
|
|
|
writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c); // return to previous settings/normal mode |
|
delay(100); // let mag respond |
|
|
|
Serial.println("Mag Self Test:"); |
|
Serial.print("Mx results:"); Serial.print( (magTest[0] - magNom[0]) * _mRes * 1000.0); Serial.println(" mG"); |
|
Serial.print("My results:"); Serial.println((magTest[0] - magNom[0]) * _mRes * 1000.0); |
|
Serial.print("Mz results:"); Serial.println((magTest[1] - magNom[1]) * _mRes * 1000.0); |
|
Serial.println("Should be between 15 and 500 mG"); |
|
delay(2000); // give some time to read the screen |
|
} |
|
|
|
|
|
// I2C scan function |
|
void LIS2MDL::I2Cscan() |
|
{ |
|
// scan for i2c devices |
|
byte error, address; |
|
int nDevices; |
|
|
|
Serial.println("Scanning..."); |
|
|
|
nDevices = 0; |
|
for(address = 1; address < 127; address++ ) |
|
{ |
|
// The i2c_scanner uses the return value of |
|
// the Write.endTransmission to see if |
|
// a device did acknowledge to the address. |
|
// Wire.beginTransmission(address); |
|
// error = Wire.endTransmission(); |
|
error = Wire.transfer(address, NULL, 0, NULL, 0); |
|
|
|
if (error == 0) |
|
{ |
|
Serial.print("I2C device found at address 0x"); |
|
if (address<16) |
|
Serial.print("0"); |
|
Serial.print(address,HEX); |
|
Serial.println(" !"); |
|
|
|
nDevices++; |
|
} |
|
else if (error==4) |
|
{ |
|
Serial.print("Unknown error at address 0x"); |
|
if (address<16) |
|
Serial.print("0"); |
|
Serial.println(address,HEX); |
|
} |
|
} |
|
if (nDevices == 0) |
|
Serial.println("No I2C devices found\n"); |
|
else |
|
Serial.println("done\n"); |
|
|
|
} |
|
|
|
|
|
// I2C read/write functions for the LIS2MDL |
|
|
|
void LIS2MDL::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) { |
|
uint8_t temp[2]; |
|
temp[0] = subAddress; |
|
temp[1] = data; |
|
Wire.transfer(address, &temp[0], 2, NULL, 0); |
|
} |
|
|
|
|
|
uint8_t LIS2MDL::readByte(uint8_t address, uint8_t subAddress) { |
|
uint8_t temp[1]; |
|
Wire.transfer(address, &subAddress, 1, &temp[0], 1); |
|
return temp[0]; |
|
} |
|
|
|
|
|
void LIS2MDL::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) { |
|
Wire.transfer(address, &subAddress, 1, dest, count); |
|
}
|
|
|