/* * Author: Jon Trulson * Copyright (c) 2017 Intel Corporation. * * The MIT License * * Permission is hereby granted, free of charge, to any person obtaining * a copy of this software and associated documentation files (the * "Software"), to deal in the Software without restriction, including * without limitation the rights to use, copy, modify, merge, publish, * distribute, sublicense, and/or sell copies of the Software, and to * permit persons to whom the Software is furnished to do so, subject to * the following conditions: * * The above copyright notice and this permission notice shall be * included in all copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include #include #include "upm_utilities.h" #include "lsm303d.h" // some useful macros to save on typing and text wrapping #undef _SHIFT #define _SHIFT(x) (_LSM303D_##x##_SHIFT) #undef _MASK #define _MASK(x) (_LSM303D_##x##_MASK) #undef _SHIFTMASK #define _SHIFTMASK(x) (_MASK(x) << _SHIFT(x)) // init lsm303d_context lsm303d_init(int bus, int addr) { lsm303d_context dev = (lsm303d_context)malloc(sizeof(struct _lsm303d_context)); if (!dev) return NULL; // zero out context memset((void *)dev, 0, sizeof(struct _lsm303d_context)); // make sure MRAA is initialized if (mraa_init() != MRAA_SUCCESS) { printf("%s: mraa_init() failed.\n", __FUNCTION__); lsm303d_close(dev); return NULL; } if (!(dev->i2c = mraa_i2c_init(bus))) { printf("%s: mraa_i2c_init() failed.\n", __FUNCTION__); lsm303d_close(dev); return NULL; } if (mraa_i2c_address(dev->i2c, addr)) { printf("%s: mraa_i2c_address() failed.\n", __FUNCTION__); lsm303d_close(dev); return NULL; } // check the chip id uint8_t chipID = lsm303d_read_reg(dev, LSM303D_REG_WHO_AM_I); if (chipID != LSM303D_CHIPID) { printf("%s: invalid chip id: %02x. Expected %02x\n", __FUNCTION__, chipID, LSM303D_CHIPID); lsm303d_close(dev); return NULL; } // call devinit with a default high resolution mode if (lsm303d_devinit(dev, LSM303D_M_RES_HIGH)) { printf("%s: lsm303d_devinit() failed.\n", __FUNCTION__); lsm303d_close(dev); return NULL; } return dev; } void lsm303d_close(lsm303d_context dev) { assert(dev != NULL); if (dev->i2c) mraa_i2c_stop(dev->i2c); free(dev); } upm_result_t lsm303d_devinit(const lsm303d_context dev, LSM303D_M_RES_T res) { assert(dev != NULL); // enable all axes and BDU uint8_t reg = lsm303d_read_reg(dev, LSM303D_REG_CTRL1); reg |= LSM303D_CTRL1_AXEN | LSM303D_CTRL1_AYEN | LSM303D_CTRL1_AZEN | LSM303D_CTRL1_BDU; if (lsm303d_write_reg(dev, LSM303D_REG_CTRL1, reg)) { printf("%s: lsm303d_write_reg() failed.\n", __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; } // enable temperature measurement and set mag resolution reg = lsm303d_read_reg(dev, LSM303D_REG_CTRL5); reg &= ~_SHIFTMASK(CTRL5_MRES); reg |= LSM303D_CTRL5_TEMP_EN | (res << _SHIFT(CTRL5_MRES)); if (lsm303d_write_reg(dev, LSM303D_REG_CTRL5, reg)) { printf("%s: lsm303d_write_reg() failed.\n", __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; } // set magnetometer to continuous mode reg = lsm303d_read_reg(dev, LSM303D_REG_CTRL7); reg &= ~_SHIFTMASK(CTRL7_MD); reg |= (LSM303D_MD_CONTINUOUS << _SHIFT(CTRL7_MD)); if (lsm303d_write_reg(dev, LSM303D_REG_CTRL7, reg)) { printf("%s: lsm303d_write_reg() failed.\n", __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; } // set ACC ODR to 100Hz by default if (lsm303d_set_acc_odr(dev, LSM303D_AODR_100HZ)) { printf("%s: lsm303d_set_acc_odr() failed.\n", __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; } // set MAG ODR to 12.5Hz by default if (lsm303d_set_mag_odr(dev, LSM303D_MODR_12_5HZ)) { printf("%s: lsm303d_set_acc_odr() failed.\n", __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; } // default to 2G acc sensitivity if (lsm303d_set_acc_full_scale(dev, LSM303D_AFS_2G)) { printf("%s: lsm303d_set_acc_full_scale() failed.\n", __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; } // default to 2 Gauss mag sensitivity if (lsm303d_set_mag_full_scale(dev, LSM303D_MFS_2)) { printf("%s: lsm303d_set_acc_full_scale() failed.\n", __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; } upm_delay_ms(10); return UPM_SUCCESS; } upm_result_t lsm303d_set_acc_full_scale(const lsm303d_context dev, LSM303D_AFS_T fs) { assert(dev != NULL); uint8_t reg = lsm303d_read_reg(dev, LSM303D_REG_CTRL2); reg &= ~_SHIFTMASK(CTRL2_AFS); reg |= (fs << _SHIFT(CTRL2_AFS)); if (lsm303d_write_reg(dev, LSM303D_REG_CTRL2, reg)) return UPM_ERROR_OPERATION_FAILED; upm_delay_ms(50); // set our scaling factor depending on current FS switch(fs) { case LSM303D_AFS_2G: dev->accScale = 0.061; break; case LSM303D_AFS_4G: dev->accScale = 0.122; break; case LSM303D_AFS_6G: dev->accScale = 0.183; break; case LSM303D_AFS_8G: dev->accScale = 0.320; break; case LSM303D_AFS_16G: dev->accScale = 0.732; break; } return UPM_SUCCESS; } upm_result_t lsm303d_set_mag_full_scale(const lsm303d_context dev, LSM303D_MFS_T fs) { assert(dev != NULL); uint8_t reg = lsm303d_read_reg(dev, LSM303D_REG_CTRL6); reg &= ~_SHIFTMASK(CTRL6_MFS); reg |= (fs << _SHIFT(CTRL6_MFS)); if (lsm303d_write_reg(dev, LSM303D_REG_CTRL6, reg)) return UPM_ERROR_OPERATION_FAILED; upm_delay_ms(50); // set our scaling factor depending on current FS switch(fs) { case LSM303D_MFS_2: dev->magScale = 0.080; break; case LSM303D_MFS_4: dev->magScale = 0.160; break; case LSM303D_MFS_8: dev->magScale = 0.320; break; case LSM303D_MFS_12: dev->magScale = 0.479; break; } return UPM_SUCCESS; } upm_result_t lsm303d_update(const lsm303d_context dev) { assert(dev != NULL); const int maxLen = 6; uint8_t buf[maxLen]; // get the temperature first, only 2 bytes if (lsm303d_read_regs(dev, LSM303D_REG_TEMP_OUT_L, buf, 2) != 2) { printf("%s: lsm303d_read_regs(temp) failed.\n", __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; } dev->temperature = (float)( (int16_t)(buf[0] | (buf[1] << 8)) << 4); // next, acc data if (lsm303d_read_regs(dev, LSM303D_REG_OUT_X_L_A, buf, maxLen) != maxLen) { printf("%s: lsm303d_read_regs(acc) failed.\n", __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; } dev->accX = (float)((int16_t)(buf[0] | (buf[1] << 8))); dev->accY = (float)((int16_t)(buf[2] | (buf[3] << 8))); dev->accZ = (float)((int16_t)(buf[4] | (buf[5] << 8))); // now mag data if (lsm303d_read_regs(dev, LSM303D_REG_OUT_X_L_M, buf, maxLen) != maxLen) { printf("%s: lsm303d_read_regs(mag) failed.\n", __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; } dev->magX = (float)((int16_t)(buf[0] | (buf[1] << 8))); dev->magY = (float)((int16_t)(buf[2] | (buf[3] << 8))); dev->magZ = (float)((int16_t)(buf[4] | (buf[5] << 8))); return UPM_SUCCESS; } uint8_t lsm303d_read_reg(const lsm303d_context dev, uint8_t reg) { assert(dev != NULL); int rv = mraa_i2c_read_byte_data(dev->i2c, reg); if (rv < 0) { printf("%s: mraa_i2c_read_byte_data() failed\n", __FUNCTION__); return 0xff; } return (uint8_t)rv; } int lsm303d_read_regs(const lsm303d_context dev, uint8_t reg, uint8_t *buffer, int len) { assert(dev != NULL); reg |= 0x80; // enable auto-increment if (mraa_i2c_read_bytes_data(dev->i2c, reg, buffer, len) != len) return -1; return len; } upm_result_t lsm303d_write_reg(const lsm303d_context dev, uint8_t reg, uint8_t val) { assert(dev != NULL); if (mraa_i2c_write_byte_data(dev->i2c, val, reg)) { printf("%s: mraa_i2c_write_byte_data() failed.\n", __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; } return UPM_SUCCESS; } float lsm303d_get_temperature(const lsm303d_context dev) { assert(dev != NULL); // It's not clear how to compute this from the datasheet, but this // seems to give a reasonably accurate result. return (dev->temperature / 128.0) + 25.0; } void lsm303d_get_magnetometer(const lsm303d_context dev, float *x, float *y, float *z) { assert(dev != NULL); // Output is in milli-Gauss - we convert and return it in uT (SI // micro-teslas) instead. if (x) *x = (dev->magX * dev->magScale) / 10.0; if (y) *y = (dev->magY * dev->magScale) / 10.0; if (z) *z = (dev->magZ * dev->magScale) / 10.0; } void lsm303d_get_accelerometer(const lsm303d_context dev, float *x, float *y, float *z) { assert(dev != NULL); if (x) *x = (dev->accX * dev->accScale) / 1000.0; if (y) *y = (dev->accY * dev->accScale) / 1000.0; if (z) *z = (dev->accZ * dev->accScale) / 1000.0; } upm_result_t lsm303d_set_acc_odr(const lsm303d_context dev, LSM303D_AODR_T odr) { assert(dev != NULL); uint8_t reg = lsm303d_read_reg(dev, LSM303D_REG_CTRL1); reg &= ~_SHIFTMASK(CTRL1_AODR); reg |= (odr << _SHIFT(CTRL1_AODR)); if (lsm303d_write_reg(dev, LSM303D_REG_CTRL1, reg)) return UPM_ERROR_OPERATION_FAILED; return UPM_SUCCESS; } upm_result_t lsm303d_set_mag_odr(const lsm303d_context dev, LSM303D_MODR_T odr) { assert(dev != NULL); uint8_t reg = lsm303d_read_reg(dev, LSM303D_REG_CTRL5); reg &= ~_SHIFTMASK(CTRL5_MODR); reg |= (odr << _SHIFT(CTRL5_MODR)); if (lsm303d_write_reg(dev, LSM303D_REG_CTRL5, reg)) return UPM_ERROR_OPERATION_FAILED; return UPM_SUCCESS; }