diff --git a/CMakeLists.txt b/CMakeLists.txt index 5fc5caa5..cbd08c5e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -196,9 +196,15 @@ endif() if(CONFIG_SENSOR_DRV_LIS3MDL) target_sources(app PRIVATE src/sensor/mag/LIS3MDL.c) endif() +if(CONFIG_SENSOR_DRV_MMC5603NJ) + target_sources(app PRIVATE src/sensor/mag/MMC5603NJ.c) +endif() if(CONFIG_SENSOR_DRV_MMC5983MA) target_sources(app PRIVATE src/sensor/mag/MMC5983MA.c) endif() +if(CONFIG_SENSOR_DRV_QMC5883L) + target_sources(app PRIVATE src/sensor/mag/QMC5883L.c) +endif() if(CONFIG_SENSOR_DRV_QMC6309) target_sources(app PRIVATE src/sensor/mag/QMC6309.c) endif() diff --git a/Kconfig.sensors_drivers b/Kconfig.sensors_drivers index c5610b27..b46564ee 100644 --- a/Kconfig.sensors_drivers +++ b/Kconfig.sensors_drivers @@ -99,11 +99,21 @@ config SENSOR_DRV_LIS3MDL default y if !SENSOR_MAG_DRIVERS_MINIMAL default n +config SENSOR_DRV_MMC5603NJ + bool "MMC5603NJ" + default y if !SENSOR_MAG_DRIVERS_MINIMAL + default n + config SENSOR_DRV_MMC5983MA bool "MMC5983MA" default y if !SENSOR_MAG_DRIVERS_MINIMAL default n +config SENSOR_DRV_QMC5883L + bool "QMC5883L" + default y if !SENSOR_MAG_DRIVERS_MINIMAL + default n + config SENSOR_DRV_QMC6309 bool "QMC6309" default y if !SENSOR_MAG_DRIVERS_MINIMAL diff --git a/src/sensor/interface.c b/src/sensor/interface.c index 61bf9c84..ca1f1abf 100644 --- a/src/sensor/interface.c +++ b/src/sensor/interface.c @@ -152,6 +152,13 @@ const sensor_ext_ssi_t *sensor_interface_ext_get(void) return ext_ssi; } +enum sensor_interface_spec sensor_interface_get_spec(enum sensor_interface_dev dev) +{ + if (dev < 0 || dev >= SENSOR_INTERFACE_DEV_COUNT) + return SENSOR_INTERFACE_SPEC_SPI; // safe default + return sensor_interface_dev_spec[dev]; +} + // TODO: spi config by device int ssi_write(enum sensor_interface_dev dev, const uint8_t *buf, uint32_t num_bytes) diff --git a/src/sensor/interface.h b/src/sensor/interface.h index 7f748988..126818e1 100644 --- a/src/sensor/interface.h +++ b/src/sensor/interface.h @@ -60,6 +60,7 @@ bool sensor_interface_imu_is_i2c(void); int sensor_interface_spi_configure(enum sensor_interface_dev dev, uint32_t frequency, uint32_t dummy_reads); void sensor_interface_ext_configure(const sensor_ext_ssi_t *ext); const sensor_ext_ssi_t *sensor_interface_ext_get(void); +enum sensor_interface_spec sensor_interface_get_spec(enum sensor_interface_dev dev); int ssi_write(enum sensor_interface_dev dev, const uint8_t *buf, uint32_t num_bytes); int ssi_read(enum sensor_interface_dev dev, uint8_t *buf, uint32_t num_bytes); diff --git a/src/sensor/mag/LIS2MDL.c b/src/sensor/mag/LIS2MDL.c index 0b006aa1..33cbf881 100644 --- a/src/sensor/mag/LIS2MDL.c +++ b/src/sensor/mag/LIS2MDL.c @@ -15,6 +15,16 @@ static uint8_t last_cfg_a = 0xff; LOG_MODULE_REGISTER(LIS2MDL, LOG_LEVEL_DBG); +// CFG_REG_C: BDU always set; 4WSPI + I2C_DIS only in SPI mode +// (I2C mode must not set I2C_DIS or it cuts off the bus) +static uint8_t lis2_cfg_c(void) +{ + uint8_t cfg_c = CFG_C_BDU; + if (sensor_interface_get_spec(SENSOR_INTERFACE_DEV_MAG) == SENSOR_INTERFACE_SPEC_SPI) + cfg_c |= CFG_C_4WSPI | CFG_C_I2C_DIS; + return cfg_c; +} + static int lis2_soft_reset(void) { int err = ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, LIS2MDL_CFG_REG_A, CFG_A_SOFT_RST); @@ -48,7 +58,14 @@ int lis2_init(float time, float *actual_time) return err; } - err = ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, LIS2MDL_CFG_REG_C, CFG_C_BDU); + err = ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, LIS2MDL_CFG_REG_C, lis2_cfg_c()); + if (err) { + LOG_ERR("Communication error"); + return err; + } + + // CFG_REG_B: LPF (ODR/4 bandwidth) + OFF_CANC (internal bias cancellation) + err = ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, LIS2MDL_CFG_REG_B, CFG_B_LPF | CFG_B_OFF_CANC); if (err) { LOG_ERR("Communication error"); return err; @@ -135,7 +152,7 @@ int lis2_update_odr(float time, float *actual_time) int err = 0; if (MD == MD_CONTINUOUS) { - err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, LIS2MDL_CFG_REG_C, CFG_C_BDU); + err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, LIS2MDL_CFG_REG_C, lis2_cfg_c()); } err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, LIS2MDL_CFG_REG_A, cfg_a); diff --git a/src/sensor/mag/LIS2MDL.h b/src/sensor/mag/LIS2MDL.h index d7aa5748..b5f620e8 100644 --- a/src/sensor/mag/LIS2MDL.h +++ b/src/sensor/mag/LIS2MDL.h @@ -14,7 +14,12 @@ #define CFG_A_COMP_TEMP_EN 0x80 #define CFG_A_SOFT_RST 0x20 -#define CFG_C_BDU 0x10 +#define CFG_B_LPF 0x01 // low-pass filter (BW ODR/4 instead of ODR/2) +#define CFG_B_OFF_CANC 0x02 // offset cancellation (internal set/reset bias cancel) + +#define CFG_C_I2C_DIS 0x20 // disable I2C interface (SPI only) +#define CFG_C_BDU 0x10 +#define CFG_C_4WSPI 0x04 // enable 4-wire SPI (SDO on pin 7) #define STATUS_ZYXDA 0x08 diff --git a/src/sensor/mag/MMC5603NJ.c b/src/sensor/mag/MMC5603NJ.c new file mode 100644 index 00000000..b30ba057 --- /dev/null +++ b/src/sensor/mag/MMC5603NJ.c @@ -0,0 +1,257 @@ +#include + +#include + +#include "MMC5603NJ.h" + +static const float sensitivity = (1.0f / 16384.0f); // mag sensitivity if using 20 bit data (16384 Counts/G) +static const float offset = 524288.0f; // mag range unsigned to signed + +static uint16_t last_state = 0xffff; +static float last_time = 0; +static uint8_t last_rawTemp = 0xff; +static int64_t oneshot_trigger_time = 0; +static bool auto_set_reset = true; + +LOG_MODULE_REGISTER(MMC5603NJ, LOG_LEVEL_DBG); + +static void mmc5603_SET(void); +static void mmc5603_RESET(void); + +int mmc5603_init(float time, float *actual_time) +{ + // moved here, it is mmc specific + mmc5603_SET(); // "deGauss" magnetometer + + // enable auto set/reset + int err = ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, MMC5603NJ_CONTROL_0, MCTRL0_AUTO_SR_EN); + if (err) + LOG_ERR("Communication error"); + + last_state = 0xffff; // reset last state + err |= mmc5603_update_odr(time, actual_time); + return (err < 0 ? err : 0); +} + +void mmc5603_shutdown(void) +{ + // reset device + last_state = 0xffff; // reset last state + int err = ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, MMC5603NJ_CONTROL_1, MCTRL1_SW_RESET); // Don't need to wait for MMC to finish reset + if (err) + LOG_ERR("Communication error"); +} + +int mmc5603_update_odr(float time, float *actual_time) +{ + int ODR; + uint8_t MODR; + uint8_t MBW; + uint8_t HPOWER = 0; + uint8_t MSET = MSET_2000; // always use lowest SET/RESET interval + last_time = time; + + if (time <= 0 || time == INFINITY) // off interpreted as oneshot + ODR = 0; + else + ODR = 1 / time; + + if (ODR > 255) + { // 1000Hz*1.2ms/1000ms = 120% active, hpower required + MODR = 255; + MBW = MBW_1_2ms; + HPOWER = MCTRL2_HPOWER; + time = 1.0 / 1000; + } + else if (ODR > 150) // Nominal working state, this should use as low power as possible + { // 255Hz*2ms/1000ms = 51% active + MODR = 255; + MBW = MBW_2_0ms; + time = 1.0 / 255; + } + else if (ODR > 75) + { // 150Hz*3.5ms/1000ms = 52% active + MODR = 150; + MBW = MBW_3_5ms; + time = 1.0 / 150; + } + else if (ODR > 50) + { // 75Hz*6.6ms/1000ms = 50% active + MODR = 75; + MBW = MBW_6_6ms; // 1.5mG RMS noise + time = 1.0 / 75; + } + else if (ODR > 20) + { // 50Hz*6.6ms/1000ms = 33% active + MODR = 50; + MBW = MBW_6_6ms; + time = 1.0 / 50; + } + else if (ODR > 10) + { // 20Hz*6.6ms/1000ms = 13% active + MODR = 20; + MBW = MBW_6_6ms; + time = 1.0 / 20; + } + else if (ODR > 1) + { // 10Hz*6.6ms/1000ms = 7% active + MODR = 10; + MBW = MBW_6_6ms; + time = 1.0 / 10; + } + else if (ODR > 0) + { // 1Hz*6.6ms/1000ms = 0.7% active + MODR = 1; + MBW = MBW_6_6ms; + time = 1.0 / 1; + } + else + { // oneshot uses the fastest measurement time + MODR = 0; + MBW = MBW_1_2ms; + time = INFINITY; + } + + uint16_t state = MODR | (HPOWER << 1); // HPOWER is bit 7, shift it out of MODR byte range + if (last_state == state) { + *actual_time = time; + return 0; /* already configured — success for err|= callers */ + } + + // set magnetometer bandwidth + int err = ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, MMC5603NJ_CONTROL_1, MBW); + + // set sample rate, then start the measurement period calculation + err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, MMC5603NJ_ODR, MODR); + err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, MMC5603NJ_CONTROL_0, MCTRL0_AUTO_SR_EN | (MODR ? MCTRL0_CMM_FREQ_EN : 0)); + + // enable continuous measurement mode if ODR is set + // enable periodic SET, set set/reset rate + err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, MMC5603NJ_CONTROL_2, HPOWER | (MODR ? (MCTRL2_CMM_EN | MCTRL2_EN_PRD_SET | MSET) : 0)); + if (err) { + LOG_ERR("Communication error"); + return err; + } + + last_state = state; + *actual_time = time; + return 0; +} + +void mmc5603_mag_oneshot(void) +{ + // enable auto set/reset and trigger oneshot + int err = ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, MMC5603NJ_CONTROL_0, (auto_set_reset ? MCTRL0_AUTO_SR_EN : 0) | MCTRL0_TAKE_MEAS_M); + oneshot_trigger_time = k_uptime_get(); + if (err) + LOG_ERR("Communication error"); +} + +bool mmc5603_mag_read(float m[3]) +{ + int err = 0; + uint8_t status = oneshot_trigger_time ? 0x00 : MSTAT_MEAS_M_DONE; + int64_t timeout = oneshot_trigger_time + 4; // 4ms timeout (1.2ms measurement at BW=11) + if (k_uptime_get() >= timeout) // already passed timeout + oneshot_trigger_time = 0; + while ((~status & MSTAT_MEAS_M_DONE) && k_uptime_get() < timeout) // wait for oneshot to complete or timeout + err |= ssi_reg_read_byte(SENSOR_INTERFACE_DEV_MAG, MMC5603NJ_STATUS, &status); + if (oneshot_trigger_time ? k_uptime_get() >= timeout : false) + LOG_ERR("Read timeout"); + oneshot_trigger_time = 0; + uint8_t rawData[9]; // x/y/z mag register data stored here + err |= ssi_burst_read(SENSOR_INTERFACE_DEV_MAG, MMC5603NJ_XOUT_0, &rawData[0], 9); // Read the 9 raw data registers into data array + if (err) + { + LOG_ERR("Communication error"); + return false; + } + mmc5603_mag_process(rawData, m); + return true; +} + +// MMC must trigger the measurement, which will take significant time +// instead, the temperature is read from the last measurement and then another measurement is immediately triggered +float mmc5603_temp_read(float bias[3]) +{ + uint8_t rawTemp; + int err = ssi_reg_read_byte(SENSOR_INTERFACE_DEV_MAG, MMC5603NJ_TOUT, &rawTemp); + // Temperature output, unsigned format. The range is -75~125°C, about 0.8°C/LSB, 00000000 stands for -75°C + float temp = rawTemp; + temp *= 0.8f; + temp -= 75; + + // USING SET AND RESET TO REMOVE BRIDGE OFFSET in datasheet + if (last_rawTemp != rawTemp && last_time > 1.0f / 50) // calculate offset at low motion only + { // TODO: does the temp register have hysteresis? + float mPos[3], mNeg[3]; + float actual_time; + + last_rawTemp = rawTemp; + for (int i = 0; i < 3; i++) // clear stored offset + bias[i] = 0; + + err |= mmc5603_update_odr(INFINITY, &actual_time); // set oneshot mode + auto_set_reset = false; // disable auto set/reset + + mmc5603_RESET(); + mmc5603_mag_oneshot(); + mmc5603_mag_read(mNeg); + + mmc5603_SET(); + mmc5603_mag_oneshot(); + mmc5603_mag_read(mPos); + + for (int i = 0; i < 3; i++) // store bridge offset for future readings + bias[i] = (mPos[i] + mNeg[i]) / 2; // ((+H + Offset) + (-H + Offset)) / 2 + + auto_set_reset = true; // enable auto set/reset + err |= mmc5603_update_odr(last_time, &actual_time); // reset odr + } + + // enable auto set/reset and trigger measurement + err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, MMC5603NJ_CONTROL_0, MCTRL0_AUTO_SR_EN | MCTRL0_TAKE_MEAS_T); + if (err < 0) + LOG_ERR("Communication error"); + return temp; +} + +void mmc5603_mag_process(uint8_t *raw_m, float m[3]) +{ + uint32_t rawMag[3]; + rawMag[0] = (uint32_t)(raw_m[0] << 12 | raw_m[1] << 4 | (raw_m[6] & 0xF0) >> 4); // Turn the 20 bits into a unsigned 32-bit value + rawMag[1] = (uint32_t)(raw_m[2] << 12 | raw_m[3] << 4 | (raw_m[7] & 0xF0) >> 4); // Turn the 20 bits into a unsigned 32-bit value + rawMag[2] = (uint32_t)(raw_m[4] << 12 | raw_m[5] << 4 | (raw_m[8] & 0xF0) >> 4); // Turn the 20 bits into a unsigned 32-bit value + for (int i = 0; i < 3; i++) // x, y, z + m[i] = ((float)rawMag[i] - offset) * sensitivity; +} + +static void mmc5603_SET(void) +{ + int err = ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, MMC5603NJ_CONTROL_0, MCTRL0_DO_SET); + if (err) + LOG_ERR("Communication error"); + k_busy_wait(1000); // self clearing after 375 ns, t_SR 1ms before other operations +} + +static void mmc5603_RESET(void) +{ + int err = ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, MMC5603NJ_CONTROL_0, MCTRL0_DO_RESET); + if (err) + LOG_ERR("Communication error"); + k_busy_wait(1000); // self clearing after 375 ns, t_SR 1ms before other operations +} + +const sensor_mag_t sensor_mag_mmc5603nj = { + *mmc5603_init, + *mmc5603_shutdown, + + *mmc5603_update_odr, + + *mmc5603_mag_oneshot, + *mmc5603_mag_read, + *mmc5603_temp_read, + + *mmc5603_mag_process, + 6, 9 // if only reading 6 bytes, the data will be lower precision +}; diff --git a/src/sensor/mag/MMC5603NJ.h b/src/sensor/mag/MMC5603NJ.h new file mode 100644 index 00000000..b9038d7c --- /dev/null +++ b/src/sensor/mag/MMC5603NJ.h @@ -0,0 +1,66 @@ +#ifndef MMC5603NJ_h +#define MMC5603NJ_h + +#include "sensor/sensor.h" + +// https://www.memsic.com/Public/Uploads/uploadfile/files/20220119/MMC5603NJDatasheetRev.B.pdf +#define MMC5603NJ_XOUT_0 0x00 +#define MMC5603NJ_TOUT 0x09 + +#define MMC5603NJ_STATUS 0x18 +#define MMC5603NJ_ODR 0x1A +#define MMC5603NJ_CONTROL_0 0x1B +#define MMC5603NJ_CONTROL_1 0x1C +#define MMC5603NJ_CONTROL_2 0x1D +#define MMC5603NJ_PRODUCT_ID 0x39 + +// Status1 bits +#define MSTAT_MEAS_M_DONE 0x40 +#define MSTAT_MEAS_T_DONE 0x80 + +// Control 0 bits +#define MCTRL0_TAKE_MEAS_M 0x01 +#define MCTRL0_TAKE_MEAS_T 0x02 +#define MCTRL0_DO_SET 0x08 +#define MCTRL0_DO_RESET 0x10 +#define MCTRL0_AUTO_SR_EN 0x20 +#define MCTRL0_CMM_FREQ_EN 0x80 + +// Control 1 bits +#define MCTRL1_SW_RESET 0x80 + +// Bandwidths (measurement time) +#define MBW_6_6ms 0x00 // 75Hz max ODR with auto SET/RESET +#define MBW_3_5ms 0x01 // 150Hz +#define MBW_2_0ms 0x02 // 255Hz +#define MBW_1_2ms 0x03 // 255Hz, 1000Hz with hpower + +// Control 2 bits +#define MCTRL2_EN_PRD_SET 0x08 +#define MCTRL2_CMM_EN 0x10 +#define MCTRL2_HPOWER 0x80 + +// Set/Reset as a function of measurements +#define MSET_1 0x00 // Set/Reset each data measurement +#define MSET_25 0x01 // each 25 data measurements +#define MSET_75 0x02 +#define MSET_100 0x03 +#define MSET_250 0x04 +#define MSET_500 0x05 +#define MSET_1000 0x06 +#define MSET_2000 0x07 + +int mmc5603_init(float time, float *actual_time); +void mmc5603_shutdown(void); + +int mmc5603_update_odr(float time, float *actual_time); + +void mmc5603_mag_oneshot(void); +bool mmc5603_mag_read(float m[3]); +float mmc5603_temp_read(float bias[3]); + +void mmc5603_mag_process(uint8_t *raw_m, float m[3]); + +extern const sensor_mag_t sensor_mag_mmc5603nj; + +#endif diff --git a/src/sensor/mag/QMC5883L.c b/src/sensor/mag/QMC5883L.c new file mode 100644 index 00000000..d45283d5 --- /dev/null +++ b/src/sensor/mag/QMC5883L.c @@ -0,0 +1,244 @@ +#include +#include + +#include + +#include "QMC5883L.h" +#include "sensor/sensor_none.h" + +// QMC5883L 3-Axis Magnetic Sensor +// I2C address 0x0D, Chip ID register 0x0D = 0xFF + +#define QMC5883L_XOUT_L 0x00 + +#define QMC5883L_STATUS 0x06 +#define QMC5883L_STATUS_DRDY 0x01 +#define QMC5883L_STATUS_OVL 0x02 +#define QMC5883L_STATUS_DOR 0x04 + +#define QMC5883L_TOUT_L 0x07 + +#define QMC5883L_CTRL_1 0x09 +#define QMC5883L_CTRL_2 0x0A +#define QMC5883L_SET_RST 0x0B +#define QMC5883L_CHIP_ID 0x0D + +#define QMC5883L_MODE_STANDBY 0x00 +#define QMC5883L_MODE_CONTINUOUS 0x01 + +#define QMC5883L_ODR_10HZ 0x00 +#define QMC5883L_ODR_50HZ 0x01 +#define QMC5883L_ODR_100HZ 0x02 +#define QMC5883L_ODR_200HZ 0x03 + +#define QMC5883L_ODR_10Hz QMC5883L_ODR_10HZ +#define QMC5883L_ODR_50Hz QMC5883L_ODR_50HZ +#define QMC5883L_ODR_100Hz QMC5883L_ODR_100HZ +#define QMC5883L_ODR_200Hz QMC5883L_ODR_200HZ + +#define QMC5883L_RNG_2G 0x00 +#define QMC5883L_RNG_8G 0x01 + +#define QMC5883L_OSR_512 0x00 +#define QMC5883L_OSR_256 0x01 +#define QMC5883L_OSR_128 0x02 +#define QMC5883L_OSR_64 0x03 + +#define QMC5883L_SOFT_RST 0x80 +#define QMC5883L_ROL_PNT 0x40 +#define QMC5883L_INT_ENB 0x01 + +static const float sensitivity = 1.0f / 3000.0f; // 3000 LSB/G at ±8G range + +static uint8_t last_odr = 0xff; +static bool lastOvfl = false; +static int64_t oneshot_trigger_time = 0; +static uint8_t last_rawData[6]; +static bool last_rawData_valid = false; +static int64_t last_mag_time_ms; +static int32_t mag_period_ms = 50; // default 10Hz + +LOG_MODULE_REGISTER(QMC5883L, LOG_LEVEL_INF); + +int qmc5883l_init(float time, float *actual_time) +{ + last_odr = 0xff; + lastOvfl = false; + oneshot_trigger_time = 0; + last_rawData_valid = false; + last_mag_time_ms = 0; + + int err = ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, QMC5883L_SET_RST, 0x01); + if (err) + LOG_ERR("Communication error"); + + err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, QMC5883L_CTRL_2, QMC5883L_ROL_PNT | QMC5883L_INT_ENB); + if (err) + LOG_ERR("Communication error"); + + err |= qmc5883l_update_odr(time, actual_time); + return (err < 0 ? err : 0); +} + +void qmc5883l_shutdown(void) +{ + int err = ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, QMC5883L_CTRL_1, QMC5883L_MODE_STANDBY); + if (err) + LOG_ERR("Communication error"); +} + +int qmc5883l_update_odr(float time, float *actual_time) +{ + int ODR; + uint8_t MODR; + uint8_t MODE; + + if (time <= 0 || time == INFINITY) + { + MODE = QMC5883L_MODE_STANDBY; + ODR = 0; + } + else + { + MODE = QMC5883L_MODE_CONTINUOUS; + ODR = 1 / time; + } + + if (MODE == QMC5883L_MODE_STANDBY) + { + MODR = QMC5883L_ODR_200Hz; + time = INFINITY; + } + else if (ODR > 100) + { + MODR = QMC5883L_ODR_200Hz; + time = 1.0f / 200; + } + else if (ODR > 50) + { + MODR = QMC5883L_ODR_100Hz; + time = 1.0f / 100; + } + else if (ODR > 10) + { + MODR = QMC5883L_ODR_50Hz; + time = 1.0f / 50; + } + else + { + MODR = QMC5883L_ODR_10Hz; + time = 1.0f / 10; + } + + uint8_t STAT = (QMC5883L_OSR_512 << 6) | (QMC5883L_RNG_8G << 4) | (MODR << 2) | MODE; + if (last_odr == STAT) { + *actual_time = time; + return 0; /* already configured — success for err|= callers */ + } + + int err; + if (MODE == QMC5883L_MODE_STANDBY) + err = ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, QMC5883L_CTRL_1, QMC5883L_MODE_STANDBY); + else + err = ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, QMC5883L_CTRL_1, STAT); + if (err) { + LOG_ERR("Communication error"); + return err; + } + + last_odr = STAT; + oneshot_trigger_time = 0; + + if (MODE != QMC5883L_MODE_STANDBY) + mag_period_ms = (int32_t)(time * 1000); + + *actual_time = time; + return 0; +} + +void qmc5883l_mag_oneshot(void) +{ + int err = ssi_reg_write_byte(SENSOR_INTERFACE_DEV_MAG, QMC5883L_CTRL_1, (QMC5883L_OSR_512 << 6) | (QMC5883L_RNG_8G << 4) | (QMC5883L_ODR_200Hz << 2) | QMC5883L_MODE_CONTINUOUS); + oneshot_trigger_time = k_uptime_get(); + if (err) + LOG_ERR("Communication error"); +} + +bool qmc5883l_mag_read(float m[3]) +{ + int err = 0; + if (oneshot_trigger_time) + { + uint8_t status = 0; + int64_t timeout = oneshot_trigger_time + 10; + while ((status & QMC5883L_STATUS_DRDY) == 0) + { + err |= ssi_reg_read_byte(SENSOR_INTERFACE_DEV_MAG, QMC5883L_STATUS, &status); + if (k_uptime_get() > timeout) + { + LOG_WRN("Data ready status timeout!"); + break; + } + } + oneshot_trigger_time = 0; + if (err) + { + LOG_ERR("Communication error"); + return false; + } + if ((status & QMC5883L_STATUS_OVL) && !lastOvfl) + { + LOG_INF("Magnetometer overflow"); + lastOvfl = true; + } + else + { + lastOvfl = false; + } + } + uint8_t rawData[6]; + err |= ssi_burst_read(SENSOR_INTERFACE_DEV_MAG, QMC5883L_XOUT_L, rawData, 6); + if (err) + LOG_ERR("Communication error"); + + int64_t now = k_uptime_get(); + if (last_rawData_valid && memcmp(rawData, last_rawData, 6) == 0) + { + if ((now - last_mag_time_ms) < (mag_period_ms + mag_period_ms / 10)) + return false; + last_mag_time_ms += mag_period_ms; + if (now - last_mag_time_ms > mag_period_ms * 2) + last_mag_time_ms = now; + } + else + { + last_mag_time_ms = now; + } + memcpy(last_rawData, rawData, 6); + last_rawData_valid = true; + qmc5883l_mag_process(rawData, m); + return true; +} + +void qmc5883l_mag_process(uint8_t *raw_m, float m[3]) +{ + for (int i = 0; i < 3; i++) + { + m[i] = ((int16_t *)raw_m)[i]; + m[i] *= sensitivity; + } +} + +const sensor_mag_t sensor_mag_qmc5883l = { + qmc5883l_init, + qmc5883l_shutdown, + + qmc5883l_update_odr, + + qmc5883l_mag_oneshot, + qmc5883l_mag_read, + mag_none_temp_read, + + qmc5883l_mag_process, + 6, 6 +}; diff --git a/src/sensor/mag/QMC5883L.h b/src/sensor/mag/QMC5883L.h new file mode 100644 index 00000000..f269f082 --- /dev/null +++ b/src/sensor/mag/QMC5883L.h @@ -0,0 +1,18 @@ +#ifndef QMC5883L_h +#define QMC5883L_h + +#include "sensor/sensor.h" + +int qmc5883l_init(float time, float *actual_time); +void qmc5883l_shutdown(void); + +int qmc5883l_update_odr(float time, float *actual_time); + +void qmc5883l_mag_oneshot(void); +bool qmc5883l_mag_read(float m[3]); + +void qmc5883l_mag_process(uint8_t *raw_m, float m[3]); + +extern const sensor_mag_t sensor_mag_qmc5883l; + +#endif diff --git a/src/sensor/scan_spi.c b/src/sensor/scan_spi.c index 24fcdd96..37aedbb2 100644 --- a/src/sensor/scan_spi.c +++ b/src/sensor/scan_spi.c @@ -66,6 +66,12 @@ int sensor_scan_spi(struct spi_dt_spec *bus, uint8_t *spi_dev_reg, int dev_addr_ continue; for (int l = 0; l < id_cnt; l++) { + // 0xFF is a "no real WHO_AM_I" sentinel (e.g. QMC5883L, an + // I2C-only part). On SPI a reserved-register read floats to + // 0xFF, so this would false-match and shadow real SPI mags + // later in the table (e.g. LIS2MDL at 0x4F). Skip it on SPI. + if (dev_id[id_ind + l] == 0xFF) + continue; if (id == dev_id[id_ind + l]) { *spi_dev_reg = reg; diff --git a/src/sensor/sensor.c b/src/sensor/sensor.c index bb8877cf..c0fdf122 100644 --- a/src/sensor/sensor.c +++ b/src/sensor/sensor.c @@ -784,6 +784,21 @@ int sensor_scan(void) sensor_mag_spi_dev.config.frequency = MHZ(10); LOG_INF("Scanning SPI bus for magnetometer"); mag_id = sensor_scan_mag_spi(&sensor_mag_spi_dev, &sensor_mag_dev_reg); + if (mag_id < 0) { + // ST magnetometers (e.g. LIS2MDL) boot in 3-wire SPI: SDO stays high-Z so 4-wire + // reads return 0x00 and the scan above fails. Blind-write CFG_REG_C (0x62) with + // 4WSPI=bit2 (enable SDO), BDU=bit4 (avoid high/low byte tearing on async reads) + // and I2C_DIS=bit5 (inhibit I2C since we're on SPI), then rescan so SDO is driven. + // (writes work in 3-wire since the host drives SDI/O) + uint8_t lis2mdl_4wspi[2] = {0x62, 0x34}; + const struct spi_buf tx_buf = {.buf = lis2mdl_4wspi, .len = sizeof(lis2mdl_4wspi)}; + const struct spi_buf_set tx = {.buffers = &tx_buf, .count = 1}; + int wspi_err = spi_write_dt(&sensor_mag_spi_dev, &tx); + if (wspi_err == 0) { + sensor_mag_dev_reg = 0xFF; + mag_id = sensor_scan_mag_spi(&sensor_mag_spi_dev, &sensor_mag_dev_reg); + } + } if (mag_id >= 0) { sensor_interface_register_sensor_mag_spi(&sensor_mag_spi_dev); } diff --git a/src/sensor/sensors.h b/src/sensor/sensors.h index 537e995e..a731defc 100644 --- a/src/sensor/sensors.h +++ b/src/sensor/sensors.h @@ -72,9 +72,15 @@ #if IS_ENABLED(CONFIG_SENSOR_DRV_LIS3MDL) #include "mag/LIS3MDL.h" #endif +#if IS_ENABLED(CONFIG_SENSOR_DRV_MMC5603NJ) +#include "mag/MMC5603NJ.h" +#endif #if IS_ENABLED(CONFIG_SENSOR_DRV_MMC5983MA) #include "mag/MMC5983MA.h" #endif +#if IS_ENABLED(CONFIG_SENSOR_DRV_QMC5883L) +#include "mag/QMC5883L.h" +#endif #if IS_ENABLED(CONFIG_SENSOR_DRV_QMC6309) #include "mag/QMC6309.h" #endif diff --git a/src/sensor/sensors_enum.h b/src/sensor/sensors_enum.h index 239de74e..c5ef90e5 100644 --- a/src/sensor/sensors_enum.h +++ b/src/sensor/sensors_enum.h @@ -105,7 +105,7 @@ memsic 00 (06:IST8306,20:IST8320,21:IST8321) 00,0F (80:QMC6310;3D:LIS3MDL) 0A,0F,4F (48:HMC5883L;3D:LIS3MDL;40:IIS2MDC/LIS2MDL) -20,2F,39 (06:MMC34160PJ;0A:MMC3630KJ,30:MMC5983MA;10:MMC5603NJ/MMC5633NJL,11:MMC5616WA) +20,39,2F (06:MMC34160PJ;10:MMC5603NJ/MMC5633NJL,11:MMC5616WA;0A:MMC3630KJ,30:MMC5983MA) 00 (80:QMC6310) 00 (90:QMC6309) */ diff --git a/src/sensor/sensors_tables.c b/src/sensor/sensors_tables.c index be522966..b22ee7ce 100644 --- a/src/sensor/sensors_tables.c +++ b/src/sensor/sensors_tables.c @@ -43,8 +43,9 @@ LOG_MODULE_DECLARE(sensor_scan, LOG_LEVEL_INF); (SLIME_DRV1(CONFIG_SENSOR_DRV_AK09940) + SLIME_DRV1(CONFIG_SENSOR_DRV_BMM150) \ + SLIME_DRV1(CONFIG_SENSOR_DRV_BMM350) + SLIME_DRV1(CONFIG_SENSOR_DRV_IST8306) \ + SLIME_DRV1(CONFIG_SENSOR_DRV_IST8308) + SLIME_DRV1(CONFIG_SENSOR_DRV_LIS2MDL) \ - + SLIME_DRV1(CONFIG_SENSOR_DRV_LIS3MDL) + SLIME_DRV1(CONFIG_SENSOR_DRV_MMC5983MA) \ - + SLIME_DRV1(CONFIG_SENSOR_DRV_QMC6309)) + + SLIME_DRV1(CONFIG_SENSOR_DRV_LIS3MDL) + SLIME_DRV1(CONFIG_SENSOR_DRV_MMC5603NJ) \ + + SLIME_DRV1(CONFIG_SENSOR_DRV_MMC5983MA) \ + + SLIME_DRV1(CONFIG_SENSOR_DRV_QMC5883L) + SLIME_DRV1(CONFIG_SENSOR_DRV_QMC6309)) // Unimplemented WHO_AM_I rows (chips that map to sensor_*_none) are kept in full // builds for probe-behavior parity, and dropped when that axis opts into MINIMAL. @@ -63,7 +64,8 @@ LOG_MODULE_DECLARE(sensor_scan, LOG_LEVEL_INF); #define SLIME_MAG_G0 \ (SLIME_MAG_KEEP_UNIMPL || IS_ENABLED(CONFIG_SENSOR_DRV_AK09940) || IS_ENABLED(CONFIG_SENSOR_DRV_IST8308)) #define SLIME_MAG_G1 \ - (SLIME_MAG_KEEP_UNIMPL || IS_ENABLED(CONFIG_SENSOR_DRV_AK09940) || IS_ENABLED(CONFIG_SENSOR_DRV_IST8308)) + (SLIME_MAG_KEEP_UNIMPL || IS_ENABLED(CONFIG_SENSOR_DRV_AK09940) || IS_ENABLED(CONFIG_SENSOR_DRV_IST8308) \ + || IS_ENABLED(CONFIG_SENSOR_DRV_QMC5883L)) #define SLIME_MAG_G2 \ (SLIME_MAG_KEEP_UNIMPL || IS_ENABLED(CONFIG_SENSOR_DRV_AK09940) || IS_ENABLED(CONFIG_SENSOR_DRV_IST8308)) #define SLIME_MAG_G3 (IS_ENABLED(CONFIG_SENSOR_DRV_BMM150)) @@ -72,7 +74,8 @@ LOG_MODULE_DECLARE(sensor_scan, LOG_LEVEL_INF); #define SLIME_MAG_G6 (SLIME_MAG_KEEP_UNIMPL || IS_ENABLED(CONFIG_SENSOR_DRV_LIS3MDL)) #define SLIME_MAG_G7 \ (SLIME_MAG_KEEP_UNIMPL || IS_ENABLED(CONFIG_SENSOR_DRV_LIS3MDL) || IS_ENABLED(CONFIG_SENSOR_DRV_LIS2MDL)) -#define SLIME_MAG_G8 (SLIME_MAG_KEEP_UNIMPL || IS_ENABLED(CONFIG_SENSOR_DRV_MMC5983MA)) +#define SLIME_MAG_G8 \ + (SLIME_MAG_KEEP_UNIMPL || IS_ENABLED(CONFIG_SENSOR_DRV_MMC5603NJ) || IS_ENABLED(CONFIG_SENSOR_DRV_MMC5983MA)) #define SLIME_MAG_G9 (SLIME_MAG_KEEP_UNIMPL) #define SLIME_MAG_G10 (IS_ENABLED(CONFIG_SENSOR_DRV_QMC6309)) @@ -307,7 +310,11 @@ const char *dev_mag_names[SENSOR_DEV_MAG_COUNT] "MMC5616WA", "MMC5983MA"}; const sensor_mag_t *sensor_mags[SENSOR_DEV_MAG_COUNT] = { &sensor_mag_none, // HMC5883 will not implement, too low quality +#if IS_ENABLED(CONFIG_SENSOR_DRV_QMC5883L) + &sensor_mag_qmc5883l, +#else &sensor_mag_none, // QMC5883 not implemented +#endif #if IS_ENABLED(CONFIG_SENSOR_DRV_QMC6309) &sensor_mag_qmc6309, #else @@ -355,7 +362,11 @@ const sensor_mag_t *sensor_mags[SENSOR_DEV_MAG_COUNT] = { #endif &sensor_mag_none, // MMC34160 &sensor_mag_none, // MMC3630 +#if IS_ENABLED(CONFIG_SENSOR_DRV_MMC5603NJ) + &sensor_mag_mmc5603nj, // MMC5603NJ/MMC5633NJL +#else &sensor_mag_none, // MMC5603/MMC5633 +#endif &sensor_mag_none, // MMC5616 #if IS_ENABLED(CONFIG_SENSOR_DRV_MMC5983MA) &sensor_mag_mmc5983ma @@ -434,7 +445,7 @@ static const uint8_t i2c_dev_mag_reg[] = { 3, 0x0A, 0x0F, 0x4F, #endif #if SLIME_MAG_G8 - 3, 0x20, 0x2F, 0x39, + 3, 0x20, 0x39, 0x2F, #endif #if SLIME_MAG_G9 1, 0x00, @@ -477,8 +488,8 @@ static const uint8_t i2c_dev_mag_id[] = { 0x48, // AK8963 #endif // 0x0D reg 0x0D - (SLIME_MAG_KEEP_UNIMPL), -#if SLIME_MAG_KEEP_UNIMPL + (SLIME_MAG_KEEP_UNIMPL || IS_ENABLED(CONFIG_SENSOR_DRV_QMC5883L)), +#if SLIME_MAG_KEEP_UNIMPL || IS_ENABLED(CONFIG_SENSOR_DRV_QMC5883L) 0xFF, // QMC5883L #endif #endif // SLIME_MAG_G1 @@ -556,6 +567,14 @@ static const uint8_t i2c_dev_mag_id[] = { (SLIME_MAG_KEEP_UNIMPL), #if SLIME_MAG_KEEP_UNIMPL 0x06, // MMC34160PJ +#endif + // 0x30 reg 0x39 (checked before 0x2F: MMC5603NJ reads 0x0A at undocumented 0x2F, colliding with MMC3630KJ) + (SLIME_MAG_KEEP_UNIMPL * 2 + (1 - SLIME_MAG_KEEP_UNIMPL) * IS_ENABLED(CONFIG_SENSOR_DRV_MMC5603NJ)), +#if SLIME_MAG_KEEP_UNIMPL || IS_ENABLED(CONFIG_SENSOR_DRV_MMC5603NJ) + 0x10, // MMC5603NJ/MMC5633NJL +#endif +#if SLIME_MAG_KEEP_UNIMPL + 0x11, // MMC5616WA #endif // 0x30 reg 0x2F (SLIME_MAG_KEEP_UNIMPL + IS_ENABLED(CONFIG_SENSOR_DRV_MMC5983MA)), @@ -564,12 +583,6 @@ static const uint8_t i2c_dev_mag_id[] = { #endif #if IS_ENABLED(CONFIG_SENSOR_DRV_MMC5983MA) 0x30, // MMC5983MA -#endif - // 0x30 reg 0x39 - (SLIME_MAG_KEEP_UNIMPL * 2), -#if SLIME_MAG_KEEP_UNIMPL - 0x10, - 0x11, // MMC5633NJL, MMC5616WA #endif #endif // SLIME_MAG_G8 #if SLIME_MAG_G9 @@ -610,7 +623,10 @@ static const int i2c_dev_mag[] = { MAG_IST8308, #endif #if SLIME_MAG_KEEP_UNIMPL - MAG_AK8963, MAG_QMC5883L, + MAG_AK8963, +#endif +#if SLIME_MAG_KEEP_UNIMPL || IS_ENABLED(CONFIG_SENSOR_DRV_QMC5883L) + MAG_QMC5883L, #endif #endif // SLIME_MAG_G1 #if SLIME_MAG_G2 @@ -663,13 +679,16 @@ static const int i2c_dev_mag[] = { #endif // SLIME_MAG_G7 #if SLIME_MAG_G8 #if SLIME_MAG_KEEP_UNIMPL - MAG_MMC34160PJ, MAG_MMC3630KJ, + MAG_MMC34160PJ, #endif -#if IS_ENABLED(CONFIG_SENSOR_DRV_MMC5983MA) - MAG_MMC5983MA, +#if SLIME_MAG_KEEP_UNIMPL || IS_ENABLED(CONFIG_SENSOR_DRV_MMC5603NJ) + MAG_MMC5633NJL, #endif #if SLIME_MAG_KEEP_UNIMPL - MAG_MMC5633NJL, MAG_MMC5616WA, + MAG_MMC5616WA, MAG_MMC3630KJ, +#endif +#if IS_ENABLED(CONFIG_SENSOR_DRV_MMC5983MA) + MAG_MMC5983MA, #endif #endif // SLIME_MAG_G8 #if SLIME_MAG_G9