Reverse geocache (pic18)

gistfile1.txt

#include <pic18.h>
#include <htc.h>
#include <peripheral/usart.h>
#include <string.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>

#include "delay.h"

// Store the attempt count in EEPROM (set to zero at program time)
__EEPROM_DATA(0, 0, 0, 0, 0, 0, 0, 0);
volatile char attempt;

#define PI_DIV_180 0.017453292519943

// Target 1
//#define TARGET_LAT 0.77712569059394
//#define TARGET_LON 2.154014240644383

// Creative crafts
//#define	TARGET_LAT 0.77801065009489
//#define TARGET_LON 2.151564903749776

// MCA Denver
#define	TARGET_LAT 0.693805134301793
#define TARGET_LON 1.832669472208235

#define RAD_2_MILE 3956.088338123168

#define LCD_PORT	TRISA
#define LCD_TRIS	PORTA
#define LCD_CS		0x10
#define LCD_RS		0x20

void
lcd_send(char byte, char reg)
{
	PORTA = 0;
	// If reg is set, it's a command
	if (!reg) PORTA |= LCD_RS;
	NOP();
	
	// Data pins are Latched on the falling edge, setup on the rising
	PORTA = LATA | LCD_CS;
	
	// The LCD module accepts high-order pins first
	PORTA = LATA | (byte >> 4) & 0x0F;
	DelayUs(10);
	
	PORTA = LATA & ~LCD_CS;	// Latch the data in;
	DelayUs(10);

	PORTA = LATA | LCD_CS;	// Get ready for the second nibble
	PORTA = LATA & 0xF0;	// Mask out data
	DelayUs(10);

	PORTA = LATA | (byte & 0x0F);
	DelayUs(10);
	
	// Set the CS pin low
	PORTA = LATA & ~LCD_CS;
	DelayUs(10);
	
	// Return to idle
	PORTA = 0;
}

void
lcd_send4(char nibble, char reg)
{
	PORTA = 0;
	// If reg is set, it's a command
	if (!reg) PORTA = LATA | LCD_RS;
	DelayUs(10);
	
	// Data pins are Latched on the falling edge, setup on the rising
	PORTA = LATA | LCD_CS;
	
	// Send only the single nibb;e
	PORTA = LATA | (nibble & 0x0F);
	DelayUs(10);

	// Set the CS pin low
	PORTA = LATA & ~LCD_CS;
	DelayUs(10);
	
	// Return to idle
	PORTA = 0;
}

void
lcd_clear()
{
	lcd_send( 0x01, 1);
	DelayMs(5);
}

void
lcd_home()
{
	lcd_send( 0x02, 1);
	DelayMs(5);
}

void
lcd_init()
{
	// LCD is on port A
	// Port A initialization from datasheet
	PORTA = 0;
	ADCON1 = 0x07;
	TRISA = 0x00;

	// Begin LCD reset function
	lcd_send4(0x03, 1);
	DelayMs(5);
	
	lcd_send4(0x03, 1);
	DelayUs(100);
	
	lcd_send4(0x03, 1);

	// Set 4 bit mode
	lcd_send4(0x02, 1);

	// Set function and start display
	lcd_send( 0x28, 1);
	lcd_send( 0x08, 1);
	lcd_send( 0x01, 1);
	lcd_send( 0x06, 1);

	// Clear the screen and set the cursor home
	lcd_clear();
	lcd_home();
	
	// Enable the display
	lcd_send(0x0C, 1);
}

void lcd_position(char line, char col)
{
	if (line == 1) {
		lcd_send(0x80 + col, 1);
	} else if (line == 2) {
		lcd_send(0x80 + 0x40 + col, 1);
	}	
}
	
void
lcd_blink()
{
	lcd_send(0x0F, 1);
}

void
lcd_off()
{
	lcd_send(0x08, 1);
}

void
lcd_on()
{
	lcd_send(0x0C, 1);
}

