Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

PID: ported to AP_HAL

Browse Source
This commit is contained in:
Pat Hickey 2012-12-01 14:35:01 +11:00 committed by Andrew Tridgell
parent 40f254af51
commit 39d14c8a93
5 changed files with 36 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/PID/PID.cpp
View File

@ -6,6 +6,9 @@
#include <math.h>
#include "PID.h"
#include <AP_HAL.h>
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo PID::var_info[] PROGMEM = {
AP_GROUPINFO("P", 0, PID, _kp, 0),
@ -15,10 +18,9 @@ const AP_Param::GroupInfo PID::var_info[] PROGMEM = {
AP_GROUPEND
};
int32_t
PID::get_pid(int32_t error, float scaler)
int32_t PID::get_pid(int32_t error, float scaler)
{
uint32_t tnow = millis();
uint32_t tnow = hal.scheduler->millis();
uint32_t dt = tnow - _last_t;
float output = 0;
float delta_time;
@ -57,7 +59,8 @@ PID::get_pid(int32_t error, float scaler)
// 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);
((delta_time / (RC + delta_time)) *
(derivative - _last_derivative));
// update state
_last_error = error;

13
libraries/PID/PID.h
View File

@ -3,11 +3,12 @@
/// @file PID.h
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
#ifndef PID_h
#define PID_h
#ifndef __PID_H__
#define __PID_H__
#include <AP_Common.h>
#include <AP_Param.h>
#include <stdlib.h>
#include <math.h> // for fabs()
/// @class PID
@ -105,10 +106,10 @@ private:
AP_Float _kd;
AP_Int16 _imax;
float _integrator; ///< integrator value
int32_t _last_error; ///< last error for derivative
float _last_derivative; ///< last derivative for low-pass filter
uint32_t _last_t; ///< last time get_pid() was called in millis
float _integrator;///< integrator value
int32_t _last_error;///< last error for derivative
float _last_derivative;///< last derivative for low-pass filter
uint32_t _last_t;///< last time get_pid() was called in millis
int32_t _get_pid(int32_t error, uint16_t dt, float scaler);

0
libraries/PID/examples/pid/Arduino.h Normal file
View File

0
libraries/PID/examples/pid/nocore.inoflag Normal file
View File

45
libraries/PID/examples/pid/pid.pde
View File

@ -3,37 +3,32 @@
* 2010 Code by Jason Short. DIYDrones.com
*/
#include <FastSerial.h>
#include <avr/pgmspace.h>
#include <AP_HAL.h>
#include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_Param.h>
#include <AP_Math.h>
#include <APM_RC.h> // ArduPilot RC Library
#include <PID.h> // ArduPilot Mega RC Library
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_HAL_AVR.h>
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
#else
const AP_HAL::HAL& hal = AP_HAL_AVR_APM1;
#endif
long radio_in;
long radio_trim;
Arduino_Mega_ISR_Registry isr_registry;
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
APM_RC_APM2 APM_RC;
#else
APM_RC_APM1 APM_RC;
#endif
PID pid;
void setup()
{
Serial.begin(38400);
Serial.println("ArduPilot Mega PID library test");
APM_RC.Init(&isr_registry); // APM Radio initialization
hal.console->println("ArduPilot Mega PID library test");
delay(1000);
hal.scheduler->delay(1000);
//rc.trim();
radio_trim = APM_RC.InputCh(0);
radio_trim = hal.rcin->read(0);
pid.kP(1);
pid.kI(0);
@ -45,16 +40,20 @@ void setup()
pid.kD(0);
pid.imax(0);
pid.load_gains();
Serial.printf("P %f I %f D %f imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax());
hal.console->printf_P(
PSTR("P %f I %f D %f imax %f\n"),
pid.kP(), pid.kI(), pid.kD(), pid.imax());
}
void loop()
{
delay(20);
hal.scheduler->delay(20);
//rc.read_pwm();
long error = APM_RC.InputCh(0) - radio_trim;
long control = pid.get_pid(error, 1);
long error = hal.rcin->read(0) - radio_trim;
long control= pid.get_pid(error, 1);
Serial.print("control: ");
Serial.println(control,DEC);
hal.console->print("control: ");
hal.console->println(control,DEC);
}
AP_HAL_MAIN();