// update IMU delta angle and delta velocity measurements
void NavEKF2_core::readIMUData()
{
const AP_InertialSensor &ins = _ahrs->get_ins();
// average IMU sampling rate
dtIMUavg = 1.0f/ins.get_sample_rate();
// the imu sample time is used as a common time reference throughout the filter
imuSampleTime_ms = hal.scheduler->millis();
// use the nominated imu or primary if not available
if (ins.use_accel(imu_index)) {
readDeltaVelocity(imu_index, imuDataNew.delVel, imuDataNew.delVelDT);
} else {
readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT);
}
// Get delta angle data from primary gyro or primary if not available
if (ins.use_gyro(imu_index)) {
readDeltaAngle(imu_index, imuDataNew.delAng);
} else {
readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng);
}
imuDataNew.delAngDT = max(ins.get_delta_time(),1.0e-4f);
// get current time stamp
imuDataNew.time_ms = imuSampleTime_ms;
// save data in the FIFO buffer
StoreIMU();
// extract the oldest available data from the FIFO buffer
imuDataDelayed = storedIMU[fifoIndexDelayed];
}
/*
* Read IMU delta angle and delta velocity measurements and downsample to 100Hz
* for storage in the data buffers used by the EKF. If the IMU data arrives at
* lower rate than 100Hz, then no downsampling or upsampling will be performed.
* Downsampling is done using a method that does not introduce coning or sculling
* errors.
*/
void NavEKF3_core::readIMUData()
{
const AP_InertialSensor &ins = _ahrs->get_ins();
// average IMU sampling rate
dtIMUavg = ins.get_loop_delta_t();
// the imu sample time is used as a common time reference throughout the filter
imuSampleTime_ms = AP_HAL::millis();
// use the nominated imu or primary if not available
if (ins.use_accel(imu_index)) {
readDeltaVelocity(imu_index, imuDataNew.delVel, imuDataNew.delVelDT);
accelPosOffset = ins.get_imu_pos_offset(imu_index);
} else {
readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT);
accelPosOffset = ins.get_imu_pos_offset(ins.get_primary_accel());
}
// Get delta angle data from primary gyro or primary if not available
if (ins.use_gyro(imu_index)) {
readDeltaAngle(imu_index, imuDataNew.delAng);
} else {
readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng);
}
imuDataNew.delAngDT = MAX(ins.get_delta_angle_dt(imu_index),1.0e-4f);
// Get current time stamp
imuDataNew.time_ms = imuSampleTime_ms;
// Accumulate the measurement time interval for the delta velocity and angle data
imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT;
imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT;
// Rotate quaternon atitude from previous to new and normalise.
// Accumulation using quaternions prevents introduction of coning errors due to downsampling
imuQuatDownSampleNew.rotate(imuDataNew.delAng);
imuQuatDownSampleNew.normalize();
// Rotate the latest delta velocity into body frame at the start of accumulation
Matrix3f deltaRotMat;
imuQuatDownSampleNew.rotation_matrix(deltaRotMat);
// Apply the delta velocity to the delta velocity accumulator
imuDataDownSampledNew.delVel += deltaRotMat*imuDataNew.delVel;
// Keep track of the number of IMU frames since the last state prediction
framesSincePredict++;
// If 10msec has elapsed, and the frontend has allowed us to start a new predict cycle, then store the accumulated IMU data
// to be used by the state prediction, ignoring the frontend permission if more than 20msec has lapsed
if ((dtIMUavg*(float)framesSincePredict >= EKF_TARGET_DT && startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 2.0f*EKF_TARGET_DT)) {
// convert the accumulated quaternion to an equivalent delta angle
imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng);
// Time stamp the data
imuDataDownSampledNew.time_ms = imuSampleTime_ms;
// Write data to the FIFO IMU buffer
storedIMU.push_youngest_element(imuDataDownSampledNew);
// calculate the achieved average time step rate for the EKF
float dtNow = constrain_float(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.0f,10.0f*EKF_TARGET_DT);
dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow;
// zero the accumulated IMU data and quaternion
imuDataDownSampledNew.delAng.zero();
imuDataDownSampledNew.delVel.zero();
imuDataDownSampledNew.delAngDT = 0.0f;
imuDataDownSampledNew.delVelDT = 0.0f;
imuQuatDownSampleNew[0] = 1.0f;
imuQuatDownSampleNew[3] = imuQuatDownSampleNew[2] = imuQuatDownSampleNew[1] = 0.0f;
// reset the counter used to let the frontend know how many frames have elapsed since we started a new update cycle
framesSincePredict = 0;
// set the flag to let the filter know it has new IMU data nad needs to run
runUpdates = true;
// extract the oldest available data from the FIFO buffer
imuDataDelayed = storedIMU.pop_oldest_element();
// protect against delta time going to zero
// TODO - check if calculations can tolerate 0
float minDT = 0.1f*dtEkfAvg;
imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT);
imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT);
// correct the extracted IMU data for sensor errors
delAngCorrected = imuDataDelayed.delAng;
delVelCorrected = imuDataDelayed.delVel;
correctDeltaAngle(delAngCorrected, imuDataDelayed.delAngDT);
correctDeltaVelocity(delVelCorrected, imuDataDelayed.delVelDT);
} else {
// we don't have new IMU data in the buffer so don't run filter updates on this time step
runUpdates = false;
}
}
/*
* Read IMU delta angle and delta velocity measurements and downsample to 100Hz
* for storage in the data buffers used by the EKF. If the IMU data arrives at
* lower rate than 100Hz, then no downsampling or upsampling will be performed.
* Downsampling is done using a method that does not introduce coning or sculling
* errors.
*/
void NavEKF2_core::readIMUData()
{
const AP_InertialSensor &ins = _ahrs->get_ins();
// average IMU sampling rate
dtIMUavg = 1.0f/ins.get_sample_rate();
// the imu sample time is used as a common time reference throughout the filter
imuSampleTime_ms = AP_HAL::millis();
// use the nominated imu or primary if not available
if (ins.use_accel(imu_index)) {
readDeltaVelocity(imu_index, imuDataNew.delVel, imuDataNew.delVelDT);
} else {
readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT);
}
// Get delta angle data from primary gyro or primary if not available
if (ins.use_gyro(imu_index)) {
readDeltaAngle(imu_index, imuDataNew.delAng);
} else {
readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng);
}
imuDataNew.delAngDT = MAX(ins.get_delta_time(),1.0e-4f);
// Get current time stamp
imuDataNew.time_ms = imuSampleTime_ms;
// remove gyro scale factor errors
imuDataNew.delAng.x = imuDataNew.delAng.x * stateStruct.gyro_scale.x;
imuDataNew.delAng.y = imuDataNew.delAng.y * stateStruct.gyro_scale.y;
imuDataNew.delAng.z = imuDataNew.delAng.z * stateStruct.gyro_scale.z;
// remove sensor bias errors
imuDataNew.delAng -= stateStruct.gyro_bias;
imuDataNew.delVel.z -= stateStruct.accel_zbias;
// Accumulate the measurement time interval for the delta velocity and angle data
imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT;
imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT;
// Rotate quaternon atitude from previous to new and normalise.
// Accumulation using quaternions prevents introduction of coning errors due to downsampling
Quaternion deltaQuat;
deltaQuat.rotate(imuDataNew.delAng);
imuQuatDownSampleNew = imuQuatDownSampleNew*deltaQuat;
imuQuatDownSampleNew.normalize();
// Rotate the accumulated delta velocity into the new frame of reference created by the latest delta angle
// This prevents introduction of sculling errors due to downsampling
Matrix3f deltaRotMat;
deltaQuat.inverse().rotation_matrix(deltaRotMat);
imuDataDownSampledNew.delVel = deltaRotMat*imuDataDownSampledNew.delVel;
// accumulate the latest delta velocity
imuDataDownSampledNew.delVel += imuDataNew.delVel;
// Keep track of the number of IMU frames since the last state prediction
framesSincePredict++;
// If 10msec has elapsed, and the frontend has allowed us to start a new predict cycle, then store the accumulated IMU data
// to be used by the state prediction, ignoring the frontend permission if more than 20msec has lapsed
if ((dtIMUavg*(float)framesSincePredict >= 0.01f && startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 0.02f)) {
// convert the accumulated quaternion to an equivalent delta angle
imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng);
// Time stamp the data
imuDataDownSampledNew.time_ms = imuSampleTime_ms;
// Write data to the FIFO IMU buffer
storedIMU.push_youngest_element(imuDataDownSampledNew);
// zero the accumulated IMU data and quaternion
imuDataDownSampledNew.delAng.zero();
imuDataDownSampledNew.delVel.zero();
imuDataDownSampledNew.delAngDT = 0.0f;
imuDataDownSampledNew.delVelDT = 0.0f;
imuQuatDownSampleNew[0] = 1.0f;
imuQuatDownSampleNew[3] = imuQuatDownSampleNew[2] = imuQuatDownSampleNew[1] = 0.0f;
// reset the counter used to let the frontend know how many frames have elapsed since we started a new update cycle
framesSincePredict = 0;
// set the flag to let the filter know it has new IMU data nad needs to run
runUpdates = true;
} else {
// we don't have new IMU data in the buffer so don't run filter updates on this time step
runUpdates = false;
}
// extract the oldest available data from the FIFO buffer
imuDataDelayed = storedIMU.pop_oldest_element();
float minDT = 0.1f*dtEkfAvg;
imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT);
imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT);
}
// update IMU delta angle and delta velocity measurements
void NavEKF2_core::readIMUData()
{
const AP_InertialSensor &ins = _ahrs->get_ins();
// average IMU sampling rate
dtIMUavg = 1.0f/ins.get_sample_rate();
// the imu sample time is used as a common time reference throughout the filter
imuSampleTime_ms = hal.scheduler->millis();
if (ins.use_accel(0) && ins.use_accel(1)) {
// dual accel mode
// delta time from each IMU
float dtDelVel0 = dtIMUavg;
float dtDelVel1 = dtIMUavg;
// delta velocity vector from each IMU
Vector3f delVel0, delVel1;
// Get delta velocity and time data from each IMU
readDeltaVelocity(0, delVel0, dtDelVel0);
readDeltaVelocity(1, delVel1, dtDelVel1);
// apply a peak hold 0.2 second time constant decaying envelope filter to the noise length on IMU 0
float alpha = 1.0f - 5.0f*dtDelVel0;
imuNoiseFiltState0 = maxf(ins.get_vibration_levels(0).length(), alpha*imuNoiseFiltState0);
// apply a peak hold 0.2 second time constant decaying envelope filter to the noise length on IMU 1
alpha = 1.0f - 5.0f*dtDelVel1;
imuNoiseFiltState1 = maxf(ins.get_vibration_levels(1).length(), alpha*imuNoiseFiltState1);
// calculate the filtered difference between acceleration vectors from IMU 0 and 1
// apply a LPF filter with a 1.0 second time constant
alpha = constrain_float(0.5f*(dtDelVel0 + dtDelVel1),0.0f,1.0f);
accelDiffFilt = (ins.get_accel(0) - ins.get_accel(1)) * alpha + accelDiffFilt * (1.0f - alpha);
float accelDiffLength = accelDiffFilt.length();
// Check the difference for excessive error and use the IMU with less noise
// Apply hysteresis to prevent rapid switching
if (accelDiffLength > 1.8f || (accelDiffLength > 1.2f && lastImuSwitchState != IMUSWITCH_MIXED)) {
if (lastImuSwitchState == IMUSWITCH_MIXED) {
// no previous fail so switch to the IMU with least noise
if (imuNoiseFiltState0 < imuNoiseFiltState1) {
lastImuSwitchState = IMUSWITCH_IMU0;
// Get data from IMU 0
imuDataNew.delVel = delVel0;
imuDataNew.delVelDT = dtDelVel0;
} else {
lastImuSwitchState = IMUSWITCH_IMU1;
// Get data from IMU 1
imuDataNew.delVel = delVel1;
imuDataNew.delVelDT = dtDelVel1;
}
} else if (lastImuSwitchState == IMUSWITCH_IMU0) {
// IMU 1 previously failed so require 5 m/s/s less noise on IMU 1 to switch
if (imuNoiseFiltState0 - imuNoiseFiltState1 > 5.0f) {
// IMU 1 is significantly less noisy, so switch
lastImuSwitchState = IMUSWITCH_IMU1;
// Get data from IMU 1
imuDataNew.delVel = delVel1;
imuDataNew.delVelDT = dtDelVel1;
}
} else {
// IMU 0 previously failed so require 5 m/s/s less noise on IMU 0 to switch across
if (imuNoiseFiltState1 - imuNoiseFiltState0 > 5.0f) {
// IMU 0 is significantly less noisy, so switch
lastImuSwitchState = IMUSWITCH_IMU0;
// Get data from IMU 0
imuDataNew.delVel = delVel0;
imuDataNew.delVelDT = dtDelVel0;
}
}
} else {
lastImuSwitchState = IMUSWITCH_MIXED;
// Use a blend of both accelerometers
imuDataNew.delVel = (delVel0 + delVel1)*0.5f;
imuDataNew.delVelDT = (dtDelVel0 + dtDelVel1)*0.5f;
}
} else {
// single accel mode - one of the first two accelerometers are unhealthy, not available or de-selected by the user
// set the switch state based on the IMU we are using to make the data source selection visible
if (ins.use_accel(0)) {
readDeltaVelocity(0, imuDataNew.delVel, imuDataNew.delVelDT);
lastImuSwitchState = IMUSWITCH_IMU0;
} else if (ins.use_accel(1)) {
readDeltaVelocity(1, imuDataNew.delVel, imuDataNew.delVelDT);
lastImuSwitchState = IMUSWITCH_IMU1;
} else {
readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT);
switch (ins.get_primary_accel()) {
case 0:
lastImuSwitchState = IMUSWITCH_IMU0;
break;
case 1:
lastImuSwitchState = IMUSWITCH_IMU1;
break;
default:
// we must be using an IMU which can't be properly represented so we set to "mixed"
lastImuSwitchState = IMUSWITCH_MIXED;
break;
}
}
}
// Get delta angle data from promary gyro
readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng);
imuDataNew.delAngDT = max(ins.get_delta_time(),1.0e-4f);
// get current time stamp
imuDataNew.time_ms = imuSampleTime_ms;
// save data in the FIFO buffer
StoreIMU();
// extract the oldest available data from the FIFO buffer
imuDataDelayed = storedIMU[fifoIndexDelayed];
}
