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 }