111 lines
2.8 KiB
C++
111 lines
2.8 KiB
C++
#include <cassert>
|
|
#include <cstdio>
|
|
#include <unistd.h>
|
|
|
|
#include "common/swaglog.h"
|
|
#include "common/utilpp.h"
|
|
|
|
#include "bmx055_magn.hpp"
|
|
|
|
static int16_t parse_xy(uint8_t lsb, uint8_t msb){
|
|
// 13 bit
|
|
uint16_t combined = (uint16_t(msb) << 5) | uint16_t(lsb >> 3);
|
|
return int16_t(combined << 3) / (1 << 3);
|
|
}
|
|
|
|
static int16_t parse_z(uint8_t lsb, uint8_t msb){
|
|
// 15 bit
|
|
uint16_t combined = (uint16_t(msb) << 7) | uint16_t(lsb >> 1);
|
|
return int16_t(combined << 1) / (1 << 1);
|
|
}
|
|
|
|
/*
|
|
static uint16_t parse_rhall(uint8_t lsb, uint8_t msb){
|
|
// 14 bit
|
|
return (uint16_t(msb) << 6) | uint16_t(lsb >> 2);
|
|
}
|
|
*/
|
|
|
|
BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {}
|
|
|
|
int BMX055_Magn::init(){
|
|
int ret;
|
|
uint8_t buffer[1];
|
|
|
|
// suspend -> sleep
|
|
ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01);
|
|
if(ret < 0){
|
|
LOGE("Enabling power failed: %d", ret);
|
|
goto fail;
|
|
}
|
|
util::sleep_for(5); // wait until the chip is powered on
|
|
|
|
// read chip ID
|
|
ret = read_register(BMX055_MAGN_I2C_REG_ID, buffer, 1);
|
|
if(ret < 0){
|
|
LOGE("Reading chip ID failed: %d", ret);
|
|
goto fail;
|
|
}
|
|
|
|
if(buffer[0] != BMX055_MAGN_CHIP_ID){
|
|
LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_MAGN_CHIP_ID);
|
|
return -1;
|
|
}
|
|
|
|
// TODO: perform self-test
|
|
|
|
// 9 REPXY and 15 REPZ for 100 Hz
|
|
// 3 REPXY and 3 REPZ for > 300 Hz
|
|
ret = set_register(BMX055_MAGN_I2C_REG_REPXY, (3 - 1) / 2);
|
|
if (ret < 0){
|
|
goto fail;
|
|
}
|
|
|
|
ret = set_register(BMX055_MAGN_I2C_REG_REPZ, 3 - 1);
|
|
if (ret < 0){
|
|
goto fail;
|
|
}
|
|
|
|
return 0;
|
|
|
|
fail:
|
|
return ret;
|
|
}
|
|
|
|
|
|
void BMX055_Magn::get_event(cereal::SensorEventData::Builder &event){
|
|
uint64_t start_time = nanos_since_boot();
|
|
uint8_t buffer[8];
|
|
|
|
int len = read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
|
|
assert(len == sizeof(buffer));
|
|
|
|
bool ready = buffer[6] & 0x1;
|
|
if (ready){
|
|
float x = parse_xy(buffer[0], buffer[1]);
|
|
float y = parse_xy(buffer[2], buffer[3]);
|
|
float z = parse_z(buffer[4], buffer[5]);
|
|
//uint16_t rhall = parse_rhall(buffer[5], buffer[6]);
|
|
|
|
// TODO: convert to micro tesla:
|
|
// https://github.com/BoschSensortec/BMM150-Sensor-API/blob/master/bmm150.c#L1614
|
|
|
|
event.setSource(cereal::SensorEventData::SensorSource::BMX055);
|
|
event.setVersion(1);
|
|
event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
|
|
event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
|
|
event.setTimestamp(start_time);
|
|
|
|
float xyz[] = {x, y, z};
|
|
auto svec = event.initMagneticUncalibrated();
|
|
svec.setV(xyz);
|
|
svec.setStatus(true);
|
|
}
|
|
|
|
// The BMX055 Magnetometer has no FIFO mode. Self running mode only goes
|
|
// up to 30 Hz. Therefore we put in forced mode, and request measurements
|
|
// at a 100 Hz. When reading the registers we have to check the ready bit
|
|
// To verify the measurement was comleted this cycle.
|
|
set_register(BMX055_MAGN_I2C_REG_MAG, BMX055_MAGN_FORCED);
|
|
}
|