Added template for LSM303AGR Driver (Still incomplete)

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@11746 110e8d01-0319-4d1e-a829-52ad28d1bb01

3 files changed, 2168 insertions, 0 deletions

diff --git a/os/ex/ST/lsm303agr.c b/os/ex/ST/lsm303agr.c
new file mode 100644
index 000000000..747ef087d
--- /dev/null
+++ b/os/ex/ST/lsm303agr.c
@@ -0,0 +1,1177 @@
+/*

+    ChibiOS - Copyright (C) 2016..2018 Rocco Marco Guglielmi

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+	

+*/

+

+/**

+ * @file    lsm303agr.c

+ * @brief   LSM303AGR MEMS interface module code.

+ *

+ * @addtogroup LSM303AGR

+ * @ingroup EX_ST

+ * @{

+ */

+

+#include "hal.h"

+#include "lsm303agr.h"

+

+/*===========================================================================*/

+/* Driver local definitions.                                                 */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Driver exported variables.                                                */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Driver local variables and types.                                         */

+/*===========================================================================*/

+

+/**

+ * @brief  Accelerometer and Compass Slave Address.

+ */

+typedef enum {

+  LSM303AGR_SAD_ACC = 0x19,        /**< SAD for accelerometer.             */

+  LSM303AGR_SAD_COMP = 0x1E        /**< SAD for compass.                   */

+} lsm303agr_sad_t;

+

+/*===========================================================================*/

+/* Driver local functions.                                                   */

+/*===========================================================================*/

+

+/**

+ * @brief   Reads registers value using I2C.

+ * @pre     The I2C interface must be initialized and the driver started.

+ * @note    IF_ADD_INC bit must be 1 in CTRL_REG8.

+ *

+ * @param[in] i2cp       pointer to the I2C interface.

+ * @param[in] sad        slave address without R bit.

+ * @param[in] reg        first sub-register address.

+ * @param[in] rxbuf      receiving buffer.

+ * @param[in] n          size of rxbuf.

+ * @return               the operation status.

+ */

+static msg_t lsm303agrI2CReadRegister(I2CDriver *i2cp, lsm303agr_sad_t sad,

+                                       uint8_t reg, uint8_t *rxbuf, size_t n) {

+

+  uint8_t txbuf = reg | LSM303AGR_MS;

+  return i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, n,

+                                  TIME_INFINITE);

+}

+

+/**

+ * @brief   Writes a value into a register using I2C.

+ * @pre     The I2C interface must be initialized and the driver started.

+ *

+ * @param[in] i2cp      pointer to the I2C interface.

+ * @param[in] sad       slave address without R bit.

+ * @param[in] txbuf     buffer containing sub-address value in first position

+ *                      and values to write.

+ * @param[in] n         size of txbuf less one (not considering the first

+ *                      element).

+ * @return              the operation status.

+ */

+static msg_t lsm303agrI2CWriteRegister(I2CDriver *i2cp, lsm303agr_sad_t sad,

+                                        uint8_t *txbuf, size_t n) {

+  if (n != 1)

+    *txbuf |= LSM303AGR_MS;

+  return i2cMasterTransmitTimeout(i2cp, sad, txbuf, n + 1, NULL, 0,

+                                  TIME_INFINITE);

+}

+

+/**

+ * @brief   Return the number of axes of the BaseAccelerometer.

+ *

+ * @param[in] ip        pointer to @p BaseAccelerometer interface.

+ *

+ * @return              the number of axes.

+ */

+static size_t acc_get_axes_number(void *ip) {

+  (void)ip;

+

+  return LSM303AGR_ACC_NUMBER_OF_AXES;

+}

+

+/**

+ * @brief   Retrieves raw data from the BaseAccelerometer.

+ * @note    This data is retrieved from MEMS register without any algebraical

+ *          manipulation.

+ * @note    The axes array must be at least the same size of the

+ *          BaseAccelerometer axes number.

+ *

+ * @param[in] ip        pointer to @p BaseAccelerometer interface.

+ * @param[out] axes     a buffer which would be filled with raw data.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    if one or more I2C errors occurred, the errors can

+ *                      be retrieved using @p i2cGetErrors().

+ * @retval MSG_TIMEOUT  if a timeout occurred before operation end.

+ */

+static msg_t acc_read_raw(void *ip, int32_t axes[]) {

+  LSM303AGRDriver* devp;

+  uint8_t buff [LSM303AGR_ACC_NUMBER_OF_AXES * 2], i;

+  int16_t tmp;

+  msg_t msg;

+

+  osalDbgCheck((ip != NULL) && (axes != NULL));

+

+  /* Getting parent instance pointer.*/

+  devp = objGetInstance(LSM303AGRDriver*, (BaseAccelerometer*)ip);

+

+  osalDbgAssert((devp->state == LSM303AGR_READY),

+                "acc_read_raw(), invalid state");

+  osalDbgAssert((devp->config->i2cp->state == I2C_READY),

+                "acc_read_raw(), channel not ready");

+

+#if LSM303AGR_SHARED_I2C

+  i2cAcquireBus(devp->config->i2cp);

+  i2cStart(devp->config->i2cp,

+           devp->config->i2ccfg);

+#endif /* LSM303AGR_SHARED_I2C */

+

+  msg = lsm303agrI2CReadRegister(devp->config->i2cp, LSM303AGR_SAD_ACC,

+                                  LSM303AGR_AD_ACC_OUT_X_L, buff,

+                                  LSM303AGR_ACC_NUMBER_OF_AXES * 2);

+

+#if LSM303AGR_SHARED_I2C

+  i2cReleaseBus(devp->config->i2cp);

+#endif /* LSM303AGR_SHARED_I2C */

+

+  if(msg == MSG_OK)

+    for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {

+      tmp = buff[2 * i] + (buff[2 * i + 1] << 8);

+      axes[i] = (int32_t)tmp;

+    }

+  return msg;

+}

+

+/**

+ * @brief   Retrieves cooked data from the BaseAccelerometer.

+ * @note    This data is manipulated according to the formula

+ *          cooked = (raw * sensitivity) - bias.

+ * @note    Final data is expressed as milli-G.

+ * @note    The axes array must be at least the same size of the

+ *          BaseAccelerometer axes number.

+ *

+ * @param[in] ip        pointer to @p BaseAccelerometer interface.

+ * @param[out] axes     a buffer which would be filled with cooked data.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    if one or more I2C errors occurred, the errors can

+ *                      be retrieved using @p i2cGetErrors().

+ * @retval MSG_TIMEOUT  if a timeout occurred before operation end.

+ */

+static msg_t acc_read_cooked(void *ip, float axes[]) {

+  LSM303AGRDriver* devp;

+  uint32_t i;

+  int32_t raw[LSM303AGR_ACC_NUMBER_OF_AXES];

+  msg_t msg;

+

+  osalDbgCheck((ip != NULL) && (axes != NULL));

+

+  /* Getting parent instance pointer.*/

+  devp = objGetInstance(LSM303AGRDriver*, (BaseAccelerometer*)ip);

+

+  osalDbgAssert((devp->state == LSM303AGR_READY),

+                "acc_read_cooked(), invalid state");

+

+  msg = acc_read_raw(ip, raw);

+  for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {

+    axes[i] = (raw[i] * devp->accsensitivity[i]) - devp->accbias[i];

+  }

+  return msg;

+}

+

+/**

+ * @brief   Set bias values for the BaseAccelerometer.

+ * @note    Bias must be expressed as milli-G.

+ * @note    The bias buffer must be at least the same size of the

+ *          BaseAccelerometer axes number.

+ *

+ * @param[in] ip        pointer to @p BaseAccelerometer interface.

+ * @param[in] bp        a buffer which contains biases.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ */

+static msg_t acc_set_bias(void *ip, float *bp) {

+  LSM303AGRDriver* devp;

+  uint32_t i;

+  msg_t msg = MSG_OK;

+

+  osalDbgCheck((ip != NULL) && (bp != NULL));

+

+  /* Getting parent instance pointer.*/

+  devp = objGetInstance(LSM303AGRDriver*, (BaseAccelerometer*)ip);

+

+  osalDbgAssert((devp->state == LSM303AGR_READY),

+                "acc_set_bias(), invalid state");

+

+  for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {

+    devp->accbias[i] = bp[i];

+  }

+  return msg;

+}

+

+/**

+ * @brief   Reset bias values for the BaseAccelerometer.

+ * @note    Default biases value are obtained from device datasheet when

+ *          available otherwise they are considered zero.

+ *

+ * @param[in] ip        pointer to @p BaseAccelerometer interface.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ */

+static msg_t acc_reset_bias(void *ip) {

+  LSM303AGRDriver* devp;

+  uint32_t i;

+  msg_t msg = MSG_OK;

+

+  osalDbgCheck(ip != NULL);

+

+  /* Getting parent instance pointer.*/

+  devp = objGetInstance(LSM303AGRDriver*, (BaseAccelerometer*)ip);

+

+  osalDbgAssert((devp->state == LSM303AGR_READY),

+                "acc_reset_bias(), invalid state");

+

+  for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++)

+    devp->accbias[i] = LSM303AGR_ACC_BIAS;

+  return msg;

+}

+

+/**

+ * @brief   Set sensitivity values for the BaseAccelerometer.

+ * @note    Sensitivity must be expressed as milli-G/LSB.

+ * @note    The sensitivity buffer must be at least the same size of the

+ *          BaseAccelerometer axes number.

+ *

+ * @param[in] ip        pointer to @p BaseAccelerometer interface.

+ * @param[in] sp        a buffer which contains sensitivities.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ */

+static msg_t acc_set_sensivity(void *ip, float *sp) {

+  LSM303AGRDriver* devp;

+  uint32_t i;

+  msg_t msg = MSG_OK;

+

+  /* Getting parent instance pointer.*/

+  devp = objGetInstance(LSM303AGRDriver*, (BaseAccelerometer*)ip);

+

+  osalDbgCheck((ip != NULL) && (sp != NULL));

+

+  osalDbgAssert((devp->state == LSM303AGR_READY),

+                "acc_set_sensivity(), invalid state");

+

+  for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {

+    devp->accsensitivity[i] = sp[i];

+  }

+  return msg;

+}

+

+/**

+ * @brief   Reset sensitivity values for the BaseAccelerometer.

+ * @note    Default sensitivities value are obtained from device datasheet.

+ *

+ * @param[in] ip        pointer to @p BaseAccelerometer interface.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    otherwise.

+ */

+static msg_t acc_reset_sensivity(void *ip) {

+  LSM303AGRDriver* devp;

+  uint32_t i;

+  msg_t msg = MSG_OK;

+

+  osalDbgCheck(ip != NULL);

+

+  /* Getting parent instance pointer.*/

+  devp = objGetInstance(LSM303AGRDriver*, (BaseAccelerometer*)ip);

+

+  osalDbgAssert((devp->state == LSM303AGR_READY),

+                "acc_reset_sensivity(), invalid state");

+

+  if(devp->config->accfullscale == LSM303AGR_ACC_FS_2G)

+    for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++)

