From f55e79e46aa03023b2dc6c5ab8cefb74bbd438a5 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 21 Sep 2020 17:33:36 +0200 Subject: [PATCH] tici: BMX055 magnetometer& temperature sensor (#2212) * BMX055 Magnetometer support * add temp sensor --- selfdrive/sensord/SConscript | 1 + selfdrive/sensord/sensors/bmx055_accel.hpp | 1 + selfdrive/sensord/sensors/bmx055_magn.cc | 78 ++++++++++++++++++++-- selfdrive/sensord/sensors/bmx055_magn.hpp | 14 ++-- selfdrive/sensord/sensors/bmx055_temp.cc | 44 ++++++++++++ selfdrive/sensord/sensors/bmx055_temp.hpp | 13 ++++ selfdrive/sensord/sensors/constants.hpp | 1 + selfdrive/sensord/sensors_qcom2.cc | 5 +- 8 files changed, 146 insertions(+), 11 deletions(-) create mode 100644 selfdrive/sensord/sensors/bmx055_temp.cc create mode 100644 selfdrive/sensord/sensors/bmx055_temp.hpp diff --git a/selfdrive/sensord/SConscript b/selfdrive/sensord/SConscript index d1bd91357c..b5c8b78f99 100644 --- a/selfdrive/sensord/SConscript +++ b/selfdrive/sensord/SConscript @@ -12,5 +12,6 @@ else: 'sensors/bmx055_accel.cc', 'sensors/bmx055_gyro.cc', 'sensors/bmx055_magn.cc', + 'sensors/bmx055_temp.cc', ] env.Program('_sensord', ['sensors_qcom2.cc'] + sensors, LIBS=[common, cereal, messaging, 'capnp', 'zmq', 'kj']) diff --git a/selfdrive/sensord/sensors/bmx055_accel.hpp b/selfdrive/sensord/sensors/bmx055_accel.hpp index 7bfb97e77d..4e613af7c9 100644 --- a/selfdrive/sensord/sensors/bmx055_accel.hpp +++ b/selfdrive/sensord/sensors/bmx055_accel.hpp @@ -7,6 +7,7 @@ // Registers of the chip #define BMX055_ACCEL_I2C_REG_ID 0x00 +#define BMX055_ACCEL_I2C_REG_TEMP 0x08 #define BMX055_ACCEL_I2C_REG_BW 0x10 #define BMX055_ACCEL_I2C_REG_HBW 0x13 #define BMX055_ACCEL_I2C_REG_FIFO 0x3F diff --git a/selfdrive/sensord/sensors/bmx055_magn.cc b/selfdrive/sensord/sensors/bmx055_magn.cc index 858d190f7f..1c23ceb6a6 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.cc +++ b/selfdrive/sensord/sensors/bmx055_magn.cc @@ -1,12 +1,29 @@ +#include +#include #include #include "common/swaglog.h" #include "bmx055_magn.hpp" +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); +} -BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {} +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); +} + +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; @@ -16,7 +33,7 @@ int BMX055_Magn::init(){ ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01); if(ret < 0){ LOGE("Enabling power failed: %d", ret); - return ret; + goto fail; } usleep(5 * 1000); // wait until the chip is powered on @@ -24,7 +41,7 @@ int BMX055_Magn::init(){ ret = read_register(BMX055_MAGN_I2C_REG_ID, buffer, 1); if(ret < 0){ LOGE("Reading chip ID failed: %d", ret); - return ret; + goto fail; } if(buffer[0] != BMX055_MAGN_CHIP_ID){ @@ -32,12 +49,61 @@ int BMX055_Magn::init(){ return -1; } - // perform self-test + // 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; +} - // sleep -> active (normal, high-precision) +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)); - return 0; + 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}; + kj::ArrayPtr vs(&xyz[0], 3); + + auto svec = event.initMagneticUncalibrated(); + svec.setV(vs); + 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); } diff --git a/selfdrive/sensord/sensors/bmx055_magn.hpp b/selfdrive/sensord/sensors/bmx055_magn.hpp index 6be15a17f1..ba4942d72f 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.hpp +++ b/selfdrive/sensord/sensors/bmx055_magn.hpp @@ -6,16 +6,22 @@ #define BMX055_MAGN_I2C_ADDR 0x10 // Registers of the chip -#define BMX055_MAGN_I2C_REG_ID 0x40 -#define BMX055_MAGN_I2C_REG_PWR_0 0x4B +#define BMX055_MAGN_I2C_REG_ID 0x40 +#define BMX055_MAGN_I2C_REG_PWR_0 0x4B +#define BMX055_MAGN_I2C_REG_MAG 0x4C +#define BMX055_MAGN_I2C_REG_DATAX_LSB 0x42 +#define BMX055_MAGN_I2C_REG_RHALL_LSB 0x48 +#define BMX055_MAGN_I2C_REG_REPXY 0x51 +#define BMX055_MAGN_I2C_REG_REPZ 0x52 // Constants -#define BMX055_MAGN_CHIP_ID 0x32 +#define BMX055_MAGN_CHIP_ID 0x32 +#define BMX055_MAGN_FORCED (0b01 << 1) class BMX055_Magn : public I2CSensor{ uint8_t get_device_address() {return BMX055_MAGN_I2C_ADDR;} public: BMX055_Magn(I2CBus *bus); int init(); - void get_event(cereal::SensorEventData::Builder &event){}; + void get_event(cereal::SensorEventData::Builder &event); }; diff --git a/selfdrive/sensord/sensors/bmx055_temp.cc b/selfdrive/sensord/sensors/bmx055_temp.cc new file mode 100644 index 0000000000..86f7d4f0c1 --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_temp.cc @@ -0,0 +1,44 @@ +#include +#include "common/swaglog.h" +#include "common/timing.h" + +#include "bmx055_temp.hpp" +#include "bmx055_accel.hpp" + + +BMX055_Temp::BMX055_Temp(I2CBus *bus) : I2CSensor(bus) {} + +int BMX055_Temp::init(){ + int ret = 0; + uint8_t buffer[1]; + + ret = read_register(BMX055_ACCEL_I2C_REG_ID, buffer, 1); + if(ret < 0){ + LOGE("Reading chip ID failed: %d", ret); + goto fail; + } + + if(buffer[0] != BMX055_ACCEL_CHIP_ID){ + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_ACCEL_CHIP_ID); + ret = -1; + goto fail; + } + +fail: + return ret; +} + +void BMX055_Temp::get_event(cereal::SensorEventData::Builder &event){ + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[1]; + int len = read_register(BMX055_ACCEL_I2C_REG_TEMP, buffer, sizeof(buffer)); + assert(len == sizeof(buffer)); + + float temp = 23.0f + int8_t(buffer[0]) / 2.0f; + + event.setSource(cereal::SensorEventData::SensorSource::BMX055); + event.setVersion(1); + event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE); + event.setTimestamp(start_time); + event.setTemperature(temp); +} diff --git a/selfdrive/sensord/sensors/bmx055_temp.hpp b/selfdrive/sensord/sensors/bmx055_temp.hpp new file mode 100644 index 0000000000..8b7119a607 --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_temp.hpp @@ -0,0 +1,13 @@ +#pragma once + +#include "sensors/i2c_sensor.hpp" +#include "sensors/bmx055_accel.hpp" + + +class BMX055_Temp : public I2CSensor { + uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;} +public: + BMX055_Temp(I2CBus *bus); + int init(); + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors/constants.hpp b/selfdrive/sensord/sensors/constants.hpp index 69db84edc9..c216f838a5 100644 --- a/selfdrive/sensord/sensors/constants.hpp +++ b/selfdrive/sensord/sensors/constants.hpp @@ -12,6 +12,7 @@ #define SENSOR_TYPE_GEOMAGNETIC_FIELD 2 #define SENSOR_TYPE_GYROSCOPE 4 #define SENSOR_TYPE_LIGHT 5 +#define SENSOR_TYPE_AMBIENT_TEMPERATURE 13 #define SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED 14 #define SENSOR_TYPE_MAGNETIC_FIELD SENSOR_TYPE_GEOMAGNETIC_FIELD #define SENSOR_TYPE_GYROSCOPE_UNCALIBRATED 16 diff --git a/selfdrive/sensord/sensors_qcom2.cc b/selfdrive/sensord/sensors_qcom2.cc index 9821c2b71c..d9da486054 100644 --- a/selfdrive/sensord/sensors_qcom2.cc +++ b/selfdrive/sensord/sensors_qcom2.cc @@ -14,6 +14,7 @@ #include "sensors/bmx055_accel.hpp" #include "sensors/bmx055_gyro.hpp" #include "sensors/bmx055_magn.hpp" +#include "sensors/bmx055_temp.hpp" #include "sensors/light_sensor.hpp" volatile sig_atomic_t do_exit = 0; @@ -38,14 +39,16 @@ int sensor_loop() { BMX055_Accel accel(i2c_bus_imu); BMX055_Gyro gyro(i2c_bus_imu); BMX055_Magn magn(i2c_bus_imu); + BMX055_Temp temp(i2c_bus_imu); LightSensor light("/sys/class/i2c-adapter/i2c-2/2-0038/iio:device1/in_intensity_both_raw"); // Sensor init std::vector sensors; sensors.push_back(&accel); sensors.push_back(&gyro); + sensors.push_back(&magn); + sensors.push_back(&temp); sensors.push_back(&light); - // sensors.push_back(&magn); for (Sensor * sensor : sensors){