Updated AP_RcChannel for AP_Var.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1509 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert 2011-01-18 04:56:45 +00:00
parent f60602d7a9
commit 8ec3a8de4a
5 changed files with 62 additions and 74 deletions

View File

@ -320,9 +320,16 @@ protected:
///
#define AP_VARDEF(_t, _n) typedef AP_VarT<_t> AP_##_n;
AP_VARDEF(float, Float); // defines AP_Float
AP_VARDEF(int8_t, Int8); // defines AP_UInt8
AP_VARDEF(int16_t, Int16); // defines AP_UInt16
AP_VARDEF(int32_t, Int32); // defines AP_UInt32
AP_VARDEF(int8_t, Int8); // defines AP_Int8
AP_VARDEF(int16_t, Int16); // defines AP_Int16
AP_VARDEF(int32_t, Int32); // defines AP_Int32
/// Rely on built in casting for other variable types
/// to minimize template creation and save memory
#define AP_Uint8 AP_Int8
#define AP_Uint16 AP_Int16
#define AP_Uint32 AP_Int32
#define AP_Bool AP_Int8
/// Many float values can be saved as 16-bit fixed-point values, reducing EEPROM
/// consumption. AP_Float16 subclasses AP_Float and overloads serialize/unserialize

View File

@ -14,31 +14,31 @@
#include "AP_RcChannel.h"
#include <AP_Common.h>
AP_RcChannel::AP_RcChannel(const char * name, const APM_RC_Class & rc, const uint8_t & ch,
AP_RcChannel::AP_RcChannel(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) :
_name(name),
AP_Var_scope(name),
_rc(rc),
_ch(new AP_EEPROM_Uint8(ch,"CH",name)),
_scale(new AP_EEPROM_Float(scale,"SCALE",name)),
_center(new AP_EEPROM_Float(center,"CNTR",name)),
_pwmMin(new AP_EEPROM_Uint16(pwmMin,"PMIN",name)),
_pwmMax(new AP_EEPROM_Uint16(pwmMax,"PMAX",name)),
_pwmNeutral(new AP_EEPROM_Uint16(pwmNeutral,"PNTRL",name)),
_pwmDeadZone(new AP_EEPROM_Uint16(pwmDeadZone,"PDEAD",name)),
_pwm(0),
_filter(new AP_EEPROM_Bool(filter,"FLTR",name)),
_reverse(new AP_EEPROM_Bool(reverse,"REV",name))
ch(ch,AP_Var::k_no_address,PSTR("CH"),this),
scale(scale,AP_Var::k_no_address,PSTR("SCALE"),this),
center(center,AP_Var::k_no_address,PSTR("CNTR"),this),
pwmMin(pwmMin,AP_Var::k_no_address,PSTR("PMIN"),this),
pwmMax(pwmMax,AP_Var::k_no_address,PSTR("PMAX"),this),
pwmNeutral(pwmNeutral,AP_Var::k_no_address,PSTR("PNTRL"),this),
pwmDeadZone(pwmDeadZone,AP_Var::k_no_address,PSTR("PDEAD"),this),
filter(filter,AP_Var::k_no_address,PSTR("FLTR"),this),
reverse(reverse,AP_Var::k_no_address,PSTR("REV"),this),
_pwm(0)
{
}
void AP_RcChannel::readRadio() {
// apply reverse
uint16_t pwmRadio = APM_RC.InputCh(getCh());
uint16_t pwmRadio = _rc.InputCh(ch);
setPwm(pwmRadio);
}
@ -46,15 +46,15 @@ void
AP_RcChannel::setPwm(uint16_t pwm)
{
//Serial.printf("pwm in setPwm: %d\n", pwm);
//Serial.printf("reverse: %s\n", (getReverse())?"true":"false");
//Serial.printf("reverse: %s\n", (reverse)?"true":"false");
// apply reverse
if(getReverse()) pwm = int16_t(getPwmNeutral()-pwm) + getPwmNeutral();
if(reverse) pwm = int16_t(pwmNeutral-pwm) + pwmNeutral;
//Serial.printf("pwm after reverse: %d\n", pwm);
// apply filter
if(getFilter()){
if(filter){
if(_pwm == 0)
_pwm = pwm;
else
@ -66,10 +66,10 @@ AP_RcChannel::setPwm(uint16_t pwm)
//Serial.printf("pwm after filter: %d\n", _pwm);
// apply deadzone
_pwm = (abs(_pwm - getPwmNeutral()) < getPwmDeadZone()) ? getPwmNeutral() : _pwm;
_pwm = (abs(_pwm - pwmNeutral) < pwmDeadZone) ? uint16_t(pwmNeutral) : _pwm;
//Serial.printf("pwm after deadzone: %d\n", _pwm);
APM_RC.OutputCh(getCh(),_pwm);
_rc.OutputCh(ch,_pwm);
}
void
@ -81,8 +81,8 @@ AP_RcChannel::setPosition(float position)
void
AP_RcChannel::mixRadio(uint16_t infStart)
{
uint16_t pwmRadio = APM_RC.InputCh(getCh());
float inf = abs( int16_t(pwmRadio - getPwmNeutral()) );
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);
@ -93,14 +93,14 @@ AP_RcChannel::_positionToPwm(const float & position)
{
uint16_t pwm;
//Serial.printf("position: %f\n", position);
float p = position - getCenter();
float p = position - center;
if(p < 0)
pwm = p * int16_t(getPwmNeutral() - getPwmMin()) /
getScale() + getPwmNeutral();
pwm = p * int16_t(pwmNeutral - pwmMin) /
scale + pwmNeutral;
else
pwm = p * int16_t(getPwmMax() - getPwmNeutral()) /
getScale() + getPwmNeutral();
constrain(pwm,getPwmMin(),getPwmMax());
pwm = p * int16_t(pwmMax - pwmNeutral) /
scale + pwmNeutral;
constrain(pwm,uint16_t(pwmMin),uint16_t(pwmMax));
return pwm;
}
@ -108,14 +108,13 @@ float
AP_RcChannel::_pwmToPosition(const uint16_t & pwm)
{
float position;
if(pwm < getPwmNeutral())
position = getScale() * int16_t(pwm - getPwmNeutral())/
int16_t(getPwmNeutral() - getPwmMin()) + getCenter();
if(pwm < pwmNeutral)
position = scale * int16_t(pwm - pwmNeutral)/
int16_t(pwmNeutral - pwmMin) + center;
else
position = getScale() * int16_t(pwm -getPwmNeutral())/
int16_t(getPwmMax() - getPwmNeutral()) + getCenter();
constrain(position,-getScale()+getCenter(),
getScale()+getCenter());
position = scale * int16_t(pwm - pwmNeutral)/
int16_t(pwmMax - pwmNeutral) + center;
constrain(position,center-scale,center+scale);
return position;
}

View File

@ -10,70 +10,54 @@
#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
class AP_RcChannel{
class AP_RcChannel : public AP_Var_scope {
public:
/// Constructor
AP_RcChannel(const char * name, const APM_RC_Class & rc, const uint8_t & ch,
AP_RcChannel(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) { setPosition(normPosition*getScale()); }
void setNormalized(float normPosition) { setPosition(normPosition*scale); }
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
uint16_t getPwm() { return _pwm; }
float getPosition() { return _pwmToPosition(_pwm); }
float getNormalized() { return getPosition()/_scale->get(); }
float getNormalized() { return getPosition()/scale; }
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->get() - 50); }
bool failSafe() { _pwm < (pwmMin - 50); }
private:
// configuration
const char * _name;
const APM_RC_Class & _rc;
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;
APM_RC_Class & _rc;
// internal states
uint16_t _pwm; // this is the internal state, position is just created when needed

View File

@ -8,7 +8,6 @@
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_RcChannel.h> // ArduPilot Mega RC Library
#include <AP_EEProm.h>
#include <APM_RC.h>
FastSerialPort0(Serial); // make sure this procees variable declarations
@ -43,7 +42,7 @@ void setup()
APM_RC.Init(); // APM Radio initialization
delay(2000);
eepromRegistry.print(Serial); // show eeprom map
//eepromRegistry.print(Serial); // show eeprom map
}
void loop()

View File

@ -8,7 +8,6 @@
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_RcChannel.h> // ArduPilot Mega RC Library
#include <AP_EEProm.h>
#include <APM_RC.h>
FastSerialPort0(Serial); // make sure this procees variable declarations
@ -51,7 +50,7 @@ void loop()
testPosition += testSign*.1;
if (testPosition > 1)
{
eepromRegistry.print(Serial); // show eeprom map
//eepromRegistry.print(Serial); // show eeprom map
testPosition = 1;
testSign = -1;
}