dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'release_v1.0.0' into beta

This commit is contained in:
Lorenz Meier 2015-07-04 17:26:56 +02:00
parent 7a24ccd51d 4372701dab
commit cc3c66483c
1 changed files with 13 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
View File

@ -38,7 +38,6 @@
*/
#include "estimator_utilities.h"
#include <algorithm>
// Define EKF_DEBUG here to enable the debug print calls
// if the macro is not set, these will be completely
@ -72,6 +71,9 @@ ekf_debug(const char *fmt, ...)
void ekf_debug(const char *fmt, ...) { while(0){} }
#endif
/* we don't want to pull in the standard lib just to swap two floats */
void swap_var(float &d1, float &d2);
float Vector3f::length(void) const
{
return sqrt(x*x + y*y + z*z);
@ -108,9 +110,9 @@ void Mat3f::identity() {
Mat3f Mat3f::transpose() const
{
Mat3f ret = *this;
std::swap(ret.x.y, ret.y.x);
std::swap(ret.x.z, ret.z.x);
std::swap(ret.y.z, ret.z.y);
swap_var(ret.x.y, ret.y.x);
swap_var(ret.x.z, ret.z.x);
swap_var(ret.y.z, ret.z.y);
return ret;
}
@ -223,3 +225,10 @@ Vector3f operator/(const Vector3f &vec, const float scalar)
vecOut.z = vec.z / scalar;
return vecOut;
}
void swap_var(float &d1, float &d2)
{
float tmp = d1;
d1 = d2;
d2 = tmp;
}