Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -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 <math.h>
#include "PID.h" #include "PID.h"
#include <AP_HAL.h>
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo PID::var_info[] PROGMEM = { const AP_Param::GroupInfo PID::var_info[] PROGMEM = {
AP_GROUPINFO("P", 0, PID, _kp, 0), AP_GROUPINFO("P", 0, PID, _kp, 0),
@ -15,10 +18,9 @@ const AP_Param::GroupInfo PID::var_info[] PROGMEM = {
AP_GROUPEND AP_GROUPEND
}; };
int32_t int32_t PID::get_pid(int32_t error, float scaler)
PID::get_pid(int32_t error, float scaler)
{ {
uint32_t tnow = millis(); uint32_t tnow = hal.scheduler->millis();
uint32_t dt = tnow - _last_t; uint32_t dt = tnow - _last_t;
float output = 0; float output = 0;
float delta_time; float delta_time;
@ -57,7 +59,8 @@ PID::get_pid(int32_t error, float scaler)
// high frequency noise that can drive the controller crazy // high frequency noise that can drive the controller crazy
float RC = 1/(2*M_PI*_fCut); float RC = 1/(2*M_PI*_fCut);
derivative = _last_derivative + derivative = _last_derivative +
(delta_time / (RC + delta_time)) * (derivative - _last_derivative); ((delta_time / (RC + delta_time)) *
(derivative - _last_derivative));
// update state // update state
_last_error = error; _last_error = error;

13
libraries/PID/PID.h
View File

@ -3,11 +3,12 @@
/// @file PID.h /// @file PID.h
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants. /// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
#ifndef PID_h #ifndef __PID_H__
#define PID_h #define __PID_H__
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Param.h> #include <AP_Param.h>
#include <stdlib.h>
#include <math.h> // for fabs() #include <math.h> // for fabs()
/// @class PID /// @class PID
@ -105,10 +106,10 @@ private:
AP_Float _kd; AP_Float _kd;
AP_Int16 _imax; AP_Int16 _imax;
float _integrator; ///< integrator value float _integrator;///< integrator value
int32_t _last_error; ///< last error for derivative int32_t _last_error;///< last error for derivative
float _last_derivative; ///< last derivative for low-pass filter float _last_derivative;///< last derivative for low-pass filter
uint32_t _last_t; ///< last time get_pid() was called in millis uint32_t _last_t;///< last time get_pid() was called in millis
int32_t _get_pid(int32_t error, uint16_t dt, float scaler); 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 * 2010 Code by Jason Short. DIYDrones.com
*/ */
#include <FastSerial.h> #include <AP_HAL.h>
#include <avr/pgmspace.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_Param.h> #include <AP_Param.h>
#include <AP_Math.h> #include <AP_Math.h>
#include <APM_RC.h> // ArduPilot RC Library
#include <PID.h> // ArduPilot Mega 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_in;
long radio_trim; 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; PID pid;
void setup() void setup()
{ {
Serial.begin(38400); hal.console->println("ArduPilot Mega PID library test");
Serial.println("ArduPilot Mega PID library test");
APM_RC.Init(&isr_registry); // APM Radio initialization
delay(1000); hal.scheduler->delay(1000);
//rc.trim(); //rc.trim();
radio_trim = APM_RC.InputCh(0); radio_trim = hal.rcin->read(0);
pid.kP(1); pid.kP(1);
pid.kI(0); pid.kI(0);
@ -45,16 +40,20 @@ void setup()
pid.kD(0); pid.kD(0);
pid.imax(0); pid.imax(0);
pid.load_gains(); 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() void loop()
{ {
delay(20); hal.scheduler->delay(20);
//rc.read_pwm(); //rc.read_pwm();
long error = APM_RC.InputCh(0) - radio_trim; long error = hal.rcin->read(0) - radio_trim;
long control = pid.get_pid(error, 1); long control= pid.get_pid(error, 1);
Serial.print("control: "); hal.console->print("control: ");
Serial.println(control,DEC); hal.console->println(control,DEC);
} }
AP_HAL_MAIN();