zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Consolidating APO. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1691 f9c3cf11-9bcb-44bc-f272-b75c42450872
master
james.goppert 14 years ago
parent
33a720bbaa
commit
a329aa18fe
7 changed files with 0 additions and 785 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 21
      libraries/AP_Controller/AP_Controller.cpp
  2. 249
      libraries/AP_Controller/AP_Controller.h
  3. 110
      libraries/AP_Controller/examples/Car/Car.pde
  4. 130
      libraries/AP_RcChannel/AP_RcChannel.cpp
  5. 68
      libraries/AP_RcChannel/AP_RcChannel.h
  6. 94
      libraries/AP_RcChannel/examples/Manual/Manual.pde
  7. 113
      libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde

21
libraries/AP_Controller/AP_Controller.cpp
Unescape View File

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

249
libraries/AP_Controller/AP_Controller.h
Unescape View File

@ -1,249 +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 <AP_RcChannel.h>
#include <APM_RC.h>
/// Block
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;block->getOutput().getSize();i++)
_input.push_back(block->getOutput()[i]);
}
const Vector < AP_Float * > & getOutput() const { return _output; }
protected:
Vector< const AP_Float * > _input;
Vector< AP_Float * > _output;
};
/// Servo Block
class ToServo: public Block
{
public:
ToServo(AP_RcChannel * ch) : _ch(ch)
{
}
// doesn't connect
virtual void connect(Block * block) {};
virtual void update(const float & dt = 0)
{
if (_input.getSize() > 0)
{
_ch->setNormalized(_input[0]->get());
}
}
private:
float _position;
AP_RcChannel * _ch;
};
/// SumGain
class SumGain : public Block
{
public:
/// Constructor that allows 1-8 sum gain pairs, more
/// can be added if necessary
SumGain(
AP_Float & var1 = AP_Float_zero, AP_Float & gain1 = AP_Float_zero,
AP_Float & var2 = AP_Float_zero, AP_Float & gain2 = AP_Float_zero,
AP_Float & var3 = AP_Float_zero, AP_Float & gain3 = AP_Float_zero,
AP_Float & var4 = AP_Float_zero, AP_Float & gain4 = AP_Float_zero,
AP_Float & var5 = AP_Float_zero, AP_Float & gain5 = AP_Float_zero,
AP_Float & var6 = AP_Float_zero, AP_Float & gain6 = AP_Float_zero,
AP_Float & var7 = AP_Float_zero, AP_Float & gain7 = AP_Float_zero,
AP_Float & var8 = AP_Float_zero, AP_Float & gain8 = AP_Float_zero)
{
if ( (&var1 != &AP_Float_zero) && (&gain1 != &AP_Float_zero) ) add(var1,gain1);
if ( (&var2 != &AP_Float_zero) && (&gain2 != &AP_Float_zero) ) add(var2,gain2);
if ( (&var3 != &AP_Float_zero) && (&gain3 != &AP_Float_zero) ) add(var3,gain3);
if ( (&var4 != &AP_Float_zero) && (&gain4 != &AP_Float_zero) ) add(var4,gain4);
if ( (&var5 != &AP_Float_zero) && (&gain5 != &AP_Float_zero) ) add(var5,gain5);
if ( (&var6 != &AP_Float_zero) && (&gain6 != &AP_Float_zero) ) add(var6,gain6);
if ( (&var7 != &AP_Float_zero) && (&gain7 != &AP_Float_zero) ) add(var7,gain7);
if ( (&var8 != &AP_Float_zero) && (&gain8 != &AP_Float_zero) ) add(var8,gain8);
// create output
_output.push_back(new AP_Float(0));
}
void add(AP_Float & var, AP_Float & gain)
{
_input.push_back(&var);
_gain.push_back(&gain);
}
virtual void update(const float & dt = 0)
{
if (_output.getSize() < 1) return;
float sum =0;
for (int i=0;i<_input.getSize();i++) sum += _input[i]->get() * _gain[i]->get() ;
_output[0]->set(sum);
}
private:
Vector< AP_Float * > _gain;
};
/// PID block
class Pid : public Block, public AP_Var_group
{
public:
Pid(AP_Var::Key key, const prog_char * name,
float kP = 0.0,
float kI = 0.0,
float kD = 0.0,
float iMax = 0.0,
uint8_t dFcut = 20.0
) :
AP_Var_group(key,name),
_kP(this,1,kP,PSTR("P")),
_kI(this,2,kI,PSTR("I")),
_kD(this,3,kD,PSTR("D")),
_iMax(this,4,iMax,PSTR("IMAX")),
_fCut(this,5,dFcut,PSTR("FCUT"))
{
_output.push_back(new AP_Float(0));
}
virtual void update(const float & dt)
{
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]->get() )/dt - _eD ) * (dt / (dt + RC));
// proportional, note must come after derivative
// because derivatve uses _eP as previous value
_eP = _input[0]->get();
// integral
_eI += _eP*dt;
// wind up guard
if (_eI > _iMax) _eI = _iMax;
else if (_eI < -_iMax) _eI = -_iMax;
// pid sum
_output[0]->set(_kP*_eP + _kI*_eI + _kD*_eD);
// 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]->get(),5);
Serial.print("output: ");
Serial.println(_output[0]->get(),5);*/
}
private:
float _eP; /// input
float _eI; /// integral of input
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 Block
{
public:
Sink(AP_Float & var, uint8_t port=0) :
_var(var), _port(port)
{
}
virtual void update(const float & dt)
{
_var = _input[_port]->get();
}
// doesn't connect
virtual void connect(Block * block) {}
private:
AP_Float & _var;
uint8_t _port;
};
/// Saturate block
/// Constrains output to a range
class Saturate : public Block
{
public:
Saturate(AP_Float & min, AP_Float & max, uint8_t port=0) :
_min(min), _max(max), _port(port)
{
}
virtual void update(const float & dt)
{
float u = _input[_port]->get();
if (u>_max) u = _max;
if (u<_min) u = _min;
_output[_port]->set(u);
}
private:
uint8_t _port;
AP_Float & _min;
AP_Float & _max;
};
/// Controller class
class AP_Controller
{
public:
void addBlock(Block * block)
{
if (_blocks.getSize() > 0)
block->connect(_blocks[_blocks.getSize()-1]);
_blocks.push_back(block);
}
void update(const double dt)
{
for (int i=0;i<_blocks.getSize();i++)
{
if (!_blocks[i]) continue;
_blocks[i]->update(dt);
}
}
private:
Vector<Block * > _blocks;
};
#endif // AP_Controller_H
// vim:ts=4:sw=4:expandtab