+      devp->accsensitivity[i] = LSM303AGR_ACC_SENS_2G;

+  else if(devp->config->accfullscale == LSM303AGR_ACC_FS_4G)

+	for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++)

+      devp->accsensitivity[i] = LSM303AGR_ACC_SENS_4G;

+  else if(devp->config->accfullscale == LSM303AGR_ACC_FS_8G)

+	for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++)

+      devp->accsensitivity[i] = LSM303AGR_ACC_SENS_8G;

+  else if(devp->config->accfullscale == LSM303AGR_ACC_FS_16G)

+	for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++)

+      devp->accsensitivity[i] = LSM303AGR_ACC_SENS_16G;

+  else {

+    osalDbgAssert(FALSE, "reset_sensivity(), accelerometer full scale issue");

+    msg = MSG_RESET;

+  }

+  return msg;

+}

+

+/**

+ * @brief   Changes the LSM303AGRDriver accelerometer fullscale value.

+ * @note    This function also rescale sensitivities and biases based on

+ *          previous and next fullscale value.

+ * @note    A recalibration is highly suggested after calling this function.

+ *

+ * @param[in] ip        pointer to @p LSM303AGRDriver interface.

+ * @param[in] fs        new fullscale value.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    otherwise.

+ */

+static msg_t acc_set_full_scale(LSM303AGRDriver *devp,

+                                lsm303agr_acc_fs_t fs) {

+  float newfs, scale;

+  uint8_t i, buff[2];

+  msg_t msg;

+

+  osalDbgCheck(devp != NULL);

+

+  osalDbgAssert((devp->state == LSM303AGR_READY),

+                "acc_set_full_scale(), invalid state");

+  osalDbgAssert((devp->config->i2cp->state == I2C_READY),

+                "acc_set_full_scale(), channel not ready");

+

+  /* Computing new fullscale value.*/

+  if(fs == LSM303AGR_ACC_FS_2G) {

+    newfs = LSM303AGR_ACC_2G;

+  }

+  else if(fs == LSM303AGR_ACC_FS_4G) {

+    newfs = LSM303AGR_ACC_4G;

+  }

+  else if(fs == LSM303AGR_ACC_FS_8G) {

+    newfs = LSM303AGR_ACC_8G;

+  }

+  else if(fs == LSM303AGR_ACC_FS_16G) {

+    newfs = LSM303AGR_ACC_16G;

+  }

+  else {

+    msg = MSG_RESET;

+    return msg;

+  }

+

+  if(newfs != devp->accfullscale) {

+    /* Computing scale value.*/

+    scale = newfs / devp->accfullscale;

+    devp->accfullscale = newfs;

+

+#if LSM303AGR_SHARED_I2C

+		i2cAcquireBus(devp->config->i2cp);

+		i2cStart(devp->config->i2cp,

+						 devp->config->i2ccfg);

+#endif /* LSM303AGR_SHARED_I2C */

+

+    /* Updating register.*/

+    msg = lsm303agrI2CReadRegister(devp->config->i2cp,

+                                   LSM303AGR_SAD_ACC,

+                                   LSM303AGR_AD_ACC_CTRL_REG4,

+                                   &buff[1], 1);

+

+#if LSM303AGR_SHARED_I2C

+        i2cReleaseBus(devp->config->i2cp);

+#endif /* LSM303AGR_SHARED_I2C */

+

+    if(msg != MSG_OK)

+      return msg;

+

+    buff[1] &= ~(LSM303AGR_CTRL_REG4_A_FS_MASK);

+    buff[1] |= fs;

+    buff[0] = LSM303AGR_AD_ACC_CTRL_REG4;

+

+#if LSM303AGR_SHARED_I2C

+    i2cAcquireBus(devp->config->i2cp);

+    i2cStart(devp->config->i2cp, devp->config->i2ccfg);

+#endif /* LSM303AGR_SHARED_I2C */

+

+    msg = lsm303agrI2CWriteRegister(devp->config->i2cp,

+                                    LSM303AGR_SAD_ACC, buff, 1);

+

+#if LSM303AGR_SHARED_I2C

+		i2cReleaseBus(devp->config->i2cp);

+#endif /* LSM303AGR_SHARED_I2C */

+

+    if(msg != MSG_OK)

+      return msg;

+

+    /* Scaling sensitivity and bias. Re-calibration is suggested anyway.*/

+    for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {

+      devp->accsensitivity[i] *= scale;

+      devp->accbias[i] *= scale;

+    }

+  }

+  return msg;

+}

+

+/**

+ * @brief   Return the number of axes of the BaseCompass.

+ *

+ * @param[in] ip        pointer to @p BaseCompass interface

+ *

+ * @return              the number of axes.

+ */

+static size_t comp_get_axes_number(void *ip) {

+

+  osalDbgCheck(ip != NULL);

+  return LSM303AGR_COMP_NUMBER_OF_AXES;

+}

+

+/**

+ * @brief   Retrieves raw data from the BaseCompass.

+ * @note    This data is retrieved from MEMS register without any algebraical

+ *          manipulation.

+ * @note    The axes array must be at least the same size of the

+ *          BaseCompass axes number.

+ *

+ * @param[in] ip        pointer to @p BaseCompass interface.

+ * @param[out] axes     a buffer which would be filled with raw data.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    if one or more I2C errors occurred, the errors can

+ *                      be retrieved using @p i2cGetErrors().

+ * @retval MSG_TIMEOUT  if a timeout occurred before operation end.

+ */

+static msg_t comp_read_raw(void *ip, int32_t axes[]) {

+  LSM303AGRDriver* devp;

+  uint8_t buff [LSM303AGR_COMP_NUMBER_OF_AXES * 2], i;

+  int16_t tmp;

+  msg_t msg;

+

+  osalDbgCheck((ip != NULL) && (axes != NULL));

+

+  /* Getting parent instance pointer.*/

+  devp = objGetInstance(LSM303AGRDriver*, (BaseCompass*)ip);

+

+  osalDbgAssert((devp->state == LSM303AGR_READY),

+                "comp_read_raw(), invalid state");

+  osalDbgAssert((devp->config->i2cp->state == I2C_READY),

+                "comp_read_raw(), channel not ready");

+

+#if LSM303AGR_SHARED_I2C

+  i2cAcquireBus(devp->config->i2cp);

+  i2cStart(devp->config->i2cp,

+           devp->config->i2ccfg);

+#endif /* LSM303AGR_SHARED_I2C */

+  msg = lsm303agrI2CReadRegister(devp->config->i2cp, LSM303AGR_SAD_COMP,

+                                  LSM303AGR_AD_COMP_OUT_X_L, buff,

+                                  LSM303AGR_COMP_NUMBER_OF_AXES * 2);

+

+#if LSM303AGR_SHARED_I2C

+  i2cReleaseBus(devp->config->i2cp);

+#endif /* LSM303AGR_SHARED_I2C */

+

+  if(msg == MSG_OK)

+    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {

+      tmp = buff[2 * i] + (buff[2 * i + 1] << 8);

+      axes[i] = (int32_t)tmp;

+    }

+  return msg;

+}

+

+/**

+ * @brief   Retrieves cooked data from the BaseCompass.

+ * @note    This data is manipulated according to the formula

+ *          cooked = (raw * sensitivity) - bias.

+ * @note    Final data is expressed as G.

+ * @note    The axes array must be at least the same size of the

+ *          BaseCompass axes number.

+ *

+ * @param[in] ip        pointer to @p BaseCompass interface.

+ * @param[out] axes     a buffer which would be filled with cooked data.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    if one or more I2C errors occurred, the errors can

+ *                      be retrieved using @p i2cGetErrors().

+ * @retval MSG_TIMEOUT  if a timeout occurred before operation end.

+ */

+static msg_t comp_read_cooked(void *ip, float axes[]) {

+  LSM303AGRDriver* devp;

+  uint32_t i;

+  int32_t raw[LSM303AGR_COMP_NUMBER_OF_AXES];

+  msg_t msg;

+

+  osalDbgCheck((ip != NULL) && (axes != NULL));

+

+

+  /* Getting parent instance pointer.*/

+  devp = objGetInstance(LSM303AGRDriver*, (BaseCompass*)ip);

+

+  osalDbgAssert((devp->state == LSM303AGR_READY),

+                "comp_read_cooked(), invalid state");

+

+  msg = comp_read_raw(ip, raw);

+  for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES ; i++) {

+    axes[i] = (raw[i] * devp->compsensitivity[i]) - devp->compbias[i];

+  }

+  return msg;

+}

+

+/**

+ * @brief   Set bias values for the BaseCompass.

+ * @note    Bias must be expressed as G.

+ * @note    The bias buffer must be at least the same size of the

+ *          BaseCompass axes number.

+ *

+ * @param[in] ip        pointer to @p BaseCompass interface.

+ * @param[in] bp        a buffer which contains biases.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ */

+static msg_t comp_set_bias(void *ip, float *bp) {

+  LSM303AGRDriver* devp;

+  uint32_t i;

+  msg_t msg = MSG_OK;

+

+  osalDbgCheck((ip != NULL) && (bp != NULL));

+

+  /* Getting parent instance pointer.*/

+  devp = objGetInstance(LSM303AGRDriver*, (BaseCompass*)ip);

+

+  osalDbgAssert((devp->state == LSM303AGR_READY),

+                "comp_set_bias(), invalid state");

+

+  for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {

+    devp->compbias[i] = bp[i];

+  }

+  return msg;

+}

+

+/**

+ * @brief   Reset bias values for the BaseCompass.

+ * @note    Default biases value are obtained from device datasheet when

+ *          available otherwise they are considered zero.

+ *

+ * @param[in] ip        pointer to @p BaseCompass interface.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ */

+static msg_t comp_reset_bias(void *ip) {

+  LSM303AGRDriver* devp;

+  uint32_t i;

+  msg_t msg = MSG_OK;

+

+  osalDbgCheck(ip != NULL);

+

+  /* Getting parent instance pointer.*/

+  devp = objGetInstance(LSM303AGRDriver*, (BaseCompass*)ip);

+

+  osalDbgAssert((devp->state == LSM303AGR_READY),

+                "comp_reset_bias(), invalid state");

+

+  for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++)

+    devp->compbias[i] = LSM303AGR_COMP_BIAS;

+  return msg;

+}

+

+/**

+ * @brief   Set sensitivity values for the BaseCompass.

+ * @note    Sensitivity must be expressed as G/LSB.

+ * @note    The sensitivity buffer must be at least the same size of the

+ *          BaseCompass axes number.

+ *

+ * @param[in] ip        pointer to @p BaseCompass interface.

+ * @param[in] sp        a buffer which contains sensitivities.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ */

