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 acc_read_cooked(void *ip, float axes[]) { uint32_t i; int32_t raw[LSM6DS0_ACC_NUMBER_OF_AXES]; msg_t msg; osalDbgCheck(((ip != NULL) && (axes != NULL)) && (((LSM6DS0Driver *)ip)->config->acccfg != NULL)); osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY), "acc_read_cooked(), invalid state"); msg = acc_read_raw(ip, raw); for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES ; i++){ axes[i] = raw[i] * ((LSM6DS0Driver *)ip)->accsensitivity[i]; axes[i] -= ((LSM6DS0Driver *)ip)->accbias[i]; } return msg; }
/** * @brief Retrieves cooked data from the BaseAccelerometer. * @note This data is manipulated according to the formula * cooked = (raw * sensitivity) - bias. * @note Final data is expressed as milli-G. * @note The axes array must be at least the same size of the * BaseAccelerometer axes number. * * @param[in] ip pointer to @p BaseAccelerometer interface. * @param[out] axes a buffer which would be filled with cooked data. * * @return The operation status. * @retval MSG_OK if the function succeeded. * @retval MSG_RESET if one or more I2C errors occurred, the errors can * be retrieved using @p i2cGetErrors(). * @retval MSG_TIMEOUT if a timeout occurred before operation end. */ static msg_t acc_read_cooked(void *ip, float axes[]) { LSM6DS0Driver* devp; uint32_t i; int32_t raw[LSM6DS0_ACC_NUMBER_OF_AXES]; msg_t msg; osalDbgCheck((ip != NULL) && (axes != NULL)); /* Getting parent instance pointer.*/ devp = objGetInstance(LSM6DS0Driver*, (BaseAccelerometer*)ip); osalDbgAssert((devp->state == LSM6DS0_READY), "acc_read_cooked(), invalid state"); msg = acc_read_raw(ip, raw); for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) { axes[i] = (raw[i] * devp->accsensitivity[i]) - devp->accbias[i]; } return msg; }