dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

The default type for XeY is float
This commit is contained in:
ChristophTobler 2017-11-08 11:40:10 +01:00
parent bba3f70a0e
commit e388e59f32
8 changed files with 26 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/RingBuffer.h
View File

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

2
EKF/airspeed_fusion.cpp
View File

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

10
EKF/common.h
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.
// 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 {

26
EKF/control.cpp
View File

@ -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();

4
EKF/covariance.cpp
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
// 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

2
EKF/ekf_helper.cpp
View File

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

3
EKF/gps_checks.cpp
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
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
View File

@ -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 {