update use EEPROM class, new scaled input for RC_Channel

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

libraries/PID/PID.cpp

@@ -72,16 +72,23 @@ PID::load_gains()
 			// load is a NOP if the gain array is managed externally
 			break;
 		case STORE_EEPROM_UINT16:
-			_kp 	= (float)(eeprom_read_word((uint16_t *)	 _address))      / 1000.0;
-			_ki 	= (float)(eeprom_read_word((uint16_t *)	(_address + 2))) / 1000.0;
-			_kd 	= (float)(eeprom_read_word((uint16_t *)	(_address + 4))) / 1000.0;
-			_imax 	= eeprom_read_word((uint16_t *)	(_address + 6)) * 100;
+		//	_kp 	= (float)(eeprom_read_word((uint16_t *)	 _address))      / 1000.0;
+		//	_ki 	= (float)(eeprom_read_word((uint16_t *)	(_address + 2))) / 1000.0;
+			//_kd 	= (float)(eeprom_read_word((uint16_t *)	(_address + 4))) / 1000.0;
+			//_imax 	= eeprom_read_word((uint16_t *)	(_address + 6)) * 100;
+			
+			_kp 	= (float)_ee.read_int(_address) / 1000.0;
+			_ki 	= (float)_ee.read_int(_address + 2) / 1000.0;
+			_kd 	= (float)_ee.read_int(_address + 4) / 1000.0;
+			_imax 	= (float)_ee.read_int(_address + 6) * 100;
+
 			break;
+			
 		case STORE_EEPROM_FLOAT:
-			eeprom_read_block((void*)&_kp,(const void*)(_address),sizeof(_kp));
-			eeprom_read_block((void*)&_ki,(const void*)(_address+4),sizeof(_ki));
-			eeprom_read_block((void*)&_kd,(const void*)(_address+8),sizeof(_kd));
-			eeprom_read_block((void*)&_imax,(const void*)(_address+12),sizeof(_imax));
+			//eeprom_read_block((void*)&_kp,(const void*)(_address),sizeof(_kp));
+			//eeprom_read_block((void*)&_ki,(const void*)(_address+4),sizeof(_ki));
+			//eeprom_read_block((void*)&_kd,(const void*)(_address+8),sizeof(_kd));
+			//eeprom_read_block((void*)&_imax,(const void*)(_address+12),sizeof(_imax));
 			break;
 	}
 }
@@ -95,16 +102,22 @@ PID::save_gains()
 			// save is a NOP if the gain array is managed externally
 			break;
 		case STORE_EEPROM_UINT16:
+			/*
 			eeprom_write_word((uint16_t *)	 _address, 		(int)(_kp * 1000));
 			eeprom_write_word((uint16_t *)	(_address + 2), (int)(_ki * 1000));
 			eeprom_write_word((uint16_t *)	(_address + 4), (int)(_kd * 1000));
 			eeprom_write_word((uint16_t *)	(_address + 6), (int)_imax/100);
+			*/
+			_ee.write_int(_address, 	(int)(_kp * 1000));
+			_ee.write_int(_address + 2, (int)(_ki * 1000));
+			_ee.write_int(_address + 4, (int)(_kd * 1000));
+			_ee.write_int(_address + 6, (int)(_imax /100));
 			break;
 		case STORE_EEPROM_FLOAT:
-			eeprom_write_block((const void *)&_kp,(void *)(_address),sizeof(_kp));
-			eeprom_write_block((const void *)&_ki,(void *)(_address+4),sizeof(_ki));
-			eeprom_write_block((const void *)&_kd,(void *)(_address+8),sizeof(_kd));
-			eeprom_write_block((const void *)&_imax,(void *)(_address+12),sizeof(_imax));
+			//eeprom_write_block((const void *)&_kp,(void *)(_address),sizeof(_kp));
+			//eeprom_write_block((const void *)&_ki,(void *)(_address+4),sizeof(_ki));
+			//eeprom_write_block((const void *)&_kd,(void *)(_address+8),sizeof(_kd));
+			//eeprom_write_block((const void *)&_imax,(void *)(_address+12),sizeof(_imax));
 			break;
 	}
 }

libraries/PID/PID.h

@@ -7,6 +7,7 @@
 #define PID_h
 
 #include <stdint.h>
