//LVC2
//Low Voltage Cutoff program for twin-engined plane (comparing two separate packs, common ground)
//Simple data logging

//Voltage thresholds set for 5 A123 lithium cells; 'normal' Lithium Polymer cells require different cutoff
//LVC program can have three thresholds but one is usually the most practical
//The thresholds cause throttle to cut until acknowledged by pilot (who does so by also cutting thottle)
//Threshold 3 keeps cutting throttle (and keeps requiring acknowledgement) every time lowest pack reaches min
//Thresholds 1 and 2 (disabled) require only one acknowledgement each (allows voltages to then drift up and down)
//Thresholds 1 and 2 are intended for early warnings if desired

//The program determines the throttle position and pack voltages every ~20ms (in sync with the RX pulse)
//Every 7 seconds the lowest pack voltages (since last write) are stored in memory
//This gives about 15minutes flight recording
//Multiply ADC values by 0.074 to convert to measured volts (4 * 4.95/1023 * 13/3.40)
//4 = scaling to fit 8bit, 4.95 = ref voltage, 4.95/1023 = ADC conversion, 13/3.4 = measured resistor divider effect
//Flight data is only overwritten before each flight once both packs are connected

//Written for PIC12F683 (8 pin) / BoostC compiler
//Personal and non-commercial use encouraged
//Copyright 2007 - David Theunissen - See www.flyelectric.ukgateway.net


//PREPROCESSOR section ========================================================

#include <system.h>			//Generic device 'include'; actual device set in compiler (Settings/Target)

#pragma DATA _CONFIG, _FCMEN_OFF & _IESO_OFF & _BOD_ON & _CPD_OFF & _CP_OFF & _MCLRE_OFF & _PWRTE_ON & _WDT_OFF & _INTOSCIO
							//Core configuration options
							

//VARIABLES section ===========================================================

#define ESC 	gpio.1			//GP1 (Pin6) is Controller
#define RX 		gpio.3			//GP3 (Pin4) is Receiver
#define LED 	gpio.5			//GP5 (Pin2) is LED

typedef unsigned char u8;		//Defines u8 as an unsigned char (8bits)
typedef unsigned short u16;		//Defines u16 as an unsigned short (16bits)

u16 packvalue;					//Used to determine individual pack ADC values
u16 div1;						//The value of resistor dividers (eg: div1 is pack 1)
u16 div2;					

u8 	k;							//Counter for multiple pack readings
u16 pack1;						//The value of pack 1, 2 ...
u16 pack2;					
u16 lowpack;					//Value of lowest pack after comparisons

u16 level3;						//3rd warning threshold
bit ack;						//Pilot acknowledgement of warnings
//u16 level1;					//1st warning threshold (disabled)
//u16 level2;					//2nd warning threshold (disabled)
//bit warn1;					//Stores 1st warning given (disabled)
//bit warn2;					//Stores 2nd warning given (disabled)

u16 pulse_good;					//Duration of last 'known good' pulse from RX
u16 pulse_new;					//Duration of 'latest' pulse from RX
signed short pulse_diff;		//Difference between new and old pulse lengths (16bit signed)
bit jitter;						//Store whether jitter or glitch is being detected (1=yes 0=no)
u8 	j;							//Counter to avoid 'over-controlling' jitter/glitch detection

u16 m_timer;					//Memory Timer - timebase for writes to memory
u8 	m_count;					//Memory Counter - prevents over-writes
u8 	t_timer;					//Throttle Timer - times how long throttle is high
u16 t_pos;						//Throttle Position (stick position)
u8 	highest_t;					//Highest throttle position between writes to memory
u8 	lowest1;					//Lowest pack1 voltages between writes to memory
u8 	lowest2;					//Lowest pack2 voltages between writes to memory
u8 	p;							//Used for short time delays and iterations


//FUNCTIONS section ===========================================================

