MMC5603NJ support (#21564)
* read chip id * set source * untested driver * periodic set * optionalpull/21576/head
parent
f554dc0ae6
commit
8eba02e119
|
@ -14,6 +14,7 @@ else:
|
|||
'sensors/lsm6ds3_accel.cc',
|
||||
'sensors/lsm6ds3_gyro.cc',
|
||||
'sensors/lsm6ds3_temp.cc',
|
||||
'sensors/mmc5603nj_magn.cc',
|
||||
]
|
||||
libs = [common, cereal, messaging, 'capnp', 'zmq', 'kj']
|
||||
if arch == "larch64":
|
||||
|
|
|
@ -10,6 +10,11 @@ int16_t read_16_bit(uint8_t lsb, uint8_t msb) {
|
|||
return int16_t(combined);
|
||||
}
|
||||
|
||||
int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0) {
|
||||
uint32_t combined = (uint32_t(b0) << 16) | (uint32_t(b1) << 8) | uint32_t(b2);
|
||||
return int32_t(combined) / (1 << 4);
|
||||
}
|
||||
|
||||
|
||||
I2CSensor::I2CSensor(I2CBus *bus) : bus(bus) {
|
||||
}
|
||||
|
|
|
@ -9,6 +9,7 @@
|
|||
|
||||
int16_t read_12_bit(uint8_t lsb, uint8_t msb);
|
||||
int16_t read_16_bit(uint8_t lsb, uint8_t msb);
|
||||
int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0);
|
||||
|
||||
|
||||
class I2CSensor : public Sensor {
|
||||
|
|
|
@ -0,0 +1,77 @@
|
|||
#include "mmc5603nj_magn.h"
|
||||
|
||||
#include <cassert>
|
||||
|
||||
#include "selfdrive/common/swaglog.h"
|
||||
#include "selfdrive/common/timing.h"
|
||||
|
||||
MMC5603NJ_Magn::MMC5603NJ_Magn(I2CBus *bus) : I2CSensor(bus) {}
|
||||
|
||||
int MMC5603NJ_Magn::init() {
|
||||
int ret = 0;
|
||||
uint8_t buffer[1];
|
||||
|
||||
ret = read_register(MMC5603NJ_I2C_REG_ID, buffer, 1);
|
||||
if(ret < 0) {
|
||||
LOGE("Reading chip ID failed: %d", ret);
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if(buffer[0] != MMC5603NJ_CHIP_ID) {
|
||||
LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], MMC5603NJ_CHIP_ID);
|
||||
ret = -1;
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// Set 100 Hz
|
||||
ret = set_register(MMC5603NJ_I2C_REG_ODR, 100);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// Set BW to 0b01 for 1-150 Hz operation
|
||||
ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_1, 0b01);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// Set compute measurement rate
|
||||
ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_CMM_FREQ_EN | MMC5603NJ_AUTO_SR_EN);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// Enable continous mode, set every 100 measurements
|
||||
ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_2, MMC5603NJ_CMM_EN | MMC5603NJ_EN_PRD_SET | 0b11);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
fail:
|
||||
return ret;
|
||||
}
|
||||
|
||||
void MMC5603NJ_Magn::get_event(cereal::SensorEventData::Builder &event) {
|
||||
|
||||
uint64_t start_time = nanos_since_boot();
|
||||
uint8_t buffer[9];
|
||||
int len = read_register(MMC5603NJ_I2C_REG_XOUT0, buffer, sizeof(buffer));
|
||||
assert(len == sizeof(buffer));
|
||||
|
||||
float scale = 1.0 / 16384.0;
|
||||
float x = read_20_bit(buffer[6], buffer[1], buffer[0]) * scale;
|
||||
float y = read_20_bit(buffer[7], buffer[3], buffer[2]) * scale;
|
||||
float z = read_20_bit(buffer[8], buffer[5], buffer[4]) * scale;
|
||||
|
||||
event.setSource(cereal::SensorEventData::SensorSource::MMC5603NJ);
|
||||
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);
|
||||
|
||||
}
|
|
@ -0,0 +1,29 @@
|
|||
#pragma once
|
||||
|
||||
#include "selfdrive/sensord/sensors/i2c_sensor.h"
|
||||
|
||||
// Address of the chip on the bus
|
||||
#define MMC5603NJ_I2C_ADDR 0x30
|
||||
|
||||
// Registers of the chip
|
||||
#define MMC5603NJ_I2C_REG_XOUT0 0x00
|
||||
#define MMC5603NJ_I2C_REG_ODR 0x1A
|
||||
#define MMC5603NJ_I2C_REG_INTERNAL_0 0x1B
|
||||
#define MMC5603NJ_I2C_REG_INTERNAL_1 0x1C
|
||||
#define MMC5603NJ_I2C_REG_INTERNAL_2 0x1D
|
||||
#define MMC5603NJ_I2C_REG_ID 0x39
|
||||
|
||||
// Constants
|
||||
#define MMC5603NJ_CHIP_ID 0x10
|
||||
#define MMC5603NJ_CMM_FREQ_EN (1 << 7)
|
||||
#define MMC5603NJ_AUTO_SR_EN (1 << 5)
|
||||
#define MMC5603NJ_CMM_EN (1 << 4)
|
||||
#define MMC5603NJ_EN_PRD_SET (1 << 3)
|
||||
|
||||
class MMC5603NJ_Magn : public I2CSensor {
|
||||
uint8_t get_device_address() {return MMC5603NJ_I2C_ADDR;}
|
||||
public:
|
||||
MMC5603NJ_Magn(I2CBus *bus);
|
||||
int init();
|
||||
void get_event(cereal::SensorEventData::Builder &event);
|
||||
};
|
|
@ -18,6 +18,7 @@
|
|||
#include "selfdrive/sensord/sensors/lsm6ds3_accel.h"
|
||||
#include "selfdrive/sensord/sensors/lsm6ds3_gyro.h"
|
||||
#include "selfdrive/sensord/sensors/lsm6ds3_temp.h"
|
||||
#include "selfdrive/sensord/sensors/mmc5603nj_magn.h"
|
||||
#include "selfdrive/sensord/sensors/sensor.h"
|
||||
|
||||
#define I2C_BUS_IMU 1
|
||||
|
@ -43,27 +44,38 @@ int sensor_loop() {
|
|||
LSM6DS3_Gyro lsm6ds3_gyro(i2c_bus_imu);
|
||||
LSM6DS3_Temp lsm6ds3_temp(i2c_bus_imu);
|
||||
|
||||
MMC5603NJ_Magn mmc5603nj_magn(i2c_bus_imu);
|
||||
|
||||
LightSensor light("/sys/class/i2c-adapter/i2c-2/2-0038/iio:device1/in_intensity_both_raw");
|
||||
|
||||
// Sensor init
|
||||
std::vector<std::pair<Sensor *, bool>> sensors_init; // Sensor, required
|
||||
sensors_init.push_back({&bmx055_accel, true});
|
||||
sensors_init.push_back({&bmx055_gyro, true});
|
||||
sensors_init.push_back({&bmx055_magn, true});
|
||||
sensors_init.push_back({&bmx055_temp, true});
|
||||
|
||||
sensors_init.push_back({&lsm6ds3_accel, true});
|
||||
sensors_init.push_back({&lsm6ds3_gyro, true});
|
||||
sensors_init.push_back({&lsm6ds3_temp, true});
|
||||
|
||||
sensors_init.push_back({&mmc5603nj_magn, false});
|
||||
|
||||
sensors_init.push_back({&light, true});
|
||||
|
||||
|
||||
// Initialize sensors
|
||||
std::vector<Sensor *> sensors;
|
||||
sensors.push_back(&bmx055_accel);
|
||||
sensors.push_back(&bmx055_gyro);
|
||||
sensors.push_back(&bmx055_magn);
|
||||
sensors.push_back(&bmx055_temp);
|
||||
|
||||
sensors.push_back(&lsm6ds3_accel);
|
||||
sensors.push_back(&lsm6ds3_gyro);
|
||||
sensors.push_back(&lsm6ds3_temp);
|
||||
|
||||
sensors.push_back(&light);
|
||||
|
||||
|
||||
for (Sensor * sensor : sensors) {
|
||||
int err = sensor->init();
|
||||
for (auto &sensor : sensors_init) {
|
||||
int err = sensor.first->init();
|
||||
if (err < 0) {
|
||||
LOGE("Error initializing sensors");
|
||||
return -1;
|
||||
// Fail on required sensors
|
||||
if (sensor.second) {
|
||||
LOGE("Error initializing sensors");
|
||||
return -1;
|
||||
}
|
||||
} else {
|
||||
sensors.push_back(sensor.first);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue