zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

The default type for XeY is float
master
ChristophTobler 7 years ago
parent
bba3f70a0e
commit
e388e59f32
8 changed files with 26 additions and 27 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      EKF/RingBuffer.h
  2. 2
      EKF/airspeed_fusion.cpp
  3. 10
      EKF/common.h
  4. 26
      EKF/control.cpp
  5. 4
      EKF/covariance.cpp
  6. 2
      EKF/ekf_helper.cpp
  7. 3
      EKF/gps_checks.cpp
  8. 4
      EKF/terrain_estimator.cpp

2
EKF/RingBuffer.h
Unescape View File

@ -130,7 +130,7 @@ public:
int index = (_head - i); int index = (_head - i);
index = index < 0 ? _size + index : index; 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 // TODO Re-evaluate the static cast and usage patterns
memcpy(static_cast<void *>(sample), static_cast<void *>(&_buffer[index]), sizeof(*sample)); memcpy(static_cast<void *>(sample), static_cast<void *>(&_buffer[index]), sizeof(*sample));

2
EKF/airspeed_fusion.cpp
Unescape View File

@ -259,7 +259,7 @@ void Ekf::resetWindStates()
Eulerf euler321(_state.quat_nominal); Eulerf euler321(_state.quat_nominal);
float euler_yaw = euler321(2); 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 // 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(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); _state.wind_vel(1) = _state.vel(1) - _airspeed_sample_delayed.true_airspeed * sinf(euler_yaw);

10
EKF/common.h
Unescape View File

@ -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. #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 // 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 // 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) #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 { struct parameters {

26
EKF/control.cpp
Unescape View File

@ -261,7 +261,7 @@ void Ekf::controlExternalVisionFusion()
fuseHeading(); 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 // Turn off EV fusion mode if no data has been received
_control_status.flags.ev_pos = false; _control_status.flags.ev_pos = false;
ECL_INFO("EKF External Vision Data Stopped"); 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 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.opt_flow // we are not yet using flow data
&& _control_status.flags.tilt_align // we know our tilt attitude && _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 // 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); _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"); ECL_INFO("EKF Optical Flow Data Stopped");
_control_status.flags.opt_flow = false; _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 // 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 // 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 ((_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 the heading is not aligned, reset the yaw and magnetic field states
if (!_control_status.flags.yaw_align) { if (!_control_status.flags.yaw_align) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); _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; _control_status.flags.gps = false;
ECL_WARN("EKF GPS data stopped"); 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); 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 // 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 // reset the vertical position and velocity states
if ((P[9][9] > sq(_params.hgt_reset_lim)) && (hgt_fusion_timeout || continuous_bad_accel_hgt)) { 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() 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) { _control_status.flags.in_air) {
_rng_stuck = true; _rng_stuck = true;
@ -969,8 +969,8 @@ 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 // 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)) { if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
_control_status.flags.wind = false; _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 // 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 // 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)) { if(_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
_control_status.flags.wind = false; _control_status.flags.wind = false;
} }
@ -1244,10 +1244,10 @@ void Ekf::controlVelPosFusion()
!_control_status.flags.opt_flow && !_control_status.flags.opt_flow &&
!_control_status.flags.ev_pos && !_control_status.flags.ev_pos &&
!(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta) && !(_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 // 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(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1); _last_known_posNE(1) = _state.pos(1);
_state.vel.setZero(); _state.vel.setZero();

4
EKF/covariance.cpp
Unescape View File

@ -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 // 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 // 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 varX = P[13][13];
float varY = P[14][14]; float varY = P[14][14];
float varZ = P[15][15]; float varZ = P[15][15];
@ -870,7 +870,7 @@ void Ekf::resetWindCovariance()
zeroRows(P,22,23); zeroRows(P,22,23);
zeroCols(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 // Use airspeed and zer sideslip assumption to set initial covariance values for wind states
// calculate the wind speed and bearing // calculate the wind speed and bearing

2
EKF/ekf_helper.cpp
Unescape View File

@ -1041,7 +1041,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl)
bool Ekf::reset_imu_bias() 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; return false;
} }

3
EKF/gps_checks.cpp
Unescape View File

@ -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 // 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);
} }

4
EKF/terrain_estimator.cpp
Unescape View File

@ -47,7 +47,7 @@ bool Ekf::initHagl()
// get most recent range measurement from buffer // get most recent range measurement from buffer
rangeSample latest_measurement = _range_buffer.get_newest(); 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 // 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; _terrain_vpos = _state.pos(2) + latest_measurement.rng * _R_rng_to_earth_2_2;
// initialise state variance to variance of measurement // initialise state variance to variance of measurement
@ -159,7 +159,7 @@ void Ekf::fuseHagl()
bool Ekf::get_terrain_valid() bool Ekf::get_terrain_valid()
{ {
if (_terrain_initialised && _range_data_continuous && !_rng_stuck && 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; return true;
} else { } else {

Write Preview
Loading…
Cancel
Save