diff --git a/system/sensord/sensors/mmc5603nj_magn.cc b/system/sensord/sensors/mmc5603nj_magn.cc index 048095786e..b69c602bdf 100644 --- a/system/sensord/sensors/mmc5603nj_magn.cc +++ b/system/sensord/sensors/mmc5603nj_magn.cc @@ -1,9 +1,12 @@ #include "mmc5603nj_magn.h" +#include #include +#include #include "common/swaglog.h" #include "common/timing.h" +#include "common/util.h" MMC5603NJ_Magn::MMC5603NJ_Magn(I2CBus *bus) : I2CSensor(bus) {} @@ -11,8 +14,8 @@ int MMC5603NJ_Magn::init() { int ret = verify_chip_id(MMC5603NJ_I2C_REG_ID, {MMC5603NJ_CHIP_ID}); if (ret == -1) return -1; - // Set 100 Hz - ret = set_register(MMC5603NJ_I2C_REG_ODR, 100); + // Set ODR to 0 + ret = set_register(MMC5603NJ_I2C_REG_ODR, 0); if (ret < 0) { goto fail; } @@ -23,18 +26,6 @@ int MMC5603NJ_Magn::init() { 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 continuous 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; } @@ -67,16 +58,36 @@ fail: return ret; } -bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) { - uint64_t start_time = nanos_since_boot(); +void MMC5603NJ_Magn::start_measurement() { + set_register(MMC5603NJ_I2C_REG_INTERNAL_0, 0b01); + util::sleep_for(5); +} + +std::vector MMC5603NJ_Magn::read_measurement() { + int len; uint8_t buffer[9]; - int len = read_register(MMC5603NJ_I2C_REG_XOUT0, buffer, sizeof(buffer)); + 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; + float x = (read_20_bit(buffer[6], buffer[1], buffer[0]) * scale) - 32.0; + float y = (read_20_bit(buffer[7], buffer[3], buffer[2]) * scale) - 32.0; + float z = (read_20_bit(buffer[8], buffer[5], buffer[4]) * scale) - 32.0; + std::vector xyz = {x, y, z}; + return xyz; +} + +bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) { + uint64_t start_time = nanos_since_boot(); + // SET - RESET cycle + set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_SET); + util::sleep_for(5); + MMC5603NJ_Magn::start_measurement(); + std::vector xyz = MMC5603NJ_Magn::read_measurement(); + + set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_RESET); + util::sleep_for(5); + MMC5603NJ_Magn::start_measurement(); + std::vector reset_xyz = MMC5603NJ_Magn::read_measurement(); auto event = msg.initEvent().initMagnetometer(); event.setSource(cereal::SensorEventData::SensorSource::MMC5603NJ); @@ -85,10 +96,13 @@ bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) { event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED); event.setTimestamp(start_time); - float xyz[] = {x, y, z}; + float vals[] = {xyz[0], xyz[1], xyz[2], reset_xyz[0], reset_xyz[1], reset_xyz[2]}; + bool valid = true; + if (std::any_of(std::begin(vals), std::end(vals), [](float val) { return val == -32.0; })) { + valid = false; + } auto svec = event.initMagneticUncalibrated(); - svec.setV(xyz); - svec.setStatus(true); - + svec.setV(vals); + svec.setStatus(valid); return true; } diff --git a/system/sensord/sensors/mmc5603nj_magn.h b/system/sensord/sensors/mmc5603nj_magn.h index fce3f3fecb..9c0fbd2521 100644 --- a/system/sensord/sensors/mmc5603nj_magn.h +++ b/system/sensord/sensors/mmc5603nj_magn.h @@ -1,5 +1,7 @@ #pragma once +#include + #include "system/sensord/sensors/i2c_sensor.h" // Address of the chip on the bus @@ -19,9 +21,14 @@ #define MMC5603NJ_AUTO_SR_EN (1 << 5) #define MMC5603NJ_CMM_EN (1 << 4) #define MMC5603NJ_EN_PRD_SET (1 << 3) +#define MMC5603NJ_SET (1 << 3) +#define MMC5603NJ_RESET (1 << 4) class MMC5603NJ_Magn : public I2CSensor { +private: uint8_t get_device_address() {return MMC5603NJ_I2C_ADDR;} + void start_measurement(); + std::vector read_measurement(); public: MMC5603NJ_Magn(I2CBus *bus); int init();