LIS2MDLSensor.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    LIS2MDLSensor.cpp
00004  * @author  SRA
00005  * @version V1.0.0
00006  * @date    February 2019
00007  * @brief   Implementation of an LIS2MDL 3 axes magnetometer sensor.
00008  ******************************************************************************
00009  * @attention
00010  *
00011  * <h2><center>&copy; COPYRIGHT(c) 2019 STMicroelectronics</center></h2>
00012  *
00013  * Redistribution and use in source and binary forms, with or without modification,
00014  * are permitted provided that the following conditions are met:
00015  *   1. Redistributions of source code must retain the above copyright notice,
00016  *      this list of conditions and the following disclaimer.
00017  *   2. Redistributions in binary form must reproduce the above copyright notice,
00018  *      this list of conditions and the following disclaimer in the documentation
00019  *      and/or other materials provided with the distribution.
00020  *   3. Neither the name of STMicroelectronics nor the names of its contributors
00021  *      may be used to endorse or promote products derived from this software
00022  *      without specific prior written permission.
00023  *
00024  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00025  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00026  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00027  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00028  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00029  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00030  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00032  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00033  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  ******************************************************************************
00036  */
00037 
00038 
00039 /* Includes ------------------------------------------------------------------*/
00040 
00041 #include "LIS2MDLSensor.h"
00042 
00043 
00044 /* Class Implementation ------------------------------------------------------*/
00045 
00046 /** Constructor
00047  * @param spi object of an helper class which handles the SPI peripheral
00048  * @param cs_pin the chip select pin
00049  * @param int_pin the interrupt pin
00050  * @param spi_type the SPI type
00051  */
00052 LIS2MDLSensor::LIS2MDLSensor(SPI *spi, PinName cs_pin, PinName int_pin, SPI_type_t spi_type) : _dev_spi(spi), _cs_pin(cs_pin), _int_irq(int_pin), _spi_type(spi_type)
00053 {
00054     assert(spi);
00055     if (cs_pin == NC) {
00056         printf("ERROR LIS2MDL CS MUST NOT BE NC\n\r");
00057         _dev_spi = NULL;
00058         _dev_i2c = NULL;
00059         return;
00060     }
00061 
00062     _reg_ctx.write_reg = LIS2MDL_io_write;
00063     _reg_ctx.read_reg = LIS2MDL_io_read;
00064     _reg_ctx.handle = (void *)this;
00065     _cs_pin = 1;
00066     _dev_i2c = NULL;
00067     _address = 0;
00068 
00069     if (_spi_type == SPI4W) {
00070         /* Enable SPI 4-Wires on the component (in this way we lose the usage of INT pin) */
00071         uint8_t data = 0x34;
00072         lis2mdl_write_reg(&_reg_ctx, LIS2MDL_CFG_REG_C, &data, 1);
00073     }
00074 }
00075 
00076 /** Constructor
00077  * @param i2c object of an helper class which handles the I2C peripheral
00078  * @param address the address of the component's instance
00079  * @param int_pin the interrupt pin
00080  */
00081 LIS2MDLSensor::LIS2MDLSensor(DevI2C *i2c, uint8_t address, PinName int_pin) : _dev_i2c(i2c), _address(address), _cs_pin(NC), _int_irq(int_pin)
00082 {
00083     assert(i2c);
00084     _dev_spi = NULL;
00085     _reg_ctx.write_reg = LIS2MDL_io_write;
00086     _reg_ctx.read_reg = LIS2MDL_io_read;
00087     _reg_ctx.handle = (void *)this;
00088 }
00089 
00090 /**
00091  * @brief  Initializing the component
00092  * @param  init pointer to device specific initalization structure
00093  * @retval 0 in case of success, an error code otherwise
00094  */
00095 int LIS2MDLSensor::init(void *init)
00096 {
00097     /* Enable BDU */
00098     if (lis2mdl_block_data_update_set(&(_reg_ctx), PROPERTY_ENABLE) != 0) {
00099         return 1;
00100     }
00101 
00102     /* Operating mode selection - power down */
00103     if (lis2mdl_operating_mode_set(&(_reg_ctx), LIS2MDL_POWER_DOWN) != 0) {
00104         return 1;
00105     }
00106 
00107     /* Output data rate selection */
00108     if (lis2mdl_data_rate_set(&(_reg_ctx), LIS2MDL_ODR_100Hz) != 0) {
00109         return 1;
00110     }
00111 
00112     /* Self Test disabled. */
00113     if (lis2mdl_self_test_set(&(_reg_ctx), PROPERTY_DISABLE) != 0) {
00114         return 1;
00115     }
00116 
00117     _mag_is_enabled = 0;
00118 
00119     return 0;
00120 }
00121 
00122 /**
00123  * @brief  Read component ID
00124  * @param  id the WHO_AM_I value
00125  * @retval 0 in case of success, an error code otherwise
00126  */
00127 int LIS2MDLSensor::read_id(uint8_t *id)
00128 {
00129     if (lis2mdl_device_id_get(&_reg_ctx, id) != 0) {
00130         return 1;
00131     }
00132 
00133     return 0;
00134 }
00135 
00136 
00137 /**
00138  * @brief Enable the LIS2MDL magnetometer sensor
00139  * @retval 0 in case of success, an error code otherwise
00140  */
00141 int LIS2MDLSensor::enable()
00142 {
00143     /* Check if the component is already enabled */
00144     if (_mag_is_enabled == 1U) {
00145         return 0;
00146     }
00147 
00148     /* Output data rate selection. */
00149     if (lis2mdl_operating_mode_set(&_reg_ctx, LIS2MDL_CONTINUOUS_MODE) != 0) {
00150         return 1;
00151     }
00152 
00153     _mag_is_enabled = 1;
00154 
00155     return 0;
00156 }
00157 
00158 /**
00159  * @brief Disable the LIS2MDL magnetometer sensor
00160  * @retval 0 in case of success, an error code otherwise
00161  */
00162 int LIS2MDLSensor::disable()
00163 {
00164     /* Check if the component is already disabled */
00165     if (_mag_is_enabled == 0U) {
00166         return 0;
00167     }
00168 
00169     /* Output data rate selection - power down. */
00170     if (lis2mdl_operating_mode_set(&_reg_ctx, LIS2MDL_POWER_DOWN) != 0) {
00171         return 1;
00172     }
00173 
00174     _mag_is_enabled = 0;
00175 
00176     return 0;
00177 }
00178 
00179 /**
00180  * @brief  Get the LIS2MDL magnetometer sensor sensitivity
00181  * @param  sensitivity pointer where the sensitivity is written
00182  * @retval 0 in case of success, an error code otherwise
00183  */
00184 int LIS2MDLSensor::get_m_sensitivity(float *sensitivity)
00185 {
00186     *sensitivity = LIS2MDL_MAG_SENSITIVITY_FS_50GAUSS;
00187 
00188     return 0;
00189 }
00190 
00191 /**
00192  * @brief  Get the LIS2MDL magnetometer sensor output data rate
00193  * @param  odr pointer where the output data rate is written
00194  * @retval 0 in case of success, an error code otherwise
00195  */
00196 int LIS2MDLSensor::get_m_odr(float *odr)
00197 {
00198     int ret = 0;
00199     lis2mdl_odr_t odr_low_level;
00200 
00201     /* Get current output data rate. */
00202     if (lis2mdl_data_rate_get(&_reg_ctx, &odr_low_level) != 0) {
00203         return 1;
00204     }
00205 
00206     switch (odr_low_level) {
00207         case LIS2MDL_ODR_10Hz:
00208             *odr = 10.0f;
00209             break;
00210 
00211         case LIS2MDL_ODR_20Hz:
00212             *odr = 20.0f;
00213             break;
00214 
00215         case LIS2MDL_ODR_50Hz:
00216             *odr = 50.0f;
00217             break;
00218 
00219         case LIS2MDL_ODR_100Hz:
00220             *odr = 100.0f;
00221             break;
00222 
00223         default:
00224             ret = 1;
00225             break;
00226     }
00227 
00228     return ret;
00229 }
00230 
00231 /**
00232  * @brief  Set the LIS2MDL magnetometer sensor output data rate
00233  * @param  odr the output data rate value to be set
00234  * @retval 0 in case of success, an error code otherwise
00235  */
00236 int LIS2MDLSensor::set_m_odr(float odr)
00237 {
00238     lis2mdl_odr_t new_odr;
00239 
00240     new_odr = (odr <= 10.000f) ? LIS2MDL_ODR_10Hz
00241               : (odr <= 20.000f) ? LIS2MDL_ODR_20Hz
00242               : (odr <= 50.000f) ? LIS2MDL_ODR_50Hz
00243               :                    LIS2MDL_ODR_100Hz;
00244 
00245     if (lis2mdl_data_rate_set(&_reg_ctx, new_odr) != 0) {
00246         return 1;
00247     }
00248 
00249     return 0;
00250 }
00251 
00252 /**
00253  * @brief  Get the LIS2MDL magnetometer sensor full scale
00254  * @param  full_scale pointer where the full scale is written
00255  * @retval 0 in case of success, an error code otherwise
00256  */
00257 int LIS2MDLSensor::get_m_fs(float *full_scale)
00258 {
00259     *full_scale = 50.0f;
00260 
00261     return 0;
00262 }
00263 
00264 /**
00265  * @brief  Set the LIS2MDL magnetometer sensor full scale
00266  * @param  full_scale the functional full scale to be set
00267  * @retval 0 in case of success, an error code otherwise
00268  */
00269 int LIS2MDLSensor::set_m_fs(float full_scale)
00270 {
00271     (void)full_scale;
00272     return 0;
00273 }
00274 
00275 /**
00276  * @brief  Get the LIS2MDL magnetometer sensor axes
00277  * @param  magnetic_field pointer where the values of the axes are written
00278  * @retval 0 in case of success, an error code otherwise
00279  */
00280 int LIS2MDLSensor::get_m_axes(int32_t *magnetic_field)
00281 {
00282     axis3bit16_t data_raw;
00283     float sensitivity;
00284 
00285     /* Read raw data values. */
00286     if (lis2mdl_magnetic_raw_get(&_reg_ctx, data_raw.u8bit) != 0) {
00287         return 1;
00288     }
00289 
00290     /* Get LIS2MDL actual sensitivity. */
00291     if (get_m_sensitivity(&sensitivity) != 0) {
00292         return 1;
00293     }
00294 
00295     /* Calculate the data. */
00296     magnetic_field[0] = (int32_t)((float)((float)data_raw.i16bit[0] * sensitivity));
00297     magnetic_field[1] = (int32_t)((float)((float)data_raw.i16bit[1] * sensitivity));
00298     magnetic_field[2] = (int32_t)((float)((float)data_raw.i16bit[2] * sensitivity));
00299 
00300     return 0;
00301 }
00302 
00303 /**
00304  * @brief  Get the LIS2MDL magnetometer sensor raw axes
00305  * @param  value pointer where the raw values of the axes are written
00306  * @retval 0 in case of success, an error code otherwise
00307  */
00308 int LIS2MDLSensor::get_m_axes_raw(int16_t *value)
00309 {
00310     axis3bit16_t data_raw;
00311 
00312     /* Read raw data values. */
00313     if (lis2mdl_magnetic_raw_get(&_reg_ctx, data_raw.u8bit) != 0) {
00314         return 1;
00315     }
00316 
00317     /* Format the data. */
00318     value[0] = data_raw.i16bit[0];
00319     value[1] = data_raw.i16bit[1];
00320     value[2] = data_raw.i16bit[2];
00321 
00322     return 0;
00323 }
00324 
00325 /**
00326  * @brief  Get the LIS2MDL register value for magnetic sensor
00327  * @param  reg address to be read
00328  * @param  data pointer where the value is written
00329  * @retval 0 in case of success, an error code otherwise
00330  */
00331 int LIS2MDLSensor::read_reg(uint8_t reg, uint8_t *data)
00332 {
00333     if (lis2mdl_read_reg(&_reg_ctx, reg, data, 1) != 0) {
00334         return 1;
00335     }
00336 
00337     return 0;
00338 }
00339 
00340 /**
00341  * @brief  Set the LIS2MDL register value for magnetic sensor
00342  * @param  pObj the device pObj
00343  * @param  reg address to be written
00344  * @param  data value to be written
00345  * @retval 0 in case of success, an error code otherwise
00346  */
00347 int LIS2MDLSensor::write_reg(uint8_t reg, uint8_t data)
00348 {
00349     if (lis2mdl_write_reg(&_reg_ctx, reg, &data, 1) != 0) {
00350         return 1;
00351     }
00352 
00353     return 0;
00354 }
00355 
00356 /**
00357  * @brief  Set self test
00358  * @param  status the value of self_test in reg CFG_REG_C
00359  * @retval 0 in case of success, an error code otherwise
00360  */
00361 int LIS2MDLSensor::set_m_self_test(uint8_t status)
00362 {
00363     if (lis2mdl_self_test_set(&_reg_ctx, status) != 0) {
00364         return 1;
00365     }
00366 
00367     return 0;
00368 }
00369 
00370 /**
00371  * @brief  Get the LIS2MDL MAG data ready bit value
00372  * @param  status the status of data ready bit
00373  * @retval 0 in case of success, an error code otherwise
00374  */
00375 int LIS2MDLSensor::get_m_drdy_status(uint8_t *status)
00376 {
00377     if (lis2mdl_mag_data_ready_get(&_reg_ctx, status) != 0) {
00378         return 1;
00379     }
00380 
00381     return 0;
00382 }
00383 
00384 
00385 
00386 int32_t LIS2MDL_io_write(void *handle, uint8_t WriteAddr, uint8_t *pBuffer, uint16_t nBytesToWrite)
00387 {
00388     return ((LIS2MDLSensor *)handle)->io_write(pBuffer, WriteAddr, nBytesToWrite);
00389 }
00390 
00391 int32_t LIS2MDL_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead)
00392 {
00393     return ((LIS2MDLSensor *)handle)->io_read(pBuffer, ReadAddr, nBytesToRead);
00394 }