RcChannel now owns channel config params.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1354 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert
parent
f2f38f2e71
commit
e3041f08fe
@ -21,11 +21,10 @@
|
||||
|
||||
#include "AP_Vector.h"
|
||||
|
||||
/**
|
||||
* Start of apo namespace
|
||||
* The above functions must be in the global
|
||||
* namespace for c++ to function properly
|
||||
*/
|
||||
///
|
||||
// Start of apo namespace
|
||||
// The above functions must be in the global
|
||||
// namespace for c++ to function properly
|
||||
namespace apo
|
||||
{
|
||||
|
||||
|
||||
@ -5,9 +5,12 @@
|
||||
// Free Software Foundation; either version 2.1 of the License, or (at
|
||||
// your option) any later version.
|
||||
//
|
||||
/// The AP variable interface. This allows different types
|
||||
/// The AP variable interface. This allows different types
|
||||
/// of variables to be passed to blocks for floating point
|
||||
/// math, memory management, etc.
|
||||
#ifndef AP_Var_H
|
||||
#define AP_Var_H
|
||||
|
||||
class AP_VarI
|
||||
{
|
||||
public:
|
||||
@ -15,9 +18,15 @@ public:
|
||||
/// Set the variable value as a float
|
||||
virtual void setF(const float & val) = 0;
|
||||
|
||||
/// Get the variable as a float
|
||||
/// Get the variable value as a float
|
||||
virtual const float & getF() = 0;
|
||||
|
||||
/// Set the variable value as an Int16
|
||||
virtual void setI(const int16_t & val) = 0;
|
||||
|
||||
/// Get the variable value as an Int16
|
||||
virtual const int16_t & getI() = 0;
|
||||
|
||||
/// Save a variable to eeprom
|
||||
virtual void save() = 0;
|
||||
|
||||
@ -48,14 +57,20 @@ public:
|
||||
{
|
||||
}
|
||||
|
||||
/// float copy constructor
|
||||
AP_Var(AP_VarI & v)
|
||||
{
|
||||
setF(v.getF());
|
||||
}
|
||||
|
||||
/// Set the variable value
|
||||
virtual void set(const type & val) {
|
||||
void set(const type & val) {
|
||||
_data = val;
|
||||
if (_sync) save();
|
||||
}
|
||||
|
||||
/// Get the variable value.
|
||||
virtual const type & get() {
|
||||
const type & get() {
|
||||
if (_sync) load();
|
||||
return _data;
|
||||
}
|
||||
@ -70,6 +85,26 @@ public:
|
||||
return get();
|
||||
}
|
||||
|
||||
/// Set the variable value as an Int16
|
||||
virtual void setI(const int16_t & val) {
|
||||
set(val);
|
||||
}
|
||||
|
||||
/// Get the variable value as an Int16
|
||||
virtual const int16_t & getI() {
|
||||
return get();
|
||||
}
|
||||
|
||||
/// Set the variable value as an Int16
|
||||
virtual void setB(const bool & val) {
|
||||
set(val);
|
||||
}
|
||||
|
||||
/// Get the variable value as an Int16
|
||||
virtual const bool & getB() {
|
||||
return get();
|
||||
}
|
||||
|
||||
/// Save a variable to eeprom
|
||||
virtual void save()
|
||||
{
|
||||
@ -93,3 +128,14 @@ protected:
|
||||
const char * _name; /// The variable name, useful for gcs and terminal output
|
||||
bool _sync; /// Whether or not to call save/load on get/set
|
||||
};
|
||||
|
||||
typedef AP_Var<float> AP_Float;
|
||||
typedef AP_Var<int8_t> AP_Int8;
|
||||
typedef AP_Var<uint8_t> AP_Uint8;
|
||||
typedef AP_Var<int16_t> AP_Int16;
|
||||
typedef AP_Var<uint16_t> AP_Uint16;
|
||||
typedef AP_Var<int32_t> AP_Int32;
|
||||
typedef AP_Var<uint32_t> AP_Unt32;
|
||||
typedef AP_Var<bool> AP_Bool;
|
||||
|
||||
#endif // AP_Var_H
|
||||
|
||||
@ -100,4 +100,14 @@ private:
|
||||
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
|
||||
|
||||
@ -16,22 +16,22 @@
|
||||
|
||||
void AP_RcChannel::readRadio() {
|
||||
// apply reverse
|
||||
uint16_t pwmRadio = APM_RC.InputCh(_ch);
|
||||
uint16_t pwmRadio = APM_RC.InputCh(getCh());
|
||||
setPwm(pwmRadio);
|
||||
}
|
||||
|
||||
void
|
||||
AP_RcChannel::setPwm(uint16_t pwm)
|
||||
{
|
||||
//Serial.printf("reverse: %s\n", (_reverse)?"true":"false");
|
||||
//Serial.printf("reverse: %s\n", (getReverse())?"true":"false");
|
||||
|
||||
// apply reverse
|
||||
if(_reverse) pwm = int16_t(_pwmNeutral-pwm) + _pwmNeutral;
|
||||
if(getReverse()) pwm = int16_t(getPwmNeutral()-pwm) + getPwmNeutral();
|
||||
|
||||
//Serial.printf("pwm after reverse: %d\n", pwm);
|
||||
|
||||
// apply filter
|
||||
if(_filter){
|
||||
if(getFilter()){
|
||||
if(_pwm == 0)
|
||||
_pwm = pwm;
|
||||
else
|
||||
@ -43,10 +43,10 @@ AP_RcChannel::setPwm(uint16_t pwm)
|
||||
//Serial.printf("pwm after filter: %d\n", _pwm);
|
||||
|
||||
// apply deadzone
|
||||
_pwm = (abs(_pwm - _pwmNeutral) < _pwmDeadZone) ? _pwmNeutral : _pwm;
|
||||
_pwm = (abs(_pwm - getPwmNeutral()) < getPwmDeadZone()) ? getPwmNeutral() : _pwm;
|
||||
|
||||
//Serial.printf("pwm after deadzone: %d\n", _pwm);
|
||||
APM_RC.OutputCh(_ch,_pwm);
|
||||
APM_RC.OutputCh(getCh(),_pwm);
|
||||
}
|
||||
|
||||
void
|
||||
@ -58,8 +58,8 @@ AP_RcChannel::setPosition(float position)
|
||||
void
|
||||
AP_RcChannel::mixRadio(uint16_t infStart)
|
||||
{
|
||||
uint16_t pwmRadio = APM_RC.InputCh(_ch);
|
||||
float inf = abs( int16_t(pwmRadio - _pwmNeutral) );
|
||||
uint16_t pwmRadio = APM_RC.InputCh(getCh());
|
||||
float inf = abs( int16_t(pwmRadio - getPwmNeutral()) );
|
||||
inf = min(inf, infStart);
|
||||
inf = ((infStart - inf) /infStart);
|
||||
setPwm(_pwm*inf + pwmRadio);
|
||||
@ -69,12 +69,15 @@ uint16_t
|
||||
AP_RcChannel::_positionToPwm(const float & position)
|
||||
{
|
||||
uint16_t pwm;
|
||||
float p = position - getCenter();
|
||||
//Serial.printf("position: %f\n", position);
|
||||
if(position < 0)
|
||||
pwm = position * int16_t(_pwmNeutral - _pwmMin) / _scale + _pwmNeutral;
|
||||
if(p < 0)
|
||||
pwm = p * int16_t(getPwmNeutral() - getPwmMin()) /
|
||||
getScale() + getPwmNeutral();
|
||||
else
|
||||
pwm = position * int16_t(_pwmMax - _pwmNeutral) / _scale + _pwmNeutral;
|
||||
constrain(pwm,_pwmMin,_pwmMax);
|
||||
pwm = p * int16_t(getPwmMax() - getPwmNeutral()) /
|
||||
getScale() + getPwmNeutral();
|
||||
constrain(pwm,getPwmMin(),getPwmMax());
|
||||
return pwm;
|
||||
}
|
||||
|
||||
@ -82,11 +85,14 @@ float
|
||||
AP_RcChannel::_pwmToPosition(const uint16_t & pwm)
|
||||
{
|
||||
float position;
|
||||
if(pwm < _pwmNeutral)
|
||||
position = _scale * int16_t(pwm - _pwmNeutral)/ int16_t(_pwmNeutral - _pwmMin);
|
||||
if(pwm < getPwmNeutral())
|
||||
position = getScale() * int16_t(pwm - getPwmNeutral())/
|
||||
int16_t(getPwmNeutral() - getPwmMin()) + getCenter();
|
||||
else
|
||||
position = _scale * int16_t(pwm - _pwmNeutral)/ int16_t(_pwmMax - _pwmNeutral);
|
||||
constrain(position,-_scale,_scale);
|
||||
position = getScale() * int16_t(pwm -getPwmNeutral())/
|
||||
int16_t(getPwmMax() - getPwmNeutral()) + getCenter();
|
||||
constrain(position,-getScale()+getCenter(),
|
||||
getScale()+getCenter());
|
||||
return position;
|
||||
}
|
||||
|
||||
|
||||
@ -9,6 +9,9 @@
|
||||
#include <stdint.h>
|
||||
#include <FastSerial.h>
|
||||
#include <APM_RC.h>
|
||||
#include <AP_Var.h>
|
||||
#include <AP_Common.h>
|
||||
#include <AP_EEProm.h>
|
||||
|
||||
/// @class AP_RcChannel
|
||||
/// @brief Object managing one RC channel
|
||||
@ -17,49 +20,83 @@ class AP_RcChannel{
|
||||
public:
|
||||
|
||||
/// Constructor
|
||||
AP_RcChannel(const APM_RC_Class & rc, const uint16_t & ch,
|
||||
const float & scale, const uint16_t & pwmMin, const uint16_t & pwmNeutral,
|
||||
const uint16_t & pwmMax, const uint16_t & pwmDeadZone,
|
||||
const bool & filter, const bool & reverse) :
|
||||
AP_RcChannel(const char * name, const 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=100,
|
||||
const bool & filter=false, const bool & reverse=false) :
|
||||
_name(name),
|
||||
_rc(rc),
|
||||
_ch(ch),
|
||||
_scale(scale),
|
||||
_pwmMin(pwmMin),
|
||||
_pwmMax(pwmMax),
|
||||
_pwmNeutral(pwmNeutral),
|
||||
_pwmDeadZone(pwmDeadZone),
|
||||
_ch(new AP_EEPROM_Uint8(ch,createName("CH"))),
|
||||
_scale(new AP_EEPROM_Float(scale,createName("SCALE"))),
|
||||
_center(new AP_EEPROM_Float(center,createName("CNTR"))),
|
||||
_pwmMin(new AP_EEPROM_Uint16(pwmMin,createName("PMIN"))),
|
||||
_pwmMax(new AP_EEPROM_Uint16(pwmMax,createName("PMAX"))),
|
||||
_pwmNeutral(new AP_EEPROM_Uint16(pwmNeutral,createName("PNTRL"))),
|
||||
_pwmDeadZone(new AP_EEPROM_Uint16(pwmDeadZone,createName("PDEAD"))),
|
||||
_pwm(0),
|
||||
_filter(filter),
|
||||
_reverse(reverse)
|
||||
_filter(new AP_EEPROM_Bool(filter,createName("FLTR"))),
|
||||
_reverse(new AP_EEPROM_Bool(reverse,createName("RVRS")))
|
||||
{
|
||||
}
|
||||
|
||||
// set servo state
|
||||
// set
|
||||
void readRadio();
|
||||
void setPwm(uint16_t pwm);
|
||||
void setPosition(float position);
|
||||
void mixRadio(uint16_t infStart);
|
||||
void setCh(const uint8_t & ch) { _ch->set(ch); }
|
||||
void setScale(const float & scale) { _scale->set(scale); }
|
||||
void setCenter(const float & center) { _center->set(center); }
|
||||
void setPwmMin(const uint16_t & pwmMin) { _pwmMin->set(pwmMin); }
|
||||
void setPwmNeutral(const uint16_t & pwmNeutral) { _pwmNeutral->set(pwmNeutral); }
|
||||
void setPwmMax(const uint16_t & pwmMax) { _pwmMax->set(pwmMax); }
|
||||
void setPwmDeadZone(const uint16_t & pwmDeadZone) { _pwmDeadZone->set(pwmDeadZone); }
|
||||
void setFilter(const bool & filter) { _filter->set(filter); }
|
||||
|
||||
// get servo state
|
||||
// get
|
||||
uint16_t getPwm() { return _pwm; }
|
||||
float getPosition() { return _pwmToPosition(_pwm); }
|
||||
float getNormalized() { return getPosition()/_scale; }
|
||||
|
||||
float getNormalized() { return getPosition()/_scale->get(); }
|
||||
const char * getName() { return _name; }
|
||||
const uint8_t & getCh() { return _ch->get(); }
|
||||
const float & getScale() { return _scale->get(); }
|
||||
const float & getCenter() { return _center->get(); }
|
||||
const uint16_t & getPwmMin() { return _pwmMin->get(); }
|
||||
const uint16_t & getPwmNeutral() { return _pwmNeutral->get(); }
|
||||
const uint16_t & getPwmMax() { return _pwmMax->get(); }
|
||||
const uint16_t & getPwmDeadZone() { return _pwmDeadZone->get(); }
|
||||
const bool & getFilter() { return _filter->get(); }
|
||||
const bool & getReverse() { return _reverse->get(); }
|
||||
|
||||
// did our read come in 50µs below the min?
|
||||
bool failSafe() { _pwm < (_pwmMin - 50); }
|
||||
bool failSafe() { _pwm < (_pwmMin->get() - 50); }
|
||||
|
||||
private:
|
||||
|
||||
// createName
|
||||
const char * createName(char * str)
|
||||
{
|
||||
char * newName;
|
||||
strcpy(newName,_name);
|
||||
strcat(newName,"_");
|
||||
strcat(newName,str);
|
||||
return (const char * )newName;
|
||||
}
|
||||
|
||||
// configuration
|
||||
const char * _name;
|
||||
const APM_RC_Class & _rc;
|
||||
const uint16_t _ch;
|
||||
const float & _scale;
|
||||
const uint16_t & _pwmMin;
|
||||
const uint16_t & _pwmNeutral;
|
||||
const uint16_t & _pwmMax;
|
||||
const uint16_t & _pwmDeadZone;
|
||||
const bool & _filter;
|
||||
const bool & _reverse;
|
||||
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;
|
||||
|
||||
// internal states
|
||||
uint16_t _pwm; // this is the internal state, position is just created when needed
|
||||
|
||||
@ -14,111 +14,64 @@
|
||||
FastSerialPort0(Serial); // make sure this procees variable declarations
|
||||
|
||||
// test settings
|
||||
uint8_t nChannels = 8;
|
||||
bool loadEEProm = false;
|
||||
bool saveEEProm = false;
|
||||
uint8_t nChannels = 1;
|
||||
|
||||
// channel configuration
|
||||
Vector< AP_EEPromVar<float> * > scale;
|
||||
Vector< AP_EEPromVar<uint16_t> * > pwmMin;
|
||||
Vector< AP_EEPromVar<uint16_t> * > pwmNeutral;
|
||||
Vector< AP_EEPromVar<uint16_t> * > pwmMax;
|
||||
Vector< AP_EEPromVar<uint16_t> * > pwmDeadZone;
|
||||
Vector< AP_Var<bool> * > filter;
|
||||
Vector< AP_Var<bool> * > reverse;
|
||||
Vector< AP_RcChannel * > rc;
|
||||
|
||||
AP_RcChannel rc[] =
|
||||
{
|
||||
AP_RcChannel("ROLL",APM_RC,0,100.0),
|
||||
/*
|
||||
AP_RcChannel("PITCH",APM_RC,1,45),
|
||||
AP_RcChannel("THR",APM_RC,2,100),
|
||||
AP_RcChannel("YAW",APM_RC,3,45),
|
||||
AP_RcChannel("CH5",APM_RC,4,1),
|
||||
AP_RcChannel("CH6",APM_RC,5,1),
|
||||
AP_RcChannel("CH7",APM_tC,6,1),
|
||||
AP_RcChannel("CH8",APM_RC,7,1)
|
||||
*/
|
||||
};
|
||||
// test position
|
||||
float testPosition = 0;
|
||||
uint16_t testPwm = 1500;
|
||||
int8_t testSign = 1;
|
||||
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(115200);
|
||||
Serial.println("ArduPilot RC Channel test");
|
||||
APM_RC.Init(); // APM Radio initialization
|
||||
|
||||
// add channels
|
||||
for (int i=0;i<nChannels;i++)
|
||||
{
|
||||
char num[5];
|
||||
itoa(i+1,num,10);
|
||||
|
||||
Serial.printf("\nInitializing channel %d", i+1);
|
||||
|
||||
// initialize eeprom settings
|
||||
scale.push_back(new AP_EEPromVar<float>(1.0,strcat("SCALE",num)));
|
||||
pwmMin.push_back(new AP_EEPromVar<uint16_t>(1200,strcat("PWM_MIN",num)));
|
||||
pwmNeutral.push_back(new AP_EEPromVar<uint16_t>(1500,strcat("PWM_NEUTRAL",num)));
|
||||
pwmMax.push_back(new AP_EEPromVar<uint16_t>(1800,strcat("PWM_MAX",num)));
|
||||
pwmDeadZone.push_back(new AP_EEPromVar<uint16_t>(10,strcat("PWM_DEADZONE",num)));
|
||||
filter.push_back(new AP_EEPromVar<bool>(false,strcat("FILTER",num)));
|
||||
reverse.push_back(new AP_EEPromVar<bool>(false,strcat("REVERSE",num)));
|
||||
|
||||
// save
|
||||
if (saveEEProm)
|
||||
{
|
||||
scale[i]->save();
|
||||
pwmMin[i]->save();
|
||||
pwmNeutral[i]->save();
|
||||
pwmMax[i]->save();
|
||||
pwmDeadZone[i]->save();
|
||||
filter[i]->save();
|
||||
reverse[i]->save();
|
||||
}
|
||||
|
||||
// load
|
||||
if (loadEEProm)
|
||||
{
|
||||
scale[i]->load();
|
||||
pwmMin[i]->load();
|
||||
pwmNeutral[i]->load();
|
||||
pwmMax[i]->load();
|
||||
pwmDeadZone[i]->load();
|
||||
filter[i]->load();
|
||||
reverse[i]->load();
|
||||
}
|
||||
|
||||
// find neutral position
|
||||
AP_RcChannel * ch = new AP_RcChannel(APM_RC,i,scale[i]->get(),
|
||||
pwmMin[i]->get(),pwmNeutral[i]->get(),pwmMax[i]->get(),
|
||||
pwmDeadZone[i]->get(),filter[i]->get(),reverse[i]->get());
|
||||
|
||||
ch->readRadio();
|
||||
pwmNeutral[i]->set(ch->getPwm());
|
||||
|
||||
// add rc channel
|
||||
rc.push_back(ch);
|
||||
}
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
// set channel positions
|
||||
Serial.println("In Loop");
|
||||
for (int i=0;i<nChannels;i++) rc[i]->setPosition(testPosition);
|
||||
|
||||
|
||||
for (int i=0;i<nChannels;i++) rc[i].setPosition(testPosition);
|
||||
Serial.printf("\ntestPosition (%f)\n",testPosition);
|
||||
for (int i=0;i<nChannels;i++) Serial.printf("%7s\t",rc[i].getName());
|
||||
Serial.println();
|
||||
Serial.printf("pwm :\t");
|
||||
for (int i=0;i<nChannels;i++) Serial.printf("%7d\t",rc[i]->getPwm());
|
||||
for (int i=0;i<nChannels;i++) Serial.printf("%7d\t",rc[i].getPwm());
|
||||
Serial.println();
|
||||
Serial.printf("position :\t");
|
||||
for (int i=0;i<nChannels;i++) Serial.printf("%7.2f\t",rc[i]->getPosition());
|
||||
for (int i=0;i<nChannels;i++) Serial.printf("%7.2f\t",rc[i].getPosition());
|
||||
Serial.println();
|
||||
|
||||
// update test value
|
||||
testPosition += testSign*.05;
|
||||
if (testPosition > 1)
|
||||
for (int i=0;i<nChannels;i++)
|
||||
{
|
||||
testPosition = 1;
|
||||
testSign = -1;
|
||||
}
|
||||
else if (testPosition < -1)
|
||||
{
|
||||
testPosition = -1;
|
||||
testSign = 1;
|
||||
testPosition += testSign*.05;
|
||||
if (testPosition > 1)
|
||||
{
|
||||
testPosition = 1;
|
||||
testSign = -1;
|
||||
}
|
||||
else if (testPosition < -1)
|
||||
{
|
||||
testPosition = -1;
|
||||
testSign = 1;
|
||||
}
|
||||
}
|
||||
|
||||
delay(500);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user