#include "Compass_RM3100.h" #include #define RM3100_POLL_REG 0x00 #define RM3100_CMM_REG 0x01 #define RM3100_CCX1_REG 0x04 #define RM3100_CCX0_REG 0x05 #define RM3100_CCY1_REG 0x06 #define RM3100_CCY0_REG 0x07 #define RM3100_CCZ1_REG 0x08 #define RM3100_CCZ0_REG 0x09 #define RM3100_TMRC_REG 0x0B #define RM3100_MX2_REG 0x24 #define RM3100_MX1_REG 0x25 #define RM3100_MX0_REG 0x26 #define RM3100_MY2_REG 0x27 #define RM3100_MY1_REG 0x28 #define RM3100_MY0_REG 0x29 #define RM3100_MZ2_REG 0x2A #define RM3100_MZ1_REG 0x2B #define RM3100_MZ0_REG 0x2C #define RM3100_BIST_REG 0x33 #define RM3100_STATUS_REG 0x34 #define RM3100_HSHAKE_REG 0x34 #define RM3100_REVID_REG 0x36 #define CCP0 0xC8 // Cycle Count values #define CCP1 0x00 #define CCP0_DEFAULT 0xC8 // Default Cycle Count values (used as a whoami check) #define CCP1_DEFAULT 0x00 #define GAIN_CC50 20.0f // LSB/uT #define GAIN_CC100 38.0f #define GAIN_CC200 75.0f #define UTESLA_TO_MGAUSS 10.0f // uT to mGauss conversion #define TMRC 0x94 // Update rate 150Hz #define CMM 0x71 // read 3 axes and set data ready if 3 axes are ready static bool read_registers(SPI_DEV_t *dev, uint8_t reg, uint8_t *buf, uint32_t size) { SPI_DEV_select(dev); bool rslt = SPI_DEV_read_registers(dev, reg, buf, size); SPI_DEV_unselect(dev); return rslt; } static bool write_register(SPI_DEV_t *dev, uint8_t reg, uint8_t val) { SPI_DEV_select(dev); bool rslt = SPI_DEV_write_register(dev, reg, val); SPI_DEV_unselect(dev); return rslt; } bool Compass_RM3100_init(Compass_RM3100_t *cps, const char *name, SPI_DEV_t *dev, GPIO_EXIT_t *drdy) { int i; cps->name = name; cps->dev = dev; cps->drdy = drdy; cps->_scaler = 1.0; cps->success = false; SPI_DEV_begin(dev, -1); // read has high bit set for SPI SPI_DEV_set_read_flag(dev, 0x80); // high retries for init //dev->set_retries(10); // use default cycle count values as a whoami test uint8_t ccx0; uint8_t ccx1; uint8_t ccy0; uint8_t ccy1; uint8_t ccz0; uint8_t ccz1; if (!read_registers(dev, RM3100_CCX1_REG, &ccx1, 1) || !read_registers(dev, RM3100_CCX0_REG, &ccx0, 1) || !read_registers(dev, RM3100_CCY1_REG, &ccy1, 1) || !read_registers(dev, RM3100_CCY0_REG, &ccy0, 1) || !read_registers(dev, RM3100_CCZ1_REG, &ccz1, 1) || !read_registers(dev, RM3100_CCZ0_REG, &ccz0, 1) || ccx1 != CCP1_DEFAULT || ccx0 != CCP0_DEFAULT || ccy1 != CCP1_DEFAULT || ccy0 != CCP0_DEFAULT || ccz1 != CCP1_DEFAULT || ccz0 != CCP0_DEFAULT) { // couldn't read one of the cycle count registers or didn't recognize the default cycle count values SPI_DEV_end(dev); return false; } write_register(dev, RM3100_TMRC_REG, TMRC); // CMM data rate write_register(dev, RM3100_CMM_REG, CMM); // CMM configuration write_register(dev, RM3100_CCX1_REG, CCP1); // cycle count x write_register(dev, RM3100_CCX0_REG, CCP0); // cycle count x write_register(dev, RM3100_CCY1_REG, CCP1); // cycle count y write_register(dev, RM3100_CCY0_REG, CCP0); // cycle count y write_register(dev, RM3100_CCZ1_REG, CCP1); // cycle count z write_register(dev, RM3100_CCZ0_REG, CCP0); // cycle count z cps->_scaler = (1 / GAIN_CC200) * UTESLA_TO_MGAUSS; // has to be changed if using a different cycle count // lower retries for run //dev->set_retries(3); //Built-in Test write_register(dev, RM3100_BIST_REG, 0b10001010); // test for (i = 0; i < 10; ++i) { uint8_t status; if (!read_registers(cps->dev, RM3100_STATUS_REG, (uint8_t *)&status, 1)) { break; } if ((status & 0x80)) { if (read_registers(dev, RM3100_BIST_REG, &status, 1)) { if ((status & 0b01110000) == 0b01110000) { write_register(dev, RM3100_BIST_REG, 0b00000000); // done cps->success = true; } } break; } osDelay(10); } SPI_DEV_end(dev); printf("RM3100: Foundas compass.\n"); return cps->success; } void Compass_RM3100_update(Compass_RM3100_t *cps) { struct PACKED { uint8_t magx_2; uint8_t magx_1; uint8_t magx_0; uint8_t magy_2; uint8_t magy_1; uint8_t magy_0; uint8_t magz_2; uint8_t magz_1; uint8_t magz_0; } data; int32_t magx = 0; int32_t magy = 0; int32_t magz = 0; if (cps->success && SPI_DEV_begin(cps->dev, 0)) { // check data ready on 3 axis uint8_t status; if (!read_registers(cps->dev, RM3100_STATUS_REG, (uint8_t *)&status, 1)) { cps->bad_cnt++; goto check_registers; } if (!(status & 0x80)) { // data not available yet cps->nrdy_cnt++; goto check_registers; } if (!read_registers(cps->dev, RM3100_MX2_REG, (uint8_t *)&data, sizeof(data))) { cps->bad_cnt++; goto check_registers; } // the 24 bits of data for each axis are in 2s complement representation // each byte is shifted to its position in a 24-bit unsigned integer and from 8 more bits to be left-aligned in a 32-bit integer magx = ((uint32_t)data.magx_2 << 24) | ((uint32_t)data.magx_1 << 16) | ((uint32_t)data.magx_0 << 8); magy = ((uint32_t)data.magy_2 << 24) | ((uint32_t)data.magy_1 << 16) | ((uint32_t)data.magy_0 << 8); magz = ((uint32_t)data.magz_2 << 24) | ((uint32_t)data.magz_1 << 16) | ((uint32_t)data.magz_0 << 8); // right-shift signed integer back to get correct measurement value magx >>= 8; magy >>= 8; magz >>= 8; // apply scaler and store in field vector cps->magx = magx * cps->_scaler; cps->magy = magy * cps->_scaler; cps->magz = magz * cps->_scaler; cps->good_cnt++; cps->seq++; check_registers: SPI_DEV_end(cps->dev); } } void Compass_RM3100_monitor(Compass_RM3100_t *cps) { cps->good_pps = cps->good_cnt; cps->bad_pps = cps->bad_cnt; cps->nrdy_pps = cps->nrdy_cnt; cps->nrdy_cnt = 0; cps->good_cnt = 0; cps->bad_cnt = 0; } void Compass_RM3100_loop(Compass_RM3100_t *cps) { while (true) { cps->drdy->task = xTaskGetCurrentTaskHandle(); ulTaskNotifyTake(pdTRUE, 50); Compass_RM3100_update(cps); } }