void HMC5883L::init()
{
writeByte(HMC5883L_ADDRESS, CONFIG_A, 0x78); // Number of samples averaged: 8, Data output rate: 75 Hz
writeByte(HMC5883L_ADDRESS, MODE, 0x01); // Single-Measurement Mode, 160 Hz max output rate
setMagGain(MagGain_13);
wait_ms(10);
}
bool Hybotics_LSM303DLHC_Mag_Unified::begin() {
// Enable I2C
Wire.begin();
// Enable the magnetometer
write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M, 0x00);
// Set the gain to a known level
setMagGain(LSM303_MAGGAIN_1_3);
return true;
}
bool Adafruit_LSM303_Mag::begin()
{
// Enable I2C
Wire.begin();
// Enable the magnetometer
write8(_i2cAddress, LSM303_REGISTER_MAG_MR_REG_M, 0x00);
// Set the gain to a known level
setMagGain(LSM303_MAGGAIN_1_3);
return true;
}
bool Adafruit_HMC5883_Unified::begin()
{
// Enable I2C
Wire.begin();
// Enable the magnetometer
write8(HMC5883_ADDRESS_MAG, HMC5883_REGISTER_MAG_MR_REG_M, 0x00);
// Set the gain to a known level
setMagGain(HMC5883_MAGGAIN_1_9);
return true;
}
bool Adafruit_LSM303_Mag_Unified::begin(lsm303MagGain gain, lsm303MagDataRate dataRate)
{
// Enable I2C
Wire.begin();
// Enable the magnetometer
write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M, 0x00);
setMagGain(gain);
setMagDataRate(dataRate);
return true;
}
bool Adafruit_LSM303_Mag_Unified::begin()
{
// Enable I2C
Wire.begin();
// Enable the magnetometer
write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M, 0x00);
// LSM303DLHC has no WHOAMI register so read CRA_REG_M to check
// the default value (0b00010000/0x10)
uint8_t reg1_a = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M);
if (reg1_a != 0x10)
{
return false;
}
// Set the gain to a known level
setMagGain(LSM303_MAGGAIN_1_3);
return true;
}
void Adafruit_LSM303_Mag_Unified::getEvent(sensors_event_t *event) {
bool readingValid = false;
/* Clear the event */
memset(event, 0, sizeof(sensors_event_t));
while(!readingValid)
{
/* Read new data */
read();
/* Make sure the sensor isn't saturating if auto-ranging is enabled */
if (!_autoRangeEnabled)
{
readingValid = true;
}
else
{
//Serial.print(_magData.x); Serial.print(" ");
//Serial.print(_magData.y); Serial.print(" ");
//Serial.print(_magData.z); Serial.println(" ");
/* Check if the sensor is saturating or not */
if ( (_magData.x >= 4090) | (_magData.x <= -4090) |
(_magData.y >= 4090) | (_magData.y <= -4090) |
(_magData.z >= 4090) | (_magData.z <= -4090) )
{
/* Saturating .... increase the range if we can */
switch(_magGain)
{
case LSM303_MAGGAIN_5_6:
setMagGain(LSM303_MAGGAIN_8_1);
readingValid = false;
//Serial.println("Changing range to +/- 8.1");
break;
case LSM303_MAGGAIN_4_7:
setMagGain(LSM303_MAGGAIN_5_6);
readingValid = false;
//Serial.println("Changing range to +/- 5.6");
break;
case LSM303_MAGGAIN_4_0:
setMagGain(LSM303_MAGGAIN_4_7);
readingValid = false;
//Serial.println("Changing range to +/- 4.7");
break;
case LSM303_MAGGAIN_2_5:
setMagGain(LSM303_MAGGAIN_4_0);
readingValid = false;
//Serial.println("Changing range to +/- 4.0");
break;
case LSM303_MAGGAIN_1_9:
setMagGain(LSM303_MAGGAIN_2_5);
readingValid = false;
//Serial.println("Changing range to +/- 2.5");
break;
case LSM303_MAGGAIN_1_3:
setMagGain(LSM303_MAGGAIN_1_9);
readingValid = false;
//Serial.println("Changing range to +/- 1.9");
break;
default:
readingValid = true;
break;
}
}
else
{
/* All values are withing range */
readingValid = true;
}
}
}
event->version = sizeof(sensors_event_t);
event->sensor_id = _sensorID;
event->type = SENSOR_TYPE_MAGNETIC_FIELD;
event->timestamp = 0;//millis();
event->magnetic.x = _magData.x / _lsm303Mag_Gauss_LSB_XY * SENSORS_GAUSS_TO_MICROTESLA;
event->magnetic.y = _magData.y / _lsm303Mag_Gauss_LSB_XY * SENSORS_GAUSS_TO_MICROTESLA;
event->magnetic.z = _magData.z / _lsm303Mag_Gauss_LSB_Z * SENSORS_GAUSS_TO_MICROTESLA;
}
int8_t LSM303_Mag::readSensor() {
bool readingValid = true;
// Read the magnetometer
uint8_t *buf = (uint8_t *) alloca(10);
memset(buf, 0x00, 10);
i2c.readX(_ADDRESS, LSM303_REGISTER_MAG_OUT_X_H_M | 0x80, 6, buf);
// Note high before low (different than accel)
uint8_t xhi = *(buf + 0);
uint8_t xlo = *(buf + 1);
uint8_t zhi = *(buf + 2);
uint8_t zlo = *(buf + 3);
uint8_t yhi = *(buf + 4);
uint8_t ylo = *(buf + 5);
// Shift values to create properly formed integer (low byte first)
_magData.x = (int16_t)(xlo | ((int16_t)xhi << 8));
_magData.y = (int16_t)(ylo | ((int16_t)yhi << 8));
_magData.z = (int16_t)(zlo | ((int16_t)zhi << 8));
// ToDo: Calculate orientation
_magData.orientation = 0.0;
if ( (_magData.x >= 4090) | (_magData.x <= -4090) |
(_magData.y >= 4090) | (_magData.y <= -4090) |
(_magData.z >= 4090) | (_magData.z <= -4090) ) {
/* Saturating .... increase the range if we can */
switch(_magGain) {
case LSM303_MAGGAIN_5_6:
setMagGain(LSM303_MAGGAIN_8_1);
readingValid = false;
break;
case LSM303_MAGGAIN_4_7:
setMagGain(LSM303_MAGGAIN_5_6);
readingValid = false;
break;
case LSM303_MAGGAIN_4_0:
setMagGain(LSM303_MAGGAIN_4_7);
readingValid = false;
break;
case LSM303_MAGGAIN_2_5:
setMagGain(LSM303_MAGGAIN_4_0);
readingValid = false;
break;
case LSM303_MAGGAIN_1_9:
setMagGain(LSM303_MAGGAIN_2_5);
readingValid = false;
break;
case LSM303_MAGGAIN_1_3:
setMagGain(LSM303_MAGGAIN_1_9);
readingValid = false;
break;
default:
readingValid = true;
break;
}
}
if (readingValid) {
updateDatum(0, _magData.x);
updateDatum(1, _magData.y);
updateDatum(2, _magData.z);
}
return SensorWrapper::SENSOR_ERROR_NO_ERROR;
}
int8_t LSM303_Mag::init() {
i2c.write8(_ADDRESS, LSM303_REGISTER_MAG_MR_REG_M, 0x00); // Enable the magnetometer
setMagGain(LSM303_MAGGAIN_1_3); // Set the gain to a known level
this->sensor_active = true;
return SensorWrapper::SENSOR_ERROR_NO_ERROR;
}
