//LVC1
//Low Voltage Cutoff program to find lowest individual cell in a three cell pack and warn pilot
//Simple data logging (can be downloaded to PC)

//LVC program can have three thresholds but for most one is best
//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 cell reaches min

//Thresholds 1 and 2 (disabled) require only one acknowledgement each (allows voltages to then drift up and down)
//These are intended for early warnings if desired

//The program determines the throttle position and cell voltages every ~20ms (in sync with the RX pulse)
//Every 7 seconds the highest throttle position and lowest cell voltage (since last write) is stored in memory
//This gives about 15minutes flight recording
//Flight data must start being downloaded while LED flashes on bootup (otherwise it's over-written)

//Written for PIC12F683 (8 pin) / BoostC compiler
//Settings chosen for Multiplex radio

//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 RX 		gpio.3			//GP3 (Pin4) is Receiver
#define LED 	gpio.4			//GP4 (Pin3) is LED
#define ESC 	gpio.5			//GP5 (Pin2) is ESC

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

u16 cellvalue;					//Used to determine individual cell ADC values
u16 div1;						//The value of resistor dividers (eg: div3 is cells 1, 2 and 3)
u16 div2;					
u16 div3;					

u8 k;							//Counter for multiple cell readings
u16 cell1;						//The value of cell 1, 2, 3 ...
u16 cell2;					
u16 cell3;					
u16 lowcell;					//Value of lowest cell 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 lowest_c;					//Lowest cell 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.0 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 a 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
		//The 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 width
			{
				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 ADCVALUE (special BoostC statement to create the 10bit value by merging two registers)
		MAKESHORT(cellvalue, adresl, adresh);
}
		
//-----------------------------------------------------------------------------

void cell_values()				//Function to trigger ADC conversions and find cell 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 resistor divider value (= cell 1)
	    adcon0 = 0b10000001;		//Makes AN0 channel active
	    run_adc();					//Run ADC conversion
	    div1 = cellvalue;			//Copy result to appropriate register
	    
	//Determine 2nd resistor divider value (= cells 1 and 2)
	    adcon0 = 0b10000101;		//Makes AN1 channel active
	    run_adc();					//Run ADC conversion
	    div2 = cellvalue;			//Copy result to appropriate register
	    
	//Determine 3rd resistor divider value (= cells 1, 2 and 3)
	    adcon0 = 0b10001001;		//Makes AN2 channel active
	    run_adc();					//Run ADC conversion
	    div3 = cellvalue;			//Copy result to appropriate register
}

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

void find_lowest()					//Averages 4 measurements and finds lowest cell value
{
	cell1 = 0;							//Initialise
	cell2 = 0;
	cell3 = 0;
	
  	for(k=0; k<4; k++)					//Measure cell values 'n' times to smooth results (don't go over 64)
	{
		cell_values();					//Determine cell values (resistor dividers actually)
		cell1 += div1;					//Sum each iteration for each cell
		cell2 += div2;
		cell3 += div3;
	}
	
	//Determine individual cell values
		cell3 = cell3 - cell2;
		cell2 = cell2 - cell1;
		
	//Divide by 4 to average results and use less space/speed up writing to EEPROM memory
		cell1 = cell1>>2;
		cell2 = cell2>>2;
		cell3 = cell3>>2;
		
	//Find lowest cell (start with Cell 1 and then compare against others)
	   	lowcell = cell1;
	  	if (cell2<lowcell) lowcell = cell2;
	   	if (cell3<lowcell) lowcell = cell3;
}
	   	
//-----------------------------------------------------------------------------
		   	
void cut_throttle()		//Function to cut throttle until acknowledged by pilot

						//1ms is generally accepted as 'low' throttle and 2ms 'high'
						//Multiplex measured 1.055ms / 2.138ms on 'MPX'
						//  and 0.951 / 2.033ms on 'Other' setting
						
{
	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 cell values
		
		if (tmr2>53 && tmr2<76 && lowcell>level3) ack=1;
								//Accept as acknowledgement if pulse width count is between 0.8
								//	 and 1.2ms and voltage has risen above minimum
								//Using a range reduces 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 (lowcell<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 (lowcell<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 (lowcell<level3)			//Lowest cell reaches lowest threshold
	{
		cut_throttle();			//Cut throttle until acknowledged (every time level 3 is reached)
	}
}

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

