Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Controller update. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1845 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert 2011-04-04 02:22:55 +00:00
parent 8c44ee2ac6
commit ea63560e00
1 changed files with 17 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_Controller/AP_Controller.h
View File

@ -97,10 +97,10 @@ public:
ToServo(AP_RcChannel * ch) : _ch(ch)
{
}
// doesn't connect
virtual void connect(Block * block) {};
virtual void update(const float & dt = 0)
{
//Serial.println("calling to servo update");
//Serial.println("input: "); Serial.println(input(0));
if (_input.getSize() > 0)
{
_ch->setNormalized(input(0));
@ -146,9 +146,15 @@ public:
}
virtual void update(const float & dt = 0)
{
//Serial.println("calling sumgain update");
if (_output.getSize() < 1) return;
float sum =0;
for (int i=0;i<_input.getSize();i++) sum += input(i) * gain(i);
for (int i=0;i<_input.getSize();i++)
{
//Serial.println("input: "); Serial.println(input(i));
//Serial.println("gain: ");Serial.println(gain(i));
sum += input(i) * gain(i);
}
output(0) = sum;
}
float gain(int i) { return *(_gain[i]); }
@ -184,6 +190,9 @@ public:
virtual void update(const float & dt)
{
//Serial.println("calling pid update");
//Serial.println("input: "); Serial.println(input(0));
if (_output.getSize() < 1 || (!_input[0]) || (!_output[0]) ) return;
// derivative with low pass
@ -204,6 +213,8 @@ public:
// pid sum
output(0) = _kP*_eP + _kI*_eI + _kD*_eD;
//Serial.println("output: "); Serial.println(output(0));
// debug output
/*
Serial.println("kP, kI, kD: ");
@ -243,10 +254,10 @@ public:
}
virtual void update(const float & dt)
{
//Serial.println("calling sink update");
//Serial.println("input: "); Serial.println(input(0));
_var = input(_port);
}
// doesn't connect
virtual void connect(Block * block) {}
private:
float & _var;
uint8_t _port;
@ -261,6 +272,7 @@ public:
_min(min), _max(max), _port(port)
{
// create output
//Serial.println("calling satruate update");
_output.push_back(new float(0.0));
}
virtual void update(const float & dt)