// ==============================================
// =      U.A.V.P Brushless UFO Controller      =
// =           Professional Version             =
// = Copyright (c) 2007 Ing. Wolfgang Mahringer =
// ==============================================
//
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 2 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License along
//  with this program; if not, write to the Free Software Foundation, Inc.,
//  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//
// ==============================================
// =  please visit http://www.uavp.de           =
// =               http://www.mahringer.co.at   =
// ==============================================

// Interrupt routine

#include "c-ufo.h"
#include "bits.h"

#include <int16cxx.h>	//interrupt support

#pragma origin 4

// Interrupt Routine
// Note the new values are stored as IGas, IRoll, INick, ITurn, I5, I6, I7
// My Spektrum DX7 gives...
//	newK6,throttle 	-> IGas
//  newK1,roll		-> IRoll
//  newK4,nick		-> INick
//  newK7,yaw		-> ITurn
//  newK3,sw		-> I5
//  newK5,camera	-> I6
//  newK2,aux		-> I7

bank0	uns16	NewK1, NewK2, NewK3, NewK4, NewK5;
//uns16	NewK6@NewK1;  //original 
//uns16	NewK7@NewK2;  //original
uns16	NewK6@NewK2;  //overwrite value that was used to make I7
uns16	NewK7@NewK5;  //overwrite value that was used to make I6
uns8	RecFlags;
uns8	NewRoll@NewK1.high8; //changed to map onto K1 - was K2
uns8	NewNick@NewK4.high8; //changed to map onto K4 - was K3