+#include <AP_EEPROMB.h>
 
 /// @class	PID
 /// @brief	Object managing one PID control
@@ -57,6 +58,7 @@ public:
 	///						array is used as kP, kI, kD and imax
 	///						respectively.
 	///
+	
 	PID(float *gain_array) :
 		_storage(STORE_OFF),
 		_gain_array(gain_array)
@@ -75,7 +77,7 @@ public:
 	/// @param scaler	An arbitrary scale factor
 	///
 	/// @returns		The updated control output.
-	///
+	///				
 	long 	get_pid(int32_t error, uint16_t dt, float scaler = 1.0);
 
 	/// Reset the PID integrator
@@ -105,15 +107,18 @@ public:
 	void	kI(const float v)		{ _gain_array[1] = v; }
 	void	kD(const float v)		{ _gain_array[2] = v; }
 	void	imax(const float v);
-
+	
+	void	address(const uint16_t v)	{ _address = v; }
+	
 	// one-shot operator for setting all of the gain properties at once
-	void operator ()(const float p, const float i, const float d, const float max)
-	{ kP(p); kI(i); kD(d); imax(max); }
+	//void operator ()(const float p, const float i, const float d, const float max)
+	//{ kP(p); kI(i); kD(d); imax(max); }
 	//@}
 	
 	float	get_integrator() 		{ return _integrator; }
 
 private:
+	AP_EEPROMB	_ee;
 	uint16_t	_address;			///< EEPROM address for save/restore of P/I/D
 	float		*_gain_array; 		///< pointer to the gains for this pid
 

libraries/PID/examples/pid/pid.pde

@@ -6,10 +6,11 @@
 #include <PID.h> // ArduPilot Mega RC Library
 #include <APM_RC.h> // ArduPilot RC Library
 
-PID pid;
 long radio_in;
 long radio_trim;
 
+PID pid(0x16);
+
 void setup()
 {
 	Serial.begin(38400);

libraries/RC_Channel/RC_Channel.cpp

@@ -73,9 +73,16 @@ RC_Channel::set_pwm(int pwm)
 		//Serial.print("range ");
 		control_in = pwm_to_range();
 		control_in = (control_in < dead_zone) ? 0 : control_in;
+		if(scale_output){
+			control_in *= scale_output;
+		}
+		
 	}else{
 		control_in = pwm_to_angle();
 		control_in = (abs(control_in) < dead_zone) ? 0 : control_in;
+		if(scale_output){
+			control_in *= scale_output;
+		}
 	}
 }
 
@@ -102,7 +109,11 @@ RC_Channel::calc_pwm(void)
 	}else{
 		pwm_out = angle_to_pwm();
 	}
+	//if(scale_output){
+	//	pwm_out *= scale_output;
+	//}
 	radio_out = pwm_out + radio_min;
+	radio_out = constrain(radio_out,radio_min, radio_max);
 }
 
 // ------------------------------------------
@@ -110,28 +121,42 @@ RC_Channel::calc_pwm(void)
 void
 RC_Channel::load_eeprom(void)
 {
-	radio_min 	= eeprom_read_word((uint16_t *)	_address);
-	radio_max	= eeprom_read_word((uint16_t *)	(_address + 2));
-	radio_trim 	= eeprom_read_word((uint16_t *)	(_address + 4));
+	//radio_min 	= eeprom_read_word((uint16_t *)	_address);
+	//radio_max	= eeprom_read_word((uint16_t *)	(_address + 2));
+	//radio_trim 	= eeprom_read_word((uint16_t *)	(_address + 4));
+	radio_min 	= _ee.read_int(_address);
+	radio_max	= _ee.read_int(_address + 2);
+	radio_trim 	= _ee.read_int(_address + 4);
 }
 
 void
 RC_Channel::save_eeprom(void)
 {
-	eeprom_write_word((uint16_t *)	_address, 			radio_min);
-	eeprom_write_word((uint16_t *)	(_address + 2), 	radio_max);
-	eeprom_write_word((uint16_t *)	(_address + 4), 	radio_trim);
+	//eeprom_write_word((uint16_t *)	_address, 			radio_min);
+	//eeprom_write_word((uint16_t *)	(_address + 2), 	radio_max);
+	//eeprom_write_word((uint16_t *)	(_address + 4), 	radio_trim);
+	
+	_ee.write_int(_address, 		radio_min);
+	_ee.write_int((_address + 2), 	radio_max);
+	_ee.write_int((_address + 4), 	radio_trim);
 }
 
 // ------------------------------------------
 void
 RC_Channel::save_trim(void)
 {
-	eeprom_write_word((uint16_t *)	(_address + 4), 	radio_trim);
+	//eeprom_write_word((uint16_t *)	(_address + 4), 	radio_trim);
+	_ee.write_int((_address + 4), 	radio_trim);
 }
 
 // ------------------------------------------
 
