Browse Source
This was used to project the GPS position forward to compensate for lag but this has become unnecessary with the introduction of the inertial nav in x and y axis.mission-4.1.18
5 changed files with 0 additions and 121 deletions
@ -1,25 +0,0 @@
@@ -1,25 +0,0 @@
|
||||
/*
|
||||
* AP_LeadFilter.cpp - GPS lag remover library for Arduino |
||||
* Code by Jason Short. DIYDrones.com |
||||
* |
||||
* This library is free software; you can redistribute it and / or |
||||
* modify it under the terms of the GNU Lesser General Public |
||||
* License as published by the Free Software Foundation; either |
||||
* version 2.1 of the License, or (at your option) any later version. |
||||
* |
||||
*/ |
||||
#include "AP_LeadFilter.h" |
||||
|
||||
// setup the control preferences
|
||||
int32_t |
||||
AP_LeadFilter::get_position(int32_t pos, int16_t vel, float lag_in_seconds) |
||||
{ |
||||
// assumes a 1 second delay in the GPS
|
||||
int16_t accel_contribution = (vel - _last_velocity) * lag_in_seconds * lag_in_seconds; |
||||
int16_t vel_contribution = vel * lag_in_seconds; |
||||
|
||||
// store velocity for next iteration
|
||||
_last_velocity = vel; |
||||
|
||||
return pos + vel_contribution + accel_contribution; |
||||
} |
@ -1,29 +0,0 @@
@@ -1,29 +0,0 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#ifndef __AP_LEADFILTER_H__ |
||||
#define __AP_LEADFILTER_H__ |
||||
|
||||
#include <stdint.h> |
||||
#include <AP_LeadFilter.h> |
||||
|
||||
/// @class AP_LeadFilter
|
||||
/// @brief Object managing GPS lag
|
||||
class AP_LeadFilter { |
||||
public: |
||||
/// Constructor
|
||||
///
|
||||
///
|
||||
AP_LeadFilter() : |
||||
_last_velocity(0) { |
||||
} |
||||
|
||||
// setup min and max radio values in CLI
|
||||
int32_t get_position(int32_t pos, int16_t vel, float lag_in_seconds = 1.0); |
||||
void clear() { _last_velocity = 0; } |
||||
|
||||
private: |
||||
int16_t _last_velocity; |
||||
|
||||
}; |
||||
|
||||
#endif // __AP_LEADFILTER_H__
|
@ -1,65 +0,0 @@
@@ -1,65 +0,0 @@
|
||||
/* |
||||
* Example of AP Lead_Filter library. |
||||
* Code by Jason Short. 2010 |
||||
* DIYDrones.com |
||||
* |
||||
*/ |
||||
|
||||
#include <AP_Common.h> |
||||
#include <AP_Progmem.h> |
||||
#include <AP_Param.h> |
||||
#include <AP_Math.h> |
||||
#include <AP_HAL.h> |
||||
#include <AP_HAL_AVR.h> |
||||
|
||||
#include <AP_LeadFilter.h> |
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; |
||||
|
||||
AP_LeadFilter xLeadFilter; // GPS lag filter |
||||
|
||||
void setup() |
||||
{ |
||||
hal.console->println("AP_LeadFilter test ver 1.0"); |
||||
hal.scheduler->delay(500); |
||||
} |
||||
|
||||
void loop() |
||||
{ |
||||
int16_t velocity; |
||||
int32_t position; |
||||
int32_t new_position; |
||||
int16_t i; |
||||
|
||||
position = 0; |
||||
velocity = 0; |
||||
xLeadFilter.clear(); |
||||
|
||||
hal.console->printf("------------------\n"); |
||||
hal.console->printf("start position = 0, lag of 1sec.\n"); |
||||
for( i = 0; i < 10; i++ ) { |
||||
// get updated position |
||||
new_position = xLeadFilter.get_position(position, velocity); // new position with velocity of 1 m/s |
||||
hal.console->printf("start pos: %ld, start vel: %d, end pos: %ld\n", (long int)position, (int)velocity, (long int)new_position); |
||||
position = new_position; |
||||
velocity += 100; |
||||
} |
||||
|
||||
position = 0; |
||||
velocity = 0; |
||||
xLeadFilter.clear(); |
||||
|
||||
hal.console->printf("------------------\n"); |
||||
hal.console->printf("start position = 0, lag of 200ms\n"); |
||||
for( i = 0; i < 10; i++ ) { |
||||
// get updated position |
||||
new_position = xLeadFilter.get_position(position, velocity, 0.200); // new position with velocity of 1 m/s |
||||
hal.console->printf("start pos: %ld, start vel: %d, end pos: %ld\n", (long int)position, (int)velocity, (long int)new_position); |
||||
position = new_position; |
||||
velocity += 100; |
||||
} |
||||
|
||||
hal.scheduler->delay(10000); |
||||
} |
||||
|
||||
AP_HAL_MAIN(); |
@ -1,2 +0,0 @@
@@ -1,2 +0,0 @@
|
||||
BOARD = mega
|
||||
include ../../../../mk/apm.mk |
Loading…
Reference in new issue