void
lcd_print(char line, const char *buffer)
{
	char i;
	
	lcd_position(line, 0);
	
	for (i = 0; i < 16 && buffer[i] != '\0'; i++) {
		lcd_send(buffer[i], 0);
		DelayMs(5);
	}
}


#define SERVO_PIN 0x08
void
servo_position(char degrees)
{
	static char last_position = 0;
	char i, j;
	
	// Basic limit checking
	if (degrees > 10) {
		degrees = 10;
	}
	
	for (i = 0; i < 100; i++) {
		PORTB |= SERVO_PIN;
		DelayUs(500);
		
		for (j = 0; j < degrees; j++) {
			DelayUs(30);			
		}
		
		PORTB &= ~SERVO_PIN;
		DelayMs(10);
	}
}

#define ON_TRIS	TRISC
#define ON_PORT	PORTC
#define ON_PIN	0x10

char buffer[256];

void DelayS(char seconds)
{
	char i;
	for (i = 0; i < seconds; i++) {
		DelayMs(200);
		DelayMs(200);
		DelayMs(200);
	}
}
/*
char
read_blocking()
{
	char byte;
	while (!DataRdyUSART()) {
		if( RCSTA & 0x02 ) {
			RCSTA = 0x00;
			RCSTA = 0x90;
		}
	}
	byte = ReadUSART();
	TXREG = byte;
	return byte;
}
*/
char
read_blocking_noprint()
{
	char byte;
	while (!DataRdyUSART()) {
		if( RCSTA & 0x02 ) {
			RCSTA = 0x00;
			RCSTA = 0x90;
		}
	}
	byte = ReadUSART();
	return byte;
}

