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

PosVelFilter/PosVelFilter.cpp@9:d5fcdcb3c89d, 2017-10-20 (annotated)

Committer:
mkelly10
Date:
Fri Oct 20 11:41:22 2017 +0000
Revision:
9:d5fcdcb3c89d
Child:
10:085ab7328054
Tested 10/19/17 Folders

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mkelly10 9:d5fcdcb3c89d 1 #include "PosVelFilter.hpp"
mkelly10 9:d5fcdcb3c89d 2 #include "StaticDefs.hpp"
mkelly10 9:d5fcdcb3c89d 3 #include "conversions.hpp"
mkelly10 9:d5fcdcb3c89d 4
mkelly10 9:d5fcdcb3c89d 5 PosVelFilter::PosVelFilter()
mkelly10 9:d5fcdcb3c89d 6 {
mkelly10 9:d5fcdcb3c89d 7 x1 = 0;
mkelly10 9:d5fcdcb3c89d 8 x2 = 0;
mkelly10 9:d5fcdcb3c89d 9 //w_n is the natural frequency of the filter bigger increases frequency response
mkelly10 9:d5fcdcb3c89d 10 w_n = 1.0;
mkelly10 9:d5fcdcb3c89d 11
mkelly10 9:d5fcdcb3c89d 12 }
mkelly10 9:d5fcdcb3c89d 13
mkelly10 9:d5fcdcb3c89d 14 void PosVelFilter::update(float deltaT, float counts)
mkelly10 9:d5fcdcb3c89d 15 {
mkelly10 9:d5fcdcb3c89d 16 //run the pos-vel estimate filter
mkelly10 9:d5fcdcb3c89d 17 //this derives the timing from last run
mkelly10 9:d5fcdcb3c89d 18 //last_time = curr_time;
mkelly10 9:d5fcdcb3c89d 19 //curr_time = time;
mkelly10 9:d5fcdcb3c89d 20 dt = deltaT;
mkelly10 9:d5fcdcb3c89d 21
mkelly10 9:d5fcdcb3c89d 22 x1_dot = x2;
mkelly10 9:d5fcdcb3c89d 23 x2_dot = (-2.0*w_n*x2)-(w_n*w_n)*x1+(w_n*w_n)*counts;
mkelly10 9:d5fcdcb3c89d 24
mkelly10 9:d5fcdcb3c89d 25 position = x1;
mkelly10 9:d5fcdcb3c89d 26 velocity = x2;
mkelly10 9:d5fcdcb3c89d 27
mkelly10 9:d5fcdcb3c89d 28 x1 += x1_dot*dt;
mkelly10 9:d5fcdcb3c89d 29 x2 += x2_dot*dt;
mkelly10 9:d5fcdcb3c89d 30
mkelly10 9:d5fcdcb3c89d 31 }
mkelly10 9:d5fcdcb3c89d 32
mkelly10 9:d5fcdcb3c89d 33 float PosVelFilter::getPosition()
mkelly10 9:d5fcdcb3c89d 34 {
mkelly10 9:d5fcdcb3c89d 35 return position;
mkelly10 9:d5fcdcb3c89d 36 }
mkelly10 9:d5fcdcb3c89d 37
mkelly10 9:d5fcdcb3c89d 38 float PosVelFilter::getVelocity()
mkelly10 9:d5fcdcb3c89d 39 {
mkelly10 9:d5fcdcb3c89d 40 return velocity;
mkelly10 9:d5fcdcb3c89d 41 }
mkelly10 9:d5fcdcb3c89d 42
mkelly10 9:d5fcdcb3c89d 43 float PosVelFilter::getDt()
mkelly10 9:d5fcdcb3c89d 44 {
mkelly10 9:d5fcdcb3c89d 45 return dt;
mkelly10 9:d5fcdcb3c89d 46 }
mkelly10 9:d5fcdcb3c89d 47
mkelly10 9:d5fcdcb3c89d 48 void PosVelFilter::writeWn(float wn)
mkelly10 9:d5fcdcb3c89d 49 {
mkelly10 9:d5fcdcb3c89d 50 w_n = wn;
mkelly10 9:d5fcdcb3c89d 51 }