110
libraries/AP_Controller/examples/Car/Car.pde
Unescape View File

@ -1,110 +0,0 @@
#include <FastSerial.h>
#include <AP_Controller.h>
#include <AP_Var.h>
#include <AP_RcChannel.h>
#include <APM_RC.h>
FastSerialPort0(Serial);
class CarController : public AP_Controller
{
private:
// state
AP_Float & velocity;
AP_Float & heading;
// control variables
AP_Float & headingCommand;
AP_Float & velocityCommand;
// channels
AP_RcChannel * steeringCh;
AP_RcChannel * throttleCh;
public:
CarController(AP_Var::Key pidStrKey, AP_Var::Key pidThrKey,
AP_Float & heading, AP_Float & velocity,
AP_Float & headingCommand, AP_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,AP_Float_unity,heading,AP_Float_negative_unity));
addBlock(new Pid(pidStrKey,PSTR("STR"),1,1,1,1,20));
addBlock(new ToServo(steeringCh));
// throttle control loop
addBlock(new SumGain(velocityCommand,AP_Float_unity,velocity,AP_Float_negative_unity));
addBlock(new Pid(pidThrKey,PSTR("THR"),1,1,1,1,20));
addBlock(new ToServo(throttleCh));
}
};
AP_Controller * controller;
Vector<AP_RcChannel * > ch;
AP_Float heading, velocity; // measurements
AP_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.get(),headingCommand.get(), ch[0]->getNormalized());
Serial.printf("| thr:%f\tcmd:(%f)\tservo:%f\n", velocity.get(), velocityCommand.get(), ch[1]->getNormalized());
}
// vim:ts=4:sw=4:expandtab

