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?
User | Revision | Line number | New 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 | } |