diff options
author | James Goppert <james.goppert@gmail.com> | 2013-01-16 13:27:04 -0500 |
---|---|---|
committer | James Goppert <james.goppert@gmail.com> | 2013-01-16 13:27:04 -0500 |
commit | ded442fd194442134accf0080be3fe73098481c1 (patch) | |
tree | 817e3948ce329aa80f939fb54a7b12bb2ee40f71 /apps/examples/kalman_demo/KalmanNav.hpp | |
parent | 41ac3fdef9e3b7210286b438f3dae50af06b814c (diff) | |
download | px4-firmware-ded442fd194442134accf0080be3fe73098481c1.tar.gz px4-firmware-ded442fd194442134accf0080be3fe73098481c1.tar.bz2 px4-firmware-ded442fd194442134accf0080be3fe73098481c1.zip |
Added position initialization.
Diffstat (limited to 'apps/examples/kalman_demo/KalmanNav.hpp')
-rw-r--r-- | apps/examples/kalman_demo/KalmanNav.hpp | 12 |
1 files changed, 7 insertions, 5 deletions
diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp index da1a7ee10..7709ac697 100644 --- a/apps/examples/kalman_demo/KalmanNav.hpp +++ b/apps/examples/kalman_demo/KalmanNav.hpp @@ -93,23 +93,23 @@ public: * State prediction * Continuous, non-linear */ - void predictState(float dt); + int predictState(float dt); /** * State covariance prediction * Continuous, linear */ - void predictStateCovariance(float dt); + int predictStateCovariance(float dt); /** * Attitude correction */ - void correctAtt(); + int correctAtt(); /** * Position correction */ - void correctPos(); + int correctPos(); /** * Overloaded update parameters @@ -166,7 +166,9 @@ protected: control::BlockParam<float> _faultPos; /**< fault detection threshold for position */ control::BlockParam<float> _faultAtt; /**< fault detection threshold for attitude */ // status - bool _positionInitialized; /**< status, if position has been init. */ + bool _attitudeInitialized; + bool _positionInitialized; + uint16_t _attitudeInitCounter; // accessors int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); } void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; } |