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@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
joel_ssc 87:6d95f853dab3 4 PosVelFilter::PosVelFilter() { // if heading_flag is set, operate on sine and cosine, return atan2(sin, cos) as postion and velocity??
danstrider 10:085ab7328054 5 x1 = 0; // pseudo position state
danstrider 10:085ab7328054 6 x2 = 0; // pseudo velocity state
joel_ssc 87:6d95f853dab3 7 x1c = 0;
joel_ssc 87:6d95f853dab3 8 x2c = 0;
joel_ssc 87:6d95f853dab3 9 x1s = 0;
joel_ssc 87:6d95f853dab3 10 x2s = 0;
joel_ssc 87:6d95f853dab3 11 w_n = 1.0; // natural frequency of the filter: bigger increases frequency response
joel_ssc 87:6d95f853dab3 12 _headingFlag = false;
mkelly10 9:d5fcdcb3c89d 13 }
mkelly10 9:d5fcdcb3c89d 14
danstrider 10:085ab7328054 15 //run the pos-vel estimate filter
danstrider 10:085ab7328054 16 void PosVelFilter::update(float deltaT, float counts) {
mkelly10 9:d5fcdcb3c89d 17 dt = deltaT;
joel_ssc 87:6d95f853dab3 18
joel_ssc 87:6d95f853dab3 19 if(!_headingFlag) {
joel_ssc 87:6d95f853dab3 20 x1_dot = x2;
joel_ssc 87:6d95f853dab3 21 x2_dot = (-2.0*w_n*x2) - (w_n*w_n)*x1 + (w_n*w_n)*counts;
mkelly10 9:d5fcdcb3c89d 22
joel_ssc 87:6d95f853dab3 23 position = x1;
joel_ssc 87:6d95f853dab3 24 velocity = x2;
joel_ssc 87:6d95f853dab3 25
joel_ssc 87:6d95f853dab3 26 x1 += x1_dot*dt;
joel_ssc 87:6d95f853dab3 27 x2 += x2_dot*dt;
joel_ssc 87:6d95f853dab3 28 }
joel_ssc 87:6d95f853dab3 29 if(_headingFlag) {
joel_ssc 87:6d95f853dab3 30 counts_c = cos(counts * _PI/180.0); // math.h cos and sine operate on radians , counts is heading degrees
joel_ssc 87:6d95f853dab3 31 counts_s = sin(counts * _PI/180.0);
joel_ssc 87:6d95f853dab3 32 // the cosine part
joel_ssc 87:6d95f853dab3 33 x1c_dot = x2c;
joel_ssc 87:6d95f853dab3 34 x2c_dot = (-2.0*w_n*x2c) - (w_n*w_n)*x1c + (w_n*w_n)*counts_c;
joel_ssc 87:6d95f853dab3 35
joel_ssc 87:6d95f853dab3 36 position_c = x1c;
joel_ssc 87:6d95f853dab3 37 velocity_c = x2c;
mkelly10 9:d5fcdcb3c89d 38
joel_ssc 87:6d95f853dab3 39 x1c += x1c_dot*dt;
joel_ssc 87:6d95f853dab3 40 x2c += x2c_dot*dt;
joel_ssc 87:6d95f853dab3 41 // now the sine portion
joel_ssc 87:6d95f853dab3 42 x1s_dot = x2s;
joel_ssc 87:6d95f853dab3 43 x2s_dot = (-2.0*w_n*x2s) - (w_n*w_n)*x1s + (w_n*w_n)*counts_s;
joel_ssc 87:6d95f853dab3 44
joel_ssc 87:6d95f853dab3 45 position_s = x1s;
joel_ssc 87:6d95f853dab3 46 velocity_s = x2s;
mkelly10 9:d5fcdcb3c89d 47
joel_ssc 87:6d95f853dab3 48 x1s += x1s_dot*dt;
joel_ssc 87:6d95f853dab3 49 x2s += x2s_dot*dt;
joel_ssc 87:6d95f853dab3 50 // now reconstitute the angle and the angular velocity
joel_ssc 87:6d95f853dab3 51 position = atan2(position_s, position_c) * 180.0/_PI;
joel_ssc 87:6d95f853dab3 52 pnew = atan2(x1s,x1c) * 180/_PI; // the next value, since the update has happened by now.
joel_ssc 87:6d95f853dab3 53 velocity = (pnew - position) ; // don't multiply by dt yet, now just an angular difference
joel_ssc 87:6d95f853dab3 54 if(position < 0 ) { position = position +360; }
joel_ssc 87:6d95f853dab3 55 if(velocity > 180 ) { velocity -= 360; }
joel_ssc 87:6d95f853dab3 56 if(velocity <= -180) { velocity += 360 ;}
joel_ssc 87:6d95f853dab3 57 velocity *= (1.0/dt);
joel_ssc 87:6d95f853dab3 58 }
mkelly10 9:d5fcdcb3c89d 59 }
mkelly10 9:d5fcdcb3c89d 60
danstrider 10:085ab7328054 61 float PosVelFilter::getPosition() {
mkelly10 9:d5fcdcb3c89d 62 return position;
mkelly10 9:d5fcdcb3c89d 63 }
mkelly10 9:d5fcdcb3c89d 64
danstrider 10:085ab7328054 65 float PosVelFilter::getVelocity() {
mkelly10 9:d5fcdcb3c89d 66 return velocity;
mkelly10 9:d5fcdcb3c89d 67 }
joel_ssc 87:6d95f853dab3 68 void PosVelFilter::setHeadingFlag(bool heading_flag) {
joel_ssc 87:6d95f853dab3 69 _headingFlag = heading_flag;
joel_ssc 87:6d95f853dab3 70 }
joel_ssc 87:6d95f853dab3 71
joel_ssc 87:6d95f853dab3 72
mkelly10 9:d5fcdcb3c89d 73
danstrider 10:085ab7328054 74 float PosVelFilter::getDt() {
mkelly10 9:d5fcdcb3c89d 75 return dt;
mkelly10 9:d5fcdcb3c89d 76 }
mkelly10 9:d5fcdcb3c89d 77
danstrider 10:085ab7328054 78 void PosVelFilter::writeWn(float wn) {
mkelly10 9:d5fcdcb3c89d 79 w_n = wn;
joel_ssc 87:6d95f853dab3 80 }