+static msg_t comp_set_sensivity(void *ip, float *sp) {

+  LSM303AGRDriver* devp;

+  uint32_t i;

+  msg_t msg = MSG_OK;

+

+  /* Getting parent instance pointer.*/

+  devp = objGetInstance(LSM303AGRDriver*, (BaseCompass*)ip);

+

+  osalDbgCheck((ip != NULL) && (sp != NULL));

+

+  osalDbgAssert((devp->state == LSM303AGR_READY),

+                "comp_set_sensivity(), invalid state");

+

+  for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {

+    devp->compsensitivity[i] = sp[i];

+  }

+  return msg;

+}

+

+/**

+ * @brief   Reset sensitivity values for the BaseCompass.

+ * @note    Default sensitivities value are obtained from device datasheet.

+ *

+ * @param[in] ip        pointer to @p BaseCompass interface.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    otherwise.

+ */

+static msg_t comp_reset_sensivity(void *ip) {

+  LSM303AGRDriver* devp;

+  uint32_t i;

+  msg_t msg = MSG_OK;

+

+  osalDbgCheck(ip != NULL);

+

+  /* Getting parent instance pointer.*/

+  devp = objGetInstance(LSM303AGRDriver*, (BaseCompass*)ip);

+

+  osalDbgAssert((devp->state == LSM303AGR_READY),

+                "comp_reset_sensivity(), invalid state");

+

+  if(devp->config->compfullscale == LSM303AGR_COMP_FS_1P3GA)

+    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {

+      if(i != 2) {

+        devp->compsensitivity[i] = LSM303AGR_COMP_SENS_XY_1P3GA;

+      }

+      else {

+        devp->compsensitivity[i] = LSM303AGR_COMP_SENS_Z_1P3GA;

+      }

+    }

+  else if(devp->config->compfullscale == LSM303AGR_COMP_FS_1P9GA)

+    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {

+      if(i != 2) {

+        devp->compsensitivity[i] = LSM303AGR_COMP_SENS_XY_1P9GA;

+      }

+      else {

+        devp->compsensitivity[i] = LSM303AGR_COMP_SENS_Z_1P9GA;

+      }

+    }

+  else if(devp->config->compfullscale == LSM303AGR_COMP_FS_2P5GA)

+    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {

+      if(i != 2) {

+        devp->compsensitivity[i] = LSM303AGR_COMP_SENS_XY_2P5GA;

+      }

+      else {

+        devp->compsensitivity[i] = LSM303AGR_COMP_SENS_Z_2P5GA;

+      }

+    }

+  else if(devp->config->compfullscale == LSM303AGR_COMP_FS_4P0GA)

+    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {

+      if(i != 2) {

+        devp->compsensitivity[i] = LSM303AGR_COMP_SENS_XY_4P0GA;

+      }

+      else {

+        devp->compsensitivity[i] = LSM303AGR_COMP_SENS_Z_4P0GA;

+      }

+    }

+  else if(devp->config->compfullscale == LSM303AGR_COMP_FS_4P7GA)

+    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {

+      if(i != 2) {

+        devp->compsensitivity[i] = LSM303AGR_COMP_SENS_XY_4P7GA;

+      }

+      else {

+        devp->compsensitivity[i] = LSM303AGR_COMP_SENS_Z_4P7GA;

+      }

+    }

+  else if(devp->config->compfullscale == LSM303AGR_COMP_FS_5P6GA)

+    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {

+      if(i != 2) {

+        devp->compsensitivity[i] = LSM303AGR_COMP_SENS_XY_5P6GA;

+      }

+      else {

+        devp->compsensitivity[i] = LSM303AGR_COMP_SENS_Z_5P6GA;

+      }

+    }

+  else if(devp->config->compfullscale == LSM303AGR_COMP_FS_8P1GA)

+    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {

+      if(i != 2) {

+        devp->compsensitivity[i] = LSM303AGR_COMP_SENS_XY_8P1GA;

+      }

+      else {

+        devp->compsensitivity[i] = LSM303AGR_COMP_SENS_Z_8P1GA;

+      }

+    }

+  else {

+    osalDbgAssert(FALSE, "comp_reset_sensivity(), compass full scale issue");

+    msg = MSG_RESET;

+  }

+  return msg;

+}

+

+/**

+ * @brief   Changes the LSM303AGRDriver compass fullscale value.

+ * @note    This function also rescale sensitivities and biases based on

+ *          previous and next fullscale value.

+ * @note    A recalibration is highly suggested after calling this function.

+ *

+ * @param[in] ip        pointer to @p LSM303AGRDriver interface.

+ * @param[in] fs        new fullscale value.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    otherwise.

+ */

+static msg_t comp_set_full_scale(LSM303AGRDriver *devp,

+                                 lsm303agr_comp_fs_t fs) {

+  float newfs, scale;

+  uint8_t i, buff[2];

+  msg_t msg;

+

+  osalDbgCheck(devp != NULL);

+

+  osalDbgAssert((devp->state == LSM303AGR_READY),

+                "comp_set_full_scale(), invalid state");

+  osalDbgAssert((devp->config->i2cp->state == I2C_READY),

+                "comp_set_full_scale(), channel not ready");

+

+  /* Computing new fullscale value.*/

+  if(fs == LSM303AGR_COMP_FS_1P3GA) {

+    newfs = LSM303AGR_COMP_1P3GA;

+  }

+  else if(fs == LSM303AGR_COMP_FS_1P9GA) {

+    newfs = LSM303AGR_COMP_1P9GA;

+  }

+  else if(fs == LSM303AGR_COMP_FS_2P5GA) {

+    newfs = LSM303AGR_COMP_2P5GA;

+  }

+  else if(fs == LSM303AGR_COMP_FS_4P0GA) {

+    newfs = LSM303AGR_COMP_4P0GA;

+  }

+  else if(fs == LSM303AGR_COMP_FS_4P7GA) {

+    newfs = LSM303AGR_COMP_4P7GA;

+  }

+  else if(fs == LSM303AGR_COMP_FS_5P6GA) {

+    newfs = LSM303AGR_COMP_5P6GA;

+  }

+  else if(fs == LSM303AGR_COMP_FS_8P1GA) {

+    newfs = LSM303AGR_COMP_8P1GA;

+  }

+  else {

+    msg = MSG_RESET;

+    return msg;

+  }

+

+  if(newfs != devp->compfullscale) {

+    /* Computing scale value.*/

+    scale = newfs / devp->compfullscale;

+    devp->compfullscale = newfs;

+

+#if LSM303AGR_SHARED_I2C

+    i2cAcquireBus(devp->config->i2cp);

+    i2cStart(devp->config->i2cp, devp->config->i2ccfg);

+#endif /* LSM303AGR_SHARED_I2C */

+

+    /* Updating register.*/

+    msg = lsm303agrI2CReadRegister(devp->config->i2cp, LSM303AGR_SAD_COMP,

+                                    LSM303AGR_AD_COMP_CRB_REG, &buff[1], 1);

+

+#if LSM303AGR_SHARED_I2C

+        i2cReleaseBus(devp->config->i2cp);

+#endif /* LSM303AGR_SHARED_I2C */

+

+    if(msg != MSG_OK)

+      return msg;

+    buff[1] &= ~(LSM303AGR_CRB_REG_M_GN_MASK);

+    buff[1] |= fs;

+    buff[0] = LSM303AGR_AD_COMP_CRB_REG;

+

+#if LSM303AGR_SHARED_I2C

+    i2cAcquireBus(devp->config->i2cp);

+    i2cStart(devp->config->i2cp, devp->config->i2ccfg);

+#endif /* LSM303AGR_SHARED_I2C */

+

+    msg = lsm303agrI2CWriteRegister(devp->config->i2cp, LSM303AGR_SAD_COMP,

+                                     buff, 1);

+

+#if LSM303AGR_SHARED_I2C

+        i2cReleaseBus(devp->config->i2cp);

+#endif /* LSM303AGR_SHARED_I2C */

+

+    if(msg != MSG_OK)

+      return msg;

+

+    /* Scaling sensitivity and bias. Re-calibration is suggested anyway.*/

+    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {

+      devp->compsensitivity[i] *= scale;

+      devp->compbias[i] *= scale;

+    }

+  }

+  return msg;

+}

+

+static const struct LSM303AGRVMT vmt_device = {

+  (size_t)0,

+  acc_set_full_scale, comp_set_full_scale

+};

+

+static const struct BaseAccelerometerVMT vmt_accelerometer = {

+  sizeof(struct LSM303AGRVMT*),

+  acc_get_axes_number, acc_read_raw, acc_read_cooked,

+  acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity

+};

+

+static const struct BaseCompassVMT vmt_compass = {

+  sizeof(struct LSM303AGRVMT*) + sizeof(BaseAccelerometer),

+  comp_get_axes_number, comp_read_raw, comp_read_cooked,

+  comp_set_bias, comp_reset_bias, comp_set_sensivity, comp_reset_sensivity

+};

+

+/*===========================================================================*/

+/* Driver exported functions.                                                */

+/*===========================================================================*/

+

+/**

+ * @brief   Initializes an instance.

+ *

+ * @param[out] devp     pointer to the @p LSM303AGRDriver object

+ *

+ * @init

+ */

+void lsm303agrObjectInit(LSM303AGRDriver *devp) {

+  devp->vmt = &vmt_device;

+  devp->acc_if.vmt = &vmt_accelerometer;

+  devp->comp_if.vmt = &vmt_compass;

+

+  devp->config = NULL;

+

+  devp->accaxes = LSM303AGR_ACC_NUMBER_OF_AXES;

+  devp->compaxes = LSM303AGR_COMP_NUMBER_OF_AXES;

+

+  devp->state = LSM303AGR_STOP;

+}

+

+/**

+ * @brief   Configures and activates LSM303AGR Complex Driver peripheral.

+ *

+ * @param[in] devp      pointer to the @p LSM303AGRDriver object

+ * @param[in] config    pointer to the @p LSM303AGRConfig object

+ *

+ * @api

+ */

