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
PosVelFilter/PosVelFilter.hpp
- Committer:
- joel_ssc
- Date:
- 2019-05-13
- Revision:
- 92:52a91656458a
- Parent:
- 87:6d95f853dab3
File content as of revision 92:52a91656458a:
#ifndef POSVELFILTER_H #define POSVELFILTER_H #include "mbed.h" #include <deque> #define _PI ((float) 3.14159265359) class PosVelFilter { public: PosVelFilter(); void update(float deltaT, float counts); void init(); float getPosition(); float getVelocity(); float getDt(); void setHeadingFlag(bool heading_flag); void writeWn(float wn); protected: 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,pnew, position_c, position_s; float velocity, velocity_c, velocity_s; bool _headingFlag; }; #endif