Merge pull request #353 from PX4/pr-expo_int_cast

EKF: use uint64_t cast for XeY to avoid float casting of variables

EKF/RingBuffer.h

@@ -130,7 +130,7 @@ public:
 			int index = (_head - i);
 			index = index < 0 ? _size + index : index;
 
-			if (timestamp >= _buffer[index].time_us && timestamp - _buffer[index].time_us < 100000) {
+			if (timestamp >= _buffer[index].time_us && timestamp - _buffer[index].time_us < (uint64_t)1e5) {
 
 				// TODO Re-evaluate the static cast and usage patterns
 				memcpy(static_cast<void *>(sample), static_cast<void *>(&_buffer[index]), sizeof(*sample));

EKF/airspeed_fusion.cpp

@@ -259,7 +259,7 @@ void Ekf::resetWindStates()
 	Eulerf euler321(_state.quat_nominal);
 	float euler_yaw = euler321(2);
 
-	if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < 5e5)) {
+	if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
 		// estimate wind using zero sideslip assumption and airspeed measurement if airspeed available
 		_state.wind_vel(0) = _state.vel(0) - _airspeed_sample_delayed.true_airspeed * cosf(euler_yaw);
 		_state.wind_vel(1) = _state.vel(1) - _airspeed_sample_delayed.true_airspeed * sinf(euler_yaw);

EKF/common.h

@@ -182,13 +182,13 @@ struct dragSample {
 #define MAG_FUSE_TYPE_AUTOFW    3	///< The same as option 0, but if fusing airspeed, magnetometer fusion is only allowed to modify the magnetic field states.
 
 // Maximum sensor intervals in usec
-#define GPS_MAX_INTERVAL	5e5	///< Maximum allowable time interval between GPS measurements (uSec)
+#define GPS_MAX_INTERVAL  (uint64_t)5e5	///< Maximum allowable time interval between GPS measurements (uSec)
-#define BARO_MAX_INTERVAL	2e5	///< Maximum allowable time interval between pressure altitude measurements (uSec)
+#define BARO_MAX_INTERVAL (uint64_t)2e5	///< Maximum allowable time interval between pressure altitude measurements (uSec)
-#define RNG_MAX_INTERVAL	2e5	///< Maximum allowable time interval between range finder  measurements (uSec)
+#define RNG_MAX_INTERVAL  (uint64_t)2e5	///< Maximum allowable time interval between range finder  measurements (uSec)
-#define EV_MAX_INTERVAL		2e5	///< Maximum allowable time interval between external vision system measurements (uSec)
+#define EV_MAX_INTERVAL   (uint64_t)2e5	///< Maximum allowable time interval between external vision system measurements (uSec)
 
 // bad accelerometer detection and mitigation
-#define BADACC_PROBATION	10E6	///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
+#define BADACC_PROBATION  (uint64_t)10e6	///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
 #define BADACC_BIAS_PNOISE	4.9f	///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2)
 
 struct parameters {

EKF/control.cpp

@@ -261,7 +261,7 @@ void Ekf::controlExternalVisionFusion()
 			fuseHeading();
 
 		}
-	} else if (_control_status.flags.ev_pos && (_time_last_imu - _time_last_ext_vision > 5e6)) {
+	} else if (_control_status.flags.ev_pos && (_time_last_imu - _time_last_ext_vision > (uint64_t)5e6)) {
 		// Turn off EV fusion mode if no data has been received
 		_control_status.flags.ev_pos = false;
 		ECL_INFO("EKF External Vision Data Stopped");
@@ -278,7 +278,7 @@ void Ekf::controlOpticalFlowFusion()
 		if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user
 				&& !_control_status.flags.opt_flow // we are not yet using flow data
 				&& _control_status.flags.tilt_align // we know our tilt attitude
-				&& (_time_last_imu - _time_last_hagl_fuse) < 5e5) // we have a valid distance to ground estimate
+				&& (_time_last_imu - _time_last_hagl_fuse) < (uint64_t)5e5) // we have a valid distance to ground estimate
 		{
 
 			// If the heading is not aligned, reset the yaw and magnetic field states
@@ -372,7 +372,7 @@ void Ekf::controlOpticalFlowFusion()
 			_last_known_posNE(1) = _state.pos(1);
 
 		}
