fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 15:27:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Euler, Quaternion: fix compiler errors for GCC 6.1.1 (#23)

Browse Source 
* Euler, Quaternion: fix compiler errors for GCC 6.1.1

GCC output:
error: implicit conversion from ‘float’ to ‘double’ to match other operand
of binary expression [-Werror=double-promotion]

* astyle: fix formatting for Euler.hpp & Quaternion.hpp
This commit is contained in:
Beat Küng
2016-07-05 23:00:35 +02:00
committed by James Goppert
parent 3c87ae78ff
commit b1f76782f6
2 changed files with 71 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Euler.hpp
+24 -17
View File
@@ -54,7 +54,7 @@ public:
*
* @param other vector to copy
*/
Euler(const Vector<Type, 3> & other) :
Euler(const Vector<Type, 3> &other) :
Vector<Type, 3>(other)
{
}
@@ -64,7 +64,7 @@ public:
*
* @param other Matrix31 to copy
*/
Euler(const Matrix<Type, 3, 1> & other) :
Euler(const Matrix<Type, 3, 1> &other) :
Vector<Type, 3>(other)
{
}
@@ -97,19 +97,20 @@ public:
*
* @param dcm Direction cosine matrix
*/
Euler(const Dcm<Type> & dcm) : Vector<Type, 3>()
Euler(const Dcm<Type> &dcm) : Vector<Type, 3>()
{
Type phi_val = Type(atan2(dcm(2,1), dcm(2,2)));
Type theta_val = Type(asin(-dcm(2,0)));
Type psi_val = Type(atan2(dcm(1,0), dcm(0,0)));
Type phi_val = Type(atan2(dcm(2, 1), dcm(2, 2)));
Type theta_val = Type(asin(-dcm(2, 0)));
Type psi_val = Type(atan2(dcm(1, 0), dcm(0, 0)));
Type pi = Type(M_PI);
if (fabs(theta_val - pi/2) < 1.0e-3) {
if (Type(fabs(theta_val - pi / Type(2))) < Type(1.0e-3)) {
phi_val = Type(0.0);
psi_val = Type(atan2(dcm(1,2), dcm(0,2)));
} else if (Type(fabs(theta_val + pi/2)) < Type(1.0e-3)) {
psi_val = Type(atan2(dcm(1, 2), dcm(0, 2)));
} else if (Type(fabs(theta_val + pi / Type(2))) < Type(1.0e-3)) {
phi_val = Type(0.0);
psi_val = Type(atan2(-dcm(1,2), -dcm(0,2)));
psi_val = Type(atan2(-dcm(1, 2), -dcm(0, 2)));
}
phi() = phi_val;
@@ -127,29 +128,35 @@ public:
*
* @param q quaternion
*/
Euler(const Quaternion<Type> & q) :
Euler(const Quaternion<Type> &q) :
Vector<Type, 3>()
{
*this = Euler(Dcm<Type>(q));
}
inline Type phi() const {
inline Type phi() const
{
return (*this)(0);
}
inline Type theta() const {
inline Type theta() const
{
return (*this)(1);
}
inline Type psi() const {
inline Type psi() const
{
return (*this)(2);
}
inline Type & phi() {
inline Type &phi()
{
return (*this)(0);
}
inline Type & theta() {
inline Type &theta()
{
return (*this)(1);
}
inline Type & psi() {
inline Type &psi()
{
return (*this)(2);
}
matrix/Quaternion.hpp
+47 -34
View File
@@ -73,7 +73,7 @@ public:
*
* @param other Matrix41 to copy
*/
Quaternion(const Matrix41 & other) :
Quaternion(const Matrix41 &other) :
Vector<Type, 4>(other)
{
}
@@ -86,18 +86,18 @@ public:
*
* @param dcm dcm to set quaternion to
*/
Quaternion(const Dcm<Type> & dcm) :
Quaternion(const Dcm<Type> &dcm) :
Vector<Type, 4>()
{
Quaternion &q = *this;
q(0) = Type(0.5 * sqrt(1 + dcm(0, 0) +
dcm(1, 1) + dcm(2, 2)));
q(0) = Type(0.5) * Type(sqrt(Type(1) + dcm(0, 0) +
dcm(1, 1) + dcm(2, 2)));
q(1) = Type((dcm(2, 1) - dcm(1, 2)) /
(4 * q(0)));
(Type(4) * q(0)));
q(2) = Type((dcm(0, 2) - dcm(2, 0)) /
(4 * q(0)));
(Type(4) * q(0)));
q(3) = Type((dcm(1, 0) - dcm(0, 1)) /
(4 * q(0)));
(Type(4) * q(0)));
}
/**
@@ -109,7 +109,7 @@ public:
*
* @param euler euler angle instance
*/
Quaternion(const Euler<Type> & euler) :
Quaternion(const Euler<Type> &euler) :
Vector<Type, 4>()
{
Quaternion &q = *this;
@@ -161,10 +161,10 @@ public:
{
const Quaternion &p = *this;
Quaternion r;
r(0) = p(0)*q(0) - p(1)*q(1) - p(2)*q(2) - p(3)*q(3);
r(1) = p(0)*q(1) + p(1)*q(0) - p(2)*q(3) + p(3)*q(2);
r(2) = p(0)*q(2) + p(1)*q(3) + p(2)*q(0) - p(3)*q(1);
r(3) = p(0)*q(3) - p(1)*q(2) + p(2)*q(1) + p(3)*q(0);
r(0) = p(0) * q(0) - p(1) * q(1) - p(2) * q(2) - p(3) * q(3);
r(1) = p(0) * q(1) + p(1) * q(0) - p(2) * q(3) + p(3) * q(2);
r(2) = p(0) * q(2) + p(1) * q(3) + p(2) * q(0) - p(3) * q(1);
r(3) = p(0) * q(3) - p(1) * q(2) + p(2) * q(1) + p(3) * q(0);
return r;
}
@@ -173,7 +173,7 @@ public:
*
* @param other quaternion to multiply with
*/
void operator*=(const Quaternion & other)
void operator*=(const Quaternion &other)
{
Quaternion &self = *this;
self = self * other;
@@ -207,7 +207,8 @@ public:
*
* @param w direction
*/
Matrix41 derivative(const Matrix31 & w) const {
Matrix41 derivative(const Matrix31 &w) const
{
const Quaternion &q = *this;
Type dataQ[] = {
q(0), -q(1), -q(2), -q(3),
@@ -218,16 +219,17 @@ public:
Matrix<Type, 4, 4> Q(dataQ);
Vector<Type, 4> v;
v(0) = 0;
v(1) = w(0,0);
v(2) = w(1,0);
v(3) = w(2,0);
v(1) = w(0, 0);
v(2) = w(1, 0);
v(3) = w(2, 0);
return Q * v * Type(0.5);
}
/**
* Invert quaternion
*/
void invert() {
void invert()
{
Quaternion &q = *this;
q(1) *= -1;
q(2) *= -1;
@@ -239,7 +241,8 @@ public:
*
* @return inverted quaternion
*/
Quaternion inversed() {
Quaternion inversed()
{
Quaternion &q = *this;
Quaternion ret;
ret(0) = q(0);
@@ -254,7 +257,8 @@ public:
*
* @param vec rotation vector
*/
void rotate(const Vector<Type, 3> &vec) {
void rotate(const Vector<Type, 3> &vec)
{
Quaternion res;
res.from_axis_angle(vec);
(*this) = (*this) * res;
@@ -269,16 +273,19 @@ public:
* @param vec rotation vector
* @return quaternion representing the rotation
*/
void from_axis_angle(Vector<Type, 3> vec) {
void from_axis_angle(Vector<Type, 3> vec)
{
Quaternion &q = *this;
Type theta = vec.norm();
if(theta < (Type)1e-10) {
if (theta < (Type)1e-10) {
q(0) = (Type)1.0;
q(1)=q(2)=q(3)=0;
q(1) = q(2) = q(3) = 0;
return;
}
vec /= theta;
from_axis_angle(vec,theta);
from_axis_angle(vec, theta);
}
/**
@@ -288,15 +295,18 @@ public:
* @param theta scalar describing angle of rotation
* @return quaternion representing the rotation
*/
void from_axis_angle(const Vector<Type, 3> &axis, Type theta) {
void from_axis_angle(const Vector<Type, 3> &axis, Type theta)
{
Quaternion &q = *this;
if(theta < (Type)1e-10) {
q(0) = (Type)1.0;
q(1)=q(2)=q(3)=0;
}
Type magnitude = sinf(theta/2.0f);
q(0) = cosf(theta/2.0f);
if (theta < (Type)1e-10) {
q(0) = (Type)1.0;
q(1) = q(2) = q(3) = 0;
}
Type magnitude = sinf(theta / 2.0f);
q(0) = cosf(theta / 2.0f);
q(1) = axis(0) * magnitude;
q(2) = axis(1) * magnitude;
q(3) = axis(2) * magnitude;
@@ -311,17 +321,20 @@ public:
*
* @return vector, direction representing rotation axis and norm representing angle
*/
Vector<Type, 3> to_axis_angle() {
Vector<Type, 3> to_axis_angle()
{
Quaternion &q = *this;
Type axis_magnitude = Type(sqrt(q(1) * q(1) + q(2) * q(2) + q(3) * q(3)));
Vector<Type, 3> vec;
vec(0) = q(1);
vec(1) = q(2);
vec(2) = q(3);
if(axis_magnitude >= (Type)1e-10) {
if (axis_magnitude >= (Type)1e-10) {
vec = vec / axis_magnitude;
vec = vec * wrap_pi((Type)2.0 * atan2f(axis_magnitude,q(0)));
vec = vec * wrap_pi((Type)2.0 * atan2f(axis_magnitude, q(0)));
}
return vec;
}
};