Fixed RC_ChannelB errors.

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

libraries/AP_Common/AP_Common.h

@@ -95,7 +95,7 @@ public:
 	/**
 	 * The default constrcutor
 	 */
-	AP_Var(type data, const char * name = "", bool sync=false) : 
+	AP_Var(const type & data, const char * name = "", const bool & sync=false) : 
 		_data(data), _name(name), _sync(sync)
 	{
 	}
@@ -103,7 +103,7 @@ public:
 	/**
 	 * Set the variable value
 	 */
-	void set(type val) {
+	void set(const type & val) {
 		_data = val; 
 		if (_sync) save();
 	}
@@ -111,7 +111,7 @@ public:
 	/**
 	 * Get the variable value.
 	 */
-	type get() {
+	const type & get() {
 		if (_sync) load();
 		return _data;
 	}
@@ -119,15 +119,15 @@ public:
 	/**
 	 *  Set the variable value as a float
 	 */
-	void setAsFloat(float val) {
-		set((type)val);
+	void setAsFloat(const float & val) {
+		set(val);
 	}
 
 	/**
 	 * Get the variable as a float
 	 */
-	float getAsFloat() {
-		return (float)get();
+	const float & getAsFloat() {
+		return get();
 	}
 
 
@@ -154,7 +154,7 @@ public:
 	 * If sync is true the a load will always occure before a get and a save will always
 	 * occure before a set.
 	 */
-	bool getSync() { return _sync; }
+	const bool & getSync() { return _sync; }
 	void setSync(bool sync) { _sync = sync; }
 
 protected:

libraries/RC_ChannelB/RC_ChannelB.cpp

@@ -25,8 +25,12 @@ void RC_ChannelB::readRadio(uint16_t pwmRadio) {
 void
 RC_ChannelB::setPwm(uint16_t pwm)
 {
+	//Serial.printf("reverse: %s\n", (_reverse)?"true":"false");
+
 	// apply reverse
-	if(_reverse) pwm = (_pwmNeutral-pwm) + _pwmNeutral;
+	if(_reverse) pwm = int16_t(_pwmNeutral-pwm) + _pwmNeutral;
+
+	//Serial.printf("pwm after reverse: %d\n", pwm);
 
 	// apply filter
 	if(_filter){
@@ -38,8 +42,12 @@ RC_ChannelB::setPwm(uint16_t pwm)
 		_pwm = pwm;
 	}
 
+	//Serial.printf("pwm after filter: %d\n", _pwm);
+
 	// apply deadzone
 	_pwm = (abs(_pwm - _pwmNeutral) < _pwmDeadZone) ? _pwmNeutral : _pwm;
+
+	//Serial.printf("pwm after deadzone: %d\n", _pwm);
 }
 
 void
@@ -51,28 +59,35 @@ RC_ChannelB::setPosition(float position)
 void
 RC_ChannelB::mixRadio(uint16_t infStart)
 {
-	float inf = abs(_pwmRadio - _pwmNeutral);
+	float inf = abs( int16_t(_pwmRadio - _pwmNeutral) );
 	inf = min(inf, infStart);
 	inf = ((infStart - inf) /infStart);
 	setPwm(_pwm*inf + _pwmRadio); 
 }
 
 uint16_t
-RC_ChannelB::_positionToPwm(float position)
+RC_ChannelB::_positionToPwm(const float & position)
 {
+	uint16_t pwm;
+	//Serial.printf("position: %f\n", position);
 	if(position < 0)
-		return (position / _scale) * (_pwmMin - _pwmNeutral);
+		pwm = position * int16_t(_pwmNeutral - _pwmMin) / _scale + _pwmNeutral;
 	else
-		return (position / _scale) * (_pwmMax - _pwmNeutral);
+		pwm = position * int16_t(_pwmMax - _pwmNeutral) / _scale + _pwmNeutral;
+	constrain(pwm,_pwmMin,_pwmMax);
+	return pwm;
 }
 
 float
-RC_ChannelB::_pwmToPosition(uint16_t pwm)
+RC_ChannelB::_pwmToPosition(const uint16_t & pwm)
 {
-	if(_pwm < _pwmNeutral)
-		return _scale * (_pwm - _pwmNeutral)/(_pwmNeutral - _pwmMin);
+	float position;
+	if(pwm < _pwmNeutral)
+		position = _scale * int16_t(pwm - _pwmNeutral)/ int16_t(_pwmNeutral - _pwmMin);
 	else
-		return _scale * (_pwm - _pwmNeutral)/(_pwmMax - _pwmNeutral);
+		position = _scale * int16_t(pwm - _pwmNeutral)/ int16_t(_pwmMax - _pwmNeutral);
+	constrain(position,-_scale,_scale);
+	return position;
 }
 
 // ------------------------------------------

libraries/RC_ChannelB/RC_ChannelB.h

@@ -1,7 +1,7 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 
 /// @file	RC_ChannelB.h
-/// @brief	RC_ChannelB manager, with EEPROM-backed storage of constants.
+/// @brief	RC_ChannelB manager
 
 #ifndef RC_ChannelB_h
 #define RC_ChannelB_h
@@ -57,15 +57,15 @@ private:
 	const uint16_t & _pwmMax;
 	const uint16_t & _pwmDeadZone;
 	const bool & _filter;
-	const int8_t & _reverse;
+	const bool & _reverse;
 
 	// internal states
 	uint16_t _pwm; // this is the internal state, positino is just created when needed
 	uint16_t _pwmRadio; // radio pwm input
 
 	// private methods
-	uint16_t _positionToPwm(float position);
-	float _pwmToPosition(uint16_t pwm);
+	uint16_t _positionToPwm(const float & position);
+	float _pwmToPosition(const uint16_t & pwm);
 };
 
 #endif	

libraries/RC_ChannelB/examples/RC_ChannelB/RC_ChannelB.pde

@@ -20,8 +20,8 @@ AP_EEPromVar<uint16_t> pwmDeadZone(100,"RC1_PWMDEADZONE");
 #define CH_1 0
 
 // configuration
-AP_Var<bool> filter(true,"RC1_FILTER");
-AP_Var<int8_t> reverse(false,"RC1_REVERSE");
+AP_Var<bool> filter(false,"RC1_FILTER");
+AP_Var<bool> reverse(false,"RC1_REVERSE");
 
 FastSerialPort0(Serial);
 
@@ -39,26 +39,34 @@ void setup()
 	APM_RC.Init();		// APM Radio initialization
 
 	delay(1000);
-	// setup radio 
+
+	// configuratoin
+	scale.set(100);
+	Serial.printf("\nscale.set(100)\n");
+	delay(2000);
+
+	// find neutral radio position
+	rc[CH_1].readRadio(APM_RC.InputCh(CH_1));
+	Serial.printf("\nrc[CH_1].readRadio(APM_RC.InputCh(CH_1))\n");
+	Serial.printf("\npwmNeutral.set(rc[CH_1].getPwm())\n");
+	pwmNeutral.set(rc[CH_1].getPwm());
+	delay(2000);
 }
 
 void loop()	
 {
-	rc[CH_1].readRadio(APM_RC.InputCh(CH_1));
-	Serial.printf("\nrc[CH_1].readRadio(APM_RC.InputCh(CH_1))\n");
-	Serial.printf("pwm: %d position: %f\n",rc[CH_1].getPwm(), rc[CH_1].getPosition());
+	// get the min pwm
+	Serial.printf("\npwmMin.get(): %d\n", pwmMin.get());
 	delay(2000);
 
-	scale.set(100);
-	Serial.printf("\nscale.set(100)\n");
-
-
+	// set by pwm 
 	rc[CH_1].setPwm(1500);
 	Serial.printf("\nrc[CH_1].setPwm(1500)\n");
 	Serial.printf("pwm: %d position: %f\n",rc[CH_1].getPwm(),
 			rc[CH_1].getPosition());
 	delay(2000);
 
+	// set by position
 	rc[CH_1].setPosition(-50);
 	Serial.printf("\nrc[CH_1].setPosition(-50))\n");
 	Serial.printf("pwm: %d position: %f\n",rc[CH_1].getPwm(),