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 power mode
00277  * @param  lp pointer where the power mode is written
00278  * @retval 0 in case of success, an error code otherwise
00279  */
00280 int LIS2MDLSensor::get_m_lp(uint8_t *lp)
00281 {
00282     int ret = 0;
00283     lis2mdl_lp_t current_lp;
00284     
00285     /* Get current power mode. */
00286     if (lis2mdl_power_mode_get(&_reg_ctx, &current_lp) != 0) {
00287         return 1;
00288     }
00289     
00290     switch (current_lp) {
00291         case LIS2MDL_HIGH_RESOLUTION:
00292             *lp = 0;
00293             break;
00294 
00295         case LIS2MDL_LOW_POWER:
00296             *lp = 1;
00297             break;
00298 
00299         default:
00300             ret = 1;
00301             break;
00302     }
00303 
00304     return ret;
00305 }
00306 
00307 /**
00308  * @brief  Set the LIS2MDL magnetometer sensor power mode
00309  * @param  lp the power mode value to be set
00310  * @retval 0 in case of success, an error code otherwise
00311  */
00312 int LIS2MDLSensor::set_m_lp(uint8_t lp)
00313 {
00314     lis2mdl_lp_t new_lp;
00315 
00316     new_lp = (lp == 0) ? LIS2MDL_HIGH_RESOLUTION
00317               : LIS2MDL_LOW_POWER;
00318 
00319     if (lis2mdl_power_mode_set(&_reg_ctx, new_lp) != 0) {
00320         return 1;
00321     }
00322 
00323     return 0;
00324 }
00325 
00326 /**
00327  * @brief  Get the LIS2MDL magnetometer sensor bandwidth
00328  * @param  lpf pointer where the bandwidth is written
00329  * @retval 0 in case of success, an error code otherwise
00330  */
00331 int LIS2MDLSensor::get_m_lpf(uint8_t *lpf)
00332 {
00333     int ret = 0;
00334     lis2mdl_lpf_t current_lpf;
00335     
00336     /* Get current bandwidth. */
00337     if (lis2mdl_low_pass_bandwidth_get(&_reg_ctx, &current_lpf) != 0) {
00338         return 1;
00339     }
00340     
00341     switch (current_lpf) {
00342         case LIS2MDL_ODR_DIV_2:
00343             *lpf = 0;
00344             break;
00345 
00346         case LIS2MDL_ODR_DIV_4:
00347             *lpf = 1;
00348             break;
00349 
00350         default:
00351             ret = 1;
00352             break;
00353     }
00354 
00355     return ret;
00356 }
00357 
00358 /**
00359  * @brief  Set the LIS2MDL magnetometer sensor bandwidth
00360  * @param  lpf the bandwidth value to be set
00361  * @retval 0 in case of success, an error code otherwise
00362  */
00363 int LIS2MDLSensor::set_m_lpf(uint8_t lpf)
00364 {
00365     lis2mdl_lpf_t new_lpf;
00366 
00367     new_lpf = (lpf == 0) ? LIS2MDL_ODR_DIV_2
00368               : LIS2MDL_ODR_DIV_4;
00369 
00370     if (lis2mdl_low_pass_bandwidth_set(&_reg_ctx, new_lpf) != 0) {
00371         return 1;
00372     }
00373 
00374     return 0;
00375 }
00376 
00377 /**
00378  * @brief  Get the LIS2MDL magnetometer sensor temperature compensation
00379  * @param  comp_temp_en pointer where the temperature compensation is written
00380  * @retval 0 in case of success, an error code otherwise
00381  */
00382 int LIS2MDLSensor::get_m_comp_temp_en(uint8_t *comp_temp_en)
00383 {
00384     if (lis2mdl_offset_temp_comp_get(&_reg_ctx, comp_temp_en) != 0) {
00385         return 1;
00386     }
00387     return 0;
00388 }
00389 
00390 /**
00391  * @brief  Set the LIS2MDL magnetometer sensor temperature compensation
00392  * @param  comp_temp_en the temperature compensation value to be set
00393  * @retval 0 in case of success, an error code otherwise
00394  */
00395 int LIS2MDLSensor::set_m_comp_temp_en(uint8_t comp_temp_en)
00396 {
00397     if (lis2mdl_offset_temp_comp_set(&_reg_ctx, comp_temp_en) != 0) {
00398         return 1;
00399     }
00400     return 0;
00401 }
00402 
00403 /**
00404  * @brief  Get the LIS2MDL magnetometer sensor offset cancellation
00405  * @param  off_canc pointer where the offset cancellation is written
00406  * @retval 0 in case of success, an error code otherwise
00407  */
00408 int LIS2MDLSensor::get_m_off_canc(uint8_t *off_canc)
00409 {
00410     int ret = 0;
00411     lis2mdl_set_rst_t current_off_canc;
00412     
00413     /* Get current offset cancellation. */
00414     if (lis2mdl_set_rst_mode_get(&_reg_ctx, &current_off_canc) != 0) {
00415         return 1;
00416     }
00417     
00418     switch (current_off_canc) {
00419         case LIS2MDL_SET_SENS_ODR_DIV_63:
00420             *off_canc = 0;
00421             break;
00422 
00423         case LIS2MDL_SENS_OFF_CANC_EVERY_ODR:
00424             *off_canc = 1;
00425             break;
00426             
00427         case LIS2MDL_SET_SENS_ONLY_AT_POWER_ON:
00428             *off_canc = 2;
00429             break;
00430 
00431         default:
00432             ret = 1;
00433             break;
00434     }
00435 
00436     return ret;
00437 }
00438 
00439 /**
00440  * @brief  Set the LIS2MDL magnetometer sensor offset cancellation
00441  * @param  off_canc the offset cancellation value to be set
00442  * @retval 0 in case of success, an error code otherwise
00443  */
00444 int LIS2MDLSensor::set_m_off_canc(uint8_t off_canc)
00445 {
00446     lis2mdl_set_rst_t new_off_canc;
00447 
00448     new_off_canc = (off_canc == 0)  ? LIS2MDL_SET_SENS_ODR_DIV_63
00449               : (off_canc == 1)     ? LIS2MDL_SENS_OFF_CANC_EVERY_ODR
00450               :                     LIS2MDL_SET_SENS_ONLY_AT_POWER_ON;
00451 
00452     if (lis2mdl_set_rst_mode_set(&_reg_ctx, new_off_canc) != 0) {
00453         return 1;
00454     }
00455 
00456     return 0;
00457 }
00458 
00459 /**
00460  * @brief  Get the LIS2MDL magnetometer sensor axes
00461  * @param  magnetic_field pointer where the values of the axes are written
00462  * @retval 0 in case of success, an error code otherwise
00463  */
00464 int LIS2MDLSensor::get_m_axes(int32_t *magnetic_field)
00465 {
00466     axis3bit16_t data_raw;
00467     float sensitivity;
00468 
00469     /* Read raw data values. */
00470     if (lis2mdl_magnetic_raw_get(&_reg_ctx, data_raw.u8bit) != 0) {
00471         return 1;
00472     }
00473 
00474     /* Get LIS2MDL actual sensitivity. */
00475     if (get_m_sensitivity(&sensitivity) != 0) {
00476         return 1;
00477     }
00478 
00479     /* Calculate the data. */
00480     magnetic_field[0] = (int32_t)((float)((float)data_raw.i16bit[0] * sensitivity));
00481     magnetic_field[1] = (int32_t)((float)((float)data_raw.i16bit[1] * sensitivity));
00482     magnetic_field[2] = (int32_t)((float)((float)data_raw.i16bit[2] * sensitivity));
00483 
00484     return 0;
00485 }
00486 
00487 /**
00488  * @brief  Get the LIS2MDL magnetometer sensor raw axes
00489  * @param  value pointer where the raw values of the axes are written
00490  * @retval 0 in case of success, an error code otherwise
00491  */
00492 int LIS2MDLSensor::get_m_axes_raw(int16_t *value)
00493 {
00494     axis3bit16_t data_raw;
00495 
00496     /* Read raw data values. */
00497     if (lis2mdl_magnetic_raw_get(&_reg_ctx, data_raw.u8bit) != 0) {
00498         return 1;
00499     }
00500 
00501     /* Format the data. */
00502     value[0] = data_raw.i16bit[0];
00503     value[1] = data_raw.i16bit[1];
00504     value[2] = data_raw.i16bit[2];
00505 
00506     return 0;
00507 }
00508 
00509 /**
00510  * @brief  Get the LIS2MDL register value for magnetic sensor
00511  * @param  reg address to be read
00512  * @param  data pointer where the value is written
00513  * @retval 0 in case of success, an error code otherwise
00514  */
00515 int LIS2MDLSensor::read_reg(uint8_t reg, uint8_t *data)
00516 {
00517     if (lis2mdl_read_reg(&_reg_ctx, reg, data, 1) != 0) {
00518         return 1;
00519     }
00520 
00521     return 0;
00522 }
00523 
00524 /**
00525  * @brief  Set the LIS2MDL register value for magnetic sensor
00526  * @param  pObj the device pObj
00527  * @param  reg address to be written
00528  * @param  data value to be written
00529  * @retval 0 in case of success, an error code otherwise
00530  */
00531 int LIS2MDLSensor::write_reg(uint8_t reg, uint8_t data)
00532 {
00533     if (lis2mdl_write_reg(&_reg_ctx, reg, &data, 1) != 0) {
00534         return 1;
00535     }
00536 
00537     return 0;
00538 }
00539 
00540 /**
00541  * @brief  Set self test
00542  * @param  status the value of self_test in reg CFG_REG_C
00543  * @retval 0 in case of success, an error code otherwise
00544  */
00545 int LIS2MDLSensor::set_m_self_test(uint8_t status)
00546 {
00547     if (lis2mdl_self_test_set(&_reg_ctx, status) != 0) {
00548         return 1;
00549     }
00550 
00551     return 0;
00552 }
00553 
00554 /**
00555  * @brief  Get the LIS2MDL MAG data ready bit value
00556  * @param  status the status of data ready bit
00557  * @retval 0 in case of success, an error code otherwise
00558  */
00559 int LIS2MDLSensor::get_m_drdy_status(uint8_t *status)
00560 {
00561     if (lis2mdl_mag_data_ready_get(&_reg_ctx, status) != 0) {
00562         return 1;
00563     }
00564 
00565     return 0;
00566 }
00567 
00568 
00569 
00570 int32_t LIS2MDL_io_write(void *handle, uint8_t WriteAddr, uint8_t *pBuffer, uint16_t nBytesToWrite)
00571 {
00572     return ((LIS2MDLSensor *)handle)->io_write(pBuffer, WriteAddr, nBytesToWrite);
00573 }
00574 
00575 int32_t LIS2MDL_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead)
00576 {
00577     return ((LIS2MDLSensor *)handle)->io_read(pBuffer, ReadAddr, nBytesToRead);
00578 }