Merge pull request #321 from dagar/pr-ekf_formatting

EKF trivial code style cleanup

EKF/RingBuffer.h

@@ -71,7 +71,7 @@ public:
 
 		_size = size;
 		// set the time elements to zero so that bad data is not retrieved from the buffers
-		for (unsigned index=0; index < _size; index++) {
+		for (unsigned index = 0; index < _size; index++) {
 			_buffer[index].time_us = 0;
 		}
 		_first_write = true;

EKF/airspeed_fusion.cpp

@@ -150,8 +150,7 @@ void Ekf::fuseAirspeed()
 		if (_tas_test_ratio > 1.0f) {
 			_innov_check_fail_status.flags.reject_airspeed = true;
 			return;
-		}
-		else {
+		} else {
 			_innov_check_fail_status.flags.reject_airspeed = false;
 		}
 

EKF/control.cpp

@@ -195,7 +195,7 @@ void Ekf::controlExternalVisionFusion()
 				for (unsigned i=0; i < output_length; i++) {
 					output_states = _output_buffer.get_from_index(i);
 					output_states.quat_nominal *= _state_reset_status.quat_change;
-					_output_buffer.push_to_index(i,output_states);
+					_output_buffer.push_to_index(i, output_states);
 				}
 
 				// apply the change in attitude quaternion to our newest quaternion estimate
@@ -424,7 +424,7 @@ void Ekf::controlGpsFusion()
 					}
 					if (_control_status.flags.gps) {
 						ECL_INFO("EKF commencing GPS fusion");
-					       _time_last_gps = _time_last_imu;
+						_time_last_gps = _time_last_imu;
 
 					}
 				}
@@ -470,7 +470,7 @@ void Ekf::controlGpsFusion()
 			// correct velocity for offset relative to IMU
 			Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f/_imu_sample_delayed.delta_ang_dt);
 			Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
-			Vector3f vel_offset_body = cross_product(ang_rate,pos_offset_body);
+			Vector3f vel_offset_body = cross_product(ang_rate, pos_offset_body);
 			Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 			_gps_sample_delayed.vel -= vel_offset_earth;
 
@@ -577,7 +577,7 @@ void Ekf::controlHeightSensorTimeouts()
 				reset_height = true;
 				ECL_WARN("EKF baro hgt timeout - reset to GPS");
 
-			} else if (reset_to_baro){
+			} else if (reset_to_baro) {
 				// set height sensor health
 				_baro_hgt_faulty = false;
 
@@ -968,11 +968,13 @@ void Ekf::checkForStuckRange()
 			_rng_stuck = false;
 
 		} else {
-			if (_range_sample_delayed.rng > _rng_check_max_val)
+			if (_range_sample_delayed.rng > _rng_check_max_val) {
 				_rng_check_max_val = _range_sample_delayed.rng;
+			}
 
-			if (_rng_check_min_val < 0.1f || _range_sample_delayed.rng < _rng_check_min_val)
+			if (_rng_check_min_val < 0.1f || _range_sample_delayed.rng < _rng_check_min_val) {
 				_rng_check_min_val = _range_sample_delayed.rng;
+			}
 
 			_range_data_ready = false;
 		}
@@ -984,11 +986,12 @@ void Ekf::checkForStuckRange()
 
 void Ekf::controlAirDataFusion()
 {
-        // control activation and initialisation/reset of wind states required for airspeed fusion
+	// control activation and initialisation/reset of wind states required for airspeed fusion
 
 	// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
-        bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6;
-        bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6;
+	bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6;
+	bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6;
+
 	if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
 		_control_status.flags.wind = false;
 
@@ -1048,15 +1051,12 @@ void Ekf::controlBetaFusion()
 		}
 
 		fuseSideslip();
