diff options
| -rw-r--r-- | src/lib/mathlib/math/Matrix.hpp | 8 | ||||
| -rw-r--r-- | src/lib/mathlib/math/Quaternion.hpp | 36 | ||||
| -rw-r--r-- | src/lib/mathlib/math/Vector.hpp | 14 | ||||
| -rw-r--r-- | src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 15 |
4 files changed, 11 insertions, 62 deletions
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 67214133a..47929ffcb 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -129,7 +129,7 @@ public: */ const Matrix<M, N> &operator =(const Matrix<M, N> &m) { memcpy(data, m.data, sizeof(data)); - return *reinterpret_cast<Matrix<M, N>*>(this); + return *static_cast<Matrix<M, N>*>(this); } /** @@ -158,7 +158,7 @@ public: for (unsigned int i = 0; i < N; i++) for (unsigned int j = 0; j < M; j++) data[i][j] += m.data[i][j]; - return *reinterpret_cast<Matrix<M, N>*>(this); + return *static_cast<Matrix<M, N>*>(this); } /** @@ -176,7 +176,7 @@ public: for (unsigned int i = 0; i < N; i++) for (unsigned int j = 0; j < M; j++) data[i][j] -= m.data[i][j]; - return *reinterpret_cast<Matrix<M, N>*>(this); + return *static_cast<Matrix<M, N>*>(this); } /** @@ -194,7 +194,7 @@ public: for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) data[i][j] *= num; - return *reinterpret_cast<Matrix<M, N>*>(this); + return *static_cast<Matrix<M, N>*>(this); } Matrix<M, N> operator /(const float num) const { diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 1b0cb3c41..3735fb3d3 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -73,42 +73,6 @@ public: Quaternion(const float *v) : Vector(v) { } - /** - * access to elements by index - */ - /* - inline float &operator ()(unsigned int i) { - return *(&a + i); - } - */ - - /** - * access to elements by index - */ - /* - inline const float &operator ()(unsigned int i) const { - return *(&a + i); - } - */ - - /** - * addition - */ - /* - const Quaternion operator +(const Quaternion &q) const { - return Quaternion(a + q.a, b + q.b, c + q.c, d + q.d); - } - */ - - /** - * subtraction - */ - /* - const Quaternion operator -(const Quaternion &q) const { - return Quaternion(a - q.a, b - q.b, c - q.c, d - q.d); - } - */ - Quaternion derivative(const Vector<3> &w) { float dataQ[] = { data[0], -data[1], -data[2], -data[3], diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 2df87c74b..8d754a321 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -135,7 +135,7 @@ public: */ const Vector<N> &operator =(const Vector<N> &v) { memcpy(data, v.data, sizeof(data)); - return *reinterpret_cast<const Vector<N>*>(this); + return *static_cast<const Vector<N>*>(this); } /** @@ -180,7 +180,7 @@ public: * uniform scaling */ const Vector<N> operator /(const float num) const { - Vector<N> temp(*reinterpret_cast<const Vector<N>*>(this)); + Vector<N> temp(*static_cast<const Vector<N>*>(this)); return temp /= num; } @@ -190,7 +190,7 @@ public: const Vector<N> &operator +=(const Vector<N> &v) { for (unsigned int i = 0; i < N; i++) data[i] += v(i); - return *reinterpret_cast<const Vector<N>*>(this); + return *static_cast<const Vector<N>*>(this); } /** @@ -199,7 +199,7 @@ public: const Vector<N> &operator -=(const Vector<N> &v) { for (unsigned int i = 0; i < N; i++) data[i] -= v(i); - return *reinterpret_cast<const Vector<N>*>(this); + return *static_cast<const Vector<N>*>(this); } /** @@ -208,7 +208,7 @@ public: const Vector<N> &operator *=(const float num) { for (unsigned int i = 0; i < N; i++) data[i] *= num; - return *reinterpret_cast<const Vector<N>*>(this); + return *static_cast<const Vector<N>*>(this); } /** @@ -217,7 +217,7 @@ public: const Vector<N> &operator /=(const float num) { for (unsigned int i = 0; i < N; i++) data[i] /= num; - return *reinterpret_cast<const Vector<N>*>(this); + return *static_cast<const Vector<N>*>(this); } /** @@ -241,7 +241,7 @@ public: * gets the length of this vector */ float length() const { - return sqrtf(*this * *reinterpret_cast<const Vector<N>*>(this)); + return sqrtf(*this * *static_cast<const Vector<N>*>(this)); } /** diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 6533eb7cf..4ab41c6ce 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -53,21 +53,6 @@ static const int8_t ret_error = -1; // error occurred KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : SuperBlock(parent, name), - // ekf matrices - F(), - G(), - P(), - P0(), - V(), - // attitude measurement ekf matrices - HAtt(), - RAtt(), - // position measurement ekf matrices - HPos(), - RPos(), - // attitude representations - C_nb(), - q(), // subscriptions _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz |