RcChannel now owns channel config params.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1354 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/AP_Common/AP_Loop.h

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

libraries/AP_Common/AP_Var.h

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

libraries/AP_EEProm/AP_EEProm.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

libraries/AP_RcChannel/AP_RcChannel.cpp

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

libraries/AP_RcChannel/AP_RcChannel.h

@@ -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);
-
-	// get servo state
+	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; }
+	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

libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde

@@ -14,101 +14,52 @@
 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
+	for (int i=0;i<nChannels;i++)
+	{
 		testPosition += testSign*.05;
 		if (testPosition > 1)
 		{
@@ -120,5 +71,7 @@ void loop()
 			testPosition = -1;
 			testSign = 1;
 		}
+	}
+
 	delay(500);
 }