Browse Source
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1691 f9c3cf11-9bcb-44bc-f272-b75c42450872master
7 changed files with 0 additions and 785 deletions
@ -1,21 +0,0 @@
@@ -1,21 +0,0 @@
|
||||
/*
|
||||
* AP_Controller.cpp |
||||
* Copyright (C) James Goppert 2010 <james.goppert@gmail.com> |
||||
* |
||||
* This file is free software: you can redistribute it and/or modify it |
||||
* under the terms of the GNU General Public License as published by the |
||||
* Free Software Foundation, either version 3 of the License, or |
||||
* (at your option) any later version. |
||||
* |
||||
* This file is distributed in the hope that it will be useful, but |
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
||||
* See the GNU General Public License for more details. |
||||
* |
||||
* You should have received a copy of the GNU General Public License along |
||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
|
||||
#include "AP_Controller.h" |
||||
|
||||
// vim:ts=4:sw=4:expandtab
|
@ -1,249 +0,0 @@
@@ -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
|
@ -1,110 +0,0 @@
@@ -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 |
@ -1,130 +0,0 @@
@@ -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; |
||||
} |
||||
|
||||
// ------------------------------------------
|
@ -1,68 +0,0 @@
@@ -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 |
@ -1,94 +0,0 @@
@@ -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(); |
||||
} |
@ -1,113 +0,0 @@
@@ -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 |
Loading…
Reference in new issue