mirror of https://github.com/rusefi/ChibiOS.git
ChibiOS/EX: improved MEMS get_axes functions
Fixed bug related to LSM6DS0 get_temperature git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@9390 35acf78f-673a-0410-8e92-d51de3d6d3f4
This commit is contained in:
parent
5c953ae7a2
commit
4ec01316ea
|
@ -190,7 +190,7 @@ static size_t get_axes_number(void *ip) {
|
|||
}
|
||||
|
||||
static msg_t read_raw(void *ip, int32_t axes[L3GD20_NUMBER_OF_AXES]) {
|
||||
|
||||
int16_t tmp;
|
||||
osalDbgCheck((ip != NULL) && (axes != NULL));
|
||||
|
||||
osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
|
||||
|
@ -205,25 +205,25 @@ static msg_t read_raw(void *ip, int32_t axes[L3GD20_NUMBER_OF_AXES]) {
|
|||
((L3GD20Driver *)ip)->config->spicfg);
|
||||
#endif /* L3GD20_SHARED_SPI */
|
||||
if(((L3GD20Driver *)ip)->config->axesenabling & L3GD20_AE_X){
|
||||
axes[0] = (int16_t)(l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
|
||||
L3GD20_AD_OUT_X_L));
|
||||
axes[0] += (int16_t)(l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
|
||||
L3GD20_AD_OUT_X_H) << 8);
|
||||
axes[0] -= ((L3GD20Driver *)ip)->bias[0];
|
||||
tmp = l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
|
||||
L3GD20_AD_OUT_X_L);
|
||||
tmp += l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
|
||||
L3GD20_AD_OUT_X_H) << 8;
|
||||
axes[0] = (int32_t)tmp + ((L3GD20Driver *)ip)->bias[0];
|
||||
}
|
||||
if(((L3GD20Driver *)ip)->config->axesenabling & L3GD20_AE_Y){
|
||||
axes[1] = (int16_t)(l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
|
||||
L3GD20_AD_OUT_Y_L));
|
||||
axes[1] += (int16_t)(l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
|
||||
L3GD20_AD_OUT_Y_H) << 8);
|
||||
axes[1] -= ((L3GD20Driver *)ip)->bias[1];
|
||||
tmp = l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
|
||||
L3GD20_AD_OUT_Y_L);
|
||||
tmp += l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
|
||||
L3GD20_AD_OUT_Y_H) << 8;
|
||||
axes[1] = (int32_t)tmp + ((L3GD20Driver *)ip)->bias[1];
|
||||
}
|
||||
if(((L3GD20Driver *)ip)->config->axesenabling & L3GD20_AE_Z){
|
||||
axes[2] = (int16_t)(l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
|
||||
L3GD20_AD_OUT_Z_L));
|
||||
axes[2] += (int16_t)(l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
|
||||
L3GD20_AD_OUT_Z_H) << 8);
|
||||
axes[2] -= ((L3GD20Driver *)ip)->bias[2];
|
||||
tmp = l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
|
||||
L3GD20_AD_OUT_Z_L);
|
||||
tmp += l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
|
||||
L3GD20_AD_OUT_Z_H) << 8;
|
||||
axes[2] = (int32_t)tmp + ((L3GD20Driver *)ip)->bias[2];
|
||||
}
|
||||
#if L3GD20_SHARED_SPI
|
||||
spiReleaseBus(((L3GD20Driver *)ip)->config->spip);
|
||||
|
|
|
@ -189,7 +189,7 @@ static size_t get_axes_number(void *ip) {
|
|||
}
|
||||
|
||||
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");
|
||||
|
@ -203,29 +203,29 @@ static msg_t read_raw(void *ip, int32_t axes[LIS3MDL_NUMBER_OF_AXES]) {
|
|||
((LIS3MDLDriver *)ip)->config->i2ccfg);
|
||||
#endif /* LIS3MDL_SHARED_I2C */
|
||||
|
||||
axes[0] = (int16_t)(lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_OUT_X_L, NULL));
|
||||
axes[0] += (int16_t)(lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_OUT_X_H, NULL) << 8);
|
||||
axes[0] -= ((LIS3MDLDriver *)ip)->bias[0];
|
||||
tmp = lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_OUT_X_L, NULL);
|
||||
tmp += lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_OUT_X_H, NULL) << 8;
|
||||
axes[0] = (int32_t)tmp - ((LIS3MDLDriver *)ip)->bias[0];
|
||||
|
||||
axes[1] = (int16_t)(lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_OUT_Y_L, NULL));
|
||||
axes[1] += (int16_t)(lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_OUT_Y_H, NULL) << 8);
|
||||
axes[1] -= ((LIS3MDLDriver *)ip)->bias[1];
|
||||
tmp = lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_OUT_Y_L, NULL);
|
||||
tmp += lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_OUT_Y_H, NULL) << 8;
|
||||
axes[1] = (int32_t)tmp - ((LIS3MDLDriver *)ip)->bias[1];
|
||||
|
||||
axes[2] = (int16_t)(lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_OUT_Z_L, NULL));
|
||||
axes[2] += (int16_t)(lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_OUT_Z_H, NULL) << 8);
|
||||
axes[2] -= ((LIS3MDLDriver *)ip)->bias[2];
|
||||
tmp = lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_OUT_Z_L, NULL);
|
||||
tmp += lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
|
||||
((LIS3MDLDriver *)ip)->config->slaveaddress,
|
||||
LIS3MDL_AD_OUT_Z_H, NULL) << 8;
|
||||
axes[2] = (int32_t)tmp - ((LIS3MDLDriver *)ip)->bias[2];
|
||||
|
||||
#if LIS3MDL_SHARED_I2C
|
||||
i2cReleaseBus(((LIS3MDLDriver *)ip)->config->i2cp);
|
||||
|
|
|
@ -44,7 +44,8 @@
|
|||
#define LSM6DS0_GYRO_SENS_500DPS ((float)0.01750f)
|
||||
#define LSM6DS0_GYRO_SENS_2000DPS ((float)0.07000f)
|
||||
|
||||
#define LSM6DS0_TEMP_SENS ((float)0.0625f)
|
||||
#define LSM6DS0_TEMP_SENS ((float)16.0f)
|
||||
#define LSM6DS0_TEMP_SENS_OFF ((float)25.0f)
|
||||
|
||||
#define LSM6DS0_DI ((uint8_t)0xFF)
|
||||
#define LSM6DS0_DI_0 ((uint8_t)0x01)
|
||||
|
@ -289,7 +290,7 @@ static size_t sens_get_axes_number(void *ip) {
|
|||
}
|
||||
|
||||
static msg_t acc_read_raw(void *ip, int32_t axes[]) {
|
||||
|
||||
int16_t tmp;
|
||||
osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
|
||||
(((LSM6DS0Driver *)ip)->config->acccfg != NULL));
|
||||
osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
|
||||
|
@ -304,31 +305,31 @@ static msg_t acc_read_raw(void *ip, int32_t axes[]) {
|
|||
((LSM6DS0Driver *)ip)->config->i2ccfg);
|
||||
#endif /* LSM6DS0_SHARED_I2C */
|
||||
if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_ACC_AE_X){
|
||||
axes[0] = (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_X_L_XL, NULL));
|
||||
axes[0] += (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_X_H_XL, NULL) << 8);
|
||||
axes[0] -= ((LSM6DS0Driver *)ip)->accbias[0];
|
||||
tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_X_L_XL, NULL);
|
||||
tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_X_H_XL, NULL) << 8;
|
||||
axes[0] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->accbias[0];
|
||||
}
|
||||
if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_ACC_AE_Y){
|
||||
axes[1] = (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_Y_L_XL, NULL));
|
||||
axes[1] += (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_Y_H_XL, NULL) << 8);
|
||||
axes[1] -= ((LSM6DS0Driver *)ip)->accbias[1];
|
||||
tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_Y_L_XL, NULL);
|
||||
tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_Y_H_XL, NULL) << 8;
|
||||
axes[1] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->accbias[1];
|
||||
}
|
||||
if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_ACC_AE_Z){
|
||||
axes[2] = (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_Z_L_XL, NULL));
|
||||
axes[2] += (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_Z_H_XL, NULL) << 8);
|
||||
axes[2] -= ((LSM6DS0Driver *)ip)->accbias[2];
|
||||
tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_Z_L_XL, NULL);
|
||||
tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_Z_H_XL, NULL) << 8;
|
||||
axes[2] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->accbias[2];
|
||||
}
|
||||
#if LSM6DS0_SHARED_I2C
|
||||
i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
|
||||
|
@ -338,7 +339,7 @@ static msg_t acc_read_raw(void *ip, int32_t axes[]) {
|
|||
}
|
||||
|
||||
static msg_t gyro_read_raw(void *ip, int32_t axes[]) {
|
||||
|
||||
int16_t tmp;
|
||||
osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
|
||||
(((LSM6DS0Driver *)ip)->config->gyrocfg != NULL));
|
||||
osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
|
||||
|
@ -353,31 +354,31 @@ static msg_t gyro_read_raw(void *ip, int32_t axes[]) {
|
|||
((LSM6DS0Driver *)ip)->config->i2ccfg);
|
||||
#endif /* LSM6DS0_SHARED_I2C */
|
||||
if(((LSM6DS0Driver *)ip)->config->gyrocfg->axesenabling & LSM6DS0_GYRO_AE_X){
|
||||
axes[0] = (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_X_L_G, NULL));
|
||||
axes[0] += (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_X_H_G, NULL) << 8);
|
||||
axes[0] -= ((LSM6DS0Driver *)ip)->gyrobias[0];
|
||||
tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_X_L_G, NULL);
|
||||
tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_X_H_G, NULL) << 8;
|
||||
axes[0] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->gyrobias[0];
|
||||
}
|
||||
if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_GYRO_AE_Y){
|
||||
axes[1] = (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_Y_L_G, NULL));
|
||||
axes[1] += (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_Y_H_G, NULL) << 8);
|
||||
axes[1] -= ((LSM6DS0Driver *)ip)->gyrobias[1];
|
||||
tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_Y_L_G, NULL);
|
||||
tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_Y_H_G, NULL) << 8;
|
||||
axes[1] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->gyrobias[1];
|
||||
}
|
||||
if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_GYRO_AE_Z){
|
||||
axes[2] = (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_Z_L_G, NULL));
|
||||
axes[2] += (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_Z_H_G, NULL) << 8);
|
||||
axes[2] -= ((LSM6DS0Driver *)ip)->gyrobias[2];
|
||||
tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_Z_L_G, NULL);
|
||||
tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_Z_H_G, NULL) << 8;
|
||||
axes[2] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->gyrobias[2];
|
||||
}
|
||||
#if LSM6DS0_SHARED_I2C
|
||||
i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
|
||||
|
@ -624,17 +625,17 @@ static msg_t sens_get_temperature(void *ip, float* tempp) {
|
|||
i2cStart(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->i2ccfg);
|
||||
#endif /* LSM6DS0_SHARED_I2C */
|
||||
temp = (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_TEMP_L, NULL));
|
||||
temp += (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_TEMP_H, NULL) << 8);
|
||||
temp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_TEMP_L, NULL);
|
||||
temp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
|
||||
((LSM6DS0Driver *)ip)->config->slaveaddress,
|
||||
LSM6DS0_AD_OUT_TEMP_H, NULL) << 8;
|
||||
#if LSM6DS0_SHARED_I2C
|
||||
i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
|
||||
#endif /* LSM6DS0_SHARED_I2C */
|
||||
#endif /* LSM6DS0_USE_I2C */
|
||||
*tempp = (float)temp * LSM6DS0_TEMP_SENS;
|
||||
*tempp = ((float)temp / LSM6DS0_TEMP_SENS) + LSM6DS0_TEMP_SENS_OFF;
|
||||
return MSG_OK;
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue