diff options
| -rw-r--r-- | os/ex/ST/lsm303dlhc.c | 610 | ||||
| -rw-r--r-- | os/ex/ST/lsm303dlhc.h | 470 | ||||
| -rw-r--r-- | readme.txt | 4 | ||||
| -rw-r--r-- | testhal/STM32/STM32F3xx/I2C-LSM303DLHC/Makefile | 7 | ||||
| -rw-r--r-- | testhal/STM32/STM32F3xx/I2C-LSM303DLHC/main.c | 315 | ||||
| -rw-r--r-- | testhal/STM32/STM32F4xx/I2C-LSM303DLHC/Makefile | 7 | ||||
| -rw-r--r-- | testhal/STM32/STM32F4xx/I2C-LSM303DLHC/main.c | 299 |
7 files changed, 1170 insertions, 542 deletions
diff --git a/os/ex/ST/lsm303dlhc.c b/os/ex/ST/lsm303dlhc.c index ab57cb5d9..c4adc8aea 100644 --- a/os/ex/ST/lsm303dlhc.c +++ b/os/ex/ST/lsm303dlhc.c @@ -33,98 +33,6 @@ /* Driver local definitions. */
/*===========================================================================*/
-#define LSM303DLHC_ACC_SENS_2G ((float)0.0610f)
-#define LSM303DLHC_ACC_SENS_4G ((float)0.1221f)
-#define LSM303DLHC_ACC_SENS_8G ((float)0.2442f)
-#define LSM303DLHC_ACC_SENS_16G ((float)0.4884f)
-
-#define LSM303DLHC_COMP_SENS_XY_1_3GA ((float)1100.0f)
-#define LSM303DLHC_COMP_SENS_XY_1_9GA ((float)855.0f)
-#define LSM303DLHC_COMP_SENS_XY_2_5GA ((float)670.0f)
-#define LSM303DLHC_COMP_SENS_XY_4_0GA ((float)450.0f)
-#define LSM303DLHC_COMP_SENS_XY_4_7GA ((float)400.0f)
-#define LSM303DLHC_COMP_SENS_XY_5_6GA ((float)330.0f)
-#define LSM303DLHC_COMP_SENS_XY_8_1GA ((float)230.0f)
-
-#define LSM303DLHC_COMP_SENS_Z_1_3GA ((float)980.0f)
-#define LSM303DLHC_COMP_SENS_Z_1_9GA ((float)765.0f)
-#define LSM303DLHC_COMP_SENS_Z_2_5GA ((float)600.0f)
-#define LSM303DLHC_COMP_SENS_Z_4_0GA ((float)400.0f)
-#define LSM303DLHC_COMP_SENS_Z_4_7GA ((float)355.0f)
-#define LSM303DLHC_COMP_SENS_Z_5_6GA ((float)295.0f)
-#define LSM303DLHC_COMP_SENS_Z_8_1GA ((float)205.0f)
-
-#define LSM303DLHC_DI ((uint8_t)0xFF)
-#define LSM303DLHC_DI_0 ((uint8_t)0x01)
-#define LSM303DLHC_DI_1 ((uint8_t)0x02)
-#define LSM303DLHC_DI_2 ((uint8_t)0x04)
-#define LSM303DLHC_DI_3 ((uint8_t)0x08)
-#define LSM303DLHC_DI_4 ((uint8_t)0x10)
-#define LSM303DLHC_DI_5 ((uint8_t)0x20)
-#define LSM303DLHC_DI_6 ((uint8_t)0x40)
-#define LSM303DLHC_DI_7 ((uint8_t)0x80)
-
-#define LSM303DLHC_AD_0 ((uint8_t)0x01)
-#define LSM303DLHC_AD_1 ((uint8_t)0x02)
-#define LSM303DLHC_AD_2 ((uint8_t)0x04)
-#define LSM303DLHC_AD_3 ((uint8_t)0x08)
-#define LSM303DLHC_AD_4 ((uint8_t)0x10)
-#define LSM303DLHC_AD_5 ((uint8_t)0x20)
-#define LSM303DLHC_AD_6 ((uint8_t)0x40)
-#define LSM303DLHC_RW ((uint8_t)0x80)
-
-#define LSM303DLHC_AD_ACC_CTRL_REG1 ((uint8_t)0x20)
-#define LSM303DLHC_AD_ACC_CTRL_REG2 ((uint8_t)0x21)
-#define LSM303DLHC_AD_ACC_CTRL_REG3 ((uint8_t)0x22)
-#define LSM303DLHC_AD_ACC_CTRL_REG4 ((uint8_t)0x23)
-#define LSM303DLHC_AD_ACC_CTRL_REG5 ((uint8_t)0x24)
-#define LSM303DLHC_AD_ACC_CTRL_REG6 ((uint8_t)0x25)
-#define LSM303DLHC_AD_ACC_REFERENCE ((uint8_t)0x26)
-#define LSM303DLHC_AD_ACC_STATUS_REG ((uint8_t)0x27)
-#define LSM303DLHC_AD_ACC_OUT_X_L ((uint8_t)0x28)
-#define LSM303DLHC_AD_ACC_OUT_X_H ((uint8_t)0x29)
-#define LSM303DLHC_AD_ACC_OUT_Y_L ((uint8_t)0x2A)
-#define LSM303DLHC_AD_ACC_OUT_Y_H ((uint8_t)0x2B)
-#define LSM303DLHC_AD_ACC_OUT_Z_L ((uint8_t)0x2C)
-#define LSM303DLHC_AD_ACC_OUT_Z_H ((uint8_t)0x2D)
-#define LSM303DLHC_AD_ACC_FIFO_CTRL_REG ((uint8_t)0x2E)
-#define LSM303DLHC_AD_ACC_FIFO_SRC_REG ((uint8_t)0x2F)
-#define LSM303DLHC_AD_ACC_INT1_CFG ((uint8_t)0x30)
-#define LSM303DLHC_AD_ACC_INT1_SRC ((uint8_t)0x31)
-#define LSM303DLHC_AD_ACC_INT1_THS ((uint8_t)0x32)
-#define LSM303DLHC_AD_ACC_INT1_DURATION ((uint8_t)0x33)
-#define LSM303DLHC_AD_ACC_INT2_CFG ((uint8_t)0x34)
-#define LSM303DLHC_AD_ACC_INT2_SRC ((uint8_t)0x35)
-#define LSM303DLHC_AD_ACC_INT2_THS ((uint8_t)0x36)
-#define LSM303DLHC_AD_ACC_INT2_DURATION ((uint8_t)0x37)
-#define LSM303DLHC_AD_ACC_CLICK_CFG ((uint8_t)0x38)
-#define LSM303DLHC_AD_ACC_CLICK_SRC ((uint8_t)0x39)
-#define LSM303DLHC_AD_ACC_CLICK_THS ((uint8_t)0x3A)
-#define LSM303DLHC_AD_ACC_TIME_LIMIT ((uint8_t)0x3B)
-#define LSM303DLHC_AD_ACC_TIME_LATENCY ((uint8_t)0x3C)
-#define LSM303DLHC_AD_ACC_TIME_WINDOW ((uint8_t)0x3D)
-#define LSM303DLHC_AD_COMP_CRA_REG ((uint8_t)0x00)
-#define LSM303DLHC_AD_COMP_CRB_REG ((uint8_t)0x01)
-#define LSM303DLHC_AD_COMP_MR_REG ((uint8_t)0x02)
-#define LSM303DLHC_AD_COMP_OUT_X_H ((uint8_t)0x03)
-#define LSM303DLHC_AD_COMP_OUT_X_L ((uint8_t)0x04)
-#define LSM303DLHC_AD_COMP_OUT_Z_H ((uint8_t)0x05)
-#define LSM303DLHC_AD_COMP_OUT_Z_L ((uint8_t)0x06)
-#define LSM303DLHC_AD_COMP_OUT_Y_H ((uint8_t)0x07)
-#define LSM303DLHC_AD_COMP_OUT_Y_L ((uint8_t)0x08)
-#define LSM303DLHC_AD_COMP_SR_REG ((uint8_t)0x09)
-#define LSM303DLHC_AD_COMP_IRA_REG ((uint8_t)0x0A)
-#define LSM303DLHC_AD_COMP_IRB_REG ((uint8_t)0x0B)
-#define LSM303DLHC_AD_COMP_IRC_REG ((uint8_t)0x0C)
-#define LSM303DLHC_AD_COMP_TEMP_OUT_H ((uint8_t)0x31)
-#define LSM303DLHC_AD_COMP_TEMP_OUT_L ((uint8_t)0x32)
-
-#define LSM303DLHC_ACC_CTRL_REG4_FS_MASK ((uint8_t)0x30)
-#define LSM303DLHC_COMP_CTRL_REGB_FS_MASK ((uint8_t)0xE0)
-
-#define TO_G ((float)0.001f)
-#define TO_SI ((float)0.00981f)
-
/*===========================================================================*/
/* Driver exported variables. */
/*===========================================================================*/
@@ -148,11 +56,11 @@ typedef enum { LSM303DLHC_SAD_ACC = 0x19, /**< SAD for accelerometer. */
LSM303DLHC_SAD_COMP = 0x1E /**< SAD for compass. */
} lsm303dlhc_sad_t;
+
/*===========================================================================*/
/* Driver local functions. */
/*===========================================================================*/
-#if (LSM303DLHC_USE_I2C) || defined(__DOXYGEN__)
/**
* @brief Reads registers value using I2C.
* @pre The I2C interface must be initialized and the driver started.
@@ -161,29 +69,16 @@ typedef enum { * @param[in] i2cp pointer to the I2C interface
* @param[in] sad slave address without R bit
* @param[in] reg first sub-register address
- * @return the read value.
+ * @param[in] rxbuf receiving buffer
+ * @param[in] n size of rxbuf
+ * @return the operation status.
*/
-uint8_t lsm303dlhcI2CReadRegister(I2CDriver *i2cp, lsm303dlhc_sad_t sad, uint8_t reg,
- msg_t* msgp) {
- msg_t msg;
-#if defined(STM32F103_MCUCONF)
- uint8_t rxbuf[2];
- msg = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 2,
- TIME_INFINITE);
- if(msgp != NULL){
- *msgp = msg;
- }
- return rxbuf[0];
-#else
- uint8_t txbuf, rxbuf;
- txbuf = reg;
- msg = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, &rxbuf, 1,
- TIME_INFINITE);
- if(msgp != NULL){
- *msgp = msg;
- }
- return rxbuf;
-#endif
+msg_t lsm303dlhcI2CReadRegister(I2CDriver *i2cp, lsm303dlhc_sad_t sad,
+ uint8_t reg, uint8_t *rxbuf, size_t n) {
+
+ uint8_t txbuf = reg | LSM303DLHC_MS;
+ return i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, n,
+ TIME_INFINITE);
}
/**
@@ -192,93 +87,18 @@ uint8_t lsm303dlhcI2CReadRegister(I2CDriver *i2cp, lsm303dlhc_sad_t sad, uint8_t *
* @param[in] i2cp pointer to the I2C interface
* @param[in] sad slave address without R bit
- * @param[in] sub sub-register address
- * @param[in] value the value to be written
+ * @param[in] txbuf buffer comtaining 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.
*/
-msg_t lsm303dlhcI2CWriteRegister(I2CDriver *i2cp, lsm303dlhc_sad_t sad, uint8_t reg,
- uint8_t value) {
- uint8_t rxbuf;
- uint8_t txbuf[2];
- if(sad == LSM303DLHC_SAD_ACC) {
- switch (reg) {
- default:
- /* Reserved register must not be written, according to the datasheet
- * this could permanently damage the device.
- */
- osalDbgAssert(FALSE, "lsm303dlhcWriteRegisterI2C(), reserved register");
- case LSM303DLHC_AD_ACC_STATUS_REG:
- case LSM303DLHC_AD_ACC_OUT_X_L:
- case LSM303DLHC_AD_ACC_OUT_X_H:
- case LSM303DLHC_AD_ACC_OUT_Y_L:
- case LSM303DLHC_AD_ACC_OUT_Y_H:
- case LSM303DLHC_AD_ACC_OUT_Z_L:
- case LSM303DLHC_AD_ACC_OUT_Z_H:
- case LSM303DLHC_AD_ACC_FIFO_SRC_REG:
- case LSM303DLHC_AD_ACC_INT1_SRC:
- case LSM303DLHC_AD_ACC_INT2_SRC:
- /* Read only registers cannot be written, the command is ignored.*/
- return MSG_RESET;
- case LSM303DLHC_AD_ACC_CTRL_REG1:
- case LSM303DLHC_AD_ACC_CTRL_REG2:
- case LSM303DLHC_AD_ACC_CTRL_REG3:
- case LSM303DLHC_AD_ACC_CTRL_REG4:
- case LSM303DLHC_AD_ACC_CTRL_REG5:
- case LSM303DLHC_AD_ACC_CTRL_REG6:
- case LSM303DLHC_AD_ACC_REFERENCE:
- case LSM303DLHC_AD_ACC_FIFO_CTRL_REG:
- case LSM303DLHC_AD_ACC_INT1_CFG:
- case LSM303DLHC_AD_ACC_INT1_THS:
- case LSM303DLHC_AD_ACC_INT1_DURATION:
- case LSM303DLHC_AD_ACC_INT2_CFG:
- case LSM303DLHC_AD_ACC_INT2_THS:
- case LSM303DLHC_AD_ACC_INT2_DURATION:
- case LSM303DLHC_AD_ACC_CLICK_CFG:
- case LSM303DLHC_AD_ACC_CLICK_SRC:
- case LSM303DLHC_AD_ACC_CLICK_THS:
- case LSM303DLHC_AD_ACC_TIME_LIMIT:
- case LSM303DLHC_AD_ACC_TIME_LATENCY:
- case LSM303DLHC_AD_ACC_TIME_WINDOW:
- txbuf[0] = reg;
- txbuf[1] = value;
- return i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0, TIME_INFINITE);
- break;
- }
- }
- if(sad == LSM303DLHC_SAD_COMP) {
- switch (reg) {
- default:
- /* Reserved register must not be written, according to the datasheet
- * this could permanently damage the device.
- */
- osalDbgAssert(FALSE, "lsm303dlhcWriteRegisterI2C(), reserved register");
- case LSM303DLHC_AD_COMP_OUT_X_H:
- case LSM303DLHC_AD_COMP_OUT_X_L:
- case LSM303DLHC_AD_COMP_OUT_Z_H:
- case LSM303DLHC_AD_COMP_OUT_Z_L:
- case LSM303DLHC_AD_COMP_OUT_Y_H:
- case LSM303DLHC_AD_COMP_OUT_Y_L:
- case LSM303DLHC_AD_COMP_SR_REG:
- case LSM303DLHC_AD_COMP_IRA_REG:
- case LSM303DLHC_AD_COMP_IRB_REG:
- case LSM303DLHC_AD_COMP_IRC_REG:
- case LSM303DLHC_AD_COMP_TEMP_OUT_H:
- case LSM303DLHC_AD_COMP_TEMP_OUT_L:
- /* Read only registers cannot be written, the command is ignored.*/
- return MSG_RESET;
- case LSM303DLHC_AD_COMP_CRA_REG:
- case LSM303DLHC_AD_COMP_CRB_REG:
- case LSM303DLHC_AD_COMP_MR_REG:
- txbuf[0] = reg;
- txbuf[1] = value;
- return i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0, TIME_INFINITE);
- break;
- }
- }
- osalDbgAssert(FALSE, "lsm303dlhcWriteRegisterI2C(), wrong SAD");
- return MSG_RESET;
+msg_t lsm303dlhcI2CWriteRegister(I2CDriver *i2cp, lsm303dlhc_sad_t sad,
+ uint8_t *txbuf, size_t n) {
+ if (n != 1)
+ *txbuf |= LSM303DLHC_MS;
+ return i2cMasterTransmitTimeout(i2cp, sad, txbuf, n + 1, NULL, 0, TIME_INFINITE);
}
-#endif /* LSM303DLHC_USE_I2C */
/*
* Interface implementation.
@@ -307,13 +127,14 @@ static size_t sens_get_axes_number(void *ip) { }
static msg_t acc_read_raw(void *ip, int32_t axes[]) {
+ uint8_t buff [LSM303DLHC_ACC_NUMBER_OF_AXES * 2], i;
int16_t tmp;
+ msg_t msg;
osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
(((LSM303DLHCDriver *)ip)->config->acccfg != NULL));
osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),
"acc_read_raw(), invalid state");
-#if LSM303DLHC_USE_I2C
osalDbgAssert((((LSM303DLHCDriver *)ip)->config->i2cp->state == I2C_READY),
"acc_read_raw(), channel not ready");
#if LSM303DLHC_SHARED_I2C
@@ -321,48 +142,31 @@ static msg_t acc_read_raw(void *ip, int32_t axes[]) { i2cStart(((LSM303DLHCDriver *)ip)->config->i2cp,
((LSM303DLHCDriver *)ip)->config->i2ccfg);
#endif /* LSM303DLHC_SHARED_I2C */
- if(((LSM303DLHCDriver *)ip)->config->acccfg->axesenabling & LSM303DLHC_ACC_AE_X){
- tmp = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
- LSM303DLHC_SAD_ACC,
- LSM303DLHC_AD_ACC_OUT_X_L, NULL);
- tmp += lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
- LSM303DLHC_SAD_ACC,
- LSM303DLHC_AD_ACC_OUT_X_H, NULL) << 8;
- axes[0] = (int32_t)tmp - ((LSM303DLHCDriver *)ip)->accbias[0];
- }
- if(((LSM303DLHCDriver *)ip)->config->acccfg->axesenabling & LSM303DLHC_ACC_AE_Y){
- tmp = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
- LSM303DLHC_SAD_ACC,
- LSM303DLHC_AD_ACC_OUT_Y_L, NULL);
- tmp += lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
- LSM303DLHC_SAD_ACC,
- LSM303DLHC_AD_ACC_OUT_Y_H, NULL) << 8;
- axes[1] = (int32_t)tmp - ((LSM303DLHCDriver *)ip)->accbias[1];
- }
- if(((LSM303DLHCDriver *)ip)->config->acccfg->axesenabling & LSM303DLHC_ACC_AE_Z){
- tmp = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
- LSM303DLHC_SAD_ACC,
- LSM303DLHC_AD_ACC_OUT_Z_L, NULL);
- tmp += lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
- LSM303DLHC_SAD_ACC,
- LSM303DLHC_AD_ACC_OUT_Z_H, NULL) << 8;
- axes[2] = (int32_t)tmp - ((LSM303DLHCDriver *)ip)->accbias[2];
- }
+
+ msg = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
+ LSM303DLHC_SAD_ACC, LSM303DLHC_AD_ACC_OUT_X_L,
+ buff, LSM303DLHC_ACC_NUMBER_OF_AXES * 2);
+ if(msg == MSG_OK)
+ for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
+ tmp = buff[2*i] + (buff[2*i+1] << 8);
+ axes[i] = (int32_t)tmp;
+ }
+
#if LSM303DLHC_SHARED_I2C
i2cReleaseBus(((LSM303DLHCDriver *)ip)->config->i2cp);
#endif /* LSM303DLHC_SHARED_I2C */
-#endif /* LSM303DLHC_USE_I2C */
- return MSG_OK;
+ return msg;
}
static msg_t comp_read_raw(void *ip, int32_t axes[]) {
+ uint8_t buff [LSM303DLHC_COMP_NUMBER_OF_AXES * 2], i;
int16_t tmp;
+ msg_t msg;
osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
(((LSM303DLHCDriver *)ip)->config->compcfg != NULL));
osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),
"comp_read_raw(), invalid state");
-#if LSM303DLHC_USE_I2C
osalDbgAssert((((LSM303DLHCDriver *)ip)->config->i2cp->state == I2C_READY),
"comp_read_raw(), channel not ready");
#if LSM303DLHC_SHARED_I2C
@@ -370,33 +174,17 @@ static msg_t comp_read_raw(void *ip, int32_t axes[]) { i2cStart(((LSM303DLHCDriver *)ip)->config->i2cp,
((LSM303DLHCDriver *)ip)->config->i2ccfg);
#endif /* LSM303DLHC_SHARED_I2C */
- tmp = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
- LSM303DLHC_SAD_COMP,
- LSM303DLHC_AD_COMP_OUT_X_L, NULL);
- tmp += lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
- LSM303DLHC_SAD_COMP,
- LSM303DLHC_AD_COMP_OUT_X_H, NULL) << 8;
- axes[0] = (int32_t)tmp - ((LSM303DLHCDriver *)ip)->compbias[0];
-
- tmp = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
- LSM303DLHC_SAD_COMP,
- LSM303DLHC_AD_COMP_OUT_Y_L, NULL);
- tmp += lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
- LSM303DLHC_SAD_COMP,
- LSM303DLHC_AD_COMP_OUT_Y_H, NULL) << 8;
- axes[1] = (int32_t)tmp - ((LSM303DLHCDriver *)ip)->compbias[1];
-
- tmp = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
- LSM303DLHC_SAD_COMP,
- LSM303DLHC_AD_COMP_OUT_Z_L, NULL);
- tmp += lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
- LSM303DLHC_SAD_COMP,
- LSM303DLHC_AD_COMP_OUT_Z_H, NULL) << 8;
- axes[2] = (int32_t)tmp - ((LSM303DLHCDriver *)ip)->compbias[2];
+ msg = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
+ LSM303DLHC_SAD_COMP, LSM303DLHC_AD_COMP_OUT_X_L,
+ buff, LSM303DLHC_COMP_NUMBER_OF_AXES * 2);
+ if(msg == MSG_OK)
+ for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
+ tmp = buff[2*i] + (buff[2*i+1] << 8);
+ axes[i] = (int32_t)tmp;
+ }
#if LSM303DLHC_SHARED_I2C
i2cReleaseBus(((LSM303DLHCDriver *)ip)->config->i2cp);
#endif /* LSM303DLHC_SHARED_I2C */
-#endif /* LSM303DLHC_USE_I2C */
return MSG_OK;
}
@@ -414,7 +202,7 @@ static msg_t sens_read_raw(void *ip, int32_t axes[]) { }
return msg;
}
-
+
static msg_t acc_read_cooked(void *ip, float axes[]) {
uint32_t i;
int32_t raw[LSM303DLHC_ACC_NUMBER_OF_AXES];
@@ -429,12 +217,7 @@ static msg_t acc_read_cooked(void *ip, float axes[]) { msg = acc_read_raw(ip, raw);
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES ; i++){
axes[i] = raw[i] * ((LSM303DLHCDriver *)ip)->accsensitivity[i];
- if(((LSM303DLHCDriver *)ip)->config->acccfg->unit == LSM303DLHC_ACC_UNIT_G){
- axes[i] *= TO_G;
- }
- else if(((LSM303DLHCDriver *)ip)->config->acccfg->unit == LSM303DLHC_ACC_UNIT_SI){
- axes[i] *= TO_SI;
- }
+ axes[i] -= ((LSM303DLHCDriver *)ip)->accbias[i];
}
return msg;
}
@@ -452,7 +235,8 @@ static msg_t comp_read_cooked(void *ip, float axes[]) { msg = comp_read_raw(ip, raw);
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES ; i++){
- axes[i] = raw[i] / ((LSM303DLHCDriver *)ip)->compsensitivity[i];
+ axes[i] = raw[i] * ((LSM303DLHCDriver *)ip)->compsensitivity[i];
+ axes[i] -= ((LSM303DLHCDriver *)ip)->compbias[i];
}
return msg;
}
@@ -593,67 +377,67 @@ static msg_t comp_reset_sensivity(void *ip) { osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),
"comp_reset_sensivity(), invalid state");
- if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1_3_GA)
+ if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1P3GA)
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) {
- ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1_3GA;
+ ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1P3GA;
}
else {
- ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1_3GA;
+ ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1P3GA;
}
}
- else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1_9_GA)
+ else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1P9GA)
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) {
- ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1_9GA;
+ ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1P9GA;
}
else {
- ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1_9GA;
+ ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1P9GA;
}
}
- else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_2_5_GA)
+ else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_2P5GA)
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) {
- ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_2_5GA;
+ ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_2P5GA;
}
else {
- ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_2_5GA;
+ ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_2P5GA;
}
}
- else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4_0_GA)
+ else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4P0GA)
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) {
- ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4_0GA;
+ ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4P0GA;
}
else {
- ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4_0GA;
+ ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4P0GA;
}
}
- else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4_7_GA)
+ else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4P7GA)
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) {
- ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4_7GA;
+ ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4P7GA;
}
else {
- ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4_7GA;
+ ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4P7GA;
}
}
- else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_5_6_GA)
+ else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_5P6GA)
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) {
- ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_5_6GA;
+ ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_5P6GA;
}
else {
- ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_5_6GA;
+ ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_5P6GA;
}
}
- else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_8_1_GA)
+ else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_8P1GA)
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) {
- ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_8_1GA;
+ ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_8P1GA;
}
else {
- ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_8_1GA;
+ ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_8P1GA;
}
}
else {
@@ -663,6 +447,79 @@ static msg_t comp_reset_sensivity(void *ip) { return MSG_OK;
}
+static msg_t acc_set_full_scale(void *ip, lsm303dlhc_acc_fs_t fs) {
+ float newfs, scale;
+ unsigned i;
+
+ if(fs == LSM303DLHC_ACC_FS_2G) {
+ newfs = LSM303DLHC_ACC_2G;
+ }
+ else if(fs == LSM303DLHC_ACC_FS_4G) {
+ newfs = LSM303DLHC_ACC_4G;
+ }
+ else if(fs == LSM303DLHC_ACC_FS_8G) {
+ newfs = LSM303DLHC_ACC_8G;
+ }
+ else if(fs == LSM303DLHC_ACC_FS_16G) {
+ newfs = LSM303DLHC_ACC_16G;
+ }
+ else {
+ return MSG_RESET;
+ }
+
+ if(newfs != ((LSM303DLHCDriver *)ip)->accfullscale) {
+ scale = newfs / ((LSM303DLHCDriver *)ip)->accfullscale;
+ ((LSM303DLHCDriver *)ip)->accfullscale = newfs;
+ /* Scaling sensitivity and bias. Re-calibration is suggested anyway. */
+ for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
+ ((LSM303DLHCDriver *)ip)->accsensitivity[i] *= scale;
+ ((LSM303DLHCDriver *)ip)->accbias[i] *= scale;
+ }
+ }
+ return MSG_OK;
+}
+
+static msg_t comp_set_full_scale(void *ip, lsm303dlhc_comp_fs_t fs) {
+ float newfs, scale;
+ unsigned i;
+
+ if(fs == LSM303DLHC_COMP_FS_1P3GA) {
+ newfs = LSM303DLHC_COMP_1P3GA;
+ }
+ else if(fs == LSM303DLHC_COMP_FS_1P9GA) {
+ newfs = LSM303DLHC_COMP_1P9GA;
+ }
+ else if(fs == LSM303DLHC_COMP_FS_2P5GA) {
+ newfs = LSM303DLHC_COMP_2P5GA;
+ }
+ else if(fs == LSM303DLHC_COMP_FS_4P0GA) {
+ newfs = LSM303DLHC_COMP_4P0GA;
+ }
+ else if(fs == LSM303DLHC_COMP_FS_4P7GA) {
+ newfs = LSM303DLHC_COMP_4P7GA;
+ }
+ else if(fs == LSM303DLHC_COMP_FS_5P6GA) {
+ newfs = LSM303DLHC_COMP_5P6GA;
+ }
+ else if(fs == LSM303DLHC_COMP_FS_8P1GA) {
+ newfs = LSM303DLHC_COMP_8P1GA;
+ }
+ else {
+ return MSG_RESET;
+ }
+
+ if(newfs != ((LSM303DLHCDriver *)ip)->compfullscale) {
+ scale = newfs / ((LSM303DLHCDriver *)ip)->compfullscale;
+ ((LSM303DLHCDriver *)ip)->compfullscale = newfs;
+ /* Scaling sensitivity and bias. Re-calibration is suggested anyway. */
+ for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
+ ((LSM303DLHCDriver *)ip)->compsensitivity[i] *= scale;
+ ((LSM303DLHCDriver *)ip)->compbias[i] *= scale;
+ }
+ }
+ return MSG_OK;
+}
+
static const struct BaseSensorVMT vmt_basesensor = {
sens_get_axes_number, sens_read_raw, sens_read_cooked
};
@@ -679,12 +536,14 @@ static const struct BaseAccelerometerVMT vmt_baseaccelerometer = { static const struct LSM303DLHCACCVMT vmt_lsm303dlhcacc = {
acc_get_axes_number, acc_read_raw, acc_read_cooked,
- acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity
+ acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity,
+ acc_set_full_scale
};
static const struct LSM303DLHCCOMPVMT vmt_lsm303dlhccomp = {
comp_get_axes_number, comp_read_raw, comp_read_cooked,
- comp_set_bias, comp_reset_bias, comp_set_sensivity, comp_reset_sensivity
+ comp_set_bias, comp_reset_bias, comp_set_sensivity, comp_reset_sensivity,
+ comp_set_full_scale
};
/*===========================================================================*/
@@ -723,6 +582,7 @@ void lsm303dlhcObjectInit(LSM303DLHCDriver *devp) { */
void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
uint32_t i;
+ uint8_t buff[5] = {0, 0, 0, 0, 0};
osalDbgCheck((devp != NULL) && (config != NULL));
osalDbgAssert((devp->state == LSM303DLHC_STOP) || (devp->state == LSM303DLHC_READY),
@@ -730,126 +590,180 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) { devp->config = config;
-#if LSM303DLHC_USE_I2C
#if LSM303DLHC_SHARED_I2C
i2cAcquireBus((devp)->config->i2cp);
#endif /* LSM303DLHC_SHARED_I2C */
i2cStart((devp)->config->i2cp,
(devp)->config->i2ccfg);
if((devp)->config->acccfg != NULL) {
- lsm303dlhcI2CWriteRegister(devp->config->i2cp,
- LSM303DLHC_SAD_ACC,
- LSM303DLHC_AD_ACC_CTRL_REG1,
- devp->config->acccfg->axesenabling |
- devp->config->acccfg->outdatarate |
- devp->config->acccfg->lowpower);
- lsm303dlhcI2CWriteRegister(devp->config->i2cp,
- LSM303DLHC_SAD_ACC,
- LSM303DLHC_AD_ACC_CTRL_REG4,
- devp->config->acccfg->endianess |
- devp->config->acccfg->fullscale |
- devp->config->acccfg->blockdataupdate |
- devp->config->acccfg->highresmode);
+
+ /* Multiple write starting address.*/
+ buff[0] = LSM303DLHC_AD_ACC_CTRL_REG1;
+
+ /* Control register 1 configuration block.*/
+ {
+ buff[1] = LSM303DLHC_CTRL_REG1_A_XEN | LSM303DLHC_CTRL_REG1_A_YEN |
+ LSM303DLHC_CTRL_REG1_A_ZEN | devp->config->acccfg->outdatarate;
+#if LSM303DLHC_ACC_USE_ADVANCED || defined(__DOXYGEN__)
+ buff[1] |= devp->config->acccfg->lowpower;
+#endif
+ }
+
+ /* Control register 2 configuration block.*/
+ {
+ buff[2] = 0;
+ }
+
+ /* Control register 3 configuration block.*/
+ {
+ buff[3] = 0;
+ }
+
+ /* Control register 4 configuration block.*/
+ {
+ buff[4] = devp->config->acccfg->fullscale;
+#if LSM303DLHC_ACC_USE_ADVANCED || defined(__DOXYGEN__)
+ buff[4] |= devp->config->acccfg->endianess |
+ devp->config->acccfg->blockdataupdate |
+ devp->config->acccfg->highresmode;
+#endif
+ }
+ lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_ACC,
+ buff, 4);
}
if((devp)->config->compcfg != NULL) {
- lsm303dlhcI2CWriteRegister(devp->config->i2cp,
- LSM303DLHC_SAD_COMP,
- LSM303DLHC_AD_COMP_CRA_REG,
- devp->config->compcfg->outputdatarate);
- lsm303dlhcI2CWriteRegister(devp->config->i2cp,
- LSM303DLHC_SAD_COMP,
- LSM303DLHC_AD_COMP_CRB_REG,
- devp->config->compcfg->fullscale);
- lsm303dlhcI2CWriteRegister(devp->config->i2cp,
- LSM303DLHC_SAD_COMP,
- LSM303DLHC_AD_COMP_MR_REG,
- devp->config->compcfg->mode);
+
+ /* Multiple write starting address.*/
+ buff[0] = LSM303DLHC_AD_COMP_CRA_REG;
+
+ /* Control register A configuration block.*/
+ {
+ buff[1] = devp->config->compcfg->outputdatarate;
+ }
+
+ /* Control register B configuration block.*/
+ {
+ buff[2] = devp->config->compcfg->fullscale;
+ }
+
+ /* Mode register configuration block.*/
+ {
+ buff[3] = 0;
+#if LSM303DLHC_COMP_USE_ADVANCED || defined(__DOXYGEN__)
+ buff[3] |= devp->config->compcfg->mode;
+#endif
+ }
+
+ lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_COMP,
+ buff, 3);
}
#if LSM303DLHC_SHARED_I2C
i2cReleaseBus((devp)->config->i2cp);
#endif /* LSM303DLHC_SHARED_I2C */
-#endif /* LSM303DLHC_USE_I2C */
+
/* Storing sensitivity information according to full scale value */
if((devp)->config->acccfg != NULL) {
- if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_2G)
+ if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_2G) {
+ devp->accfullscale = LSM303DLHC_ACC_2G;
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_2G;
- else if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_4G)
+ }
+ else if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_4G) {
+ devp->accfullscale = LSM303DLHC_ACC_4G;
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_4G;
- else if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_8G)
+ }
+ else if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_8G) {
+ devp->accfullscale = LSM303DLHC_ACC_8G;
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_8G;
- else if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_16G)
+ }
+ else if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_16G) {
+ devp->accfullscale = LSM303DLHC_ACC_16G;
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_16G;
+ }
else
osalDbgAssert(FALSE, "lsm303dlhcStart(), accelerometer full scale issue");
}
if((devp)->config->compcfg != NULL) {
- if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1_3_GA)
+ if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1P3GA) {
+ devp->compfullscale = LSM303DLHC_COMP_1P3GA;
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) {
- devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1_3GA;
+ devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1P3GA;
}
else {
- devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1_3GA;
+ devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1P3GA;
}
}
- else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1_9_GA)
+ }
+ else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1P9GA) {
+ devp->compfullscale = LSM303DLHC_COMP_1P9GA;
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) {
- devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1_9GA;
+ devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1P9GA;
}
else {
- devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1_9GA;
+ devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1P9GA;
}
}
- else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_2_5_GA)
+ }
+ else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_2P5GA) {
+ devp->compfullscale = LSM303DLHC_COMP_2P5GA;
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) {
- devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_2_5GA;
+ devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_2P5GA;
}
else {
- devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_2_5GA;
+ devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_2P5GA;
}
}
- else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4_0_GA)
+ }
+ else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4P0GA) {
+ devp->compfullscale = LSM303DLHC_COMP_4P0GA;
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) {
- devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4_0GA;
+ devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4P0GA;
}
else {
- devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4_0GA;
+ devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4P0GA;
}
}
- else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4_7_GA)
+ }
+ else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4P7GA) {
+ devp->compfullscale = LSM303DLHC_COMP_4P7GA;
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) {
- devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4_7GA;
+ devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4P7GA;
}
else {
- devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4_7GA;
+ devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4P7GA;
}
}
- else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_5_6_GA)
+ }
+ else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_5P6GA) {
+ devp->compfullscale = LSM303DLHC_COMP_5P6GA;
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) {
- devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_5_6GA;
+ devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_5P6GA;
}
else {
- devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_5_6GA;
+ devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_5P6GA;
}
}
- else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_8_1_GA)
+ }
+ else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_8P1GA) {
+ devp->compfullscale = LSM303DLHC_COMP_8P1GA;
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) {
- devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_8_1GA;
+ devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_8P1GA;
}
else {
- devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_8_1GA;
+ devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_8P1GA;
}
}
+ }
else
osalDbgAssert(FALSE, "lsm303dlhcStart(), compass full scale issue");
}
@@ -867,13 +781,13 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) { * @api
*/
void lsm303dlhcStop(LSM303DLHCDriver *devp) {
+ uint8_t buff[2];
osalDbgCheck(devp != NULL);
osalDbgAssert((devp->state == LSM303DLHC_STOP) || (devp->state == LSM303DLHC_READY),
"lsm303dlhcStop(), invalid state");
-#if (LSM303DLHC_USE_I2C)
if (devp->state == LSM303DLHC_STOP) {
#if LSM303DLHC_SHARED_I2C
i2cAcquireBus((devp)->config->i2cp);
@@ -881,24 +795,24 @@ void lsm303dlhcStop(LSM303DLHCDriver *devp) { (devp)->config->i2ccfg);
#endif /* LSM303DLHC_SHARED_I2C */
if((devp)->config->acccfg != NULL) {
- lsm303dlhcI2CWriteRegister(devp->config->i2cp,
- LSM303DLHC_SAD_ACC,
- LSM303DLHC_AD_ACC_CTRL_REG1,
- LSM303DLHC_ACC_AE_DISABLED |
- LSM303DLHC_ACC_ODR_PD);
+ buff[0] = LSM303DLHC_AD_ACC_CTRL_REG1;
+ buff[1] = LSM303DLHC_ACC_AE_DISABLED | LSM303DLHC_ACC_ODR_PD;
+ lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_ACC,
+ buff, 1);
}
if((devp)->config->compcfg != NULL) {
- lsm303dlhcI2CWriteRegister(devp->config->i2cp,
- LSM303DLHC_SAD_COMP,
- LSM303DLHC_AD_COMP_MR_REG,
- LSM303DLHC_COMP_MD_SLEEP);
+ buff[0] = LSM303DLHC_AD_COMP_MR_REG;
+ buff[1] = LSM303DLHC_COMP_MD_SLEEP;
+ lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_ACC,
+ buff, 1);
+ lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_COMP,
+ buff, 1);
}
i2cStop((devp)->config->i2cp);
#if LSM303DLHC_SHARED_I2C
i2cReleaseBus((devp)->config->i2cp);
#endif /* LSM303DLHC_SHARED_I2C */
}
-#endif /* LSM303DLHC_USE_I2C */
devp->state = LSM303DLHC_STOP;
}
/** @} */
diff --git a/os/ex/ST/lsm303dlhc.h b/os/ex/ST/lsm303dlhc.h index 17fd55ec2..7500fa95b 100644 --- a/os/ex/ST/lsm303dlhc.h +++ b/os/ex/ST/lsm303dlhc.h @@ -20,7 +20,7 @@ /**
* @file lsm303dlhc.h
- * @brief LSM303DLHC MEMS interface module header.
+ * @brief LSM303DLHC MEMS interface module header.
*
* @{
*/
@@ -35,14 +35,256 @@ /*===========================================================================*/
/**
- * @brief LSM303DLHC accelerometer subsystem number of axes.
+ * @name Version identification
+ * @{
+ */
+/**
+ * @brief LSM303DLHC driver version string.
+ */
+#define EX_LSM303DLHC_VERSION "1.0.0"
+
+/**
+ * @brief LSM303DLHC driver version major number.
+ */
+#define EX_LSM303DLHC_MAJOR 1
+
+/**
+ * @brief LSM303DLHC driver version minor number.
+ */
+#define EX_LSM303DLHC_MINOR 0
+
+/**
+ * @brief LSM303DLHC driver version patch number.
+ */
+#define EX_LSM303DLHC_PATCH 0
+/** @} */
+
+/**
+ * @brief LSM303DLHC accelerometer subsystem characteristics.
+ *
+ * @{
*/
-#define LSM303DLHC_ACC_NUMBER_OF_AXES ((size_t) 3U)
+#define LSM303DLHC_ACC_NUMBER_OF_AXES 3U
+
+#define LSM303DLHC_ACC_2G 2.0f
+#define LSM303DLHC_ACC_4G 4.0f
+#define LSM303DLHC_ACC_8G 8.0f
+#define LSM303DLHC_ACC_16G 16.0f
+
+#define LSM303DLHC_ACC_SENS_2G 0.0610f
+#define LSM303DLHC_ACC_SENS_4G 0.1221f
+#define LSM303DLHC_ACC_SENS_8G 0.2442f
+#define LSM303DLHC_ACC_SENS_16G 0.4884f
+/** @} */
/**
- * @brief LSM303DLHC compass subsystem number of axes.
+ * @brief LSM303DLHC compass subsystem characteristics.
+ *
+ * @{
*/
-#define LSM303DLHC_COMP_NUMBER_OF_AXES ((size_t) 3U)
+#define LSM303DLHC_COMP_NUMBER_OF_AXES 3U
+
+#define LSM303DLHC_COMP_1P3GA 1.3f
+#define LSM303DLHC_COMP_1P9GA 1.9f
+#define LSM303DLHC_COMP_2P5GA 2.5f
+#define LSM303DLHC_COMP_4P0GA 4.0f
+#define LSM303DLHC_COMP_4P7GA 4.7f
+#define LSM303DLHC_COMP_5P6GA 5.6f
+#define LSM303DLHC_COMP_8P1GA 8.1f
+
+#define LSM303DLHC_COMP_SENS_XY_1P3GA 0.000909f
+#define LSM303DLHC_COMP_SENS_XY_1P9GA 0.001169f
+#define LSM303DLHC_COMP_SENS_XY_2P5GA 0.0014925f
+#define LSM303DLHC_COMP_SENS_XY_4P0GA 0.0022222f
+#define LSM303DLHC_COMP_SENS_XY_4P7GA 0.0025000f
+#define LSM303DLHC_COMP_SENS_XY_5P6GA 0.0030303f
+#define LSM303DLHC_COMP_SENS_XY_8P1GA 0.0043478f
+
+#define LSM303DLHC_COMP_SENS_Z_1P3GA 0.0010204f
+#define LSM303DLHC_COMP_SENS_Z_1P9GA 0.0013071f
+#define LSM303DLHC_COMP_SENS_Z_2P5GA 0.0016666f
+#define LSM303DLHC_COMP_SENS_Z_4P0GA 0.0025000f
+#define LSM303DLHC_COMP_SENS_Z_4P7GA 0.0028169f
+#define LSM303DLHC_COMP_SENS_Z_5P6GA 0.0033898f
+#define LSM303DLHC_COMP_SENS_Z_8P1GA 0.0048780f
+/** @} */
+
+/**
+ * @name LSM303DLHC communication interfaces related bit masks
+ * @{
+ */
+#define LSM303DLHC_DI_MASK 0xFF /**< Data In mask */
+#define LSM303DLHC_DI(n) (1 << n) /**< Data In bit n */
+#define LSM303DLHC_AD_MASK 0x7F /**< Address Data mask */
+#define LSM303DLHC_AD(n) (1 << n) /**< Address Data bit n */
+#define LSM303DLHC_MS (1 << 7) /**< Multiple read write */
+/** @} */
+
+/**
+ * @name LSM303DLHC register addresses
+ * @{
+ */
+#define LSM303DLHC_AD_ACC_CTRL_REG1 0x20
+#define LSM303DLHC_AD_ACC_CTRL_REG2 0x21
+#define LSM303DLHC_AD_ACC_CTRL_REG3 0x22
+#define LSM303DLHC_AD_ACC_CTRL_REG4 0x23
+#define LSM303DLHC_AD_ACC_CTRL_REG5 0x24
+#define LSM303DLHC_AD_ACC_CTRL_REG6 0x25
+#define LSM303DLHC_AD_ACC_REFERENCE 0x26
+#define LSM303DLHC_AD_ACC_STATUS_REG 0x27
+#define LSM303DLHC_AD_ACC_OUT_X_L 0x28
+#define LSM303DLHC_AD_ACC_OUT_X_H 0x29
+#define LSM303DLHC_AD_ACC_OUT_Y_L 0x2A
+#define LSM303DLHC_AD_ACC_OUT_Y_H 0x2B
+#define LSM303DLHC_AD_ACC_OUT_Z_L 0x2C
+#define LSM303DLHC_AD_ACC_OUT_Z_H 0x2D
+#define LSM303DLHC_AD_ACC_FIFO_CTRL_REG 0x2E
+#define LSM303DLHC_AD_ACC_FIFO_SRC_REG 0x2F
+#define LSM303DLHC_AD_ACC_INT1_CFG 0x30
+#define LSM303DLHC_AD_ACC_INT1_SRC 0x31
+#define LSM303DLHC_AD_ACC_INT1_THS 0x32
+#define LSM303DLHC_AD_ACC_INT1_DURATION 0x33
+#define LSM303DLHC_AD_ACC_INT2_CFG 0x34
+#define LSM303DLHC_AD_ACC_INT2_SRC 0x35
+#define LSM303DLHC_AD_ACC_INT2_THS 0x36
+#define LSM303DLHC_AD_ACC_INT2_DURATION 0x37
+#define LSM303DLHC_AD_ACC_CLICK_CFG 0x38
+#define LSM303DLHC_AD_ACC_CLICK_SRC 0x39
+#define LSM303DLHC_AD_ACC_CLICK_THS 0x3A
+#define LSM303DLHC_AD_ACC_TIME_LIMIT 0x3B
+#define LSM303DLHC_AD_ACC_TIME_LATENCY 0x3C
+#define LSM303DLHC_AD_ACC_TIME_WINDOW 0x3D
+#define LSM303DLHC_AD_COMP_CRA_REG 0x00
+#define LSM303DLHC_AD_COMP_CRB_REG 0x01
+#define LSM303DLHC_AD_COMP_MR_REG 0x02
+#define LSM303DLHC_AD_COMP_OUT_X_H 0x03
+#define LSM303DLHC_AD_COMP_OUT_X_L 0x04
+#define LSM303DLHC_AD_COMP_OUT_Z_H 0x05
+#define LSM303DLHC_AD_COMP_OUT_Z_L 0x06
+#define LSM303DLHC_AD_COMP_OUT_Y_H 0x07
+#define LSM303DLHC_AD_COMP_OUT_Y_L 0x08
+#define LSM303DLHC_AD_COMP_SR_REG 0x09
+#define LSM303DLHC_AD_COMP_IRA_REG 0x0A
+#define LSM303DLHC_AD_COMP_IRB_REG 0x0B
+#define LSM303DLHC_AD_COMP_IRC_REG 0x0C
+#define LSM303DLHC_AD_COMP_TEMP_OUT_H 0x31
+#define LSM303DLHC_AD_COMP_TEMP_OUT_L 0x32
+/** @} */
+
+/**
+ * @name LSM303DLHC_CTRL_REG1_A register bits definitions
+ * @{
+ */
+#define LSM303DLHC_CTRL_REG1_A_MASK 0xFF
+#define LSM303DLHC_CTRL_REG1_A_XEN (1 << 0)
+#define LSM303DLHC_CTRL_REG1_A_YEN (1 << 1)
+#define LSM303DLHC_CTRL_REG1_A_ZEN (1 << 2)
+#define LSM303DLHC_CTRL_REG1_A_LPEN (1 << 3)
+#define LSM303DLHC_CTRL_REG1_A_ODR0 (1 << 4)
+#define LSM303DLHC_CTRL_REG1_A_ODR1 (1 << 5)
+#define LSM303DLHC_CTRL_REG1_A_ODR2 (1 << 6)
+#define LSM303DLHC_CTRL_REG1_A_ODR3 (1 << 7)
+/** @} */
+
+/**
+ * @name LSM303DLHC_CTRL_REG2_A register bits definitions
+ * @{
+ */
+#define LSM303DLHC_CTRL_REG2_A_MASK 0xFF
+#define LSM303DLHC_CTRL_REG2_A_HPIS1 (1 << 0)
+#define LSM303DLHC_CTRL_REG2_A_HPIS2 (1 << 1)
+#define LSM303DLHC_CTRL_REG2_A_HPCLICK (1 << 2)
+#define LSM303DLHC_CTRL_REG2_A_FDS (1 << 3)
+#define LSM303DLHC_CTRL_REG2_A_HPCF1 (1 << 4)
+#define LSM303DLHC_CTRL_REG2_A_HPCF2 (1 << 5)
+#define LSM303DLHC_CTRL_REG2_A_HPM0 (1 << 6)
+#define LSM303DLHC_CTRL_REG2_A_HPM1 (1 << 7)
+/** @} */
+
+/**
+ * @name LSM303DLHC_CTRL_REG3_A register bits definitions
+ * @{
+ */
+#define LSM303DLHC_CTRL_REG3_A_MASK 0xFD
+#define LSM303DLHC_CTRL_REG3_A_I1_OVERRUN (1 << 1)
+#define LSM303DLHC_CTRL_REG3_A_I1_WTM (1 << 2)
+#define LSM303DLHC_CTRL_REG3_A_I1_DRDY2 (1 << 3)
+#define LSM303DLHC_CTRL_REG3_A_I1_DRDY1 (1 << 4)
+#define LSM303DLHC_CTRL_REG3_A_I1_AOI2 (1 << 5)
+#define LSM303DLHC_CTRL_REG3_A_I1_AOI1 (1 << 6)
+#define LSM303DLHC_CTRL_REG3_A_I1_CLICK (1 << 7)
+/** @} */
+
+/**
+ * @name LSM303DLHC_CTRL_REG4_A register bits definitions
+ * @{
+ */
+#define LSM303DLHC_CTRL_REG4_A_MASK 0xF9
+#define LSM303DLHC_CTRL_REG4_A_SIM (1 << 0)
+#define LSM303DLHC_CTRL_REG4_A_HR (1 << 3)
+#define LSM303DLHC_CTRL_REG4_A_FS0 (1 << 4)
+#define LSM303DLHC_CTRL_REG4_A_FS1 (1 << 5)
+#define LSM303DLHC_CTRL_REG4_A_BLE (1 << 6)
+#define LSM303DLHC_CTRL_REG4_A_BDU (1 << 7)
+/** @} */
+
+/**
+ * @name LSM303DLHC_CTRL_REG5_A register bits definitions
+ * @{
+ */
+#define LSM303DLHC_CTRL_REG5_A_MASK 0xCF
+#define LSM303DLHC_CTRL_REG5_A_D4D_INT2 (1 << 0)
+#define LSM303DLHC_CTRL_REG5_A_LIR_INT2 (1 << 1)
+#define LSM303DLHC_CTRL_REG5_A_D4D_INT1 (1 << 2)
+#define LSM303DLHC_CTRL_REG5_A_LIR_INT1 (1 << 3)
+#define LSM303DLHC_CTRL_REG5_A_FIFO_EN (1 << 6)
+#define LSM303DLHC_CTRL_REG5_A_BOOT (1 << 7)
+/** @} */
+
+/**
+ * @name LSM303DLHC_CTRL_REG6_A register bits definitions
+ * @{
+ */
+#define LSM303DLHC_CTRL_REG6_A_MASK 0xFA
+#define LSM303DLHC_CTRL_REG6_A_H_LACTIVE (1 << 1)
+#define LSM303DLHC_CTRL_REG6_A_P2_ACT (1 << 3)
+#define LSM303DLHC_CTRL_REG6_A_BOOT_I1 (1 << 4)
+#define LSM303DLHC_CTRL_REG6_A_I2_INT2 (1 << 5)
+#define LSM303DLHC_CTRL_REG6_A_I2_INT1 (1 << 6)
+#define LSM303DLHC_CTRL_REG6_A_I2_CLICKEN (1 << 7)
+/** @} */
+
+/**
+ * @name LSM303DLHC_CRA_REG_M register bits definitions
+ * @{
+ */
+#define LSM303DLHC_CRA_REG_M_MASK 0x9C
+#define LSM303DLHC_CRA_REG_M_DO0 (1 << 2)
+#define LSM303DLHC_CRA_REG_M_DO1 (1 << 3)
+#define LSM303DLHC_CRA_REG_M_DO2 (1 << 4)
+#define LSM303DLHC_CRA_REG_M_TEMP_EN (1 << 7)
+/** @} */
+
+/**
+ * @name LSM303DLHC_CRB_REG_M register bits definitions
+ * @{
+ */
+#define LSM303DLHC_CRB_REG_M_MASK 0xE0
+#define LSM303DLHC_CRB_REG_M_GN0 (1 << 5)
+#define LSM303DLHC_CRB_REG_M_GN1 (1 << 6)
+#define LSM303DLHC_CRB_REG_M_GN2 (1 << 7)
+
+/**
+ * @name LSM303DLHC_CRB_REG_M register bits definitions
+ * @{
+ */
+#define LSM303DLHC_MR_REG_M_MASK 0x03
+#define LSM303DLHC_MR_REG_M_MD0 (1 << 0)
+#define LSM303DLHC_MR_REG_M_MD1 (1 << 1)
+
+/** @} */
+
+//TODO: ADD more LSM303DLHC register bits definitions
/*===========================================================================*/
/* Driver pre-compile time settings. */
@@ -53,21 +295,23 @@ * @{
*/
/**
- * @brief LSM303DLHC SPI interface selector.
- * @details If set to @p TRUE the support for SPI is included.
+ * @brief LSM303DLHC accelerometer subsystem advanced configurations
+ * switch.
+ * @details If set to @p TRUE more configurations are available.
* @note The default is @p FALSE.
*/
-#if !defined(LSM303DLHC_USE_SPI) || defined(__DOXYGEN__)
-#define LSM303DLHC_USE_SPI FALSE
+#if !defined(LSM303DLHC_ACC_USE_ADVANCED) || defined(__DOXYGEN__)
+#define LSM303DLHC_ACC_USE_ADVANCED FALSE
#endif
/**
- * @brief LSM303DLHC I2C interface selector.
- * @details If set to @p TRUE the support for I2C is included.
- * @note The default is @p TRUE.
+ * @brief LSM303DLHC compass subsystem advanced configurations
+ * switch.
+ * @details If set to @p TRUE more configurations are available.
+ * @note The default is @p FALSE.
*/
-#if !defined(LSM303DLHC_USE_I2C) || defined(__DOXYGEN__)
-#define LSM303DLHC_USE_I2C TRUE
+#if !defined(LSM303DLHC_COMP_USE_ADVANCED) || defined(__DOXYGEN__)
+#define LSM303DLHC_COMP_USE_ADVANCED FALSE
#endif
/**
@@ -85,16 +329,8 @@ /* Derived constants and error checks. */
/*===========================================================================*/
-#if LSM303DLHC_USE_SPI && LSM303DLHC_USE_I2C
-#error "LSM303DLHC_USE_SPI and LSM303DLHC_USE_I2C cannot be both true"
-#endif
-
-#if LSM303DLHC_USE_SPI && !HAL_USE_SPI
-#error "LSM303DLHC_USE_SPI requires HAL_USE_SPI"
-#endif
-
-#if LSM303DLHC_USE_I2C && !HAL_USE_I2C
-#error "LSM303DLHC_USE_I2C requires HAL_USE_I2C"
+#if !HAL_USE_I2C
+#error "LSM303DLHC requires HAL_USE_I2C"
#endif
#if LSM303DLHC_SHARED_I2C && !I2C_USE_MUTUAL_EXCLUSION
@@ -111,7 +347,7 @@ */
/**
- * @brief LSM303DLHC accelerometer subsystem full scale.
+ * @brief LSM303DLHC accelerometer subsystem full scale.
*/
typedef enum {
LSM303DLHC_ACC_FS_2G = 0x00, /**< Full scale ±2g. */
@@ -121,7 +357,7 @@ typedef enum { } lsm303dlhc_acc_fs_t;
/**
- * @brief LSM303DLHC accelerometer subsystem output data rate.
+ * @brief LSM303DLHC accelerometer subsystem output data rate.
*/
typedef enum {
LSM303DLHC_ACC_ODR_PD = 0x00, /**< Power down */
@@ -138,7 +374,7 @@ typedef enum { /**
- * @brief LSM303DLHC accelerometer subsystem axes enabling.
+ * @brief LSM303DLHC accelerometer subsystem axes enabling.
*/
typedef enum {
LSM303DLHC_ACC_AE_DISABLED = 0x00,/**< All axes disabled. */
@@ -152,7 +388,7 @@ typedef enum { } lsm303dlhc_acc_ae_t;
/**
- * @brief LSM303DLHC accelerometer subsystem low power mode.
+ * @brief LSM303DLHC accelerometer subsystem low power mode.
*/
typedef enum {
LSM303DLHC_ACC_LP_DISABLED = 0x00,/**< Low power mode disabled. */
@@ -160,7 +396,7 @@ typedef enum { } lsm303dlhc_acc_lp_t;
/**
- * @brief LSM303DLHC accelerometer subsystem high resolution mode.
+ * @brief LSM303DLHC accelerometer subsystem high resolution mode.
*/
typedef enum {
LSM303DLHC_ACC_HR_DISABLED = 0x00,/**< High resolution mode disabled. */
@@ -168,7 +404,7 @@ typedef enum { } lsm303dlhc_acc_hr_t;
/**
- * @brief LSM303DLHC accelerometer subsystem block data update.
+ * @brief LSM303DLHC accelerometer subsystem block data update.
*/
typedef enum {
LSM303DLHC_ACC_BDU_CONT = 0x00, /**< Continuous update */
@@ -183,52 +419,44 @@ typedef enum { LSM303DLHC_ACC_END_BIG = 0x40 /**< Big Endian */
} lsm303dlhc_acc_end_t;
-
-/**
- * @brief LSM303DLHC accelerometer subsystem unit.
- */
-typedef enum {
- LSM303DLHC_ACC_UNIT_G = 0x00, /**< Cooked data in g. */
- LSM303DLHC_ACC_UNIT_MG = 0x01, /**< Cooked data in mg. */
- LSM303DLHC_ACC_UNIT_SI = 0x02, /**< Cooked data in m/s^2. */
-} lsm303dlhc_acc_unit_t;
-
/**
- * @brief LSM303DLHC accelerometer subsystem configuration structure.
+ * @brief LSM303DLHC accelerometer subsystem configuration structure.
*/
typedef struct {
/**
- * @brief LSM303DLHC accelerometer subsystem full scale.
+ * @brief LSM303DLHC initial sensitivity.
*/
- lsm303dlhc_acc_fs_t fullscale;
+ float sensitivity[LSM303DLHC_ACC_NUMBER_OF_AXES];
/**
- * @brief LSM303DLHC accelerometer subsystem output data rate.
+ * @brief LSM303DLHC initial bias.
*/
- lsm303dlhc_acc_odr_t outdatarate;
+ float bias[LSM303DLHC_ACC_NUMBER_OF_AXES];
/**
- * @brief LSM303DLHC accelerometer subsystem axes enabling.
+ * @brief LSM303DLHC accelerometer subsystem initial full scale.
*/
- lsm303dlhc_acc_ae_t axesenabling;
+ lsm303dlhc_acc_fs_t fullscale;
+ /**
+ * @brief LSM303DLHC accelerometer subsystem output data rate.
+ */
+ lsm303dlhc_acc_odr_t outdatarate;
+#if LSM303DLHC_ACC_USE_ADVANCED || defined(__DOXYGEN__)
/**
- * @brief LSM303DLHC accelerometer subsystem low power mode.
+ * @brief LSM303DLHC accelerometer subsystem low power mode.
*/
lsm303dlhc_acc_lp_t lowpower;
/**
- * @brief LSM303DLHC accelerometer subsystem high resolution mode.
+ * @brief LSM303DLHC accelerometer subsystem high resolution mode.
*/
lsm303dlhc_acc_hr_t highresmode;
/**
- * @brief LSM303DLHC accelerometer subsystem block data update.
+ * @brief LSM303DLHC accelerometer subsystem block data update.
*/
lsm303dlhc_acc_bdu_t blockdataupdate;
/**
* @brief LSM303DLHC accelerometer endianness.
*/
lsm303dlhc_acc_end_t endianess;
- /**
- * @brief LSM303DLHC accelerometer subsystem unit.
- */
- lsm303dlhc_acc_unit_t unit;
+#endif
} LSM303DLHCAccConfig;
/** @} */
@@ -237,26 +465,26 @@ typedef struct { * @{
*/
/**
- * @brief LSM303DLHC compass subsystem full scale.
+ * @brief LSM303DLHC compass subsystem full scale.
*/
typedef enum {
- LSM303DLHC_COMP_FS_1_3_GA = 0x20, /**< Full scale ±1.3 Gauss */
- LSM303DLHC_COMP_FS_1_9_GA = 0x40, /**< Full scale ±1.9 Gauss */
- LSM303DLHC_COMP_FS_2_5_GA = 0x60, /**< Full scale ±2.5 Gauss */
- LSM303DLHC_COMP_FS_4_0_GA = 0x80, /**< Full scale ±4.0 Gauss */
- LSM303DLHC_COMP_FS_4_7_GA = 0xA0, /**< Full scale ±4.7 Gauss */
- LSM303DLHC_COMP_FS_5_6_GA = 0xC0, /**< Full scale ±5.6 Gauss */
- LSM303DLHC_COMP_FS_8_1_GA = 0xE0 /**< Full scale ±8.1 Gauss */
+ LSM303DLHC_COMP_FS_1P3GA = 0x20, /**< Full scale ±1.3 Gauss */
+ LSM303DLHC_COMP_FS_1P9GA = 0x40, /**< Full scale ±1.9 Gauss */
+ LSM303DLHC_COMP_FS_2P5GA = 0x60, /**< Full scale ±2.5 Gauss */
+ LSM303DLHC_COMP_FS_4P0GA = 0x80, /**< Full scale ±4.0 Gauss */
+ LSM303DLHC_COMP_FS_4P7GA = 0xA0, /**< Full scale ±4.7 Gauss */
+ LSM303DLHC_COMP_FS_5P6GA = 0xC0, /**< Full scale ±5.6 Gauss */
+ LSM303DLHC_COMP_FS_8P1GA = 0xE0 /**< Full scale ±8.1 Gauss */
} lsm303dlhc_comp_fs_t;
/**
- * @brief LSM303DLHC compass subsystem output data rate.
+ * @brief LSM303DLHC compass subsystem output data rate.
*/
typedef enum {
- LSM303DLHC_COMP_ODR_0_75HZ = 0x00,/**< ODR 0.75 Hz */
- LSM303DLHC_COMP_ODR_1_5HZ = 0x04, /**< ODR 1.5 Hz */
- LSM303DLHC_COMP_ODR_3_0HZ = 0x08, /**< ODR 3 Hz */
- LSM303DLHC_COMP_ODR_7_5HZ = 0x0C, /**< ODR 7.5 Hz */
+ LSM303DLHC_COMP_ODR_0P75HZ = 0x00,/**< ODR 0.75 Hz */
+ LSM303DLHC_COMP_ODR_1P5HZ = 0x04, /**< ODR 1.5 Hz */
+ LSM303DLHC_COMP_ODR_3P0HZ = 0x08, /**< ODR 3 Hz */
+ LSM303DLHC_COMP_ODR_7P5HZ = 0x0C, /**< ODR 7.5 Hz */
LSM303DLHC_COMP_ODR_15HZ = 0x10, /**< ODR 15 Hz */
LSM303DLHC_COMP_ODR_30HZ = 0x14, /**< ODR 30 Hz */
LSM303DLHC_COMP_ODR_75HZ = 0x18, /**< ODR 75 Hz */
@@ -264,7 +492,7 @@ typedef enum { } lsm303dlhc_comp_odr_t;
/**
- * @brief LSM303DLHC compass subsystem working mode.
+ * @brief LSM303DLHC compass subsystem working mode.
*/
typedef enum {
LSM303DLHC_COMP_MD_CONT = 0x00, /**< Continuous-Conversion Mode */
@@ -273,21 +501,23 @@ typedef enum { } lsm303dlhc_comp_md_t;
/**
- * @brief LSM303DLHC compass subsystem configuration structure.
+ * @brief LSM303DLHC compass subsystem configuration structure.
*/
typedef struct {
/**
- * @brief LSM303DLHC compass subsystem full scale.
+ * @brief LSM303DLHC compass subsystem initial full scale.
*/
lsm303dlhc_comp_fs_t fullscale;
/**
- * @brief LSM303DLHC compass subsystem output data rate.
+ * @brief LSM303DLHC compass subsystem output data rate.
*/
lsm303dlhc_comp_odr_t outputdatarate;
+#if LSM303DLHC_COMP_USE_ADVANCED || defined(__DOXYGEN__)
/**
- * @brief LSM303DLHC compass subsystem working mode.
+ * @brief LSM303DLHC compass subsystem working mode.
*/
lsm303dlhc_comp_md_t mode;
+#endif
} LSM303DLHCCompConfig;
/** @} */
@@ -297,7 +527,7 @@ typedef struct { */
/**
- * @brief Driver state machine possible states.
+ * @brief Driver state machine possible states.
*/
typedef enum {
LSM303DLHC_UNINIT = 0, /**< Not initialized. */
@@ -306,26 +536,9 @@ typedef enum { } lsm303dlhc_state_t;
/**
- * @brief LSM303DLHC configuration structure.
+ * @brief LSM303DLHC configuration structure.
*/
typedef struct {
-#if (LSM303DLHC_USE_SPI) || defined(__DOXYGEN__)
- /**
- * @brief SPI driver associated to this LSM303DLHC.
- */
- SPIDriver *spip;
- /**
- * @brief SPI configuration associated to this LSM303DLHC accelerometer
- * subsystem.
- */
- const SPIConfig *accspicfg;
- /**
- * @brief SPI configuration associated to this LSM303DLHC compass
- * subsystem.
- */
- const SPIConfig *compspicfg;
-#endif /* LSM303DLHC_USE_SPI */
-#if (LSM303DLHC_USE_I2C) || defined(__DOXYGEN__)
/**
* @brief I2C driver associated to this LSM303DLHC.
*/
@@ -335,30 +548,29 @@ typedef struct { * subsystem.
*/
const I2CConfig *i2ccfg;
-#endif /* LSM303DLHC_USE_I2C */
/**
- * @brief LSM303DLHC accelerometer subsystem configuration structure
+ * @brief LSM303DLHC accelerometer subsystem configuration structure
*/
- const LSM303DLHCAccConfig *acccfg;
+ const LSM303DLHCAccConfig *acccfg;
/**
- * @brief LSM303DLHC compass subsystem configuration structure
+ * @brief LSM303DLHC compass subsystem configuration structure
*/
- const LSM303DLHCCompConfig *compcfg;
+ const LSM303DLHCCompConfig *compcfg;
} LSM303DLHCConfig;
/**
- * @brief Structure representing a LSM303DLHC driver.
+ * @brief Structure representing a LSM303DLHC driver.
*/
typedef struct LSM303DLHCDriver LSM303DLHCDriver;
/**
- * @brief @p LSM303DLHC accelerometer subsystem specific methods.
+ * @brief @p LSM303DLHC accelerometer subsystem specific methods.
*/
#define _lsm303dlhc_acc_methods \
_base_accelerometer_methods
/**
- * @brief @p LSM303DLHC compass subsystem specific methods.
+ * @brief @p LSM303DLHC compass subsystem specific methods.
*/
#define _lsm303dlhc_comp_methods \
_base_compass_methods
@@ -366,42 +578,50 @@ typedef struct LSM303DLHCDriver LSM303DLHCDriver; /**
* @extends BaseAccelerometerVMT
*
- * @brief @p LSM303DLHC accelerometer virtual methods table.
+ * @brief @p LSM303DLHC accelerometer virtual methods table.
*/
struct LSM303DLHCACCVMT {
- _lsm303dlhc_acc_methods
+ _lsm303dlhc_acc_methods \
+ /* Change full scale value of LSM303DLHC accelerometer subsystem .*/ \
+ msg_t (*set_full_scale)(void *instance, lsm303dlhc_acc_fs_t fs); \
};
/**
* @extends BaseCompassVMT
*
- * @brief @p LSM303DLHC compass virtual methods table.
+ * @brief @p LSM303DLHC compass virtual methods table.
*/
struct LSM303DLHCCOMPVMT {
- _lsm303dlhc_comp_methods
+ _lsm303dlhc_comp_methods \
+ /* Change full scale value of LSM303DLHC compass subsystem .*/ \
+ msg_t (*set_full_scale)(void *instance, lsm303dlhc_comp_fs_t fs); \
};
/**
- * @brief @p LSM303DLHCDriver specific data.
+ * @brief @p LSM303DLHCDriver specific data.
*/
#define _lsm303dlhc_data \
_base_accelerometer_data \
_base_compass_data \
/* Driver state.*/ \
- lsm303dlhc_state_t state; \
+ lsm303dlhc_state_t state; \
/* Current configuration data.*/ \
- const LSM303DLHCConfig *config; \
+ const LSM303DLHCConfig *config; \
/* Current accelerometer sensitivity.*/ \
float accsensitivity[LSM303DLHC_ACC_NUMBER_OF_AXES]; \
/* Accelerometer bias data.*/ \
- int32_t accbias[LSM303DLHC_ACC_NUMBER_OF_AXES]; \
+ float accbias[LSM303DLHC_ACC_NUMBER_OF_AXES]; \
+ /* Current accelerometer full scale value.*/ \
+ float accfullscale; \
/* Current compass sensitivity.*/ \
float compsensitivity[LSM303DLHC_COMP_NUMBER_OF_AXES];\
- /* Bias data.*/ \
- int32_t compbias[LSM303DLHC_COMP_NUMBER_OF_AXES];
-
+ /* Compass bias data.*/ \
+ float compbias[LSM303DLHC_COMP_NUMBER_OF_AXES]; \
+ /* Current compass full scale value.*/ \
+ float compfullscale;
+
/**
- * @brief LSM303DLHC 6-axis accelerometer/compass class.
+ * @brief LSM303DLHC 6-axis accelerometer/compass class.
*/
struct LSM303DLHCDriver {
/** @brief BaseSensor Virtual Methods Table. */
@@ -422,6 +642,34 @@ struct LSM303DLHCDriver { /* Driver macros. */
/*===========================================================================*/
+/**
+ * @brief Change accelerometer fullscale value.
+ *
+ * @param[in] ip pointer to a @p BaseAccelerometer class.
+ * @param[in] fs the new full scale value.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ * @retval MSG_RESET if one or more errors occurred.
+ * @api
+ */
+#define accelerometerSetFullScale(ip, fs) \
+ (ip)->vmt_lsm303dlhcacc->set_full_scale(ip, fs)
+
+/**
+ * @brief Change compass fullscale value.
+ *
+ * @param[in] ip pointer to a @p BaseCompass class.
+ * @param[in] fs the new full scale value.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ * @retval MSG_RESET if one or more errors occurred.
+ * @api
+ */
+#define compassSetFullScale(ip, fs) \
+ (ip)->vmt_lsm303dlhccomp->set_full_scale(ip, fs)
+
/*===========================================================================*/
/* External declarations. */
/*===========================================================================*/
diff --git a/readme.txt b/readme.txt index 26cdd141f..e00c28b66 100644 --- a/readme.txt +++ b/readme.txt @@ -79,7 +79,9 @@ *****************************************************************************
*** Next ***
-- EX: Added gyroscope implementation for ST L3GD20 devices.
+- EX: Added compass implementation for ST LSM303DLHC device.
+- EX: Added accelerometer implementation for ST LSM303DLHC device.
+- EX: Added gyroscope implementation for ST L3GD20 device.
- HAL: Added events handling to the PAL driver. The EXT driver is now
deprecated but still supported.
- EX: Added flash implementation for Micron M25Qxxx devices.
diff --git a/testhal/STM32/STM32F3xx/I2C-LSM303DLHC/Makefile b/testhal/STM32/STM32F3xx/I2C-LSM303DLHC/Makefile index fe15d208b..707dd7bd5 100644 --- a/testhal/STM32/STM32F3xx/I2C-LSM303DLHC/Makefile +++ b/testhal/STM32/STM32F3xx/I2C-LSM303DLHC/Makefile @@ -100,6 +100,7 @@ include $(CHIBIOS)/os/common/ports/ARMCMx/compilers/GCC/mk/port_v7m.mk # Other files (optional).
include $(CHIBIOS)/os/ex/ST/lsm303dlhc.mk
include $(CHIBIOS)/os/hal/lib/streams/streams.mk
+include $(CHIBIOS)/os/various/shell/shell.mk
# Define linker script file here
LDSCRIPT= $(STARTUPLD)/STM32F303xC.ld
@@ -115,6 +116,7 @@ CSRC = $(STARTUPSRC) \ $(BOARDSRC) \
$(LSM303DLHCSRC) \
$(STREAMSSRC) \
+ $(SHELLSRC) \
usbcfg.c main.c
# C++ sources that can be compiled in ARM or THUMB mode depending on the global
@@ -148,7 +150,7 @@ ASMXSRC = $(STARTUPASM) $(PORTASM) $(OSALASM) INCDIR = $(CHIBIOS)/os/license \
$(STARTUPINC) $(KERNINC) $(PORTINC) $(OSALINC) \
$(HALINC) $(PLATFORMINC) $(BOARDINC) $(LSM303DLHCINC) \
- $(STREAMSINC) $(CHIBIOS)/os/various
+ $(STREAMSINC) $(SHELLINC)
#
# Project, sources and paths
@@ -198,7 +200,8 @@ CPPWARN = -Wall -Wextra -Wundef #
# List all user C define here, like -D_DEBUG=1
-UDEFS = -DCHPRINTF_USE_FLOAT=1
+UDEFS = -DCHPRINTF_USE_FLOAT=1 -DSHELL_CMD_TEST_ENABLED=0 \
+ -DLSM303DLHC_ACC_USE_ADVANCED=0 -DLSM303DLHC_COMP_USE_ADVANCED=0
# Define ASM defines here
UADEFS =
diff --git a/testhal/STM32/STM32F3xx/I2C-LSM303DLHC/main.c b/testhal/STM32/STM32F3xx/I2C-LSM303DLHC/main.c index 5cee22670..30bcf3881 100644 --- a/testhal/STM32/STM32F3xx/I2C-LSM303DLHC/main.c +++ b/testhal/STM32/STM32F3xx/I2C-LSM303DLHC/main.c @@ -18,28 +18,25 @@ #include "hal.h"
#include "usbcfg.h"
+#include "string.h"
+#include "shell.h"
#include "chprintf.h"
-#include "lsm303dlhc.h"
-/*
- * DP resistor control is not possible on the STM32F3-Discovery, using stubs
- * for the connection macros.
- */
-#define usb_lld_connect_bus(usbp)
-#define usb_lld_disconnect_bus(usbp)
-
-/* Enable use of special ANSI escape sequences */
-#define CHPRINTF_USE_ANSI_CODE TRUE
+#include "lsm303dlhc.h"
-static BaseSequentialStream * chp = (BaseSequentialStream*) &SDU1;
+/*===========================================================================*/
+/* LSM303DLHC related. */
+/*===========================================================================*/
/* LSM303DLHC Driver: This object represent an LSM303DLHC instance */
static LSM303DLHCDriver LSM303DLHCD1;
-static int32_t rawdata[LSM303DLHC_ACC_NUMBER_OF_AXES + LSM303DLHC_COMP_NUMBER_OF_AXES];
-static float cookeddata[LSM303DLHC_ACC_NUMBER_OF_AXES + LSM303DLHC_COMP_NUMBER_OF_AXES];
+static int32_t rawdata[LSM303DLHC_ACC_NUMBER_OF_AXES +
+ LSM303DLHC_COMP_NUMBER_OF_AXES];
+static float cookeddata[LSM303DLHC_ACC_NUMBER_OF_AXES +
+ LSM303DLHC_COMP_NUMBER_OF_AXES];
-static char axesID[LSM303DLHC_ACC_NUMBER_OF_AXES] = {'X', 'Y', 'Z'};
+static char axisID[LSM303DLHC_ACC_NUMBER_OF_AXES] = {'X', 'Y', 'Z'};
static uint32_t i;
static const I2CConfig i2ccfg = {
@@ -51,20 +48,24 @@ static const I2CConfig i2ccfg = { };
static const LSM303DLHCAccConfig lsm303dlhcacccfg = {
- LSM303DLHC_ACC_FS_2G,
- LSM303DLHC_ACC_ODR_100Hz,
- LSM303DLHC_ACC_AE_XYZ,
+ {0, 0, 0}, /* Use default sensitivity.*/
+ {0, 0, 0}, /* Use default bias.*/
+ LSM303DLHC_ACC_FS_4G, /* Full scale value 2g.*/
+ LSM303DLHC_ACC_ODR_100Hz, /* Output data rate 100 Hz.*/
+#if LSM303DLHC_ACC_USE_ADVANCED || defined(__DOXYGEN__)
LSM303DLHC_ACC_LP_DISABLED,
LSM303DLHC_ACC_HR_DISABLED,
LSM303DLHC_ACC_BDU_BLOCK,
LSM303DLHC_ACC_END_LITTLE,
- LSM303DLHC_ACC_UNIT_G
+#endif
};
static const LSM303DLHCCompConfig lsm303dlhccompcfg = {
- LSM303DLHC_COMP_FS_1_3_GA,
- LSM303DLHC_COMP_ODR_30HZ,
- LSM303DLHC_COMP_MD_CONT,
+ LSM303DLHC_COMP_FS_1P3GA, /* Full scale value 1.3 Gauss.*/
+ LSM303DLHC_COMP_ODR_30HZ, /* Output data rate 30 Hz.*/
+#if LSM303DLHC_COMP_USE_ADVANCED || defined(__DOXYGEN__)
+ LSM303DLHC_COMP_MD_BLOCK
+#endif
};
static const LSM303DLHCConfig lsm303dlhccfg = {
@@ -74,6 +75,233 @@ static const LSM303DLHCConfig lsm303dlhccfg = { &lsm303dlhccompcfg
};
+/*===========================================================================*/
+/* Command line related. */
+/*===========================================================================*/
+
+/*
+ * DP resistor control is not possible on the STM32F3-Discovery, using stubs
+ * for the connection macros.
+ */
+#define usb_lld_connect_bus(usbp)
+#define usb_lld_disconnect_bus(usbp)
+
+/* Enable use of special ANSI escape sequences */
+#define CHPRINTF_USE_ANSI_CODE TRUE
+#define SHELL_WA_SIZE THD_WORKING_AREA_SIZE(2048)
+
+static void cmd_read(BaseSequentialStream *chp, int argc, char *argv[]) {
+ (void)argv;
+ if (argc != 2) {
+ chprintf(chp, "Usage: read [acc|comp|both] [raw|cooked]\r\n");
+ return;
+ }
+
+ while (chnGetTimeout((BaseChannel *)chp, 150) == Q_TIMEOUT) {
+ if (!strcmp (argv[0], "acc")) {
+ if (!strcmp (argv[1], "raw")) {
+#if CHPRINTF_USE_ANSI_CODE
+ chprintf(chp, "\033[2J\033[1;1H");
+#endif
+ accelerometerReadRaw(&LSM303DLHCD1, rawdata);
+ chprintf(chp, "LSM303DLHC Accelerometer raw data...\r\n");
+ for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %d\r\n", axisID[i], rawdata[i]);
+ }
+ }
+ else if (!strcmp (argv[1], "cooked")) {
+#if CHPRINTF_USE_ANSI_CODE
+ chprintf(chp, "\033[2J\033[1;1H");
+#endif
+ accelerometerReadCooked(&LSM303DLHCD1, cookeddata);
+ chprintf(chp, "LSM303DLHC Accelerometer cooked data...\r\n");
+ for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %.4f mG\r\n", axisID[i], cookeddata[i]);
+ }
+ }
+ else {
+ chprintf(chp, "Usage: read [acc|comp|both] [raw|cooked]\r\n");
+ return;
+ }
+ }
+ else if (!strcmp (argv[0], "comp")) {
+ if (!strcmp (argv[1], "raw")) {
+#if CHPRINTF_USE_ANSI_CODE
+ chprintf(chp, "\033[2J\033[1;1H");
+#endif
+ compassReadRaw(&LSM303DLHCD1, rawdata);
+ chprintf(chp, "LSM303DLHC Compass raw data...\r\n");
+ for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %d\r\n", axisID[i], rawdata[i]);
+ }
+ }
+ else if (!strcmp (argv[1], "cooked")) {
+#if CHPRINTF_USE_ANSI_CODE
+ chprintf(chp, "\033[2J\033[1;1H");
+#endif
+ compassReadCooked(&LSM303DLHCD1, cookeddata);
+ chprintf(chp, "LSM303DLHC Compass cooked data...\r\n");
+ for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %.4f Gauss\r\n", axisID[i], cookeddata[i]);
+ }
+ }
+ else {
+ chprintf(chp, "Usage: read [acc|comp|both] [raw|cooked]\r\n");
+ return;
+ }
+ }
+ else if (!strcmp (argv[0], "both")) {
+ if (!strcmp (argv[1], "raw")) {
+#if CHPRINTF_USE_ANSI_CODE
+ chprintf(chp, "\033[2J\033[1;1H");
+#endif
+ sensorReadRaw(&LSM303DLHCD1, rawdata);
+ chprintf(chp, "LSM303DLHC Accelerometer raw data...\r\n");
+ for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %d\r\n", axisID[i], rawdata[i]);
+ }
+ chprintf(chp, "LSM303DLHC Compass raw data...\r\n");
+ for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %d\r\n", axisID[i],
+ rawdata[i + LSM303DLHC_ACC_NUMBER_OF_AXES]);
+ }
+ }
+ else if (!strcmp (argv[1], "cooked")) {
+#if CHPRINTF_USE_ANSI_CODE
+ chprintf(chp, "\033[2J\033[1;1H");
+#endif
+ sensorReadCooked(&LSM303DLHCD1, cookeddata);
+ chprintf(chp, "LSM303DLHC Accelerometer cooked data...\r\n");
+ for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %.4f mG\r\n", axisID[i], cookeddata[i]);
+ }
+ chprintf(chp, "LSM303DLHC Compass cooked data...\r\n");
+ for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %.4f Gauss\r\n", axisID[i],
+ cookeddata[i + LSM303DLHC_ACC_NUMBER_OF_AXES]);
+ }
+ }
+ else {
+ chprintf(chp, "Usage: read [acc|comp|both] [raw|cooked]\r\n");
+ return;
+ }
+ }
+ else {
+ chprintf(chp, "Usage: read [acc|comp|both] [raw|cooked]\r\n");
+ return;
+ }
+ }
+ chprintf(chp, "Stopped\r\n");
+}
+
+static void cmd_fullscale(BaseSequentialStream *chp, int argc, char *argv[]) {
+ (void)argv;
+ if (argc < 1) {
+ chprintf(chp, "Usage: fullscale [acc|comp] [value]\r\n");
+ return;
+ }
+ if (!strcmp (argv[0], "acc")) {
+#if CHPRINTF_USE_ANSI_CODE
+ chprintf(chp, "\033[2J\033[1;1H");
+#endif
+ if(!strcmp (argv[1], "2G")) {
+ accelerometerSetFullScale(&LSM303DLHCD1, LSM303DLHC_ACC_FS_2G);
+ chprintf(chp, "LSM303DLHC Accelerometer full scale set to 2G...\r\n");
+ }
+ else if(!strcmp (argv[1], "4G")) {
+ accelerometerSetFullScale(&LSM303DLHCD1, LSM303DLHC_ACC_FS_4G);
+ chprintf(chp, "LSM303DLHC Accelerometer full scale set to 4G...\r\n");
+ }
+ else if(!strcmp (argv[1], "8G")) {
+ accelerometerSetFullScale(&LSM303DLHCD1, LSM303DLHC_ACC_FS_8G);
+ chprintf(chp, "LSM303DLHC Accelerometer full scale set to 8G...\r\n");
+ }
+ else if(!strcmp (argv[1], "16G")) {
+ accelerometerSetFullScale(&LSM303DLHCD1, LSM303DLHC_ACC_FS_16G);
+ chprintf(chp, "LSM303DLHC Accelerometer full scale set to 4G...\r\n");
+ }
+ else {
+ chprintf(chp, "Usage: fullscale acc [2G|4G|8G|16G]\r\n");
+ return;
+ }
+ }
+ else if (!strcmp (argv[0], "comp")) {
+#if CHPRINTF_USE_ANSI_CODE
+ chprintf(chp, "\033[2J\033[1;1H");
+#endif
+ if(!strcmp (argv[1], "1.3")) {
+ compassSetFullScale(&LSM303DLHCD1, LSM303DLHC_COMP_FS_1P3GA);
+ chprintf(chp, "LSM303DLHC Compass full scale set to 1.3 Gauss...\r\n");
+ }
+ else if(!strcmp (argv[1], "1.9")) {
+ compassSetFullScale(&LSM303DLHCD1, LSM303DLHC_COMP_FS_1P9GA);
+ chprintf(chp, "LSM303DLHC Compass full scale set to 1.9 Gauss...\r\n");
+ }
+ else if(!strcmp (argv[1], "2.5")) {
+ compassSetFullScale(&LSM303DLHCD1, LSM303DLHC_COMP_FS_2P5GA);
+ chprintf(chp, "LSM303DLHC Compass full scale set to 2.5 Gauss...\r\n");
+ }
+ else if(!strcmp (argv[1], "4.0")) {
+ compassSetFullScale(&LSM303DLHCD1, LSM303DLHC_COMP_FS_4P0GA);
+ chprintf(chp, "LSM303DLHC Compass full scale set to 4.0 Gauss...\r\n");
+ }
+ else if(!strcmp (argv[1], "4.7")) {
+ compassSetFullScale(&LSM303DLHCD1, LSM303DLHC_COMP_FS_4P7GA);
+ chprintf(chp, "LSM303DLHC Compass full scale set to 4.7 Gauss...\r\n");
+ }
+ else if(!strcmp (argv[1], "5.6")) {
+ compassSetFullScale(&LSM303DLHCD1, LSM303DLHC_COMP_FS_5P6GA);
+ chprintf(chp, "LSM303DLHC Compass full scale set to 5.6 Gauss...\r\n");
+ }
+ else if(!strcmp (argv[1], "8.1")) {
+ compassSetFullScale(&LSM303DLHCD1, LSM303DLHC_COMP_FS_8P1GA);
+ chprintf(chp, "LSM303DLHC Compass full scale set to 8.1 Gauss...\r\n");
+ }
+ else {
+ chprintf(chp, "Usage: fullscale comp [1.3|1.9|2.5|4.0|4.7|5.6|8.1]\r\n");
+ return;
+ }
+ }
+ else {
+ chprintf(chp, "Usage: fullscale [acc|comp] [value]\r\n");
+ return;
+ }
+}
+
+static const ShellCommand commands[] = {
+ {"read", cmd_read},
+ {"fullscale", cmd_fullscale},
+ {NULL, NULL}
+};
+
+static const ShellConfig shell_cfg1 = {
+ (BaseSequentialStream *)&SDU1,
+ commands
+};
+
+/*===========================================================================*/
+/* Main code. */
+/*===========================================================================*/
+
+/*
+ * LED blinker thread, times are in milliseconds.
+ */
+static THD_WORKING_AREA(waThread1, 128);
+static THD_FUNCTION(Thread1, arg) {
+
+ (void)arg;
+ chRegSetThreadName("blinker");
+ while (true) {
+ systime_t time;
+
+ time = serusbcfg.usbp->state == USB_ACTIVE ? 250 : 500;
+ palClearLine(LINE_LED3_RED);
+ chThdSleepMilliseconds(time);
+ palSetLine(LINE_LED3_RED);
+ chThdSleepMilliseconds(time);
+ }
+}
+
/*
* Application entry point.
*/
@@ -106,33 +334,34 @@ int main(void) { usbConnectBus(serusbcfg.usbp);
/*
+ * Creates the blinker thread.
+ */
+ chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO + 1, Thread1, NULL);
+
+ /*
* LSM303DLHC Object Initialization
*/
lsm303dlhcObjectInit(&LSM303DLHCD1);
+ /*
+ * Activates the LSM303DLHC driver.
+ */
lsm303dlhcStart(&LSM303DLHCD1, &lsm303dlhccfg);
- while (TRUE) {
- palToggleLine(LINE_LED3_RED);
- sensorReadRaw(&LSM303DLHCD1, rawdata);
- sensorReadCooked(&LSM303DLHCD1, cookeddata);
- chprintf(chp, "ACCELEROMETER DATA\r\n");
- for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
- chprintf(chp, "RAW-%c:%d\t\t", axesID[i], rawdata[i]);
- chprintf(chp, "\r\n");
- for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
- chprintf(chp, "COOKED-%c:%.3f g\t", axesID[i], cookeddata[i]);
-
- chprintf(chp, "\r\nCOMPSCOPE DATA\r\n");
- for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++)
- chprintf(chp, "RAW-%c:%d\t\t", axesID[i], rawdata[i + 3]);
- chprintf(chp, "\r\n");
- for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++)
- chprintf(chp, "COOKED-%c:%.3f Gauss\t", axesID[i], cookeddata[i + 3]);
- chprintf(chp, "\r\n");
- chThdSleepMilliseconds(100);
-#if CHPRINTF_USE_ANSI_CODE
- chprintf(chp, "\033[2J\033[1;1H");
-#endif
+ /*
+ * Shell manager initialization.
+ */
+ shellInit();
+
+ while(TRUE) {
+ if (SDU1.config->usbp->state == USB_ACTIVE) {
+ thread_t *shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
+ "shell", NORMALPRIO + 1,
+ shellThread, (void *)&shell_cfg1);
+ chThdWait(shelltp); /* Waiting termination. */
+ }
+ chThdSleepMilliseconds(1000);
}
+ lsm303dlhcStop(&LSM303DLHCD1);
+ return 0;
}
diff --git a/testhal/STM32/STM32F4xx/I2C-LSM303DLHC/Makefile b/testhal/STM32/STM32F4xx/I2C-LSM303DLHC/Makefile index 9c2b36c4f..664c9a60e 100644 --- a/testhal/STM32/STM32F4xx/I2C-LSM303DLHC/Makefile +++ b/testhal/STM32/STM32F4xx/I2C-LSM303DLHC/Makefile @@ -100,6 +100,7 @@ include $(CHIBIOS)/os/common/ports/ARMCMx/compilers/GCC/mk/port_v7m.mk # Other files (optional).
include $(CHIBIOS)/os/ex/ST/lsm303dlhc.mk
include $(CHIBIOS)/os/hal/lib/streams/streams.mk
+include $(CHIBIOS)/os/various/shell/shell.mk
# Define linker script file here
LDSCRIPT= $(STARTUPLD)/STM32F401xC.ld
@@ -115,6 +116,7 @@ CSRC = $(STARTUPSRC) \ $(BOARDSRC) \
$(LSM303DLHCSRC) \
$(STREAMSSRC) \
+ $(SHELLSRC) \
usbcfg.c main.c
# C++ sources that can be compiled in ARM or THUMB mode depending on the global
@@ -148,7 +150,7 @@ ASMXSRC = $(STARTUPASM) $(PORTASM) $(OSALASM) INCDIR = $(CHIBIOS)/os/license \
$(STARTUPINC) $(KERNINC) $(PORTINC) $(OSALINC) \
$(HALINC) $(PLATFORMINC) $(BOARDINC) $(LSM303DLHCINC) \
- $(STREAMSINC) $(CHIBIOS)/os/various
+ $(STREAMSINC) $(SHELLINC)
#
# Project, sources and paths
@@ -198,7 +200,8 @@ CPPWARN = -Wall -Wextra -Wundef #
# List all user C define here, like -D_DEBUG=1
-UDEFS = -DCHPRINTF_USE_FLOAT=1
+UDEFS = -DCHPRINTF_USE_FLOAT=1 -DSHELL_CMD_TEST_ENABLED=0 \
+ -DLSM303GLHC_ACC_USE_ADVANCED=0 -DLSM303GLHC_COMP_USE_ADVANCED=0
# Define ASM defines here
UADEFS =
diff --git a/testhal/STM32/STM32F4xx/I2C-LSM303DLHC/main.c b/testhal/STM32/STM32F4xx/I2C-LSM303DLHC/main.c index e492ef240..0ab38a92c 100644 --- a/testhal/STM32/STM32F4xx/I2C-LSM303DLHC/main.c +++ b/testhal/STM32/STM32F4xx/I2C-LSM303DLHC/main.c @@ -18,21 +18,25 @@ #include "hal.h"
#include "usbcfg.h"
+#include "string.h"
+#include "shell.h"
#include "chprintf.h"
-#include "lsm303dlhc.h"
-/* Enable use of special ANSI escape sequences */
-#define CHPRINTF_USE_ANSI_CODE TRUE
+#include "lsm303dlhc.h"
-static BaseSequentialStream * chp = (BaseSequentialStream*) &SDU1;
+/*===========================================================================*/
+/* LSM303DLHC related. */
+/*===========================================================================*/
/* LSM303DLHC Driver: This object represent an LSM303DLHC instance */
static LSM303DLHCDriver LSM303DLHCD1;
-static int32_t rawdata[LSM303DLHC_ACC_NUMBER_OF_AXES + LSM303DLHC_COMP_NUMBER_OF_AXES];
-static float cookeddata[LSM303DLHC_ACC_NUMBER_OF_AXES + LSM303DLHC_COMP_NUMBER_OF_AXES];
+static int32_t rawdata[LSM303DLHC_ACC_NUMBER_OF_AXES +
+ LSM303DLHC_COMP_NUMBER_OF_AXES];
+static float cookeddata[LSM303DLHC_ACC_NUMBER_OF_AXES +
+ LSM303DLHC_COMP_NUMBER_OF_AXES];
-static char axesID[LSM303DLHC_ACC_NUMBER_OF_AXES] = {'X', 'Y', 'Z'};
+static char axisID[LSM303DLHC_ACC_NUMBER_OF_AXES] = {'X', 'Y', 'Z'};
static uint32_t i;
static const I2CConfig i2ccfg = {
@@ -42,20 +46,24 @@ static const I2CConfig i2ccfg = { };
static const LSM303DLHCAccConfig lsm303dlhcacccfg = {
- LSM303DLHC_ACC_FS_2G,
- LSM303DLHC_ACC_ODR_100Hz,
- LSM303DLHC_ACC_AE_XYZ,
+ {0, 0, 0}, /* Use default sensitivity.*/
+ {0, 0, 0}, /* Use default bias.*/
+ LSM303DLHC_ACC_FS_4G, /* Full scale value 2g.*/
+ LSM303DLHC_ACC_ODR_100Hz, /* Output data rate 100 Hz.*/
+#if LSM303DLHC_ACC_USE_ADVANCED || defined(__DOXYGEN__)
LSM303DLHC_ACC_LP_DISABLED,
LSM303DLHC_ACC_HR_DISABLED,
LSM303DLHC_ACC_BDU_BLOCK,
LSM303DLHC_ACC_END_LITTLE,
- LSM303DLHC_ACC_UNIT_G
+#endif
};
static const LSM303DLHCCompConfig lsm303dlhccompcfg = {
- LSM303DLHC_COMP_FS_1_3_GA,
- LSM303DLHC_COMP_ODR_30HZ,
+ LSM303DLHC_COMP_FS_1P3GA, /* Full scale value 1.3 Gauss.*/
+ LSM303DLHC_COMP_ODR_30HZ, /* Output data rate 30 Hz.*/
+#if LSM303DLHC_COMP_USE_ADVANCED || defined(__DOXYGEN__)
LSM303DLHC_COMP_MD_BLOCK
+#endif
};
static const LSM303DLHCConfig lsm303dlhccfg = {
@@ -65,6 +73,226 @@ static const LSM303DLHCConfig lsm303dlhccfg = { &lsm303dlhccompcfg
};
+/*===========================================================================*/
+/* Command line related. */
+/*===========================================================================*/
+
+/* Enable use of special ANSI escape sequences */
+#define CHPRINTF_USE_ANSI_CODE TRUE
+#define SHELL_WA_SIZE THD_WORKING_AREA_SIZE(2048)
+
+static void cmd_read(BaseSequentialStream *chp, int argc, char *argv[]) {
+ (void)argv;
+ if (argc != 2) {
+ chprintf(chp, "Usage: read [acc|comp|both] [raw|cooked]\r\n");
+ return;
+ }
+
+ while (chnGetTimeout((BaseChannel *)chp, 150) == Q_TIMEOUT) {
+ if (!strcmp (argv[0], "acc")) {
+ if (!strcmp (argv[1], "raw")) {
+#if CHPRINTF_USE_ANSI_CODE
+ chprintf(chp, "\033[2J\033[1;1H");
+#endif
+ accelerometerReadRaw(&LSM303DLHCD1, rawdata);
+ chprintf(chp, "LSM303DLHC Accelerometer raw data...\r\n");
+ for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %d\r\n", axisID[i], rawdata[i]);
+ }
+ }
+ else if (!strcmp (argv[1], "cooked")) {
+#if CHPRINTF_USE_ANSI_CODE
+ chprintf(chp, "\033[2J\033[1;1H");
+#endif
+ accelerometerReadCooked(&LSM303DLHCD1, cookeddata);
+ chprintf(chp, "LSM303DLHC Accelerometer cooked data...\r\n");
+ for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %.4f mG\r\n", axisID[i], cookeddata[i]);
+ }
+ }
+ else {
+ chprintf(chp, "Usage: read [acc|comp|both] [raw|cooked]\r\n");
+ return;
+ }
+ }
+ else if (!strcmp (argv[0], "comp")) {
+ if (!strcmp (argv[1], "raw")) {
+#if CHPRINTF_USE_ANSI_CODE
+ chprintf(chp, "\033[2J\033[1;1H");
+#endif
+ compassReadRaw(&LSM303DLHCD1, rawdata);
+ chprintf(chp, "LSM303DLHC Compass raw data...\r\n");
+ for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %d\r\n", axisID[i], rawdata[i]);
+ }
+ }
+ else if (!strcmp (argv[1], "cooked")) {
+#if CHPRINTF_USE_ANSI_CODE
+ chprintf(chp, "\033[2J\033[1;1H");
+#endif
+ compassReadCooked(&LSM303DLHCD1, cookeddata);
+ chprintf(chp, "LSM303DLHC Compass cooked data...\r\n");
+ for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %.4f Gauss\r\n", axisID[i], cookeddata[i]);
+ }
+ }
+ else {
+ chprintf(chp, "Usage: read [acc|comp|both] [raw|cooked]\r\n");
+ return;
+ }
+ }
+ else if (!strcmp (argv[0], "both")) {
+ if (!strcmp (argv[1], "raw")) {
+#if CHPRINTF_USE_ANSI_CODE
+ chprintf(chp, "\033[2J\033[1;1H");
+#endif
+ sensorReadRaw(&LSM303DLHCD1, rawdata);
+ chprintf(chp, "LSM303DLHC Accelerometer raw data...\r\n");
+ for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %d\r\n", axisID[i], rawdata[i]);
+ }
+ chprintf(chp, "LSM303DLHC Compass raw data...\r\n");
+ for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %d\r\n", axisID[i],
+ rawdata[i + LSM303DLHC_ACC_NUMBER_OF_AXES]);
+ }
+ }
+ else if (!strcmp (argv[1], "cooked")) {
+#if CHPRINTF_USE_ANSI_CODE
+ chprintf(chp, "\033[2J\033[1;1H");
+#endif
+ sensorReadCooked(&LSM303DLHCD1, cookeddata);
+ chprintf(chp, "LSM303DLHC Accelerometer cooked data...\r\n");
+ for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %.4f mG\r\n", axisID[i], cookeddata[i]);
+ }
+ chprintf(chp, "LSM303DLHC Compass cooked data...\r\n");
+ for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %.4f Gauss\r\n", axisID[i],
+ cookeddata[i + LSM303DLHC_ACC_NUMBER_OF_AXES]);
+ }
+ }
+ else {
+ chprintf(chp, "Usage: read [acc|comp|both] [raw|cooked]\r\n");
+ return;
+ }
+ }
+ else {
+ chprintf(chp, "Usage: read [acc|comp|both] [raw|cooked]\r\n");
+ return;
+ }
+ }
+ chprintf(chp, "Stopped\r\n");
+}
+
+static void cmd_fullscale(BaseSequentialStream *chp, int argc, char *argv[]) {
+ (void)argv;
+ if (argc < 1) {
+ chprintf(chp, "Usage: fullscale [acc|comp] [value]\r\n");
+ return;
+ }
+ if (!strcmp (argv[0], "acc")) {
+#if CHPRINTF_USE_ANSI_CODE
+ chprintf(chp, "\033[2J\033[1;1H");
+#endif
+ if(!strcmp (argv[1], "2G")) {
+ accelerometerSetFullScale(&LSM303DLHCD1, LSM303DLHC_ACC_FS_2G);
+ chprintf(chp, "LSM303DLHC Accelerometer full scale set to 2G...\r\n");
+ }
+ else if(!strcmp (argv[1], "4G")) {
+ accelerometerSetFullScale(&LSM303DLHCD1, LSM303DLHC_ACC_FS_4G);
+ chprintf(chp, "LSM303DLHC Accelerometer full scale set to 4G...\r\n");
+ }
+ else if(!strcmp (argv[1], "8G")) {
+ accelerometerSetFullScale(&LSM303DLHCD1, LSM303DLHC_ACC_FS_8G);
+ chprintf(chp, "LSM303DLHC Accelerometer full scale set to 8G...\r\n");
+ }
+ else if(!strcmp (argv[1], "16G")) {
+ accelerometerSetFullScale(&LSM303DLHCD1, LSM303DLHC_ACC_FS_16G);
+ chprintf(chp, "LSM303DLHC Accelerometer full scale set to 4G...\r\n");
+ }
+ else {
+ chprintf(chp, "Usage: fullscale acc [2G|4G|8G|16G]\r\n");
+ return;
+ }
+ }
+ else if (!strcmp (argv[0], "comp")) {
+#if CHPRINTF_USE_ANSI_CODE
+ chprintf(chp, "\033[2J\033[1;1H");
+#endif
+ if(!strcmp (argv[1], "1.3")) {
+ compassSetFullScale(&LSM303DLHCD1, LSM303DLHC_COMP_FS_1P3GA);
+ chprintf(chp, "LSM303DLHC Compass full scale set to 1.3 Gauss...\r\n");
+ }
+ else if(!strcmp (argv[1], "1.9")) {
+ compassSetFullScale(&LSM303DLHCD1, LSM303DLHC_COMP_FS_1P9GA);
+ chprintf(chp, "LSM303DLHC Compass full scale set to 1.9 Gauss...\r\n");
+ }
+ else if(!strcmp (argv[1], "2.5")) {
+ compassSetFullScale(&LSM303DLHCD1, LSM303DLHC_COMP_FS_2P5GA);
+ chprintf(chp, "LSM303DLHC Compass full scale set to 2.5 Gauss...\r\n");
+ }
+ else if(!strcmp (argv[1], "4.0")) {
+ compassSetFullScale(&LSM303DLHCD1, LSM303DLHC_COMP_FS_4P0GA);
+ chprintf(chp, "LSM303DLHC Compass full scale set to 4.0 Gauss...\r\n");
+ }
+ else if(!strcmp (argv[1], "4.7")) {
+ compassSetFullScale(&LSM303DLHCD1, LSM303DLHC_COMP_FS_4P7GA);
+ chprintf(chp, "LSM303DLHC Compass full scale set to 4.7 Gauss...\r\n");
+ }
+ else if(!strcmp (argv[1], "5.6")) {
+ compassSetFullScale(&LSM303DLHCD1, LSM303DLHC_COMP_FS_5P6GA);
+ chprintf(chp, "LSM303DLHC Compass full scale set to 5.6 Gauss...\r\n");
+ }
+ else if(!strcmp (argv[1], "8.1")) {
+ compassSetFullScale(&LSM303DLHCD1, LSM303DLHC_COMP_FS_8P1GA);
+ chprintf(chp, "LSM303DLHC Compass full scale set to 8.1 Gauss...\r\n");
+ }
+ else {
+ chprintf(chp, "Usage: fullscale comp [1.3|1.9|2.5|4.0|4.7|5.6|8.1]\r\n");
+ return;
+ }
+ }
+ else {
+ chprintf(chp, "Usage: fullscale [acc|comp] [value]\r\n");
+ return;
+ }
+}
+
+static const ShellCommand commands[] = {
+ {"read", cmd_read},
+ {"fullscale", cmd_fullscale},
+ {NULL, NULL}
+};
+
+static const ShellConfig shell_cfg1 = {
+ (BaseSequentialStream *)&SDU1,
+ commands
+};
+
+/*===========================================================================*/
+/* Main code. */
+/*===========================================================================*/
+
+/*
+ * LED blinker thread, times are in milliseconds.
+ */
+static THD_WORKING_AREA(waThread1, 128);
+static THD_FUNCTION(Thread1, arg) {
+
+ (void)arg;
+ chRegSetThreadName("blinker");
+ while (true) {
+ systime_t time;
+
+ time = serusbcfg.usbp->state == USB_ACTIVE ? 250 : 500;
+ palClearLine(LINE_LED5);
+ chThdSleepMilliseconds(time);
+ palSetLine(LINE_LED5);
+ chThdSleepMilliseconds(time);
+ }
+}
+
/*
* Application entry point.
*/
@@ -97,33 +325,34 @@ int main(void) { usbConnectBus(serusbcfg.usbp);
/*
+ * Creates the blinker thread.
+ */
+ chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO + 1, Thread1, NULL);
+
+ /*
* LSM303DLHC Object Initialization
*/
lsm303dlhcObjectInit(&LSM303DLHCD1);
+ /*
+ * Activates the LSM303DLHC driver.
+ */
lsm303dlhcStart(&LSM303DLHCD1, &lsm303dlhccfg);
- while (TRUE) {
- palToggleLine(LINE_LED3);
- sensorReadRaw(&LSM303DLHCD1, rawdata);
- sensorReadCooked(&LSM303DLHCD1, cookeddata);
- chprintf(chp, "ACCELEROMETER DATA\r\n");
- for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
- chprintf(chp, "RAW-%c:%d\t\t", axesID[i], rawdata[i]);
- chprintf(chp, "\r\n");
- for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
- chprintf(chp, "COOKED-%c:%.3f g\t", axesID[i], cookeddata[i]);
-
- chprintf(chp, "\r\nCOMPSCOPE DATA\r\n");
- for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++)
- chprintf(chp, "RAW-%c:%d\t\t", axesID[i], rawdata[i + 3]);
- chprintf(chp, "\r\n");
- for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++)
- chprintf(chp, "COOKED-%c:%.3f Gauss\t", axesID[i], cookeddata[i + 3]);
- chprintf(chp, "\r\n");
- chThdSleepMilliseconds(100);
-#if CHPRINTF_USE_ANSI_CODE
- chprintf(chp, "\033[2J\033[1;1H");
-#endif
+ /*
+ * Shell manager initialization.
+ */
+ shellInit();
+
+ while(TRUE) {
+ if (SDU1.config->usbp->state == USB_ACTIVE) {
+ thread_t *shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
+ "shell", NORMALPRIO + 1,
+ shellThread, (void *)&shell_cfg1);
+ chThdWait(shelltp); /* Waiting termination. */
+ }
+ chThdSleepMilliseconds(1000);
}
+ lsm303dlhcStop(&LSM303DLHCD1);
+ return 0;
}
|