diff --git a/libraries/AP_Controller/AP_Controller.cpp b/libraries/AP_Controller/AP_Controller.cpp deleted file mode 100644 index 078a94e2e9..0000000000 --- a/libraries/AP_Controller/AP_Controller.cpp +++ /dev/null @@ -1,21 +0,0 @@ -/* - * AP_Controller.cpp - * Copyright (C) James Goppert 2010 - * - * 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 . - */ - -#include "AP_Controller.h" - -// vim:ts=4:sw=4:expandtab diff --git a/libraries/AP_Controller/AP_Controller.h b/libraries/AP_Controller/AP_Controller.h deleted file mode 100644 index f0e353223e..0000000000 --- a/libraries/AP_Controller/AP_Controller.h +++ /dev/null @@ -1,249 +0,0 @@ -/* - * AP_Controller.h - * Copyright (C) James Goppert 2010 - * - * 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 . - */ - -#ifndef AP_Controller_H -#define AP_Controller_H - -#include -#include -#include -#include -#include - - -/// 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 _blocks; -}; - -#endif // AP_Controller_H - -// vim:ts=4:sw=4:expandtab diff --git a/libraries/AP_Controller/examples/Car/Car.pde b/libraries/AP_Controller/examples/Car/Car.pde deleted file mode 100644 index f86cfaca6c..0000000000 --- a/libraries/AP_Controller/examples/Car/Car.pde +++ /dev/null @@ -1,110 +0,0 @@ -#include -#include -#include -#include -#include - -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 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 diff --git a/libraries/AP_RcChannel/AP_RcChannel.cpp b/libraries/AP_RcChannel/AP_RcChannel.cpp deleted file mode 100644 index 19c84735e8..0000000000 --- a/libraries/AP_RcChannel/AP_RcChannel.cpp +++ /dev/null @@ -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 -#include -#include "AP_RcChannel.h" -#include - -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; -} - -// ------------------------------------------ diff --git a/libraries/AP_RcChannel/AP_RcChannel.h b/libraries/AP_RcChannel/AP_RcChannel.h deleted file mode 100644 index e9271bb212..0000000000 --- a/libraries/AP_RcChannel/AP_RcChannel.h +++ /dev/null @@ -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 -#include -#include -#include - -/// @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 diff --git a/libraries/AP_RcChannel/examples/Manual/Manual.pde b/libraries/AP_RcChannel/examples/Manual/Manual.pde deleted file mode 100644 index 0649b1f643..0000000000 --- a/libraries/AP_RcChannel/examples/Manual/Manual.pde +++ /dev/null @@ -1,94 +0,0 @@ -/* - Example of RC_Channel library. - Code by James Goppert/ Jason Short. 2010 - DIYDrones.com - -*/ - -#include -#include -#include // ArduPilot Mega RC Library -#include -#include - -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 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;ireadRadio(); - - // print channel names - Serial.print("\t\t"); - static char name[7]; - for (int i=0;icopy_name(name,7); - Serial.printf("%7s\t",name); - } - Serial.println(); - - // print pwm - Serial.printf("pwm :\t"); - for (int i=0;igetPwm()); - Serial.println(); - - // print position - Serial.printf("position :\t"); - for (int i=0;igetPosition()); - Serial.println(); - - delay(500); - } -}; - -RadioTest * test; - -void setup() -{ - test = new RadioTest; -} - -void loop() -{ - test->update(); -} diff --git a/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde b/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde deleted file mode 100644 index 2d5de8215d..0000000000 --- a/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde +++ /dev/null @@ -1,113 +0,0 @@ -/* - Example of RC_Channel library. - Code by James Goppert/ Jason Short. 2010 - DIYDrones.com - -*/ - -#include -#include -#include // ArduPilot Mega RC Library -#include -#include - -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 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;isetNormalized(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;icopy_name(name,7); - Serial.printf("%7s\t",name); - } - Serial.println(); - - // print pwm - Serial.printf("pwm :\t"); - for (int i=0;igetPwm()); - Serial.println(); - - // print position - Serial.printf("position :\t"); - for (int i=0;igetPosition()); - Serial.println(); - - delay(500); - } -}; - -RadioTest * test; - -void setup() -{ - test = new RadioTest; -} - -void loop() -{ - test->update(); -} - -// vim:ts=4:sw=4:expandtab