+void lsm303agrStart(LSM303AGRDriver *devp, const LSM303AGRConfig *config) {

+  uint32_t i;

+  uint8_t cr[6];

+  osalDbgCheck((devp != NULL) && (config != NULL));

+

+  osalDbgAssert((devp->state == LSM303AGR_STOP) ||

+                (devp->state == LSM303AGR_READY),

+                "lsm303agrStart(), invalid state");

+

+  devp->config = config;

+

+  /* Configuring Accelerometer subsystem.*/

+

+  /* Multiple write starting address.*/

+  cr[0] = LSM303AGR_AD_ACC_CTRL_REG1;

+

+  /* Control register 1 configuration block.*/

+  {

+    cr[1] = LSM303AGR_CTRL_REG1_A_XEN | LSM303AGR_CTRL_REG1_A_YEN |

+            LSM303AGR_CTRL_REG1_A_ZEN | devp->config->accoutdatarate;

+#if LSM303AGR_ACC_USE_ADVANCED || defined(__DOXYGEN__)

+    cr[1] |= devp->config->acclowpower;

+#endif

+  }

+

+  /* Control register 2 configuration block.*/

+  {

+    cr[2] = 0;

+  }

+

+  /* Control register 3 configuration block.*/

+  {

+    cr[3] = 0;

+  }

+

+  /* Control register 4 configuration block.*/

+  {

+    cr[4] = devp->config->accfullscale;

+#if LSM303AGR_ACC_USE_ADVANCED || defined(__DOXYGEN__)

+    cr[4] |= devp->config->accendianess |

+             devp->config->accblockdataupdate |

+             devp->config->acchighresmode;

+#endif

+  }

+

+#if LSM303AGR_SHARED_I2C

+  i2cAcquireBus((devp)->config->i2cp);

+#endif /* LSM303AGR_SHARED_I2C */

+  i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);

+

+  lsm303agrI2CWriteRegister(devp->config->i2cp, LSM303AGR_SAD_ACC, cr, 4);

+

+#if LSM303AGR_SHARED_I2C

+  i2cReleaseBus((devp)->config->i2cp);

+#endif /* LSM303AGR_SHARED_I2C */

+

+  /* Storing sensitivity according to user settings */

+  if(devp->config->accfullscale == LSM303AGR_ACC_FS_2G) {

+    devp->accfullscale = LSM303AGR_ACC_2G;

+    for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {

+      if(devp->config->accsensitivity == NULL)

+        devp->accsensitivity[i] = LSM303AGR_ACC_SENS_2G;

+      else

+        devp->accsensitivity[i] = devp->config->accsensitivity[i];

+    }

+  }

+  else if(devp->config->accfullscale == LSM303AGR_ACC_FS_4G) {

+    devp->accfullscale = LSM303AGR_ACC_4G;

+    for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {

+      if(devp->config->accsensitivity == NULL)

+        devp->accsensitivity[i] = LSM303AGR_ACC_SENS_4G;

+      else

+        devp->accsensitivity[i] = devp->config->accsensitivity[i];

+    }

+  }

+  else if(devp->config->accfullscale == LSM303AGR_ACC_FS_8G) {

+    devp->accfullscale = LSM303AGR_ACC_8G;

+    for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {

+      if(devp->config->accsensitivity == NULL)

+        devp->accsensitivity[i] = LSM303AGR_ACC_SENS_8G;

+      else

+        devp->accsensitivity[i] = devp->config->accsensitivity[i];

+    }

+  }

+  else if(devp->config->accfullscale == LSM303AGR_ACC_FS_16G) {

+    devp->accfullscale = LSM303AGR_ACC_16G;

+    for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {

+      if(devp->config->accsensitivity == NULL)

+        devp->accsensitivity[i] = LSM303AGR_ACC_SENS_16G;

+      else

+        devp->accsensitivity[i] = devp->config->accsensitivity[i];

+    }

+  }

+  else

+    osalDbgAssert(FALSE, "lsm303agrStart(), accelerometer full scale issue");

+

+  /* Storing bias information */

+  if(devp->config->accbias != NULL)

+    for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++)

+      devp->accbias[i] = devp->config->accbias[i];

+  else

+    for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++)

+      devp->accbias[i] = LSM303AGR_ACC_BIAS;

+

+  /* Configuring Compass subsystem */

+  /* Multiple write starting address.*/

+  cr[0] = LSM303AGR_AD_COMP_CRA_REG;

+

+  /* Control register A configuration block.*/

+  {

+    cr[1] = devp->config->compoutputdatarate;

+  }

+

+  /* Control register B configuration block.*/

+  {

+    cr[2] = devp->config->compfullscale;

+  }

+

+  /* Mode register configuration block.*/

+  {

+    cr[3] = 0;

+#if LSM303AGR_COMP_USE_ADVANCED || defined(__DOXYGEN__)

+    cr[3] |= devp->config->compmode;

+#endif

+  }

+

+#if LSM303AGR_SHARED_I2C

+  i2cAcquireBus((devp)->config->i2cp);

+  i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);

+#endif /* LSM303AGR_SHARED_I2C */

+

+  lsm303agrI2CWriteRegister(devp->config->i2cp, LSM303AGR_SAD_COMP,

+                             cr, 3);

+

+#if LSM303AGR_SHARED_I2C

+  i2cReleaseBus((devp)->config->i2cp);

+#endif /* LSM303AGR_SHARED_I2C */

+

+  if(devp->config->compfullscale == LSM303AGR_COMP_FS_1P3GA) {

+    devp->compfullscale = LSM303AGR_COMP_1P3GA;

+    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {

+      if(devp->config->compsensitivity == NULL) {

+        if(i != 2) {

+          devp->compsensitivity[i] = LSM303AGR_COMP_SENS_XY_1P3GA;

+        }

+        else {

+          devp->compsensitivity[i] = LSM303AGR_COMP_SENS_Z_1P3GA;

+        }

+      }

+      else {

+        devp->compsensitivity[i] = devp->config->compsensitivity[i];

+      }

+    }

+  }

+  else if(devp->config->compfullscale == LSM303AGR_COMP_FS_1P9GA) {

+    devp->compfullscale = LSM303AGR_COMP_1P9GA;

+    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {

+      if(devp->config->compsensitivity == NULL) {

+        if(i != 2) {

+          devp->compsensitivity[i] = LSM303AGR_COMP_SENS_XY_1P9GA;

+        }

+        else {

+          devp->compsensitivity[i] = LSM303AGR_COMP_SENS_Z_1P9GA;

+        }

+      }

+      else {

+        devp->compsensitivity[i] = devp->config->compsensitivity[i];

+      }

+    }

+  }

+  else if(devp->config->compfullscale == LSM303AGR_COMP_FS_2P5GA) {

+    devp->compfullscale = LSM303AGR_COMP_2P5GA;

+    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {

+      if(devp->config->compsensitivity == NULL) {

+        if(i != 2) {

+          devp->compsensitivity[i] = LSM303AGR_COMP_SENS_XY_2P5GA;

+        }

+        else {

+          devp->compsensitivity[i] = LSM303AGR_COMP_SENS_Z_2P5GA;

+        }

+      }

+      else {

+        devp->compsensitivity[i] = devp->config->compsensitivity[i];

+      }

+    }

+  }

+  else if(devp->config->compfullscale == LSM303AGR_COMP_FS_4P0GA) {

+    devp->compfullscale = LSM303AGR_COMP_4P0GA;

+    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {

+      if(devp->config->compsensitivity == NULL) {

+        if(i != 2) {

+          devp->compsensitivity[i] = LSM303AGR_COMP_SENS_XY_4P0GA;

+        }

+        else {

+          devp->compsensitivity[i] = LSM303AGR_COMP_SENS_Z_4P0GA;

+        }

+      }

+      else {

+        devp->compsensitivity[i] = devp->config->compsensitivity[i];

+      }

+    }

+  }

+  else if(devp->config->compfullscale == LSM303AGR_COMP_FS_4P7GA) {

+    devp->compfullscale = LSM303AGR_COMP_4P7GA;

+    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {

+      if(devp->config->compsensitivity == NULL) {

+        if(i != 2) {

+          devp->compsensitivity[i] = LSM303AGR_COMP_SENS_XY_4P7GA;

+        }

+        else {

+          devp->compsensitivity[i] = LSM303AGR_COMP_SENS_Z_4P7GA;

+        }

+      }

+      else {

+        devp->compsensitivity[i] = devp->config->compsensitivity[i];

+      }

+    }

+  }

+  else if(devp->config->compfullscale == LSM303AGR_COMP_FS_5P6GA) {

+    devp->compfullscale = LSM303AGR_COMP_5P6GA;

+    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {

+      if(devp->config->compsensitivity == NULL) {

+        if(i != 2) {

+          devp->compsensitivity[i] = LSM303AGR_COMP_SENS_XY_5P6GA;

+        }

+        else {

+          devp->compsensitivity[i] = LSM303AGR_COMP_SENS_Z_5P6GA;

+        }

+      }

+      else {

+        devp->compsensitivity[i] = devp->config->compsensitivity[i];

+      }

+    }

+  }

+  else if(devp->config->compfullscale == LSM303AGR_COMP_FS_8P1GA) {

+    devp->compfullscale = LSM303AGR_COMP_8P1GA;

+    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {

+      if(devp->config->compsensitivity == NULL) {

+        if(i != 2) {

+          devp->compsensitivity[i] = LSM303AGR_COMP_SENS_XY_8P1GA;

+        }

+        else {

+          devp->compsensitivity[i] = LSM303AGR_COMP_SENS_Z_8P1GA;

+        }

+      }

+      else {

+        devp->compsensitivity[i] = devp->config->compsensitivity[i];

+      }

+    }

+  }

+  else

+    osalDbgAssert(FALSE, "lsm303agrStart(), compass full scale issue");

+

+  /* Storing bias information */

+  if(devp->config->compbias != NULL)

+    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++)

+      devp->compbias[i] = devp->config->compbias[i];

+  else

+    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++)

+      devp->compbias[i] = LSM303AGR_COMP_BIAS;

+

+  /* This is the MEMS transient recovery time */

+  osalThreadSleepMilliseconds(5);

+

+  devp->state = LSM303AGR_READY;

+}

+

+/**

+ * @brief   Deactivates the LSM303AGR Complex Driver peripheral.

+ *

+ * @param[in] devp       pointer to the @p LSM303AGRDriver object

+ *

+ * @api

+ */

+void lsm303agrStop(LSM303AGRDriver *devp) {

+  uint8_t cr[2];

+  osalDbgCheck(devp != NULL);

+

+  osalDbgAssert((devp->state == LSM303AGR_STOP) ||

+                (devp->state == LSM303AGR_READY),

+                "lsm303agrStop(), invalid state");

+

+  if (devp->state == LSM303AGR_READY) {

+#if	LSM303AGR_SHARED_I2C

+    i2cAcquireBus((devp)->config->i2cp);

+    i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);

+#endif /* LSM303AGR_SHARED_I2C */

+

+    /* Disabling accelerometer. */

+    cr[0] = LSM303AGR_AD_ACC_CTRL_REG1;

+    cr[1] = LSM303AGR_ACC_AE_DISABLED | LSM303AGR_ACC_ODR_PD;

+    lsm303agrI2CWriteRegister(devp->config->i2cp, LSM303AGR_SAD_ACC,

+                               cr, 1);

+

+    /* Disabling compass. */

+    cr[0] = LSM303AGR_AD_COMP_MR_REG;

+    cr[1] = LSM303AGR_COMP_MD_SLEEP;

+    lsm303agrI2CWriteRegister(devp->config->i2cp, LSM303AGR_SAD_ACC,

+                               cr, 1);

+    lsm303agrI2CWriteRegister(devp->config->i2cp, LSM303AGR_SAD_COMP,

+                               cr, 1);

+

+    i2cStop((devp)->config->i2cp);

+#if	LSM303AGR_SHARED_I2C

+    i2cReleaseBus((devp)->config->i2cp);

+#endif /* LSM303AGR_SHARED_I2C */

+  }			