void write_data()			//Function to write to EEPROM flash memory
{
	intcon.7 = 0;				//Disable all interrupts if not done elsewhere
    pir1.7 = 0;					//EEIF: Clear this bit which gets set after each write
	
	eecon2 = 0x55;				//Required before writing to memory
	eecon2 = 0xAA;				//Required before writing to memory
	eecon1.1 = 1;				//WR: Writes data eedata to address eeadr

	while(eecon1.1==1);			//Wait for write to complete (becomes 0 when complete)

	eeadr++;					//Increment the address by 1 for next write (8 bit 0-255)

	//intcon.7 = 1;				//Enable interrupts if appropriate
}

//-----------------------------------------------------------------------------

void esc_rx()					//Function to mirror RX input to ESC output
{
	//Time duration of each 'high' pulse
	//Timer1 with 1:1 pre-scaler and 4MHz clock should count once every 1us (1000 for a 1ms pulse)
	//Pulses normally go high for between 1 and 2ms every 20ms or so
	//Futaba FF9 generates 1312 at low throttle/low trim and 2572 at high throttle (1.0 - ~2.0ms on scope)
	
		tmr1h=0;						//Initialise Timer1 registers
		tmr1l=0;
		pir1.0=0;						//TMR1IF 'interrupt' flag (overflows)
		
		while (RX==0);					//Wait for RX to finish being low
		t1con.0 = 1;					//Start Timer1
		while (RX==1);					//Wait for RX to finish being high
		t1con.0 = 0;					//Stop Timer1


	//Check for jitter
	//The counter divides the pulse into ~1000 steps
	//A 3 step jitter has been observed (eg: 1316 then 1319)
	//This may confuse some ESCs so is filtered out below
	//One 'click' on FF9 digital trim is 6 steps (eg: 1316 becomes 1322)
		
		//Timer1 has two 8bit registers that need to be merged to enable comparisons
			MAKESHORT(pulse_new, tmr1l, tmr1h);//'pulse_new' now represents duration of pulse
			
		//Determine the difference between last 'good' pulse and latest new one
			pulse_diff = pulse_new - pulse_good;
			
		//Set jitter flag if change in pulse is just jitter (ie: changes by 3 steps or less)
			if (pulse_diff >= -3 && pulse_diff < 0) jitter=1;		//-3 to 0
			else if (pulse_diff > 0 && pulse_diff <= 3) jitter=1;	//0 to +3
			else jitter=0;

		//To avoid being 'over-protective', reset flag after 10 consecutive 'detections'
		//This prevents the device from holding the ESC at one level for more than ~0.2sec
			if (jitter==1 && j<10) j++;		//Increment jitter counter up to 10 in a row
			else if (jitter==1 && j>=10)
			{
				jitter=0;					//Allow new pulse through after 10 consecutive 'failures'
				j=0;						//Reset counter
			}
			else j=0;						//Reset counter after each 'good' pulse


	//De-glitch if pulses are too long or short
		if (pulse_new > 3000) jitter=1;		//>2.3ms
		if (pulse_new < 1000) jitter=1;		//<0.76ms

		
	//Make ESC go high for same duration
	
		//To 'play back' the same duration, set timer registers to values that cause them to overflow
		//8bit timer registers overflow after reaching '255' so deduct the high pulse count (above) from 255
			if (jitter==0)				//If there is NO jitter/glitch - replay new signal
			{
				tmr1l = 255 - tmr1l;		//Set the Timer1 registers to overflow after same duration as measured
				tmr1h = 255 - tmr1h;
				pir1.0 = 0;					//Reset overflow flag
				
				ESC=1;						//Set ESC high to play back same duration
				t1con.0 = 1;				//Start Timer1
				while(pir1.0==0);			//pir1.0 flag becomes 1 when timer overflows
				t1con.0 = 0;				//Stop Timer1
				ESC = 0;
				
				pulse_good = pulse_new;		//Store this latest 'good' pulse
			}

			else						//If there IS jitter/glitch - replay last 'good' pulse
			{
				LOBYTE(tmr1l, pulse_good);	//Restore previous 'good' pulse
				HIBYTE(tmr1h, pulse_good);
				
				tmr1l = 255 - tmr1l;		//Set the Timer1 registers to overflow
				tmr1h = 255 - tmr1h;
				pir1.0 = 0;					//Reset overflow flag
				
				ESC=1;						//Set ESC high to play back same duration
				t1con.0 = 1;				//Start Timer1
				while(pir1.0==0);			//pir1.0 flag becomes 1 when timer overflows
				t1con.0 = 0;				//Stop Timer1
				ESC = 0;
			}
}