void process_location()
{
	// We'll want to wait for a little while,
	// until the solution is a little more stable.
	static char samples = 0;

	char  lat_degrees;
	char  lon_degrees;
	char  lat_hemisphere;
	char  lon_hemisphere;
	float lat_minutes;
	float lon_minutes;
	
	float lat_radians;
	float lon_radians;
	
	char byte;
	
	samples += 1;
		
	lcd_position(2, 15);
	lcd_send(samples + '0', 0);

	// Once we've received 20 samples, compute a solution
//	if (samples >= 20) {
	if (1) {
	
		// Ignore the ','
		byte = read_blocking_noprint();

		// Get the whole latitude degrees
		byte = read_blocking_noprint();
		lat_degrees  = (byte - '0') * 10;
		byte = read_blocking_noprint();
		lat_degrees += (byte - '0');
				
		// Get the whole, then fractional latitude minutes
		byte = read_blocking_noprint();
		lat_minutes  = (byte - '0') * 10;
		byte = read_blocking_noprint();
		lat_minutes += (byte - '0');
		
		// Ignore the decimal place
		byte = read_blocking_noprint();
		// But make sure it's there
		if (byte != '.') return;		
		
		// Get the fractional latitude minutes
		// If any isn't a number we need to quit processing characters
		byte = read_blocking_noprint();
		if (byte >= '0' &&
			byte <= '9') {
			lat_minutes += ((float)(byte - '0')) / 10.;

			byte = read_blocking_noprint();
			if (byte >= '0' &&
				byte <= '9') {
				lat_minutes += ((float)(byte - '0')) / 100.;

				byte = read_blocking_noprint();
				if (byte >= '0' &&
					byte <= '9') {
					lat_minutes += ((float)(byte - '0')) / 1000.;

					byte = read_blocking_noprint();
					if (byte >= '0' &&
						byte <= '9') {
						lat_minutes += ((float)(byte - '0')) / 10000.;
					}
				}
			}
		}

		// Scan past the next ','
		while (byte != ',') {
			byte = read_blocking_noprint();
		}
		
		byte = read_blocking_noprint();

		// Quick sanity check
		// If this fails, bail out
		lat_hemisphere = byte;
		if (lat_hemisphere != 'N' &&
			lat_hemisphere != 'S') {
			return;
		}
	
		// Scan past the next ','
		do {
			byte = read_blocking_noprint();
		} while (byte != ',');
			
		// Get the whole longitude degrees
		byte = read_blocking_noprint();
		lon_degrees  = (byte - '0') * 100;
		byte = read_blocking_noprint();
		lon_degrees += (byte - '0') * 10;
		byte = read_blocking_noprint();
		lon_degrees += (byte - '0');

		// Get the whole, then fractional longitude minutes
		byte = read_blocking_noprint();
		lon_minutes  = (byte - '0') * 10;
		byte = read_blocking_noprint();
		lon_minutes += (byte - '0');
		
		// Ignore the decimal place
		byte = read_blocking_noprint();
		// But make sure it's there
		if (byte != '.') return;		

		// Get the fractional longitude minutes
		// If any isn't a number we need to quit processing characters
		byte = read_blocking_noprint();
		if (byte >= '0' &&
			byte <= '9') {
			lon_minutes += ((float)(byte - '0')) / 10.;

			byte = read_blocking_noprint();
			if (byte >= '0' &&
				byte <= '9') {
				lon_minutes += ((float)(byte - '0')) / 100.;
				
				byte = read_blocking_noprint();
				if (byte >= '0' &&
					byte <= '9') {
					lon_minutes += ((float)(byte - '0')) / 1000.;

					byte = read_blocking_noprint();
					if (byte >= '0' &&
						byte <= '9') {
						lon_minutes += ((float)(byte - '0')) / 10000.; 
					}
				}
			}
		}
		
		// Scan past the next ','
		while (byte != ',') {
			byte = read_blocking_noprint();
		}
		
		byte = read_blocking_noprint();
		
		// Quick sanity check
		// If this fails, bail out
		lon_hemisphere = byte;
		if (lon_hemisphere != 'E' &&
			lon_hemisphere != 'W') {
			return;
		}
		
		// Fake LAX numbers
//		lat_hemisphere = 'N';
//		lon_hemisphere = 'W';
//		lat_degrees = 33;
//		lon_degrees = 118;
//		lat_minutes = 57;
//		lon_minutes = 24;
	
		// Print the derrived lat/lon to the LCD
		sprintf(buffer, " %c  %d %2.4f  ",
				lat_hemisphere, lat_degrees, lat_minutes);
		lcd_print(1, buffer);
		sprintf(buffer, " %c %d %2.4f  ",
				lon_hemisphere, lon_degrees, lon_minutes);
		lcd_print(2, buffer);

		DelayS(5);
	
		// Now that we have all the raw data, Convert it to radians.
		// Using the fake values for LAX, should equal:
		// lat_radians = ( 33 + (57/60) * pi/180 = 0.592539
		// lon_radians = (118 + (24/60) * pi/180 = 2.066470
		lat_radians = (lat_degrees + (lat_minutes/60)) * PI_DIV_180;
		lon_radians = (lon_degrees + (lon_minutes/60)) * PI_DIV_180;
	
//		sprintf(buffer, "  %1.10f  ", lat_radians);
//		lcd_print(1, buffer);
//		sprintf(buffer, "  %1.10f  ", lon_radians);
//		lcd_print(2, buffer);
	
		// Calculate the distance
		float distance;
		float lat_sin = sin((lat_radians - TARGET_LAT)/2);
		float lon_sin = sin((lon_radians - TARGET_LON)/2);
		distance = 2 * asin( sqrt((lat_sin * lat_sin) +
								  cos(lat_radians) *
								  cos(TARGET_LAT) *
								  (lon_sin * lon_sin)));
	
		// Convert the distance from radians to miles
		distance = distance * RAD_2_MILE;
	
		// Print the distance
		lcd_print(1,    "    Distance    ");
		sprintf(buffer, " %8.4f miles ", distance);
		lcd_print(2, buffer);
	
		DelayS(5);
	
		if (distance < .1) {
			lcd_print(1, "Congratulations!");
			lcd_print(2, "Present unlocked");
			EEPROM_WRITE(1, 1);
			DelayS(5);
			lcd_off();
			servo_position(100);
		} else {
			lcd_print(1, " Sorry, too far ");

			attempt += 1;
			lcd_print(2, "Attempt    of 50");
			lcd_position(2, 8);
			lcd_send( (attempt / 10) + '0', 0);
			lcd_send( (attempt % 10) + '0', 0);			

			EEPROM_WRITE(0, attempt);
			DelayS(5);
		}

		PORTC = 0x00;
		while (1);
	}
}