+  devp->state = LSM303AGR_STOP;

+}

+/** @} */

diff --git a/os/ex/ST/lsm303agr.h b/os/ex/ST/lsm303agr.h
new file mode 100644
index 000000000..517d0dc76
--- /dev/null
+++ b/os/ex/ST/lsm303agr.h
@@ -0,0 +1,981 @@
+/*

+    ChibiOS - Copyright (C) 2016..2018 Rocco Marco Guglielmi

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+

+*/

+

+/**

+ * @file    lsm303agr.h

+ * @brief LSM303AGR MEMS interface module header.

+ *

+ * @addtogroup LSM303AGR

+ * @ingroup EX_ST

+ * @{

+ */

+#ifndef _LSM303AGR_H_

+#define _LSM303AGR_H_

+

+#include "hal_accelerometer.h"

+#include "hal_compass.h"

+

+/*===========================================================================*/

+/* Driver constants.                                                         */

+/*===========================================================================*/

+

+/**

+ * @name    Version identification

+ * @{

+ */

+/**

+ * @brief   LSM303AGR driver version string.

+ */

+#define EX_LSM303AGR_VERSION                "1.0.0"

+

+/**

+ * @brief   LSM303AGR driver version major number.

+ */

+#define EX_LSM303AGR_MAJOR                  1

+

+/**

+ * @brief   LSM303AGR driver version minor number.

+ */

+#define EX_LSM303AGR_MINOR                  0

+

+/**

+ * @brief   LSM303AGR driver version patch number.

+ */

+#define EX_LSM303AGR_PATCH                  0

+/** @} */

+

+/**

+ * @brief   LSM303AGR accelerometer subsystem characteristics.

+ * @note    Sensitivity is expressed as milli-G/LSB whereas

+ *          1 milli-G = 0.00980665 m/s^2.

+ * @note    Bias is expressed as milli-G.

+ *

+ * @{

+ */

+#define LSM303AGR_ACC_NUMBER_OF_AXES        3U

+

+#define LSM303AGR_ACC_2G                    2.0f

+#define LSM303AGR_ACC_4G                    4.0f

+#define LSM303AGR_ACC_8G                    8.0f

+#define LSM303AGR_ACC_16G                   16.0f

+

+#define LSM303AGR_ACC_SENS_HRES_2G          0.98f

+#define LSM303AGR_ACC_SENS_HRES_4G          1.95f

+#define LSM303AGR_ACC_SENS_HRES_8G          3.9f

+#define LSM303AGR_ACC_SENS_HRES_16G         11.7f

+

+#define LSM303AGR_ACC_SENS_NORM_2G          3.9f

+#define LSM303AGR_ACC_SENS_NORM_4G          7.82f

+#define LSM303AGR_ACC_SENS_NORM_8G          15.63f

+#define LSM303AGR_ACC_SENS_NORM_16G         46.9f

+

+#define LSM303AGR_ACC_SENS_LPOW_2G          15.63f

+#define LSM303AGR_ACC_SENS_LPOW_4G          31.26f

+#define LSM303AGR_ACC_SENS_LPOW_8G          62.52f

+#define LSM303AGR_ACC_SENS_LPOW_16G         187.58f

+

+#define LSM303AGR_ACC_BIAS                  0.0f

+/** @} */

+

+/**

+ * @brief   LSM303AGR compass subsystem characteristics.

+ * @note    Sensitivity is expressed as G/LSB whereas G stands for Gauss.

+ * @note    Bias is expressed as G.

+ *

+ * @{

+ */

+#define LSM303AGR_COMP_NUMBER_OF_AXES       3U

+

+#define LSM303AGR_COMP_50GA                 50.0f

+

+#define LSM303AGR_COMP_SENS_50GA            0.00015f

+

+#define LSM303AGR_COMP_BIAS                 0.0f

+/** @} */

+

+/**

+ * @name   LSM303AGR communication interfaces related bit masks

+ * @{

+ */

+#define LSM303AGR_DI_MASK                   0xFF

+#define LSM303AGR_DI(n)                     (1 << n)

+#define LSM303AGR_AD_MASK                   0x7F

+#define LSM303AGR_AD(n)                     (1 << n)

+#define LSM303AGR_MS                        (1 << 7)

+/** @} */

+

+/**

+ * @name    LSM303AGR register addresses

+ * @{

+ */

+#define LSM303AGR_AD_STATUS_REG_AUX_A       0x07

+#define LSM303AGR_AD_OUT_TEMP_L_A           0x0C

+#define LSM303AGR_AD_OUT_TEMP_H_A           0x0D

+#define LSM303AGR_AD_INT_COUNTER_REG_A      0x0E

+#define LSM303AGR_AD_WHO_AM_I_A             0x0F

+#define LSM303AGR_AD_TEMP_CFG_REG_A         0x1F

+#define LSM303AGR_AD_CTRL_REG1_A            0x20

+#define LSM303AGR_AD_CTRL_REG2_A            0x21

+#define LSM303AGR_AD_CTRL_REG3_A            0x22

+#define LSM303AGR_AD_CTRL_REG4_A            0x23

+#define LSM303AGR_AD_CTRL_REG5_A            0x24

+#define LSM303AGR_AD_CTRL_REG6_A            0x25

+#define LSM303AGR_AD_REFERENCE_A            0x26

+#define LSM303AGR_AD_STATUS_REG_A           0x27

+#define LSM303AGR_AD_OUT_X_L_A              0x28

+#define LSM303AGR_AD_OUT_X_H_A              0x29

+#define LSM303AGR_AD_OUT_Y_L_A              0x2A

+#define LSM303AGR_AD_OUT_Y_H_A              0x2B

+#define LSM303AGR_AD_OUT_Z_L_A              0x2C

+#define LSM303AGR_AD_OUT_Z_H_A              0x2D

+#define LSM303AGR_AD_FIFO_CTRL_REG_A        0x2E

+#define LSM303AGR_AD_FIFO_SRC_REG_A         0x2F

+#define LSM303AGR_AD_INT1_CFG_A             0x30

+#define LSM303AGR_AD_INT1_SRC_A             0x31

+#define LSM303AGR_AD_INT1_THS_A             0x32

+#define LSM303AGR_AD_INT1_DURATION_A        0x33

+#define LSM303AGR_AD_INT2_CFG_A             0x34

+#define LSM303AGR_AD_INT2_SRC_A             0x35

+#define LSM303AGR_AD_INT2_THS_A             0x36

+#define LSM303AGR_AD_INT2_DURATION_A        0x37

+#define LSM303AGR_AD_CLICK_CFG_A            0x38

+#define LSM303AGR_AD_CLICK_SRC_A            0x39

+#define LSM303AGR_AD_CLICK_THS_A            0x3A

+#define LSM303AGR_AD_TIME_LIMIT_A           0x3B

+#define LSM303AGR_AD_TIME_LATENCY_A         0x3C

+#define LSM303AGR_AD_TIME_WINDOW_A          0x3D

+#define LSM303AGR_AD_ACT_THS_A              0x3E

+#define LSM303AGR_AD_ACT_DUR_A              0x3F

+#define LSM303AGR_AD_OFFSET_X_REG_L_M       0x45

+#define LSM303AGR_AD_OFFSET_X_REG_H_M       0x46

+#define LSM303AGR_AD_OFFSET_Y_REG_L_M       0x47

+#define LSM303AGR_AD_OFFSET_Y_REG_H_M       0x48

+#define LSM303AGR_AD_OFFSET_Z_REG_L_M       0x49

+#define LSM303AGR_AD_OFFSET_Z_REG_H_M       0x4A

+#define LSM303AGR_AD_WHO_AM_I_M             0x4F

+#define LSM303AGR_AD_CFG_REG_A_M            0x60

+#define LSM303AGR_AD_CFG_REG_B_M            0x61

+#define LSM303AGR_AD_CFG_REG_C_M            0x62

+#define LSM303AGR_AD_INT_CRTL_REG_M         0x63

+#define LSM303AGR_AD_INT_SOURCE_REG_M       0x64

+#define LSM303AGR_AD_INT_THS_L_REG_M        0x65

+#define LSM303AGR_AD_INT_THS_H_REG_M        0x66

+#define LSM303AGR_AD_STATUS_REG_M           0x67

+#define LSM303AGR_AD_OUTX_L_REG_M           0x68

+#define LSM303AGR_AD_OUTX_H_REG_M           0x69

+#define LSM303AGR_AD_OUTY_L_REG_M           0x6A

+#define LSM303AGR_AD_OUTY_H_REG_M           0x6B

+#define LSM303AGR_AD_OUTZ_L_REG_M           0x6C

+#define LSM303AGR_AD_OUTZ_H_REG_M           0x6D

+/** @} */

+

+/**

+ * @name    LSM303AGR_TEMP_CFG_REG_A register bits definitions

+ * @{

+ */

+#define LSM303AGR_TEMP_CFG_REG_A_TEMP_EN0   (1 << 0)

+#define LSM303AGR_TEMP_CFG_REG_A_TEMP_EN1   (1 << 0)

+/** @} */

+

+/**

+ * @name    LSM303AGR_CTRL_REG1_A register bits definitions

+ * @{

+ */

+#define LSM303AGR_CTRL_REG1_A_XEN           (1 << 0)

+#define LSM303AGR_CTRL_REG1_A_YEN           (1 << 1)

+#define LSM303AGR_CTRL_REG1_A_ZEN           (1 << 2)

+#define LSM303AGR_CTRL_REG1_A_LPEN          (1 << 3)

+#define LSM303AGR_CTRL_REG1_A_ODR0          (1 << 4)

+#define LSM303AGR_CTRL_REG1_A_ODR1          (1 << 5)

+#define LSM303AGR_CTRL_REG1_A_ODR2          (1 << 6)

+#define LSM303AGR_CTRL_REG1_A_ODR3          (1 << 7)

+/** @} */

+

+/**

+ * @name    LSM303AGR_CTRL_REG2_A register bits definitions

+ * @{

+ */ 

+#define LSM303AGR_CTRL_REG2_A_HPIS1         (1 << 0)

+#define LSM303AGR_CTRL_REG2_A_HPIS2         (1 << 1)

+#define LSM303AGR_CTRL_REG2_A_HPCLICK       (1 << 2)

+#define LSM303AGR_CTRL_REG2_A_FDS           (1 << 3)

+#define LSM303AGR_CTRL_REG2_A_HPCF1         (1 << 4)

