Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_LeadFilter: moved lag to be a parameter passed into the get_position function.

Browse Source 
This saves 4 bytes of memory and will work better with the way we initialise the gps.
This commit is contained in:
rmackay9 2012-09-20 15:27:27 +09:00
parent 2110231ee3
commit 30aa104000
3 changed files with 12 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_LeadFilter/AP_LeadFilter.cpp
View File

@ -17,25 +17,13 @@
#include "AP_LeadFilter.h" #include "AP_LeadFilter.h"
/*
* int32_t
* AP_LeadFilter::get_position(int32_t pos, int16_t vel)
* {
* vel = (_last_velocity + vel) / 2;
* pos += vel;
* pos += (vel - _last_velocity);
* _last_velocity = vel;
* return pos;
* }
*/
// setup the control preferences // setup the control preferences
int32_t int32_t
AP_LeadFilter::get_position(int32_t pos, int16_t vel) AP_LeadFilter::get_position(int32_t pos, int16_t vel, float lag_in_seconds)
{ {
// assumes a 1 second delay in the GPS // assumes a 1 second delay in the GPS
int16_t accel_contribution = (vel - _last_velocity) * _lag * _lag; int16_t accel_contribution = (vel - _last_velocity) * lag_in_seconds * lag_in_seconds;
int16_t vel_contribution = vel * _lag; int16_t vel_contribution = vel * lag_in_seconds;
// store velocity for next iteration // store velocity for next iteration
_last_velocity = vel; _last_velocity = vel;

6
libraries/AP_LeadFilter/AP_LeadFilter.h
View File

@ -16,17 +16,15 @@ public:
/// ///
/// ///
AP_LeadFilter() : AP_LeadFilter() :
_last_velocity(0), _lag(1.0) { _last_velocity(0) {
} }
// setup min and max radio values in CLI // setup min and max radio values in CLI
int32_t get_position(int32_t pos, int16_t vel); int32_t get_position(int32_t pos, int16_t vel, float lag_in_seconds = 1.0);
void set_lag(float delay_in_seconds) { _lag = delay_in_seconds; }
void clear() { _last_velocity = 0; } void clear() { _last_velocity = 0; }
private: private:
int16_t _last_velocity; int16_t _last_velocity;
float _lag;
}; };

12
libraries/AP_LeadFilter/examples/AP_LeadFilter/AP_LeadFilter.pde
View File

@ -38,14 +38,17 @@ void setup()
void loop() void loop()
{ {
int16_t velocity = 0; int16_t velocity;
int32_t position = 0; int32_t position;
int32_t new_position; int32_t new_position;
int16_t i; int16_t i;
position = 0;
velocity = 0;
xLeadFilter.clear();
Serial.printf("------------------\n"); Serial.printf("------------------\n");
Serial.printf("start position = 0, lag of 1sec.\n"); Serial.printf("start position = 0, lag of 1sec.\n");
xLeadFilter.set_lag(1.0);
for( i = 0; i < 10; i++ ) { for( i = 0; i < 10; i++ ) {
// get updated position // get updated position
new_position = xLeadFilter.get_position(position, velocity); // new position with velocity of 1 m/s new_position = xLeadFilter.get_position(position, velocity); // new position with velocity of 1 m/s
@ -57,13 +60,12 @@ void loop()
position = 0; position = 0;
velocity = 0; velocity = 0;
xLeadFilter.clear(); xLeadFilter.clear();
xLeadFilter.set_lag(0.200);
Serial.printf("------------------\n"); Serial.printf("------------------\n");
Serial.printf("start position = 0, lag of 200ms\n"); Serial.printf("start position = 0, lag of 200ms\n");
for( i = 0; i < 10; i++ ) { for( i = 0; i < 10; i++ ) {
// get updated position // get updated position
new_position = xLeadFilter.get_position(position, velocity); // new position with velocity of 1 m/s new_position = xLeadFilter.get_position(position, velocity, 0.200); // new position with velocity of 1 m/s
Serial.printf("start pos: %ld, start vel: %d, end pos: %ld\n", (long int)position, (int)velocity, (long int)new_position); Serial.printf("start pos: %ld, start vel: %d, end pos: %ld\n", (long int)position, (int)velocity, (long int)new_position);
position = new_position; position = new_position;
velocity += 100; velocity += 100;