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
- 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() { // if heading_flag is set, operate on sine and cosine, return atan2(sin, cos) as postion and velocity?? x1 = 0; // pseudo position state x2 = 0; // pseudo velocity state x1c = 0; x2c = 0; x1s = 0; x2s = 0; w_n = 1.0; // natural frequency of the filter: bigger increases frequency response _headingFlag = false; } //run the pos-vel estimate filter void PosVelFilter::update(float deltaT, float counts) { dt = deltaT; if(!_headingFlag) { 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; } if(_headingFlag) { counts_c = cos(counts * _PI/180.0); // math.h cos and sine operate on radians , counts is heading degrees counts_s = sin(counts * _PI/180.0); // the cosine part x1c_dot = x2c; x2c_dot = (-2.0*w_n*x2c) - (w_n*w_n)*x1c + (w_n*w_n)*counts_c; position_c = x1c; velocity_c = x2c; x1c += x1c_dot*dt; x2c += x2c_dot*dt; // now the sine portion x1s_dot = x2s; x2s_dot = (-2.0*w_n*x2s) - (w_n*w_n)*x1s + (w_n*w_n)*counts_s; position_s = x1s; velocity_s = x2s; x1s += x1s_dot*dt; x2s += x2s_dot*dt; // now reconstitute the angle and the angular velocity position = atan2(position_s, position_c) * 180.0/_PI; pnew = atan2(x1s,x1c) * 180/_PI; // the next value, since the update has happened by now. velocity = (pnew - position) ; // don't multiply by dt yet, now just an angular difference if(position < 0 ) { position = position +360; } if(velocity > 180 ) { velocity -= 360; } if(velocity <= -180) { velocity += 360 ;} velocity *= (1.0/dt); } } float PosVelFilter::getPosition() { return position; } float PosVelFilter::getVelocity() { return velocity; } void PosVelFilter::setHeadingFlag(bool heading_flag) { _headingFlag = heading_flag; } float PosVelFilter::getDt() { return dt; } void PosVelFilter::writeWn(float wn) { w_n = wn; }