+#define LSM303AGR_CTRL_REG2_A_HPCF2         (1 << 5)

+#define LSM303AGR_CTRL_REG2_A_HPM0          (1 << 6)

+#define LSM303AGR_CTRL_REG2_A_HPM0          (1 << 7)

+/** @} */

+

+/**

+ * @name    LSM303AGR_CTRL_REG3_A register bits definitions

+ * @{

+ */

+#define LSM303AGR_CTRL_REG3_A_I1_OVERRUN    (1 << 1)

+#define LSM303AGR_CTRL_REG3_A_I1_WTM        (1 << 2)

+#define LSM303AGR_CTRL_REG3_A_I1_DRDY2      (1 << 3)

+#define LSM303AGR_CTRL_REG3_A_I1_DRDY1      (1 << 4)

+#define LSM303AGR_CTRL_REG3_A_I1_AOI2       (1 << 5)

+#define LSM303AGR_CTRL_REG3_A_I1_AOI1       (1 << 6)

+#define LSM303AGR_CTRL_REG3_A_I1_CLICK      (1 << 7)

+/** @} */

+

+/**

+ * @name    LSM303AGR_CTRL_REG4_A register bits definitions

+ * @{

+ */

+#define LSM303AGR_CTRL_REG4_A_SPI_ENABLE    (1 << 0) 

+#define LSM303AGR_CTRL_REG4_A_ST0           (1 << 1)

+#define LSM303AGR_CTRL_REG4_A_ST1           (1 << 2)

+#define LSM303AGR_CTRL_REG4_A_HR            (1 << 3)

+#define LSM303AGR_CTRL_REG4_A_FS_MASK       0x30

+#define LSM303AGR_CTRL_REG4_A_FS0           (1 << 4)

+#define LSM303AGR_CTRL_REG4_A_FS1           (1 << 5)

+#define LSM303AGR_CTRL_REG4_A_BLE           (1 << 6)

+#define LSM303AGR_CTRL_REG4_A_BDU           (1 << 7)

+/** @} */

+

+/**

+ * @name    LSM303AGR_CTRL_REG5_A register bits definitions

+ * @{

+ */

+#define LSM303AGR_CTRL_REG5_A_D4D_INT2      (1 << 0)

+#define LSM303AGR_CTRL_REG5_A_LIR_INT2      (1 << 1)

+#define LSM303AGR_CTRL_REG5_A_D4D_INT1      (1 << 2)

+#define LSM303AGR_CTRL_REG5_A_LIR_INT1      (1 << 3)

+#define LSM303AGR_CTRL_REG5_A_FIFO_EN       (1 << 6)

+#define LSM303AGR_CTRL_REG5_A_BOOT          (1 << 7)

+/** @} */

+

+/**

+ * @name    LSM303AGR_CTRL_REG6_A register bits definitions

+ * @{

+ */

+#define LSM303AGR_CTRL_REG6_A_H_LACTIVE     (1 << 1)

+#define LSM303AGR_CTRL_REG6_A_P2_ACT        (1 << 3)

+#define LSM303AGR_CTRL_REG6_A_BOOT_I2       (1 << 4)

+#define LSM303AGR_CTRL_REG6_A_I2_INT2       (1 << 5)

+#define LSM303AGR_CTRL_REG6_A_I2_INT1       (1 << 6)

+#define LSM303AGR_CTRL_REG6_A_I2_CLICKEN    (1 << 7)

+/** @} */

+

+/**

+ * @name    LSM303AGR_CFG_REG_A register bits definitions

+ * @{

+ */

+#define LSM303AGR_CFG_REG_A_M_MD0           (1 << 0)

+#define LSM303AGR_CFG_REG_A_M_MD1           (1 << 1)

+#define LSM303AGR_CFG_REG_A_M_ODR0          (1 << 2)

+#define LSM303AGR_CFG_REG_A_M_ODR1          (1 << 3)

+#define LSM303AGR_CFG_REG_A_M_LP            (1 << 4)

+#define LSM303AGR_CFG_REG_A_M_SOFT_RST      (1 << 5)

+#define LSM303AGR_CFG_REG_A_M_REBOOT        (1 << 6)

+#define LSM303AGR_CFG_REG_A_M_COMP_TEMP_EN  (1 << 7)

+/** @} */

+

+/**

+ * @name    LSM303AGR_CFG_REG_B register bits definitions

+ * @{

+ */

+#define LSM303AGR_CFG_REG_B_M_LPF           (1 << 0)

+#define LSM303AGR_CFG_REG_B_M_OFF_CANC      (1 << 1)

+#define LSM303AGR_CFG_REG_B_M_SET_FREQ      (1 << 2)

+#define LSM303AGR_CFG_REG_B_M_INT_ON        (1 << 3)

+#define LSM303AGR_CFG_REG_B_M_OFF_CANC_OS   (1 << 4)

+/** @} */

+

+/**

+ * @name    LSM303AGR_CFG_REG_C register bits definitions

+ * @{

+ */

+#define LSM303AGR_CFG_REG_C_M_INT_MAG       (1 << 0)

+#define LSM303AGR_CFG_REG_C_M_SELF_TEST     (1 << 1)

+#define LSM303AGR_CFG_REG_C_M_BLE           (1 << 3)

+#define LSM303AGR_CFG_REG_C_M_BDU           (1 << 4)

+#define LSM303AGR_CFG_REG_C_M_I2C_DIS       (1 << 5)

+#define LSM303AGR_CFG_REG_C_M_INT_MAG_PIN   (1 << 6)

+/** @} */

+

+/*===========================================================================*/

+/* Driver pre-compile time settings.                                         */

+/*===========================================================================*/

+

+/**

+ * @name    Configuration options

+ * @{

+ */

+/**

+ * @brief   LSM303AGR SPI interface switch.

+ * @details If set to @p TRUE the support for SPI is included.

+ * @note    The default is @p FALSE.

+ */

+#if !defined(LSM303AGR_USE_SPI) || defined(__DOXYGEN__)

+#define LSM303AGR_USE_SPI                  FALSE

+#endif

+

+/**

+ * @brief   LSM303AGR shared SPI switch.

+ * @details If set to @p TRUE the device acquires SPI bus ownership

+ *          on each transaction.

+ * @note    The default is @p FALSE. Requires SPI_USE_MUTUAL_EXCLUSION.

+ */

+#if !defined(LSM303AGR_SHARED_SPI) || defined(__DOXYGEN__)

+#define LSM303AGR_SHARED_SPI               FALSE

+#endif

+

+/**

+ * @brief   LSM303AGR I2C interface switch.

+ * @details If set to @p TRUE the support for I2C is included.

+ * @note    The default is @p TRUE.

+ */

+#if !defined(LSM303AGR_USE_I2C) || defined(__DOXYGEN__)

+#define LSM303AGR_USE_I2C                  TRUE

+#endif

+

+/**

+ * @brief   LSM303AGR shared I2C switch.

+ * @details If set to @p TRUE the device acquires I2C bus ownership

+ *          on each transaction.

+ * @note    The default is @p FALSE. Requires I2C_USE_MUTUAL_EXCLUSION.

+ */

+#if !defined(LSM303AGR_SHARED_I2C) || defined(__DOXYGEN__)

+#define LSM303AGR_SHARED_I2C               FALSE

+#endif

+

+/**

+ * @brief   LSM303AGR accelerometer subsystem advanced configurations

+ *          switch.

+ * @details If set to @p TRUE more configurations are available.

+ * @note    The default is @p FALSE.

+ */

+#if !defined(LSM303AGR_ACC_USE_ADVANCED) || defined(__DOXYGEN__)

+#define LSM303AGR_ACC_USE_ADVANCED         FALSE

+#endif

+

+/**

+ * @brief   LSM303AGR compass subsystem advanced configurations

+ *          switch.

+ * @details If set to @p TRUE more configurations are available.

+ * @note    The default is @p FALSE.

+ */

+#if !defined(LSM303AGR_COMP_USE_ADVANCED) || defined(__DOXYGEN__)

+#define LSM303AGR_COMP_USE_ADVANCED        FALSE

+#endif

+/** @} */

+

+/*===========================================================================*/

+/* Derived constants and error checks.                                       */

+/*===========================================================================*/

+

+#if !(LSM303AGR_USE_SPI ^ LSM303AGR_USE_I2C)

+#error "LSM303AGR_USE_SPI and LSM303AGR_USE_I2C cannot be both true or both false"

+#endif

+

+#if LSM303AGR_USE_SPI && !HAL_USE_SPI

+#error "LSM303AGR_USE_SPI requires HAL_USE_SPI"

+#endif

+

+#if LSM303AGR_SHARED_SPI && !SPI_USE_MUTUAL_EXCLUSION

+#error "LSM303AGR_SHARED_SPI requires SPI_USE_MUTUAL_EXCLUSION"

+#endif

+

+#if LSM303AGR_USE_I2C && !HAL_USE_I2C

+#error "LSM303AGR_USE_I2C requires HAL_USE_I2C"

+#endif

+

+#if LSM303AGR_SHARED_I2C && !I2C_USE_MUTUAL_EXCLUSION

+#error "LSM303AGR_SHARED_I2C requires I2C_USE_MUTUAL_EXCLUSION"

+#endif

+

+/**

+ * @todo    Add support for LSM303AGR over SPI.

+ */

+#if LSM303AGR_USE_SPI

+#error "LSM303AGR over SPI still not supported"

+#endif

+

+/*===========================================================================*/

+/* Driver data structures and types.                                         */

+/*===========================================================================*/

+

+/**

+ * @name    LSM303AGR accelerometer subsystem data structures and types.

+ * @{

+ */

+/**

+ * @brief Structure representing a LSM303AGR driver.

+ */

+typedef struct LSM303AGRDriver LSM303AGRDriver;

+

+/**

+ * @brief LSM303AGR accelerometer subsystem full scale.

+ */

+typedef enum {

+  LSM303AGR_ACC_FS_2G = 0x00,      /**< Full scale ±2g.                    */

+  LSM303AGR_ACC_FS_4G = 0x10,      /**< Full scale ±4g.                    */

+  LSM303AGR_ACC_FS_8G = 0x20,      /**< Full scale ±8g.                    */

+  LSM303AGR_ACC_FS_16G  = 0x30     /**< Full scale ±16g.                   */

+} lsm303agr_acc_fs_t;

+

+/**

+ * @brief LSM303AGR accelerometer subsystem output data rate.

+ */

