// 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();
}
}
}
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];
}
/** 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];
}
/** 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];
}
/** 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;
}
// 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];
}
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;
}
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;
}
}
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);
}
}
}
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;
}
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);
}
/** 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 *)®Save) == 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 *)®Save) == false)
{
// TODO: error handling
return false;
}
return true; //testStatus;
}