From 3bb0008af59835fc331e94ca5d2abcc5c329303b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Feb 2015 12:21:33 +0100 Subject: Ashtech driver: Avoid unnecessary double precision conversion calls --- src/lib/mathlib/math/Quaternion.hpp | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) (limited to 'src/lib') diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index b3cca30c6..bf6e3365d 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * Pavel Kirienko - * Lorenz Meier + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,6 +35,10 @@ * @file Quaternion.hpp * * Quaternion class + * + * @author Anton Babushkin + * @author Pavel Kirienko + * @author Lorenz Meier */ #ifndef QUATERNION_HPP @@ -124,10 +125,14 @@ public: double sinTheta_2 = sin(double(pitch) / 2.0); double cosPsi_2 = cos(double(yaw) / 2.0); double sinPsi_2 = sin(double(yaw) / 2.0); - data[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2; - data[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2; - data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2; - data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2; + + /* operations executed in double to avoid loss of precision through + * consecutive multiplications. Result stored as float. + */ + data[0] = static_cast(cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2); + data[1] = static_cast(sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2); + data[2] = static_cast(cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2); + data[3] = static_cast(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2); } void from_dcm(const Matrix<3, 3> &m) { -- cgit v1.2.3