From c6c33142ceb6bf59b8c9b8e32e94ae5ea7959dbd Mon Sep 17 00:00:00 2001
From: Andrew Chambers <achamber@gmail.com>
Date: Thu, 3 Jul 2014 11:32:27 -0700
Subject: [PATCH] Using proper math library. Corrected speed variance
 calculation

---
 src/modules/uavcan/gnss_receiver.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp
index 65a7b4a2a3..3e98bdf144 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