Пример #1
0
// ASA* registers
void ak8963GetAdjustment(int8_t *x, int8_t *y, int8_t *z)
{
  i2cdevRead(I2Cx, devAddr, AK8963_RA_ASAX, 3, buffer);
  *x = buffer[0];
  *y = buffer[1];
  *z = buffer[2];
}
static void sensorsTask(void *param)
{
  systemWaitStart();

  while (1)
  {
    if (pdTRUE == xSemaphoreTake(sensorsDataReady, portMAX_DELAY))
    {
      // data is ready to be read
      uint8_t dataLen = (uint8_t) (14 + (isMagnetometerPresent ? 6 : 0) + (isBarometerPresent ? 5 : 0));

      i2cdevRead(I2C3_DEV, MPU6500_ADDRESS_AD0_HIGH, MPU6500_RA_ACCEL_XOUT_H, dataLen, buffer);
      // these functions process the respective data and queue it on the output queues
      processAccGyroMeasurements(&(buffer[0]));
      if (isMagnetometerPresent) { processMagnetometerMeasurements(&(buffer[14])); }
      if (isBarometerPresent) { processBarometerMeasurements(&(buffer[isMagnetometerPresent ? 20 : 14])); }

      vTaskSuspendAll(); // ensure all queues are populated at the same time
      xQueueOverwrite(accelerometerDataQueue, &sensors.acc);
      xQueueOverwrite(gyroDataQueue, &sensors.gyro);
      if (isMagnetometerPresent) { xQueueOverwrite(magnetometerDataQueue, &sensors.mag); }
      if (isBarometerPresent) { xQueueOverwrite(barometerDataQueue, &sensors.baro); }
      xTaskResumeAll();
    }
  }
}
Пример #3
0
int16_t ak8963GetHeadingZ()
{
  i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_CNTL, AK8963_MODE_SINGLE);
//  delay(10);
  i2cdevRead(I2Cx, devAddr, AK8963_RA_HZL, 2, buffer);
  return (((int16_t) buffer[1]) << 8) | buffer[0];
}
Пример #4
0
/** Get Z-axis heading measurement.
 * @return 16-bit signed integer with Z-axis heading
 * @see HMC5883L_RA_DATAZ_H
 */
int16_t hmc5883lGetHeadingZ()
{
  // each axis read requires that ALL axis registers be read, even if only
  // one is used; this was not done ineffiently in the code by accident
  i2cdevRead(I2Cx, devAddr, HMC5883L_RA_DATAX_H, 6, buffer);
  if (mode == HMC5883L_MODE_SINGLE) i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
  return (((int16_t)buffer[2]) << 8) | buffer[3];
}
Пример #5
0
/** Get 3-axis heading measurements.
 * In the event the ADC reading overflows or underflows for the given channel,
 * or if there is a math overflow during the bias measurement, this data
 * register will contain the value -4096. This register value will clear when
 * after the next valid measurement is made. Note that this method automatically
 * clears the appropriate bit in the MODE register if Single mode is active.
 * @param x 16-bit signed integer container for X-axis heading
 * @param y 16-bit signed integer container for Y-axis heading
 * @param z 16-bit signed integer container for Z-axis heading
 * @see HMC5883L_RA_DATAX_H
 */
void hmc5883lGetHeading(int16_t *x, int16_t *y, int16_t *z)
{
  i2cdevRead(I2Cx, devAddr, HMC5883L_RA_DATAX_H, 6, buffer);
  if (mode == HMC5883L_MODE_SINGLE) i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
  *x = (((int16_t)buffer[0]) << 8) | buffer[1];
  *y = (((int16_t)buffer[4]) << 8) | buffer[5];
  *z = (((int16_t)buffer[2]) << 8) | buffer[3];
}
Пример #6
0
/** Verify the I2C connection.
 * Make sure the device is connected and responds as expected.
 * @return True if connection is valid, false otherwise
 */
