From fc84c15426ae2b21343ba03346cfb179ee685462 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 24 Nov 2011 13:28:14 -0500 Subject: [PATCH] Separated controller block class. Also improved documentation of AP_ControllerBlock. --- ArduBoat/ControllerBoat.h | 4 +- ArduRover/ControllerCar.h | 2 +- ArduRover/ControllerTank.h | 2 +- apo/ControllerPlane.h | 2 +- apo/ControllerQuad.h | 2 +- libraries/APO/APO.h | 1 + libraries/APO/AP_ArmingMechanism.cpp | 1 - libraries/APO/AP_Controller.cpp | 8 +- libraries/APO/AP_Controller.h | 352 +++++++-------------------- libraries/APO/AP_ControllerBlock.cpp | 179 ++++++++++++++ libraries/APO/AP_ControllerBlock.h | 237 ++++++++++++++++++ libraries/AP_Common/include/menu.h | 2 + 12 files changed, 508 insertions(+), 284 deletions(-) create mode 100644 libraries/APO/AP_ControllerBlock.cpp create mode 100644 libraries/APO/AP_ControllerBlock.h diff --git a/ArduBoat/ControllerBoat.h b/ArduBoat/ControllerBoat.h index 2e32a371e7..9db7354b0f 100644 --- a/ArduBoat/ControllerBoat.h +++ b/ArduBoat/ControllerBoat.h @@ -8,7 +8,7 @@ #ifndef CONTROLLERBOAT_H_ #define CONTROLLERBOAT_H_ -#include "../APO/AP_Controller.h" +#include "../APO/APO.h" namespace apo { @@ -16,7 +16,7 @@ class ControllerBoat: public AP_Controller { public: ControllerBoat(AP_Navigator * nav, AP_Guide * guide, AP_HardwareAbstractionLayer * hal) : - AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode), + AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl), pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut), pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP, diff --git a/ArduRover/ControllerCar.h b/ArduRover/ControllerCar.h index 2a244f1cfd..1989f8de8e 100644 --- a/ArduRover/ControllerCar.h +++ b/ArduRover/ControllerCar.h @@ -16,7 +16,7 @@ class ControllerCar: public AP_Controller { public: ControllerCar(AP_Navigator * nav, AP_Guide * guide, AP_HardwareAbstractionLayer * hal) : - AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode), + AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl), pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut), pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP, diff --git a/ArduRover/ControllerTank.h b/ArduRover/ControllerTank.h index 0c091d6d70..a72edad6c7 100644 --- a/ArduRover/ControllerTank.h +++ b/ArduRover/ControllerTank.h @@ -16,7 +16,7 @@ class ControllerTank: public AP_Controller { public: ControllerTank(AP_Navigator * nav, AP_Guide * guide, AP_HardwareAbstractionLayer * hal) : - AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_str,0.1,-0.9,0.9),ch_mode), + AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_str,0.1,-0.9,0.9),ch_mode,k_cntrl), pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, steeringI, steeringD, steeringIMax, steeringYMax, steeringDFCut), pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP, diff --git a/apo/ControllerPlane.h b/apo/ControllerPlane.h index 3c9d6e56c6..f6e17a46b0 100644 --- a/apo/ControllerPlane.h +++ b/apo/ControllerPlane.h @@ -16,7 +16,7 @@ class ControllerPlane: public AP_Controller { public: ControllerPlane(AP_Navigator * nav, AP_Guide * guide, AP_HardwareAbstractionLayer * hal) : - AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,this,ch_thrust,ch_yaw,0.1,-0.9,0.9),ch_mode), + AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,this,ch_thrust,ch_yaw,0.1,-0.9,0.9),ch_mode,k_cntrl), _trimGroup(k_trim, PSTR("trim_")), _rdrAilMix(&_group, 2, rdrAilMix, PSTR("rdrAilMix")), _needsTrim(false), diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index 0923f2a691..3a920464a3 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -18,7 +18,7 @@ class ControllerQuad: public AP_Controller { public: ControllerQuad(AP_Navigator * nav, AP_Guide * guide, AP_HardwareAbstractionLayer * hal) : - AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,this,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode), + AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,this,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode, k_cntrl), pidRoll(new AP_Var_group(k_pidRoll, PSTR("ROLL_")), 1, PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU, PID_ATT_LIM, PID_ATT_DFCUT), diff --git a/libraries/APO/APO.h b/libraries/APO/APO.h index 360b93dada..3fe10bb203 100644 --- a/libraries/APO/APO.h +++ b/libraries/APO/APO.h @@ -11,6 +11,7 @@ #include "AP_Autopilot.h" #include "AP_Guide.h" #include "AP_Controller.h" +#include "AP_ControllerBlock.h" #include "AP_HardwareAbstractionLayer.h" #include "AP_MavlinkCommand.h" #include "AP_Navigator.h" diff --git a/libraries/APO/AP_ArmingMechanism.cpp b/libraries/APO/AP_ArmingMechanism.cpp index bb6bf22a53..6236e590c4 100644 --- a/libraries/APO/AP_ArmingMechanism.cpp +++ b/libraries/APO/AP_ArmingMechanism.cpp @@ -3,7 +3,6 @@ * */ - #include "AP_ArmingMechanism.h" #include "AP_Controller.h" #include "AP_RcChannel.h" diff --git a/libraries/APO/AP_Controller.cpp b/libraries/APO/AP_Controller.cpp index 9925923f59..54ae93ddbd 100644 --- a/libraries/APO/AP_Controller.cpp +++ b/libraries/APO/AP_Controller.cpp @@ -5,16 +5,15 @@ * Author: jgoppert */ -#include "AP_Controller.h" #include "../FastSerial/FastSerial.h" #include "AP_ArmingMechanism.h" #include "AP_BatteryMonitor.h" #include "AP_HardwareAbstractionLayer.h" -#include "../AP_Common/include/menu.h" #include "AP_RcChannel.h" #include "../GCS_MAVLink/include/mavlink_types.h" #include "constants.h" #include "AP_CommLink.h" +#include "AP_Controller.h" namespace apo { @@ -92,10 +91,6 @@ void AP_Controller::update(const float dt) { } // this sends commands to motors - handleMotors(); -} - -void AP_Controller::handleMotors() { if(getState()==MAV_STATE_ACTIVE) { digitalWrite(_hal->aLedPin, HIGH); setMotors(); @@ -105,6 +100,5 @@ void AP_Controller::handleMotors() { } } - } // namespace apo // vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Controller.h b/libraries/APO/AP_Controller.h index 639ec22b71..cd3c10a81e 100644 --- a/libraries/APO/AP_Controller.h +++ b/libraries/APO/AP_Controller.h @@ -19,315 +19,127 @@ #ifndef AP_Controller_H #define AP_Controller_H -#include -#include "../GCS_MAVLink/GCS_MAVLink.h" -#include +// inclusions +#include "../AP_Common/AP_Common.h" #include "../AP_Common/AP_Var.h" -#include "AP_Var_keys.h" - -class AP_Var_group; +#include +#include +#include "../GCS_MAVLink/GCS_MAVLink.h" namespace apo { +// forward declarations within apo class AP_HardwareAbstractionLayer; class AP_Guide; class AP_Navigator; class Menu; class AP_ArmingMechanism; -/// Controller class +/// +// The control system class. +// Given where the vehicle wants to go and where it is, +// this class is responsible for sending commands to the +// motors. It is also responsible for monitoring manual +// input. class AP_Controller { public: + /// + // The controller constructor. + // Creates the control system. + // @nav the navigation system + // @guide the guidance system + // @hal the hardware abstraction layer + // @armingMechanism the device that controls arming/ disarming + // @chMode the channel that the mode switch is on + // @key the unique key for the control system saved AP_Var variables + // @name the name of the control system 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 uint16_t key, const prog_char_t * name = NULL); - // derived class cannot override + /// + // The loop callback function. + // The callback function for the controller loop. + // This is inherited from loop. + // This function cannot be overriden. + // @dt The loop update interval. void update(const float dt); - void setAllRadioChannelsToNeutral(); - void setAllRadioChannelsManually(); - void handleMotors(); - // derived class must define + /// + // This sets all radio outputs to neutral. + // This function cannot be overriden. + void setAllRadioChannelsToNeutral(); + + /// + // This sets all radio outputs using the radio input. + // This function cannot be overriden. + void setAllRadioChannelsManually(); + + /// + // Sets the motor pwm outputs. + // This function sets the motors given the control system outputs. + // This function must be defined. There is no default implementation. virtual void setMotors() = 0; + + /// + // The manual control loop function. + // This uses radio to control the aircraft. + // This function must be defined. There is no default implementation. + // @dt The loop update interval. virtual void manualLoop(const float dt) = 0; + + /// + // The automatic control update function. + // This loop is responsible for taking the + // vehicle to a waypoint. + // This function must be defined. There is no default implementation. + // @dt The loop update interval. virtual void autoLoop(const float dt) = 0; + + /// + // Handles failsafe events. + // This function is responsible for setting the mode of the vehicle during + // a failsafe event (low battery, loss of gcs comms, ...). + // This function must be defined. There is no default implementation. virtual void handleFailsafe() = 0; - // access + /// + // The mode accessor. + // @return The current vehicle mode. MAV_MODE getMode() { return _mode; } + /// + // The mode setter. + // @mode The mode to set the vehicle to. void setMode(MAV_MODE mode) { _mode = mode; } + /// + // The state acessor. + // @return The current state of the vehicle. MAV_STATE getState() const { return _state; } + /// + // state setter + // @sate The state to set the vehicle to. void setState(MAV_STATE state) { _state = state; } protected: - AP_Navigator * _nav; - AP_Guide * _guide; - AP_HardwareAbstractionLayer * _hal; - AP_ArmingMechanism * _armingMechanism; - uint8_t _chMode; - AP_Var_group _group; - MAV_MODE _mode; - MAV_STATE _state; -}; - -class AP_ControllerBlock { -public: - AP_ControllerBlock(AP_Var_group * group, uint8_t groupStart, - uint8_t groupLength) : - _group(group), _groupStart(groupStart), - _groupEnd(groupStart + groupLength) { - } - uint8_t getGroupEnd() { - return _groupEnd; - } -protected: - AP_Var_group * _group; /// helps with parameter management - uint8_t _groupStart; - uint8_t _groupEnd; -}; - -class BlockLowPass: public AP_ControllerBlock { -public: - 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; - } -protected: - AP_Float _fCut; - float _y; -}; - -class BlockSaturation: public AP_ControllerBlock { -public: - 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) { - - // pid sum - float y = input; - - // saturation - if (y > _yMax) - y = _yMax; - if (y < -_yMax) - y = -_yMax; - return y; - } -protected: - AP_Float _yMax; /// output saturation -}; - -class BlockDerivative { -public: - 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; - } -protected: - float _lastInput; /// last input - bool firstRun; -}; - -class BlockIntegral { -public: - BlockIntegral() : - _i(0) { - } - float update(const float & input, const float & dt) { - _i += input * dt; - return _i; - } -protected: - float _i; /// integral -}; - -class BlockP: public AP_ControllerBlock { -public: - 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")) { - } - - float update(const float & input) { - return _kP * input; - } -protected: - AP_Float _kP; /// proportional gain -}; - -class BlockI: public AP_ControllerBlock { -public: - 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) { - } - - float update(const float & input, const float & dt) { - // integral - _eI += input * dt; - _eI = _blockSaturation.update(_eI); - return _kI * _eI; - } -protected: - AP_Float _kI; /// integral gain - BlockSaturation _blockSaturation; /// integrator saturation - float _eI; /// integral of input -}; - -class BlockD: public AP_ControllerBlock { -public: - 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); - // proportional, note must come after derivative - // because derivatve uses _eP as previous value - _eP = input; - return _kD * _eD; - } -protected: - BlockLowPass _blockLowPass; - AP_Float _kD; /// derivative gain - float _eP; /// input -}; - -class BlockPID: public AP_ControllerBlock { -public: - 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) { - } - - float update(const float & input, const float & dt) { - return _blockSaturation.update( - _blockP.update(input) + _blockI.update(input, dt) - + _blockD.update(input, dt)); - } -protected: - BlockP _blockP; - BlockI _blockI; - BlockD _blockD; - BlockSaturation _blockSaturation; -}; - -class BlockPI: public AP_ControllerBlock { -public: - 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) { - } - - float update(const float & input, const float & dt) { - - float y = _blockP.update(input) + _blockI.update(input, dt); - return _blockSaturation.update(y); - } -protected: - BlockP _blockP; - BlockI _blockI; - BlockSaturation _blockSaturation; -}; - -class BlockPD: public AP_ControllerBlock { -public: - 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) { - } - - float update(const float & input, const float & dt) { - - float y = _blockP.update(input) + _blockD.update(input, dt); - return _blockSaturation.update(y); - } -protected: - BlockP _blockP; - BlockD _blockD; - BlockSaturation _blockSaturation; -}; - -/// PID with derivative feedback (ignores reference derivative) -class BlockPIDDfb: public AP_ControllerBlock { -public: - 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); - } -protected: - BlockP _blockP; - BlockI _blockI; - BlockSaturation _blockSaturation; - BlockLowPass _blockLowPass; - AP_Float _kD; /// derivative gain + AP_Navigator * _nav; /// navigator + AP_Guide * _guide; /// guide + AP_HardwareAbstractionLayer * _hal; /// hardware abstraction layer + AP_ArmingMechanism * _armingMechanism; /// controls arming/ disarming + uint8_t _chMode; /// the channel the mode switch is on + AP_Var_group _group; /// holds controller parameters + MAV_MODE _mode; /// vehicle mode (auto, guided, manual, failsafe, ...) + MAV_STATE _state; /// vehicle state (active, standby, boot, calibrating ...) }; } // apo diff --git a/libraries/APO/AP_ControllerBlock.cpp b/libraries/APO/AP_ControllerBlock.cpp new file mode 100644 index 0000000000..526c3ae4d9 --- /dev/null +++ b/libraries/APO/AP_ControllerBlock.cpp @@ -0,0 +1,179 @@ +/* + * AP_ControllerBlock.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_ControllerBlock.h" +#include + +namespace apo { + +AP_ControllerBlock::AP_ControllerBlock(AP_Var_group * group, uint8_t groupStart, + uint8_t groupLength) : + _group(group), _groupStart(groupStart), + _groupEnd(groupStart + groupLength) { +} + +BlockLowPass::BlockLowPass(AP_Var_group * group, uint8_t groupStart, float fCut, + const prog_char_t * fCutLabel) : + AP_ControllerBlock(group, groupStart, 1), + _fCut(group, groupStart, fCut, fCutLabel ? : PSTR("fCut")), + _y(0) { +} +float BlockLowPass::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; +} + +BlockSaturation::BlockSaturation(AP_Var_group * group, uint8_t groupStart, float yMax, + const prog_char_t * yMaxLabel) : + AP_ControllerBlock(group, groupStart, 1), + _yMax(group, groupStart, yMax, yMaxLabel ? : PSTR("yMax")) { +} +float BlockSaturation::update(const float & input) { + + // pid sum + float y = input; + + // saturation + if (y > _yMax) + y = _yMax; + if (y < -_yMax) + y = -_yMax; + return y; +} + +BlockDerivative::BlockDerivative() : + _lastInput(0), firstRun(true) { +} +float BlockDerivative::update(const float & input, const float & dt) { + float derivative = (input - _lastInput) / dt; + _lastInput = input; + if (firstRun) { + firstRun = false; + return 0; + } else + return derivative; +} + +BlockIntegral::BlockIntegral() : + _i(0) { +} +float BlockIntegral::update(const float & input, const float & dt) { + _i += input * dt; + return _i; +} + +BlockP::BlockP(AP_Var_group * group, uint8_t groupStart, float kP, + const prog_char_t * kPLabel) : + AP_ControllerBlock(group, groupStart, 1), + _kP(group, groupStart, kP, kPLabel ? : PSTR("p")) { +} + +float BlockP::update(const float & input) { + return _kP * input; +} + +BlockI::BlockI(AP_Var_group * group, uint8_t groupStart, float kI, float iMax, + const prog_char_t * kILabel, + const prog_char_t * iMaxLabel) : + AP_ControllerBlock(group, groupStart, 2), + _kI(group, groupStart, kI, kILabel ? : PSTR("i")), + _blockSaturation(group, groupStart + 1, iMax, iMaxLabel ? : PSTR("iMax")), + _eI(0) { +} + +float BlockI::update(const float & input, const float & dt) { + // integral + _eI += input * dt; + _eI = _blockSaturation.update(_eI); + return _kI * _eI; +} + +BlockD::BlockD(AP_Var_group * group, uint8_t groupStart, float kD, uint8_t dFCut, + const prog_char_t * kDLabel, + const prog_char_t * dFCutLabel) : + AP_ControllerBlock(group, groupStart, 2), + _blockLowPass(group, groupStart, dFCut, + dFCutLabel ? : PSTR("dFCut")), + _kD(group, _blockLowPass.getGroupEnd(), kD, + kDLabel ? : PSTR("d")), _eP(0) { +} +float BlockD::update(const float & input, const float & dt) { + // derivative with low pass + float _eD = _blockLowPass.update((input - _eP) / dt, dt); + // proportional, note must come after derivative + // because derivatve uses _eP as previous value + _eP = input; + return _kD * _eD; +} + +BlockPID::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) { +} + +float BlockPID::update(const float & input, const float & dt) { + return _blockSaturation.update( + _blockP.update(input) + _blockI.update(input, dt) + + _blockD.update(input, dt)); +} + +BlockPI::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) { +} + +float BlockPI::update(const float & input, const float & dt) { + + float y = _blockP.update(input) + _blockI.update(input, dt); + return _blockSaturation.update(y); +} + +BlockPD::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) { +} + +float BlockPD::update(const float & input, const float & dt) { + + float y = _blockP.update(input) + _blockD.update(input, dt); + return _blockSaturation.update(y); +} + +BlockPIDDfb::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, + const prog_char_t * dLabel) : + 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 BlockPIDDfb::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); +} + +} // namespace apo +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_ControllerBlock.h b/libraries/APO/AP_ControllerBlock.h new file mode 100644 index 0000000000..012a34a374 --- /dev/null +++ b/libraries/APO/AP_ControllerBlock.h @@ -0,0 +1,237 @@ +/* + * AP_ControllerBlock.h + * Copyright (C) James Goppert 2010 + * + * 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 . + */ + +#ifndef AP_ControllerBlock_H +#define AP_ControllerBlock_H + +// inclusions +#include "../AP_Common/AP_Common.h" +#include "../AP_Common/AP_Var.h" + +// ArduPilotOne namespace +namespace apo { + +/// +// The abstract class defining a controller block. +class AP_ControllerBlock { +public: + /// + // Controller block constructor. + // This creates a controller block. + // @group The group containing the class parameters. + // @groupStart The start of the group. Used for chaining parameters. + // @groupEnd The end of the group. + AP_ControllerBlock(AP_Var_group * group, uint8_t groupStart, + uint8_t groupLength); + + /// + // Get the end of the AP_Var group. + // This is used for chaining multiple AP_Var groups into one. + uint8_t getGroupEnd() { + return _groupEnd; + } +protected: + AP_Var_group * _group; /// Contains all the parameters of the class. + uint8_t _groupStart; /// The start of the AP_Var group. Used for chaining parameters. + uint8_t _groupEnd; /// The end of the AP_Var group. +}; + +/// +// A low pass filter block. +// This takes a signal and smooths it out. It +// cuts all frequencies higher than the frequency provided. +class BlockLowPass: public AP_ControllerBlock { +public: + /// + // The constructor. + // @group The group containing the class parameters. + // @groupStart The start of the group. Used for chaining parameters. + // @fCut The cut-off frequency in Hz for smoothing. + BlockLowPass(AP_Var_group * group, uint8_t groupStart, float fCut, + const prog_char_t * fCutLabel = NULL); + + /// + // The update function. + // @input The input signal. + // @dt The timer interval. + // @return The output (smoothed) signal. + float update(const float & input, const float & dt); +protected: + AP_Float _fCut; /// The cut-off frequency in Hz. + float _y; /// The internal state of the low pass filter. +}; + +/// +// This block saturates a signal. +// If the signal is above max it is set to max. +// If it is below -max it is set to -max. +class BlockSaturation: public AP_ControllerBlock { +public: + /// + // Controller block constructor. + // This creates a controller block. + // @group The group containing the class parameters. + // @groupStart The start of the group. Used for chaining parameters. + // @yMax The maximum threshold. + BlockSaturation(AP_Var_group * group, uint8_t groupStart, float yMax, + const prog_char_t * yMaxLabel = NULL); + /// + // The update function. + // @input The input signal. + // @return The output (saturated) signal. + float update(const float & input); +protected: + AP_Float _yMax; /// output saturation +}; + +/// +// This block calculates a derivative. +class BlockDerivative { +public: + /// The constructor. + BlockDerivative(); + + /// + // The update function. + // @input The input signal. + // @return The derivative. + float update(const float & input, const float & dt); +protected: + float _lastInput; /// The last input to the block. + bool firstRun; /// Keeps track of first run inorder to decide if _lastInput should be used. +}; + +/// This block calculates an integral. +class BlockIntegral { +public: + /// The constructor. + BlockIntegral(); + /// + // The update function. + // @input The input signal. + // @dt The timer interval. + // @return The output (integrated) signal. + float update(const float & input, const float & dt); +protected: + float _i; /// integral +}; + +/// +// This is a proportional block with built-in gain. +class BlockP: public AP_ControllerBlock { +public: + BlockP(AP_Var_group * group, uint8_t groupStart, float kP, + const prog_char_t * kPLabel = NULL); + /// + // The update function. + // @input The input signal. + // @return The output signal (kP*input). + float update(const float & input); +protected: + AP_Float _kP; /// proportional gain +}; + +/// +// This is a integral block with built-in gain. +class BlockI: public AP_ControllerBlock { +public: + BlockI(AP_Var_group * group, uint8_t groupStart, float kI, float iMax, + const prog_char_t * kILabel = NULL, + const prog_char_t * iMaxLabel = NULL); + float update(const float & input, const float & dt); +protected: + AP_Float _kI; /// integral gain + BlockSaturation _blockSaturation; /// integrator saturation + float _eI; /// internal integrator state +}; + +/// +// This is a derivative block with built-in gain. +class BlockD: public AP_ControllerBlock { +public: + 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); + float update(const float & input, const float & dt); +protected: + BlockLowPass _blockLowPass; /// The low-pass filter block + AP_Float _kD; /// The derivative gain + float _eP; /// The previous state +}; + +/// +// This is a proportional, integral, derivative block with built-in gains. +class BlockPID: public AP_ControllerBlock { +public: + BlockPID(AP_Var_group * group, uint8_t groupStart, float kP, float kI, + float kD, float iMax, float yMax, uint8_t dFcut); + float update(const float & input, const float & dt); +protected: + BlockP _blockP; /// The proportional block. + BlockI _blockI; /// The integral block. + BlockD _blockD; /// The derivative block. + BlockSaturation _blockSaturation; /// The saturation block. +}; + +/// +// This is a proportional, integral block with built-in gains. +class BlockPI: public AP_ControllerBlock { +public: + BlockPI(AP_Var_group * group, uint8_t groupStart, float kP, float kI, + float iMax, float yMax); + float update(const float & input, const float & dt); +protected: + BlockP _blockP; /// The proportional block. + BlockI _blockI; /// The derivative block. + BlockSaturation _blockSaturation; /// The saturation block. +}; + +/// +// This is a proportional, derivative block with built-in gains. +class BlockPD: public AP_ControllerBlock { +public: + BlockPD(AP_Var_group * group, uint8_t groupStart, float kP, float kI, + float kD, float iMax, float yMax, uint8_t dFcut); + float update(const float & input, const float & dt); +protected: + BlockP _blockP; /// The proportional block. + BlockD _blockD; /// The derivative block. + BlockSaturation _blockSaturation; /// The saturation block. +}; + +/// PID with derivative feedback (ignores reference derivative) +class BlockPIDDfb: public AP_ControllerBlock { +public: + 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); + float update(const float & input, const float & derivative, + const float & dt); +protected: + BlockP _blockP; /// The proportional block. + BlockI _blockI; /// The integral block. + BlockSaturation _blockSaturation; /// The saturation block. + BlockLowPass _blockLowPass; /// The low-pass filter block. + AP_Float _kD; /// derivative gain +}; + +} // apo + +#endif // AP_ControllerBlock_H +// vim:ts=4:sw=4:expandtab diff --git a/libraries/AP_Common/include/menu.h b/libraries/AP_Common/include/menu.h index 1e4e3a0d89..2cff1b9aca 100644 --- a/libraries/AP_Common/include/menu.h +++ b/libraries/AP_Common/include/menu.h @@ -16,6 +16,8 @@ #ifndef __AP_COMMON_MENU_H #define __AP_COMMON_MENU_H +#include + #define MENU_COMMANDLINE_MAX 32 ///< maximum input line length #define MENU_ARGS_MAX 4 ///< maximum number of arguments #define MENU_COMMAND_MAX 14 ///< maximum size of a command name