From 43cd36e5b60c2ab2e562d0ed40fa5c2be6bab950 Mon Sep 17 00:00:00 2001 From: "james.goppert" Date: Fri, 25 Mar 2011 17:08:35 +0000 Subject: [PATCH] Added back some APO libraries. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1813 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- .../AP_Controller.cpp} | 29 +- libraries/AP_Controller/AP_Controller.h | 281 ++++++++++++++++++ libraries/AP_Controller/examples/Car/Car.pde | 110 +++++++ libraries/AP_EEProm/AP_EEProm.h | 121 -------- .../examples/AP_EEProm/AP_EEProm.pde | 89 ------ libraries/AP_EEProm/keywords.txt | 0 libraries/AP_RcChannel/AP_RcChannel.cpp | 130 ++++++++ libraries/AP_RcChannel/AP_RcChannel.h | 68 +++++ .../AP_RcChannel/examples/Manual/Manual.pde | 94 ++++++ .../examples/ServoSweep/ServoSweep.pde | 113 +++++++ 10 files changed, 799 insertions(+), 236 deletions(-) rename libraries/{AP_EEProm/AP_EEProm.cpp => AP_Controller/AP_Controller.cpp} (51%) create mode 100644 libraries/AP_Controller/AP_Controller.h create mode 100644 libraries/AP_Controller/examples/Car/Car.pde delete mode 100644 libraries/AP_EEProm/AP_EEProm.h delete mode 100644 libraries/AP_EEProm/examples/AP_EEProm/AP_EEProm.pde delete mode 100644 libraries/AP_EEProm/keywords.txt create mode 100644 libraries/AP_RcChannel/AP_RcChannel.cpp create mode 100644 libraries/AP_RcChannel/AP_RcChannel.h create mode 100644 libraries/AP_RcChannel/examples/Manual/Manual.pde create mode 100644 libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde diff --git a/libraries/AP_EEProm/AP_EEProm.cpp b/libraries/AP_Controller/AP_Controller.cpp similarity index 51% rename from libraries/AP_EEProm/AP_EEProm.cpp rename to libraries/AP_Controller/AP_Controller.cpp index df30c9c46e..078a94e2e9 100644 --- a/libraries/AP_EEProm/AP_EEProm.cpp +++ b/libraries/AP_Controller/AP_Controller.cpp @@ -1,5 +1,5 @@ /* - * AP_EEProm.cpp + * AP_Controller.cpp * Copyright (C) James Goppert 2010 * * This file is free software: you can redistribute it and/or modify it @@ -16,29 +16,6 @@ * with this program. If not, see . */ -#include -void AP_EEPromRegistry::print(BetterStream & stream) -{ - stream.printf("\nEEPROM Registry\n"); - for (int i=0;igetEntryId(), - (*this)[i]->getEntryParentName(), - (*this)[i]->getEntryName(), - (*this)[i]->getEntry(), - (*this)[i]->getEntryAddress()); - } -} +#include "AP_Controller.h" - -void AP_EEPromRegistry::add(AP_EEPromEntryI * entry, uint16_t & id, uint16_t & address, size_t size) -{ - if (_newAddress + size > _maxSize) return; - address = _newAddress; - _newAddress += size; - id = _newId++; - push_back(entry); -} - -extern AP_EEPromRegistry eepromRegistry(1024); +// vim:ts=4:sw=4:expandtab diff --git a/libraries/AP_Controller/AP_Controller.h b/libraries/AP_Controller/AP_Controller.h new file mode 100644 index 0000000000..ea998c1492 --- /dev/null +++ b/libraries/AP_Controller/AP_Controller.h @@ -0,0 +1,281 @@ +/* + * 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 "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;igetOutput().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 double dt) + { + for (int i=0;i<_blocks.getSize();i++) + { + if (!_blocks[i]) continue; + _blocks[i]->update(dt); + } + } + + private: + Vector _blocks; +}; + + +/// Servo Block +class ToServo : public AP_Controller::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)); + } + } +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) + { + if (_output.getSize() < 1) return; + float sum =0; + for (int i=0;i<_input.getSize();i++) sum += input(i) * gain(i); + output(0) = sum; + } + float gain(int i) { return *(_gain[i]); } +private: + Vector _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) + { + 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; + + // 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) + { + _var = input(_port); + } + // doesn't connect + virtual void connect(Block * block) {} +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 + _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 diff --git a/libraries/AP_Controller/examples/Car/Car.pde b/libraries/AP_Controller/examples/Car/Car.pde new file mode 100644 index 0000000000..f86cfaca6c --- /dev/null +++ b/libraries/AP_Controller/examples/Car/Car.pde @@ -0,0 +1,110 @@ +#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_EEProm/AP_EEProm.h b/libraries/AP_EEProm/AP_EEProm.h deleted file mode 100644 index f9daef2ad0..0000000000 --- a/libraries/AP_EEProm/AP_EEProm.h +++ /dev/null @@ -1,121 +0,0 @@ -#ifndef AP_EEProm_H -#define AP_EEProm_H - -/* - * AP_EEProm.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 . - */ - -#include -#include -#include -#include -#include - -/// The interface for data entries in the eeprom registry -class AP_EEPromEntryI -{ -public: - /// Pure virtual function for setting the data value - /// as a float. The function must handle the cast to - /// the stored variable types. - virtual void setEntry(const float & val) = 0; - - /// Pure virtual function for getting data as a float. - /// The function must handle the cast from the - /// stored variable types. - virtual const float getEntry() = 0; - - /// Pure virtual function for getting entry name. - virtual const char * getEntryName() = 0; - - /// Pure virtual function for getting entry parent name. - virtual const char * getEntryParentName() = 0; - - /// Get the id of the variable. - virtual const uint16_t & getEntryId() = 0; - - /// Get the address of the variable. - virtual const uint16_t & getEntryAddress() = 0; -}; - -///The main EEProm Registry class. -class AP_EEPromRegistry : public Vector -{ -public: - - /// Default constructor - AP_EEPromRegistry(uint16_t maxSize) : - _newAddress(0), _newId(0), _maxSize(maxSize) - { - } - - /// Add an entry to the registry - void add(AP_EEPromEntryI * entry, uint16_t & id, uint16_t & address, size_t size); - - /// Print on desired serial port - void print(BetterStream & stream); - -private: - uint16_t _newAddress; /// the address for the next new variable - uint16_t _newId; /// the id of the next new variable - uint16_t _maxSize; /// the maximum size of the eeprom memory -}; - -/// Global eepromRegistry declaration. -extern AP_EEPromRegistry eepromRegistry; - -/// The EEProm Variable template class. -/// This class implements get/set/save/load etc for the -/// abstract template type. -template -class AP_EEPromVar : public AP_EEPromEntryI, public AP_Var -{ -public: - /// The default constrcutor - AP_EEPromVar(type data = 0, const char * name = "", const char * parentName = "", bool sync=false) : - AP_Var(data,name,parentName,sync) - { - eepromRegistry.add(this,_id,_address,sizeof(type)); - } - - virtual void setEntry(const float & val) { this->setF(val); } - virtual const float getEntry() { return this->getF(); } - virtual const char * getEntryName() { return this->getName(); } - virtual const char * getEntryParentName() { return this->getParentName(); } - - /// Get the id of the variable. - virtual const uint16_t & getEntryId() { return _id; } - - /// Get the address of the variable. - virtual const uint16_t & getEntryAddress() { return _address; } - -private: - uint16_t _id; /// Variable identifier - uint16_t _address; /// EEProm address of variable -}; - -typedef AP_EEPromVar AP_EEPROM_Float; -typedef AP_EEPromVar AP_EEPROM_Int8; -typedef AP_EEPromVar AP_EEPROM_Uint8; -typedef AP_EEPromVar AP_EEPROM_Int16; -typedef AP_EEPromVar AP_EEPROM_Uint16; -typedef AP_EEPromVar AP_EEPROM_Int32; -typedef AP_EEPromVar AP_EEPROM_Unt32; -typedef AP_EEPromVar AP_EEPROM_Bool; - - -#endif diff --git a/libraries/AP_EEProm/examples/AP_EEProm/AP_EEProm.pde b/libraries/AP_EEProm/examples/AP_EEProm/AP_EEProm.pde deleted file mode 100644 index 3a1b242f60..0000000000 --- a/libraries/AP_EEProm/examples/AP_EEProm/AP_EEProm.pde +++ /dev/null @@ -1,89 +0,0 @@ -/* - * Libraries - */ -#include -#include -#include - -AP_EEPromVar var(1,"TEST_VAR1"); -AP_EEPromVar var2(2.0,"TEST_VAR2"); -AP_EEPromVar var3(-700,"TEST_VAR3"); -FastSerialPort0(Serial); - -void setup() -{ - Serial.begin(115200); - Serial.println("starting"); - displayMemory(); -} - -void loop() -{ - Serial.print("\n\nUn-synced variable demo (default)\n"); - - var.setSync(false); - var.set(123); - Serial.printf_P(PSTR("\nvar.setSync(false): var.set(123): %d\n"), var.get()); - delay(2000); - - var.save(); - var.set(456); - Serial.printf_P(PSTR("\nvar.save(): var.set(456): %d\n"), var.get()); - delay(2000); - - var.load(); - Serial.printf_P(PSTR("\nval.load(): %d\n"), var.get()); - delay(2000); - - uint16_t id = var.getEntryId(); - Serial.printf_P(PSTR("\nvar.getEntryId(): %d\n"), id); - delay(2000); - - Serial.printf_P(PSTR("\neepromRegistry(id)->getEntry(): %f\n"), eepromRegistry[id]->getEntry()); - delay(2000); - - eepromRegistry[id]->setEntry(456); - Serial.printf_P(PSTR("\neepromRegistry(id)->setEntry(456): %d\n"), var.get()); - delay(2000); - - Serial.printf_P(PSTR("\nprint the parameters name by id: %s\n"), eepromRegistry[id]->getEntryName()); - delay(2000); - - Serial.printf_P(PSTR("\nprint the parameters address by id: %d\n"), int(eepromRegistry[id]->getEntryAddress())); - delay(2000); - - - Serial.print("\n\nSynced variable demo\n"); - - var2.setSync(true); - var2.set(1.23); - Serial.printf_P(PSTR("\nvar2.setSync(false): var2.set(1.23): %f\n"), var2.get()); - delay(2000); - - var2.save(); - var2.set(4.56); - Serial.printf_P(PSTR("\nvar2.save(): var2.set(4.56): %f\n"), var2.get()); - delay(2000); - - var2.load(); - Serial.printf_P(PSTR("\nvar2.load(): %f\n"), var2.get()); - delay(2000); - - id = var2.getEntryId(); - Serial.printf_P(PSTR("\nvar2.getEntryId(): %d\n"), id); - delay(2000); - - Serial.printf_P(PSTR("\neepromRegistry(id)->getEntry(): %f\n"), eepromRegistry[id]->getEntry()); - delay(2000); - - eepromRegistry[id]->setEntry(4.56); - Serial.printf_P(PSTR("\neepromRegistry(id)->setEntry(4.56): %f\n"), var2.get()); - delay(2000); - - Serial.printf_P(PSTR("\nprint the parameters name by id: %s\n"), eepromRegistry[id]->getEntryName()); - delay(2000); - - Serial.printf_P(PSTR("\nprint the parameters address by id: %d\n"), int(eepromRegistry[id]->getEntryAddress())); - - delay(5000); -} diff --git a/libraries/AP_EEProm/keywords.txt b/libraries/AP_EEProm/keywords.txt deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/libraries/AP_RcChannel/AP_RcChannel.cpp b/libraries/AP_RcChannel/AP_RcChannel.cpp new file mode 100644 index 0000000000..1d0fef3bfb --- /dev/null +++ b/libraries/AP_RcChannel/AP_RcChannel.cpp @@ -0,0 +1,130 @@ +/* + 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_t * 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 new file mode 100644 index 0000000000..9b0522f669 --- /dev/null +++ b/libraries/AP_RcChannel/AP_RcChannel.h @@ -0,0 +1,68 @@ +// -*- 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_t * 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 new file mode 100644 index 0000000000..0649b1f643 --- /dev/null +++ b/libraries/AP_RcChannel/examples/Manual/Manual.pde @@ -0,0 +1,94 @@ +/* + 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 new file mode 100644 index 0000000000..2d5de8215d --- /dev/null +++ b/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde @@ -0,0 +1,113 @@ +/* + 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