From 7c158aa59bd5e849111a273b1e5084b2c698c3f1 Mon Sep 17 00:00:00 2001 From: waltjohnson Date: Thu, 19 May 2016 09:58:18 -0600 Subject: [PATCH 1/3] Addressed C99 compiler issues of initializing variables in header and unused included statements. --- EKF/ekf.cpp | 3 +++ EKF/ekf.h | 4 ++-- EKF/estimator_interface.cpp | 2 +- EKF/estimator_interface.h | 10 +++++----- EKF/geo.cpp | 5 +++-- EKF/mathlib.cpp | 2 +- EKF/mathlib.h | 12 ++++++++---- 7 files changed, 23 insertions(+), 15 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index d0d0025b2e..f37adb933d 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -57,6 +57,9 @@ #endif +const float Ekf::_k_earth_rate = 0.000072921f; +const float Ekf::_gravity_mss = 9.80665f; + Ekf::Ekf(): _filter_initialised(false), _earth_rate_initialised(false), diff --git a/EKF/ekf.h b/EKF/ekf.h index 55ffa5c363..f276f6ed75 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -133,8 +133,8 @@ public: private: static const uint8_t _k_num_states = 24; - const float _k_earth_rate = 0.000072921f; - const float _gravity_mss = 9.80665f; + static const float _k_earth_rate; + static const float _gravity_mss; float _dt_ekf_avg; // average update rate of the ekf diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 81fb4d13cc..fe2dc0ba76 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -226,7 +226,7 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float *data) } if (time_usec - _time_last_range > 45000) { - rangeSample range_sample_new; + rangeSample range_sample_new = {}; range_sample_new.rng = *data; rng = *data; range_sample_new.time_us -= _params.range_delay_ms * 1000; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index e4c09b125f..ddc06aa5d4 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -260,11 +260,11 @@ protected: bool _imu_updated; // true if the ekf should update (completed downsampling process) bool _initialised; // true if the ekf interface instance (data buffering) is initialized - bool _NED_origin_initialised = false; - bool _gps_speed_valid = false; - float _gps_origin_eph = 0.0f; // horizontal position uncertainty of the GPS origin - float _gps_origin_epv = 0.0f; // vertical position uncertainty of the GPS origin - struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians) + bool _NED_origin_initialised; + bool _gps_speed_valid; + float _gps_origin_eph; // horizontal position uncertainty of the GPS origin + float _gps_origin_epv; // vertical position uncertainty of the GPS origin + struct map_projection_reference_s _pos_ref; // Contains WGS-84 position latitude and longitude (radians) bool _mag_healthy; // computed by mag innovation test bool _airspeed_healthy; // computed by airspeed innovation test diff --git a/EKF/geo.cpp b/EKF/geo.cpp index 55cd5ca47f..1966e21743 100644 --- a/EKF/geo.cpp +++ b/EKF/geo.cpp @@ -42,8 +42,8 @@ * @author Anton Babushkin */ #ifdef POSIX_SHARED -#include -#include +//#include +//#include #include #include #include @@ -95,6 +95,7 @@ * */ +#include #include "geo.h" /** set this always to the sampling in degrees for the table below */ diff --git a/EKF/mathlib.cpp b/EKF/mathlib.cpp index 2cd1190cf0..c72b959730 100644 --- a/EKF/mathlib.cpp +++ b/EKF/mathlib.cpp @@ -56,7 +56,7 @@ float max(float val1, float val2) return (val1 > val2) ? val1 : val2; } -float constrain(float &val, float min, float max) +float constrain(float val, float min, float max) { return (val < min) ? min : ((val > max) ? max : val); } diff --git a/EKF/mathlib.h b/EKF/mathlib.h index a20adad331..0c96a8f9d4 100644 --- a/EKF/mathlib.h +++ b/EKF/mathlib.h @@ -41,18 +41,22 @@ #ifndef MATHLIB_H #define MATHLIB_H #ifdef POSIX_SHARED -#include -#include +// #include +// #include #define M_PI_F 3.14159265358979323846f +#ifndef M_PI +#define M_PI (3.14159265358979323846f) +#endif + namespace math { -using namespace Eigen; +// using namespace Eigen; using namespace std; float min(float val1, float val2); float max(float val1, float val2); -float constrain(float &val, float min, float max); +float constrain(float val, float min, float max); float radians(float degrees); float degrees(float radians); From 6eaa00e3f93a7df5a0c4d87ae4cbdd24b800fa6c Mon Sep 17 00:00:00 2001 From: waltjohnson Date: Thu, 19 May 2016 10:07:45 -0600 Subject: [PATCH 2/3] isfinite() is not part of the math class, rather the math.h header file. --- EKF/geo.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/EKF/geo.cpp b/EKF/geo.cpp index 1966e21743..bb33d9ebf5 100644 --- a/EKF/geo.cpp +++ b/EKF/geo.cpp @@ -709,7 +709,7 @@ float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now, float _wrap_pi(float bearing) { /* value is inf or NaN */ - if (!math::isfinite(bearing)) { + if (!isfinite(bearing)) { return bearing; } @@ -739,7 +739,7 @@ float _wrap_pi(float bearing) float _wrap_2pi(float bearing) { /* value is inf or NaN */ - if (!math::isfinite(bearing)) { + if (!isfinite(bearing)) { return bearing; } @@ -769,7 +769,7 @@ float _wrap_2pi(float bearing) float _wrap_180(float bearing) { /* value is inf or NaN */ - if (!math::isfinite(bearing)) { + if (!isfinite(bearing)) { return bearing; } @@ -799,7 +799,7 @@ float _wrap_180(float bearing) float _wrap_360(float bearing) { /* value is inf or NaN */ - if (!math::isfinite(bearing)) { + if (!isfinite(bearing)) { return bearing; } From a9ca8c294c9d6f5901fe38f9b1920cdfde9bd451 Mon Sep 17 00:00:00 2001 From: waltjohnson Date: Thu, 19 May 2016 10:34:16 -0600 Subject: [PATCH 3/3] Generalized isfinite() in similar form as done in ekf.cpp https://github.com/PX4/ecl/blob/e3b9800cac459e7c279395c18191a872f465f848/EKF/ekf.cpp#L45-L52 --- EKF/geo.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/EKF/geo.cpp b/EKF/geo.cpp index bb33d9ebf5..a1fa93ab1b 100644 --- a/EKF/geo.cpp +++ b/EKF/geo.cpp @@ -50,6 +50,15 @@ #include #include +#ifndef __PX4_QURT +#if defined(__cplusplus) && !defined(__PX4_NUTTX) +#include +#define ISFINITE(x) std::isfinite(x) +#else +#define ISFINITE(x) isfinite(x) +#endif +#endif + /**************************************************************************** * * Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved. @@ -709,7 +718,7 @@ float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now, float _wrap_pi(float bearing) { /* value is inf or NaN */ - if (!isfinite(bearing)) { + if (!ISFINITE(bearing)) { return bearing; } @@ -739,7 +748,7 @@ float _wrap_pi(float bearing) float _wrap_2pi(float bearing) { /* value is inf or NaN */ - if (!isfinite(bearing)) { + if (!ISFINITE(bearing)) { return bearing; } @@ -769,7 +778,7 @@ float _wrap_2pi(float bearing) float _wrap_180(float bearing) { /* value is inf or NaN */ - if (!isfinite(bearing)) { + if (!ISFINITE(bearing)) { return bearing; } @@ -799,7 +808,7 @@ float _wrap_180(float bearing) float _wrap_360(float bearing) { /* value is inf or NaN */ - if (!isfinite(bearing)) { + if (!ISFINITE(bearing)) { return bearing; }