-	} else if (_control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow > 5e6)) {
+	} else if (_control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow > (uint64_t)5e6)) {
 		ECL_INFO("EKF Optical Flow Data Stopped");
 		_control_status.flags.opt_flow = false;
 
@@ -388,7 +388,7 @@ void Ekf::controlGpsFusion()
 		// Determine if we should use GPS aiding for velocity and horizontal position
 		// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
 		if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) {
-			if (_control_status.flags.tilt_align && _NED_origin_initialised && (_time_last_imu - _last_gps_fail_us > 5e6)) {
+			if (_control_status.flags.tilt_align && _NED_origin_initialised && (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6)) {
 				// If the heading is not aligned, reset the yaw and magnetic field states
 				if (!_control_status.flags.yaw_align) {
 					_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
@@ -471,7 +471,7 @@ void Ekf::controlGpsFusion()
 
 		}
 
-	} else if (_control_status.flags.gps && (_time_last_imu - _time_last_gps > 10e6)) {
+	} else if (_control_status.flags.gps && (_time_last_imu - _time_last_gps > (uint64_t)10e6)) {
 			_control_status.flags.gps = false;
 			ECL_WARN("EKF GPS data stopped");
 
@@ -515,7 +515,7 @@ void Ekf::controlHeightSensorTimeouts()
 	bool continuous_bad_accel_hgt = ((_time_last_imu - _time_good_vert_accel) > (unsigned)_params.bad_acc_reset_delay_us);
 
 	// check if height has been inertial deadreckoning for too long
-	bool hgt_fusion_timeout = ((_time_last_imu - _time_last_hgt_fuse) > 5e6);
+	bool hgt_fusion_timeout = ((_time_last_imu - _time_last_hgt_fuse) > (uint64_t)5e6);
 
 	// reset the vertical position and velocity states
 	if ((P[9][9] > sq(_params.hgt_reset_lim)) && (hgt_fusion_timeout || continuous_bad_accel_hgt)) {
@@ -936,7 +936,7 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
 
 void Ekf::checkForStuckRange()
 {
-	if (_range_data_ready && _range_sample_delayed.time_us - _time_last_rng_ready > 10e6 &&
+	if (_range_data_ready && _range_sample_delayed.time_us - _time_last_rng_ready > (uint64_t)10e6 &&
 			_control_status.flags.in_air) {
 		_rng_stuck = true;
 
@@ -969,8 +969,8 @@ void Ekf::controlAirDataFusion()
 	// 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 airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > (uint64_t)10e6;
-	bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6;
+	bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > (uint64_t)10e6;
 
 	if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
 		_control_status.flags.wind = false;
@@ -1012,8 +1012,8 @@ void Ekf::controlBetaFusion()
 	// control activation and initialisation/reset of wind states required for synthetic sideslip fusion 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 sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6;
+	bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > (uint64_t)10e6;
-	bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6;
+	bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > (uint64_t)10e6;
 	if(_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
 		_control_status.flags.wind = false;
 	}
@@ -1244,10 +1244,10 @@ void Ekf::controlVelPosFusion()
 			!_control_status.flags.opt_flow &&
 			!_control_status.flags.ev_pos &&
 			!(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta) &&
-			((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height))
+			((_time_last_imu - _time_last_fake_gps > (uint64_t)2e5) || _fuse_height))
 	{
 		// Reset position and velocity states if we re-commence this aiding method
-		if ((_time_last_imu - _time_last_fake_gps) > 4E5) {
+		if ((_time_last_imu - _time_last_fake_gps) > (uint64_t)4e5) {
 			_last_known_posNE(0) = _state.pos(0);
 			_last_known_posNE(1) = _state.pos(1);
 			_state.vel.setZero();

EKF/covariance.cpp

@@ -799,7 +799,7 @@ void Ekf::fixCovarianceErrors()
 
 		// if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of
 		// the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue
-		if (_time_last_imu - _time_acc_bias_check > 7E6) {
+		if (_time_last_imu - _time_acc_bias_check > (uint64_t)7e6) {
 			float varX = P[13][13];
 			float varY = P[14][14];
 			float varZ = P[15][15];
@@ -870,7 +870,7 @@ void Ekf::resetWindCovariance()
 	zeroRows(P,22,23);
 	zeroCols(P,22,23);
 
-	if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < 5e5)) {
+	if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
 		// Use airspeed and zer sideslip assumption to set initial covariance values for wind states
 
 		// calculate the wind speed and bearing

EKF/ekf_helper.cpp

@@ -1041,7 +1041,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl)
 
 bool Ekf::reset_imu_bias()
 {
-	if (_imu_sample_delayed.time_us - _last_imu_bias_cov_reset_us < 10 * (1000 * 1000)) {
+	if (_imu_sample_delayed.time_us - _last_imu_bias_cov_reset_us < (uint64_t)10e6) {
 		return false;
 
 	}

EKF/gps_checks.cpp

@@ -232,6 +232,5 @@ bool Ekf::gps_is_good(struct gps_message *gps)
 	}
 
 	// continuous period without fail of 10 seconds required to return a healthy status
-	return (_time_last_imu - _last_gps_fail_us > 1e7);
+	return (_time_last_imu - _last_gps_fail_us > (uint64_t)1e7);
 }
-

EKF/terrain_estimator.cpp

@@ -47,7 +47,7 @@ bool Ekf::initHagl()
 	// get most recent range measurement from buffer
 	rangeSample latest_measurement = _range_buffer.get_newest();
 
-	if ((_time_last_imu - latest_measurement.time_us) < 2e5 && _R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
+	if ((_time_last_imu - latest_measurement.time_us) < (uint64_t)2e5 && _R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
 		// if we have a fresh measurement, use it to initialise the terrain estimator
 		_terrain_vpos = _state.pos(2) + latest_measurement.rng * _R_rng_to_earth_2_2;
 		// initialise state variance to variance of measurement
@@ -159,7 +159,7 @@ void Ekf::fuseHagl()
 bool Ekf::get_terrain_valid()
 {
 	if (_terrain_initialised && _range_data_continuous && !_rng_stuck &&
-		  (_time_last_imu - _time_last_hagl_fuse < 5E6)) {
+		  (_time_last_imu - _time_last_hagl_fuse < (uint64_t)5e6)) {
 		return true;
 
 	} else {