+typedef enum {

+  LSM303AGR_ACC_ODR_PD = 0x00,     /**< Power down                         */

+  LSM303AGR_ACC_ODR_1Hz = 0x10,    /**< ODR 1 Hz                           */

+  LSM303AGR_ACC_ODR_10Hz = 0x20,   /**< ODR 10 Hz                          */

+  LSM303AGR_ACC_ODR_25Hz = 0x30,   /**< ODR 25 Hz                          */

+  LSM303AGR_ACC_ODR_50Hz = 0x40,   /**< ODR 50 Hz                          */

+  LSM303AGR_ACC_ODR_100Hz = 0x50,  /**< ODR 100 Hz                         */

+  LSM303AGR_ACC_ODR_200Hz = 0x60,  /**< ODR 200 Hz                         */

+  LSM303AGR_ACC_ODR_400Hz = 0x70,  /**< ODR 400 Hz                         */

+  LSM303AGR_ACC_ODR_1620Hz = 0x80, /**< ODR 1620 Hz (LP only)              */

+  LSM303AGR_ACC_ODR_1344Hz = 0x90  /**< ODR 1344 Hz or 5376 Hz in LP       */

+} lsm303agr_acc_odr_t;

+

+/**

+ * @brief LSM303AGR accelerometer subsystem axes enabling.

+ */

+typedef enum {

+  LSM303AGR_ACC_AE_DISABLED = 0x00,/**< All axes disabled.                 */

+  LSM303AGR_ACC_AE_X = 0x01,       /**< Only X-axis enabled.               */

+  LSM303AGR_ACC_AE_Y = 0x02,       /**< Only Y-axis enabled.               */

+  LSM303AGR_ACC_AE_XY = 0x03,      /**< X and Y axes enabled.              */

+  LSM303AGR_ACC_AE_Z = 0x04,       /**< Only Z-axis enabled.               */

+  LSM303AGR_ACC_AE_XZ = 0x05,      /**< X and Z axes enabled.              */

+  LSM303AGR_ACC_AE_YZ = 0x06,      /**< Y and Z axes enabled.              */

+  LSM303AGR_ACC_AE_XYZ = 0x07      /**< All axes enabled.                  */

+} lsm303agr_acc_ae_t;

+

+/**

+ * @brief LSM303AGR accelerometer subsystem low power mode.

+ */

+typedef enum {

+  LSM303AGR_ACC_LP_DISABLED = 0x00,/**< Low power mode disabled.           */

+  LSM303AGR_ACC_LP_ENABLED = 0x40  /**< Low power mode enabled.            */

+} lsm303agr_acc_lp_t;

+

+/**

+ * @brief LSM303AGR accelerometer subsystem high resolution mode.

+ */

+typedef enum {

+  LSM303AGR_ACC_HR_DISABLED = 0x00,/**< High resolution mode disabled.     */

+  LSM303AGR_ACC_HR_ENABLED = 0x08  /**< High resolution mode enabled.      */

+} lsm303agr_acc_hr_t;

+

+/**

+ * @brief LSM303AGR accelerometer subsystem block data update.

+ */

+typedef enum {

+  LSM303AGR_ACC_BDU_CONT = 0x00,   /**< Continuous update                  */

+  LSM303AGR_ACC_BDU_BLOCK = 0x80   /**< Update blocked                     */

+} lsm303agr_acc_bdu_t;

+

+/**

+ * @brief  LSM303AGR accelerometer endianness.

+ */

+typedef enum {

+  LSM303AGR_ACC_END_LITTLE = 0x00, /**< Little Endian                      */

+  LSM303AGR_ACC_END_BIG = 0x40     /**< Big Endian                         */

+} lsm303agr_acc_end_t;

+

+/**

+ * @name    LSM303AGR compass subsystem data structures and types.

+ * @{

+ */

+/**

+ * @brief LSM303AGR compass subsystem full scale.

+ */

+typedef enum {

+  LSM303AGR_COMP_FS_1P3GA = 0x20,  /**< Full scale ±1.3 Gauss              */

+  LSM303AGR_COMP_FS_1P9GA = 0x40,  /**< Full scale ±1.9 Gauss              */

+  LSM303AGR_COMP_FS_2P5GA = 0x60,  /**< Full scale ±2.5 Gauss              */

+  LSM303AGR_COMP_FS_4P0GA = 0x80,  /**< Full scale ±4.0 Gauss              */

+  LSM303AGR_COMP_FS_4P7GA = 0xA0,  /**< Full scale ±4.7 Gauss              */

+  LSM303AGR_COMP_FS_5P6GA = 0xC0,  /**< Full scale ±5.6 Gauss              */

+  LSM303AGR_COMP_FS_8P1GA = 0xE0   /**< Full scale ±8.1 Gauss              */

+} lsm303agr_comp_fs_t;

+

+/**

+ * @brief LSM303AGR compass subsystem output data rate.

+ */

+typedef enum {

+  LSM303AGR_COMP_ODR_0P75HZ = 0x00,/**< ODR 0.75 Hz                        */

+  LSM303AGR_COMP_ODR_1P5HZ = 0x04, /**< ODR 1.5 Hz                         */

+  LSM303AGR_COMP_ODR_3P0HZ = 0x08, /**< ODR 3 Hz                           */

+  LSM303AGR_COMP_ODR_7P5HZ = 0x0C, /**< ODR 7.5 Hz                         */

+  LSM303AGR_COMP_ODR_15HZ = 0x10,  /**< ODR 15 Hz                          */

+  LSM303AGR_COMP_ODR_30HZ = 0x14,  /**< ODR 30 Hz                          */

+  LSM303AGR_COMP_ODR_75HZ = 0x18,  /**< ODR 75 Hz                          */

+  LSM303AGR_COMP_ODR_220HZ = 0x1C  /**< ODR 220 Hz                         */

+} lsm303agr_comp_odr_t;

+

+/**

+ * @brief LSM303AGR compass subsystem working mode.

+ */

+typedef enum {

+  LSM303AGR_COMP_MD_CONT = 0x00,   /**< Continuous-Conversion Mode         */

+  LSM303AGR_COMP_MD_BLOCK = 0x01,  /**< Single-Conversion Mode             */

+  LSM303AGR_COMP_MD_SLEEP = 0x02   /**< Sleep Mode                         */

+} lsm303agr_comp_md_t;

+

+/**

+ * @name    LSM303AGR main system data structures and types.

+ * @{

+ */

+/**

+ * @brief Driver state machine possible states.

+ */

+typedef enum {

+  LSM303AGR_UNINIT = 0,            /**< Not initialized.                   */

+  LSM303AGR_STOP = 1,              /**< Stopped.                           */

+  LSM303AGR_READY = 2,             /**< Ready.                             */

+} lsm303agr_state_t;

+

+/**

+ * @brief LSM303AGR configuration structure.

+ */

+typedef struct {

+  /**

+   * @brief I2C driver associated to this LSM303AGR.

+   */

+  I2CDriver                 *i2cp;

+  /**

+   * @brief I2C configuration associated to this LSM303AGR.

+   */

+  const I2CConfig           *i2ccfg;

+  /**

+   * @brief LSM303AGR accelerometer subsystem initial sensitivity.

+   */

+  float                     *accsensitivity;

+  /**

+   * @brief LSM303AGR accelerometer subsystem initial bias.

+   */

+  float                     *accbias;

+  /**

+   * @brief LSM303AGR accelerometer subsystem initial full scale.

+   */

+  lsm303agr_acc_fs_t       accfullscale;

+  /**

+   * @brief LSM303AGR accelerometer subsystem output data rate.

+   */

+  lsm303agr_acc_odr_t      accoutdatarate;

+#if LSM303AGR_ACC_USE_ADVANCED || defined(__DOXYGEN__)

+  /**

+   * @brief LSM303AGR accelerometer subsystem low power mode.

+   */

+  lsm303agr_acc_lp_t       acclowpower;

+  /**

+   * @brief LSM303AGR accelerometer subsystem high resolution mode.

+   */

+  lsm303agr_acc_hr_t       acchighresmode;

+  /**

+   * @brief LSM303AGR accelerometer subsystem block data update.

+   */

+  lsm303agr_acc_bdu_t      accblockdataupdate;

+  /**

+   * @brief  LSM303AGR accelerometer endianness.

+   */

+  lsm303agr_acc_end_t      accendianess;

+#endif

+  /**

+   * @brief LSM303AGR compass initial sensitivity.

+   */

+  float                     *compsensitivity;

+  /**

+   * @brief LSM303AGR compass initial bias.

+   */

+  float                     *compbias;

+  /**

+   * @brief LSM303AGR compass subsystem initial full scale.

+   */

+  lsm303agr_comp_fs_t      compfullscale;

+  /**

+   * @brief LSM303AGR compass subsystem output data rate.

+   */

+  lsm303agr_comp_odr_t     compoutputdatarate;

+#if LSM303AGR_COMP_USE_ADVANCED || defined(__DOXYGEN__)

+  /**

+   * @brief LSM303AGR compass subsystem working mode.

+   */

+  lsm303agr_comp_md_t      compmode;

+#endif

+} LSM303AGRConfig;

+

+/**

+ * @brief   @p LSM303AGR specific methods.

+ */

+#define _lsm303agr_methods_alone                                           \

+  /* Change full scale value of LSM303AGR accelerometer subsystem.*/       \

+  msg_t (*acc_set_full_scale)(LSM303AGRDriver *devp,                       \

+                              lsm303agr_acc_fs_t fs);                      \

+  /* Change full scale value of LSM303AGR compass subsystem.*/             \

+  msg_t (*comp_set_full_scale)(LSM303AGRDriver *devp,                      \

+                              lsm303agr_comp_fs_t fs);                     \

+

+/**

+ * @brief   @p LSM303AGR specific methods with inherited ones.

+ */

+#define _lsm303agr_methods                                                 \

+  _base_object_methods                                                      \

+  _lsm303agr_methods_alone

+

+/**

+ * @extends BaseObjectVMT

+ *

+ * @brief @p LSM303AGR virtual methods table.

+ */

+struct LSM303AGRVMT {

+  _lsm303agr_methods

+};

+

+/**

+ * @brief @p LSM303AGRDriver specific data.

+ */

+#define _lsm303agr_data                                                    \

+  _base_sensor_data                                                         \

+  /* Driver state.*/                                                        \

+  lsm303agr_state_t        state;                                          \

+  /* Current configuration data.*/                                          \

+  const LSM303AGRConfig    *config;                                        \

+  /* Accelerometer subsystem axes number.*/                                 \

+  size_t                    accaxes;                                        \

+  /* Accelerometer subsystem current sensitivity.*/                         \

+  float                     accsensitivity[LSM303AGR_ACC_NUMBER_OF_AXES];  \

+  /* Accelerometer subsystem current bias .*/                               \

+  float                     accbias[LSM303AGR_ACC_NUMBER_OF_AXES];         \

+  /* Accelerometer subsystem current full scale value.*/                    \

+  float                     accfullscale;                                   \

+  /* Compass subsystem axes number.*/                                       \

+  size_t                    compaxes;                                       \

+  /* Compass subsystem current sensitivity.*/                               \

+  float                     compsensitivity[LSM303AGR_COMP_NUMBER_OF_AXES];\

+  /* Compass subsystem current bias.*/                                      \

