fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-31 02:51:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Fix isfinite calls

Browse Source
This commit is contained in:
Lorenz Meier
2015-06-13 11:31:55 +02:00
parent b06a533555
commit 9d0d6ba2bf
1 changed files with 16 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+16 -16
View File
@@ -727,9 +727,9 @@ void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp,
double lat, double lon, float gps_alt, float baro_alt) double lat, double lon, float gps_alt, float baro_alt)
{ {
// Reference altitude // Reference altitude
if (isfinite(_ekf->states[9])) { if (PX4_ISFINITE(_ekf->states[9])) {
_filter_ref_offset = _ekf->states[9]; _filter_ref_offset = _ekf->states[9];
} else if (isfinite(-_ekf->hgtMea)) { } else if (PX4_ISFINITE(-_ekf->hgtMea)) {
_filter_ref_offset = -_ekf->hgtMea; _filter_ref_offset = -_ekf->hgtMea;
} else { } else {
_filter_ref_offset = -_baro.altitude; _filter_ref_offset = -_baro.altitude;
@@ -862,12 +862,12 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
_local_pos.z_global = false; _local_pos.z_global = false;
_local_pos.yaw = _att.yaw; _local_pos.yaw = _att.yaw;
if (!isfinite(_local_pos.x) || if (!PX4_ISFINITE(_local_pos.x) ||
!isfinite(_local_pos.y) || !PX4_ISFINITE(_local_pos.y) ||
!isfinite(_local_pos.z) || !PX4_ISFINITE(_local_pos.z) ||
!isfinite(_local_pos.vx) || !PX4_ISFINITE(_local_pos.vx) ||
!isfinite(_local_pos.vy) || !PX4_ISFINITE(_local_pos.vy) ||
!isfinite(_local_pos.vz)) !PX4_ISFINITE(_local_pos.vz))
{ {
// bad data, abort publication // bad data, abort publication
return; return;
@@ -921,12 +921,12 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
_global_pos.eph = _gps.eph; _global_pos.eph = _gps.eph;
_global_pos.epv = _gps.epv; _global_pos.epv = _gps.epv;
if (!isfinite(_global_pos.lat) || if (!PX4_ISFINITE(_global_pos.lat) ||
!isfinite(_global_pos.lon) || !PX4_ISFINITE(_global_pos.lon) ||
!isfinite(_global_pos.alt) || !PX4_ISFINITE(_global_pos.alt) ||
!isfinite(_global_pos.vel_n) || !PX4_ISFINITE(_global_pos.vel_n) ||
!isfinite(_global_pos.vel_e) || !PX4_ISFINITE(_global_pos.vel_e) ||
!isfinite(_global_pos.vel_d)) !PX4_ISFINITE(_global_pos.vel_d))
{ {
// bad data, abort publication // bad data, abort publication
return; return;
@@ -1378,7 +1378,7 @@ void AttitudePositionEstimatorEKF::pollData()
// update LPF // update LPF
float filter_step = (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); float filter_step = (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
if (isfinite(filter_step)) { if (PX4_ISFINITE(filter_step)) {
_gps_alt_filt += filter_step; _gps_alt_filt += filter_step;
} }
} }
@@ -1473,7 +1473,7 @@ void AttitudePositionEstimatorEKF::pollData()
_ekf->baroHgt = _baro.altitude; _ekf->baroHgt = _baro.altitude;
float filter_step = (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); float filter_step = (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
if (isfinite(filter_step)) { if (PX4_ISFINITE(filter_step)) {
_baro_alt_filt += filter_step; _baro_alt_filt += filter_step;
} }