Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Started updating AP_Controller and AP_RcChannel for AP_Var

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1559 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert 2011-01-26 07:05:22 +00:00
parent 557b5c7615
commit 82a257982a
2 changed files with 35 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
libraries/AP_Controller/AP_Controller.h
View File

@ -37,11 +37,11 @@ public:
{ {
} }
const char * getName() { return _name; } const char * getName() { return _name; }
const Vector < AP_VarI * > & getOutput() const { return _output; } const Vector < AP_Var * > & getOutput() const { return _output; }
protected: protected:
const char * _name; const char * _name;
Vector< AP_VarI * > _input; Vector< AP_Var * > _input;
Vector< AP_VarI * > _output; Vector< AP_Var * > _output;
}; };
/// Servo Block /// Servo Block
@ -59,7 +59,7 @@ public:
virtual void update(const float & dt = 0) virtual void update(const float & dt = 0)
{ {
if (_input.getSize() > 0) if (_input.getSize() > 0)
_ch->setPosition(_output[0]->getF()); _ch->setPosition(_output[0]);
} }
private: private:
float _position; float _position;
@ -73,17 +73,17 @@ public:
/// Constructor that allows 1-8 sum gain pairs, more /// Constructor that allows 1-8 sum gain pairs, more
/// can be added if necessary /// can be added if necessary
SumGain( SumGain(
AP_VarI * var1, AP_VarI * gain1, AP_Var * var1, AP_Var * gain1,
AP_VarI * var2 = NULL, AP_VarI * gain2 = NULL, AP_Var * var2 = NULL, AP_Var * gain2 = NULL,
AP_VarI * var3 = NULL, AP_VarI * gain3 = NULL, AP_Var * var3 = NULL, AP_Var * gain3 = NULL,
AP_VarI * var4 = NULL, AP_VarI * gain4 = NULL, AP_Var * var4 = NULL, AP_Var * gain4 = NULL,
AP_VarI * var5 = NULL, AP_VarI * gain5 = NULL, AP_Var * var5 = NULL, AP_Var * gain5 = NULL,
AP_VarI * var6 = NULL, AP_VarI * gain6 = NULL, AP_Var * var6 = NULL, AP_Var * gain6 = NULL,
AP_VarI * var7 = NULL, AP_VarI * gain7 = NULL, AP_Var * var7 = NULL, AP_Var * gain7 = NULL,
AP_VarI * var8 = NULL, AP_VarI * gain8 = NULL) : AP_Var * var8 = NULL, AP_Var * gain8 = NULL) :
_gain() _gain()
{ {
_output.push_back(new AP_Float(0,"","")); _output.push_back(new AP_Float(0));
if (var1 && gain1) add(var1,gain1); if (var1 && gain1) add(var1,gain1);
if (var2 && gain2) add(var2,gain2); if (var2 && gain2) add(var2,gain2);
if (var3 && gain3) add(var3,gain3); if (var3 && gain3) add(var3,gain3);
@ -93,7 +93,7 @@ public:
if (var7 && gain7) add(var7,gain7); if (var7 && gain7) add(var7,gain7);
if (var8 && gain8) add(var8,gain8); if (var8 && gain8) add(var8,gain8);
} }
void add(AP_VarI * var, AP_VarI * gain) void add(AP_Var * var, AP_Var * gain)
{ {
_input.push_back(var); _input.push_back(var);
_gain.push_back(gain); _gain.push_back(gain);
@ -109,31 +109,32 @@ public:
_output[0]->setF(0); _output[0]->setF(0);
for (int i=0;i<_input.getSize();i++) for (int i=0;i<_input.getSize();i++)
{ {
_output[0]->setF( _output[i]->getF() + _input[i]->getF()*_gain[i]->getF()); _output[0]->setF( _output[i] + _input[i]*_gain[i]);
} }
} }
private: private:
Vector< AP_VarI * > _gain; Vector< AP_Var * > _gain;
}; };
/// PID block /// PID block
class Pid : public Block class Pid : public Block
{ {
public: public:
Pid(const char * name="", Pid(AP_Var::Key key, const prog_char * name,
const float & kP=0, float kP = 0.0,
const float & kI=0, float kI = 0.0,
const float & kD=0, float kD = 0.0,
const float & iMax=1, float iMax = 0.0,
const uint8_t & dFcut=20 uint8_t dFcut = 20.0,
) : ) :
_kP(new AP_EEPROM_Float(kP,"KP",name)), _group(key,name),
_kI(new AP_EEPROM_Float(kI,"KI",name)), _kP(&_group,0,kP,P_STR("P")),
_kD(new AP_EEPROM_Float(kD,"KD",name)), _kI(&_group,0,kP,P_STR("I")),
_iMax(new AP_EEPROM_Float(iMax,"IMAX",name)), _kD(&_group,0,kP,P_STR("D")),
_dFcut(new AP_EEPROM_Uint8(dFcut,"DFCUT",name)) _iMax(&_group,0,kP,P_STR("IMAX")),
_fCut(&_group,0,kP,P_STR("FCUT")),
{ {
_output.push_back(new AP_Float(0,"OUT",name)); _output.push_back(new AP_Float(0));
} }
virtual void connect(Block * block) virtual void connect(Block * block)
{ {
@ -145,18 +146,18 @@ public:
if (_output.getSize() < 1) return; if (_output.getSize() < 1) return;
// derivative // derivative
float RC = 1/(2*M_PI*_dFcut->get()); // low pass filter float RC = 1/(2*M_PI*_fCut->get()); // low pass filter
_eD = _eD + ( ((_e - _input[0]->getF()))/dt - _eD ) * (dt / (dt + RC)); _eD = _eD + ( ((_e - _input[0]))/dt - _eD ) * (dt / (dt + RC));
// proportional, note must come after derivative // proportional, note must come after derivative
// because derivatve uses _e as previous value // because derivatve uses _e as previous value
_e = _input[0]->getF(); _e = _input[0];
// integral // integral
_eI += _e*dt; _eI += _e*dt;
// pid sum // pid sum
_output[0]->setF(_kP->getF()*_e + _kI->getF()*_eI + _kD->getF()*_eD); _output[0]->setF(_kP*_e + _kI*_eI + _kD*_eD);
} }
private: private:
float _e; /// input float _e; /// input
@ -166,7 +167,7 @@ private:
AP_Float * _kI; /// integral gain AP_Float * _kI; /// integral gain
AP_Float * _kD; /// derivative gain AP_Float * _kD; /// derivative gain
AP_Float * _iMax; /// integrator saturation AP_Float * _iMax; /// integrator saturation
AP_Uint8 * _dFcut; /// derivative low-pass cut freq (Hz) AP_Uint8 * _fCut; /// derivative low-pass cut freq (Hz)
}; };
/// Controller class /// Controller class

2
libraries/AP_RcChannel/AP_RcChannel.h
View File

@ -13,7 +13,7 @@
/// @class AP_RcChannel /// @class AP_RcChannel
/// @brief Object managing one RC channel /// @brief Object managing one RC channel
class AP_RcChannel : public AP_Var_scope { class AP_RcChannel : public AP_Var_group {
public: public: