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@92:52a91656458a, 2019-05-13 (annotated)

Committer:
joel_ssc
Date:
Mon May 13 19:25:26 2019 +0000
Revision:
92:52a91656458a
Parent:
87:6d95f853dab3 
version for first flight test, timeouts not yet set correctly

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mkelly10 9:d5fcdcb3c89d 1 #include "PosVelFilter.hpp"
danstrider 10:085ab7328054 2 //#include "StaticDefs.hpp"
mkelly10 9:d5fcdcb3c89d 3
danstrider 10:085ab7328054 4 PosVelFilter::PosVelFilter() {
danstrider 10:085ab7328054 5 x1 = 0; // pseudo position state
danstrider 10:085ab7328054 6 x2 = 0; // pseudo velocity state
danstrider 10:085ab7328054 7
danstrider 10:085ab7328054 8 w_n = 1.0; // natural frequency of the filter bigger increases frequency response
mkelly10 9:d5fcdcb3c89d 9 }
mkelly10 9:d5fcdcb3c89d 10
danstrider 10:085ab7328054 11 //run the pos-vel estimate filter
danstrider 10:085ab7328054 12 void PosVelFilter::update(float deltaT, float counts) {
mkelly10 9:d5fcdcb3c89d 13 dt = deltaT;
mkelly10 9:d5fcdcb3c89d 14
mkelly10 9:d5fcdcb3c89d 15 x1_dot = x2;
danstrider 10:085ab7328054 16 x2_dot = (-2.0*w_n*x2) - (w_n*w_n)*x1 + (w_n*w_n)*counts;
mkelly10 9:d5fcdcb3c89d 17
mkelly10 9:d5fcdcb3c89d 18 position = x1;
mkelly10 9:d5fcdcb3c89d 19 velocity = x2;
mkelly10 9:d5fcdcb3c89d 20
mkelly10 9:d5fcdcb3c89d 21 x1 += x1_dot*dt;
mkelly10 9:d5fcdcb3c89d 22 x2 += x2_dot*dt;
mkelly10 9:d5fcdcb3c89d 23 }
mkelly10 9:d5fcdcb3c89d 24
danstrider 10:085ab7328054 25 float PosVelFilter::getPosition() {
mkelly10 9:d5fcdcb3c89d 26 return position;
mkelly10 9:d5fcdcb3c89d 27 }
mkelly10 9:d5fcdcb3c89d 28
danstrider 10:085ab7328054 29 float PosVelFilter::getVelocity() {
mkelly10 9:d5fcdcb3c89d 30 return velocity;
mkelly10 9:d5fcdcb3c89d 31 }
mkelly10 9:d5fcdcb3c89d 32
danstrider 10:085ab7328054 33 float PosVelFilter::getDt() {
mkelly10 9:d5fcdcb3c89d 34 return dt;
mkelly10 9:d5fcdcb3c89d 35 }
mkelly10 9:d5fcdcb3c89d 36
danstrider 10:085ab7328054 37 void PosVelFilter::writeWn(float wn) {
mkelly10 9:d5fcdcb3c89d 38 w_n = wn;
mkelly10 9:d5fcdcb3c89d 39 }