mirror of https://github.com/rusefi/ChibiOS.git
Updated L3GD20
git-svn-id: https://svn.code.sf.net/p/chibios/svn2/trunk@11643 110e8d01-0319-4d1e-a829-52ad28d1bb01
This commit is contained in:
parent
d4ff2bd45d
commit
3f7504385e
|
@ -1,5 +1,5 @@
|
|||
/*
|
||||
ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi
|
||||
ChibiOS - Copyright (C) 2016-2018 Rocco Marco Guglielmi
|
||||
|
||||
This file is part of ChibiOS.
|
||||
|
||||
|
@ -86,165 +86,310 @@ static void l3gd20SPIWriteRegister(SPIDriver *spip, uint8_t reg, size_t n,
|
|||
}
|
||||
#endif /* L3GD20_USE_SPI */
|
||||
|
||||
/*
|
||||
* Interface implementation.
|
||||
/**
|
||||
* @brief Return the number of axes of the BaseGyroscope.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseGyroscope interface.
|
||||
*
|
||||
* @return the number of axes.
|
||||
*/
|
||||
static size_t get_axes_number(void *ip) {
|
||||
static size_t gyro_get_axes_number(void *ip) {
|
||||
(void)ip;
|
||||
|
||||
osalDbgCheck(ip != NULL);
|
||||
return L3GD20_NUMBER_OF_AXES;
|
||||
return L3GD20_GYRO_NUMBER_OF_AXES;
|
||||
}
|
||||
|
||||
static msg_t read_raw(void *ip, int32_t axes[L3GD20_NUMBER_OF_AXES]) {
|
||||
/**
|
||||
* @brief Retrieves raw data from the BaseGyroscope.
|
||||
* @note This data is retrieved from MEMS register without any algebraical
|
||||
* manipulation.
|
||||
* @note The axes array must be at least the same size of the
|
||||
* BaseGyroscope axes number.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseGyroscope interface.
|
||||
* @param[out] axes a buffer which would be filled with raw data.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
*/
|
||||
static msg_t gyro_read_raw(void *ip, int32_t axes[L3GD20_GYRO_NUMBER_OF_AXES]) {
|
||||
L3GD20Driver* devp;
|
||||
int16_t tmp;
|
||||
uint8_t i, buff [2 * L3GD20_NUMBER_OF_AXES];
|
||||
uint8_t i, buff [2 * L3GD20_GYRO_NUMBER_OF_AXES];
|
||||
msg_t msg = MSG_OK;
|
||||
|
||||
osalDbgCheck((ip != NULL) && (axes != NULL));
|
||||
osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
|
||||
"read_raw(), invalid state");
|
||||
|
||||
/* Getting parent instance pointer.*/
|
||||
devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);
|
||||
|
||||
osalDbgAssert((devp->state == L3GD20_READY),
|
||||
"gyro_read_raw(), invalid state");
|
||||
#if L3GD20_USE_SPI
|
||||
osalDbgAssert((((L3GD20Driver *)ip)->config->spip->state == SPI_READY),
|
||||
"read_raw(), channel not ready");
|
||||
osalDbgAssert((devp->config->spip->state == SPI_READY),
|
||||
"gyro_read_raw(), channel not ready");
|
||||
|
||||
#if L3GD20_SHARED_SPI
|
||||
spiAcquireBus(((L3GD20Driver *)ip)->config->spip);
|
||||
spiStart(((L3GD20Driver *)ip)->config->spip,
|
||||
((L3GD20Driver *)ip)->config->spicfg);
|
||||
spiAcquireBus(devp->config->spip);
|
||||
spiStart(devp->config->spip,
|
||||
devp->config->spicfg);
|
||||
#endif /* L3GD20_SHARED_SPI */
|
||||
|
||||
l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip, L3GD20_AD_OUT_X_L,
|
||||
L3GD20_NUMBER_OF_AXES * 2, buff);
|
||||
l3gd20SPIReadRegister(devp->config->spip, L3GD20_AD_OUT_X_L,
|
||||
L3GD20_GYRO_NUMBER_OF_AXES * 2, buff);
|
||||
|
||||
#if L3GD20_SHARED_SPI
|
||||
spiReleaseBus(((L3GD20Driver *)ip)->config->spip);
|
||||
spiReleaseBus(devp->config->spip);
|
||||
#endif /* L3GD20_SHARED_SPI */
|
||||
#endif /* L3GD20_USE_SPI */
|
||||
|
||||
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
|
||||
tmp = buff[2*i] + (buff[2*i+1] << 8);
|
||||
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
|
||||
tmp = buff[2 * i] + (buff[2 * i + 1] << 8);
|
||||
axes[i] = (int32_t)tmp;
|
||||
}
|
||||
return msg;
|
||||
}
|
||||
|
||||
static msg_t read_cooked(void *ip, float axes[]) {
|
||||
/**
|
||||
* @brief Retrieves cooked data from the BaseGyroscope.
|
||||
* @note This data is manipulated according to the formula
|
||||
* cooked = (raw * sensitivity) - bias.
|
||||
* @note Final data is expressed as DPS.
|
||||
* @note The axes array must be at least the same size of the
|
||||
* BaseGyroscope axes number.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseGyroscope interface.
|
||||
* @param[out] axes a buffer which would be filled with cooked data.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
*/
|
||||
static msg_t gyro_read_cooked(void *ip, float axes[]) {
|
||||
L3GD20Driver* devp;
|
||||
uint32_t i;
|
||||
int32_t raw[L3GD20_NUMBER_OF_AXES];
|
||||
int32_t raw[L3GD20_GYRO_NUMBER_OF_AXES];
|
||||
msg_t msg;
|
||||
|
||||
osalDbgCheck((ip != NULL) && (axes != NULL));
|
||||
|
||||
osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
|
||||
"read_cooked(), invalid state");
|
||||
/* Getting parent instance pointer.*/
|
||||
devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);
|
||||
|
||||
msg = read_raw(ip, raw);
|
||||
for(i = 0; i < L3GD20_NUMBER_OF_AXES ; i++){
|
||||
axes[i] = raw[i] * ((L3GD20Driver *)ip)->sensitivity[i];
|
||||
axes[i] -= ((L3GD20Driver *)ip)->bias[i];
|
||||
osalDbgAssert((devp->state == L3GD20_READY),
|
||||
"gyro_read_cooked(), invalid state");
|
||||
#if L3GD20_USE_SPI
|
||||
osalDbgAssert((devp->config->spip->state == SPI_READY),
|
||||
"gyro_read_cooked(), channel not ready");
|
||||
#endif
|
||||
|
||||
msg = gyro_read_raw(ip, raw);
|
||||
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++){
|
||||
axes[i] = raw[i] * devp->gyrosensitivity[i];
|
||||
axes[i] -= devp->gyrobias[i];
|
||||
}
|
||||
return msg;
|
||||
}
|
||||
|
||||
static msg_t sample_bias(void *ip) {
|
||||
/**
|
||||
* @brief Samples bias values for the BaseGyroscope.
|
||||
* @note The L3GD20 shall not be moved during the whole procedure.
|
||||
* @note After this function internal bias is automatically updated.
|
||||
* @note The behavior of this function depends on @P L3GD20_BIAS_ACQ_TIMES
|
||||
* and @p L3GD20_BIAS_SETTLING_US.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseGyroscope interface.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
*/
|
||||
static msg_t gyro_sample_bias(void *ip) {
|
||||
L3GD20Driver* devp;
|
||||
uint32_t i, j;
|
||||
int32_t raw[L3GD20_NUMBER_OF_AXES];
|
||||
int32_t buff[L3GD20_NUMBER_OF_AXES] = {0, 0, 0};
|
||||
int32_t raw[L3GD20_GYRO_NUMBER_OF_AXES];
|
||||
int32_t buff[L3GD20_GYRO_NUMBER_OF_AXES] = {0, 0, 0};
|
||||
msg_t msg;
|
||||
|
||||
osalDbgCheck(ip != NULL);
|
||||
|
||||
osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
|
||||
"sample_bias(), invalid state");
|
||||
/* Getting parent instance pointer.*/
|
||||
devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);
|
||||
|
||||
osalDbgAssert((devp->state == L3GD20_READY),
|
||||
"gyro_sample_bias(), invalid state");
|
||||
#if L3GD20_USE_SPI
|
||||
osalDbgAssert((devp->config->spip->state == SPI_READY),
|
||||
"gyro_sample_bias(), channel not ready");
|
||||
#endif
|
||||
|
||||
for(i = 0; i < L3GD20_BIAS_ACQ_TIMES; i++){
|
||||
msg = read_raw(ip, raw);
|
||||
msg = gyro_read_raw(ip, raw);
|
||||
if(msg != MSG_OK)
|
||||
return msg;
|
||||
for(j = 0; j < L3GD20_NUMBER_OF_AXES; j++){
|
||||
for(j = 0; j < L3GD20_GYRO_NUMBER_OF_AXES; j++){
|
||||
buff[j] += raw[j];
|
||||
}
|
||||
osalThreadSleepMicroseconds(L3GD20_BIAS_SETTLING_uS);
|
||||
osalThreadSleepMicroseconds(L3GD20_BIAS_SETTLING_US);
|
||||
}
|
||||
|
||||
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++){
|
||||
((L3GD20Driver *)ip)->bias[i] = (buff[i] / L3GD20_BIAS_ACQ_TIMES);
|
||||
((L3GD20Driver *)ip)->bias[i] *= ((L3GD20Driver *)ip)->sensitivity[i];
|
||||
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++){
|
||||
devp->gyrobias[i] = (buff[i] / L3GD20_BIAS_ACQ_TIMES);
|
||||
devp->gyrobias[i] *= devp->gyrosensitivity[i];
|
||||
}
|
||||
return msg;
|
||||
}
|
||||
|
||||
static msg_t set_bias(void *ip, float *bp) {
|
||||
/**
|
||||
* @brief Set bias values for the BaseGyroscope.
|
||||
* @note Bias must be expressed as DPS.
|
||||
* @note The bias buffer must be at least the same size of the BaseGyroscope
|
||||
* axes number.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseGyroscope interface.
|
||||
* @param[in] bp a buffer which contains biases.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
*/
|
||||
static msg_t gyro_set_bias(void *ip, float *bp) {
|
||||
L3GD20Driver* devp;
|
||||
uint32_t i;
|
||||
|
||||
osalDbgCheck((ip != NULL) && (bp !=NULL));
|
||||
osalDbgCheck((ip != NULL) && (bp != NULL));
|
||||
|
||||
osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY) ||
|
||||
(((L3GD20Driver *)ip)->state == L3GD20_STOP),
|
||||
"set_bias(), invalid state");
|
||||
/* Getting parent instance pointer.*/
|
||||
devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);
|
||||
|
||||
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
|
||||
((L3GD20Driver *)ip)->bias[i] = bp[i];
|
||||
osalDbgAssert((devp->state != L3GD20_UNINIT),
|
||||
"gyro_set_bias(), invalid state");
|
||||
|
||||
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
|
||||
devp->gyrobias[i] = bp[i];
|
||||
}
|
||||
return MSG_OK;
|
||||
}
|
||||
|
||||
static msg_t reset_bias(void *ip) {
|
||||
/**
|
||||
* @brief Reset bias values for the BaseGyroscope.
|
||||
* @note Default biases value are obtained from device datasheet when
|
||||
* available otherwise they are considered zero.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseGyroscope interface.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
*/
|
||||
static msg_t gyro_reset_bias(void *ip) {
|
||||
L3GD20Driver* devp;
|
||||
uint32_t i;
|
||||
|
||||
osalDbgCheck(ip != NULL);
|
||||
|
||||
osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY) ||
|
||||
(((L3GD20Driver *)ip)->state == L3GD20_STOP),
|
||||
"reset_bias(), invalid state");
|
||||
/* Getting parent instance pointer.*/
|
||||
devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);
|
||||
|
||||
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
|
||||
((L3GD20Driver *)ip)->bias[i] = 0;
|
||||
osalDbgAssert((devp->state != L3GD20_UNINIT),
|
||||
"gyro_reset_bias(), invalid state");
|
||||
|
||||
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++)
|
||||
devp->gyrobias[i] = 0.0;
|
||||
return MSG_OK;
|
||||
}
|
||||
|
||||
static msg_t set_sensivity(void *ip, float *sp) {
|
||||
/**
|
||||
* @brief Set sensitivity values for the BaseGyroscope.
|
||||
* @note Sensitivity must be expressed as DPS/LSB.
|
||||
* @note The sensitivity buffer must be at least the same size of the
|
||||
* BaseGyroscope axes number.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseGyroscope interface.
|
||||
* @param[in] sp a buffer which contains sensitivities.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
*/
|
||||
static msg_t gyro_set_sensivity(void *ip, float *sp) {
|
||||
L3GD20Driver* devp;
|
||||
uint32_t i;
|
||||
|
||||
osalDbgCheck((ip != NULL) && (sp !=NULL));
|
||||
|
||||
osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
|
||||
"set_sensivity(), invalid state");
|
||||
/* Getting parent instance pointer.*/
|
||||
devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);
|
||||
|
||||
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
|
||||
((L3GD20Driver *)ip)->sensitivity[i] = sp[i];
|
||||
osalDbgAssert((devp->state != L3GD20_UNINIT),
|
||||
"gyro_set_sensivity(), invalid state");
|
||||
|
||||
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
|
||||
devp->gyrosensitivity[i] = sp[i];
|
||||
}
|
||||
return MSG_OK;
|
||||
}
|
||||
|
||||
static msg_t reset_sensivity(void *ip) {
|
||||
/**
|
||||
* @brief Reset sensitivity values for the BaseGyroscope.
|
||||
* @note Default sensitivities value are obtained from device datasheet.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseGyroscope interface.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
* @retval MSG_RESET otherwise.
|
||||
*/
|
||||
static msg_t gyro_reset_sensivity(void *ip) {
|
||||
L3GD20Driver* devp;
|
||||
uint32_t i;
|
||||
msg_t msg = MSG_OK;
|
||||
|
||||
osalDbgCheck(ip != NULL);
|
||||
|
||||
osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
|
||||
"reset_sensivity(), invalid state");
|
||||
/* Getting parent instance pointer.*/
|
||||
devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);
|
||||
|
||||
if(((L3GD20Driver *)ip)->config->fullscale == L3GD20_FS_250DPS)
|
||||
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
|
||||
((L3GD20Driver *)ip)->sensitivity[i] = L3GD20_SENS_250DPS;
|
||||
else if(((L3GD20Driver *)ip)->config->fullscale == L3GD20_FS_500DPS)
|
||||
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
|
||||
((L3GD20Driver *)ip)->sensitivity[i] = L3GD20_SENS_500DPS;
|
||||
else if(((L3GD20Driver *)ip)->config->fullscale == L3GD20_FS_2000DPS)
|
||||
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
|
||||
((L3GD20Driver *)ip)->sensitivity[i] = L3GD20_SENS_2000DPS;
|
||||
osalDbgAssert((devp->state != L3GD20_UNINIT),
|
||||
"gyro_reset_sensivity(), invalid state");
|
||||
|
||||
if(devp->config->gyrofullscale == L3GD20_FS_250DPS)
|
||||
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++)
|
||||
devp->gyrosensitivity[i] = L3GD20_SENS_250DPS;
|
||||
else if(devp->config->gyrofullscale == L3GD20_FS_500DPS)
|
||||
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++)
|
||||
devp->gyrosensitivity[i] = L3GD20_SENS_500DPS;
|
||||
else if(devp->config->gyrofullscale == L3GD20_FS_2000DPS)
|
||||
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++)
|
||||
devp->gyrosensitivity[i] = L3GD20_SENS_2000DPS;
|
||||
else {
|
||||
osalDbgAssert(FALSE, "reset_sensivity(), full scale issue");
|
||||
osalDbgAssert(FALSE, "gyro_reset_sensivity(), full scale issue");
|
||||
return MSG_RESET;
|
||||
}
|
||||
return MSG_OK;
|
||||
return msg;
|
||||
}
|
||||
|
||||
static msg_t set_full_scale(void *ip, l3gd20_fs_t fs) {
|
||||
/**
|
||||
* @brief Changes the L3GD20Driver gyroscope fullscale value.
|
||||
* @note This function also rescale sensitivities and biases based on
|
||||
* previous and next fullscale value.
|
||||
* @note A recalibration is highly suggested after calling this function.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseGyroscope interface.
|
||||
* @param[in] fs new fullscale value.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
* @retval MSG_RESET otherwise.
|
||||
*/
|
||||
static msg_t gyro_set_full_scale(L3GD20Driver *devp, l3gd20_fs_t fs) {
|
||||
float newfs, scale;
|
||||
uint8_t i, cr;
|
||||
msg_t msg = MSG_OK;
|
||||
|
||||
osalDbgCheck(devp != NULL);
|
||||
|
||||
osalDbgAssert((devp->state != L3GD20_UNINIT),
|
||||
"gyro_set_full_scale(), invalid state");
|
||||
#if L3GD20_USE_SPI
|
||||
osalDbgAssert((devp->config->spip->state == SPI_READY),
|
||||
"gyro_set_full_scale(), channel not ready");
|
||||
#endif
|
||||
|
||||
if(fs == L3GD20_FS_250DPS) {
|
||||
newfs = L3GD20_250DPS;
|
||||
|
@ -259,23 +404,23 @@ static msg_t set_full_scale(void *ip, l3gd20_fs_t fs) {
|
|||
return MSG_RESET;
|
||||
}
|
||||
|
||||
if(newfs != ((L3GD20Driver *)ip)->fullscale) {
|
||||
scale = newfs / ((L3GD20Driver *)ip)->fullscale;
|
||||
((L3GD20Driver *)ip)->fullscale = newfs;
|
||||
if(newfs != devp->gyrofullscale) {
|
||||
scale = newfs / devp->gyrofullscale;
|
||||
devp->gyrofullscale = newfs;
|
||||
|
||||
#if L3GD20_USE_SPI
|
||||
#if L3GD20_SHARED_SPI
|
||||
spiAcquireBus(((L3GD20Driver *)ip)->config->spip);
|
||||
spiStart(((L3GD20Driver *)ip)->config->spip,
|
||||
((L3GD20Driver *)ip)->config->spicfg);
|
||||
spiAcquireBus(devp->config->spip);
|
||||
spiStart(devp->config->spip,
|
||||
devp->config->spicfg);
|
||||
#endif /* L3GD20_SHARED_SPI */
|
||||
|
||||
/* Updating register.*/
|
||||
l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
|
||||
l3gd20SPIReadRegister(devp->config->spip,
|
||||
L3GD20_AD_CTRL_REG4, 1, &cr);
|
||||
|
||||
#if L3GD20_SHARED_SPI
|
||||
spiReleaseBus(((L3GD20Driver *)ip)->config->spip);
|
||||
spiReleaseBus(devp->config->spip);
|
||||
#endif /* L3GD20_SHARED_SPI */
|
||||
#endif /* L3GD20_USE_SPI */
|
||||
cr &= ~(L3GD20_CTRL_REG4_FS_MASK);
|
||||
|
@ -283,36 +428,37 @@ static msg_t set_full_scale(void *ip, l3gd20_fs_t fs) {
|
|||
|
||||
#if L3GD20_USE_SPI
|
||||
#if L3GD20_SHARED_SPI
|
||||
spiAcquireBus(((L3GD20Driver *)ip)->config->spip);
|
||||
spiStart(((L3GD20Driver *)ip)->config->spip,
|
||||
((L3GD20Driver *)ip)->config->spicfg);
|
||||
spiAcquireBus(devp->config->spip);
|
||||
spiStart(devp->config->spip,
|
||||
devp->config->spicfg);
|
||||
#endif /* L3GD20_SHARED_SPI */
|
||||
|
||||
l3gd20SPIWriteRegister(((L3GD20Driver *)ip)->config->spip,
|
||||
l3gd20SPIWriteRegister(devp->config->spip,
|
||||
L3GD20_AD_CTRL_REG4, 1, &cr);
|
||||
#if L3GD20_SHARED_SPI
|
||||
spiReleaseBus(((L3GD20Driver *)ip)->config->spip);
|
||||
spiReleaseBus(devp->config->spip);
|
||||
#endif /* L3GD20_SHARED_SPI */
|
||||
#endif /* L3GD20_USE_SPI */
|
||||
|
||||
/* Scaling sensitivity and bias. Re-calibration is suggested anyway. */
|
||||
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
|
||||
((L3GD20Driver *)ip)->sensitivity[i] *= scale;
|
||||
((L3GD20Driver *)ip)->bias[i] *= scale;
|
||||
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
|
||||
devp->gyrosensitivity[i] *= scale;
|
||||
devp->gyrobias[i] *= scale;
|
||||
}
|
||||
}
|
||||
return MSG_OK;
|
||||
return msg;
|
||||
}
|
||||
|
||||
static const struct BaseSensorVMT vmt_sensor = {
|
||||
get_axes_number, read_raw, read_cooked
|
||||
static const struct L3GD20VMT vmt_device = {
|
||||
(size_t)0,
|
||||
gyro_set_full_scale
|
||||
};
|
||||
|
||||
static const struct L3GD20GyroscopeVMT vmt_gyroscope = {
|
||||
get_axes_number, read_raw, read_cooked,
|
||||
sample_bias, set_bias, reset_bias,
|
||||
set_sensivity, reset_sensivity,
|
||||
set_full_scale
|
||||
static const struct BaseGyroscopeVMT vmt_gyroscope = {
|
||||
sizeof(struct L3GD20VMT*),
|
||||
gyro_get_axes_number, gyro_read_raw, gyro_read_cooked,
|
||||
gyro_sample_bias, gyro_set_bias, gyro_reset_bias,
|
||||
gyro_set_sensivity, gyro_reset_sensivity
|
||||
};
|
||||
|
||||
/*===========================================================================*/
|
||||
|
@ -328,11 +474,13 @@ static const struct L3GD20GyroscopeVMT vmt_gyroscope = {
|
|||
*/
|
||||
void l3gd20ObjectInit(L3GD20Driver *devp) {
|
||||
uint32_t i;
|
||||
devp->vmt_sensor = &vmt_sensor;
|
||||
devp->vmt_gyroscope = &vmt_gyroscope;
|
||||
|
||||
devp->vmt = &vmt_device;
|
||||
devp->gyro_if.vmt = &vmt_gyroscope;
|
||||
|
||||
devp->config = NULL;
|
||||
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
|
||||
devp->bias[i] = 0.0f;
|
||||
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++)
|
||||
devp->gyrobias[i] = 0.0f;
|
||||
devp->state = L3GD20_STOP;
|
||||
}
|
||||
|
||||
|
@ -358,36 +506,35 @@ void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) {
|
|||
{
|
||||
cr[0] = L3GD20_CTRL_REG1_XEN | L3GD20_CTRL_REG1_YEN |
|
||||
L3GD20_CTRL_REG1_ZEN | L3GD20_CTRL_REG1_PD |
|
||||
devp->config->outputdatarate;
|
||||
devp->config->gyrooutputdatarate;
|
||||
#if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)
|
||||
cr[0] |= devp->config->bandwidth;
|
||||
cr[0] |= devp->config->gyrobandwidth;
|
||||
#endif
|
||||
}
|
||||
|
||||
/* Control register 2 configuration block.*/
|
||||
{
|
||||
#if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)
|
||||
if(devp->config->hpmode != L3GD20_HPM_BYPASSED)
|
||||
cr[1] = devp->config->hpmode | devp->config->hpconfiguration;
|
||||
if(devp->config->gyrohpmode != L3GD20_HPM_BYPASSED)
|
||||
cr[1] = devp->config->gyrohpmode | devp->config->gyrohpconfiguration;
|
||||
#endif
|
||||
}
|
||||
|
||||
/* Control register 4 configuration block.*/
|
||||
{
|
||||
cr[3] = devp->config->fullscale;
|
||||
cr[3] = devp->config->gyrofullscale;
|
||||
#if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)
|
||||
cr[3] |= devp->config->blockdataupdate |
|
||||
devp->config->endianness;
|
||||
cr[3] |= devp->config->gyroblockdataupdate |
|
||||
devp->config->gyroendianness;
|
||||
#endif
|
||||
}
|
||||
|
||||
/* Control register 5 configuration block.*/
|
||||
{
|
||||
|
||||
#if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)
|
||||
if((devp->config->hpmode != L3GD20_HPM_BYPASSED)) {
|
||||
if((devp->config->gyrohpmode != L3GD20_HPM_BYPASSED)) {
|
||||
cr[4] = L3GD20_CTRL_REG5_HPEN;
|
||||
if(devp->config->lp2mode != L3GD20_LP2M_BYPASSED) {
|
||||
if(devp->config->gyrolp2mode != L3GD20_LP2M_BYPASSED) {
|
||||
cr[4] |= L3GD20_CTRL_REG5_INT1_SEL1 |
|
||||
L3GD20_CTRL_REG5_OUT_SEL1;
|
||||
}
|
||||
|
@ -401,52 +548,52 @@ void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) {
|
|||
|
||||
#if L3GD20_USE_SPI
|
||||
#if L3GD20_SHARED_SPI
|
||||
spiAcquireBus((devp)->config->spip);
|
||||
spiAcquireBus(devp->config->spip);
|
||||
#endif /* L3GD20_SHARED_SPI */
|
||||
spiStart((devp)->config->spip,
|
||||
(devp)->config->spicfg);
|
||||
spiStart(devp->config->spip,
|
||||
devp->config->spicfg);
|
||||
|
||||
l3gd20SPIWriteRegister(devp->config->spip, L3GD20_AD_CTRL_REG1,
|
||||
5, cr);
|
||||
#if L3GD20_SHARED_SPI
|
||||
spiReleaseBus((devp)->config->spip);
|
||||
spiReleaseBus(devp->config->spip);
|
||||
#endif /* L3GD20_SHARED_SPI */
|
||||
#endif /* L3GD20_USE_SPI */
|
||||
|
||||
/* Storing sensitivity information according to full scale.*/
|
||||
if(devp->config->fullscale == L3GD20_FS_250DPS) {
|
||||
devp->fullscale = L3GD20_250DPS;
|
||||
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
|
||||
if (devp->config->sensitivity == NULL)
|
||||
devp->sensitivity[i] = L3GD20_SENS_250DPS;
|
||||
if(devp->config->gyrofullscale == L3GD20_FS_250DPS) {
|
||||
devp->gyrofullscale = L3GD20_250DPS;
|
||||
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
|
||||
if (devp->config->gyrosensitivity == NULL)
|
||||
devp->gyrosensitivity[i] = L3GD20_SENS_250DPS;
|
||||
else
|
||||
devp->sensitivity[i] = devp->config->sensitivity[i];
|
||||
devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
|
||||
}
|
||||
}
|
||||
else if(devp->config->fullscale == L3GD20_FS_500DPS) {
|
||||
devp->fullscale = L3GD20_500DPS;
|
||||
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
|
||||
if (devp->config->sensitivity == NULL)
|
||||
devp->sensitivity[i] = L3GD20_SENS_500DPS;
|
||||
else if(devp->config->gyrofullscale == L3GD20_FS_500DPS) {
|
||||
devp->gyrofullscale = L3GD20_500DPS;
|
||||
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
|
||||
if (devp->config->gyrosensitivity == NULL)
|
||||
devp->gyrosensitivity[i] = L3GD20_SENS_500DPS;
|
||||
else
|
||||
devp->sensitivity[i] = devp->config->sensitivity[i];
|
||||
devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
|
||||
}
|
||||
}
|
||||
else if(devp->config->fullscale == L3GD20_FS_2000DPS) {
|
||||
devp->fullscale = L3GD20_2000DPS;
|
||||
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
|
||||
if (devp->config->sensitivity == NULL)
|
||||
devp->sensitivity[i] = L3GD20_SENS_2000DPS;
|
||||
else if(devp->config->gyrofullscale == L3GD20_FS_2000DPS) {
|
||||
devp->gyrofullscale = L3GD20_2000DPS;
|
||||
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
|
||||
if (devp->config->gyrosensitivity == NULL)
|
||||
devp->gyrosensitivity[i] = L3GD20_SENS_2000DPS;
|
||||
else
|
||||
devp->sensitivity[i] = devp->config->sensitivity[i];
|
||||
devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
|
||||
}
|
||||
}
|
||||
else
|
||||
osalDbgAssert(FALSE, "l3gd20Start(), full scale issue");
|
||||
|
||||
if(devp->config->bias != NULL) {
|
||||
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
|
||||
devp->bias[i] = devp->config->bias[i];
|
||||
if(devp->config->gyrobias != NULL) {
|
||||
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
|
||||
devp->gyrobias[i] = devp->config->gyrobias[i];
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -476,17 +623,17 @@ void l3gd20Stop(L3GD20Driver *devp) {
|
|||
|
||||
#if L3GD20_USE_SPI
|
||||
#if L3GD20_SHARED_SPI
|
||||
spiAcquireBus((devp)->config->spip);
|
||||
spiStart((devp)->config->spip,
|
||||
(devp)->config->spicfg);
|
||||
spiAcquireBus(devp->config->spip);
|
||||
spiStart(devp->config->spip,
|
||||
devp->config->spicfg);
|
||||
#endif /* L3GD20_SHARED_SPI */
|
||||
|
||||
l3gd20SPIWriteRegister(devp->config->spip, L3GD20_AD_CTRL_REG1,
|
||||
1, &cr1);
|
||||
spiStop((devp)->config->spip);
|
||||
spiStop(devp->config->spip);
|
||||
|
||||
#if L3GD20_SHARED_SPI
|
||||
spiReleaseBus((devp)->config->spip);
|
||||
spiReleaseBus(devp->config->spip);
|
||||
#endif /* L3GD20_SHARED_SPI */
|
||||
#endif /* L3GD20_USE_SPI */
|
||||
}
|
||||
|
|
|
@ -1,5 +1,5 @@
|
|||
/*
|
||||
ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi
|
||||
ChibiOS - Copyright (C) 2016-2018 Rocco Marco Guglielmi
|
||||
|
||||
This file is part of ChibiOS.
|
||||
|
||||
|
@ -42,7 +42,7 @@
|
|||
/**
|
||||
* @brief L3GD20 driver version string.
|
||||
*/
|
||||
#define EX_L3GD20_VERSION "1.0.5"
|
||||
#define EX_L3GD20_VERSION "1.0.6"
|
||||
|
||||
/**
|
||||
* @brief L3GD20 driver version major number.
|
||||
|
@ -57,15 +57,17 @@
|
|||
/**
|
||||
* @brief L3GD20 driver version patch number.
|
||||
*/
|
||||
#define EX_L3GD20_PATCH 5
|
||||
#define EX_L3GD20_PATCH 65
|
||||
/** @} */
|
||||
|
||||
/**
|
||||
* @brief L3GD20 characteristics.
|
||||
* @brief L3GD20 gyroscope system characteristics.
|
||||
* @note Sensitivity is expressed as DPS/LSB whereas DPS stand for Degree
|
||||
* per second [°/s].
|
||||
*
|
||||
* @{
|
||||
*/
|
||||
#define L3GD20_NUMBER_OF_AXES 3U
|
||||
#define L3GD20_GYRO_NUMBER_OF_AXES 3U
|
||||
|
||||
#define L3GD20_250DPS 250.0f
|
||||
#define L3GD20_500DPS 500.0f
|
||||
|
@ -124,99 +126,99 @@
|
|||
* @name L3GD20_CTRL_REG1 register bits definitions
|
||||
* @{
|
||||
*/
|
||||
#define L3GD20_CTRL_REG1_MASK 0xFF /**< L3GD20_CTRL_REG1 mask */
|
||||
#define L3GD20_CTRL_REG1_XEN (1 << 0) /**< X axis enable */
|
||||
#define L3GD20_CTRL_REG1_YEN (1 << 1) /**< Y axis enable */
|
||||
#define L3GD20_CTRL_REG1_ZEN (1 << 2) /**< Z axis enable */
|
||||
#define L3GD20_CTRL_REG1_PD (1 << 3) /**< Power-down mode enable */
|
||||
#define L3GD20_CTRL_REG1_BW0 (1 << 4) /**< Bandwidth bit 0 */
|
||||
#define L3GD20_CTRL_REG1_BW1 (1 << 5) /**< Bandwidth bit 1 */
|
||||
#define L3GD20_CTRL_REG1_DR0 (1 << 6) /**< Output data rate bit 0 */
|
||||
#define L3GD20_CTRL_REG1_DR1 (1 << 7) /**< Output data rate bit 1 */
|
||||
#define L3GD20_CTRL_REG1_MASK 0xFF
|
||||
#define L3GD20_CTRL_REG1_XEN (1 << 0)
|
||||
#define L3GD20_CTRL_REG1_YEN (1 << 1)
|
||||
#define L3GD20_CTRL_REG1_ZEN (1 << 2)
|
||||
#define L3GD20_CTRL_REG1_PD (1 << 3)
|
||||
#define L3GD20_CTRL_REG1_BW0 (1 << 4)
|
||||
#define L3GD20_CTRL_REG1_BW1 (1 << 5)
|
||||
#define L3GD20_CTRL_REG1_DR0 (1 << 6)
|
||||
#define L3GD20_CTRL_REG1_DR1 (1 << 7)
|
||||
/** @} */
|
||||
|
||||
/**
|
||||
* @name L3GD20_CTRL_REG2 register bits definitions
|
||||
* @{
|
||||
*/
|
||||
#define L3GD20_CTRL_REG2_MASK 0x3F /**< L3GD20_CTRL_REG2 mask */
|
||||
#define L3GD20_CTRL_REG2_HPCF0 (1 << 0) /**< HP filter cutoff bit 0 */
|
||||
#define L3GD20_CTRL_REG2_HPCF1 (1 << 1) /**< HP filter cutoff bit 1 */
|
||||
#define L3GD20_CTRL_REG2_HPCF2 (1 << 2) /**< HP filter cutoff bit 2 */
|
||||
#define L3GD20_CTRL_REG2_HPCF3 (1 << 3) /**< HP filter cutoff bit 3 */
|
||||
#define L3GD20_CTRL_REG2_HPM0 (1 << 4) /**< HP filter mode bit 0 */
|
||||
#define L3GD20_CTRL_REG2_HPM1 (1 << 5) /**< HP filter mode bit 1 */
|
||||
#define L3GD20_CTRL_REG2_MASK 0x3F
|
||||
#define L3GD20_CTRL_REG2_HPCF0 (1 << 0)
|
||||
#define L3GD20_CTRL_REG2_HPCF1 (1 << 1)
|
||||
#define L3GD20_CTRL_REG2_HPCF2 (1 << 2)
|
||||
#define L3GD20_CTRL_REG2_HPCF3 (1 << 3)
|
||||
#define L3GD20_CTRL_REG2_HPM0 (1 << 4)
|
||||
#define L3GD20_CTRL_REG2_HPM1 (1 << 5)
|
||||
/** @} */
|
||||
|
||||
/**
|
||||
* @name L3GD20_CTRL_REG3 register bits definitions
|
||||
* @{
|
||||
*/
|
||||
#define L3GD20_CTRL_REG3_MASK 0xFF /**< L3GD20_CTRL_REG3 mask */
|
||||
#define L3GD20_CTRL_REG3_I2_EMPTY (1 << 0) /**< FIFO empty IRQ */
|
||||
#define L3GD20_CTRL_REG3_I2_ORUN (1 << 1) /**< FIFO overrun IRQ */
|
||||
#define L3GD20_CTRL_REG3_I2_WTM (1 << 2) /**< FIFO watermark IRQ */
|
||||
#define L3GD20_CTRL_REG3_I2_DRDY (1 << 3) /**< Data ready */
|
||||
#define L3GD20_CTRL_REG3_PP_OD (1 << 4) /**< Push-pull / Open Drain */
|
||||
#define L3GD20_CTRL_REG3_H_LACTIVE (1 << 5) /**< IRQ active */
|
||||
#define L3GD20_CTRL_REG3_I1_BOOT (1 << 6) /**< Boot status available */
|
||||
#define L3GD20_CTRL_REG3_I1_INT1 (1 << 7) /**< IRQ enable */
|
||||
#define L3GD20_CTRL_REG3_MASK 0xFF
|
||||
#define L3GD20_CTRL_REG3_I2_EMPTY (1 << 0)
|
||||
#define L3GD20_CTRL_REG3_I2_ORUN (1 << 1)
|
||||
#define L3GD20_CTRL_REG3_I2_WTM (1 << 2)
|
||||
#define L3GD20_CTRL_REG3_I2_DRDY (1 << 3)
|
||||
#define L3GD20_CTRL_REG3_PP_OD (1 << 4)
|
||||
#define L3GD20_CTRL_REG3_H_LACTIVE (1 << 5)
|
||||
#define L3GD20_CTRL_REG3_I1_BOOT (1 << 6)
|
||||
#define L3GD20_CTRL_REG3_I1_INT1 (1 << 7)
|
||||
/** @} */
|
||||
|
||||
/**
|
||||
* @name L3GD20_CTRL_REG4 register bits definitions
|
||||
* @{
|
||||
*/
|
||||
#define L3GD20_CTRL_REG4_MASK 0xF1 /**< L3GD20_CTRL_REG4 mask */
|
||||
#define L3GD20_CTRL_REG4_SIM (1 << 0) /**< SPI mode */
|
||||
#define L3GD20_CTRL_REG4_FS_MASK 0x30 /**< Full scale field mask */
|
||||
#define L3GD20_CTRL_REG4_FS0 (1 << 4) /**< Full scale bit 0 */
|
||||
#define L3GD20_CTRL_REG4_FS1 (1 << 5) /**< Full scale bit 1 */
|
||||
#define L3GD20_CTRL_REG4_BLE (1 << 6) /**< Big/little endian data */
|
||||
#define L3GD20_CTRL_REG4_BDU (1 << 7) /**< Block data update */
|
||||
#define L3GD20_CTRL_REG4_MASK 0xF1
|
||||
#define L3GD20_CTRL_REG4_SIM (1 << 0)
|
||||
#define L3GD20_CTRL_REG4_FS_MASK 0x30
|
||||
#define L3GD20_CTRL_REG4_FS0 (1 << 4)
|
||||
#define L3GD20_CTRL_REG4_FS1 (1 << 5)
|
||||
#define L3GD20_CTRL_REG4_BLE (1 << 6)
|
||||
#define L3GD20_CTRL_REG4_BDU (1 << 7)
|
||||
/** @} */
|
||||
|
||||
/**
|
||||
* @name L3GD20_CTRL_REG5 register bits definitions
|
||||
* @{
|
||||
*/
|
||||
#define L3GD20_CTRL_REG5_MASK 0xDF /**< L3GD20_CTRL_REG5 mask */
|
||||
#define L3GD20_CTRL_REG5_OUT_SEL0 (1 << 0) /**< Out selection bit 0 */
|
||||
#define L3GD20_CTRL_REG5_OUT_SEL1 (1 << 1) /**< Out selection bit 1 */
|
||||
#define L3GD20_CTRL_REG5_INT1_SEL0 (1 << 2) /**< INT1 selection bit 0 */
|
||||
#define L3GD20_CTRL_REG5_INT1_SEL1 (1 << 3) /**< INT1 selection bit 1 */
|
||||
#define L3GD20_CTRL_REG5_HPEN (1 << 4) /**< HP filter enable */
|
||||
#define L3GD20_CTRL_REG5_FIFO_EN (1 << 6) /**< FIFO enable */
|
||||
#define L3GD20_CTRL_REG5_BOOT (1 << 7) /**< Reboot memory content */
|
||||
#define L3GD20_CTRL_REG5_MASK 0xDF
|
||||
#define L3GD20_CTRL_REG5_OUT_SEL0 (1 << 0)
|
||||
#define L3GD20_CTRL_REG5_OUT_SEL1 (1 << 1)
|
||||
#define L3GD20_CTRL_REG5_INT1_SEL0 (1 << 2)
|
||||
#define L3GD20_CTRL_REG5_INT1_SEL1 (1 << 3)
|
||||
#define L3GD20_CTRL_REG5_HPEN (1 << 4)
|
||||
#define L3GD20_CTRL_REG5_FIFO_EN (1 << 6)
|
||||
#define L3GD20_CTRL_REG5_BOOT (1 << 7)
|
||||
/** @} */
|
||||
|
||||
/**
|
||||
* @name L3GD20_INT1_CFG register bits definitions
|
||||
* @{
|
||||
*/
|
||||
#define L3GD20_INT1_CFG_MASK 0xFF /**< L3GD20_INT1_CFG mask */
|
||||
#define L3GD20_INT1_CFG_XLIE (1 << 0) /**< Enable INT1 on X low */
|
||||
#define L3GD20_INT1_CFG_XHIE (1 << 1) /**< Enable INT1 on X high */
|
||||
#define L3GD20_INT1_CFG_YLIE (1 << 2) /**< Enable INT1 on Y low */
|
||||
#define L3GD20_INT1_CFG_YHIE (1 << 3) /**< Enable INT1 on Y high */
|
||||
#define L3GD20_INT1_CFG_ZLIE (1 << 4) /**< Enable INT1 on Z low */
|
||||
#define L3GD20_INT1_CFG_ZHIE (1 << 5) /**< Enable INT1 on Z high */
|
||||
#define L3GD20_INT1_CFG_LIR (1 << 6) /**< Latch INT1 */
|
||||
#define L3GD20_INT1_CFG_AND_OR (1 << 7) /**< AND OR combination */
|
||||
#define L3GD20_INT1_CFG_MASK 0xFF
|
||||
#define L3GD20_INT1_CFG_XLIE (1 << 0)
|
||||
#define L3GD20_INT1_CFG_XHIE (1 << 1)
|
||||
#define L3GD20_INT1_CFG_YLIE (1 << 2)
|
||||
#define L3GD20_INT1_CFG_YHIE (1 << 3)
|
||||
#define L3GD20_INT1_CFG_ZLIE (1 << 4)
|
||||
#define L3GD20_INT1_CFG_ZHIE (1 << 5)
|
||||
#define L3GD20_INT1_CFG_LIR (1 << 6)
|
||||
#define L3GD20_INT1_CFG_AND_OR (1 << 7)
|
||||
/** @} */
|
||||
|
||||
/**
|
||||
* @name L3GD20_INT1_SRC register bits definitions
|
||||
* @{
|
||||
*/
|
||||
#define L3GD20_INT1_SRC_MASK 0x7F /**< L3GD20_INT1_SRC mask */
|
||||
#define L3GD20_INT1_SRC_XL (1 << 0) /**< X low event */
|
||||
#define L3GD20_INT1_SRC_XH (1 << 1) /**< X high event */
|
||||
#define L3GD20_INT1_SRC_YL (1 << 2) /**< Y low event */
|
||||
#define L3GD20_INT1_SRC_YH (1 << 3) /**< Y high event */
|
||||
#define L3GD20_INT1_SRC_ZL (1 << 4) /**< Z low event */
|
||||
#define L3GD20_INT1_SRC_ZH (1 << 5) /**< Z high event */
|
||||
#define L3GD20_INT1_SRC_IA (1 << 6) /**< Interrupt active */
|
||||
#define L3GD20_INT1_SRC_MASK 0x7F
|
||||
#define L3GD20_INT1_SRC_XL (1 << 0)
|
||||
#define L3GD20_INT1_SRC_XH (1 << 1)
|
||||
#define L3GD20_INT1_SRC_YL (1 << 2)
|
||||
#define L3GD20_INT1_SRC_YH (1 << 3)
|
||||
#define L3GD20_INT1_SRC_ZL (1 << 4)
|
||||
#define L3GD20_INT1_SRC_ZH (1 << 5)
|
||||
#define L3GD20_INT1_SRC_IA (1 << 6)
|
||||
/** @} */
|
||||
|
||||
/*===========================================================================*/
|
||||
|
@ -277,8 +279,8 @@
|
|||
* @brief Settling time for bias removal
|
||||
* @details This is the time between each bias acquisition.
|
||||
*/
|
||||
#if !defined(L3GD20_BIAS_SETTLING_uS) || defined(__DOXYGEN__)
|
||||
#define L3GD20_BIAS_SETTLING_uS 5000
|
||||
#if !defined(L3GD20_BIAS_SETTLING_US) || defined(__DOXYGEN__)
|
||||
#define L3GD20_BIAS_SETTLING_US 5000
|
||||
#endif
|
||||
/** @} */
|
||||
|
||||
|
@ -294,7 +296,6 @@
|
|||
#error "L3GD20_USE_SPI requires HAL_USE_SPI"
|
||||
#endif
|
||||
|
||||
//TODO: add I2C support.
|
||||
#if L3GD20_USE_I2C
|
||||
#error "L3GD20 over I2C still not supported"
|
||||
#endif
|
||||
|
@ -315,6 +316,11 @@
|
|||
* @name L3GD20 data structures and types.
|
||||
* @{
|
||||
*/
|
||||
/**
|
||||
* @brief Structure representing a L3GD20 driver.
|
||||
*/
|
||||
typedef struct L3GD20Driver L3GD20Driver;
|
||||
|
||||
/**
|
||||
* @brief L3GD20 full scale.
|
||||
*/
|
||||
|
@ -322,7 +328,7 @@ typedef enum {
|
|||
L3GD20_FS_250DPS = 0x00, /**< Full scale 250 degree per second. */
|
||||
L3GD20_FS_500DPS = 0x10, /**< Full scale 500 degree per second. */
|
||||
L3GD20_FS_2000DPS = 0x20 /**< Full scale 2000 degree per second. */
|
||||
}l3gd20_fs_t;
|
||||
} l3gd20_fs_t;
|
||||
|
||||
/**
|
||||
* @brief L3GD20 output data rate and bandwidth.
|
||||
|
@ -332,7 +338,7 @@ typedef enum {
|
|||
L3GD20_ODR_190HZ = 0x40, /**< Output data rate 190 Hz. */
|
||||
L3GD20_ODR_380HZ = 0x80, /**< Output data rate 380 Hz. */
|
||||
L3GD20_ODR_760HZ = 0xC0 /**< Output data rate 760 Hz. */
|
||||
}l3gd20_odr_t;
|
||||
} l3gd20_odr_t;
|
||||
|
||||
/**
|
||||
* @brief L3GD20 low pass filter 1 bandwidth.
|
||||
|
@ -342,14 +348,15 @@ typedef enum {
|
|||
L3GD20_BW1 = 0x40, /**< LPF1 bandwidth. Depends on ODR. */
|
||||
L3GD20_BW2 = 0x80, /**< LPF1 bandwidth. Depends on ODR. */
|
||||
L3GD20_BW3 = 0xC0 /**< LPF1 bandwidth. Depends on ODR. */
|
||||
}l3gd20_bw_t;
|
||||
} l3gd20_bw_t;
|
||||
|
||||
/**
|
||||
* @brief L3GD20 block data update.
|
||||
*/
|
||||
typedef enum {
|
||||
L3GD20_BDU_CONTINUOUS = 0x00, /**< Block data continuously updated. */
|
||||
L3GD20_BDU_BLOCKED = 0x80 /**< Block data updated after reading. */
|
||||
}l3gd20_bdu_t;
|
||||
} l3gd20_bdu_t;
|
||||
|
||||
/**
|
||||
* @brief L3GD20 HP filter mode.
|
||||
|
@ -359,7 +366,7 @@ typedef enum {
|
|||
L3GD20_HPM_REFERENCE = 0x10, /**< Reference signal for filtering. */
|
||||
L3GD20_HPM_AUTORESET = 0x30, /**< Autoreset on interrupt event. */
|
||||
L3GD20_HPM_BYPASSED = 0xFF /**< HP filter bypassed */
|
||||
}l3gd20_hpm_t;
|
||||
} l3gd20_hpm_t;
|
||||
|
||||
/**
|
||||
* @brief L3GD20 HP configuration.
|
||||
|
@ -375,7 +382,7 @@ typedef enum {
|
|||
L3GD20_HPCF_7 = 0x07, /**< Depends on ODR (Table 26 for more).*/
|
||||
L3GD20_HPCF_8 = 0x08, /**< Depends on ODR (Table 26 for more).*/
|
||||
L3GD20_HPCF_9 = 0x09 /**< Depends on ODR (Table 26 for more).*/
|
||||
}l3gd20_hpcf_t;
|
||||
} l3gd20_hpcf_t;
|
||||
|
||||
/**
|
||||
* @brief L3GD20 LP2 filter mode.
|
||||
|
@ -384,7 +391,7 @@ typedef enum {
|
|||
typedef enum {
|
||||
L3GD20_LP2M_ON = 0x00, /**< LP2 filter activated. */
|
||||
L3GD20_LP2M_BYPASSED = 0xFF, /**< LP2 filter bypassed. */
|
||||
}l3gd20_lp2m_t;
|
||||
} l3gd20_lp2m_t;
|
||||
|
||||
/**
|
||||
* @brief L3GD20 endianness.
|
||||
|
@ -392,7 +399,7 @@ typedef enum {
|
|||
typedef enum {
|
||||
L3GD20_END_LITTLE = 0x00, /**< Little endian. */
|
||||
L3GD20_END_BIG = 0x40 /**< Big endian. */
|
||||
}l3gd20_end_t;
|
||||
} l3gd20_end_t;
|
||||
|
||||
/**
|
||||
* @brief Driver state machine possible states.
|
||||
|
@ -400,7 +407,7 @@ typedef enum {
|
|||
typedef enum {
|
||||
L3GD20_UNINIT = 0, /**< Not initialized. */
|
||||
L3GD20_STOP = 1, /**< Stopped. */
|
||||
L3GD20_READY = 2, /**< Ready. */
|
||||
L3GD20_READY = 2 /**< Ready. */
|
||||
} l3gd20_state_t;
|
||||
|
||||
/**
|
||||
|
@ -429,110 +436,101 @@ typedef struct {
|
|||
const I2CConfig *i2ccfg;
|
||||
#endif /* L3GD20_USE_I2C */
|
||||
/**
|
||||
* @brief L3GD20 initial sensitivity.
|
||||
* @brief L3GD20 gyroscope system initial sensitivity.
|
||||
*/
|
||||
float *sensitivity;
|
||||
float *gyrosensitivity;
|
||||
/**
|
||||
* @brief L3GD20 initial bias.
|
||||
* @brief L3GD20 gyroscope system initial bias.
|
||||
*/
|
||||
float *bias;
|
||||
float *gyrobias;
|
||||
/**
|
||||
* @brief L3GD20 initial full scale value.
|
||||
* @brief L3GD20 gyroscope system initial full scale value.
|
||||
*/
|
||||
l3gd20_fs_t fullscale;
|
||||
l3gd20_fs_t gyrofullscale;
|
||||
/**
|
||||
* @brief L3GD20 output data rate selection.
|
||||
* @brief L3GD20 gyroscope system output data rate selection.
|
||||
*/
|
||||
l3gd20_odr_t outputdatarate;
|
||||
l3gd20_odr_t gyrooutputdatarate;
|
||||
#if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)
|
||||
/**
|
||||
* @brief L3GD20 block data update.
|
||||
* @brief L3GD20 gyroscope system block data update.
|
||||
*/
|
||||
l3gd20_bdu_t blockdataupdate;
|
||||
l3gd20_bdu_t gyroblockdataupdate;
|
||||
/**
|
||||
* @brief L3GD20 endianness.
|
||||
* @brief L3GD20 gyroscope system endianness.
|
||||
*/
|
||||
l3gd20_end_t endianness;
|
||||
l3gd20_end_t gyroendianness;
|
||||
/**
|
||||
* @brief L3GD20 LP1 filter bandwidth.
|
||||
* @brief L3GD20 gyroscope system LP1 filter bandwidth.
|
||||
*/
|
||||
l3gd20_bw_t bandwidth;
|
||||
l3gd20_bw_t gyrobandwidth;
|
||||
/**
|
||||
* @brief L3GD20 HP filter mode.
|
||||
* @brief L3GD20 gyroscope system HP filter mode.
|
||||
*/
|
||||
l3gd20_hpm_t hpmode;
|
||||
l3gd20_hpm_t gyrohpmode;
|
||||
/**
|
||||
* @brief L3GD20 HP configuration.
|
||||
* @brief L3GD20 gyroscope system HP configuration.
|
||||
*/
|
||||
l3gd20_hpcf_t hpconfiguration;
|
||||
l3gd20_hpcf_t gyrohpconfiguration;
|
||||
/**
|
||||
* @brief L3GD20 LP2 filter mode.
|
||||
* @brief L3GD20 gyroscope system LP2 filter mode.
|
||||
* @details To activate LP2 HP should be active
|
||||
*/
|
||||
l3gd20_lp2m_t lp2mode;
|
||||
l3gd20_lp2m_t gyrolp2mode;
|
||||
#endif
|
||||
} L3GD20Config;
|
||||
|
||||
/**
|
||||
* @brief @p L3GD20 gyroscope subsystem specific methods.
|
||||
* @brief @p L3GD20 specific methods.
|
||||
*/
|
||||
#define _l3gd20_gyroscope_methods_alone \
|
||||
#define _l3gd20_methods_alone \
|
||||
/* Change full scale value of L3GD20.*/ \
|
||||
msg_t (*set_full_scale)(void *instance, l3gd20_fs_t fs);
|
||||
msg_t (*gyro_set_full_scale)(L3GD20Driver *instance, l3gd20_fs_t fs);
|
||||
|
||||
/**
|
||||
* @brief @p L3GD20 gyroscope subsystem specific methods with inherited ones.
|
||||
* @brief @p L3GD20 specific methods with inherited ones.
|
||||
*/
|
||||
#define _l3gd20_gyroscope_methods \
|
||||
_base_gyroscope_methods \
|
||||
_l3gd20_gyroscope_methods_alone
|
||||
#define _l3gd20_methods \
|
||||
_base_object_methods \
|
||||
_l3gd20_methods_alone
|
||||
|
||||
/**
|
||||
* @extends BaseGyroscopeVMT
|
||||
* @extends BaseObjectVMT
|
||||
*
|
||||
* @brief @p L3GD20 gyroscope virtual methods table.
|
||||
* @brief @p L3GD20 virtual methods table.
|
||||
*/
|
||||
struct L3GD20GyroscopeVMT {
|
||||
_l3gd20_gyroscope_methods
|
||||
struct L3GD20VMT {
|
||||
_l3gd20_methods
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief @p L3GD20Driver specific data.
|
||||
*/
|
||||
#define _l3gd20_data \
|
||||
_base_gyroscope_data \
|
||||
_base_sensor_data \
|
||||
/* Driver state.*/ \
|
||||
l3gd20_state_t state; \
|
||||
/* Current configuration data.*/ \
|
||||
const L3GD20Config *config; \
|
||||
/* Current sensitivity data.*/ \
|
||||
float sensitivity[L3GD20_NUMBER_OF_AXES]; \
|
||||
/* Current Bias data.*/ \
|
||||
float bias[L3GD20_NUMBER_OF_AXES]; \
|
||||
/* Current full scale value.*/ \
|
||||
float fullscale;
|
||||
/* Gyroscope subsystem axes number.*/ \
|
||||
size_t gyroaxes; \
|
||||
/* Gyroscope subsystem current sensitivity.*/ \
|
||||
float gyrosensitivity[L3GD20_GYRO_NUMBER_OF_AXES]; \
|
||||
/* Gyroscope subsystem current Bias.*/ \
|
||||
float gyrobias[L3GD20_GYRO_NUMBER_OF_AXES]; \
|
||||
/* Gyroscope subsystem current full scale value.*/ \
|
||||
float gyrofullscale;
|
||||
|
||||
/**
|
||||
* @extends BaseGyroscope
|
||||
*
|
||||
* @brief L3GD20 3-axis gyroscope class.
|
||||
* @details This class extends @p BaseGyroscope by adding physical
|
||||
* driver implementation.
|
||||
*/
|
||||
struct L3GD20Driver {
|
||||
/** @brief BaseSensor Virtual Methods Table. */
|
||||
const struct BaseSensorVMT *vmt_sensor;
|
||||
_base_sensor_data
|
||||
/** @brief L3GD20 Gyroscope Virtual Methods Table. */
|
||||
const struct L3GD20GyroscopeVMT *vmt_gyroscope;
|
||||
_base_gyroscope_data
|
||||
/** @brief Virtual Methods Table. */
|
||||
const struct L3GD20VMT *vmt;
|
||||
/** @brief Base gyroscope interface.*/
|
||||
BaseGyroscope gyro_if;
|
||||
_l3gd20_data
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Structure representing a L3GD20 driver.
|
||||
*/
|
||||
typedef struct L3GD20Driver L3GD20Driver;
|
||||
/** @} */
|
||||
|
||||
/*===========================================================================*/
|
||||
|
@ -540,18 +538,152 @@ typedef struct L3GD20Driver L3GD20Driver;
|
|||
/*===========================================================================*/
|
||||
|
||||
/**
|
||||
* @brief Change gyroscope full scale value.
|
||||
* @brief Return the number of axes of the BaseGyroscope.
|
||||
*
|
||||
* @param[in] ip pointer to a @p L3GD20Driver class.
|
||||
* @param[in] fs the new full scale value.
|
||||
* @param[in] devp pointer to @p L3GD20Driver.
|
||||
*
|
||||
* @return the number of axes.
|
||||
*
|
||||
* @api
|
||||
*/
|
||||
#define l3gd20GyroscopeGetAxesNumber(devp) \
|
||||
gyroscopeGetAxesNumber(&((devp)->gyro_if))
|
||||
|
||||
/**
|
||||
* @brief Retrieves raw data from the BaseGyroscope.
|
||||
* @note This data is retrieved from MEMS register without any algebraical
|
||||
* manipulation.
|
||||
* @note The axes array must be at least the same size of the
|
||||
* BaseGyroscope axes number.
|
||||
*
|
||||
* @param[in] devp pointer to @p L3GD20Driver.
|
||||
* @param[out] axes a buffer which would be filled with raw data.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
* @retval MSG_RESET if one or more errors occurred.
|
||||
*
|
||||
* @api
|
||||
*/
|
||||
#define gyroscopeSetFullScale(ip, fs) \
|
||||
(ip)->vmt_gyroscope->set_full_scale(ip, fs)
|
||||
#define l3gd20GyroscopeReadRaw(devp, axes) \
|
||||
gyroscopeReadRaw(&((devp)->gyro_if), axes)
|
||||
|
||||
/**
|
||||
* @brief Retrieves cooked data from the BaseGyroscope.
|
||||
* @note This data is manipulated according to the formula
|
||||
* cooked = (raw * sensitivity) - bias.
|
||||
* @note Final data is expressed as DPS.
|
||||
* @note The axes array must be at least the same size of the
|
||||
* BaseGyroscope axes number.
|
||||
*
|
||||
* @param[in] devp pointer to @p L3GD20Driver.
|
||||
* @param[out] axes a buffer which would be filled with cooked data.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
*
|
||||
* @api
|
||||
*/
|
||||
#define l3gd20GyroscopeReadCooked(devp, axes) \
|
||||
gyroscopeReadCooked(&((devp)->gyro_if), axes)
|
||||
|
||||
/**
|
||||
* @brief Samples bias values for the BaseGyroscope.
|
||||
* @note The L3GD20 shall not be moved during the whole procedure.
|
||||
* @note After this function internal bias is automatically updated.
|
||||
* @note The behavior of this function depends on @P L3GD20_BIAS_ACQ_TIMES
|
||||
* and @p L3GD20_BIAS_SETTLING_US.
|
||||
*
|
||||
* @param[in] devp pointer to @p L3GD20Driver.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
*
|
||||
* @api
|
||||
*/
|
||||
#define l3gd20GyroscopeSampleBias(devp) \
|
||||
gyroscopeSampleBias(&((devp)->gyro_if))
|
||||
|
||||
/**
|
||||
* @brief Set bias values for the BaseGyroscope.
|
||||
* @note Bias must be expressed as DPS.
|
||||
* @note The bias buffer must be at least the same size of the BaseGyroscope
|
||||
* axes number.
|
||||
*
|
||||
* @param[in] devp pointer to @p L3GD20Driver.
|
||||
* @param[in] bp a buffer which contains biases.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
*
|
||||
* @api
|
||||
*/
|
||||
#define l3gd20GyroscopeSetBias(devp, bp) \
|
||||
gyroscopeSetBias(&((devp)->gyro_if), bp)
|
||||
|
||||
/**
|
||||
* @brief Reset bias values for the BaseGyroscope.
|
||||
* @note Default biases value are obtained from device datasheet when
|
||||
* available otherwise they are considered zero.
|
||||
*
|
||||
* @param[in] devp pointer to @p L3GD20Driver.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
*
|
||||
* @api
|
||||
*/
|
||||
#define l3gd20GyroscopeResetBias(devp) \
|
||||
gyroscopeResetBias(&((devp)->gyro_if))
|
||||
|
||||
/**
|
||||
* @brief Set sensitivity values for the BaseGyroscope.
|
||||
* @note Sensitivity must be expressed as DPS/LSB.
|
||||
* @note The sensitivity buffer must be at least the same size of the
|
||||
* BaseGyroscope axes number.
|
||||
*
|
||||
* @param[in] devp pointer to @p L3GD20Driver.
|
||||
* @param[in] sp a buffer which contains sensitivities.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
*
|
||||
* @api
|
||||
*/
|
||||
#define l3gd20GyroscopeSetSensitivity(devp, sp) \
|
||||
gyroscopeSetSensitivity(&((devp)->gyro_if), sp)
|
||||
|
||||
/**
|
||||
* @brief Reset sensitivity values for the BaseGyroscope.
|
||||
* @note Default sensitivities value are obtained from device datasheet.
|
||||
*
|
||||
* @param[in] devp pointer to @p L3GD20Driver.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
* @retval MSG_RESET otherwise.
|
||||
*
|
||||
* @api
|
||||
*/
|
||||
#define l3gd20GyroscopeResetSensitivity(devp) \
|
||||
gyroscopeResetSensitivity(&((devp)->gyro_if))
|
||||
|
||||
/**
|
||||
* @brief Changes the L3GD20Driver gyroscope fullscale value.
|
||||
* @note This function also rescale sensitivities and biases based on
|
||||
* previous and next fullscale value.
|
||||
* @note A recalibration is highly suggested after calling this function.
|
||||
*
|
||||
* @param[in] devp pointer to @p L3GD20Driver.
|
||||
* @param[in] fs new fullscale value.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
* @retval MSG_RESET otherwise.
|
||||
*
|
||||
* @api
|
||||
*/
|
||||
#define l3gd20GyroscopeSetFullScale(devp, fs) \
|
||||
(devp)->vmt->acc_set_full_scale(devp, fs)
|
||||
|
||||
/*===========================================================================*/
|
||||
/* External declarations. */
|
||||
|
|
Loading…
Reference in New Issue