Updated EX: LIS3MDL driver updated to v1.0.0
git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@9736 35acf78f-673a-0410-8e92-d51de3d6d3f4
This commit is contained in:
Rocco Marco Guglielmi
parent
3c0557bcc4
commit
74881fe821
|
|
@ -33,55 +33,6 @@
|
|||
/* Driver local definitions. */
|
||||
/*===========================================================================*/
|
||||
|
||||
#define LIS3MDL_SENS_4GA ((float)6842.0f)
|
||||
#define LIS3MDL_SENS_8GA ((float)3421.0f)
|
||||
#define LIS3MDL_SENS_12GA ((float)2281.0f)
|
||||
#define LIS3MDL_SENS_16GA ((float)1711.0f)
|
||||
|
||||
#define LIS3MDL_TEMP_SENS ((float)8.0f)
|
||||
|
||||
#define LIS3MDL_DI ((uint8_t)0xFF)
|
||||
#define LIS3MDL_DI_0 ((uint8_t)0x01)
|
||||
#define LIS3MDL_DI_1 ((uint8_t)0x02)
|
||||
#define LIS3MDL_DI_2 ((uint8_t)0x04)
|
||||
#define LIS3MDL_DI_3 ((uint8_t)0x08)
|
||||
#define LIS3MDL_DI_4 ((uint8_t)0x10)
|
||||
#define LIS3MDL_DI_5 ((uint8_t)0x20)
|
||||
#define LIS3MDL_DI_6 ((uint8_t)0x40)
|
||||
#define LIS3MDL_DI_7 ((uint8_t)0x80)
|
||||
|
||||
#define LIS3MDL_AD ((uint8_t)0x3F)
|
||||
#define LIS3MDL_AD_0 ((uint8_t)0x01)
|
||||
#define LIS3MDL_AD_1 ((uint8_t)0x02)
|
||||
#define LIS3MDL_AD_2 ((uint8_t)0x04)
|
||||
#define LIS3MDL_AD_3 ((uint8_t)0x08)
|
||||
#define LIS3MDL_AD_4 ((uint8_t)0x10)
|
||||
#define LIS3MDL_AD_5 ((uint8_t)0x20)
|
||||
#define LIS3MDL_AD_6 ((uint8_t)0x40)
|
||||
#define LIS3MDL_RW ((uint8_t)0x80)
|
||||
|
||||
#define LIS3MDL_AD_WHO_AM_I ((uint8_t)0x0F)
|
||||
#define LIS3MDL_AD_CTRL_REG1 ((uint8_t)0x20)
|
||||
#define LIS3MDL_AD_CTRL_REG2 ((uint8_t)0x21)
|
||||
#define LIS3MDL_AD_CTRL_REG3 ((uint8_t)0x22)
|
||||
#define LIS3MDL_AD_CTRL_REG4 ((uint8_t)0x23)
|
||||
#define LIS3MDL_AD_CTRL_REG5 ((uint8_t)0x24)
|
||||
#define LIS3MDL_AD_STATUS_REG ((uint8_t)0x27)
|
||||
#define LIS3MDL_AD_OUT_X_L ((uint8_t)0x28)
|
||||
#define LIS3MDL_AD_OUT_X_H ((uint8_t)0x29)
|
||||
#define LIS3MDL_AD_OUT_Y_L ((uint8_t)0x2A)
|
||||
#define LIS3MDL_AD_OUT_Y_H ((uint8_t)0x2B)
|
||||
#define LIS3MDL_AD_OUT_Z_L ((uint8_t)0x2C)
|
||||
#define LIS3MDL_AD_OUT_Z_H ((uint8_t)0x2D)
|
||||
#define LIS3MDL_AD_TEMP_OUT_L ((uint8_t)0x2E)
|
||||
#define LIS3MDL_AD_TEMP_OUT_H ((uint8_t)0x2F)
|
||||
#define LIS3MDL_AD_INT_CFG ((uint8_t)0x30)
|
||||
#define LIS3MDL_AD_INT_SOURCE ((uint8_t)0x31)
|
||||
#define LIS3MDL_AD_INT_THS_L ((uint8_t)0x32)
|
||||
#define LIS3MDL_AD_INT_THS_H ((uint8_t)0x33)
|
||||
|
||||
#define LIS3MDL_CTRL_REG2_FS_MASK ((uint8_t)0x60)
|
||||
|
||||
/*===========================================================================*/
|
||||
/* Driver exported variables. */
|
||||
/*===========================================================================*/
|
||||
|
|
@ -133,46 +84,16 @@ uint8_t lis3mdlI2CReadRegister(I2CDriver *i2cp, lis3mdl_sad_t sad, uint8_t reg,
|
|||
*
|
||||
* @param[in] i2cp pointer to the I2C interface
|
||||
* @param[in] sad slave address without R bit
|
||||
* @param[in] sub sub-register address
|
||||
* @param[in] reg sub-register address
|
||||
* @param[in] value the value to be written
|
||||
* @return the operation status.
|
||||
*/
|
||||
msg_t lis3mdlI2CWriteRegister(I2CDriver *i2cp, lis3mdl_sad_t sad, uint8_t reg,
|
||||
uint8_t value) {
|
||||
uint8_t rxbuf;
|
||||
uint8_t txbuf[2];
|
||||
switch (reg) {
|
||||
default:
|
||||
/* Reserved register must not be written, according to the datasheet
|
||||
* this could permanently damage the device.
|
||||
*/
|
||||
chDbgAssert(FALSE, "lis3mdlI2CWriteRegister(), reserved register");
|
||||
case LIS3MDL_AD_WHO_AM_I:
|
||||
case LIS3MDL_AD_STATUS_REG:
|
||||
case LIS3MDL_AD_OUT_X_L:
|
||||
case LIS3MDL_AD_OUT_X_H:
|
||||
case LIS3MDL_AD_OUT_Y_L:
|
||||
case LIS3MDL_AD_OUT_Y_H:
|
||||
case LIS3MDL_AD_OUT_Z_L:
|
||||
case LIS3MDL_AD_OUT_Z_H:
|
||||
case LIS3MDL_AD_TEMP_OUT_L:
|
||||
case LIS3MDL_AD_TEMP_OUT_H:
|
||||
case LIS3MDL_AD_INT_SOURCE:
|
||||
case LIS3MDL_AD_INT_THS_L:
|
||||
case LIS3MDL_AD_INT_THS_H:
|
||||
/* Read only registers cannot be written, the command is ignored.*/
|
||||
return MSG_RESET;
|
||||
case LIS3MDL_AD_CTRL_REG1:
|
||||
case LIS3MDL_AD_CTRL_REG2:
|
||||
case LIS3MDL_AD_CTRL_REG3:
|
||||
case LIS3MDL_AD_CTRL_REG4:
|
||||
case LIS3MDL_AD_CTRL_REG5:
|
||||
case LIS3MDL_AD_INT_CFG:
|
||||
txbuf[0] = reg;
|
||||
txbuf[1] = value;
|
||||
return i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0, TIME_INFINITE);
|
||||
break;
|
||||
}
|
||||
txbuf[0] = reg;
|
||||
txbuf[1] = value;
|
||||
return i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, NULL, 0, TIME_INFINITE);
|
||||
}
|
||||
#endif /* LIS3MDL_USE_I2C */
|
||||
|
||||
|
|
@ -189,9 +110,8 @@ static msg_t read_raw(void *ip, int32_t axes[LIS3MDL_NUMBER_OF_AXES]) {
|
|||
uint16_t tmp;
|
||||
osalDbgCheck((ip != NULL) && (axes != NULL));
|
||||
osalDbgAssert((((LIS3MDLDriver *)ip)->state == LIS3MDL_READY),
|
||||
"read_raw(), invalid state");
|
||||
"read_raw(), invalid state");
|
||||
|
||||
#if LIS3MDL_USE_I2C
|
||||
osalDbgAssert((((LIS3MDLDriver *)ip)->config->i2cp->state == I2C_READY),
|
||||
"read_raw(), channel not ready");
|
||||
#if LIS3MDL_SHARED_I2C
|
||||
|
|
@ -199,35 +119,44 @@ static msg_t read_raw(void *ip, int32_t axes[LIS3MDL_NUMBER_OF_AXES]) {
|
|||
i2cStart(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->i2ccfg);
|
||||
#endif /* LIS3MDL_SHARED_I2C */
|
||||
|
||||
tmp = lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_OUT_X_L, NULL);
|
||||
LIS3MDL_AD_OUT_X_L, &msg);
|
||||
if (msg != MSG_OK)
|
||||
return msg;
|
||||
tmp += lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_OUT_X_H, NULL) << 8;
|
||||
LIS3MDL_AD_OUT_X_H, &msg) << 8;
|
||||
if (msg != MSG_OK)
|
||||
return msg;
|
||||
axes[0] = (int32_t)tmp - ((LIS3MDLDriver *)ip)->bias[0];
|
||||
|
||||
tmp = lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_OUT_Y_L, NULL);
|
||||
LIS3MDL_AD_OUT_Y_L, &msg);
|
||||
if (msg != MSG_OK)
|
||||
return msg;
|
||||
tmp += lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_OUT_Y_H, NULL) << 8;
|
||||
LIS3MDL_AD_OUT_Y_H, &msg) << 8;
|
||||
if (msg != MSG_OK)
|
||||
return msg;
|
||||
axes[1] = (int32_t)tmp - ((LIS3MDLDriver *)ip)->bias[1];
|
||||
|
||||
tmp = lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_OUT_Z_L, NULL);
|
||||
LIS3MDL_AD_OUT_Z_L, &msg);
|
||||
if (msg != MSG_OK)
|
||||
return msg;
|
||||
tmp += lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_OUT_Z_H, NULL) << 8;
|
||||
LIS3MDL_AD_OUT_Z_H, &msg) << 8;
|
||||
if (msg != MSG_OK)
|
||||
return msg;
|
||||
axes[2] = (int32_t)tmp - ((LIS3MDLDriver *)ip)->bias[2];
|
||||
|
||||
#if LIS3MDL_SHARED_I2C
|
||||
i2cReleaseBus(((LIS3MDLDriver *)ip)->config->i2cp);
|
||||
#endif /* LIS3MDL_SHARED_I2C */
|
||||
#endif /* LIS3MDL_USE_I2C */
|
||||
return MSG_OK;
|
||||
}
|
||||
|
||||
|
|
@ -239,11 +168,12 @@ static msg_t read_cooked(void *ip, float axes[]) {
|
|||
osalDbgCheck((ip != NULL) && (axes != NULL));
|
||||
|
||||
osalDbgAssert((((LIS3MDLDriver *)ip)->state == LIS3MDL_READY),
|
||||
"read_cooked(), invalid state");
|
||||
"read_cooked(), invalid state");
|
||||
|
||||
msg = read_raw(ip, raw);
|
||||
for(i = 0; i < LIS3MDL_NUMBER_OF_AXES ; i++){
|
||||
axes[i] = raw[i] / ((LIS3MDLDriver *)ip)->sensitivity[i];
|
||||
axes[i] -= ((LIS3MDLDriver *)ip)->bias[i];
|
||||
}
|
||||
return msg;
|
||||
}
|
||||
|
|
@ -318,27 +248,52 @@ static msg_t reset_sensivity(void *ip) {
|
|||
return MSG_OK;
|
||||
}
|
||||
|
||||
static msg_t get_temperature(void *ip, float* tempp) {
|
||||
int16_t temp;
|
||||
#if LIS3MDL_USE_I2C
|
||||
osalDbgAssert((((LIS3MDLDriver *)ip)->config->i2cp->state == I2C_READY),
|
||||
"gyro_read_raw(), channel not ready");
|
||||
#if LIS3MDL_SHARED_I2C
|
||||
i2cAcquireBus(((LIS3MDLDriver *)ip)->config->i2cp);
|
||||
i2cStart(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->i2ccfg);
|
||||
#endif /* LIS3MDL_SHARED_I2C */
|
||||
temp = (int16_t)(lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_TEMP_OUT_L, NULL));
|
||||
temp += (int16_t)(lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_TEMP_OUT_H, NULL) << 8);
|
||||
#if LIS3MDL_SHARED_I2C
|
||||
i2cReleaseBus(((LIS3MDLDriver *)ip)->config->i2cp);
|
||||
#endif /* LIS3MDL_SHARED_I2C */
|
||||
#endif /* LIS3MDL_USE_I2C */
|
||||
*tempp = (float)temp / LIS3MDL_TEMP_SENS;
|
||||
static msg_t set_full_scale(void *ip, lis3mdl_fs_t fs) {
|
||||
float newfs, scale;
|
||||
uint8_t i, cr;
|
||||
msg_t msg;
|
||||
|
||||
if(fs == LIS3MDL_FS_4GA) {
|
||||
newfs = LIS3MDL_4GA;
|
||||
}
|
||||
else if(fs == LIS3MDL_FS_8GA) {
|
||||
newfs = LIS3MDL_8GA;
|
||||
}
|
||||
else if(fs == LIS3MDL_FS_12GA) {
|
||||
newfs = LIS3MDL_12GA;
|
||||
}
|
||||
else if(fs == LIS3MDL_FS_16GA) {
|
||||
newfs = LIS3MDL_16GA;
|
||||
}
|
||||
else {
|
||||
return MSG_RESET;
|
||||
}
|
||||
|
||||
if(newfs != ((LIS3MDLDriver *)ip)->fullscale) {
|
||||
scale = newfs / ((LIS3MDLDriver *)ip)->fullscale;
|
||||
((LIS3MDLDriver *)ip)->fullscale = newfs;
|
||||
|
||||
/* Updating register.*/
|
||||
cr = lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_CTRL_REG2, &msg);
|
||||
if(msg != MSG_OK)
|
||||
return msg;
|
||||
|
||||
cr &= ~(LIS3MDL_CTRL_REG2_FS_MASK);
|
||||
cr |= fs;
|
||||
msg = lis3mdlI2CWriteRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_CTRL_REG2, cr);
|
||||
if(msg != MSG_OK)
|
||||
return msg;
|
||||
|
||||
/* Scaling sensitivity and bias. Re-calibration is suggested anyway. */
|
||||
for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++) {
|
||||
((LIS3MDLDriver *)ip)->sensitivity[i] *= scale;
|
||||
((LIS3MDLDriver *)ip)->bias[i] *= scale;
|
||||
}
|
||||
}
|
||||
return MSG_OK;
|
||||
}
|
||||
|
||||
|
|
@ -354,7 +309,7 @@ static const struct BaseCompassVMT vmt_basecompass = {
|
|||
static const struct LIS3MDLVMT vmt_lis3mdl = {
|
||||
get_axes_number, read_raw, read_cooked,
|
||||
set_bias, reset_bias, set_sensivity, reset_sensivity,
|
||||
get_temperature
|
||||
set_full_scale
|
||||
};
|
||||
|
||||
/*===========================================================================*/
|
||||
|
|
@ -389,6 +344,7 @@ void lis3mdlObjectInit(LIS3MDLDriver *devp) {
|
|||
*/
|
||||
void lis3mdlStart(LIS3MDLDriver *devp, const LIS3MDLConfig *config) {
|
||||
uint32_t i;
|
||||
uint8_t cr;
|
||||
osalDbgCheck((devp != NULL) && (config != NULL));
|
||||
|
||||
osalDbgAssert((devp->state == LIS3MDL_STOP) || (devp->state == LIS3MDL_READY),
|
||||
|
|
@ -402,46 +358,82 @@ void lis3mdlStart(LIS3MDLDriver *devp, const LIS3MDLConfig *config) {
|
|||
#endif /* LIS3MDL_SHARED_I2C */
|
||||
i2cStart((devp)->config->i2cp,
|
||||
(devp)->config->i2ccfg);
|
||||
lis3mdlI2CWriteRegister(devp->config->i2cp,
|
||||
devp->config->slaveaddress,
|
||||
LIS3MDL_AD_CTRL_REG1,
|
||||
devp->config->temperature |
|
||||
devp->config->outputdatarate |
|
||||
devp->config->operationmodexy);
|
||||
lis3mdlI2CWriteRegister(devp->config->i2cp,
|
||||
devp->config->slaveaddress,
|
||||
LIS3MDL_AD_CTRL_REG2,
|
||||
devp->config->fullscale);
|
||||
lis3mdlI2CWriteRegister(devp->config->i2cp,
|
||||
devp->config->slaveaddress,
|
||||
LIS3MDL_AD_CTRL_REG3,
|
||||
devp->config->conversionmode);
|
||||
lis3mdlI2CWriteRegister(devp->config->i2cp,
|
||||
devp->config->slaveaddress,
|
||||
LIS3MDL_AD_CTRL_REG4,
|
||||
devp->config->operationmodez |
|
||||
devp->config->endianness);
|
||||
lis3mdlI2CWriteRegister(devp->config->i2cp,
|
||||
devp->config->slaveaddress,
|
||||
LIS3MDL_AD_CTRL_REG5,
|
||||
devp->config->blockdataupdate);
|
||||
|
||||
/* Control register 1 configuration block.*/
|
||||
{
|
||||
cr = devp->config->outputdatarate;
|
||||
#if LIS3MDL_USE_ADVANCED || defined(__DOXYGEN__)
|
||||
cr |= devp->config->operationmodexy;
|
||||
#else
|
||||
cr |= LIS3MDL_CTRL_REG1_OM0 | LIS3MDL_CTRL_REG1_OM1;
|
||||
#endif
|
||||
lis3mdlI2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
|
||||
LIS3MDL_AD_CTRL_REG1, cr);
|
||||
}
|
||||
|
||||
/* Control register 2 configuration block.*/
|
||||
{
|
||||
cr = devp->config->fullscale;
|
||||
lis3mdlI2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
|
||||
LIS3MDL_AD_CTRL_REG2, cr);
|
||||
}
|
||||
|
||||
/* Control register 3 configuration block.*/
|
||||
{
|
||||
cr = 0;
|
||||
#if LIS3MDL_USE_ADVANCED || defined(__DOXYGEN__)
|
||||
cr = devp->config->conversionmode;
|
||||
#endif
|
||||
lis3mdlI2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
|
||||
LIS3MDL_AD_CTRL_REG3, cr);
|
||||
}
|
||||
|
||||
/* Control register 4 configuration block.*/
|
||||
{
|
||||
cr = 0;
|
||||
#if LIS3MDL_USE_ADVANCED || defined(__DOXYGEN__)
|
||||
cr = devp->config->operationmodez | devp->config->endianness;
|
||||
#endif
|
||||
lis3mdlI2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
|
||||
LIS3MDL_AD_CTRL_REG4, cr);
|
||||
}
|
||||
|
||||
/* Control register 5 configuration block.*/
|
||||
{
|
||||
cr = 0;
|
||||
#if LIS3MDL_USE_ADVANCED || defined(__DOXYGEN__)
|
||||
cr = devp->config->blockdataupdate;
|
||||
#endif
|
||||
lis3mdlI2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
|
||||
LIS3MDL_AD_CTRL_REG5, cr);
|
||||
}
|
||||
|
||||
|
||||
#if LIS3MDL_SHARED_I2C
|
||||
i2cReleaseBus((devp)->config->i2cp);
|
||||
#endif /* LIS3MDL_SHARED_I2C */
|
||||
#endif /* LIS3MDL_USE_I2C */
|
||||
/* Storing sensitivity information according to full scale value */
|
||||
if(devp->config->fullscale == LIS3MDL_FS_4GA)
|
||||
if(devp->config->fullscale == LIS3MDL_FS_4GA) {
|
||||
devp->fullscale = LIS3MDL_4GA;
|
||||
for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)
|
||||
devp->sensitivity[i] = LIS3MDL_SENS_4GA;
|
||||
else if(devp->config->fullscale == LIS3MDL_FS_8GA)
|
||||
}
|
||||
else if(devp->config->fullscale == LIS3MDL_FS_8GA) {
|
||||
devp->fullscale = LIS3MDL_8GA;
|
||||
for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)
|
||||
devp->sensitivity[i] = LIS3MDL_SENS_8GA;
|
||||
else if(devp->config->fullscale == LIS3MDL_FS_12GA)
|
||||
}
|
||||
else if(devp->config->fullscale == LIS3MDL_FS_12GA) {
|
||||
devp->fullscale = LIS3MDL_12GA;
|
||||
for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)
|
||||
devp->sensitivity[i] = LIS3MDL_SENS_12GA;
|
||||
else if(devp->config->fullscale == LIS3MDL_FS_16GA)
|
||||
}
|
||||
else if(devp->config->fullscale == LIS3MDL_FS_16GA) {
|
||||
devp->fullscale = LIS3MDL_16GA;
|
||||
for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)
|
||||
devp->sensitivity[i] = LIS3MDL_SENS_16GA;
|
||||
}
|
||||
else
|
||||
osalDbgAssert(FALSE, "lis3mdlStart(), compass full scale issue");
|
||||
devp->state = LIS3MDL_READY;
|
||||
|
|
@ -455,7 +447,7 @@ void lis3mdlStart(LIS3MDLDriver *devp, const LIS3MDLConfig *config) {
|
|||
* @api
|
||||
*/
|
||||
void lis3mdlStop(LIS3MDLDriver *devp) {
|
||||
|
||||
uint8_t cr;
|
||||
osalDbgCheck(devp != NULL);
|
||||
|
||||
osalDbgAssert((devp->state == LIS3MDL_STOP) || (devp->state == LIS3MDL_READY),
|
||||
|
|
@ -468,10 +460,9 @@ void lis3mdlStop(LIS3MDLDriver *devp) {
|
|||
i2cStart((devp)->config->i2cp,
|
||||
(devp)->config->i2ccfg);
|
||||
#endif /* LIS3MDL_SHARED_I2C */
|
||||
lis3mdlI2CWriteRegister(devp->config->i2cp,
|
||||
devp->config->slaveaddress,
|
||||
LIS3MDL_AD_CTRL_REG3,
|
||||
LIS3MDL_MD_POWER_DOWN);
|
||||
cr = LIS3MDL_CTRL_REG3_MD0 | LIS3MDL_CTRL_REG3_MD1;
|
||||
lis3mdlI2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
|
||||
LIS3MDL_AD_CTRL_REG3, cr);
|
||||
i2cStop((devp)->config->i2cp);
|
||||
#if LIS3MDL_SHARED_I2C
|
||||
i2cReleaseBus((devp)->config->i2cp);
|
||||
|
|
|
|||
|
|
@ -24,7 +24,6 @@
|
|||
*
|
||||
* @{
|
||||
*/
|
||||
|
||||
#ifndef _LIS3MDL_H_
|
||||
#define _LIS3MDL_H_
|
||||
|
||||
|
|
@ -35,9 +34,141 @@
|
|||
/*===========================================================================*/
|
||||
|
||||
/**
|
||||
* @brief LIS3MDL number of axes.
|
||||
* @name Version identification
|
||||
* @{
|
||||
*/
|
||||
#define LIS3MDL_NUMBER_OF_AXES ((size_t) 3U)
|
||||
/**
|
||||
* @brief LIS3MDL driver version string.
|
||||
*/
|
||||
#define EX_LIS3MDL_VERSION "1.0.0"
|
||||
|
||||
/**
|
||||
* @brief LIS3MDL driver version major number.
|
||||
*/
|
||||
#define EX_LIS3MDL_MAJOR 1
|
||||
|
||||
/**
|
||||
* @brief LIS3MDL driver version minor number.
|
||||
*/
|
||||
#define EX_LIS3MDL_MINOR 0
|
||||
|
||||
/**
|
||||
* @brief LIS3MDL driver version patch number.
|
||||
*/
|
||||
#define EX_LIS3MDL_PATCH 0
|
||||
/** @} */
|
||||
|
||||
/**
|
||||
* @brief LIS3MDL characteristics.
|
||||
*
|
||||
* @{
|
||||
*/
|
||||
#define LIS3MDL_NUMBER_OF_AXES 3U
|
||||
|
||||
#define LIS3MDL_4GA 4.0f
|
||||
#define LIS3MDL_8GA 8.0f
|
||||
#define LIS3MDL_12GA 12.0f
|
||||
#define LIS3MDL_16GA 16.0f
|
||||
|
||||
#define LIS3MDL_SENS_4GA 6842.0f
|
||||
#define LIS3MDL_SENS_8GA 3421.0f
|
||||
#define LIS3MDL_SENS_12GA 2281.0f
|
||||
#define LIS3MDL_SENS_16GA 1711.0f
|
||||
/** @} */
|
||||
|
||||
/**
|
||||
* @name LIS3MDL communication interfaces related bit masks
|
||||
* @{
|
||||
*/
|
||||
#define LIS3MDL_DI_MASK 0xFF /**< Data In mask */
|
||||
#define LIS3MDL_DI(n) (1 << n) /**< Data In bit n */
|
||||
#define LIS3MDL_AD_MASK 0x3F /**< Address Data mask */
|
||||
#define LIS3MDL_AD(n) (1 << n) /**< Address Data bit n */
|
||||
#define LIS3MDL_MS (1 << 6) /**< Multiple selection */
|
||||
#define LIS3MDL_RW (1 << 7) /**< Read Write selector */
|
||||
/** @} */
|
||||
|
||||
/**
|
||||
* @name LIS3MDL register addresses
|
||||
* @{
|
||||
*/
|
||||
#define LIS3MDL_AD_WHO_AM_I 0x0F
|
||||
#define LIS3MDL_AD_CTRL_REG1 0x20
|
||||
#define LIS3MDL_AD_CTRL_REG2 0x21
|
||||
#define LIS3MDL_AD_CTRL_REG3 0x22
|
||||
#define LIS3MDL_AD_CTRL_REG4 0x23
|
||||
#define LIS3MDL_AD_CTRL_REG5 0x24
|
||||
#define LIS3MDL_AD_STATUS_REG 0x27
|
||||
#define LIS3MDL_AD_OUT_X_L 0x28
|
||||
#define LIS3MDL_AD_OUT_X_H 0x29
|
||||
#define LIS3MDL_AD_OUT_Y_L 0x2A
|
||||
#define LIS3MDL_AD_OUT_Y_H 0x2B
|
||||
#define LIS3MDL_AD_OUT_Z_L 0x2C
|
||||
#define LIS3MDL_AD_OUT_Z_H 0x2D
|
||||
#define LIS3MDL_AD_TEMP_OUT_L 0x2E
|
||||
#define LIS3MDL_AD_TEMP_OUT_H 0x2F
|
||||
#define LIS3MDL_AD_INT_CFG 0x30
|
||||
#define LIS3MDL_AD_INT_SOURCE 0x31
|
||||
#define LIS3MDL_AD_INT_THS_L 0x32
|
||||
#define LIS3MDL_AD_INT_THS_H 0x33
|
||||
/** @} */
|
||||
|
||||
/**
|
||||
* @name LIS3MDL_CTRL_REG1 register bits definitions
|
||||
* @{
|
||||
*/
|
||||
#define LIS3MDL_CTRL_REG1_MASK 0xFF /**< LIS3MDL_CTRL_REG1 mask */
|
||||
#define LIS3MDL_CTRL_REG1_ST (1 << 0) /**< Self test enable */
|
||||
#define LIS3MDL_CTRL_REG1_FAST_ODR (1 << 1) /**< Fast data rate */
|
||||
#define LIS3MDL_CTRL_REG1_DO0 (1 << 2) /**< Output data rate bit 0 */
|
||||
#define LIS3MDL_CTRL_REG1_DO1 (1 << 3) /**< Output data rate bit 1 */
|
||||
#define LIS3MDL_CTRL_REG1_DO2 (1 << 4) /**< Output data rate bit 2 */
|
||||
#define LIS3MDL_CTRL_REG1_OM0 (1 << 5) /**< X-Y mode bit 0 */
|
||||
#define LIS3MDL_CTRL_REG1_OM1 (1 << 6) /**< X-Y mode bit 1 */
|
||||
#define LIS3MDL_CTRL_REG1_TEMP_EN (1 << 7) /**< Temperature sensor */
|
||||
/** @} */
|
||||
|
||||
/**
|
||||
* @name LIS3MDL_CTRL_REG2 register bits definitions
|
||||
* @{
|
||||
*/
|
||||
#define LIS3MDL_CTRL_REG2_MASK 0x6C /**< LIS3MDL_CTRL_REG2 mask */
|
||||
#define LIS3MDL_CTRL_REG2_SOFT_RST (1 << 2) /**< Soft reset */
|
||||
#define LIS3MDL_CTRL_REG2_REBOOT (1 << 3) /**< Reboot memory */
|
||||
#define LIS3MDL_CTRL_REG2_FS_MASK 0x60 /**< Full scale mask */
|
||||
#define LIS3MDL_CTRL_REG2_FS0 (1 << 5) /**< Full scale bit 0 */
|
||||
#define LIS3MDL_CTRL_REG2_FS1 (1 << 6) /**< Full scale bit 1 */
|
||||
/** @} */
|
||||
|
||||
/**
|
||||
* @name LIS3MDL_CTRL_REG3 register bits definitions
|
||||
* @{
|
||||
*/
|
||||
#define LIS3MDL_CTRL_REG3_MASK 0x27 /**< LIS3MDL_CTRL_REG3 mask */
|
||||
#define LIS3MDL_CTRL_REG3_MD0 (1 << 0) /**< Operating mode bit 0 */
|
||||
#define LIS3MDL_CTRL_REG3_MD1 (1 << 1) /**< Operating mode bit 1 */
|
||||
#define LIS3MDL_CTRL_REG3_SIM (1 << 2) /**< SPI interface mode */
|
||||
#define LIS3MDL_CTRL_REG3_LP (1 << 5) /**< Low power */
|
||||
/** @} */
|
||||
|
||||
/**
|
||||
* @name LIS3MDL_CTRL_REG4 register bits definitions
|
||||
* @{
|
||||
*/
|
||||
#define LIS3MDL_CTRL_REG4_MASK 0x0E /**< LIS3MDL_CTRL_REG4 mask */
|
||||
#define LIS3MDL_CTRL_REG4_BLE (1 << 1) /**< Endianess */
|
||||
#define LIS3MDL_CTRL_REG4_OMZ0 (1 << 2) /**< Z mode bit 0 */
|
||||
#define LIS3MDL_CTRL_REG4_OMZ1 (1 << 3) /**< Z mode bit 1 */
|
||||
/** @} */
|
||||
|
||||
/**
|
||||
* @name LIS3MDL_CTRL_REG5 register bits definitions
|
||||
* @{
|
||||
*/
|
||||
#define LIS3MDL_CTRL_REG5_MASK 0xC0 /**< LIS3MDL_CTRL_REG5 mask */
|
||||
#define LIS3MDL_CTRL_REG5_BDU (1 << 6) /**< Block data update */
|
||||
#define LIS3MDL_CTRL_REG5_FAST_READ (1 << 7) /**< Fast read mode */
|
||||
/** @} */
|
||||
|
||||
/*===========================================================================*/
|
||||
/* Driver pre-compile time settings. */
|
||||
|
|
@ -65,6 +196,15 @@
|
|||
#define LIS3MDL_USE_I2C TRUE
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief LIS3MDL advanced configurations switch.
|
||||
* @details If set to @p TRUE more configurations are available.
|
||||
* @note The default is @p FALSE.
|
||||
*/
|
||||
#if !defined(LIS3MDL_USE_ADVANCED) || defined(__DOXYGEN__)
|
||||
#define LIS3MDL_USE_ADVANCED FALSE
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief LIS3MDL shared I2C switch.
|
||||
* @details If set to @p TRUE the device acquires I2C bus ownership
|
||||
|
|
@ -104,6 +244,7 @@
|
|||
* @name LIS3MDL data structures and types
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief LIS3MDL slave address
|
||||
*/
|
||||
|
|
@ -116,10 +257,10 @@ typedef enum {
|
|||
* @brief LIS3MDL full scale
|
||||
*/
|
||||
typedef enum {
|
||||
LIS3MDL_FS_4GA = 0x00, /**< ±4 Gauss */
|
||||
LIS3MDL_FS_8GA = 0x02, /**< ±8 Gauss */
|
||||
LIS3MDL_FS_12GA = 0x04, /**< ±12 Gauss */
|
||||
LIS3MDL_FS_16GA = 0x0C /**< ±16 Gauss */
|
||||
LIS3MDL_FS_4GA = 0x00, /**< ±4 Gauss */
|
||||
LIS3MDL_FS_8GA = 0x20, /**< ±8 Gauss */
|
||||
LIS3MDL_FS_12GA = 0x40, /**< ±12 Gauss */
|
||||
LIS3MDL_FS_16GA = 0x60 /**< ±16 Gauss */
|
||||
}lis3mdl_fs_t;
|
||||
|
||||
/**
|
||||
|
|
@ -210,7 +351,6 @@ typedef enum {
|
|||
* @brief LIS3MDL configuration structure.
|
||||
*/
|
||||
typedef struct {
|
||||
|
||||
#if (LIS3MDL_USE_SPI) || defined(__DOXYGEN__)
|
||||
/**
|
||||
* @brief SPI driver associated to this LIS3MDL.
|
||||
|
|
@ -231,6 +371,14 @@ typedef struct {
|
|||
*/
|
||||
const I2CConfig *i2ccfg;
|
||||
#endif /* LIS3MDL_USE_I2C */
|
||||
/**
|
||||
* @brief LIS3MDL initial sensitivity.
|
||||
*/
|
||||
float sensitivity[LIS3MDL_NUMBER_OF_AXES];
|
||||
/**
|
||||
* @brief LIS3MDL initial bias.
|
||||
*/
|
||||
float bias[LIS3MDL_NUMBER_OF_AXES];
|
||||
/**
|
||||
* @brief LIS3MDL slave address
|
||||
*/
|
||||
|
|
@ -243,6 +391,7 @@ typedef struct {
|
|||
* @brief LIS3MDL output data rate
|
||||
*/
|
||||
lis3mdl_odr_t outputdatarate;
|
||||
#if LIS3MDL_USE_ADVANCED || defined(__DOXYGEN__)
|
||||
/**
|
||||
* @brief LIS3MDL low power mode configuration
|
||||
*/
|
||||
|
|
@ -259,10 +408,6 @@ typedef struct {
|
|||
* @brief LIS3MDL operation mode for Z axis
|
||||
*/
|
||||
lis3mdl_omz_t operationmodez;
|
||||
/**
|
||||
* @brief LIS3MDL temperature sensor enabling
|
||||
*/
|
||||
lis3mdl_temp_t temperature;
|
||||
/**
|
||||
* @brief LIS3MDL block data update
|
||||
*/
|
||||
|
|
@ -271,6 +416,7 @@ typedef struct {
|
|||
* @brief LIS3MDL endianness
|
||||
*/
|
||||
lis3mdl_end_t endianness;
|
||||
#endif
|
||||
} LIS3MDLConfig;
|
||||
|
||||
/**
|
||||
|
|
@ -283,9 +429,8 @@ typedef struct LIS3MDLDriver LIS3MDLDriver;
|
|||
*/
|
||||
#define _lis3mdl_methods \
|
||||
_base_compass_methods \
|
||||
/* Retrieve the temperature of LIS3MDL chip.*/ \
|
||||
msg_t (*get_temperature)(void *instance, float* temperature);
|
||||
|
||||
/* Change full scale value of LIS3MDL.*/ \
|
||||
msg_t (*set_full_scale)(void *instance, lis3mdl_fs_t fs);
|
||||
|
||||
/**
|
||||
* @extends BaseCompassVMT
|
||||
|
|
@ -308,7 +453,9 @@ struct LIS3MDLVMT {
|
|||
/* Current sensitivity.*/ \
|
||||
float sensitivity[LIS3MDL_NUMBER_OF_AXES]; \
|
||||
/* Bias data.*/ \
|
||||
int32_t bias[LIS3MDL_NUMBER_OF_AXES];
|
||||
int32_t bias[LIS3MDL_NUMBER_OF_AXES]; \
|
||||
/* Current full scale value.*/ \
|
||||
float fullscale;
|
||||
|
||||
/**
|
||||
* @extends BaseCompass
|
||||
|
|
@ -333,20 +480,18 @@ struct LIS3MDLDriver {
|
|||
/*===========================================================================*/
|
||||
|
||||
/**
|
||||
* @brief Get current MEMS temperature.
|
||||
* @detail This information is very useful especially for high accuracy IMU.
|
||||
* @note Temperature sensor must be enabled using a proper configuration.
|
||||
* @brief Change compass full scale value.
|
||||
*
|
||||
* @param[in] ip pointer to a @p BaseCompass class.
|
||||
* @param[out] temp the MEMS temperature as single precision floating.
|
||||
* @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 compassGetTemp(ip, tpp) \
|
||||
(ip)->vmt_lis3mdl->get_temperature(ip, tpp)
|
||||
#define compassSetFullScale(ip, fs) \
|
||||
(ip)->vmt_lis3mdl->set_full_scale(ip, fs)
|
||||
|
||||
/*===========================================================================*/
|
||||
/* External declarations. */
|
||||
|
|
|
|||
|
|
@ -100,6 +100,7 @@ include $(CHIBIOS)/os/common/ports/ARMCMx/compilers/GCC/mk/port_v7m.mk
|
|||
# Other files (optional).
|
||||
include $(CHIBIOS)/os/ex/ST/lis3mdl.mk
|
||||
include $(CHIBIOS)/os/hal/lib/streams/streams.mk
|
||||
include $(CHIBIOS)/os/various/shell/shell.mk
|
||||
|
||||
# Define linker script file here
|
||||
LDSCRIPT= $(STARTUPLD)/STM32L476xG.ld
|
||||
|
|
@ -115,6 +116,7 @@ CSRC = $(STARTUPSRC) \
|
|||
$(BOARDSRC) \
|
||||
$(LIS3MDLSRC) \
|
||||
$(STREAMSSRC) \
|
||||
$(SHELLSRC) \
|
||||
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) $(LIS3MDLINC) \
|
||||
$(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 \
|
||||
-DLIS3MDL_USE_ADVANCED=0
|
||||
|
||||
# Define ASM defines here
|
||||
UADEFS =
|
||||
|
|
|
|||
|
|
@ -33,7 +33,7 @@
|
|||
<intAttribute key="org.eclipse.cdt.launch.ATTR_BUILD_BEFORE_LAUNCH_ATTR" value="2"/>
|
||||
<stringAttribute key="org.eclipse.cdt.launch.COREFILE_PATH" value=""/>
|
||||
<stringAttribute key="org.eclipse.cdt.launch.DEBUGGER_REGISTER_GROUPS" value=""/>
|
||||
<stringAttribute key="org.eclipse.cdt.launch.FORMAT" value="<?xml version="1.0" encoding="UTF-8" standalone="no"?><contentList><content id="null-lis3mdlI2CReadRegister-(format)" val="4"/><content id="null-read_raw-(format)" val="1"/><content id="null-main-(format)" val="2"/><content id="null-lsm6ds0I2CReadRegister-(format)" val="0"/><content id="null-sens_get_temperature-(format)" val="2"/></contentList>"/>
|
||||
<stringAttribute key="org.eclipse.cdt.launch.FORMAT" value="<?xml version="1.0" encoding="UTF-8" standalone="no"?><contentList><content id="null-sens_get_temperature-(format)" val="2"/><content id="null-lsm6ds0I2CReadRegister-(format)" val="0"/><content id="null-main-(format)" val="2"/><content id="null-read_raw-(format)" val="1"/><content id="null-lis3mdlI2CReadRegister-(format)" val="4"/><content id="null-lis3mdlI2CWriteRegister-(format)" val="0"/><content id="*txbuf-null-lis3mdlI2CWriteRegister-(format)" val="2"/></contentList>"/>
|
||||
<stringAttribute key="org.eclipse.cdt.launch.GLOBAL_VARIABLES" value="<?xml version="1.0" encoding="UTF-8" standalone="no"?> <globalVariableList/> "/>
|
||||
<stringAttribute key="org.eclipse.cdt.launch.MEMORY_BLOCKS" value="<?xml version="1.0" encoding="UTF-8" standalone="no"?> <memoryBlockExpressionList/> "/>
|
||||
<stringAttribute key="org.eclipse.cdt.launch.PROGRAM_NAME" value="./build/ch.elf"/>
|
||||
|
|
|
|||
|
|
@ -13,50 +13,160 @@
|
|||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#include "ch.h"
|
||||
#include "hal.h"
|
||||
|
||||
#include "string.h"
|
||||
#include "shell.h"
|
||||
#include "chprintf.h"
|
||||
|
||||
#include "lis3mdl.h"
|
||||
|
||||
/* Enable use of special ANSI escape sequences */
|
||||
#define CHPRINTF_USE_ANSI_CODE TRUE
|
||||
/*===========================================================================*/
|
||||
/* LIS3MDL related. */
|
||||
/*===========================================================================*/
|
||||
|
||||
static BaseSequentialStream * chp = (BaseSequentialStream*) &SD2;
|
||||
|
||||
/* LIS3MDL Driver: This object represent an LIS3MDL instance */
|
||||
/* LIS3MDL Driver: This object represent an LIS3MDL instance.*/
|
||||
static LIS3MDLDriver LIS3MDLD1;
|
||||
|
||||
static int32_t rawdata[LIS3MDL_NUMBER_OF_AXES];
|
||||
static float cookeddata[LIS3MDL_NUMBER_OF_AXES];
|
||||
|
||||
static char axesID[LIS3MDL_NUMBER_OF_AXES] = {'X', 'Y', 'Z'};
|
||||
static char axisID[LIS3MDL_NUMBER_OF_AXES] = {'X', 'Y', 'Z'};
|
||||
static uint32_t i;
|
||||
|
||||
static const I2CConfig i2ccfg = {
|
||||
STM32_TIMINGR_PRESC(15U) |
|
||||
STM32_TIMINGR_SCLDEL(4U) | STM32_TIMINGR_SDADEL(2U) |
|
||||
STM32_TIMINGR_SCLH(15U) | STM32_TIMINGR_SCLL(21U),
|
||||
0,
|
||||
0
|
||||
STM32_TIMINGR_PRESC(15U) |
|
||||
STM32_TIMINGR_SCLDEL(4U) | STM32_TIMINGR_SDADEL(2U) |
|
||||
STM32_TIMINGR_SCLH(15U) | STM32_TIMINGR_SCLL(21U),
|
||||
0,
|
||||
0
|
||||
};
|
||||
|
||||
static const LIS3MDLConfig lis3mdlcfg = {
|
||||
&I2CD1,
|
||||
&i2ccfg,
|
||||
LIS3MDL_SAD_VCC,
|
||||
LIS3MDL_FS_4GA,
|
||||
LIS3MDL_ODR_40HZ,
|
||||
LIS3MDL_LP_DISABLED,
|
||||
LIS3MDL_MD_CONTINUOUS,
|
||||
LIS3MDL_OMXY_ULTRA,
|
||||
LIS3MDL_OMZ_ULTRA,
|
||||
LIS3MDL_TEMP_ENABLED,
|
||||
LIS3MDL_BDU_BLOCKED,
|
||||
LIS3MDL_END_LITTLE
|
||||
static LIS3MDLConfig LIS3MDLcfg = {
|
||||
&I2CD1, /* Pointer to I2C Driver.*/
|
||||
&i2ccfg, /* Pointer to I2C Configuration.*/
|
||||
{0, 0, 0}, /* Use default sensitivity.*/
|
||||
{0, 0, 0}, /* Use default bias.*/
|
||||
LIS3MDL_SAD_VCC, /* Slave address.*/
|
||||
LIS3MDL_FS_4GA, /* Full scale value.*/
|
||||
LIS3MDL_ODR_40HZ, /* Output data rate.*/
|
||||
#if LIS3MDL_USE_ADVANCED || defined(__DOXYGEN__)
|
||||
LIS3MDL_LP_T,
|
||||
LIS3MDL_MD_T,
|
||||
LIS3MDL_OMXY_T,
|
||||
LIS3MDL_OMZ_T,
|
||||
LIS3MDL_BDU_T,
|
||||
LIS3MDL_END_T
|
||||
#endif
|
||||
};
|
||||
|
||||
/*===========================================================================*/
|
||||
/* 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 != 1) {
|
||||
chprintf(chp, "Usage: read [raw|cooked]\r\n");
|
||||
return;
|
||||
}
|
||||
|
||||
while (chnGetTimeout((BaseChannel *)chp, 150) == Q_TIMEOUT) {
|
||||
if (!strcmp (argv[0], "raw")) {
|
||||
#if CHPRINTF_USE_ANSI_CODE
|
||||
chprintf(chp, "\033[2J\033[1;1H");
|
||||
#endif
|
||||
compassReadRaw(&LIS3MDLD1, rawdata);
|
||||
chprintf(chp, "LIS3MDL compass raw data...\r\n");
|
||||
for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++) {
|
||||
chprintf(chp, "%c-axis: %d\r\n", axisID[i], rawdata[i]);
|
||||
}
|
||||
}
|
||||
else if (!strcmp (argv[0], "cooked")) {
|
||||
#if CHPRINTF_USE_ANSI_CODE
|
||||
chprintf(chp, "\033[2J\033[1;1H");
|
||||
#endif
|
||||
compassReadCooked(&LIS3MDLD1, cookeddata);
|
||||
chprintf(chp, "LIS3MDL compass cooked data...\r\n");
|
||||
for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++) {
|
||||
chprintf(chp, "%c-axis: %.3f Gauss\r\n", axisID[i], cookeddata[i]);
|
||||
}
|
||||
}
|
||||
else {
|
||||
chprintf(chp, "Usage: read [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 [4|8|12|16]\r\n");
|
||||
return;
|
||||
}
|
||||
#if CHPRINTF_USE_ANSI_CODE
|
||||
chprintf(chp, "\033[2J\033[1;1H");
|
||||
#endif
|
||||
if(!strcmp (argv[0], "4")) {
|
||||
compassSetFullScale(&LIS3MDLD1, LIS3MDL_FS_4GA);
|
||||
chprintf(chp, "LIS3MDL compass full scale set to 4 Gauss...\r\n");
|
||||
}
|
||||
else if(!strcmp (argv[0], "8")) {
|
||||
compassSetFullScale(&LIS3MDLD1, LIS3MDL_FS_8GA);
|
||||
chprintf(chp, "LIS3MDL compass full scale set to 8 Gauss...\r\n");
|
||||
}
|
||||
else if(!strcmp (argv[0], "12")) {
|
||||
compassSetFullScale(&LIS3MDLD1, LIS3MDL_FS_12GA);
|
||||
chprintf(chp, "LIS3MDL compass full scale set to 12 Gauss...\r\n");
|
||||
}
|
||||
else if(!strcmp (argv[0], "16")) {
|
||||
compassSetFullScale(&LIS3MDLD1, LIS3MDL_FS_16GA);
|
||||
chprintf(chp, "LIS3MDL compass full scale set to 16 Gauss...\r\n");
|
||||
}
|
||||
else {
|
||||
chprintf(chp, "Usage: fullscale [4|8|12|16]\r\n");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
static const ShellCommand commands[] = {
|
||||
{"read", cmd_read},
|
||||
{"fullscale", cmd_fullscale},
|
||||
{NULL, NULL}
|
||||
};
|
||||
|
||||
static const ShellConfig shell_cfg1 = {
|
||||
(BaseSequentialStream *)&SD2,
|
||||
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) {
|
||||
palClearLine(LINE_LED_GREEN);
|
||||
chThdSleepMilliseconds(250);
|
||||
palSetLine(LINE_LED_GREEN);
|
||||
chThdSleepMilliseconds(250);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Application entry point.
|
||||
*/
|
||||
|
|
@ -80,27 +190,25 @@ int main(void) {
|
|||
*/
|
||||
sdStart(&SD2, NULL);
|
||||
|
||||
/*
|
||||
* LIS3MDL Object Initialization
|
||||
*/
|
||||
|
||||
/* Creates the blinker thread.*/
|
||||
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO + 1, Thread1, NULL);
|
||||
|
||||
/* LIS3MDL Object Initialization.*/
|
||||
lis3mdlObjectInit(&LIS3MDLD1);
|
||||
|
||||
lis3mdlStart(&LIS3MDLD1, &lis3mdlcfg);
|
||||
/* Activates the LIS3MDL driver.*/
|
||||
lis3mdlStart(&LIS3MDLD1, &LIS3MDLcfg);
|
||||
|
||||
while (TRUE) {
|
||||
palToggleLine(LINE_LED_GREEN);
|
||||
compassReadRaw(&LIS3MDLD1, rawdata);
|
||||
compassReadCooked(&LIS3MDLD1, cookeddata);
|
||||
#if CHPRINTF_USE_ANSI_CODE
|
||||
chprintf(chp, "\033[2J\033[1;1H");
|
||||
#endif
|
||||
chprintf(chp, "COMPASS DATA\r\n");
|
||||
for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)
|
||||
chprintf(chp, "RAW-%c:%d\t\t", axesID[i], rawdata[i]);
|
||||
chprintf(chp, "\r\n");
|
||||
for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)
|
||||
chprintf(chp, "COOKED-%c:%.3f Gauss\t", axesID[i], cookeddata[i]);
|
||||
chprintf(chp, "\r\n");
|
||||
chThdSleepMilliseconds(100);
|
||||
/* Shell manager initialization.*/
|
||||
shellInit();
|
||||
|
||||
while(TRUE) {
|
||||
thread_t *shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
|
||||
"shell", NORMALPRIO + 1,
|
||||
shellThread, (void *)&shell_cfg1);
|
||||
chThdWait(shelltp); /* Waiting termination. */
|
||||
}
|
||||
lis3mdlStop(&LIS3MDLD1);
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue