Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Added back some APO libraries.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1813 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert 2011-03-25 17:08:35 +00:00
parent 0a00e4ff21
commit 43cd36e5b6
10 changed files with 799 additions and 236 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_EEProm/AP_EEProm.cpp → libraries/AP_Controller/AP_Controller.cpp
View File

@ -1,5 +1,5 @@
/*
* AP_EEProm.cpp
* 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
@ -16,29 +16,6 @@
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_EEProm.h>
void AP_EEPromRegistry::print(BetterStream & stream)
{
stream.printf("\nEEPROM Registry\n");
for (int i=0;i<getSize();i++)
{
stream.printf("id:\t%u\t%s\t%s\tval:\t%10.4f\taddr:\t%u\t\n",
(*this)[i]->getEntryId(),
(*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

281
libraries/AP_Controller/AP_Controller.h Normal file
View File

@ -0,0 +1,281 @@
/*
* 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 double 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)
{
}
// 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<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)
{
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

110
libraries/AP_Controller/examples/Car/Car.pde Normal file
View File

@ -0,0 +1,110 @@
#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

121
libraries/AP_EEProm/AP_EEProm.h
View File

@ -1,121 +0,0 @@
#ifndef AP_EEProm_H
#define AP_EEProm_H
/*
* AP_EEProm.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/>.
*/
#include <AP_Common.h>
#include <AP_Vector.h>
#include <avr/eeprom.h>
#include <avr/pgmspace.h>
#include <BetterStream.h>
/// 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<AP_EEPromEntryI *>
{
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 type>
class AP_EEPromVar : public AP_EEPromEntryI, public AP_Var<type>
{
public:
/// The default constrcutor
AP_EEPromVar(type data = 0, const char * name = "", const char * parentName = "", bool sync=false) :
AP_Var<type>(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<float> AP_EEPROM_Float;
typedef AP_EEPromVar<int8_t> AP_EEPROM_Int8;
typedef AP_EEPromVar<uint8_t> AP_EEPROM_Uint8;
typedef AP_EEPromVar<int16_t> AP_EEPROM_Int16;
typedef AP_EEPromVar<uint16_t> AP_EEPROM_Uint16;
typedef AP_EEPromVar<int32_t> AP_EEPROM_Int32;
typedef AP_EEPromVar<uint32_t> AP_EEPROM_Unt32;
typedef AP_EEPromVar<bool> AP_EEPROM_Bool;
#endif

89
libraries/AP_EEProm/examples/AP_EEProm/AP_EEProm.pde
View File

@ -1,89 +0,0 @@
/*
* Libraries
*/
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_EEProm.h>
AP_EEPromVar<int> var(1,"TEST_VAR1");
AP_EEPromVar<float> var2(2.0,"TEST_VAR2");
AP_EEPromVar<int16_t> 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);
}

0
libraries/AP_EEProm/keywords.txt
View File

130
libraries/AP_RcChannel/AP_RcChannel.cpp Normal file
View File

@ -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 <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_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;
}
// ------------------------------------------

68
libraries/AP_RcChannel/AP_RcChannel.h Normal file
View File

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

94
libraries/AP_RcChannel/examples/Manual/Manual.pde Normal file
View File

@ -0,0 +1,94 @@
/*
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 Normal file
View File

@ -0,0 +1,113 @@
/*
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