bool hmc5883lTestConnection()
{
  if (i2cdevRead(I2Cx, devAddr, HMC5883L_RA_ID_A, 3, buffer))
  {
    return (buffer[0] == 'H' && buffer[1] == '4' && buffer[2] == '3');
  }

  return false;
}
Пример #7
0
// H* registers
void ak8963GetHeading(int16_t *x, int16_t *y, int16_t *z)
{
//  i2cdevWriteByte(I2Cx, devAddr, AK8963_RA_CNTL, AK8963_MODE_SINGLE);
//  delay(10);
  i2cdevRead(I2Cx, devAddr, AK8963_RA_HXL, 6, buffer);
  *x = (((int16_t) buffer[1]) << 8) | buffer[0];
  *y = (((int16_t) buffer[3]) << 8) | buffer[2];
  *z = (((int16_t) buffer[5]) << 8) | buffer[4];
}
Пример #8
0
bool i2cdevReadBit(I2C_TypeDef *I2Cx, uint8_t devAddress, uint8_t memAddress,
                     uint8_t bitNum, uint8_t *data)
{
  uint8_t byte;
  bool status;
  
  status = i2cdevRead(I2Cx, devAddress, memAddress, 1, &byte);
  *data = byte & (1 << bitNum);

  return status;
}
Пример #9
0
bool lps25hGetData(float* pressure, float* temperature, float* asl)
{
  uint8_t data[5];
  uint32_t rawPressure;
  int16_t rawTemp;
  bool status;

  status =  i2cdevRead(I2Cx, devAddr, LPS25H_PRESS_OUT_XL | LPS25H_ADDR_AUTO_INC, 3, data);
  // If LPS25H moving avg filter is activated the temp must be read out in separate read.
  status &= i2cdevRead(I2Cx, devAddr, LPS25H_TEMP_OUT_L | LPS25H_ADDR_AUTO_INC, 2, &data[3]);

  rawPressure = ((uint32_t)data[2] << 16) | ((uint32_t)data[1] << 8) | data[0];
  *pressure = (float)rawPressure / LPS25H_LSB_PER_MBAR;

  rawTemp = ((int16_t)data[4] << 8) | data[3];
  *temperature = LPS25H_TEMP_OFFSET + ((float)rawTemp / LPS25H_LSB_PER_CELSIUS);

  *asl = lps25hPressureToAltitude(pressure);

  return status;
}
/*!
 * @brief Generic burst read
 *
 * @param [out] dev_id I2C address, SPI chip select or user desired identifier
 *
 * @return Zero if successful, otherwise an error code
 */
static bstdr_ret_t bmi088_burst_read(uint8_t dev_id, uint8_t reg_addr, uint8_t *reg_data, uint16_t len)
{
  /**< Burst read code comes here */
  if (i2cdevRead(I2C3_DEV, dev_id, reg_addr, (uint16_t) len, reg_data))
  {
    return BSTDR_OK;
  }
  else
  {
    return BSTDR_E_CON_ERROR;
  }
}
Пример #11
0
bool lps25hTestConnection(void)
{
  uint8_t id;
  bool status;

  if (!isInit)
	return false;


  status = i2cdevRead(I2Cx, devAddr, LPS25H_WHO_AM_I, 1, &id);

  if (status == true && id == LPS25H_WAI_ID)
	  return true;

  return false;
}
static void sensorsTask(void *param)
{
  systemWaitStart();

  sensorsSetupSlaveRead();

  while (1)
  {
    if (pdTRUE == xSemaphoreTake(sensorsDataReady, portMAX_DELAY))
    {
      sensorData.interruptTimestamp = imuIntTimestamp;
      // data is ready to be read
      uint8_t dataLen = (uint8_t) (SENSORS_MPU6500_BUFF_LEN +
              (isMagnetometerPresent ? SENSORS_MAG_BUFF_LEN : 0) +
              (isBarometerPresent ? SENSORS_BARO_BUFF_LEN : 0));

      i2cdevRead(I2C3_DEV, MPU6500_ADDRESS_AD0_HIGH, MPU6500_RA_ACCEL_XOUT_H, dataLen, buffer);
      // these functions process the respective data and queue it on the output queues
      processAccGyroMeasurements(&(buffer[0]));
      if (isMagnetometerPresent)
      {
          processMagnetometerMeasurements(&(buffer[SENSORS_MPU6500_BUFF_LEN]));
      }
      if (isBarometerPresent)
      {
          processBarometerMeasurements(&(buffer[isMagnetometerPresent ?
                  SENSORS_MPU6500_BUFF_LEN + SENSORS_MAG_BUFF_LEN : SENSORS_MPU6500_BUFF_LEN]));
      }

      xQueueOverwrite(accelerometerDataQueue, &sensorData.acc);
      xQueueOverwrite(gyroDataQueue, &sensorData.gyro);
      if (isMagnetometerPresent)
      {
        xQueueOverwrite(magnetometerDataQueue, &sensorData.mag);
      }
      if (isBarometerPresent)
      {
        xQueueOverwrite(barometerDataQueue, &sensorData.baro);
      }

      // Unlock stabilizer task
      xSemaphoreGive(dataReady);
    }
  }
}
Пример #13
0
bool lps25hGetData(float* pressure, float* temperature, float* asl)
{
    uint8_t data[5];
    uint32_t rawPressure;
    int16_t rawTemp;
    bool status;

    status = i2cdevRead(I2Cx, devAddr, LPS25H_PRESS_OUT_XL | LPS25H_ADDR_AUTO_INC, 5, data);

    rawPressure = ((uint32_t)data[2] << 16) | ((uint32_t)data[1] << 8) | data[0];
    *pressure = (float)rawPressure / LPS25H_LSB_PER_MBAR;

    rawTemp = ((int16_t)data[4] << 8) | data[3];
    *temperature = LPS25H_TEMP_OFFSET + ((float)rawTemp / LPS25H_LSB_PER_CELSIUS);

    *asl = lps25hPressureToAltitude(pressure);

    return status;
}
Пример #14
0
bool i2cdevReadByte(I2C_TypeDef *I2Cx, uint8_t devAddress, uint8_t memAddress,
                    uint8_t *data)
{
  return i2cdevRead(I2Cx, devAddress, memAddress, 1, data);
}
bool i2cdevReadByte(I2C_Dev *dev, uint8_t devAddress, uint8_t memAddress,
                    uint8_t *data)
{
  return i2cdevRead(dev, devAddress, memAddress, 1, data);
}
Пример #16
0
/** Do a self test.
 * @return True if self test passed, false otherwise
 */
