From e388e59f326d12d0bb5cad5d128a258bd866e18c Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Wed, 8 Nov 2017 11:40:10 +0100 Subject: [PATCH] EKF: use uint64_t cast for XeY to avoid float casting of variables The default type for XeY is float --- EKF/RingBuffer.h | 2 +- EKF/airspeed_fusion.cpp | 2 +- EKF/common.h | 10 +++++----- EKF/control.cpp | 26 +++++++++++++------------- EKF/covariance.cpp | 4 ++-- EKF/ekf_helper.cpp | 2 +- EKF/gps_checks.cpp | 3 +-- EKF/terrain_estimator.cpp | 4 ++-- 8 files changed, 26 insertions(+), 27 deletions(-) diff --git a/EKF/RingBuffer.h b/EKF/RingBuffer.h index f318800d1c..10d18127dc 100644 --- a/EKF/RingBuffer.h +++ b/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(sample), static_cast(&_buffer[index]), sizeof(*sample)); diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 6d8ae81132..fb9d53dee3 100644 --- a/EKF/airspeed_fusion.cpp +++ b/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); diff --git a/EKF/common.h b/EKF/common.h index a998987e1d..b5e370c99c 100644 --- a/EKF/common.h +++ b/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 BARO_MAX_INTERVAL 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 EV_MAX_INTERVAL 2e5 ///< Maximum allowable time interval between external vision system measurements (uSec) +#define GPS_MAX_INTERVAL (uint64_t)5e5 ///< Maximum allowable time interval between GPS measurements (uSec) +#define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec) +#define RNG_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between range finder 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 { diff --git a/EKF/control.cpp b/EKF/control.cpp index 5daa3e92ca..f238d12fff 100644 --- a/EKF/control.cpp +++ b/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 sideslip_timed_out = _time_last_imu - _time_last_beta_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 > (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 airspeed_timed_out = _time_last_imu - _time_last_arsp_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 > (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(); diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 33bfbc2fe7..2f7c06ef5c 100644 --- a/EKF/covariance.cpp +++ b/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 diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 4c49905671..05bb3d775d 100644 --- a/EKF/ekf_helper.cpp +++ b/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; } diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 6f7b8afaf7..e43f17dc49 100644 --- a/EKF/gps_checks.cpp +++ b/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); } - diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index da6b56755a..7223ba510d 100644 --- a/EKF/terrain_estimator.cpp +++ b/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 {