Added alternate RC_Channel class.

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

libraries/RC_ChannelB/RC_ChannelB.cpp

@@ -0,0 +1,193 @@
+/*
+	RC_ChannelB.cpp - Radio library for Arduino
+	Code by Jason Short. DIYDrones.com
+	
+	This library is free software; you can redistribute it and / or
+		modify it under the terms of the GNU Lesser General Public
+		License as published by the Free Software Foundation; either
+		version 2.1 of the License, or (at your option) any later version.
+
+*/
+
+#include <math.h>
+#include <avr/eeprom.h>
+#include "WProgram.h"
+#include "RC_ChannelB.h"
+
+#define ANGLE 0
+#define RANGE 1
+
+// setup the control preferences
+void 	
+RC_ChannelB::set_range(int low, int high)
+{
+	_type 	= RANGE;
+	_high 	= high;
+	_low 	= low;
+}
+
+void
+RC_ChannelB::set_angle(int angle)
+{
+	_type 	= ANGLE;
+	_high 	= angle;
+}
+
+void
+RC_ChannelB::set_reverse(bool reverse)
+{
+	if (reverse) _reverse = -1;
+	else _reverse = 1;
+}
+
+void
+RC_ChannelB::set_filter(bool filter)
+{
+	_filter = filter;
+}
+
+// call after first read
+void
+RC_ChannelB::trim()
+{
+	radio_trim = radio_in;
+	
+}
+
+// read input from APM_RC - create a control_in value
+void
+RC_ChannelB::set_pwm(int pwm)
+{
+	//Serial.print(pwm,DEC);
+
+	if(_filter){
+		if(radio_in == 0)
+			radio_in = pwm;
+		else
+			radio_in = ((pwm + radio_in) >> 1);		// Small filtering
+	}else{
+		radio_in = pwm;
+	}
+	
+	if(_type == RANGE){
+		//Serial.print("range ");
+		control_in = pwm_to_range();
+		control_in = (control_in < dead_zone) ? 0 : control_in;
+	}else{
+		control_in = pwm_to_angle();
+		control_in = (abs(control_in) < dead_zone) ? 0 : control_in;
+	}
+}
+
+int
+RC_ChannelB::control_mix(float value)
+{
+	return (1 - abs(control_in / _high)) * value + control_in;
+}
+
+// are we below a threshold?
+bool
+RC_ChannelB::get_failsafe(void)
+{
+	return (radio_in < (radio_min - 50));
+}
+
+// returns just the PWM without the offset from radio_min
+void
+RC_ChannelB::calc_pwm(void)
+{
+
+	if(_type == RANGE){
+		pwm_out = range_to_pwm();
+	}else{
+		pwm_out = angle_to_pwm();
+	}
+	radio_out = pwm_out + radio_min;
+}
+
+// ------------------------------------------
+
+void
+RC_ChannelB::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));
+}
+
+void
+RC_ChannelB::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);
+}
+
+// ------------------------------------------
+void
+RC_ChannelB::save_trim(void)
+{
+	eeprom_write_word((uint16_t *)	(_address + 4), 	radio_trim);
+}
+
+// ------------------------------------------
+
+void
+RC_ChannelB::update_min_max()
+{
+	radio_min = min(radio_min, radio_in);
+	radio_max = max(radio_max, radio_in);
+}
+
+// ------------------------------------------
+
+int16_t 
+RC_ChannelB::pwm_to_angle()
+{
+	if(radio_in < radio_trim)
+		return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_trim - radio_min));
+	else
+		return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_max  - radio_trim));
+}
+
+float 
+RC_ChannelB::norm_input()
+{
+	if(radio_in < radio_trim)
+		return _reverse * (float)(radio_in - radio_trim) / (float)(radio_trim - radio_min);
+	else
+		return _reverse * (float)(radio_in - radio_trim) / (float)(radio_max  - radio_trim);
+}
+
+float 
+RC_ChannelB::norm_output()
+{
+	if(radio_out < radio_trim)
+		return (float)(radio_out - radio_trim) / (float)(radio_trim - radio_min);
+	else
+		return (float)(radio_out - radio_trim) / (float)(radio_max  - radio_trim);
+}
+
+int16_t
+RC_ChannelB::angle_to_pwm()
+{
+	if(servo_out < 0)
+		return (((float)servo_out / (float)_high) * (float)(radio_max - radio_trim));
+	else
+		return (((float)servo_out / (float)_high) * (float)(radio_trim - radio_min));
+}
+
+// ------------------------------------------
+
+int16_t
+RC_ChannelB::pwm_to_range()
+{
+	return _reverse * (_low + ((_high - _low) * ((float)(radio_in - radio_min) / (float)(radio_max - radio_min))));
+}
+
+int16_t
+RC_ChannelB::range_to_pwm()
+{
+	return (((float)servo_out / (float)(_high - _low)) * (float)(radio_max - radio_min));
+}
+

