Diff: LIS2MDLSensor.cpp

Revision:

1:8562ae1a0534

Parent:

0:671edf39d961

Child:

2:0d9d7f8f871b

diff -r 671edf39d961 -r 8562ae1a0534 LIS2MDLSensor.cpp
--- a/LIS2MDLSensor.cpp	Tue Mar 05 18:06:37 2019 +0000
+++ b/LIS2MDLSensor.cpp	Wed Jul 24 14:18:39 2019 +0000
@@ -51,28 +51,26 @@
  */
 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)
 {
-  assert (spi);
-  if (cs_pin == NC) 
-  {
-    printf ("ERROR LIS2MDL CS MUST NOT BE NC\n\r");       
-    _dev_spi = NULL;
-    _dev_i2c = NULL;
-    return;
-  }
+    assert(spi);
+    if (cs_pin == NC) {
+        printf("ERROR LIS2MDL CS MUST NOT BE NC\n\r");
+        _dev_spi = NULL;
+        _dev_i2c = NULL;
+        return;
+    }
 
-  _reg_ctx.write_reg = LIS2MDL_io_write;
-  _reg_ctx.read_reg = LIS2MDL_io_read;
-  _reg_ctx.handle = (void *)this;
-  _cs_pin = 1;    
-  _dev_i2c = NULL;
-  _address = 0;
-    
-  if (_spi_type == SPI4W)
-  {
-    /* Enable SPI 4-Wires on the component (in this way we lose the usage of INT pin) */
-    uint8_t data = 0x34;
-    lis2mdl_write_reg(&_reg_ctx, LIS2MDL_CFG_REG_C, &data, 1);
-  }
+    _reg_ctx.write_reg = LIS2MDL_io_write;
+    _reg_ctx.read_reg = LIS2MDL_io_read;
+    _reg_ctx.handle = (void *)this;
+    _cs_pin = 1;
+    _dev_i2c = NULL;
+    _address = 0;
+
+    if (_spi_type == SPI4W) {
+        /* Enable SPI 4-Wires on the component (in this way we lose the usage of INT pin) */
+        uint8_t data = 0x34;
+        lis2mdl_write_reg(&_reg_ctx, LIS2MDL_CFG_REG_C, &data, 1);
+    }
 }
 
 /** Constructor
@@ -82,11 +80,11 @@
  */
 LIS2MDLSensor::LIS2MDLSensor(DevI2C *i2c, uint8_t address, PinName int_pin) : _dev_i2c(i2c), _address(address), _cs_pin(NC), _int_irq(int_pin)
 {
-  assert (i2c);
-  _dev_spi = NULL;
-  _reg_ctx.write_reg = LIS2MDL_io_write;
-  _reg_ctx.read_reg = LIS2MDL_io_read;
-  _reg_ctx.handle = (void *)this;
+    assert(i2c);
+    _dev_spi = NULL;
+    _reg_ctx.write_reg = LIS2MDL_io_write;
+    _reg_ctx.read_reg = LIS2MDL_io_read;
+    _reg_ctx.handle = (void *)this;
 }
 
 /**
@@ -96,33 +94,29 @@
  */
 int LIS2MDLSensor::init(void *init)
 {
-  /* Enable BDU */
-  if (lis2mdl_block_data_update_set(&(_reg_ctx), PROPERTY_ENABLE) != 0)
-  {
-    return 1;
-  }
+    /* Enable BDU */
+    if (lis2mdl_block_data_update_set(&(_reg_ctx), PROPERTY_ENABLE) != 0) {
+        return 1;
+    }
 
-  /* Operating mode selection - power down */
-  if (lis2mdl_operating_mode_set(&(_reg_ctx), LIS2MDL_POWER_DOWN) != 0)
-  {
-    return 1;
-  }
+    /* Operating mode selection - power down */
+    if (lis2mdl_operating_mode_set(&(_reg_ctx), LIS2MDL_POWER_DOWN) != 0) {
+        return 1;
+    }
 
-  /* Output data rate selection */
-  if (lis2mdl_data_rate_set(&(_reg_ctx), LIS2MDL_ODR_100Hz) != 0)
-  {
-    return 1;
-  }
+    /* Output data rate selection */
+    if (lis2mdl_data_rate_set(&(_reg_ctx), LIS2MDL_ODR_100Hz) != 0) {
+        return 1;
+    }
 
-  /* Self Test disabled. */
-  if (lis2mdl_self_test_set(&(_reg_ctx), PROPERTY_DISABLE) != 0)
-  {
-    return 1;
-  }
+    /* Self Test disabled. */
+    if (lis2mdl_self_test_set(&(_reg_ctx), PROPERTY_DISABLE) != 0) {
+        return 1;
+    }
 
-  _mag_is_enabled = 0;
+    _mag_is_enabled = 0;
 
-  return 0;
+    return 0;
 }
 
 /**
@@ -132,12 +126,11 @@
  */
 int LIS2MDLSensor::read_id(uint8_t *id)
 {
-  if (lis2mdl_device_id_get(&_reg_ctx, id) != 0)
-  {
-    return 1;
-  }
+    if (lis2mdl_device_id_get(&_reg_ctx, id) != 0) {
+        return 1;
+    }
 
-  return 0;
+    return 0;
 }
 
 
