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

exPosVelFilter/exPosVelFilter.txt

Committer:
joel_ssc
Date:
2019-05-13
Revision:
92:52a91656458a
Parent:
87:6d95f853dab3

File content as of revision 92:52a91656458a:

#include "PosVelFilter.hpp"
//#include "StaticDefs.hpp"

PosVelFilter::PosVelFilter() {
    x1 = 0; // pseudo position state
    x2 = 0; // pseudo velocity state
    
    w_n = 1.0; // natural frequency of the filter bigger increases frequency response
}

//run the pos-vel estimate filter
void PosVelFilter::update(float deltaT, float counts) {
    dt = deltaT;

    x1_dot = x2;
    x2_dot = (-2.0*w_n*x2) - (w_n*w_n)*x1 + (w_n*w_n)*counts;

    position = x1;
    velocity = x2;

    x1 += x1_dot*dt;
    x2 += x2_dot*dt;
}

float PosVelFilter::getPosition() {
    return position;
}

float PosVelFilter::getVelocity() {
    return velocity;
}

float PosVelFilter::getDt() {
    return dt;
}

void PosVelFilter::writeWn(float wn) {
    w_n = wn;
}