diff options
| author | Andrew Chambers <achamber@gmail.com> | 2014-07-03 11:32:27 -0700 |
|---|---|---|
| committer | Andrew Chambers <achamber@gmail.com> | 2014-07-03 11:32:27 -0700 |
| commit | c6c33142ceb6bf59b8c9b8e32e94ae5ea7959dbd (patch) | |
| tree | dd5a9ebd9d2bdc79e6ef252d093c068bd02aa0d8 /src/modules/uavcan/gnss_receiver.cpp | |
| parent | 6c5e3d53412fa1cdad687818328b3bfc1a83e9ca (diff) | |
| download | px4-firmware-c6c33142ceb6bf59b8c9b8e32e94ae5ea7959dbd.tar.gz px4-firmware-c6c33142ceb6bf59b8c9b8e32e94ae5ea7959dbd.tar.bz2 px4-firmware-c6c33142ceb6bf59b8c9b8e32e94ae5ea7959dbd.zip | |
Using proper math library. Corrected speed variance calculation
Diffstat (limited to 'src/modules/uavcan/gnss_receiver.cpp')
| -rw-r--r-- | src/modules/uavcan/gnss_receiver.cpp | 5 |
1 files changed, 3 insertions, 2 deletions
diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index 65a7b4a2a..3e98bdf14 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -41,6 +41,7 @@ #include "gnss_receiver.hpp" #include <systemlib/err.h> +#include <mathlib/mathlib.h> #define MM_PER_CM 10 // Millimeters per centimeter @@ -86,7 +87,7 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav if (valid_position_covariance) { float pos_cov[9]; msg.position_covariance.unpackSquareMatrix(pos_cov); - _report.p_variance_m = std::max(pos_cov[0], pos_cov[4]); + _report.p_variance_m = math::max(pos_cov[0], pos_cov[4]); _report.eph_m = sqrtf(_report.p_variance_m); } else { _report.p_variance_m = -1.0; @@ -96,7 +97,7 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav if (valid_velocity_covariance) { float vel_cov[9]; msg.velocity_covariance.unpackSquareMatrix(vel_cov); - _report.s_variance_m_s = vel_cov[0] + vel_cov[4] + vel_cov[8]; + _report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]); /* There is a nonlinear relationship between the velocity vector and the heading. * Use Jacobian to transform velocity covariance to heading covariance |