//-----------------------------------------------------------------------------

void run_adc()						//Function to run ADC conversions
{
    //Run ADC
		for(p=0; p<1; p++);				//Delay for acquisition capacitor to u8ge (1 = ~14uS)
		pir1.6 = 0;						//Clear the ADC Complete flag
		adcon0.1 = 1;					//GO: Start ADC acquisition
		while(pir1.6==0);				//Wait for conversion to complete (become 1)
		
	//Store ADC value in 'packvalue' (special BoostC statement to create the 10bit value by merging two registers)
		MAKESHORT(packvalue, adresl, adresh);
}
		
//-----------------------------------------------------------------------------

void pack_values()				//Function to trigger ADC conversions and find pack values
{
    //adcon0 structure...			//1  Right justify ADC values
    								//0  Use Vdd as reference
    								//00 Not used
	   								//10 Input channel (CHS0) is AN2 (THIS PART CHANGES BELOW)
    								//0  ADC capture (GO) not started
    								//1  ADC enabled

	//Determine 1st pack value
	    adcon0 = 0b10001001;		//Makes AN2 channel active
	    run_adc();					//Run ADC conversion
	    div1 = packvalue;			//Copy result to appropriate register
	    
	//Determine 2nd pack value
	    adcon0 = 0b10001101;		//Makes AN3 channel active
	    run_adc();					//Run ADC conversion
	    div2 = packvalue;			//Copy result to appropriate register
}

//-----------------------------------------------------------------------------

void find_lowest()					//Averages 4 measurements and finds lowest pack value
{
	pack1 = 0;							//Initialise
	pack2 = 0;
	
  	for(k=0; k<4; k++)					//Measure pack values 'n' times to smooth results (don't go over 64)
	{
		pack_values();					//Determine pack values (resistor dividers actually)
		pack1 += div1;					//Sum each iteration for each pack
		pack2 += div2;
	}
	
	//Divide by 16 to average results (/4) and use less space/speed up writing to EEPROM memory (/4)
	//A single ADC value can be as high as 966 so needs dividing by 4 to fit into an 8bit memory location
		pack1 = pack1>>4;
		pack2 = pack2>>4;
		
	//Find lowest pack (start with pack 1 and then compare against others)
	   	lowpack = pack1;
	  	if (pack2<lowpack) lowpack = pack2;
}
	   	
//-----------------------------------------------------------------------------
		   	
void cut_throttle()			//Function to cut throttle until acknowledged by pilot
{
	while (ack==0)				//Loop while the acknowledge flag remains 0
								//ie: keep motor off until acknowledged
	{
	//Instructs ESC to stop motor
		ESC=1;					//Start new high pulse
		delay_ms(1);			//for 1ms (= low throttle)
		ESC=0;					//Go low (statements below maintain RX pulse cycle timing of ~20ms)

	//Measure throttle position
		while (RX==0);			//Wait for RX to finish being low (to capture a full high signal)
		t2con.2 = 1;			//Start Timer2 (increments tmr2 to 255 before looping)
								//Tests using delay_ms(): 63=1ms, 126=2ms, 188=3ms
		while (RX==1);			//Wait for RX to finish being high
		t2con.2 = 0;			//Stop Timer2
		
	//Decide whether to accept acknowledgement
		find_lowest();			//Get latest pack values
		
		if (tmr2>53 && tmr2<73 && lowpack>level3) ack=1;
								//Accept as acknowledgement if pulse width count is between 0.8 and 1.1ms
								//	and voltage has risen above minimum
								//Purpose of a range is to reduce likelihood of count being a glitch
		
		tmr2 = 0;				//Initialise Timer2
	}
	
	ack=0;						//Reset the acknowledgement flag
}