130
libraries/AP_RcChannel/AP_RcChannel.cpp
Unescape View File

@ -1,130 +0,0 @@
/*
AP_RcChannel.cpp - Radio library for Arduino
Code by Jason Short, James Goppert. DIYDrones.com
This library is free software; you can redistribute it and / or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
#include <math.h>
#include <avr/eeprom.h>
#include "AP_RcChannel.h"
#include <AP_Common.h>
AP_RcChannel::AP_RcChannel(AP_Var::Key key, const prog_char * name, APM_RC_Class & rc, const uint8_t & ch,
const float & scale, const float & center,
const uint16_t & pwmMin,
const uint16_t & pwmNeutral, const uint16_t & pwmMax,
const uint16_t & pwmDeadZone,
const bool & filter, const bool & reverse) :
AP_Var_group(key,name),
_rc(rc),
ch(this,0,ch,PSTR("CH")),
scale(this,1,scale,PSTR("SCALE")),
center(this,2,center,PSTR("CENTER")),
pwmMin(this,3,pwmMin,PSTR("PMIN")),
pwmMax(this,4,pwmMax,PSTR("PMAX")),
pwmNeutral(this,5,pwmNeutral,PSTR("PNTRL")),
pwmDeadZone(this,6,pwmDeadZone,PSTR("PDEAD")),
filter(this,7,filter,PSTR("FLTR")),
reverse(this,8,reverse,PSTR("REV")),
_pwm(0)
{
setNormalized(0.0);
}
void AP_RcChannel::readRadio() {
// apply reverse
uint16_t pwmRadio = _rc.InputCh(ch);
setPwm(pwmRadio);
}
void
AP_RcChannel::setPwm(uint16_t pwm)
{
//Serial.printf("pwm in setPwm: %d\n", pwm);
//Serial.printf("reverse: %s\n", (reverse)?"true":"false");
// apply reverse
if(reverse) pwm = int16_t(pwmNeutral-pwm) + pwmNeutral;
//Serial.printf("pwm after reverse: %d\n", pwm);
// apply filter
if(filter){
if(_pwm == 0)
_pwm = pwm;
else
_pwm = ((pwm + _pwm) >> 1); // Small filtering
}else{
_pwm = pwm;
}
//Serial.printf("pwm after filter: %d\n", _pwm);
// apply deadzone
_pwm = (abs(_pwm - pwmNeutral) < pwmDeadZone) ? uint16_t(pwmNeutral) : _pwm;
//Serial.printf("pwm after deadzone: %d\n", _pwm);
_rc.OutputCh(ch,_pwm);
}
void
AP_RcChannel::setPosition(float position)
{
if (position > scale) position = scale;
else if (position < -scale) position = -scale;
setPwm(_positionToPwm(position));
}
void
AP_RcChannel::setNormalized(float normPosition)
{
setPosition(normPosition*scale);
}
void
AP_RcChannel::mixRadio(uint16_t infStart)
{
uint16_t pwmRadio = _rc.InputCh(ch);
float inf = abs( int16_t(pwmRadio - pwmNeutral) );
inf = min(inf, infStart);
inf = ((infStart - inf) /infStart);
setPwm(_pwm*inf + pwmRadio);
}
uint16_t
AP_RcChannel::_positionToPwm(const float & position)
{
uint16_t pwm;
//Serial.printf("position: %f\n", position);
float p = position - center;
if(p < 0)
pwm = p * int16_t(pwmNeutral - pwmMin) /
scale + pwmNeutral;
else
pwm = p * int16_t(pwmMax - pwmNeutral) /
scale + pwmNeutral;
constrain(pwm,uint16_t(pwmMin),uint16_t(pwmMax));
return pwm;
}
float
AP_RcChannel::_pwmToPosition(const uint16_t & pwm)
{
float position;
if(pwm < pwmNeutral)
position = scale * int16_t(pwm - pwmNeutral)/
int16_t(pwmNeutral - pwmMin) + center;
else
position = scale * int16_t(pwm - pwmNeutral)/
int16_t(pwmMax - pwmNeutral) + center;
constrain(position,center-scale,center+scale);
return position;
}
// ------------------------------------------

68
libraries/AP_RcChannel/AP_RcChannel.h
Unescape View File

@ -1,68 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AP_RcChannel.h
/// @brief AP_RcChannel manager
#ifndef AP_RcChannel_h
#define AP_RcChannel_h
#include <stdint.h>
#include <APM_RC.h>
#include <AP_Common.h>
#include <AP_Var.h>
/// @class AP_RcChannel
/// @brief Object managing one RC channel
class AP_RcChannel : public AP_Var_group {
public:
/// Constructor
AP_RcChannel(AP_Var::Key key, const prog_char * name, APM_RC_Class & rc, const uint8_t & ch,
const float & scale=45.0, const float & center=0.0,
const uint16_t & pwmMin=1200,
const uint16_t & pwmNeutral=1500, const uint16_t & pwmMax=1800,
const uint16_t & pwmDeadZone=10,
const bool & filter=false, const bool & reverse=false);
// configuration
AP_Uint8 ch;
AP_Float scale;
AP_Float center;
AP_Uint16 pwmMin;
AP_Uint16 pwmNeutral;
AP_Uint16 pwmMax;
AP_Uint16 pwmDeadZone;
AP_Bool filter;
AP_Bool reverse;
// set
void readRadio();
void setPwm(uint16_t pwm);
void setPosition(float position);
void setNormalized(float normPosition);
void mixRadio(uint16_t infStart);
// get
uint16_t getPwm() { return _pwm; }
float getPosition() { return _pwmToPosition(_pwm); }
float getNormalized() { return getPosition()/scale; }
// did our read come in 50µs below the min?
bool failSafe() { _pwm < (pwmMin - 50); }
private:
// configuration
const char * _name;
APM_RC_Class & _rc;
// internal states
uint16_t _pwm; // this is the internal state, position is just created when needed
// private methods
uint16_t _positionToPwm(const float & position);
float _pwmToPosition(const uint16_t & pwm);
};
#endif

94
libraries/AP_RcChannel/examples/Manual/Manual.pde
Unescape View File

@ -1,94 +0,0 @@
/*
Example of RC_Channel library.
Code by James Goppert/ Jason Short. 2010
DIYDrones.com
*/
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_RcChannel.h> // ArduPilot Mega RC Library
#include <APM_RC.h>
#include <AP_Vector.h>
FastSerialPort0(Serial); // make sure this proceeds variable declarations
// test settings
class RadioTest
{
private:
float testPosition;
int8_t testSign;
enum
{
version,
rollKey,
pitchKey,
thrKey,
yawKey,
ch5Key,
ch6Key,
ch7Key,
ch8Key
};
Vector<AP_RcChannel *> ch;
public:
RadioTest() : testPosition(2), testSign(1)
{
ch.push_back(new AP_RcChannel(rollKey,PSTR("ROLL"),APM_RC,1,45));
ch.push_back(new AP_RcChannel(pitchKey,PSTR("PITCH"),APM_RC,2,45));
ch.push_back(new AP_RcChannel(thrKey,PSTR("THR"),APM_RC,3,100));
ch.push_back(new AP_RcChannel(yawKey,PSTR("YAW"),APM_RC,4,45));
ch.push_back(new AP_RcChannel(ch5Key,PSTR("CH5"),APM_RC,5,1));
ch.push_back(new AP_RcChannel(ch6Key,PSTR("CH6"),APM_RC,6,1));
ch.push_back(new AP_RcChannel(ch7Key,PSTR("CH7"),APM_RC,7,1));
ch.push_back(new AP_RcChannel(ch8Key,PSTR("CH8"),APM_RC,8,1));
Serial.begin(115200);
delay(2000);
Serial.println("ArduPilot RC Channel test");
APM_RC.Init(); // APM Radio initialization
delay(2000);
}
void update()
{
// read the radio
for (int i=0;i<ch.getSize();i++) ch[i]->readRadio();
// print channel names
Serial.print("\t\t");
static char name[7];
for (int i=0;i<ch.getSize();i++)
{
ch[i]->copy_name(name,7);
Serial.printf("%7s\t",name);
}
Serial.println();
// print pwm
Serial.printf("pwm :\t");
for (int i=0;i<ch.getSize();i++) Serial.printf("%7d\t",ch[i]->getPwm());
Serial.println();
// print position
Serial.printf("position :\t");
for (int i=0;i<ch.getSize();i++) Serial.printf("%7.2f\t",ch[i]->getPosition());
Serial.println();
delay(500);
}
};
RadioTest * test;
void setup()
{
test = new RadioTest;
}
void loop()
{
test->update();
}