- 	}
-
-
-
+	}
 }
 
 void Ekf::controlDragFusion()
 {
-	if (_params.fusion_mode & MASK_USE_DRAG ) {
+	if (_params.fusion_mode & MASK_USE_DRAG) {
 		if (_control_status.flags.in_air) {
 			if (!_control_status.flags.wind) {
 				// reset the wind states and covariances when starting drag accel fusion

EKF/covariance.cpp

@@ -95,7 +95,7 @@ void Ekf::initialiseCovariance()
 
 	// earth frame and body frame magnetic field
 	// set to observation variance
-	for (uint8_t index=16; index <= 21; index ++) {
+	for (uint8_t index = 16; index <= 21; index ++) {
 		P[index][index] = sq(_params.mag_noise);
 	}
 
@@ -733,12 +733,14 @@ void Ekf::fixCovarianceErrors()
 		const float minSafeStateVar = 1e-9f;
 		float maxStateVar = minSafeStateVar;
 		bool resetRequired = false;
-		for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) {
-		    if (P[stateIndex][stateIndex] > maxStateVar) {
-			maxStateVar = P[stateIndex][stateIndex];
-		    } else if (P[stateIndex][stateIndex] < minSafeStateVar) {
-			resetRequired = true;
-		    }
+
+		for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
+			if (P[stateIndex][stateIndex] > maxStateVar) {
+				maxStateVar = P[stateIndex][stateIndex];
+
+			} else if (P[stateIndex][stateIndex] < minSafeStateVar) {
+				resetRequired = true;
+			}
 		}
 
 		// To ensure stability of the covariance matrix operations, the ratio of a max and min variance must
@@ -746,31 +748,37 @@ void Ekf::fixCovarianceErrors()
 		// Also limit variance to a maximum equivalent to a 1g uncertainty
 		const float minStateVarTarget = 1E-8f;
 		float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget);
-		for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) {
-			P[stateIndex][stateIndex] = math::constrain(P[stateIndex][stateIndex], minAllowedStateVar, sq(_gravity_mss * _dt_ekf_avg));
+
+		for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
+			P[stateIndex][stateIndex] = math::constrain(P[stateIndex][stateIndex], minAllowedStateVar,
+						    sq(_gravity_mss * _dt_ekf_avg));
 		}
 
 		// If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero
 		if (resetRequired) {
-		    float delVelBiasVar[3];
-		    // store all delta velocity bias variances
-		    for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) {
-			delVelBiasVar[stateIndex-13] = P[stateIndex][stateIndex];
-		    }
-		    // reset all delta velocity bias covariances
-		    zeroCols(P,13,15);
-		    // restore all delta velocity bias variances
-		    for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) {
-			P[stateIndex][stateIndex] = delVelBiasVar[stateIndex-13];
-		    }
+			float delVelBiasVar[3];
+
+			// store all delta velocity bias variances
+			for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
+				delVelBiasVar[stateIndex - 13] = P[stateIndex][stateIndex];
+			}
+
+			// reset all delta velocity bias covariances
+			zeroCols(P, 13, 15);
+
+			// restore all delta velocity bias variances
+			for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
+				P[stateIndex][stateIndex] = delVelBiasVar[stateIndex - 13];
+			}
 		}
 
 		// Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong
 		// calculate accel bias term aligned with the gravity vector
 		float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg;
 		float down_dvel_bias = 0.0f;
-		for (uint8_t axis_index=0; axis_index < 3; axis_index++) {
-			down_dvel_bias += _state.accel_bias(axis_index) * _R_to_earth(2,axis_index);
+
+		for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
+			down_dvel_bias += _state.accel_bias(axis_index) * _R_to_earth(2, axis_index);
 		}
 
 		// check that the vertical componenent of accel bias is consistent with both the vertical position and velocity innovation
@@ -838,7 +846,7 @@ void Ekf::fixCovarianceErrors()
 }
 
 void Ekf::resetMagCovariance()
-{	
+{
 	// set the quaternion covariance terms to zero
 	zeroRows(P,0,3);
 	zeroCols(P,0,3);
@@ -893,5 +901,4 @@ void Ekf::resetWindCovariance()
 		P[23][23] = sq(1.0f);
 
 	}
-
 }

EKF/drag_fusion.cpp

@@ -43,7 +43,7 @@
 #include "mathlib.h"
 
 void Ekf::fuseDrag()