libraries/RC_ChannelB/RC_ChannelB.h

@@ -0,0 +1,105 @@
+// -*- 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.
+
+#ifndef RC_ChannelB_h
+#define RC_ChannelB_h
+
+#include <stdint.h>
+
+/// @class	RC_ChannelB
+/// @brief	Object managing one RC channel
+class RC_ChannelB{
+  public:	
+	/// Constructor
+	///
+	/// A RC_ChannelB constructed in this fashion does not support save/restore.
+	///
+	RC_ChannelB() :
+		_address(0)
+	{}
+
+	/// Constructor
+	///
+	/// @param address	EEPROM base address at which RC_ChannelB parameters
+	///					are stored.  Zero if the RC_ChannelB does not support
+	///					save/restore.
+	///
+	RC_ChannelB(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(void);
+	void 		save_eeprom(void);		
+	void 		save_trim(void);		
+	void		set_filter(bool filter);
+
+	// setup the control preferences
+	void 		set_range(int low, int high);
+	void 		set_angle(int angle);
+	void 		set_reverse(bool reverse);
+
+	// read input from APM_RC - create a control_in value
+	void 		set_pwm(int pwm);
+	
+	// pwm is stored here
+	int16_t		radio_in;
+
+	// call after first set_pwm
+	void 		trim();
+	
+	// did our read come in 50µs below the min?
+	bool		get_failsafe(void);
+	
+	// value generated from PWM
+	int16_t 	control_in;
+	int16_t 	dead_zone; // used to keep noise down and create a dead zone.
+	
+	int			control_mix(float value);
+	
+	// current values to the servos - degrees * 100 (approx assuming servo is -45 to 45 degrees except [3] is 0 to 100
+	int16_t 	servo_out;
+
+	// generate PWM from servo_out value
+	void 		calc_pwm(void);
+
+	// PWM is without the offset from radio_min
+	int16_t 	pwm_out;
+	int16_t 	radio_out;
+
+	int16_t 	radio_min;
+	int16_t 	radio_trim;
+	int16_t 	radio_max;
+	
+	// includes offset from PWM
+	//int16_t 	get_radio_out(void);
+
+	int16_t		pwm_to_angle();
+	float		norm_input();
+	float		norm_output();
+	int16_t		angle_to_pwm();
+	int16_t		pwm_to_range();
+	int16_t		range_to_pwm();
+	
+  private:
+	bool		_filter;
+	int8_t 		_reverse;
+
+	int16_t		_address;			///< EEPROM address for save/restore of P/I/D
+	bool 		_type;				
+	int16_t 	_high;
+	int16_t 	_low;
+};
+
+#endif	
+
+
+
+

libraries/RC_ChannelB/examples/RC_ChannelB/RC_ChannelB.pde

@@ -0,0 +1,175 @@
+/*
+	Example of RC_Channel library.
+	Code by Jason Short. 2010 
+	DIYDrones.com
+
+*/
+
+#include <APM_RC.h> 		// ArduPilot Mega RC Library
+#include <RC_Channel.h> 	// ArduPilot Mega RC Library
+
+
+#define EE_RADIO_1 0x00	// all gains stored from here
+#define EE_RADIO_2 0x06	// all gains stored from here
+#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()
+{
+	Serial.begin(38400);
+	Serial.println("ArduPilot RC Channel test");
+	APM_RC.Init();		// APM Radio initialization
+
+	delay(1000);
+	// setup radio
+	
+	// read eepom or set manually
+	/*
+	rc_1.radio_min = 1100;
+	rc_1.radio_max = 1900;
+	
+	rc_2.radio_min = 1100;
+	rc_2.radio_max = 1900;
+
+	rc_3.radio_min = 1100;
+	rc_3.radio_max = 1900;
+
+	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_4.set_pwm(APM_RC.InputCh(CH_4));
+
+	rc_1.trim();
+	rc_2.trim();
+	rc_4.trim();
+	
+	
+}
+
+void loop()	
+{
+	delay(20);
+	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_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();
+}
+
+void print_pwm()
+{
+	Serial.print("ch1 ");
+	Serial.print(rc_1.control_in, DEC);
+	Serial.print("\tch2: ");
+	Serial.print(rc_2.control_in, DEC);
+	Serial.print("\tch3 :");
+	Serial.print(rc_3.control_in, DEC);
+	Serial.print("\tch4 :");
+	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);
+}