int
main(void)
{	
	int seconds = 0;

	// Turn on the USART to read from the GPS
	// We need to do this first because it changes TRISC settings
	OpenUSART(USART_ASYNCH_MODE &
			  USART_EIGHT_BIT &
			  USART_CONT_RX &
			  USART_BRGH_HIGH,
			  51); 	// 4800 baud
	
	// Set the "on" pin high
	PORTC =  0x10;
	TRISC =  0xAF;	

	// Read the completion flag, power down if already unlocked
	attempt = EEPROM_READ(1);
	if (attempt) {
		// Start the LCD
		lcd_init();

		lcd_off();
		
		// Run the servo briefly to make sure it's closed
		TRISB = ~SERVO_PIN;
		servo_position(100);

		lcd_on();
		
		// Print message
		lcd_print(1, "Present unlocked");
		lcd_print(2, " Powering down. ");

		// Wait and power down
		DelayS(5);

		PORTC = 0x00;
		while(1);
	} else {
		// Start the LCD
		lcd_init();
		
		lcd_off();
		
		// Run the servo briefly to make sure it's closed
		TRISB = ~SERVO_PIN;
		servo_position(0);
		
		lcd_on();
		
		// Print a message to the screen
		lcd_print(1, "Acquiring Signal");
		lcd_print(2, "Please wait. (0)");				
	}

	// Read the attempt counter
	attempt = EEPROM_READ(0);
	
	// The outer loop looks for the '$' character
	// It's purpose is to find sentence beginnings
	while(1) {
		char byte;
		char i;
	
		// Wait for a serial byte
		byte = read_blocking_noprint();
			
		// We're in this during the entire duration of the sentence
		if (byte == '$') {

			// Copy the 5 character sentance name
			for (i = 0; i < 5; i++) {
				byte = read_blocking_noprint();
				buffer[i] = byte;
			}
			
			// Add the null terminator
			buffer[5] = '\0';
			
			// Compare to GPRMC (recommended minimum information)
			// This is the only sentence we care about
			if (strcmp(buffer, "GPRMC") == 0) {

				// The dummy read swallows the ','
				byte = read_blocking_noprint();

				// Next, we wait for the next ',', skipping the time
				do {
					byte = read_blocking_noprint();
				} while (byte != ',');
				
				// This byte is very important.  If it's 'A' we have a fix
				// If it's V, we don't yet.
				byte = read_blocking_noprint();
				if (byte == 'A') {
					process_location();
				}
				
				seconds += 1;
				
				char minutes = seconds / 60;
				// After 10 mintes, shutdown no matter what
				if (minutes == 10) {
					attempt += 1;
					EEPROM_WRITE(0,attempt);
					lcd_print(1, "No signal found.");
					lcd_print(2, "Attempt    of 50");
					lcd_position(2, 8);
					lcd_send( (attempt / 10) + '0', 0);
					lcd_send( (attempt % 10) + '0', 0);
					DelayS(5);
					PORTC = 0x00;
					while (1);
				} else {
					lcd_position(2, 14);
					lcd_send(minutes + '0', 0);
				}
			}			
		
		}
	}

	return 0;
}