-{ 
+{
 	float SH_ACC[4] = {}; // Variable used to optimise calculations of measurement jacobian
 	float H_ACC[24] = {}; // Observation Jacobian
 	float SK_ACC[9] = {}; // Variable used to optimise calculations of the Kalman gain vector
@@ -85,7 +85,7 @@ void Ekf::fuseDrag()
 	rel_wind = earth_to_body * rel_wind;
 
 	// perform sequential fusion of XY specific forces
-	for (uint8_t axis_index = 0; axis_index <2; axis_index++) {
+	for (uint8_t axis_index = 0; axis_index < 2; axis_index++) {
 		// calculate observation jacobiam and Kalman gain vectors
 		if (axis_index == 0) {
 			// Estimate the airspeed from the measured drag force and ballistic coefficient
@@ -94,7 +94,7 @@ void Ekf::fuseDrag()
 
 			// Estimate the derivative of specific force wrt airspeed along the X axis
 			// Limit lower value to prevent arithmetic exceptions
-			float Kacc = fmaxf(1e-1f,rho * BC_inv_x * airSpd);
+			float Kacc = fmaxf(1e-1f, rho * BC_inv_x * airSpd);
 
 			SH_ACC[0] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
 			SH_ACC[1] = vn - vwn;
@@ -149,11 +149,13 @@ void Ekf::fuseDrag()
 			// calculate the predicted acceleration and innovation measured along the X body axis
 			float drag_sign;
 			if (rel_wind(axis_index) >= 0.0f) {
-			    drag_sign = 1.0f;
+				drag_sign = 1.0f;
+
 			} else {
-			    drag_sign = -1.0f;
+				drag_sign = -1.0f;
 			}
-			float predAccel = -BC_inv_x * 0.5f*rho*sq(rel_wind(axis_index)) * drag_sign;
+
+			float predAccel = -BC_inv_x * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign;
 			_drag_innov[axis_index] = predAccel - mea_acc;
 			_drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]);
 
@@ -164,7 +166,7 @@ void Ekf::fuseDrag()
 
 			// Estimate the derivative of specific force wrt airspeed along the X axis
 			// Limit lower value to prevent arithmetic exceptions
-			float Kacc = fmaxf(1e-1f,rho * BC_inv_y * airSpd);
+			float Kacc = fmaxf(1e-1f, rho * BC_inv_y * airSpd);
 
 			SH_ACC[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
 			SH_ACC[1] = vn - vwn;
@@ -221,11 +223,13 @@ void Ekf::fuseDrag()
 			// calculate the predicted acceleration and innovation measured along the Y body axis
 			float drag_sign;
 			if (rel_wind(axis_index) >= 0.0f) {
-			    drag_sign = 1.0f;
+				drag_sign = 1.0f;
+
 			} else {
-			    drag_sign = -1.0f;
+				drag_sign = -1.0f;
 			}
-			float predAccel = -BC_inv_y * 0.5f*rho*sq(rel_wind(axis_index)) * drag_sign;
+
+			float predAccel = -BC_inv_y * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign;
 			_drag_innov[axis_index] = predAccel - mea_acc;
 			_drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]);
 

EKF/ekf.cpp

@@ -43,21 +43,6 @@
 #include "ekf.h"
 #include "mathlib.h"
 
-#ifndef __PX4_QURT
-#if defined(__cplusplus) && !defined(__PX4_NUTTX)
-#include <cmath>
-#define ISFINITE(x) std::isfinite(x)
-#else
-#define ISFINITE(x) isfinite(x)
-#endif
-#endif
-
-#if defined(__PX4_QURT)
-// Missing math.h defines
-#define ISFINITE(x) __builtin_isfinite(x)
-#endif
-
-
 bool Ekf::init(uint64_t timestamp)
 {
 	bool ret = initialise_interface(timestamp);
@@ -617,7 +602,8 @@ void Ekf::calculateOutputStates()
 			unsigned index = _output_vert_buffer.get_oldest_index();
 			unsigned index_next;
 			unsigned size = _output_vert_buffer.get_length();
-			for (unsigned counter=0; counter < (size - 1); counter++) {
+
+			for (unsigned counter = 0; counter < (size - 1); counter++) {
 				index_next = (index + 1) % size;
 				current_state = _output_vert_buffer.get_from_index(index);
 				next_state = _output_vert_buffer.get_from_index(index_next);
@@ -625,7 +611,7 @@ void Ekf::calculateOutputStates()
 				// correct the velocity
 				if (counter == 0) {
 					current_state.vel_d += vel_d_correction;
-					_output_vert_buffer.push_to_index(index,current_state);
+					_output_vert_buffer.push_to_index(index, current_state);
 				}
 				next_state.vel_d += vel_d_correction;
 
@@ -633,7 +619,7 @@ void Ekf::calculateOutputStates()
 				next_state.vel_d_integ = current_state.vel_d_integ + (current_state.vel_d + next_state.vel_d) * 0.5f * next_state.dt;
 
 				// push the updated data to the buffer
-				_output_vert_buffer.push_to_index(index_next,next_state);
+				_output_vert_buffer.push_to_index(index_next, next_state);
 
 				// advance the index
 				index = (index + 1) % size;
@@ -664,7 +650,7 @@ void Ekf::calculateOutputStates()
 			// loop through the output filter state history and apply the corrections to the velocity and position states
 			outputSample output_states;
 			unsigned max_index = _output_buffer.get_length() - 1;
-			for (unsigned index=0; index <= max_index; index++) {
+			for (unsigned index = 0; index <= max_index; index++) {
 				output_states = _output_buffer.get_from_index(index);
 
 				// a constant  velocity correction is applied
@@ -674,7 +660,7 @@ void Ekf::calculateOutputStates()
 				output_states.pos += pos_correction;
 
 				// push the updated data to the buffer
-				_output_buffer.push_to_index(index,output_states);
+				_output_buffer.push_to_index(index, output_states);
 
 			}
 

EKF/ekf_helper.cpp

@@ -351,10 +351,10 @@ bool Ekf::realignYawGPS()
 		bool badVelInnov = ((_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f)) && _control_status.flags.gps;
 
 		// calculate GPS course over ground angle
-		float gpsCOG = atan2f(_gps_sample_delayed.vel(1),_gps_sample_delayed.vel(0));
+		float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
 
 		// calculate course yaw angle
-		float ekfGOG = atan2f(_state.vel(1),_state.vel(0));
+		float ekfGOG = atan2f(_state.vel(1), _state.vel(0));
 
 		// Check the EKF and GPS course over ground for consistency
 		float courseYawError = gpsCOG - ekfGOG;
@@ -444,10 +444,10 @@ bool Ekf::realignYawGPS()
 			// add the reset amount to the output observer buffered data
 			outputSample output_states;
 			unsigned output_length = _output_buffer.get_length();
-			for (unsigned i=0; i < output_length; i++) {
+			for (unsigned i = 0; i < output_length; i++) {
 				output_states = _output_buffer.get_from_index(i);
 				output_states.quat_nominal *= _state_reset_status.quat_change;
-				_output_buffer.push_to_index(i,output_states);
+				_output_buffer.push_to_index(i, output_states);
 			}
 
 			// apply the change in attitude quaternion to our newest quaternion estimate
@@ -527,10 +527,10 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 		// add the reset amount to the output observer buffered data
 		outputSample output_states;
 		unsigned output_length = _output_buffer.get_length();
-		for (unsigned i=0; i < output_length; i++) {
+		for (unsigned i = 0; i < output_length; i++) {
 			output_states = _output_buffer.get_from_index(i);
 			output_states.quat_nominal *= _state_reset_status.quat_change;
-			_output_buffer.push_to_index(i,output_states);
+			_output_buffer.push_to_index(i, output_states);
 		}
 
 		// apply the change in attitude quaternion to our newest quaternion estimate
@@ -664,7 +664,7 @@ void Ekf::calcMagDeclination()
 	// set source of magnetic declination for internal use
 	if (_flt_mag_align_complete) {
 		// Use value consistent with earth field state
-		_mag_declination = atan2f(_state.mag_I(1),_state.mag_I(0));
+		_mag_declination = atan2f(_state.mag_I(1), _state.mag_I(0));
 	} else if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
 		// use parameter value until GPS is available, then use value returned by geo library
 		if (_NED_origin_initialised) {

EKF/geo.cpp

@@ -42,23 +42,15 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 #ifdef POSIX_SHARED
-//#include <unistd.h>
-//#include <pthread.h>
+
+#include "ecl.h"
+
 #include <stdio.h>
 #include <math.h>
 #include <stdbool.h>
 #include <string.h>
 #include <float.h>
 
-#ifndef __PX4_QURT
-#if defined(__cplusplus) && !defined(__PX4_NUTTX)
-#include <cmath>
-#define ISFINITE(x) std::isfinite(x)
-#else
-#define ISFINITE(x) isfinite(x)
-#endif
-#endif
-
 /****************************************************************************
  *
  *   Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved.
@@ -473,7 +465,7 @@ float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_ne
 	double d_lon = lon_next_rad - lon_now_rad;
 
 	/* conscious mix of double and float trig function to maximize speed and efficiency */
-	float theta = atan2f(sin(d_lon) * cos(lat_next_rad) ,
+	float theta = atan2f(sin(d_lon) * cos(lat_next_rad),
 			     cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
 
 	theta = _wrap_pi(theta);

EKF/mag_fusion.cpp

@@ -207,7 +207,7 @@ void Ekf::fuseMag()
 				Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]);
 				Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]);
 			} else {
-				for (uint8_t i=0; i<16; i++) {
+				for (uint8_t i = 0; i < 16; i++) {
 					Kfusion[i] = 0.0f;
 				}
 				Kfusion[22] = 0.0f;
@@ -261,7 +261,7 @@ void Ekf::fuseMag()
 				Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]);
 				Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]);
 			} else {
-				for (uint8_t i=0; i<16; i++) {
+				for (uint8_t i = 0; i < 16; i++) {
 					Kfusion[i] = 0.0f;
 				}
 				Kfusion[22] = 0.0f;
@@ -315,7 +315,7 @@ void Ekf::fuseMag()
 				Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]);
 				Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]);
 			} else {
-				for (uint8_t i=0; i<16; i++) {
+				for (uint8_t i = 0; i < 16; i++) {
 					Kfusion[i] = 0.0f;
 				}
 				Kfusion[22] = 0.0f;
@@ -523,9 +523,9 @@ void Ekf::fuseHeading()
 		[ cos(pitch)*sin(yaw) + cos(yaw)*sin(pitch)*sin(roll),  cos(roll)*cos(yaw), sin(pitch)*sin(yaw) - cos(pitch)*cos(yaw)*sin(roll)]
 		[                               -cos(roll)*sin(pitch),           sin(roll),                                cos(pitch)*cos(roll)]
 		*/
-		float yaw = atan2f(-_R_to_earth(0, 1) , _R_to_earth(1, 1)); // first rotation (yaw)
+		float yaw = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)); // first rotation (yaw)
 		float roll = asinf(_R_to_earth(2, 1)); // second rotation (roll)