+  float                     compbias[LSM303AGR_COMP_NUMBER_OF_AXES];       \

+  /* Compass subsystem current full scale value.*/                          \

+  float                     compfullscale;

+

+/**

+ * @brief LSM303AGR 6-axis accelerometer/compass class.

+ */

+struct LSM303AGRDriver {

+  /** @brief Virtual Methods Table.*/

+  const struct LSM303AGRVMT  *vmt;

+  /** @brief Base accelerometer interface.*/

+  BaseAccelerometer           acc_if;

+  /** @brief Base compass interface.*/

+  BaseCompass                 comp_if;

+  _lsm303agr_data

+};

+/** @} */

+

+/*===========================================================================*/

+/* Driver macros.                                                            */

+/*===========================================================================*/

+

+/**

+ * @brief   Return the number of axes of the BaseAccelerometer.

+ *

+ * @param[in] devp      pointer to @p LSM303AGRDriver.

+ *

+ * @return              the number of axes.

+ *

+ * @api

+ */

+#define lsm303agrAccelerometerGetAxesNumber(devp)                          \

+        accelerometerGetAxesNumber(&((devp)->acc_if))

+

+/**

+ * @brief   Retrieves raw data from the BaseAccelerometer.

+ * @note    This data is retrieved from MEMS register without any algebraical

+ *          manipulation.

+ * @note    The axes array must be at least the same size of the

+ *          BaseAccelerometer axes number.

+ *

+ * @param[in] devp      pointer to @p LSM303AGRDriver.

+ * @param[out] axes     a buffer which would be filled with raw data.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    if one or more I2C errors occurred, the errors can

+ *                      be retrieved using @p i2cGetErrors().

+ * @retval MSG_TIMEOUT  if a timeout occurred before operation end.

+ *

+ * @api

+ */

+#define lsm303agrAccelerometerReadRaw(devp, axes)                          \

+        accelerometerReadRaw(&((devp)->acc_if), axes)

+

+/**

+ * @brief   Retrieves cooked data from the BaseAccelerometer.

+ * @note    This data is manipulated according to the formula

+ *          cooked = (raw * sensitivity) - bias.

+ * @note    Final data is expressed as milli-G.

+ * @note    The axes array must be at least the same size of the

+ *          BaseAccelerometer axes number.

+ *

+ * @param[in] devp      pointer to @p LSM303AGRDriver.

+ * @param[out] axes     a buffer which would be filled with cooked data.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    if one or more I2C errors occurred, the errors can

+ *                      be retrieved using @p i2cGetErrors().

+ * @retval MSG_TIMEOUT  if a timeout occurred before operation end.

+ *

+ * @api

+ */

+#define lsm303agrAccelerometerReadCooked(devp, axes)                       \

+        accelerometerReadCooked(&((devp)->acc_if), axes)

+

+/**

+ * @brief   Set bias values for the BaseAccelerometer.

+ * @note    Bias must be expressed as milli-G.

+ * @note    The bias buffer must be at least the same size of the

+ *          BaseAccelerometer axes number.

+ *

+ * @param[in] devp      pointer to @p LSM303AGRDriver.

+ * @param[in] bp        a buffer which contains biases.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ *

+ * @api

+ */

+#define lsm303agrAccelerometerSetBias(devp, bp)                            \

+        accelerometerSetBias(&((devp)->acc_if), bp)

+

+/**

+ * @brief   Reset bias values for the BaseAccelerometer.

+ * @note    Default biases value are obtained from device datasheet when

+ *          available otherwise they are considered zero.

+ *

+ * @param[in] devp      pointer to @p LSM303AGRDriver.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ *

+ * @api

+ */

+#define lsm303agrAccelerometerResetBias(devp)                              \

+        accelerometerResetBias(&((devp)->acc_if))

+

+/**

+ * @brief   Set sensitivity values for the BaseAccelerometer.

+ * @note    Sensitivity must be expressed as milli-G/LSB.

+ * @note    The sensitivity buffer must be at least the same size of the

+ *          BaseAccelerometer axes number.

+ *

+ * @param[in] devp      pointer to @p LSM303AGRDriver.

+ * @param[in] sp        a buffer which contains sensitivities.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ *

+ * @api

+ */

+#define lsm303agrAccelerometerSetSensitivity(devp, sp)                     \

+        accelerometerSetSensitivity(&((devp)->acc_if), sp)

+

+/**

+ * @brief   Reset sensitivity values for the BaseAccelerometer.

+ * @note    Default sensitivities value are obtained from device datasheet.

+ *

+ * @param[in] devp      pointer to @p LSM303AGRDriver.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    otherwise.

+ *

+ * @api

+ */

+#define lsm303agrAccelerometerResetSensitivity(devp)                       \

+        accelerometerResetSensitivity(&((devp)->acc_if))

+

+/**

+ * @brief   Changes the LSM303AGRDriver accelerometer fullscale value.

+ * @note    This function also rescale sensitivities and biases based on

+ *          previous and next fullscale value.

+ * @note    A recalibration is highly suggested after calling this function.

+ *

+ * @param[in] devp      pointer to @p LSM303AGRDriver.

+ * @param[in] fs        new fullscale value.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    otherwise.

+ *

+ * @api

+ */

+#define lsm303agrAccelerometerSetFullScale(devp, fs)                       \

+        (devp)->vmt->acc_set_full_scale(devp, fs)

+

+/**

+ * @brief   Return the number of axes of the BaseCompass.

+ *

+ * @param[in] devp      pointer to @p LSM303AGRDriver.

+ *

+ * @return              the number of axes.

+ *

+ * @api

+ */

+#define lsm303agrCompassGetAxesNumber(devp)                                \

+        compassGetAxesNumber(&((devp)->comp_if))

+

+/**

+ * @brief   Retrieves raw data from the BaseCompass.

+ * @note    This data is retrieved from MEMS register without any algebraical

+ *          manipulation.

+ * @note    The axes array must be at least the same size of the

+ *          BaseCompass axes number.

+ *

+ * @param[in] ip        pointer to @p BaseCompass interface.

+ * @param[out] axes     a buffer which would be filled with raw data.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    if one or more I2C errors occurred, the errors can

+ *                      be retrieved using @p i2cGetErrors().

+ * @retval MSG_TIMEOUT  if a timeout occurred before operation end.

+ *

+ * @api

+ */

+#define lsm303agrCompassReadRaw(devp, axes)                                \

+        compassReadRaw(&((devp)->comp_if), axes)

+

+/**

+ * @brief   Retrieves cooked data from the BaseCompass.

+ * @note    This data is manipulated according to the formula

+ *          cooked = (raw * sensitivity) - bias.

+ * @note    Final data is expressed as G.

+ * @note    The axes array must be at least the same size of the

+ *          BaseCompass axes number.

+ *

+ * @param[in] ip        pointer to @p BaseCompass interface.

+ * @param[out] axes     a buffer which would be filled with cooked data.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    if one or more I2C errors occurred, the errors can

+ *                      be retrieved using @p i2cGetErrors().

+ * @retval MSG_TIMEOUT  if a timeout occurred before operation end.

+ *

+ * @api

+ */

+#define lsm303agrCompassReadCooked(devp, axes)                             \

+        compassReadCooked(&((devp)->comp_if), axes)

+

+/**

+ * @brief   Set bias values for the BaseCompass.

+ * @note    Bias must be expressed as G.

+ * @note    The bias buffer must be at least the same size of the

+ *          BaseCompass axes number.

+ *

+ * @param[in] ip        pointer to @p BaseCompass interface.

+ * @param[in] bp        a buffer which contains biases.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ *

+ * @api

+ */

+#define lsm303agrCompassSetBias(devp, bp)                                  \

+        compassSetBias(&((devp)->comp_if), bp)

+

+/**

+ * @brief   Reset bias values for the BaseCompass.

+ * @note    Default biases value are obtained from device datasheet when

+ *          available otherwise they are considered zero.

+ *

+ * @param[in] devp      pointer to @p LSM303AGRDriver.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ *

+ * @api

+ */

+#define lsm303agrCompassResetBias(devp)                                    \

+        compassResetBias(&((devp)->comp_if))

+

+/**

+ * @brief   Set sensitivity values for the BaseCompass.

+ * @note    Sensitivity must be expressed as G/LSB.

+ * @note    The sensitivity buffer must be at least the same size of the

+ *          BaseCompass axes number.

+ *

+ * @param[in] devp      pointer to @p LSM303AGRDriver.

+ * @param[in] sp        a buffer which contains sensitivities.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ *

+ * @api

+ */

+#define lsm303agrCompassSetSensitivity(devp, sp)                           \

+        compassSetSensitivity(&((devp)->comp_if), sp)

+

+/**

+ * @brief   Reset sensitivity values for the BaseCompass.

+ * @note    Default sensitivities value are obtained from device datasheet.

+ *

+ * @param[in] devp      pointer to @p LSM303AGRDriver.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    otherwise.

+ *

+ * @api

+ */

+#define lsm303agrCompassResetSensitivity(devp)                             \

+        compassResetSensitivity(&((devp)->comp_if))

+

+/**

+ * @brief   Changes the LSM303AGRDriver compass fullscale value.

+ * @note    This function also rescale sensitivities and biases based on

+ *          previous and next fullscale value.

+ * @note    A recalibration is highly suggested after calling this function.

+ *

+ * @param[in] devp      pointer to @p LSM303AGRDriver.

+ * @param[in] fs        new fullscale value.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    otherwise.

+ *

+ * @api

+ */

+#define lsm303agrCompassSetFullScale(devp, fs)                             \

+        (devp)->vmt->comp_set_full_scale(devp, fs)

+

+/*===========================================================================*/

+/* External declarations.                                                    */

+/*===========================================================================*/

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+  void lsm303agrObjectInit(LSM303AGRDriver *devp);

+  void lsm303agrStart(LSM303AGRDriver *devp, const LSM303AGRConfig *config);

+  void lsm303agrStop(LSM303AGRDriver *devp);

+#ifdef __cplusplus

+}

+#endif

+

+#endif /* _LSM303AGR_H_ */

+

+/** @} */

diff --git a/os/ex/ST/lsm303agr.mk b/os/ex/ST/lsm303agr.mk
new file mode 100644
index 000000000..e94ed0891
--- /dev/null
+++ b/os/ex/ST/lsm303agr.mk
@@ -0,0 +1,10 @@
+# List of all the LSM303AGR device files.

+LSM303AGRSRC := $(CHIBIOS)/os/ex/ST/lsm303agr.c

+

+# Required include directories

+LSM303AGRINC := $(CHIBIOS)/os/hal/lib/peripherals/sensors \

+                 $(CHIBIOS)/os/ex/ST

+

+# Shared variables

+ALLCSRC += $(LSM303AGRSRC)

+ALLINC  += $(LSM303AGRINC)
\ No newline at end of file