示例#1
0
文件: l3gd20.c 项目: rusefi/ChibiOS
/**
 * @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;
}
示例#2
0
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;
}
示例#3
0
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;
}
示例#4
0
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;
}
示例#5
0
文件: l3gd20.c 项目: rusefi/ChibiOS
/**
 * @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;
}