-		float pitch = atan2f(-_R_to_earth(2, 0) , _R_to_earth(2, 2)); // third rotation (pitch)
+		float pitch = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
 
 		predicted_hdg = yaw; // we will need the predicted heading to calculate the innovation
 
@@ -812,7 +812,7 @@ void Ekf::fuseDeclination()
 	Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN);
 
 	// calculate innovation and constrain
-	float innovation = atan2f(magE , magN) - _mag_declination;
+	float innovation = atan2f(magE, magN) - _mag_declination;
 	innovation = math::constrain(innovation, -0.5f, 0.5f);
 
 	// apply covariance correction via P_new = (I -K*H)*P

EKF/optflow_fusion.cpp

@@ -76,7 +76,7 @@ void Ekf::fuseOptFlow()
 	Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
 
 	// calculate the velocity of the sensor reelative to the imu in body frame
-	Vector3f vel_rel_imu_body = cross_product(_flow_sample_delayed.gyroXYZ , pos_offset_body);
+	Vector3f vel_rel_imu_body = cross_product(_flow_sample_delayed.gyroXYZ, pos_offset_body);
 
 	// calculate the velocity of the sensor in the earth frame
 	Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;

ecl.h

@@ -56,3 +56,17 @@
 #define ECL_ERR printf
 
 #endif
+
+#ifndef __PX4_QURT
+#if defined(__cplusplus) && !defined(__PX4_NUTTX)
+#include <cmath>
+#define ISFINITE(x) std::isfinite(x)
+#else
+#define ISFINITE(x) isfinite(x)
+#endif
+#endif
+
+#if defined(__PX4_QURT)
+// Missing math.h defines
+#define ISFINITE(x) __builtin_isfinite(x)
+#endif