ardupilot/Tools/ArduPPM/ATMega328p/Encoder-PPM-v3.c

// ------------------------------------------------------------------------------------------------------------------------------------------------------------
// ArduPPM Version v0.9.87
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
// ARDUCOPTER 2 : PPM ENCODER for AT Mega 328p and APM v1.4 Boards
// By:John Arne Birkeland - 2011
//
//  By: Olivier ADLER - 2011 - APM v1.4 adaptation and testing
//
// Compiled with Atmel AVR Studio 4.0 / AVR GCC
// ------------------------------------------------------------------------------------------------------------------------------------------------------------

// 		Changelog		//
//
// Code based on John Arne PPM v1 encoder. Mux / Led / Failsafe control from Olivier ADLER.
// Adaptation to APM v1.4 / ATMEGA 328p by Olivier ADLER, with great code base, help  and advices from John Arne.
//
//	0.9.0 -> 0.9.4 : experimental versions. Not publicly available. Jitter problems, good reliability.
// 	
//	New PPM code base V2 from John Arne designed for 32u2 AVRs
//
//	0.9.5 : first reliable and jitter free version based on new John PPM V2 code and Olivier interrupt nesting idea.
//	0.9.6 : enhanced jitter free version with non bloking servo interrupt and ultra fast ppm generator interrupt(John's ideas)
//	0.9.7 : mux (passthrough mode) switchover reliability enhancements and error reporting improvements.
//	0.9.75 : implemented ppm_encoder.h library with support for both atmega328p and atmega32u2 chips
//	0.9.76 : timers 0 and 2 replaced by a delayed loop for simplicity. Timer 0 and 2 are now free for use.
//	              reworked error detection with settable time window, errors threshold and Led delay
//	0.9.77 : Implemented ppm_encoder.h into latest version.
//	0.9.78 : Implemented optimzed assembly compare interrupt
//	0.9.79 : Removed Non Blocking attribute for servo input interrupt
//	0.9.80 : Removed non blocking for compare interrupt, added optionnal jitter filter and optionnal non blocking attribute for assembly version of compare interrupt
//	0.9.81 : Added PPM PASSTROUGH Mode and LED Codes function to report special modes
//	0.9.82 : LED codes function simplification
//	0.9.83 : Implemented PPM passtrough failsafe
//	0.9.84 : Corrected pin and port names in Encoder-PPM.c according to #define for Mega32U2 compatibility
//	0.9.85 : Added brownout reset detection flag
//	0.9.86 : Added a #define to disable Radio Passthrough mode (hardware failsafe for Arduplane)
//	0.9.87 : #define correction for radio passthrough (was screwed up).
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
// PREPROCESSOR DIRECTIVES
// ------------------------------------------------------------------------------------------------------------------------------------------------------------

#include "..\Libraries\ppm_encoder.h"
#include <util/delay.h>


#define	ERROR_THRESHOLD		2									// Number of servo input errors before alerting
#define ERROR_DETECTION_WINDOW	3000 * LOOP_TIMER_10MS			// Detection window for error detection (default to 30s)
#define	ERROR_CONDITION_DELAY	500 * LOOP_TIMER_10MS			// Servo error condition LED delay (LED blinking duration)

#define PASSTHROUGH_MODE_ENABLED	// Comment this line to remove CH8 radio passthrough mode support (hardware failsafe for Arduplane)
#define PASSTHROUGH_CHANNEL		8 * 2	// Channel for passthrough mode selection
#define PASSTHROUGH_CHANNEL_OFF_US		ONE_US * 1600 - PPM_PRE_PULSE	// Passthrough off threshold
#define PASSTHROUGH_CHANNEL_ON_US		ONE_US * 1800 - PPM_PRE_PULSE	// Passthrough on threshold

#define THROTTLE_CHANNEL		3 * 2	// Throttle Channel
#define THROTTLE_CHANNEL_LED_TOGGLE_US		ONE_US * 1200 - PPM_PRE_PULSE	// Throttle Channel Led toggle threshold
#define LED_LOW_BLINKING_RATE	125 * LOOP_TIMER_10MS // Led blink rate for low throttle position (half period)

// Timers

#define TIMER0_10MS		156			// Timer0 ticks for 10 ms duration
#define TIMER1_10MS		20000		// Timer1 ticks for 10 ms duration
#define TIMER2_100MS		1562	// Timer2 ticks for 100 ms duration
#define LOOP_TIMER_10MS	10			// Loop timer ticks for 10 ms duration

// LED Code

#define	SPACE_SHORT_DURATION	40 * LOOP_TIMER_10MS	// Space after short symbol
#define	SPACE_LONG_DURATION	75 * LOOP_TIMER_10MS		// Space after long symbol
#define	SYMBOL_SHORT_DURATION	20 * LOOP_TIMER_10MS	// Short symbol duration
#define	SYMBOL_LONG_DURATION	100 * LOOP_TIMER_10MS	// Long symbol duration
#define	INTER_CODE_DURATION	150 * LOOP_TIMER_10MS		// Inter code duration

