Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Separated controller block class.

Browse Source 
Also improved documentation of AP_ControllerBlock.
This commit is contained in:
James Goppert 2011-11-24 13:28:14 -05:00
parent aaca5094b6
commit fc84c15426
12 changed files with 508 additions and 284 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduBoat/ControllerBoat.h
View File

@ -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,

2
ArduRover/ControllerCar.h
View File

@ -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,

2
ArduRover/ControllerTank.h
View File

@ -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,

2
apo/ControllerPlane.h
View File

@ -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),

2
apo/ControllerQuad.h
View File

@ -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),

1
libraries/APO/APO.h
View File

@ -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"

1
libraries/APO/AP_ArmingMechanism.cpp
View File

@ -3,7 +3,6 @@
*
*/
#include "AP_ArmingMechanism.h"
#include "AP_Controller.h"
#include "AP_RcChannel.h"

8
libraries/APO/AP_Controller.cpp
View File

@ -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

352
libraries/APO/AP_Controller.h
View File

@ -19,315 +19,127 @@
#ifndef AP_Controller_H
#define AP_Controller_H
#include <inttypes.h>
#include "../GCS_MAVLink/GCS_MAVLink.h"
#include <math.h>
// inclusions
#include "../AP_Common/AP_Common.h"
#include "../AP_Common/AP_Var.h"
#include "AP_Var_keys.h"
class AP_Var_group;
#include <inttypes.h>
#include <math.h>
#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

179
libraries/APO/AP_ControllerBlock.cpp Normal file
View File

@ -0,0 +1,179 @@
/*
* AP_ControllerBlock.cpp
*
* Created on: Apr 30, 2011
* Author: jgoppert
*/
#include "AP_ControllerBlock.h"
#include <math.h>
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

237
libraries/APO/AP_ControllerBlock.h Normal file
View File

@ -0,0 +1,237 @@
/*
* AP_ControllerBlock.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_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

2
libraries/AP_Common/include/menu.h
View File

@ -16,6 +16,8 @@
#ifndef __AP_COMMON_MENU_H
#define __AP_COMMON_MENU_H
#include <inttypes.h>
#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