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

EKF: Use #define function for isfinite check

Browse Source
This commit is contained in:
Paul Riseborough
2018-09-19 06:32:43 +10:00
committed by Paul Riseborough
parent 91f886cb5e
commit d1f3f4c916
2 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+1 -1
View File
@@ -513,7 +513,7 @@ void Ekf::controlGpsFusion()
// GPS yaw aiding selection logic // GPS yaw aiding selection logic
if ((_params.fusion_mode & MASK_USE_GPSYAW) if ((_params.fusion_mode & MASK_USE_GPSYAW)
&& isfinite(_gps_sample_delayed.yaw) && ISFINITE(_gps_sample_delayed.yaw)
&& _control_status.flags.tilt_align && _control_status.flags.tilt_align
&& (!_control_status.flags.gps_yaw || !_control_status.flags.yaw_align) && (!_control_status.flags.gps_yaw || !_control_status.flags.yaw_align)
&& (_time_last_imu - _time_last_gps < 2 * GPS_MAX_INTERVAL)) { && (_time_last_imu - _time_last_gps < 2 * GPS_MAX_INTERVAL)) {
EKF/gps_yaw_fusion.cpp
+2 -2
View File
@@ -59,7 +59,7 @@ void Ekf::fuseGpsAntYaw()
float measured_hdg; float measured_hdg;
// check if data has been set to NAN indicating no measurement // check if data has been set to NAN indicating no measurement
if (isfinite(_gps_sample_delayed.yaw)) { if (ISFINITE(_gps_sample_delayed.yaw)) {
// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement // calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
measured_hdg = _gps_sample_delayed.yaw + _gps_yaw_offset; measured_hdg = _gps_sample_delayed.yaw + _gps_yaw_offset;
@@ -293,7 +293,7 @@ void Ekf::fuseGpsAntYaw()
bool Ekf::resetGpsAntYaw() bool Ekf::resetGpsAntYaw()
{ {
// check if data has been set to NAN indicating no measurement // check if data has been set to NAN indicating no measurement
if (isfinite(_gps_sample_delayed.yaw)) { if (ISFINITE(_gps_sample_delayed.yaw)) {
// define the predicted antenna array vector and rotate into earth frame // define the predicted antenna array vector and rotate into earth frame
Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f}; Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};