Flying Sea Glider / Mbed 2 deprecated 2019_10may_firstflight_jcw_nosd

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
--- 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