ardupilot/libraries/APO/AP_Controller.h

168 lines
4.2 KiB
C++

/*
* AP_Controller.h
* Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef AP_Controller_H
#define AP_Controller_H
#include "AP_Navigator.h"
#include "AP_Guide.h"
#include "AP_HardwareAbstractionLayer.h"
#include "../AP_Common/AP_Vector.h"
#include "../AP_Common/AP_Var.h"
namespace apo {
/// Controller class
class AP_Controller {
public:
AP_Controller(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
_nav(nav), _guide(guide), _hal(hal) {
}
bool commLost() {
if (_hal->getTimeSinceLastHeartBeat() > heartbeatTimeout) return true;
else return false;
}
virtual void update(const float & dt) = 0;
MAV_MODE getMode() { return _mode; }
protected:
MAV_MODE _mode;
AP_Navigator * _nav;
AP_Guide * _guide;
AP_HardwareAbstractionLayer * _hal;
};
class Pid2 {
public:
Pid2(AP_Var::Key key, const prog_char_t * name, float kP = 0.0,
float kI = 0.0, float kD = 0.0, float iMax = 0.0, float yMax = 0.0,
uint8_t dFcut = 20.0) :
_group(key, name), _eP(0), _eI(0), _eD(0),
_kP(&_group, 1, kP, PSTR("P")),
_kI(&_group, 2, kI, PSTR("I")),
_kD(&_group, 3, kD, PSTR("D")),
_iMax(&_group, 4, iMax, PSTR("IMAX")),
_yMax(&_group, 5, yMax, PSTR("YMAX")),
_fCut(&_group, 6, dFcut, PSTR("FCUT")) {
}
float update(const float & input, const float & dt) {
// derivative with low pass
float RC = 1 / (2 * M_PI * _fCut); // low pass filter
_eD = _eD + ((_eP - input) / dt - _eD) * (dt / (dt + RC));
// proportional, note must come after derivative
// because derivatve uses _eP as previous value
_eP = input;
// integral
_eI += _eP * dt;
// wind up guard
if (_eI > _iMax)
_eI = _iMax;
if (_eI < -_iMax)
_eI = -_iMax;
// pid sum
float y = _kP * _eP + _kI * _eI + _kD * _eD;
// saturation
if (y > _yMax)
y = _yMax;
if (y < -_yMax)
y = -_yMax;
return y;
}
protected:
AP_Var_group _group; /// helps with parameter management
float _eP; /// input
float _eI; /// integral of input
float _eD; /// derivative of input
AP_Float _kP; /// proportional gain
AP_Float _kI; /// integral gain
AP_Float _kD; /// derivative gain
AP_Float _iMax; /// integrator saturation
AP_Float _yMax; /// output saturation
AP_Uint8 _fCut; /// derivative low-pass cut freq (Hz)
};
/// PID(DFB) block
class PidDFB2 {
public:
PidDFB2(AP_Var::Key key, const prog_char_t * name,
float kP = 0.0, float kI = 0.0, float kD = 0.0, float iMax = 0.0,
float yMax = 0.0) :
_group(key, name), _eP(0), _eI(0), _eD(0),
_kP(&_group, 1, kP, PSTR("P")),
_kI(&_group, 2, kI, PSTR("I")),
_kD(&_group, 3, kD, PSTR("D")),
_iMax(&_group, 4, iMax, PSTR("IMAX")),
_yMax(&_group, 5, yMax, PSTR("YMAX")) {
}
float update(const float & input, const float & derivative, const float & dt) {
// proportional, note must come after derivative
// because derivative uses _eP as previous value
_eP = input;
// integral
_eI += _eP * dt;
// wind up guard
if (_eI > _iMax)
_eI = _iMax;
if (_eI < -_iMax)
_eI = -_iMax;
// pid sum
float y = _kP * _eP + _kI * _eI - _kD * derivative;
// saturation
if (y > _yMax)
y = _yMax;
if (y < -_yMax)
y = -_yMax;
return y;
}
protected:
AP_Var_group _group; /// helps with parameter management
float _eP; /// input
float _eI; /// integral of input
float _eD; /// derivative of input
AP_Float _kP; /// proportional gain
AP_Float _kI; /// integral gain
AP_Float _kD; /// derivative gain
AP_Float _iMax; /// integrator saturation
AP_Float _yMax; /// integrator saturation
};
} // apo
#endif // AP_Controller_H
// vim:ts=4:sw=4:expandtab