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 @@ |
|||||||
/*
|
|
||||||
* 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 @@ |
|||||||
// -*- 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 @@ |
|||||||
/* |
|
||||||
* 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 @@ |
|||||||
BOARD = mega
|
|
||||||
include ../../../../mk/apm.mk |
|
Loading…
Reference in new issue