From 929ddb56857f473db4f051d3b83fe5d4d88a4898 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 11 Mar 2015 23:42:37 -0700 Subject: [PATCH] AP_Compass: fix a math error in the compass calibrator --- libraries/AP_Compass/CompassCalibrator.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index 227863898a..4c83716355 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -350,7 +350,7 @@ void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const param_t& float length = (softiron*(sample+offset)).length(); // 0: radius - ret[0] = 1; + ret[0] = 1.0f; // 1-3: offsets ret[1] = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C))/length); ret[2] = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C))/length); @@ -464,7 +464,7 @@ void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const param ret[4] = -1.0f * ((sample.y + offset.y) * B)/length; ret[5] = -1.0f * ((sample.z + offset.z) * C)/length; // 6-8: off-diagonals - ret[6] = -1.0f * (((sample.y + offset.y) * A) + ((sample.z + offset.z) * B))/length; + ret[6] = -1.0f * (((sample.y + offset.y) * A) + ((sample.x + offset.x) * B))/length; ret[7] = -1.0f * (((sample.z + offset.z) * A) + ((sample.x + offset.x) * C))/length; ret[8] = -1.0f * (((sample.z + offset.z) * B) + ((sample.y + offset.y) * C))/length; }