113
libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde
Unescape View File

@ -1,113 +0,0 @@
/*
Example of RC_Channel library.
Code by James Goppert/ Jason Short. 2010
DIYDrones.com
*/
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_RcChannel.h> // ArduPilot Mega RC Library
#include <APM_RC.h>
#include <AP_Vector.h>
FastSerialPort0(Serial); // make sure this proceeds variable declarations
// test settings
class RadioTest
{
private:
float testPosition;
int8_t testSign;
enum
{
version,
rollKey,
pitchKey,
thrKey,
yawKey,
ch5Key,
ch6Key,
ch7Key,
ch8Key
};
Vector<AP_RcChannel *> ch;
public:
RadioTest() : testPosition(2), testSign(1)
{
ch.push_back(new AP_RcChannel(rollKey,PSTR("ROLL"),APM_RC,1,45));
ch.push_back(new AP_RcChannel(pitchKey,PSTR("PITCH"),APM_RC,2,45));
ch.push_back(new AP_RcChannel(thrKey,PSTR("THR"),APM_RC,3,100));
ch.push_back(new AP_RcChannel(yawKey,PSTR("YAW"),APM_RC,4,45));
ch.push_back(new AP_RcChannel(ch5Key,PSTR("CH5"),APM_RC,5,1));
ch.push_back(new AP_RcChannel(ch6Key,PSTR("CH6"),APM_RC,6,1));
ch.push_back(new AP_RcChannel(ch7Key,PSTR("CH7"),APM_RC,7,1));
ch.push_back(new AP_RcChannel(ch8Key,PSTR("CH8"),APM_RC,8,1));
Serial.begin(115200);
delay(2000);
Serial.println("ArduPilot RC Channel test");
APM_RC.Init(); // APM Radio initialization
delay(2000);
}
void update()
{
// update test value
testPosition += testSign*.1;
if (testPosition > 1)
{
//eepromRegistry.print(Serial); // show eeprom map
testPosition = 1;
testSign = -1;
}
else if (testPosition < -1)
{
testPosition = -1;
testSign = 1;
}
// set channel positions
for (int i=0;i<ch.getSize();i++) ch[i]->setNormalized(testPosition);
// print test position
Serial.printf("\nnormalized position (%f)\n",testPosition);
// print channel names
Serial.print("\t\t");
static char name[7];
for (int i=0;i<ch.getSize();i++)
{
ch[i]->copy_name(name,7);
Serial.printf("%7s\t",name);
}
Serial.println();
// print pwm
Serial.printf("pwm :\t");
for (int i=0;i<ch.getSize();i++) Serial.printf("%7d\t",ch[i]->getPwm());
Serial.println();
// print position
Serial.printf("position :\t");
for (int i=0;i<ch.getSize();i++) Serial.printf("%7.2f\t",ch[i]->getPosition());
Serial.println();
delay(500);
}
};
RadioTest * test;
void setup()
{
test = new RadioTest;
}
void loop()
{
test->update();
}
// vim:ts=4:sw=4:expandtab
Write Preview
Loading…
Cancel
Save