EKF: Fix variable names to match convention

EKF/ekf.cpp

@@ -259,10 +259,10 @@ bool Ekf::initialiseFilter(void)
 			// increment the sample count and apply a LPF to the measurement
 			_mag_counter ++;
 			// don't start using data until we can be certain all bad initial data has been flushed
-			if (_mag_counter == (uint8_t)(OBS_BUFFER_LENGTH + 1)) {
+			if (_mag_counter == (uint8_t)(_obs_buffer_length + 1)) {
 				// initialise filter states
 				_mag_filt_state = _mag_sample_delayed.mag;
-			} else if (_mag_counter > (uint8_t)(OBS_BUFFER_LENGTH + 1)) {
+			} else if (_mag_counter > (uint8_t)(_obs_buffer_length + 1)) {
 				// noise filter the data
 				_mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f;
 			}
@@ -306,10 +306,10 @@ bool Ekf::initialiseFilter(void)
 				// increment the sample count and apply a LPF to the measurement
 				_hgt_counter ++;
 				// don't start using data until we can be certain all bad initial data has been flushed
-				if (_hgt_counter == (uint8_t)(OBS_BUFFER_LENGTH + 1)) {
+				if (_hgt_counter == (uint8_t)(_obs_buffer_length + 1)) {
 					// initialise filter states
 					_rng_filt_state = _range_sample_delayed.rng;
-				} else if (_hgt_counter > (uint8_t)(OBS_BUFFER_LENGTH + 1)) {
+				} else if (_hgt_counter > (uint8_t)(_obs_buffer_length + 1)) {
 					// noise filter the data
 					_rng_filt_state = 0.9f * _rng_filt_state + 0.1f * _range_sample_delayed.rng;
 				}
@@ -330,10 +330,10 @@ bool Ekf::initialiseFilter(void)
 				// increment the sample count and apply a LPF to the measurement
 				_hgt_counter ++;
 				// don't start using data until we can be certain all bad initial data has been flushed
-				if (_hgt_counter == (uint8_t)(OBS_BUFFER_LENGTH + 1)) {
+				if (_hgt_counter == (uint8_t)(_obs_buffer_length + 1)) {
 					// initialise filter states
 					_baro_hgt_offset = _baro_sample_delayed.hgt;
-				} else if (_hgt_counter > (uint8_t)(OBS_BUFFER_LENGTH + 1)) {
+				} else if (_hgt_counter > (uint8_t)(_obs_buffer_length + 1)) {
 					// noise filter the data
 					_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
 				}
@@ -347,9 +347,9 @@ bool Ekf::initialiseFilter(void)
 	}
 
 	// check to see if we have enough measurements and return false if not
-	bool hgt_count_fail = _hgt_counter <= 2*OBS_BUFFER_LENGTH;
-	bool mag_count_fail = _mag_counter <= 2*OBS_BUFFER_LENGTH;
-	bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 2*OBS_BUFFER_LENGTH);
+	bool hgt_count_fail = _hgt_counter <= 2*_obs_buffer_length;
+	bool mag_count_fail = _mag_counter <= 2*_obs_buffer_length;
+	bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 2*_obs_buffer_length);
 	if (hgt_count_fail || mag_count_fail || ev_count_fail) {
 		return false;
 

EKF/estimator_interface.cpp

@@ -48,8 +48,8 @@
 
 
 EstimatorInterface::EstimatorInterface():
-	OBS_BUFFER_LENGTH(10),
-	IMU_BUFFER_LENGTH(30),
+	_obs_buffer_length(10),
+	_imu_buffer_length(30),
 	_min_obs_interval_us(0),
 	_dt_imu_avg(0.0f),
 	_imu_ticks(0),
@@ -140,7 +140,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
 
 		// calculate the minimum interval between observations required to guarantee no loss of data
 		// this will occur if data is overwritten before its time stamp falls behind the fusion time horizon
-		_min_obs_interval_us = (_imu_sample_new.time_us - _imu_sample_delayed.time_us)/(OBS_BUFFER_LENGTH - 1);
+		_min_obs_interval_us = (_imu_sample_new.time_us - _imu_sample_delayed.time_us)/(_obs_buffer_length - 1);
 
 	} else {
 		_imu_updated = false;
@@ -351,36 +351,36 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 							 math::max(_params.airspeed_delay_ms, _params.baro_delay_ms))))));
 
 	// calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