interrupt irq(void)
{
	uns16 CCPR1 @0x15;

	int_save_registers;	// save W and STATUS

	if( TMR2IF )	// 5 or 14 ms have elapsed without an active edge
	{
		TMR2IF = 0;	// quit int
#ifndef RX_PPM	// single PPM pulse train from receiver
		if( _FirstTimeout )	// 5 ms have been gone by...
		{
			PR2 = TMR2_5MS;		// set compare reg to 5ms
			goto ErrorRestart;
		}
		_FirstTimeout = 1;
		PR2 = TMR2_14MS;		// set compare reg to 14ms
#endif
		RecFlags = 0;
	}
	if( CCP1IF )
	{
#ifdef RX_PPM	// single PPM pulse train from receiver - NOT CHANGED FOR SPRKTRUM DX7
PORTC.0=1;
//		PR2 = TMR2_5MS;		// set compare reg to 5ms
		TMR2 = 0;	// re-set timer and postscaler
		TMR2IF = 0;	// quit int
		_FirstTimeout = 0;

		if( RecFlags == 0 )
			NewK1 = CCPR1;
		else
		if( RecFlags == 1 )
		{
			NewK2 = CCPR1;
			NewK1 = CCPR1 - NewK1;
		}
		else
		if( RecFlags == 2 )
		{
			NewK3 = CCPR1;
			NewK2 = CCPR1 - NewK2;
		}
		else
		if( RecFlags == 3 )
		{
			NewK4 = CCPR1;
			NewK3 = CCPR1 - NewK3;
		}
		else
		if( RecFlags == 4 )
		{
			NewK5 = CCPR1;
			NewK4 = CCPR1 - NewK4;
		}
		else
		if( RecFlags == 5 )
		{
			NewK5 = CCPR1 - NewK5;
// all complete! Now copy all the values at once
			NewK1 >>= 1;
			NewK2 >>= 1;
			NewK3 >>= 1;
			NewK4 >>= 1;
			NewK5 >>= 1;
// sanity check
// NewKx has values in 4us units now. content must be 256..511 (1024-2047us)
			if( (NewK1.high8 == 1) &&
			    (NewK2.high8 == 1) &&
			    (NewK3.high8 == 1) &&
			    (NewK4.high8 == 1) &&
			    (NewK5.high8 == 1) )
			{
				if( FutabaMode )
				{
					IGas  = NewK3.low8;
#ifdef EXCHROLLNICK
					NewRoll = NewK2.low8;
					NewNick = NewK1.low8;
#else	// REAL transmitters use this
					NewRoll = NewK1.low8;
					NewNick = NewK2.low8;
#endif
				}
				else
				{
					IGas  = NewK1.low8;
					NewRoll = NewK2.low8;
					NewNick = NewK3.low8;
				}

				IK5 = NewK5.low8;

				NewRoll -= _Neutral;
				NewNick -= _Neutral;
				if( DoubleRate )
				{
					(int)NewRoll >>= 1;
					(int)NewNick >>= 1;
				}

		// New Roll/Nick = (3*Old_Roll + New_Roll)/4
		//
		// smooths away jitter from the transmitter
		// very good idea from ufo_hans (Hans Haider), THANK YOU!

				NewK5 = (long)IRoll;
				NewK5 <<= 1;
				NewK5 += (long)IRoll;
				(long)NewK5 += (int)NewRoll;
				NewK5 >>= 2;
				IRoll = NewK5.low8;

				NewK5 = (long)INick;
				NewK5 <<= 1;
				NewK5 += (long)INick;
				(long)NewK5 += (int)NewNick;
				NewK5 >>= 2;
				INick = NewK5.low8;
		// --				

				NewK6 = CCPR1;
				
				ITurn = NewK4.low8 - _Neutral;
				_NoSignal = 0;
				_NewValues = 1;
			}
			else	// values are unsafe
				goto ErrorRestart;
		}
		else
		if( RecFlags == 6 )
		{
			NewK7 = CCPR1;
			NewK6 = CCPR1 - NewK6;
			IK6 = NewK6 >> 1;
		}
		else
		if( RecFlags == 7 )
		{
			NewK7 = CCPR1 - NewK7;
			IK7 = NewK7 >> 1;
			RecFlags = -1;
		}
		else	// values are unsafe
		{
ErrorRestart:
PORTC.0=1;
			_NewValues = 0;
			_NoSignal = 1;		// Signal lost
			RecFlags = -1;
		}

		CCP1IF = 0;		// quit int
		RecFlags++;
PORTC.0=0;
#else	// standard usage (3 or 4 channels)
		CCPR1.low8 = CCPR1L;
		CCPR1.high8 = CCPR1H;
		CCP1M0 ^= 1;	// toggle edge bit
		PR2 = TMR2_5MS;		// set compare reg to 5ms
		TMR2 = 0;	// re-set timer and postscaler
		TMR2IF = 0;	// quit int
		_FirstTimeout = 0;

		if( NegativePPM ^ CCP1M0  )	// a negative edge
		{
			if( RecFlags == 0 )
			{
				NewK1 = CCPR1;
			}
			else
			if( RecFlags == 2 )
			{
				NewK3 = CCPR1;
				NewK2 = CCPR1 - NewK2; // used for I7 - make it now
				IK7 = NewK2 >> 1; // now we can reuse NewK2 for NewK6
			}
			else
			if( RecFlags == 4 )
			{
				NewK5 = CCPR1;
				NewK4 = CCPR1 - NewK4; // used for INick
			}
			else
			if( RecFlags == 6 )
			{
				NewK7 = CCPR1;
				NewK6 = CCPR1 - NewK6; // used for IGas
			}
			else	// values are unsafe
				goto ErrorRestart;
		}
		else	// a positive edge
		{
			if( RecFlags == 1 )
			{
				NewK2 = CCPR1;
				NewK1 = CCPR1 - NewK1; // used for IRoll
			}
			else
			if( RecFlags == 3 )
			{
				NewK4 = CCPR1;
				NewK3 = CCPR1 - NewK3; // used for I5
			}
			else
			if( RecFlags == 5 )
			{
				NewK6 = CCPR1; // this was at the end of the section before
				NewK5 = CCPR1 - NewK5; // used to make I6 - do it now
				IK6 = NewK5 >> 1;  // now we can reuse NewK5 for NewK7
			}
			else
			if( RecFlags == 7 )
			{
				NewK7 = CCPR1 - NewK7; //used for ITurn

// all complete! Now copy all the values at once
				NewK1 >>= 1;
				NewK3 >>= 1;
				NewK4 >>= 1;
				NewK6 >>= 1; //overlaps NewK2
				NewK7 >>= 1; //overlaps NewK5
				//already got IK6 and IK7 from NewK2 and NewK5
// sanity check
// NewKx has values in 4us units now. content must be 256..511 (1024-2047us)
				if( (NewK1.high8 == 1) &&
				    (NewK3.high8 == 1) &&
				    (NewK4.high8 == 1) &&
				    (NewK6.high8 == 1) &&
				    (NewK7.high8 == 1) )
				{
					IGas  = NewK6.low8;
					NewRoll = NewK1.low8;
					NewNick = NewK4.low8;

					IK5 = NewK3.low8;

					NewRoll -= _Neutral;
					NewNick -= _Neutral;
					if( DoubleRate )
					{
						(int)NewRoll >>= 1;
						(int)NewNick >>= 1;
					}

		// New Roll/Nick = (3*Old_Roll + New_Roll)/4
		//
		// smooths away jitter from the transmitter
		// very good idea from ufo_hans (Hans Haider), THANK YOU!

					NewK3 = (long)IRoll;
					NewK3 <<= 1;
					NewK3 += (long)IRoll;
					(long)NewK3 += (int)NewRoll;
					NewK3 >>= 2;
					IRoll = NewK3.low8;

					NewK3 = (long)INick;
					NewK3 <<= 1;
					NewK3 += (long)INick;
					(long)NewK3 += (int)NewNick;
					NewK3 >>= 2;
					INick = NewK3.low8;
		// --				

					ITurn = NewK7.low8 - _Neutral;

					_NoSignal = 0;
					_NewValues = 1;
				}
				else	// values are unsafe
					goto ErrorRestart;

				RecFlags = -1;
			}
			else
			{
ErrorRestart:
				_NewValues = 0;
				_NoSignal = 1;		// Signal lost
				RecFlags = -1;
				if( NegativePPM )
					CCP1M0 = 1;	// wait for positive edge next
				else
					CCP1M0 = 0;	// wait for negative edge next
			}	

		}
		CCP1IF = 0;		// quit int
		RecFlags++;
#endif
	}
	if( T0IF && T0IE )
	{
		T0IF = 0;	// quit int
		TimeSlot--;
	}
	
	int_restore_registers;
}