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:
6 months ago
Revision:
87:6d95f853dab3
Parent:
10:085ab7328054

File content as of revision 87:6d95f853dab3:

#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