void main()					//main program
{
//Main port settings
	trisio.0 = 1;				//Sets GP0/AN0 (pin7) to Input (Cell1)
	trisio.1 = 1;				//Sets GP1/AN1 (pin6) to Input (Cell2)
	trisio.2 = 1;				//sets GP2/AN2 (pin5) to Input (Cell3)
	trisio.3 = 1;				//sets GP3 (pin4) to Input (RX)
	trisio.4 = 0;				//sets GP4/AN3 (pin3) to Output (Led)
	trisio.5 = 0;				//Sets GP5 (pin2) to Output (ESC)
    
	ansel.0 = 1;				//Sets AN0 (pin7) to analogue (Cell1)
	ansel.1 = 1;				//Sets AN1 (pin6) to analogue (Cell2)
    ansel.2 = 1;				//Sets AN2 (pin5) to analogue (Cell3)
    ansel.3 = 0;				//Sets AN3 (pin3) to digital
    
    cmcon0 = 0b111;				//Switches all 3 comparator ports off
	
//Static ADC settings
    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
    
//Initialise ESC etc.
	ESC=0;						//Start ESC low for safety
   	tmr2=0;
	ack=0;
   	m_timer=0;
   	m_count=0;
   	t_timer=0;
   	t_pos=0;
   	highest_t=0;
   	lowest_c=0xFF;
//	warn1=0;					//Disabled
//	warn2=0;					//Disabled
   	
//Static Timer1 settings (to mirror RX pulses)
	t1con=0;					//Initialise entire register (not automatic on resets)
								//This also sets prescaler to 1
	
//Static Timer2 settings (to measure pilot acknowledgements)
	t2con.1=1;					//Sets prescaler to 16
	
//Static EEPROM data logging settings
    eecon1.3 = 0;				//WRERR: clear on startup in case previously set
	eecon1.2 = 1;				//WREN: enable writing to memory
    eeadr = 0;					//Address to be used for first write

//Set warning thresholds to suit resistor divider (15k/43k) and cell voltages (lipo)
//	level1=161;					//1st warning 3.0v (disabled)
//	level2=155;					//2nd warning 2.9v (disabled)
//	level3=150;					//3rd warning 2.8v (higher setting being used below)
	level3=161;					//The only warning enabled 3.0v

//Flash LED three times on startup	
//Download of flight data must start in this 3sec window to avoid being over-written
	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);
	}

//Initialise EEPROM				//This overwrites memory so start download before LEDs stop flashing!!!
	for (p=0; p<255; p++)
	{
		eedata = 0;
		write_data();
	}
    eeadr = 0;					//Address to be used for first write in rest of program
				
//-----------------------------------------------------------------------------

	while(1)					//Loop in sync with each RX pulse (so statements below must be <20ms)
	{ 
		esc_rx();						//Mirror RX input to ESC output
		find_lowest();					//Measure cells 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 throttle position
		t_pos = t_pos>>4;				//Divide by 16 to reduce size for writing to memory
		
		if (t_pos>158)					//Futaba FF9 throttle ranges 82-160; Multiplex 95-180
										//So 75% throttle for Futaba is 140 and MPX 158
		{
			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 (only while RX pulse is being received)
		if (m_timer<325)				//Establish timing of writes and traps highest/lowest while waiting
										//325=~7 seconds (assuming 21.5ms pulse length) = 15min recording
		{
			if (t_pos>highest_t)
			{
				LOBYTE(highest_t, t_pos);//Record highest throttle position between writes
										 //'t_pos' is 16bit but divide by 16 above fits bottom 8
			}
			if (lowcell<lowest_c)
			{
				LOBYTE(lowest_c, lowcell);//Record lowest cell value experienced between writes
										  //'lowcell' is 16bit but size reduced to 8 in find_lowest()
			}
			m_timer ++;					//Increment timer for each RX pulse cycle
		}
		
		else
		{	
			if (m_count<255)
			{	
				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 = lowest_c;		//Lowest cell voltage
				write_data();			//Write to memory
				m_count ++;				//Increment number of writes to memory (to prevent over-writing)
			}
			highest_t = 0;				//Initialise for reuse
			lowest_c = 0xFFFF;			//Initialise for reuse
			m_timer = 0;				//Reset timer
		}
	
		
	}							//end while
}								//end main