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:
- danstrider
- Date:
- 2017-10-23
- Revision:
- 10:085ab7328054
- Parent:
- 9:d5fcdcb3c89d
- Child:
- 87:6d95f853dab3
File content as of revision 10:085ab7328054:
#include "PosVelFilter.hpp" //#include "StaticDefs.hpp" PosVelFilter::PosVelFilter() { x1 = 0; // pseudo position state x2 = 0; // pseudo velocity state w_n = 1.0; // natural frequency of the filter bigger increases frequency response } //run the pos-vel estimate filter void PosVelFilter::update(float deltaT, float counts) { dt = deltaT; 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; } float PosVelFilter::getPosition() { return position; } float PosVelFilter::getVelocity() { return velocity; } float PosVelFilter::getDt() { return dt; } void PosVelFilter::writeWn(float wn) { w_n = wn; }