//-----------------------------------------------------------------------------

void level_checks()			//Checks voltage thresholds to decide when to warn pilot / cut throttle
{
	
/* Levels 1 and 2 disabled

	if (lowpack<level1 && warn1==0)//At 1st threshold and 1st warning not given before
	{
		cut_throttle();			//Cut throttle until acknowledged
		warn1=1;				//Set warning flag so check not performed again for this level
		return;					//Return to main function (don't want multiple checks for same event)
	}
		
	if (lowpack<level2 && warn2==0)//At 2nd threshold and 2nd warning not given before
	{
		cut_throttle();			//Cut throttle until acknowledged
		warn2=1;				//Set warning flag so check not performed again for this level
		return;					//Return to main function (don't want multiple checks for same event)
	}
*/
		
	if (lowpack<level3)			//Lowest pack reaches lowest threshold
	{
		cut_throttle();			//Cut throttle until acknowledged (every time level 3 is reached)
	}
}

//===========================================================================

void main()					//main program
{
//Main port settings (1=Input 0=Output)
	trisio.1 = 0;				//Sets GP1/AN1 (pin6) to Output (ESC)
	trisio.2 = 1;				//sets GP2/AN2 (pin5) to Input (Pack1)
	trisio.3 = 1;				//sets GP3 (pin4) to Input (RX)
	trisio.4 = 1;				//sets GP4/AN3 (pin3) to Intput (Pack2)
	trisio.5 = 0;				//Sets GP5 (pin2) to Output (LED)

//ADC settings (1=Analog 0=Digital)
	ansel.1 = 0;				//Sets AN1 (pin6) to digital (ESC)
    ansel.2 = 1;				//Sets AN2 (pin5) to analogue (Pack1)
    ansel.3 = 1;				//Sets AN3 (pin3) to analogue (Pack2)
    
    ansel.4 = 1;				//ADCS: Set AD Conversion Clock to Fosc/16
    ansel.5 = 0;				//part of above
    ansel.6 = 1;				//part of above
    pie1.6 = 0;					//ADC interrupts disabled
    
//Comparator settings
    cmcon0 = 0b111;				//Switches all 3 comparator ports off
	
//Static Timer1 settings (to mirror RX pulses)
	t1con=0;					//Initialise entire register and sets timer1 prescaler to 1
	
//Static Timer2 settings (to measure pilot acknowledgements)
	t2con.1=1;					//Sets timer2 prescaler to 16
	
//------------------------------------------------------------------------------

//Initialisation
	ESC=0;						//Start ESC low for safety
   	ack=0;
   	m_timer=0;
   	m_count=0;
   	t_timer=0;
   	t_pos=0;
   	highest_t=0;
   	lowest1=0xFF;
   	lowest2=0xFF;
//	warn1=0;					//Disabled
//	warn2=0;					//Disabled
   	
//Set warning thresholds to suit resistor divider (1k/2k8) and pack voltages (5xA123)
//13v yields 3.40v in prototype (= 696 ADC value) assuming 5v reference (actually 4.95v)
//'find_lowest()' function divides ADC value by 4 so 2.6v threshold is 174 (696/4)
//	level1=?;					//1st warning ?v (disabled)
//	level2=?;					//2nd warning ?v (disabled)
	level3=176;					//3rd warning 2.6v (13v per pack)
	
//Wait for both packs to be connected and ensure they exceed minimum voltage
	find_lowest();				//Measure packs and find lowest
	while (lowpack<level3)		//Flash LED rapidly while waiting
	{
		LED=1;
		delay_ms(125);
		LED=0;
		delay_ms(125);
		
		find_lowest();			//Continue checking if both packs are present
	}

//Static EEPROM data logging settings
    eecon1.3 = 0;				//WRERR: clear on startup in case previously set
	eecon1.2 = 1;				//WREN: enable writing to memory

//Initialise/overwrite EEPROM memory but only if we get past the 'both packs connected' check above
    eeadr = 0;					//Address to be used for first write
	for (p=0; p<255; p++)
	{
		eedata = 0;
		write_data();
	}
    eeadr = 0;					//Address to be used for first write in rest of program
				
//Flash LED three times slowly once ready to start flying
	for (p=0; p<3; p++)
	{
		LED=1;
		delay_ms(250);			//Standard BoostC delay periods can't exceed 255 (8bit)
		delay_ms(250);
		LED=0;
		delay_ms(250);
		delay_ms(250);
	}

//------------------------------------------------------------------------------

//The main program starts here once startup steps have been completed

	while(1)					//Loops in sync with each RX pulse (so statements below must be <20ms)
	{ 
		esc_rx();						//Mirror RX input to ESC output
		find_lowest();					//Measure packs and find lowest

		
	//Check voltage levels and reduce throttle if appropriate
	//To reduce false-triggering, ignore voltages just after throttle has been opened wide
	
		t_pos = pulse_new;				//Pick up latest throttle position
		t_pos = t_pos>>4;				//Divide by 16 to reduce size for writing to memory
		
		if (t_pos>140)					//Throttle ranges 82-160 so 140 is ~75%
		{
			if (t_timer<10)				//Requires throttle to have been high for over 220ms (10 * ~22ms)
			{
				t_timer ++;				//Increment timer each time this is checked (ie: every ~220ms)
			}
			else						//Throttle has been high long enough to avoid 'surge' voltage drops
			{
				level_checks();			//Check voltage thresholds and take appropriate action
			}
		}
		
		else							//When the thottle is not high (no special delay)
		{
			t_timer = 0;				//Reset the 'throttle high' timer
			level_checks();				//Check voltage thresholds and take appropriate action
		}

		
	//Log 256 data records at regular intervals
		if (m_timer<325)			//This 'if' section captures the highest/lowest values between writes
									//It also establishes timing of writes to memory
									//325=~7 seconds (assuming 21.5ms pulse length) = 15min recording
		{
			if (t_pos>highest_t)		//Record highest throttle position between writes
			{
				LOBYTE(highest_t, t_pos);//'t_pos' is 16bit but divide by 16 above fits bottom 8
			}
			
			if (pack1<lowest1)			//Record lowest pack1 value experienced between writes
			{
				LOBYTE(lowest1, pack1);
			}
			
			if (pack2<lowest2)			//Record lowest pack2 value experienced between writes
			{
				LOBYTE(lowest2, pack2);	
			}
			
			m_timer ++;					//Increment timer for each RX pulse cycle
		}
		
		else						//This 'else' section performs the actual writes to memory every 'n' seconds
		{	
			/* Not writing throttle position to memory at this time
			if (m_count<255)			//If there is still space (there are only 256 memory locations)
			{	
				eedata = highest_t;		//Highest throttle position
				write_data();			//Write to memory
				m_count ++;				//Increment number of writes to memory (to prevent over-writing)
			}*/
			
			if (m_count<255)
			{	
				eedata = lowest1;		//Lowest pack1 voltage
				write_data();			//Write to memory
				m_count ++;				//Increment number of writes to memory (to prevent over-writing)
			}
			
			if (m_count<255)
			{	
				eedata = lowest2;		//Lowest pack2 voltage
				write_data();			//Write to memory
				m_count ++;				//Increment number of writes to memory (to prevent over-writing)
			}
			
			highest_t = 0;				//Initialise for reuse
			lowest1 = 0xFF;				//Initialise for reuse
			lowest2 = 0xFF;				//Initialise for reuse
			m_timer = 0;				//Reset timer
		}
	
		
	}							//end while
}								//end main