diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-30 00:46:29 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-30 00:46:29 +0200 |
commit | 28a31708f98eefa4ceb04617f2da3dd7892c99fa (patch) | |
tree | b96ffb29110b72d164a7b6dd45ec481612a05e6a /src/modules/ekf_att_pos_estimator/estimator_utilities.h | |
parent | 6f75d1a20f6c66a8bfb87032048d1369d01fbb5e (diff) | |
parent | afb5271bfb2f2645f0d48a802ed15b8f6509cd58 (diff) | |
download | px4-firmware-28a31708f98eefa4ceb04617f2da3dd7892c99fa.tar.gz px4-firmware-28a31708f98eefa4ceb04617f2da3dd7892c99fa.tar.bz2 px4-firmware-28a31708f98eefa4ceb04617f2da3dd7892c99fa.zip |
Merged estimator fixes and mavlink rate config bits
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/estimator_utilities.h')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_utilities.h | 82 |
1 files changed, 82 insertions, 0 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h new file mode 100644 index 000000000..d47568b62 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -0,0 +1,82 @@ +#include <math.h> +#include <stdint.h> + +#pragma once + +#define GRAVITY_MSS 9.80665f +#define deg2rad 0.017453292f +#define rad2deg 57.295780f +#define pi 3.141592657f +#define earthRate 0.000072921f +#define earthRadius 6378145.0 +#define earthRadiusInv 1.5678540e-7 + +class Vector3f +{ +private: +public: + float x; + float y; + float z; + + float length(void) const; + void zero(void); +}; + +class Mat3f +{ +private: +public: + Vector3f x; + Vector3f y; + Vector3f z; + + Mat3f(); + + void identity(); + Mat3f transpose(void) const; +}; + +Vector3f operator*(float sclIn1, Vector3f vecIn1); +Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator*( Mat3f matIn, Vector3f vecIn); +Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator*(Vector3f vecIn1, float sclIn1); + +void swap_var(float &d1, float &d2); + +enum GPS_FIX { + GPS_FIX_NOFIX = 0, + GPS_FIX_2D = 2, + GPS_FIX_3D = 3 +}; + +struct ekf_status_report { + bool error; + bool velHealth; + bool posHealth; + bool hgtHealth; + bool velTimeout; + bool posTimeout; + bool hgtTimeout; + bool imuTimeout; + uint32_t velFailTime; + uint32_t posFailTime; + uint32_t hgtFailTime; + float states[32]; + unsigned n_states; + bool angNaN; + bool summedDelVelNaN; + bool KHNaN; + bool KHPNaN; + bool PNaN; + bool covarianceNaN; + bool kalmanGainsNaN; + bool statesNaN; + bool gyroOffsetsExcessive; + bool covariancesExcessive; + bool velOffsetExcessive; +}; + +void ekf_debug(const char *fmt, ...);
\ No newline at end of file |