/**
* @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_GYRO_NUMBER_OF_AXES];
int32_t buff[L3GD20_GYRO_NUMBER_OF_AXES] = {0, 0, 0};
msg_t msg;
osalDbgCheck(ip != NULL);
/* 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 = gyro_read_raw(ip, raw);
if(msg != MSG_OK)
return msg;
for(j = 0; j < L3GD20_GYRO_NUMBER_OF_AXES; j++){
buff[j] += raw[j];
}
osalThreadSleepMicroseconds(L3GD20_BIAS_SETTLING_US);
}
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 gyro_sample_bias(void *ip) {
uint32_t i, j;
int32_t raw[LSM6DS0_GYRO_NUMBER_OF_AXES];
int32_t buff[LSM6DS0_GYRO_NUMBER_OF_AXES] = {0, 0, 0};
msg_t msg;
osalDbgCheck(ip != NULL);
osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
"sample_bias(), invalid state");
for(i = 0; i < LSM6DS0_GYRO_BIAS_ACQ_TIMES; i++){
msg = gyro_read_raw(ip, raw);
if(msg != MSG_OK)
return msg;
for(j = 0; j < LSM6DS0_GYRO_NUMBER_OF_AXES; j++){
buff[j] += raw[j];
}
osalThreadSleepMicroseconds(LSM6DS0_GYRO_BIAS_SETTLING_uS);
}
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++){
((LSM6DS0Driver *)ip)->gyrobias[i] = buff[i] / LSM6DS0_GYRO_BIAS_ACQ_TIMES;
}
return msg;
}
static msg_t sens_read_raw(void *ip, int32_t axes[]) {
int32_t* bp = axes;
msg_t msg;
if (((LSM6DS0Driver *)ip)->config->acccfg != NULL) {
msg = acc_read_raw(ip, bp);
if(msg != MSG_OK)
return msg;
bp += LSM6DS0_ACC_NUMBER_OF_AXES;
}
if (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL) {
msg = gyro_read_raw(ip, bp);
}
return msg;
}
static msg_t gyro_read_cooked(void *ip, float axes[]) {
uint32_t i;
int32_t raw[LSM6DS0_GYRO_NUMBER_OF_AXES];
msg_t msg;
osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
(((LSM6DS0Driver *)ip)->config->gyrocfg != NULL));
osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
"gyro_read_cooked(), invalid state");
msg = gyro_read_raw(ip, raw);
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES ; i++){
axes[i] = raw[i] * ((LSM6DS0Driver *)ip)->gyrosensitivity[i];
}
return msg;
}
/**
* @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_GYRO_NUMBER_OF_AXES];
msg_t msg;
osalDbgCheck((ip != NULL) && (axes != NULL));
/* Getting parent instance pointer.*/
devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);
osalDbgAssert((devp->state == L3GD20_READY),
"gyro_read_cooked(), invalid state");
msg = gyro_read_raw(ip, raw);
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++){
axes[i] = (raw[i] * devp->gyrosensitivity[i]) - devp->gyrobias[i];
}
return msg;
}