mirror of https://github.com/ArduPilot/ardupilot
Removed AP_Controller.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1848 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert@gmail.com
parent
f67c4d7c99
commit
eb6962568b
|
|
@ -1,21 +0,0 @@
|
||||||
/*
|
|
||||||
* AP_Controller.cpp
|
|
||||||
* 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/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "AP_Controller.h"
|
|
||||||
|
|
||||||
// vim:ts=4:sw=4:expandtab
|
|
||||||
|
|
@ -1,293 +0,0 @@
|
||||||
/*
|
|
||||||
* 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 <AP_Common.h>
|
|
||||||
#include <AP_Vector.h>
|
|
||||||
#include <AP_Var.h>
|
|
||||||
#include <APM_RC.h>
|
|
||||||
#include "AP_RcChannel.h"
|
|
||||||
|
|
||||||
const float one = 1.0;
|
|
||||||
const float zero = 0.0;
|
|
||||||
const float negativeOne = -1.0;
|
|
||||||
|
|
||||||
/// Controller class
|
|
||||||
class AP_Controller
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
class Block
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
Block() :
|
|
||||||
_input(), _output()
|
|
||||||
{
|
|
||||||
}
|
|
||||||
virtual void update(const float & dt) = 0;
|
|
||||||
virtual void connect( Block * block)
|
|
||||||
{
|
|
||||||
if (!block) return;
|
|
||||||
for (int i=0;i<block->getOutput().getSize();i++)
|
|
||||||
_input.push_back(block->getOutput()[i]);
|
|
||||||
}
|
|
||||||
const Vector < float * > & getOutput() const { return _output; }
|
|
||||||
const float & input(int i) { return (*_input[i]);}
|
|
||||||
float & output(int i) { return (*_output[i]);}
|
|
||||||
protected:
|
|
||||||
Vector< const float * > _input;
|
|
||||||
Vector< float * > _output;
|
|
||||||
};
|
|
||||||
|
|
||||||
void addBlock(Block * block)
|
|
||||||
{
|
|
||||||
if (!block)
|
|
||||||
{
|
|
||||||
Serial.println("Attempint to add a null block");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
if (_blocks.getSize() > 0)
|
|
||||||
{
|
|
||||||
if (_blocks[_blocks.getSize()-1] == NULL)
|
|
||||||
{
|
|
||||||
Serial.println("Attempted to connect to null block");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
block->connect(_blocks[_blocks.getSize()-1]);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
_blocks.push_back(block);
|
|
||||||
}
|
|
||||||
virtual void update(const float & dt)
|
|
||||||
{
|
|
||||||
for (int i=0;i<_blocks.getSize();i++)
|
|
||||||
{
|
|
||||||
if (!_blocks[i]) continue;
|
|
||||||
_blocks[i]->update(dt);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
Vector<Block * > _blocks;
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
/// Servo Block
|
|
||||||
class ToServo : public AP_Controller::Block
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
ToServo(AP_RcChannel * ch) : _ch(ch)
|
|
||||||
{
|
|
||||||
}
|
|
||||||
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));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
private:
|
|
||||||
float _position;
|
|
||||||
AP_RcChannel * _ch;
|
|
||||||
};
|
|
||||||
|
|
||||||
/// SumGain
|
|
||||||
class SumGain : public AP_Controller::Block
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
/// Constructor that allows 1-8 sum gain pairs, more
|
|
||||||
/// can be added if necessary
|
|
||||||
SumGain(
|
|
||||||
const float * var1 = NULL, const float * gain1 = NULL,
|
|
||||||
const float * var2 = NULL, const float * gain2 = NULL,
|
|
||||||
const float * var3 = NULL, const float * gain3 = NULL,
|
|
||||||
const float * var4 = NULL, const float * gain4 = NULL,
|
|
||||||
const float * var5 = NULL, const float * gain5 = NULL,
|
|
||||||
const float * var6 = NULL, const float * gain6 = NULL,
|
|
||||||
const float * var7 = NULL, const float * gain7 = NULL,
|
|
||||||
const float * var8 = NULL, const float * gain8 = NULL)
|
|
||||||
{
|
|
||||||
if ( (var1 != NULL) && (gain1 != NULL) ) add(var1,gain1);
|
|
||||||
if ( (var2 != NULL) && (gain2 != NULL) ) add(var2,gain2);
|
|
||||||
if ( (var3 != NULL) && (gain3 != NULL) ) add(var3,gain3);
|
|
||||||
if ( (var4 != NULL) && (gain4 != NULL) ) add(var4,gain4);
|
|
||||||
if ( (var5 != NULL) && (gain5 != NULL) ) add(var5,gain5);
|
|
||||||
if ( (var6 != NULL) && (gain6 != NULL) ) add(var6,gain6);
|
|
||||||
if ( (var7 != NULL) && (gain7 != NULL) ) add(var7,gain7);
|
|
||||||
if ( (var8 != NULL) && (gain8 != NULL) ) add(var8,gain8);
|
|
||||||
|
|
||||||
// create output
|
|
||||||
_output.push_back(new float(0.0));
|
|
||||||
}
|
|
||||||
void add(const float * var, const float * gain)
|
|
||||||
{
|
|
||||||
_input.push_back(var);
|
|
||||||
_gain.push_back(gain);
|
|
||||||
}
|
|
||||||
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++)
|
|
||||||
{
|
|
||||||
//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]); }
|
|
||||||
private:
|
|
||||||
Vector<const float *> _gain;
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
/// PID block
|
|
||||||
class Pid : public AP_Controller::Block
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
Pid(AP_Var::Key key, const prog_char_t * name,
|
|
||||||
float kP = 0.0,
|
|
||||||
float kI = 0.0,
|
|
||||||
float kD = 0.0,
|
|
||||||
float iMax = 0.0,
|
|
||||||
uint8_t dFcut = 20.0
|
|
||||||
) :
|
|
||||||
_group(key,name),
|
|
||||||
_eP(0),
|
|
||||||
_eI(0),
|
|
||||||
_eD(0),
|
|
||||||
_kP(&_group,1,kP,PSTR("P")),
|
|
||||||
_kI(&_group,2,kI,PSTR("I")),
|
|
||||||
_kD(&_group,3,kD,PSTR("D")),
|
|
||||||
_iMax(&_group,4,iMax,PSTR("IMAX")),
|
|
||||||
_fCut(&_group,5,dFcut,PSTR("FCUT"))
|
|
||||||
{
|
|
||||||
// create output
|
|
||||||
_output.push_back(new float(0.0));
|
|
||||||
}
|
|
||||||
|
|
||||||
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
|
|
||||||
float RC = 1/(2*M_PI*_fCut); // low pass filter
|
|
||||||
_eD = _eD + ( ( _eP - input(0) )/dt - _eD ) * (dt / (dt + RC));
|
|
||||||
|
|
||||||
// proportional, note must come after derivative
|
|
||||||
// because derivatve uses _eP as previous value
|
|
||||||
_eP = input(0);
|
|
||||||
|
|
||||||
// integral
|
|
||||||
_eI += _eP*dt;
|
|
||||||
|
|
||||||
// wind up guard
|
|
||||||
if (_eI > _iMax) _eI = _iMax;
|
|
||||||
else if (_eI < -_iMax) _eI = -_iMax;
|
|
||||||
|
|
||||||
// pid sum
|
|
||||||
output(0) = _kP*_eP + _kI*_eI + _kD*_eD;
|
|
||||||
|
|
||||||
//Serial.println("output: "); Serial.println(output(0));
|
|
||||||
|
|
||||||
// debug output
|
|
||||||
/*
|
|
||||||
Serial.println("kP, kI, kD: ");
|
|
||||||
Serial.print(_kP,5); Serial.print(" ");
|
|
||||||
Serial.print(_kI,5); Serial.print(" ");
|
|
||||||
Serial.println(_kD,5);
|
|
||||||
Serial.print("eP, eI, eD: ");
|
|
||||||
Serial.print(_eP,5); Serial.print(" ");
|
|
||||||
Serial.print(_eI,5); Serial.print(" ");
|
|
||||||
Serial.println(_eD,5);
|
|
||||||
Serial.print("input: ");
|
|
||||||
Serial.println(input(0),5);
|
|
||||||
Serial.print("output: ");
|
|
||||||
Serial.println(output(0),5);
|
|
||||||
*/
|
|
||||||
}
|
|
||||||
private:
|
|
||||||
AP_Var_group _group; /// helps with parameter management
|
|
||||||
AP_Float _eP; /// input
|
|
||||||
AP_Float _eI; /// integral of input
|
|
||||||
AP_Float _eD; /// derivative of input
|
|
||||||
AP_Float _kP; /// proportional gain
|
|
||||||
AP_Float _kI; /// integral gain
|
|
||||||
AP_Float _kD; /// derivative gain
|
|
||||||
AP_Float _iMax; /// integrator saturation
|
|
||||||
AP_Uint8 _fCut; /// derivative low-pass cut freq (Hz)
|
|
||||||
};
|
|
||||||
|
|
||||||
/// Sink block
|
|
||||||
/// saves input port to variable
|
|
||||||
class Sink : public AP_Controller::Block
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
Sink(float & var, uint8_t port=0) :
|
|
||||||
_var(var), _port(port)
|
|
||||||
{
|
|
||||||
}
|
|
||||||
virtual void update(const float & dt)
|
|
||||||
{
|
|
||||||
//Serial.println("calling sink update");
|
|
||||||
//Serial.println("input: "); Serial.println(input(0));
|
|
||||||
_var = input(_port);
|
|
||||||
}
|
|
||||||
private:
|
|
||||||
float & _var;
|
|
||||||
uint8_t _port;
|
|
||||||
};
|
|
||||||
|
|
||||||
/// Saturate block
|
|
||||||
/// Constrains output to a range
|
|
||||||
class Saturate : public AP_Controller::Block
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
Saturate(float & min, float & max, uint8_t port=0) :
|
|
||||||
_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)
|
|
||||||
{
|
|
||||||
float u = input(_port);
|
|
||||||
if (u>_max) u = _max;
|
|
||||||
if (u<_min) u = _min;
|
|
||||||
output(_port) = u;
|
|
||||||
}
|
|
||||||
private:
|
|
||||||
uint8_t _port;
|
|
||||||
float & _min;
|
|
||||||
float & _max;
|
|
||||||
};
|
|
||||||
|
|
||||||
#endif // AP_Controller_H
|
|
||||||
|
|
||||||
// vim:ts=4:sw=4:expandtab
|
|
||||||
|
|
@ -1,122 +0,0 @@
|
||||||
#include <FastSerial.h>
|
|
||||||
#include <AP_Controller.h>
|
|
||||||
#include <AP_Var.h>
|
|
||||||
#include <AP_RcChannel.h>
|
|
||||||
#include <APM_RC.h>
|
|
||||||
#include <AP_Common.h>
|
|
||||||
|
|
||||||
FastSerialPort0(Serial);
|
|
||||||
float test1;
|
|
||||||
float test2;
|
|
||||||
|
|
||||||
class CarController : public AP_Controller
|
|
||||||
{
|
|
||||||
private:
|
|
||||||
// state
|
|
||||||
float * velocity;
|
|
||||||
float * heading;
|
|
||||||
|
|
||||||
// control variables
|
|
||||||
float * headingCommand;
|
|
||||||
float * velocityCommand;
|
|
||||||
|
|
||||||
// channels
|
|
||||||
AP_RcChannel * steeringCh;
|
|
||||||
AP_RcChannel * throttleCh;
|
|
||||||
|
|
||||||
static float one;
|
|
||||||
static float negOne;
|
|
||||||
|
|
||||||
public:
|
|
||||||
CarController(AP_Var::Key pidStrKey, AP_Var::Key pidThrKey,
|
|
||||||
float * heading, float * velocity,
|
|
||||||
float * headingCommand, float * velocityCommand,
|
|
||||||
AP_RcChannel * steeringCh, AP_RcChannel * throttleCh) :
|
|
||||||
heading(heading), velocity(velocity),
|
|
||||||
headingCommand(headingCommand), velocityCommand(velocityCommand),
|
|
||||||
steeringCh(steeringCh), throttleCh(throttleCh)
|
|
||||||
{
|
|
||||||
// steering control loop
|
|
||||||
addBlock(new SumGain(headingCommand,&one,heading,&negOne));
|
|
||||||
addBlock(new Pid(pidStrKey,PSTR("STR"),1,1,1,1,20));
|
|
||||||
//addBlock(new Sink(test1));
|
|
||||||
addBlock(new ToServo(steeringCh));
|
|
||||||
|
|
||||||
|
|
||||||
// throttle control loop
|
|
||||||
addBlock(new SumGain(velocityCommand,&one,velocity,&negOne));
|
|
||||||
addBlock(new Pid(pidThrKey,PSTR("THR"),1,1,1,1,20));
|
|
||||||
//addBlock(new Sink(test2));
|
|
||||||
addBlock(new ToServo(throttleCh));
|
|
||||||
}
|
|
||||||
};
|
|
||||||
float CarController::negOne = -1;
|
|
||||||
float CarController::one = 1;
|
|
||||||
|
|
||||||
AP_Controller * controller;
|
|
||||||
Vector<AP_RcChannel * > ch;
|
|
||||||
|
|
||||||
float heading, velocity; // measurements
|
|
||||||
float headingCommand, velocityCommand; // guidance data
|
|
||||||
int8_t sign = 1;
|
|
||||||
float value = 0;
|
|
||||||
|
|
||||||
enum keys
|
|
||||||
{
|
|
||||||
config,
|
|
||||||
chStrKey,
|
|
||||||
chThrKey,
|
|
||||||
pidStrKey,
|
|
||||||
pidThrKey
|
|
||||||
};
|
|
||||||
|
|
||||||
void setup()
|
|
||||||
{
|
|
||||||
Serial.begin(115200);
|
|
||||||
|
|
||||||
// variables
|
|
||||||
heading = 0;
|
|
||||||
velocity = 0;
|
|
||||||
headingCommand = 0;
|
|
||||||
velocityCommand = 0;
|
|
||||||
|
|
||||||
// rc channels
|
|
||||||
APM_RC.Init();
|
|
||||||
ch.push_back(new AP_RcChannel(chStrKey,PSTR("STR"),APM_RC,1,45));
|
|
||||||
ch.push_back(new AP_RcChannel(chThrKey,PSTR("THR"),APM_RC,2,45));
|
|
||||||
|
|
||||||
// controller
|
|
||||||
controller = new CarController(pidStrKey,pidThrKey,&heading,&velocity,&headingCommand,&velocityCommand,ch[0],ch[1]);
|
|
||||||
}
|
|
||||||
|
|
||||||
void loop()
|
|
||||||
{
|
|
||||||
// loop rate 20 Hz
|
|
||||||
delay(1000.0/20);
|
|
||||||
|
|
||||||
// feedback signal, 1/3 Hz sawtooth
|
|
||||||
if (value > 1)
|
|
||||||
{
|
|
||||||
value = 1;
|
|
||||||
sign = -1;
|
|
||||||
}
|
|
||||||
else if (value < -1)
|
|
||||||
{
|
|
||||||
value = -1;
|
|
||||||
sign = 1;
|
|
||||||
}
|
|
||||||
value += sign/20.0/3.0;
|
|
||||||
velocity = value;
|
|
||||||
heading = value;
|
|
||||||
|
|
||||||
// update controller
|
|
||||||
controller->update(1./20);
|
|
||||||
|
|
||||||
// output
|
|
||||||
Serial.printf("hdng:%f\tcmd:(%f)\tservo:%f\t", heading,headingCommand, ch[0]->getNormalized());
|
|
||||||
Serial.printf("| thr:%f\tcmd:(%f)\tservo:%f\n", velocity, velocityCommand, ch[1]->getNormalized());
|
|
||||||
|
|
||||||
//Serial.printf("test 1: %f test 2: %f\n", test1, test2);
|
|
||||||
}
|
|
||||||
|
|
||||||
// vim:ts=4:sw=4:expandtab
|
|
||||||
Loading…
Reference in New Issue