fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-02 06:00:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Ashtech driver: Avoid unnecessary double precision conversion calls

Browse Source
This commit is contained in:
Lorenz Meier
2015-02-08 12:21:33 +01:00
parent e5e42650c4
commit 3bb0008af5
3 changed files with 49 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mathlib/math/Quaternion.hpp
+13 -8
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
* Pavel Kirienko <pavel.kirienko@gmail.com>
* Lorenz Meier <lm@inf.ethz.ch>
* 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 <anton.babushkin@me.com>
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* @author Lorenz Meier <lorenz@px4.io>
*/
#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<float>(cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2);
data[1] = static_cast<float>(sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2);
data[2] = static_cast<float>(cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2);
data[3] = static_cast<float>(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2);
}
void from_dcm(const Matrix<3, 3> &m) {