ardupilot/libraries/APO/AP_Controller.h

337 lines
9.8 KiB
C
Raw Normal View History

2011-09-28 21:51:12 -03:00
/*
* 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 <inttypes.h>
#include "../GCS_MAVLink/GCS_MAVLink.h"
#include <math.h>
#include "../AP_Common/AP_Var.h"
#include "AP_Var_keys.h"
class AP_Var_group;
2011-09-28 21:51:12 -03:00
namespace apo {
class AP_HardwareAbstractionLayer;
class AP_Guide;
class AP_Navigator;
class Menu;
class AP_ArmingMechanism;
2011-09-28 21:51:12 -03:00
/// Controller class
class AP_Controller {
public:
2011-10-26 13:31:11 -03:00
AP_Controller(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal,
AP_ArmingMechanism * armingMechanism,
const uint8_t _chMode,
const uint16_t key = k_cntrl,
const prog_char_t * name = NULL);
// derived class cannot override
void update(const float dt);
void setAllRadioChannelsToNeutral();
2011-10-26 13:31:11 -03:00
void setAllRadioChannelsManually();
void handleMotors();
// derived class must define
virtual void setMotors() = 0;
virtual void manualLoop(const float dt) = 0;
virtual void autoLoop(const float dt) = 0;
virtual void handleFailsafe() = 0;
// access
MAV_MODE getMode() {
return _mode;
}
void setMode(MAV_MODE mode) {
_mode = mode;
}
MAV_STATE getState() const {
return _state;
}
void setState(MAV_STATE state) {
_state = state;
}
2011-09-28 21:51:12 -03:00
protected:
2011-10-26 13:31:11 -03:00
AP_Navigator * _nav;
AP_Guide * _guide;
AP_HardwareAbstractionLayer * _hal;
AP_ArmingMechanism * _armingMechanism;
uint8_t _chMode;
2011-10-26 13:31:11 -03:00
AP_Var_group _group;
MAV_MODE _mode;
MAV_STATE _state;
2011-09-28 21:51:12 -03:00
};
class AP_ControllerBlock {
public:
2011-10-26 13:31:11 -03:00
AP_ControllerBlock(AP_Var_group * group, uint8_t groupStart,
uint8_t groupLength) :
_group(group), _groupStart(groupStart),
_groupEnd(groupStart + groupLength) {
}
uint8_t getGroupEnd() {
return _groupEnd;
}
2011-09-28 21:51:12 -03:00
protected:
2011-10-26 13:31:11 -03:00
AP_Var_group * _group; /// helps with parameter management
uint8_t _groupStart;
uint8_t _groupEnd;
2011-09-28 21:51:12 -03:00
};
class BlockLowPass: public AP_ControllerBlock {
public:
2011-10-26 13:31:11 -03:00
BlockLowPass(AP_Var_group * group, uint8_t groupStart, float fCut,
const prog_char_t * fCutLabel = NULL) :
AP_ControllerBlock(group, groupStart, 1),
_fCut(group, groupStart, fCut, fCutLabel ? : PSTR("fCut")),
_y(0) {
}
float update(const float & input, const float & dt) {
float RC = 1 / (2 * M_PI * _fCut); // low pass filter
_y = _y + (input - _y) * (dt / (dt + RC));
return _y;
}
2011-09-28 21:51:12 -03:00
protected:
2011-10-26 13:31:11 -03:00
AP_Float _fCut;
float _y;
2011-09-28 21:51:12 -03:00
};
class BlockSaturation: public AP_ControllerBlock {
public:
2011-10-26 13:31:11 -03:00
BlockSaturation(AP_Var_group * group, uint8_t groupStart, float yMax,
const prog_char_t * yMaxLabel = NULL) :
AP_ControllerBlock(group, groupStart, 1),
_yMax(group, groupStart, yMax, yMaxLabel ? : PSTR("yMax")) {
}
float update(const float & input) {
2011-09-28 21:51:12 -03:00
2011-10-26 13:31:11 -03:00
// pid sum
float y = input;
2011-09-28 21:51:12 -03:00
2011-10-26 13:31:11 -03:00
// saturation
if (y > _yMax)
y = _yMax;
if (y < -_yMax)
y = -_yMax;
return y;
}
2011-09-28 21:51:12 -03:00
protected:
2011-10-26 13:31:11 -03:00
AP_Float _yMax; /// output saturation
2011-09-28 21:51:12 -03:00
};
class BlockDerivative {
public:
2011-10-26 13:31:11 -03:00
BlockDerivative() :
_lastInput(0), firstRun(true) {
}
float update(const float & input, const float & dt) {
float derivative = (input - _lastInput) / dt;
_lastInput = input;
if (firstRun) {
firstRun = false;
return 0;
} else
return derivative;
}
2011-09-28 21:51:12 -03:00
protected:
2011-10-26 13:31:11 -03:00
float _lastInput; /// last input
bool firstRun;
2011-09-28 21:51:12 -03:00
};
class BlockIntegral {
public:
2011-10-26 13:31:11 -03:00
BlockIntegral() :
_i(0) {
}
float update(const float & input, const float & dt) {
_i += input * dt;
return _i;
}
2011-09-28 21:51:12 -03:00
protected:
2011-10-26 13:31:11 -03:00
float _i; /// integral
2011-09-28 21:51:12 -03:00
};
class BlockP: public AP_ControllerBlock {
public:
2011-10-26 13:31:11 -03:00
BlockP(AP_Var_group * group, uint8_t groupStart, float kP,
const prog_char_t * kPLabel = NULL) :
AP_ControllerBlock(group, groupStart, 1),
_kP(group, groupStart, kP, kPLabel ? : PSTR("p")) {
}
2011-09-28 21:51:12 -03:00
2011-10-26 13:31:11 -03:00
float update(const float & input) {
return _kP * input;
}
2011-09-28 21:51:12 -03:00
protected:
2011-10-26 13:31:11 -03:00
AP_Float _kP; /// proportional gain
2011-09-28 21:51:12 -03:00
};
class BlockI: public AP_ControllerBlock {
public:
2011-10-26 13:31:11 -03:00
BlockI(AP_Var_group * group, uint8_t groupStart, float kI, float iMax,
const prog_char_t * kILabel = NULL,
const prog_char_t * iMaxLabel = NULL) :
AP_ControllerBlock(group, groupStart, 2),
_kI(group, groupStart, kI, kILabel ? : PSTR("i")),
_blockSaturation(group, groupStart + 1, iMax, iMaxLabel ? : PSTR("iMax")),
_eI(0) {
}
2011-09-28 21:51:12 -03:00
2011-10-26 13:31:11 -03:00
float update(const float & input, const float & dt) {
// integral
_eI += input * dt;
_eI = _blockSaturation.update(_eI);
return _kI * _eI;
}
2011-09-28 21:51:12 -03:00
protected:
2011-10-26 13:31:11 -03:00
AP_Float _kI; /// integral gain
BlockSaturation _blockSaturation; /// integrator saturation
float _eI; /// integral of input
2011-09-28 21:51:12 -03:00
};
class BlockD: public AP_ControllerBlock {
public:
2011-10-26 13:31:11 -03:00
BlockD(AP_Var_group * group, uint8_t groupStart, float kD, uint8_t dFCut,
const prog_char_t * kDLabel = NULL,
const prog_char_t * dFCutLabel = NULL) :
AP_ControllerBlock(group, groupStart, 2),
_blockLowPass(group, groupStart, dFCut,
dFCutLabel ? : PSTR("dFCut")),
_kD(group, _blockLowPass.getGroupEnd(), kD,
kDLabel ? : PSTR("d")), _eP(0) {
}
float update(const float & input, const float & dt) {
// derivative with low pass
float _eD = _blockLowPass.update((input - _eP) / dt, dt);
2011-10-26 13:31:11 -03:00
// proportional, note must come after derivative
// because derivatve uses _eP as previous value
_eP = input;
return _kD * _eD;
}
2011-09-28 21:51:12 -03:00
protected:
2011-10-26 13:31:11 -03:00
BlockLowPass _blockLowPass;
AP_Float _kD; /// derivative gain
float _eP; /// input
2011-09-28 21:51:12 -03:00
};
class BlockPID: public AP_ControllerBlock {
public:
2011-10-26 13:31:11 -03:00
BlockPID(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float kD, float iMax, float yMax, uint8_t dFcut) :
AP_ControllerBlock(group, groupStart, 6),
_blockP(group, groupStart, kP),
_blockI(group, _blockP.getGroupEnd(), kI, iMax),
_blockD(group, _blockI.getGroupEnd(), kD, dFcut),
_blockSaturation(group, _blockD.getGroupEnd(), yMax) {
}
2011-09-28 21:51:12 -03:00
2011-10-26 13:31:11 -03:00
float update(const float & input, const float & dt) {
return _blockSaturation.update(
_blockP.update(input) + _blockI.update(input, dt)
+ _blockD.update(input, dt));
}
2011-09-28 21:51:12 -03:00
protected:
2011-10-26 13:31:11 -03:00
BlockP _blockP;
BlockI _blockI;
BlockD _blockD;
BlockSaturation _blockSaturation;
2011-09-28 21:51:12 -03:00
};
class BlockPI: public AP_ControllerBlock {
public:
2011-10-26 13:31:11 -03:00
BlockPI(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float iMax, float yMax) :
AP_ControllerBlock(group, groupStart, 4),
_blockP(group, groupStart, kP),
_blockI(group, _blockP.getGroupEnd(), kI, iMax),
_blockSaturation(group, _blockI.getGroupEnd(), yMax) {
}
2011-09-28 21:51:12 -03:00
2011-10-26 13:31:11 -03:00
float update(const float & input, const float & dt) {
2011-09-28 21:51:12 -03:00
2011-10-26 13:31:11 -03:00
float y = _blockP.update(input) + _blockI.update(input, dt);
return _blockSaturation.update(y);
}
2011-09-28 21:51:12 -03:00
protected:
2011-10-26 13:31:11 -03:00
BlockP _blockP;
BlockI _blockI;
BlockSaturation _blockSaturation;
2011-09-28 21:51:12 -03:00
};
class BlockPD: public AP_ControllerBlock {
public:
2011-10-26 13:31:11 -03:00
BlockPD(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float kD, float iMax, float yMax, uint8_t dFcut) :
AP_ControllerBlock(group, groupStart, 4),
_blockP(group, groupStart, kP),
_blockD(group, _blockP.getGroupEnd(), kD, dFcut),
_blockSaturation(group, _blockD.getGroupEnd(), yMax) {
}
2011-09-28 21:51:12 -03:00
2011-10-26 13:31:11 -03:00
float update(const float & input, const float & dt) {
2011-09-28 21:51:12 -03:00
2011-10-26 13:31:11 -03:00
float y = _blockP.update(input) + _blockD.update(input, dt);
return _blockSaturation.update(y);
}
2011-09-28 21:51:12 -03:00
protected:
2011-10-26 13:31:11 -03:00
BlockP _blockP;
BlockD _blockD;
BlockSaturation _blockSaturation;
2011-09-28 21:51:12 -03:00
};
/// PID with derivative feedback (ignores reference derivative)
class BlockPIDDfb: public AP_ControllerBlock {
public:
2011-10-26 13:31:11 -03:00
BlockPIDDfb(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float kD, float iMax, float yMax, float dFCut,
const prog_char_t * dFCutLabel = NULL,
const prog_char_t * dLabel = NULL) :
AP_ControllerBlock(group, groupStart, 5),
_blockP(group, groupStart, kP),
_blockI(group, _blockP.getGroupEnd(), kI, iMax),
_blockSaturation(group, _blockI.getGroupEnd(), yMax),
_blockLowPass(group, _blockSaturation.getGroupEnd(), dFCut,
dFCutLabel ? : PSTR("dFCut")),
_kD(group, _blockLowPass.getGroupEnd(), kD, dLabel ? : PSTR("d"))
{
}
float update(const float & input, const float & derivative,
const float & dt) {
float y = _blockP.update(input) + _blockI.update(input, dt) - _kD
* _blockLowPass.update(derivative,dt);
return _blockSaturation.update(y);
}
2011-09-28 21:51:12 -03:00
protected:
2011-10-26 13:31:11 -03:00
BlockP _blockP;
BlockI _blockI;
BlockSaturation _blockSaturation;
BlockLowPass _blockLowPass;
AP_Float _kD; /// derivative gain
2011-09-28 21:51:12 -03:00
};
} // apo
#endif // AP_Controller_H
// vim:ts=4:sw=4:expandtab