From 081beacb8d49a854f73c3f0c278e904e5bb70316 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 23 Jan 2016 10:10:17 +0900 Subject: [PATCH] AP_InertialSensor: replace sqrt with safe_sqrt to resolve compiler warning Also add suppressing comment for missing break at end of switch --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index f4d6238a49..e7c3d2df2f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1334,7 +1334,7 @@ void AP_InertialSensor::_acal_save_calibrations() // determine trim from aligned sample vs misaligned sample Vector3f cross = (misaligned_sample%aligned_sample); float dot = (misaligned_sample*aligned_sample); - Quaternion q(sqrt(sq(misaligned_sample.length())*sq(aligned_sample.length()))+dot, cross.x, cross.y, cross.z); + Quaternion q(safe_sqrt(sq(misaligned_sample.length())*sq(aligned_sample.length()))+dot, cross.x, cross.y, cross.z); q.normalize(); _trim_roll = q.get_euler_roll(); _trim_pitch = q.get_euler_pitch(); @@ -1343,7 +1343,7 @@ void AP_InertialSensor::_acal_save_calibrations() break; default: _new_trim = false; - //noop + /* no break */ } if (fabsf(_trim_roll) > radians(10) ||