cleanup: removed unused AP_PID library
This commit is contained in:
Andrew Tridgell
parent
6b8c39dd10
commit
5b4321d367
@ -1,65 +0,0 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
/// @file AP_PID.cpp
|
||||
/// @brief Generic PID algorithm
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#include "AP_PID.h"
|
||||
|
||||
AP_PID::AP_PID()
|
||||
{
|
||||
}
|
||||
|
||||
long
|
||||
AP_PID::get_pid(int32_t error, uint16_t dt, float scaler)
|
||||
{
|
||||
float output = 0;
|
||||
float delta_time = (float)dt / 1000.0;
|
||||
|
||||
// Compute proportional component
|
||||
output += error * _kp;
|
||||
|
||||
// Compute derivative component if time has elapsed
|
||||
if ((fabs(_kd) > 0) && (dt > 0)) {
|
||||
float derivative = (error - _last_error) / delta_time;
|
||||
|
||||
// discrete low pass filter, cuts out the
|
||||
// high frequency noise that can drive the controller crazy
|
||||
float RC = 1/(2*M_PI*_fCut);
|
||||
derivative = _last_derivative +
|
||||
(delta_time / (RC + delta_time)) * (derivative - _last_derivative);
|
||||
|
||||
// update state
|
||||
_last_error = error;
|
||||
_last_derivative = derivative;
|
||||
|
||||
// add in derivative component
|
||||
output += _kd * derivative;
|
||||
}
|
||||
|
||||
// scale the P and D components
|
||||
output *= scaler;
|
||||
|
||||
// Compute integral component if time has elapsed
|
||||
if ((fabs(_ki) > 0) && (dt > 0)) {
|
||||
_integrator += (error * _ki) * scaler * delta_time;
|
||||
if (_integrator < -_imax) {
|
||||
_integrator = -_imax;
|
||||
} else if (_integrator > _imax) {
|
||||
_integrator = _imax;
|
||||
}
|
||||
output += _integrator;
|
||||
}
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
void
|
||||
AP_PID::reset_I()
|
||||
{
|
||||
_integrator = 0;
|
||||
_last_error = 0;
|
||||
_last_derivative = 0;
|
||||
}
|
||||
|
||||
@ -1,73 +0,0 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
/// @file AP_PID.h
|
||||
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
|
||||
|
||||
#ifndef AP_PID_h
|
||||
#define AP_PID_h
|
||||
|
||||
#include <AP_Common.h>
|
||||
#include <math.h> // for fabs()
|
||||
|
||||
/// @class AP_PID
|
||||
/// @brief Object managing one PID control
|
||||
class AP_PID {
|
||||
public:
|
||||
|
||||
AP_PID();
|
||||
|
||||
long get_pid(int32_t error, uint16_t dt, float scaler = 1.0);
|
||||
|
||||
/// Reset the PID integrator
|
||||
///
|
||||
void reset_I();
|
||||
|
||||
void kP(const float v) {
|
||||
_kp = v;
|
||||
}
|
||||
void kI(const float v) {
|
||||
_ki = v;
|
||||
}
|
||||
void kD(const float v) {
|
||||
_kd = v;
|
||||
}
|
||||
void imax(const int16_t v) {
|
||||
_imax = v;
|
||||
}
|
||||
|
||||
float kP() {
|
||||
return _kp;
|
||||
}
|
||||
float kI() {
|
||||
return _ki;
|
||||
}
|
||||
float kD() {
|
||||
return _kd;
|
||||
}
|
||||
float imax() {
|
||||
return _imax;
|
||||
}
|
||||
|
||||
float get_integrator() const {
|
||||
return _integrator;
|
||||
}
|
||||
|
||||
private:
|
||||
float _kp;
|
||||
float _ki;
|
||||
float _kd;
|
||||
float _imax;
|
||||
|
||||
float _integrator; ///< integrator value
|
||||
int32_t _last_error; ///< last error for derivative
|
||||
float _last_derivative; ///< last derivative for low-pass filter
|
||||
|
||||
/// Low pass filter cut frequency for derivative calculation.
|
||||
///
|
||||
/// 20 Hz becasue anything over that is probably noise, see
|
||||
/// http://en.wikipedia.org/wiki/Low-pass_filter.
|
||||
///
|
||||
static const uint8_t _fCut = 20;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -1,52 +0,0 @@
|
||||
/*
|
||||
* Example of PID library.
|
||||
* 2010 Code by Jason Short. DIYDrones.com
|
||||
*/
|
||||
|
||||
#include <Arduino_Mega_ISR_Registry.h>
|
||||
#include <FastSerial.h>
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
#include <APM_RC.h> // ArduPilot RC Library
|
||||
#include <AP_PID.h> // ArduPilot Mega RC Library
|
||||
|
||||
long radio_in;
|
||||
long radio_trim;
|
||||
|
||||
Arduino_Mega_ISR_Registry isr_registry;
|
||||
AP_PID pid;
|
||||
APM_RC_APM1 APM_RC;
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(115200);
|
||||
Serial.println("ArduPilot Mega AP_PID library test");
|
||||
|
||||
isr_registry.init();
|
||||
APM_RC.Init(&isr_registry); // APM Radio initialization
|
||||
|
||||
delay(1000);
|
||||
//rc.trim();
|
||||
radio_trim = APM_RC.InputCh(0);
|
||||
|
||||
pid.kP(1);
|
||||
pid.kI(0);
|
||||
pid.kD(0.5);
|
||||
pid.imax(50);
|
||||
pid.kP(0);
|
||||
pid.kI(0);
|
||||
pid.kD(0);
|
||||
pid.imax(0);
|
||||
Serial.printf("P %f I %f D %f imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax());
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
delay(20);
|
||||
//rc.read_pwm();
|
||||
long error = APM_RC.InputCh(0) - radio_trim;
|
||||
long control = pid.get_pid(error, 20, 1);
|
||||
|
||||
Serial.print("control: ");
|
||||
Serial.println(control,DEC);
|
||||
}
|
||||
@ -1,2 +0,0 @@
|
||||
BOARD = mega
|
||||
include ../../../AP_Common/Arduino.mk
|
||||
@ -1,9 +0,0 @@
|
||||
PID KEYWORD1
|
||||
get_pid KEYWORD2
|
||||
reset_I KEYWORD2
|
||||
kP KEYWORD2
|
||||
kD KEYWORD2
|
||||
kI KEYWORD2
|
||||
imax KEYWORD2
|
||||
load_gains KEYWORD2
|
||||
save_gains KEYWORD2
|
||||
Loading…
Reference in New Issue
Block a user