-	IMU_BUFFER_LENGTH = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1;
+	_imu_buffer_length = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1;
 
 	// set the observaton buffer length to handle the minimum time of arrival between observations in combination
 	// with the worst case delay from current time to ekf fusion time
 	// allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter
 	uint16_t ekf_delay_ms = max_time_delay_ms + (int)(ceil((float)max_time_delay_ms * 0.5f));
-	OBS_BUFFER_LENGTH = (ekf_delay_ms / _params.sensor_interval_min_ms) + 1;
+	_obs_buffer_length = (ekf_delay_ms / _params.sensor_interval_min_ms) + 1;
 
 	// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)
-	OBS_BUFFER_LENGTH = math::min(OBS_BUFFER_LENGTH,IMU_BUFFER_LENGTH);
-
-	ECL_INFO("EKF IMU buffer length = %i",(int)IMU_BUFFER_LENGTH);
-	ECL_INFO("EKF observation buffer length = %i",(int)OBS_BUFFER_LENGTH);
-
-	if (!(_imu_buffer.allocate(IMU_BUFFER_LENGTH) &&
-	      _gps_buffer.allocate(OBS_BUFFER_LENGTH) &&
-	      _mag_buffer.allocate(OBS_BUFFER_LENGTH) &&
-	      _baro_buffer.allocate(OBS_BUFFER_LENGTH) &&
-	      _range_buffer.allocate(OBS_BUFFER_LENGTH) &&
-	      _airspeed_buffer.allocate(OBS_BUFFER_LENGTH) &&
-	      _flow_buffer.allocate(OBS_BUFFER_LENGTH) &&
-	      _ext_vision_buffer.allocate(OBS_BUFFER_LENGTH) &&
-	      _output_buffer.allocate(IMU_BUFFER_LENGTH))) {
+	_obs_buffer_length = math::min(_obs_buffer_length,_imu_buffer_length);
+
+	ECL_INFO("EKF IMU buffer length = %i",(int)_imu_buffer_length);
+	ECL_INFO("EKF observation buffer length = %i",(int)_obs_buffer_length);
+
+	if (!(_imu_buffer.allocate(_imu_buffer_length) &&
+	      _gps_buffer.allocate(_obs_buffer_length) &&
+	      _mag_buffer.allocate(_obs_buffer_length) &&
+	      _baro_buffer.allocate(_obs_buffer_length) &&
+	      _range_buffer.allocate(_obs_buffer_length) &&
+	      _airspeed_buffer.allocate(_obs_buffer_length) &&
+	      _flow_buffer.allocate(_obs_buffer_length) &&
+	      _ext_vision_buffer.allocate(_obs_buffer_length) &&
+	      _output_buffer.allocate(_imu_buffer_length))) {
 		ECL_ERR("EKF buffer allocation failed!");
 		unallocate_buffers();
 		return false;
 	}
 
 	// zero the data in the observation buffers
-	for (int index=0; index < OBS_BUFFER_LENGTH; index++) {
+	for (int index=0; index < _obs_buffer_length; index++) {
 		gpsSample gps_sample_init = {};
 		_gps_buffer.push(gps_sample_init);
 		magSample mag_sample_init = {};
@@ -398,7 +398,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	}
 
 	// zero the data in the imu data and output observer state buffers
-	for (int index=0; index < IMU_BUFFER_LENGTH; index++) {
+	for (int index=0; index < _imu_buffer_length; index++) {
 		imuSample imu_sample_init = {};
 		_imu_buffer.push(imu_sample_init);
 		outputSample output_sample_init = {};

EKF/estimator_interface.h

@@ -280,14 +280,14 @@ protected:
 	 max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS * 0.001)
 	 This can be adjusted to match the max sensor data rate plus some margin for jitter.
 	*/
-	uint8_t OBS_BUFFER_LENGTH;
+	uint8_t _obs_buffer_length;
 	/*
 	IMU_BUFFER_LENGTH defines how many IMU samples we buffer which sets the time delay from current time to the
 	EKF fusion time horizon and therefore the maximum sensor time offset relative to the IMU that we can compensate for.
 	max sensor time offet (msec) =  IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS
 	This can be adjusted to a value that is FILTER_UPDATE_PERIOD_MS longer than the maximum observation time delay.
 	*/
-	uint8_t IMU_BUFFER_LENGTH;
+	uint8_t _imu_buffer_length;
 	static const unsigned FILTER_UPDATE_PERIOD_MS = 10;	// ekf prediction period in milliseconds
 
 	unsigned _min_obs_interval_us; // minimum time interval between observations that will guarantee data is not lost (usec)