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

Added more warnings, and fixed issues.

Browse Source
This commit is contained in:
jgoppert
2015-11-05 11:17:54 -05:00
parent 6ba15eeaa0
commit 0062cbcbe2
10 changed files with 84 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+17 -2
View File
@@ -20,9 +20,24 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH}
set(CMAKE_CXX_FLAGS
${CMAKE_CXX_FLAGS}
-Wall
-Weffc++
-Werror
-Wno-sign-compare
-Wextra
-Wshadow
-Wfloat-equal
-Wpointer-arith
-Wmissing-declarations
-Wno-unused-parameter
-Werror=format-security
-Werror=array-bounds
-Wfatal-errors
-Werror=unused-variable
-Werror=reorder
-Werror=uninitialized
-Werror=init-self
-Wcast-qual
-Wconversion
-Wcast-align
-Werror
)
if (COVERALLS)
matrix/Dcm.hpp
+1 -1
View File
@@ -31,7 +31,7 @@ public:
Matrix<Type, 3, 3>::setIdentity();
}
Dcm(const Type *data) : Matrix<Type, 3, 3>(data)
Dcm(const Type *data_) : Matrix<Type, 3, 3>(data_)
{
}
matrix/Euler.hpp
+13 -18
View File
@@ -30,33 +30,28 @@ public:
{
}
Euler(Type phi, Type theta, Type psi) : Vector<Type, 3>()
Euler(Type phi_, Type theta_, Type psi_) : Vector<Type, 3>()
{
this->phi() = phi;
this->theta() = theta;
this->psi() = psi;
phi() = phi_;
theta() = theta_;
psi() = psi_;
}
Euler(const Dcm<Type> & dcm) {
Type theta = asin(-dcm(2, 0));
Type phi = 0;
Type psi = 0;
theta() = Type(asin(-dcm(2, 0)));
if (fabs(theta - M_PI_2) < 1.0e-3) {
psi = atan2(dcm(1, 2) - dcm(0, 1),
dcm(0, 2) + dcm(1, 1));
if (fabs(theta() - M_PI_2) < 1.0e-3) {
psi() = Type(atan2(dcm(1, 2) - dcm(0, 1),
dcm(0, 2) + dcm(1, 1)));
} else if (fabs(theta + M_PI_2) < 1.0e-3) {
psi = atan2(dcm(1, 2) - dcm(0, 1),
dcm(0, 2) + dcm(1, 1));
} else if (fabs(theta() + M_PI_2) < 1.0e-3) {
psi() = Type(atan2(dcm(1, 2) - dcm(0, 1),
dcm(0, 2) + dcm(1, 1)));
} else {
phi = atan2f(dcm(2, 1), dcm(2, 2));
psi = atan2f(dcm(1, 0), dcm(0, 0));
phi() = Type(atan2f(dcm(2, 1), dcm(2, 2)));
psi() = Type(atan2f(dcm(1, 0), dcm(0, 0)));
}
this->phi() = phi;
this->theta() = theta;
this->psi() = psi;
}
Euler(const Quaternion<Type> & q) {
matrix/Matrix.hpp
+13 -12
View File
@@ -38,10 +38,10 @@ public:
{
}
Matrix(const Type *data) :
Matrix(const Type *data_) :
_data()
{
memcpy(_data, data, sizeof(_data));
memcpy(_data, data_, sizeof(_data));
}
Matrix(const Matrix &other) :
@@ -113,10 +113,11 @@ public:
{
Matrix<Type, M, N> res;
const Matrix<Type, M, N> &self = *this;
static const Type eps = Type(1e-7);
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
if (self(i , j) != other(i, j)) {
if (fabs(self(i , j) - other(i, j)) > eps) {
return false;
}
}
@@ -298,7 +299,7 @@ public:
Matrix<Type, M, N> r;
for (int i=0; i<M; i++) {
for (int j=0; j<M; j++) {
r(i,j) = fabs((*this)(i,j));
r(i,j) = Type(fabs((*this)(i,j)));
}
}
return r;
@@ -306,30 +307,30 @@ public:
Type max()
{
Type max = (*this)(0,0);
Type max_val = (*this)(0,0);
for (int i=0; i<M; i++) {
for (int j=0; j<M; j++) {
Type val = (*this)(i,j);
if (val > max) {
max = val;
if (val > max_val) {
max_val = val;
}
}
}
return max;
return max_val;
}
Type min()
{
Type min = (*this)(0,0);
Type min_val = (*this)(0,0);
for (int i=0; i<M; i++) {
for (int j=0; j<M; j++) {
Type val = (*this)(i,j);
if (val < min) {
min = val;
if (val < min_val) {
min_val = val;
}
}
}
return min;
return min_val;
}
};
matrix/Quaternion.hpp
+15 -15
View File
@@ -40,24 +40,24 @@ public:
Vector<Type, 4>()
{
Quaternion &q = *this;
q(0) = 0.5 * sqrt(1 + dcm(0, 0) +
dcm(1, 1) + dcm(2, 2));
q(1) = (dcm(2, 1) - dcm(1, 2)) /
(4 * q(0));
q(2) = (dcm(0, 2) - dcm(2, 0)) /
(4 * q(0));
q(3) = (dcm(1, 0) - dcm(0, 1)) /
(4 * q(0));
q(0) = Type(0.5 * sqrt(1 + dcm(0, 0) +
dcm(1, 1) + dcm(2, 2)));
q(1) = Type((dcm(2, 1) - dcm(1, 2)) /
(4 * q(0)));
q(2) = Type((dcm(0, 2) - dcm(2, 0)) /
(4 * q(0)));
q(3) = Type((dcm(1, 0) - dcm(0, 1)) /
(4 * q(0)));
}
Quaternion(const Euler<Type> & euler) {
Quaternion &q = *this;
Type cosPhi_2 = cos(euler.phi() / 2.0);
Type cosTheta_2 = cos(euler.theta() / 2.0);
Type cosPsi_2 = cos(euler.psi() / 2.0);
Type sinPhi_2 = sin(euler.phi() / 2.0);
Type sinTheta_2 = sin(euler.theta() / 2.0);
Type sinPsi_2 = sin(euler.psi() / 2.0);
Type cosPhi_2 = Type(cos(euler.phi() / 2.0));
Type cosTheta_2 = Type(cos(euler.theta() / 2.0));
Type cosPsi_2 = Type(cos(euler.psi() / 2.0));
Type sinPhi_2 = Type(sin(euler.phi() / 2.0));
Type sinTheta_2 = Type(sin(euler.theta() / 2.0));
Type sinPsi_2 = Type(sin(euler.psi() / 2.0));
q(0) = cosPhi_2 * cosTheta_2 * cosPsi_2 +
sinPhi_2 * sinTheta_2 * sinPsi_2;
q(1) = sinPhi_2 * cosTheta_2 * cosPsi_2 -
@@ -102,7 +102,7 @@ public:
v(1) = w(0);
v(2) = w(1);
v(3) = w(2);
return Q * v * 0.5;
return Q * v * Type(0.5);
}
};
matrix/SquareMatrix.hpp
+3 -3
View File
@@ -31,8 +31,8 @@ public:
{
}
SquareMatrix(const Type *data) :
Matrix<Type, M, M>(data)
SquareMatrix(const Type *data_) :
Matrix<Type, M, M>(data_)
{
}
@@ -181,7 +181,7 @@ public:
SquareMatrix<float, M> res;
res.setIdentity();
SquareMatrix<float, M> A_pow = *this;
float k_fact = 1;
size_t k_fact = 1;
size_t k = 1;
while (k < n) {
matrix/Vector.hpp
+1 -15
View File
@@ -58,7 +58,7 @@ public:
Type norm() const {
const Vector &a(*this);
return sqrt(a.dot(a));
return Type(sqrt(a.dot(a)));
}
inline void normalize() {
@@ -81,20 +81,6 @@ public:
return res;
}
bool operator==(const Vector<Type, M> &other) const
{
Vector<Type, M> res;
const Vector<Type, M> &self = *this;
for (size_t i = 0; i < M; i++) {
if (self(i) != other(i)) {
return false;
}
}
return true;
}
Vector<Type, M> operator-(const Vector<Type, M> &other) const
{
Vector<Type, M> res;
test/attitude.cpp
+11 -11
View File
@@ -16,31 +16,31 @@ int main()
double eps = 1e-6;
// check data
Eulerf euler_check(0.1, 0.2, 0.3);
Quatf q_check(0.98334744, 0.0342708, 0.10602051, .14357218);
Eulerf euler_check(0.1f, 0.2f, 0.3f);
Quatf q_check(0.98334744f, 0.0342708f, 0.10602051f, .14357218f);
float dcm_data[] = {
0.93629336, -0.27509585, 0.21835066,
0.28962948, 0.95642509, -0.03695701,
-0.19866933, 0.0978434 , 0.97517033
0.93629336f, -0.27509585f, 0.21835066f,
0.28962948f, 0.95642509f, -0.03695701f,
-0.19866933f, 0.0978434f, 0.97517033f
};
Dcmf dcm_check(dcm_data);
// euler ctor
euler_check.T().print();
assert((euler_check - Vector3f(0.1, 0.2, 0.3)).norm() < eps);
assert((euler_check - Vector3f(0.1f, 0.2f, 0.3f)).norm() < eps);
// quaternion ctor
Quatf q(1, 2, 3, 4);
assert(q(0) == 1);
assert(q(1) == 2);
assert(q(2) == 3);
assert(q(3) == 4);
assert(fabs(q(0) - 1) < eps);
assert(fabs(q(1) - 2) < eps);
assert(fabs(q(2) - 3) < eps);
assert(fabs(q(3) - 4) < eps);
q.T().print();
q.normalize();
q.T().print();
assert((q - Quatf(
0.18257419, 0.36514837, 0.54772256, 0.73029674)
0.18257419f, 0.36514837f, 0.54772256f, 0.73029674f)
).norm() < eps);
// euler to quaternion
test/setIdentity.cpp
+2 -2
View File
@@ -13,10 +13,10 @@ int main()
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
if (i == j) {
assert(A(i, j) == 1);
assert( fabs(A(i, j) - 1) < 1e-7);
} else {
assert(A(i, j) == 0);
assert( fabs(A(i, j) - 0) < 1e-7);
}
}
}
test/vectorAssignment.cpp
+8 -6
View File
@@ -14,17 +14,19 @@ int main()
v.print();
assert(v(0) == 1);
assert(v(1) == 2);
assert(v(2) == 3);
static const float eps = 1e-7f;
assert(fabs(v(0) - 1) < eps);
assert(fabs(v(1) - 2) < eps);
assert(fabs(v(2) - 3) < eps);
Vector3f v2(4, 5, 6);
v2.print();
assert(v2(0) == 4);
assert(v2(1) == 5);
assert(v2(2) == 6);
assert(fabs(v2(0) - 4) < eps);
assert(fabs(v2(1) - 5) < eps);
assert(fabs(v2(2) - 6) < eps);
return 0;
}