#define INTER_CODE		0		// Symbols value for coding
#define SHORT_SYMBOL	1
#define LONG_SYMBOL		2
#define SHORT_SPACE		3
#define LONG_SPACE		4
#define LOOP			5



// ------------------------------------------------------------------------------------------------------------------------------------------------------------
// PPM ENCODER INIT AND AUXILIARY TASKS
// ------------------------------------------------------------------------------------------------------------------------------------------------------------

int main(void)
{
	// ------------------------------------------------------------------------------------------------------------------------------------------------------------
	// LOCAL VARIABLES
	// ------------------------------------------------------------------------------------------------------------------------------------------------------------
	bool init = true; // We are inside init sequence
	bool mux_passthrough = false; // Mux passthrough mode status Flag : passthrough is off
	uint16_t led_acceleration; // Led acceleration based on throttle stick position
	bool servo_error_condition = false;	//	Servo signal error condition
	
	static uint16_t servo_error_detection_timer=0;		// Servo error detection timer
	static uint16_t servo_error_condition_timer=0; 		// Servo error condition timer
	static uint16_t blink_led_timer = 0; 		// Blink led timer
	
	#ifdef PASSTHROUGH_MODE_ENABLED
	static uint8_t mux_timer = 0;				// Mux timer
	static uint8_t mux_counter = 0;				// Mux counter
	static int8_t mux_check = 0;
	static uint16_t mux_ppm = 500;
	#endif
	
	static uint16_t led_code_timer = 0;	// Blink Code Timer
	static uint8_t led_code_symbol = 0;	// Blink Code current symbol

	
	// ------------------------------------------------------------------------------------------------------------------------------------------------------------
	// LOCAL FUNCTIONS
	// ------------------------------------------------------------------------------------------------------------------------------------------------------------
	
	// ------------------------------------------------------------------------------
	// Led blinking (non blocking) function
	// ------------------------------------------------------------------------------
		
	uint8_t blink_led ( uint16_t half_period )	// ( half_period max = 65 s )
	{
						
		blink_led_timer++;
		
		if ( blink_led_timer < half_period ) // If half period has not been reached
		{
			return 0; // Exit timer function and return 0
		}
		else	// half period reached - LED Toggle
		{
			PPM_PORT ^= ( 1 << PB0 );	// Toggle status LED
			blink_led_timer = 0;	// Blink led timer reset
						
			return 1;	// half period reached - Exit timer function and return 1
		}
	
	}
	
	// ------------------------------------------------------------------------------
	// Led code (non blocking) function
	// ------------------------------------------------------------------------------
	
	void blink_code_led ( uint8_t code )
	{
		
		const uint8_t coding[2][14] = {
		
		// PPM_PASSTROUGH_MODE
		{ INTER_CODE, LONG_SYMBOL, LONG_SPACE, SHORT_SYMBOL, SHORT_SPACE, SHORT_SYMBOL, LOOP }, 
		
		// JETI_MODE
		{ INTER_CODE, LONG_SYMBOL, LONG_SPACE, SHORT_SYMBOL, SHORT_SPACE, SHORT_SYMBOL, SHORT_SPACE, SHORT_SYMBOL,LOOP }
		
		};
		
		led_code_timer++;		
					
						
			switch ( coding [ code - 2 ] [ led_code_symbol ] )
			{
				case INTER_CODE:
				
				if ( led_code_timer < ( INTER_CODE_DURATION ) ) return;
				else PPM_PORT |= ( 1 << PB0 );		// Enable status LED
				break;
				
				case LONG_SYMBOL:	// Long symbol
				
				if ( led_code_timer < ( SYMBOL_LONG_DURATION ) ) return;
				else PPM_PORT &= ~( 1 << PB0 );	// Disable status LED
				break;
				
				case SHORT_SYMBOL:	// Short symbol
								
				if ( led_code_timer < ( SYMBOL_SHORT_DURATION ) ) return;
				else PPM_PORT &= ~( 1 << PB0 );	// Disable status LED
				break;
				
				case SHORT_SPACE:	// Short space
				
				if ( led_code_timer < ( SPACE_SHORT_DURATION ) ) return;
				else PPM_PORT |= ( 1 << PB0 );		// Enable status LED
				break;
				
				case LONG_SPACE:	// Long space
				
				if ( led_code_timer < ( SPACE_LONG_DURATION ) ) return;
				else PPM_PORT |= ( 1 << PB0 );		// Enable status LED
				break;
				
				case LOOP:	// Loop to code start
				led_code_symbol = 0;
				return;
				break;
				
			}
						
		led_code_timer = 0;	// Code led timer reset
		led_code_symbol++;	// Next symbol
		
		return; // LED code function return
		
	}
	
		
	// ------------------------------------------------------------------------------
	// ppm reading helper - interrupt safe and non blocking function
	// ------------------------------------------------------------------------------
	uint16_t ppm_read( uint8_t channel )
	{
		uint16_t ppm_tmp = ppm[ channel ];
		while( ppm_tmp != ppm[ channel ] ) ppm_tmp = ppm[ channel ];

		return ppm_tmp;
	}

	// ------------------------------------------------------------------------------------------------------------------------------------------------------------
	// INITIALISATION CODE
	// ------------------------------------------------------------------------------------------------------------------------------------------------------------
	
	// ------------------------------------------------------------------------------	
	// Reset Source checkings
	// ------------------------------------------------------------------------------
	if (MCUSR & 1)	// Power-on Reset
	{
	   MCUSR=0; // Clear MCU Status register
	   // custom code here
	}
	else if (MCUSR & 2)	// External Reset
	{
	   MCUSR=0; // Clear MCU Status register
	   // custom code here
	}
	else if (MCUSR & 4)	// Brown-Out Reset
	{
	   MCUSR=0; // Clear MCU Status register
	   brownout_reset=true;
	}
	else	// Watchdog Reset
	{
	   MCUSR=0; // Clear MCU Status register
	   // custom code here
	}

	
	// ------------------------------------------------------------------------------
	// Servo input and PPM generator init
	// ------------------------------------------------------------------------------
	ppm_encoder_init();

	// ------------------------------------------------------------------------------
	// Outputs init
	// ------------------------------------------------------------------------------
	PPM_DDR |= ( 1 << PB0 );	// Set LED pin (PB0) to output
	PPM_DDR |= ( 1 << PB1 );	// Set MUX pin (PB1) to output
	PPM_DDR |= ( 1 << PPM_OUTPUT_PIN );	// Set PPM pin (PPM_OUTPUT_PIN, OC1B) to output
	
	// ------------------------------------------------------------------------------		
	// Timer0 init (normal mode) used for LED control and custom code
	// ------------------------------------------------------------------------------
	TCCR0A = 0x00;	// Clock source: System Clock
	TCCR0B = 0x05;	// Set 1024x prescaler - Clock value: 15.625 kHz - 16 ms max time
	TCNT0 = 0x00;
	OCR0A = 0x00;		// OC0x outputs: Disconnected
	OCR0B = 0x00;
	TIMSK0 = 0x00;		// Timer 1 interrupt disable
	
	// ------------------------------------------------------------------------------
	// Enable global interrupt
	// ------------------------------------------------------------------------------
	sei();			// Enable Global interrupt flag
	
	// ------------------------------------------------------------------------------
	// Disable radio passthrough (mux chip A/B control)
	// ------------------------------------------------------------------------------
	PPM_PORT |= ( 1 << PB1 );	// Set PIN B1 to disable Radio passthrough (mux)
	
	
	
	
	// ------------------------------------------------------------------------------
	// Check for first valid servo signal
	// ------------------------------------------------------------------------------
	while( 1 )
	{
		if ( servo_error_condition || servo_input_missing )	// We have an error 
		{
			blink_led ( 6 * LOOP_TIMER_10MS ); // Status LED very fast blink if invalid servo input or missing signal
		}
		else		// We are running normally
		{
			init = false;	// initialisation is done,
			switch ( servo_input_mode )
			{
				case SERVO_PWM_MODE: // Normal PWM mode
				goto PWM_LOOP;
				break;
			
				case PPM_PASSTROUGH_MODE: // PPM_PASSTROUGH_MODE
				goto PPM_PASSTHROUGH_LOOP;
				break;
				
				default:	// Normal PWM mode
				goto PWM_LOOP;
				break;
			}
		}
		_delay_us (970); // Slow down while loop
	}
	

	// ------------------------------------------------------------------------------------------------------------------------------------------------------------
	// AUXILIARY TASKS
	// ------------------------------------------------------------------------------------------------------------------------------------------------------------
	PWM_LOOP: // SERVO_PWM_MODE
	while( 1 )
	{
		#ifdef PASSTHROUGH_MODE_ENABLED
		// ------------------------------------------------------------------------------
		// Radio passthrough control (mux chip A/B control)
		// ------------------------------------------------------------------------------
			
		mux_timer++; // Increment mux timer
					
		if ( mux_timer > ( 3 * LOOP_TIMER_10MS ) )	// Check Passthrough Channel every 30ms
		{
		
			mux_timer = 0; // Reset mux timer
																

			if ( mux_counter++ < 5)		// Check passthrough channel position 5 times
			{

				mux_ppm = ppm_read( PASSTHROUGH_CHANNEL - 1 ); // Safely read passthrough channel ppm position

				if ( mux_ppm < ( PASSTHROUGH_CHANNEL_OFF_US ) )	// Check ppm value and update validation counter
				{
					mux_check -= 1;
				}
				else if ( mux_ppm > ( PASSTHROUGH_CHANNEL_ON_US ) )
				{
					mux_check += 1;
				}

			}
			else							// Check 
			{		
				switch ( mux_check )	// If all 5 checks are the same, update mux status flag
				{
					case -5:
					mux_passthrough = false;
					PPM_PORT |= ( 1 << PB1 );	// Set PIN B1 (Mux) to disable Radio passthrough
					break;
			
					case 5:
					mux_passthrough = true;
					PPM_PORT &= ~( 1 << PB1 );	// Reset PIN B1 (Mux) to enable Radio passthrough
					break;
				}
				mux_check = 0; 			// Reset mux validation counter
				mux_counter = 0;		// Reset mux counter
			}
					
		}			
		
		#endif

		// ------------------------------------------------------------------------------
		// Status LED control
		// ------------------------------------------------------------------------------
		if ( servo_error_condition || servo_input_missing )	// We have an error 
		{
			blink_led ( 6 * LOOP_TIMER_10MS ); // Status LED very fast blink if invalid servo input or missing signal
		}
		else		// We are running normally
		{
			if ( mux_passthrough == false ) //  Normal mode : status LED toggle speed from throttle position
			{
				led_acceleration = ( ppm[THROTTLE_CHANNEL - 1] - ( PPM_SERVO_MIN ) ) / 2;
				blink_led ( LED_LOW_BLINKING_RATE - led_acceleration );
			}
			else 	//  Passthrough mode : status LED never flashing
			{
				//  Enable status LED if throttle > THROTTLE_CHANNEL_LED_TOGGLE_US
				if ( ppm[THROTTLE_CHANNEL - 1] > ( THROTTLE_CHANNEL_LED_TOGGLE_US ) )
				{
					PPM_PORT |= ( 1 << PB0 );
				}
				// Disable status LED if throttle <= THROTTLE_CHANNEL_LED_TOGGLE_US
				else if ( ppm[THROTTLE_CHANNEL - 1] <= ( THROTTLE_CHANNEL_LED_TOGGLE_US ) )
				{
					PPM_PORT &= ~( 1 << PB0 );	
				}
			}
		}

		// ------------------------------------------------------------------------------
		// Servo input error detection
		// ------------------------------------------------------------------------------
		
		// If there are too many errors during the detection time window, then trig servo error condition
		
		if ( servo_input_errors > 0 )									// Start error rate checking if an error did occur
		{
			if ( servo_error_detection_timer > ( ERROR_DETECTION_WINDOW ) )	// If 10s delay reached
			{
				servo_error_detection_timer = 0;	 						// Reset error detection timer

				servo_input_errors = 0; 									// Reset servo input error counter
				
			}
			else															// If 10s delay is not reached
			{
				servo_error_detection_timer++;  							// Increment servo error timer value
				
				if ( servo_input_errors >= ( ERROR_THRESHOLD ) )	// If there are too many errors
				{
					servo_error_condition = true;							// Enable error condition flag
					servo_input_errors = 0;									// Reset servo input error counter
					servo_error_detection_timer = 0;						// Reset servo error detection timer
					servo_error_condition_timer = 0;						// Reset servo error condition timer
				}
			}
			
		}

		
		// Servo error condition flag (will control blinking LED)

		if ( servo_error_condition == true )	// We are in error condition
		{	
																	
			if ( servo_error_condition_timer > ( ERROR_CONDITION_DELAY ) )	// If 3s delay reached
			{
				servo_error_condition_timer = 0;	 						// Reset servo error condition timer

				servo_error_condition = false; 	// Reset servo error condition flag (Led will stop very fast blink)
			}

			else servo_error_condition_timer++; // If 3s delay is not reached update servo error condition timer value
		}
		
		_delay_us (950); // Slow down while loop
		
	}	// PWM Loop end

	
	
	PPM_PASSTHROUGH_LOOP: // PPM_PASSTROUGH_MODE
	
	while (1)
	{
		// ------------------------------------------------------------------------------
		// Status LED control
		// ------------------------------------------------------------------------------
		
		if ( servo_input_missing )	// We have an error 
		{
			blink_led ( 6 * LOOP_TIMER_10MS ); // Status LED very fast blink if invalid servo input or missing signal
		}
		else		// We are running normally
		{
			blink_code_led ( PPM_PASSTROUGH_MODE ); // Blink LED according to mode 2 code  (one long, two shorts).
		}
		
		_delay_us (970); // Slow down this loop
		
	
	}	// PPM_PASSTHROUGH Loop end
	
} // main function end