@@ -147,21 +140,19 @@
  */
 int LIS2MDLSensor::enable()
 {
-  /* Check if the component is already enabled */
-  if (_mag_is_enabled == 1U)
-  {
-    return 0;
-  }
+    /* Check if the component is already enabled */
+    if (_mag_is_enabled == 1U) {
+        return 0;
+    }
 
-  /* Output data rate selection. */
-  if (lis2mdl_operating_mode_set(&_reg_ctx, LIS2MDL_CONTINUOUS_MODE) != 0)
-  {
-    return 1;
-  }
+    /* Output data rate selection. */
+    if (lis2mdl_operating_mode_set(&_reg_ctx, LIS2MDL_CONTINUOUS_MODE) != 0) {
+        return 1;
+    }
 
-  _mag_is_enabled = 1;
+    _mag_is_enabled = 1;
 
-  return 0;
+    return 0;
 }
 
 /**
@@ -170,21 +161,19 @@
  */
 int LIS2MDLSensor::disable()
 {
-  /* Check if the component is already disabled */
-  if (_mag_is_enabled == 0U)
-  {
-    return 0;
-  }
+    /* Check if the component is already disabled */
+    if (_mag_is_enabled == 0U) {
+        return 0;
+    }
 
-  /* Output data rate selection - power down. */
-  if (lis2mdl_operating_mode_set(&_reg_ctx, LIS2MDL_POWER_DOWN) != 0)
-  {
-    return 1;
-  }
+    /* Output data rate selection - power down. */
+    if (lis2mdl_operating_mode_set(&_reg_ctx, LIS2MDL_POWER_DOWN) != 0) {
+        return 1;
+    }
 
-  _mag_is_enabled = 0;
+    _mag_is_enabled = 0;
 
-  return 0;
+    return 0;
 }
 
 /**
@@ -194,9 +183,9 @@
  */
 int LIS2MDLSensor::get_m_sensitivity(float *sensitivity)
 {
-  *sensitivity = LIS2MDL_MAG_SENSITIVITY_FS_50GAUSS;
+    *sensitivity = LIS2MDL_MAG_SENSITIVITY_FS_50GAUSS;
 
-  return 0;
+    return 0;
 }
 
 /**
@@ -206,39 +195,37 @@
  */
 int LIS2MDLSensor::get_m_odr(float *odr)
 {
-  int ret = 0;
-  lis2mdl_odr_t odr_low_level;
+    int ret = 0;
+    lis2mdl_odr_t odr_low_level;
 
-  /* Get current output data rate. */
-  if (lis2mdl_data_rate_get(&_reg_ctx, &odr_low_level) != 0)
-  {
-    return 1;
-  }
+    /* Get current output data rate. */
+    if (lis2mdl_data_rate_get(&_reg_ctx, &odr_low_level) != 0) {
+        return 1;
+    }
 
-  switch (odr_low_level)
-  {
-    case LIS2MDL_ODR_10Hz:
-      *odr = 10.0f;
-      break;
+    switch (odr_low_level) {
+        case LIS2MDL_ODR_10Hz:
+            *odr = 10.0f;
+            break;
 
-    case LIS2MDL_ODR_20Hz:
-      *odr = 20.0f;
-      break;
+        case LIS2MDL_ODR_20Hz:
+            *odr = 20.0f;
+            break;
 
-    case LIS2MDL_ODR_50Hz:
-      *odr = 50.0f;
-      break;
+        case LIS2MDL_ODR_50Hz:
+            *odr = 50.0f;
+            break;
 
-    case LIS2MDL_ODR_100Hz:
-      *odr = 100.0f;
-      break;
+        case LIS2MDL_ODR_100Hz:
+            *odr = 100.0f;
+            break;
 
-    default:
-      ret = 1;
-      break;
-  }
+        default:
+            ret = 1;
+            break;
+    }
 
-  return ret;
+    return ret;
 }
 
 /**
@@ -248,19 +235,18 @@
  */
 int LIS2MDLSensor::set_m_odr(float odr)
 {
-  lis2mdl_odr_t new_odr;
-
-  new_odr = (odr <= 10.000f) ? LIS2MDL_ODR_10Hz
-          : (odr <= 20.000f) ? LIS2MDL_ODR_20Hz
-          : (odr <= 50.000f) ? LIS2MDL_ODR_50Hz
-          :                    LIS2MDL_ODR_100Hz;
+    lis2mdl_odr_t new_odr;
 
-  if (lis2mdl_data_rate_set(&_reg_ctx, new_odr) != 0)
-  {
-    return 1;
-  }
+    new_odr = (odr <= 10.000f) ? LIS2MDL_ODR_10Hz
+              : (odr <= 20.000f) ? LIS2MDL_ODR_20Hz
+              : (odr <= 50.000f) ? LIS2MDL_ODR_50Hz
+              :                    LIS2MDL_ODR_100Hz;
 
-  return 0;
+    if (lis2mdl_data_rate_set(&_reg_ctx, new_odr) != 0) {
+        return 1;
+    }
+
+    return 0;
 }
 
 /**
@@ -270,9 +256,9 @@
  */
 int LIS2MDLSensor::get_m_fs(float *full_scale)
 {
-  *full_scale = 50.0f;
+    *full_scale = 50.0f;
 
-  return 0;
+    return 0;
 }
 
 /**
@@ -282,8 +268,8 @@
  */
 int LIS2MDLSensor::set_m_fs(float full_scale)
 {
-  (void)full_scale;
-  return 0;
+    (void)full_scale;
+    return 0;
 }
 
 /**
@@ -293,27 +279,25 @@
  */
 int LIS2MDLSensor::get_m_axes(int32_t *magnetic_field)
 {
-  axis3bit16_t data_raw;
-  float sensitivity;
+    axis3bit16_t data_raw;
+    float sensitivity;
 
-  /* Read raw data values. */
-  if (lis2mdl_magnetic_raw_get(&_reg_ctx, data_raw.u8bit) != 0)
-  {
-    return 1;
-  }
+    /* Read raw data values. */
+    if (lis2mdl_magnetic_raw_get(&_reg_ctx, data_raw.u8bit) != 0) {
+        return 1;
+    }
 
-  /* Get LIS2MDL actual sensitivity. */
-  if (get_m_sensitivity(&sensitivity) != 0)
-  {
-    return 1;
-  }
+    /* Get LIS2MDL actual sensitivity. */
+    if (get_m_sensitivity(&sensitivity) != 0) {
+        return 1;
+    }
 
-  /* Calculate the data. */
-  magnetic_field[0] = (int32_t)((float)((float)data_raw.i16bit[0] * sensitivity));
-  magnetic_field[1] = (int32_t)((float)((float)data_raw.i16bit[1] * sensitivity));
-  magnetic_field[2] = (int32_t)((float)((float)data_raw.i16bit[2] * sensitivity));
+    /* Calculate the data. */
+    magnetic_field[0] = (int32_t)((float)((float)data_raw.i16bit[0] * sensitivity));
+    magnetic_field[1] = (int32_t)((float)((float)data_raw.i16bit[1] * sensitivity));
+    magnetic_field[2] = (int32_t)((float)((float)data_raw.i16bit[2] * sensitivity));
 
-  return 0;
+    return 0;
 }
 
 /**
@@ -323,20 +307,19 @@
  */
 int LIS2MDLSensor::get_m_axes_raw(int16_t *value)
 {
-  axis3bit16_t data_raw;
+    axis3bit16_t data_raw;
+
+    /* Read raw data values. */
+    if (lis2mdl_magnetic_raw_get(&_reg_ctx, data_raw.u8bit) != 0) {
+        return 1;
+    }
 
-  /* Read raw data values. */
-  if (lis2mdl_magnetic_raw_get(&_reg_ctx, data_raw.u8bit) != 0)
-  {
-    return 1;
-  }
+    /* Format the data. */
+    value[0] = data_raw.i16bit[0];
+    value[1] = data_raw.i16bit[1];
+    value[2] = data_raw.i16bit[2];
 
-  /* Format the data. */
-  value[0] = data_raw.i16bit[0];
-  value[1] = data_raw.i16bit[1];
-  value[2] = data_raw.i16bit[2];
-
-  return 0;
+    return 0;
 }
 
 /**
@@ -347,12 +330,11 @@
  */
 int LIS2MDLSensor::read_reg(uint8_t reg, uint8_t *data)
 {
-  if (lis2mdl_read_reg(&_reg_ctx, reg, data, 1) != 0)
-  {
-    return 1;
-  }
+    if (lis2mdl_read_reg(&_reg_ctx, reg, data, 1) != 0) {
+        return 1;
+    }
 
-  return 0;
+    return 0;
 }
 
 /**
@@ -364,12 +346,11 @@
  */
 int LIS2MDLSensor::write_reg(uint8_t reg, uint8_t data)
 {
-  if (lis2mdl_write_reg(&_reg_ctx, reg, &data, 1) != 0)
-  {
-    return 1;
-  }
+    if (lis2mdl_write_reg(&_reg_ctx, reg, &data, 1) != 0) {
+        return 1;
+    }
 
-  return 0;
+    return 0;
 }
 
 /**
@@ -379,12 +360,11 @@
  */
 int LIS2MDLSensor::set_m_self_test(uint8_t status)
 {
-  if (lis2mdl_self_test_set(&_reg_ctx, status) != 0)
-  {
-    return 1;
-  }
+    if (lis2mdl_self_test_set(&_reg_ctx, status) != 0) {
+        return 1;
+    }
 
-  return 0;
+    return 0;
 }
 
 /**
@@ -394,22 +374,21 @@
  */
 int LIS2MDLSensor::get_m_drdy_status(uint8_t *status)
 {
-  if (lis2mdl_mag_data_ready_get(&_reg_ctx, status) != 0)
-  {
-    return 1;
-  }
+    if (lis2mdl_mag_data_ready_get(&_reg_ctx, status) != 0) {
+        return 1;
+    }
 
-  return 0;
+    return 0;
 }
 
 
 
 int32_t LIS2MDL_io_write(void *handle, uint8_t WriteAddr, uint8_t *pBuffer, uint16_t nBytesToWrite)
 {
-  return ((LIS2MDLSensor *)handle)->io_write(pBuffer, WriteAddr, nBytesToWrite);
+    return ((LIS2MDLSensor *)handle)->io_write(pBuffer, WriteAddr, nBytesToWrite);
 }
 
 int32_t LIS2MDL_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead)
 {
-  return ((LIS2MDLSensor *)handle)->io_read(pBuffer, ReadAddr, nBytesToRead);
+    return ((LIS2MDLSensor *)handle)->io_read(pBuffer, ReadAddr, nBytesToRead);
 }