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

Formatting and added Scalar.

Browse Source
This commit is contained in:
jgoppert
2015-11-03 19:38:31 -05:00
parent 76e86cf937
commit a36ff9f1e5
21 changed files with 811 additions and 697 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Euler.hpp
+54 -25
View File
@@ -24,35 +24,64 @@ template<typename Type>
class Euler : public Vector<Type, 3>
{
public:
virtual ~Euler() {};
virtual ~Euler() {};
Euler() : Vector<Type, 3>()
{
}
Euler() : Vector<Type, 3>()
{
}
Euler(Type roll, Type pitch, Type yaw) : Vector<Type, 3>()
{
Euler &v(*this);
v(0) = roll;
v(1) = pitch;
v(2) = yaw;
}
Euler(Type phi, Type theta, Type psi) : Vector<Type, 3>()
{
this->phi() = phi;
this->theta() = theta;
this->psi() = psi;
}
Euler(const Dcm<Type> & dcm) {
// TODO
Euler &e = *this;
e(0) = 0;
e(1) = 0;
e(2) = 0;
}
Euler(const Dcm<Type> & dcm) {
Type theta = asin(-dcm(2, 0));
Type phi = 0;
Type psi = 0;
Euler(const Quaternion<Type> & q) {
// TODO
Euler &e = *this;
e(0) = 0;
e(1) = 0;
e(2) = 0;
}
if (abs(theta - M_PI_2) < 1.0e-3) {
psi = atan2(dcm(1, 2) - dcm(0, 1),
dcm(0, 2) + dcm(1, 1));
} else if (abs(theta + M_PI_2) < 1.0e-3) {
psi = 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));
}
this->phi() = phi;
this->theta() = theta;
this->psi() = psi;
}
Euler(const Quaternion<Type> & q) {
*this = Euler(Dcm<Type>(q));
}
inline Type phi() const {
return (*this)(0);
}
inline Type theta() const {
return (*this)(1);
}
inline Type psi() const {
return (*this)(2);
}
inline Type & phi() {
return (*this)(0);
}
inline Type & theta() {
return (*this)(1);
}
inline Type & psi() {
return (*this)(2);
}
};