bool hmc5883lSelfTest()
{
  bool testStatus = true;
  int16_t mxp, myp, mzp;  // positive magnetometer measurements
  int16_t mxn, myn, mzn;  // negative magnetometer measurements
  struct
  {
    uint8_t configA;
    uint8_t configB;
    uint8_t mode;
  } regSave;

  // Save register values
  if (i2cdevRead(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, sizeof(regSave), (uint8_t *)&regSave) == false)
  {
    // TODO: error handling
    return false;
  }
  // Set gain (sensitivity)
  hmc5883lSetGain(HMC5883L_ST_GAIN);

  // Write CONFIG_A register and do positive test
  i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_CONFIG_A,
      (HMC5883L_AVERAGING_1 << (HMC5883L_CRA_AVERAGE_BIT - HMC5883L_CRA_AVERAGE_LENGTH + 1)) |
      (HMC5883L_RATE_15 << (HMC5883L_CRA_RATE_BIT - HMC5883L_CRA_RATE_LENGTH + 1)) |
      (HMC5883L_BIAS_POSITIVE << (HMC5883L_CRA_BIAS_BIT - HMC5883L_CRA_BIAS_LENGTH + 1)));

  /* Perform test measurement & check results */
  hmc5883lSetMode(HMC5883L_MODE_SINGLE);
  vTaskDelay(M2T(HMC5883L_ST_DELAY_MS));
  hmc5883lGetHeading(&mxp, &myp, &mzp);

  // Write CONFIG_A register and do negative test
  i2cdevWriteByte(I2Cx, devAddr, HMC5883L_RA_CONFIG_A,
      (HMC5883L_AVERAGING_1 << (HMC5883L_CRA_AVERAGE_BIT - HMC5883L_CRA_AVERAGE_LENGTH + 1)) |
      (HMC5883L_RATE_15 << (HMC5883L_CRA_RATE_BIT - HMC5883L_CRA_RATE_LENGTH + 1)) |
      (HMC5883L_BIAS_NEGATIVE << (HMC5883L_CRA_BIAS_BIT - HMC5883L_CRA_BIAS_LENGTH + 1)));

  /* Perform test measurement & check results */
  hmc5883lSetMode(HMC5883L_MODE_SINGLE);
  vTaskDelay(M2T(HMC5883L_ST_DELAY_MS));
  hmc5883lGetHeading(&mxn, &myn, &mzn);

  if (hmc5883lEvaluateSelfTest(HMC5883L_ST_X_MIN, HMC5883L_ST_X_MAX, mxp, "pos X") &&
      hmc5883lEvaluateSelfTest(HMC5883L_ST_Y_MIN, HMC5883L_ST_Y_MAX, myp, "pos Y") &&
      hmc5883lEvaluateSelfTest(HMC5883L_ST_Z_MIN, HMC5883L_ST_Z_MAX, mzp, "pos Z") &&
      hmc5883lEvaluateSelfTest(-HMC5883L_ST_X_MAX, -HMC5883L_ST_X_MIN, mxn, "neg X") &&
      hmc5883lEvaluateSelfTest(-HMC5883L_ST_Y_MAX, -HMC5883L_ST_Y_MIN, myn, "neg Y") &&
      hmc5883lEvaluateSelfTest(-HMC5883L_ST_Z_MAX, -HMC5883L_ST_Z_MIN, mzn, "neg Z"))
  {
    DEBUG_PRINT("Self test [OK].\n");
  }
  else
  {
    testStatus = false;
  }

  // Restore registers
  if (i2cdevWrite(I2Cx, devAddr, HMC5883L_RA_CONFIG_A, sizeof(regSave), (uint8_t *)&regSave) == false)
  {
    // TODO: error handling
    return false;
  }

  return true; //testStatus;
}