From abb6eb24711e9dc953ae20bd7c60c2093ad61ce9 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 21 Jun 2012 11:14:20 -0700 Subject: [PATCH] Added a crude Lead Filter. Needs some fine tuning, lag specification, scaling. Works great on 4hz GPS such as Mtek. --- libraries/AP_LeadFilter/AP_LeadFilter.cpp | 29 +++++++++ libraries/AP_LeadFilter/AP_LeadFilter.h | 29 +++++++++ .../examples/AP_LeadFilter/AP_LeadFilter.pde | 63 +++++++++++++++++++ .../examples/AP_LeadFilter/Makefile | 2 + 4 files changed, 123 insertions(+) create mode 100644 libraries/AP_LeadFilter/AP_LeadFilter.cpp create mode 100644 libraries/AP_LeadFilter/AP_LeadFilter.h create mode 100644 libraries/AP_LeadFilter/examples/AP_LeadFilter/AP_LeadFilter.pde create mode 100644 libraries/AP_LeadFilter/examples/AP_LeadFilter/Makefile diff --git a/libraries/AP_LeadFilter/AP_LeadFilter.cpp b/libraries/AP_LeadFilter/AP_LeadFilter.cpp new file mode 100644 index 0000000000..30e4efcc54 --- /dev/null +++ b/libraries/AP_LeadFilter/AP_LeadFilter.cpp @@ -0,0 +1,29 @@ +/* + 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. + +*/ + +#if defined(ARDUINO) && ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +#include "AP_LeadFilter.h" + +// setup the control preferences +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; +} diff --git a/libraries/AP_LeadFilter/AP_LeadFilter.h b/libraries/AP_LeadFilter/AP_LeadFilter.h new file mode 100644 index 0000000000..adb2cfcffc --- /dev/null +++ b/libraries/AP_LeadFilter/AP_LeadFilter.h @@ -0,0 +1,29 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file RC_Channel.h +/// @brief RC_Channel manager, with EEPROM-backed storage of constants. + +#ifndef AP_LeadFilter_h +#define AP_LeadFilter_h + +#include + +/// @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); + + private: + int16_t _last_velocity; + +}; + +#endif diff --git a/libraries/AP_LeadFilter/examples/AP_LeadFilter/AP_LeadFilter.pde b/libraries/AP_LeadFilter/examples/AP_LeadFilter/AP_LeadFilter.pde new file mode 100644 index 0000000000..47e4ade948 --- /dev/null +++ b/libraries/AP_LeadFilter/examples/AP_LeadFilter/AP_LeadFilter.pde @@ -0,0 +1,63 @@ +/* + Example of AP Lead_Filter library. + Code by Jason Short. 2010 + DIYDrones.com + +*/ + +#include +#include // ArduPilot Mega Common Library +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include // GPS Lead filter + +// define APM1 or APM2 +#define APM_HARDWARE_APM1 1 +#define APM_HARDWARE_APM2 2 + +// set your hardware type here +#define CONFIG_APM_HARDWARE APM_HARDWARE_APM2 + +Arduino_Mega_ISR_Registry isr_registry; + +//////////////////////////////////////////////////////////////////////////////// +// Serial ports +//////////////////////////////////////////////////////////////////////////////// +// +// Note that FastSerial port buffers are allocated at ::begin time, +// so there is not much of a penalty to defining ports that we don't +// use. +// +FastSerialPort0(Serial); // FTDI/console +FastSerialPort1(Serial1); // GPS port +FastSerialPort3(Serial3); // Telemetry port + +//////////////////////////////////////////// +// RC Hardware +//////////////////////////////////////////// +#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + APM_RC_APM2 APM_RC; +#else + APM_RC_APM1 APM_RC; +#endif + +AP_LeadFilter xLeadFilter; // GPS lag filter + +void setup() +{ + Serial.begin(115200); + Serial.println("ArduPilot RC Channel test"); + + delay(500); + + int32_t temp = xLeadFilter.get_position(0, 100); + Serial.printf("temp %ld \n", temp); +} + +void loop() +{ +} + + + + diff --git a/libraries/AP_LeadFilter/examples/AP_LeadFilter/Makefile b/libraries/AP_LeadFilter/examples/AP_LeadFilter/Makefile new file mode 100644 index 0000000000..85b4d8856b --- /dev/null +++ b/libraries/AP_LeadFilter/examples/AP_LeadFilter/Makefile @@ -0,0 +1,2 @@ +BOARD = mega +include ../../../AP_Common/Arduino.mk