mirror of https://github.com/ArduPilot/ardupilot
Consolidating APO.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1691 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert
parent
33a720bbaa
commit
a329aa18fe
|
|
@ -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 @@
|
||||||
/*
|
|
||||||
* 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 @@
|
||||||
#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 @@
|
||||||
/*
|
|
||||||
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 @@
|
||||||
// -*- 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 @@
|
||||||
/*
|
|
||||||
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 @@
|
||||||
/*
|
|
||||||
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