added filter option

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

libraries/RC_Channel/RC_Channel.cpp

@@ -9,15 +9,14 @@
 
 */
 
+#include <math.h>
+#include <avr/eeprom.h>
+#include "WProgram.h"
 #include "RC_Channel.h"
 
 #define ANGLE 0
 #define RANGE 1
 
-RC_Channel::RC_Channel() : _high(1)
-{
-}
-
 // setup the control preferences
 void 	
 RC_Channel::set_range(int low, int high)
@@ -34,6 +33,12 @@ RC_Channel::set_angle(int angle)
 	_high 	= angle;
 }
 
+void
+RC_Channel::set_filter(bool filter)
+{
+	_filter = filter;
+}
+
 // call after first read
 void
 RC_Channel::trim()
@@ -48,11 +53,15 @@ RC_Channel::set_pwm(int pwm)
 {
 	//Serial.print(pwm,DEC);
 
-	if(radio_in == 0)
+	if(_filter){
+		if(radio_in == 0)
+			radio_in = pwm;
+		else
+			radio_in = ((pwm + radio_in) >> 1);		// Small filtering
+	}else{
 		radio_in = pwm;
-	else
+	}
-		radio_in = ((pwm + radio_in) >> 1);		// Small filtering
+	
-
 	if(_type == RANGE){
 		//Serial.print("range ");
 		control_in = pwm_to_range();
@@ -70,7 +79,7 @@ RC_Channel::control_mix(float value)
 }
 
 // are we below a threshold?
-boolean
+bool
 RC_Channel::get_failsafe(void)
 {
 	return (radio_in < (radio_min - 50));
@@ -90,28 +99,30 @@ RC_Channel::calc_pwm(void)
 }
 
 void
-RC_Channel::load_eeprom(int address)
+RC_Channel::load_eeprom(void)
 {
-	radio_min 	= eeprom_read_word((uint16_t *)	address);
+	radio_min 	= eeprom_read_word((uint16_t *)	_address);
-	radio_max	= eeprom_read_word((uint16_t *)	(address + 2));
+	radio_max	= eeprom_read_word((uint16_t *)	(_address + 2));
-	radio_trim 	= eeprom_read_word((uint16_t *)	(address + 4));
+	radio_trim 	= eeprom_read_word((uint16_t *)	(_address + 4));
 }
 
 void
-RC_Channel::save_eeprom(int address)
+RC_Channel::save_eeprom(void)
 {
-	eeprom_write_word((uint16_t *)	address, 		radio_min);
+	eeprom_write_word((uint16_t *)	_address, 		radio_min);
-	eeprom_write_word((uint16_t *)	(address + 2), 	radio_max);
+	eeprom_write_word((uint16_t *)	(_address + 2), 	radio_max);
-	eeprom_write_word((uint16_t *)	(address + 4), 	radio_trim);
+	eeprom_write_word((uint16_t *)	(_address + 4), 	radio_trim);
 }
 
 // ------------------------------------------
+
 void
 RC_Channel::update_min_max()
 {
 	radio_min = min(radio_min, radio_in);
-	radio_max = min(radio_max, radio_in);
+	radio_max = max(radio_max, radio_in);
 }
+
 // ------------------------------------------
 
 int16_t 

libraries/RC_Channel/RC_Channel.h

@@ -1,27 +1,44 @@
-/*
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
-	RC_Channel library.
+
-	Code by Jason Short. 2010 
+/// @file	RC_Channel.h
-	DIYDrones.com
+/// @brief	RC_Channel manager, with EEPROM-backed storage of constants.
-*/
 
 #ifndef RC_Channel_h
 #define RC_Channel_h
 
-#include <inttypes.h>
+#include <stdint.h>
-#include <avr/eeprom.h>
-#include "WProgram.h"
 
-class RC_Channel
+/// @class	RC_Channel
-{
+/// @brief	Object managing one RC channel
-  public:
+class RC_Channel{
-	RC_Channel();
+  public:	
-	
+	/// Constructor
-	// setup in CLI
+	///
+	/// A RC_Channel constructed in this fashion does not support save/restore.
+	///
+	RC_Channel() :
+		_address(0)
+	{}
+
+	/// Constructor
+	///
+	/// @param address	EEPROM base address at which RC_Channel parameters
+	///					are stored.  Zero if the RC_Channel does not support
+	///					save/restore.
+	///
+	RC_Channel(uint16_t address) :
+		_address(address),
+		_high(1),
+		_filter(true)
+	{}
+
+	// setup min and max radio values in CLI
 	void 		update_min_max();
 	
 	// startup
-	void 		load_eeprom(int address);
+	void 		load_eeprom(void);
-	void 		save_eeprom(int address);		
+	void 		save_eeprom(void);		
+	void		set_filter(bool filter);
 
 	// setup the control preferences
 	void 		set_range(int low, int high);
@@ -37,7 +54,7 @@ class RC_Channel
 	void 		trim();
 	
 	// did our read come in 50µs below the min?
-	boolean		get_failsafe(void);
+	bool		get_failsafe(void);
 	
 	// value generated from PWM
 	int16_t 	control_in;
@@ -63,17 +80,14 @@ class RC_Channel
 	//int16_t 	get_radio_out(void);
 	
   private:
-	int16_t		channel_filter();
+	bool		_filter;
 	int16_t		pwm_to_angle();
 	int16_t		angle_to_pwm();
 	int16_t		pwm_to_range();
 	int16_t		range_to_pwm();
 
-	byte 		_channel;
+	int16_t		_address;			///< EEPROM address for save/restore of P/I/D
-	boolean 	_type;
+	bool 		_type;				
-	boolean 	_filter;
-
-		
 	int16_t 	_high;
 	int16_t 	_low;
 };

libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde

@@ -8,15 +8,34 @@
 #include <APM_RC.h> 		// ArduPilot Mega RC Library
 #include <RC_Channel.h> 	// ArduPilot Mega RC Library
 
-RC_Channel rc_1;
+
-RC_Channel rc_2;
+#define EE_RADIO_1 0x00	// all gains stored from here
-RC_Channel rc_3;
+#define EE_RADIO_2 0x06	// all gains stored from here
-RC_Channel rc_4;
+#define EE_RADIO_3 0x0C	// all gains stored from here
+#define EE_RADIO_4 0x12	// all gains stored from here
+#define EE_RADIO_5 0x18	// all gains stored from here
+#define EE_RADIO_6 0x1E	// all gains stored from here
+#define EE_RADIO_7 0x24	// all gains stored from here
+#define EE_RADIO_8 0x2A	// all gains stored from here
+
+
+RC_Channel rc_1(EE_RADIO_1);
+RC_Channel rc_2(EE_RADIO_2);
+RC_Channel rc_3(EE_RADIO_3);
+RC_Channel rc_4(EE_RADIO_4);
+RC_Channel rc_5(EE_RADIO_5);
+RC_Channel rc_6(EE_RADIO_6);
+RC_Channel rc_7(EE_RADIO_7);
+RC_Channel rc_8(EE_RADIO_8);
 
 #define CH_1 0
 #define CH_2 1
 #define CH_3 2
 #define CH_4 3
+#define CH_5 4
+#define CH_6 5
+#define CH_7 6
+#define CH_8 7
 
 void setup()
 {
@@ -28,6 +47,7 @@ void setup()
 	// setup radio
 	
 	// read eepom or set manually
+	/*
 	rc_1.radio_min = 1100;
 	rc_1.radio_max = 1900;
 	
@@ -39,23 +59,40 @@ void setup()
 
 	rc_4.radio_min = 1100;
 	rc_4.radio_max = 1900;
+	*/
+
+
+	rc_1.load_eeprom();
+	rc_2.load_eeprom();
+	rc_3.load_eeprom();
+	rc_4.load_eeprom();
+	rc_5.load_eeprom();
+	rc_6.load_eeprom();
+	rc_7.load_eeprom();
+	rc_8.load_eeprom();
+	
+	print_radio_values();
 	
 	// set type of output, symmetrical angles or a number range;
 	rc_1.set_angle(4500);
 	rc_2.set_angle(4500);
 	rc_3.set_range(0,1000);
 	rc_4.set_angle(3000);
+	rc_5.set_range(0,1000);
+	rc_6.set_range(0,1000);
+	rc_7.set_range(0,1000);
+	rc_8.set_range(0,1000);
 	
 	// set midpoint value
 	rc_1.set_pwm(APM_RC.InputCh(CH_1));
 	rc_2.set_pwm(APM_RC.InputCh(CH_2));
-	rc_3.set_pwm(APM_RC.InputCh(CH_3));
 	rc_4.set_pwm(APM_RC.InputCh(CH_4));
 
 	rc_1.trim();
 	rc_2.trim();
-	rc_3.trim();
 	rc_4.trim();
+	
+	
 }
 
 void loop()	
@@ -65,6 +102,10 @@ void loop()
 	rc_2.set_pwm(APM_RC.InputCh(CH_2));
 	rc_3.set_pwm(APM_RC.InputCh(CH_3));
 	rc_4.set_pwm(APM_RC.InputCh(CH_4));
+	rc_5.set_pwm(APM_RC.InputCh(CH_5));
+	rc_6.set_pwm(APM_RC.InputCh(CH_6));
+	rc_7.set_pwm(APM_RC.InputCh(CH_7));
+	rc_8.set_pwm(APM_RC.InputCh(CH_8));
 	
 	print_pwm();
 }
@@ -78,5 +119,57 @@ void print_pwm()
 	Serial.print("\tch3 :");
 	Serial.print(rc_3.control_in, DEC);
 	Serial.print("\tch4 :");
-	Serial.println(rc_4.control_in, DEC);
+	Serial.print(rc_4.control_in, DEC);
-}
+	Serial.print("\tch5 :");
+	Serial.print(rc_5.control_in, DEC);
+	Serial.print("\tch6 :");
+	Serial.print(rc_6.control_in, DEC);
+	Serial.print("\tch7 :");
+	Serial.print(rc_7.control_in, DEC);
+	Serial.print("\tch8 :");
+	Serial.println(rc_8.control_in, DEC);
+}
+
+void 
+print_radio_values()
+{
+	Serial.print("CH1: ");
+	Serial.print(rc_1.radio_min, DEC);
+	Serial.print(" | ");
+	Serial.println(rc_1.radio_max, DEC);
+
+	Serial.print("CH2: ");
+	Serial.print(rc_2.radio_min, DEC);
+	Serial.print(" | ");
+	Serial.println(rc_2.radio_max, DEC);
+
+	Serial.print("CH3: ");
+	Serial.print(rc_3.radio_min, DEC);
+	Serial.print(" | ");
+	Serial.println(rc_3.radio_max, DEC);
+
+	Serial.print("CH4: ");
+	Serial.print(rc_4.radio_min, DEC);
+	Serial.print(" | ");
+	Serial.println(rc_4.radio_max, DEC);
+
+	Serial.print("CH5: ");
+	Serial.print(rc_5.radio_min, DEC);
+	Serial.print(" | ");
+	Serial.println(rc_5.radio_max, DEC);
+
+	Serial.print("CH6: ");
+	Serial.print(rc_6.radio_min, DEC);
+	Serial.print(" | ");
+	Serial.println(rc_6.radio_max, DEC);
+
+	Serial.print("CH7: ");
+	Serial.print(rc_7.radio_min, DEC);
+	Serial.print(" | ");
+	Serial.println(rc_7.radio_max, DEC);
+
+	Serial.print("CH8: ");
+	Serial.print(rc_8.radio_min, DEC);
+	Serial.print(" | ");
+	Serial.println(rc_8.radio_max, DEC);
+}