most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown
Dependencies: mbed MODSERIAL FATFileSystem
Diff: PosVelFilter/PosVelFilter.hpp
- Revision:
- 87:6d95f853dab3
- Parent:
- 10:085ab7328054
diff -r ba3a118b0080 -r 6d95f853dab3 PosVelFilter/PosVelFilter.hpp --- a/PosVelFilter/PosVelFilter.hpp Thu May 02 20:34:16 2019 +0000 +++ b/PosVelFilter/PosVelFilter.hpp Wed May 08 13:24:04 2019 +0000 @@ -2,6 +2,8 @@ #define POSVELFILTER_H #include "mbed.h" +#include <deque> +#define _PI ((float) 3.14159265359) class PosVelFilter { @@ -15,19 +17,21 @@ float getPosition(); float getVelocity(); float getDt(); - + void setHeadingFlag(bool heading_flag); void writeWn(float wn); protected: - float x1; - float x2; - float x2_dot; - float x1_dot; + float x1, x1c,x1s; + float x2, x2c,x2s; + float x1_dot, x1c_dot, x1s_dot; + float x2_dot, x2c_dot, x2s_dot; float w_n; + float counts_c, counts_s; float dt; - float position; - float velocity; + float position,pnew, position_c, position_s; + float velocity, velocity_c, velocity_s; + bool _headingFlag; }; #endif \ No newline at end of file