bool Ekf::update()
{
bool ret = false; // indicates if there has been an update
if (!_filter_initialised) {
_filter_initialised = initialiseFilter();
if (!_filter_initialised) {
return false;
}
}
//printStates();
//printStatesFast();
// prediction
if (_imu_updated) {
ret = true;
predictState();
predictCovariance();
}
// measurement updates
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
//fuseHeading();
fuseMag(_mag_fuse_index);
_mag_fuse_index = (_mag_fuse_index + 1) % 3;
}
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
_fuse_height = true;
}
if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
_fuse_pos = true;
_fuse_vert_vel = true;
_fuse_hor_vel = true;
} else if (_time_last_imu - _time_last_gps > 2000000 && _time_last_imu - _time_last_fake_gps > 70000) {
// if gps does not update then fake position and horizontal veloctiy measurements
_fuse_hor_vel = true; // we only fake horizontal velocity because we still have baro
_gps_sample_delayed.vel.setZero();
}
if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
fuseVelPosHeight();
_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
}
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
fuseRange();
}
if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed)) {
fuseAirspeed();
}
calculateOutputStates();
return ret;
}
/*! **********************************************************************
* Function: range(void)
*
* Include: Range.h
*
* @brief Samples the range
*
* Description: Takes a number of samples of the ultrasonic sensor at a specified
* rate. Continues to sample the IR sensor at a different rate while
* sampling the ultrasonic. Then combines the ranges and sets the
* target state
*
* Arguments: None
*
* Returns: the range
*************************************************************************/
unsigned int range(void)
{
#define range_IR sumIR //Come conenient name changes
#define range_US sumUS
char i;
unsigned int k;
unsigned int temp;
unsigned long int sumUS = 0;
unsigned long int sumIR = 0;
int IR_samples = 0;
unsigned char delayUS = 100 / rateUS; //100Hz will give 1 delay increment of 10ms
unsigned char delayIR = 10000 / rateIR; //10KHz will give 1 delay increment of 0.1ms
//Multiplex onto the IR sensor
// SetChanADC(ADC_IR_READ);
ADCON0 = ADC_IR_READ;
for (i = 0; i < numSamples; i++)
{
configureRange(); //Still have to reconfigure each time???
beginUS();
//Continue sampling the IR while waiting for the ultrasonic
while (measuringUS)
{
ADCON0bits.GO = 1;
while (ADCON0bits.GO_NOT_DONE);
temp = ADRES;
if (temp > 100) sumIR += IR_CONV(temp);
else IR_samples--;
//sumIR += IR_CONV(ADRES >> 6);
IR_samples++;
Delay100TCYx(delayIR); //Delays in inrements of 100Tcy, which is 100 x 1us for 4MHz clock -> 0.1ms or 10KHz
}
//get range of ultrasonic reading
sumUS += rangeUS(25); ///Standard room temperature for now @todo Read in temperature for US calculation
Delay10KTCYx(delayUS); //Delays in increments of 10KTcy, which is 10,000 * 1us for a 4MHz clock
}
if (numSamples) sumUS = sumUS / numSamples; //Calculate the average Ultrasonic range
else sumUS = 0;
//Average all IR samples taken, and convert to distance
if (IR_samples) range_IR = sumIR / IR_samples;
else sumIR = 0;
// Save the range value for printing raw rang
lastIRRange = range_IR;
lastUSRange = range_US;
// Eliminate out of range values
if (range_IR > m_maxRange) range_IR = 0;
if (range_US > m_maxRange) range_US = 0;
if (range_IR < m_minRange) range_IR = 0;
if (range_US < m_minRange) range_US = 0;
return lastRange = fuseRange(range_US, range_IR);
#undef range_IR
#undef range_US
}