+void
+RC_Channel::zero_min_max()
+{
+	radio_min = radio_min = radio_in;
+}
+
 void
 RC_Channel::update_min_max()
 {
@@ -141,13 +166,16 @@ RC_Channel::update_min_max()
 
 // ------------------------------------------
 
-int16_t 
+int16_t
 RC_Channel::pwm_to_angle()
 {
 	if(radio_in < radio_trim)
-		return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_trim - radio_min));
+		return _reverse * ((long)_high * (long)(radio_in - radio_trim)) / (long)(radio_trim - radio_min);
 	else
-		return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_max  - radio_trim));
+		return _reverse * ((long)_high * (long)(radio_in - radio_trim)) / (long)(radio_max  - radio_trim);
+		
+		//return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_max  - radio_trim));
+		//return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_trim - radio_min));
 }
 
 float 
@@ -172,9 +200,12 @@ int16_t
 RC_Channel::angle_to_pwm()
 {
 	if(servo_out < 0)
-		return (((float)servo_out / (float)_high) * (float)(radio_max - radio_trim));
+		return ((long)servo_out * (long)(radio_max - radio_trim)) / (long)_high;
 	else
-		return (((float)servo_out / (float)_high) * (float)(radio_trim - radio_min));
+		return ((long)servo_out * (long)(radio_trim - radio_min)) / (long)_high;
+
+		//return (((float)servo_out / (float)_high) * (float)(radio_max - radio_trim));
+		//return (((float)servo_out / (float)_high) * (float)(radio_trim - radio_min));
 }
 
 // ------------------------------------------
@@ -182,12 +213,16 @@ RC_Channel::angle_to_pwm()
 int16_t
 RC_Channel::pwm_to_range()
 {
-	return _reverse * (_low + ((_high - _low) * ((float)(radio_in - radio_min) / (float)(radio_max - radio_min))));
+	//return (_low + ((_high - _low) * ((float)(radio_in - radio_min) / (float)(radio_max - radio_min))));
+	return (_low + ((long)(_high - _low) * (long)(radio_in - radio_min)) / (long)(radio_max - radio_min));
 }
 
 int16_t
 RC_Channel::range_to_pwm()
 {
-	return (((float)servo_out / (float)(_high - _low)) * (float)(radio_max - radio_min));
+	//return (((float)(servo_out - _low) / (float)(_high - _low)) * (float)(radio_max - radio_min));
+	return ((long)(servo_out - _low) * (long)(radio_max - radio_min)) / (long)(_high - _low);
 }
 
+
+

libraries/RC_Channel/RC_Channel.h

@@ -7,6 +7,7 @@
 #define RC_Channel_h
 
 #include <stdint.h>
+#include <AP_EEPROMB.h>
 
 /// @class	RC_Channel
 /// @brief	Object managing one RC channel
@@ -29,11 +30,13 @@ class RC_Channel{
 	RC_Channel(uint16_t address) :
 		_address(address),
 		_high(1),
-		_filter(true)
+		_filter(true),
+		_reverse(1)
 	{}
 
 	// setup min and max radio values in CLI
 	void 		update_min_max();
+	void 		zero_min_max();
 	
 	// startup
 	void 		load_eeprom(void);
@@ -87,6 +90,8 @@ class RC_Channel{
 	int16_t		angle_to_pwm();
 	int16_t		pwm_to_range();
 	int16_t		range_to_pwm();
+
+	float		scale_output;
 	
   private:
 	bool		_filter;
@@ -96,6 +101,7 @@ class RC_Channel{
 	bool 		_type;				
 	int16_t 	_high;
 	int16_t 	_low;
+	AP_EEPROMB	_ee;
 };
 
 #endif