Thursday, September 25, 2008

main.c file

#include <p18cxxx.h>
#include <delays.h>
#include <timers.h>
#include <stdlib.h>
#include <usart.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
#include <adc.h>
#include <pwm.h>
#include <i2c.h>
#include "Tracker.h"

// config fuse
#pragma config OSC = HSPLL
#pragma config WDT = OFF
#pragma config PWRT = ON
#pragma config LVP = OFF
#pragma config BOREN = ON
#pragma config BORV = 1
#pragma config XINST = OFF

#define pi 3.14159265359

#define LC PORTAbits.RA4 // Led com

#define HEIGHT_MODE_VARIABLE 0
#define HEIGHT_MODE_FIX 1

int PWM1,PWM2;
int S1,S2;
long S1_zero, S2_zero, counter, S1_offset, S2_offset, S1_old, S2_old;
unsigned int last_pos_counter, pos_counter;
char str[6],TXcount,valid_row,i;
char strcom[80];
char height_mode, manual_coords;

long longitude_d, longitude_m, latitude_d, latitude_m, h;
long longitude_dc, longitude_mc, latitude_dc, latitude_mc, hc;
long dx, dy, dz, cos100_longitude, w, w_zero, v, v_zero, dw;
char key_pressed, last_key_pressed;
char buf[20];
char state, sat_in_view0, sat_in_view1, ind_quality;
unsigned char checksum;
char ignore_for_checksum;

#pragma code high_vector=0x08
void interrupt_at_high_vector(void)
{
_asm GOTO ISRgest _endasm
}
#pragma code
#pragma interrupt ISRgest


void main()
{

LATA = 0x00;
LATB = 0x00;
LATC = 0x00;

TRISA = 0b11101111; // all input, RA4 GPIO
TRISB = 0b11111100;   // 6 GPIO (RB7-RB2), 2 Servo (RB0 & RB1)
TRISC = 0b11011000; // I2C, Serial, PWM

ADCON1 = 0b00001111; // ADC off
ADCON2 = 0b10110010; // 16 TAD, Fosc/32
ADCON0 = 0x00; // ADC OFF

flashLED(5);

BAUDCONbits.BRG16 = 1; // baud rate generator a 16 bit
OpenUSART (USART_TX_INT_OFF &
USART_RX_INT_ON &   
USART_ASYNCH_MODE &
USART_EIGHT_BIT &
USART_CONT_RX &
USART_BRGH_HIGH, 1041);

// Timer
OpenTimer0( TIMER_INT_OFF & T0_16BIT & T0_SOURCE_INT & T0_PS_1_1); // servo frame
OpenTimer1(TIMER_INT_ON & T1_PS_1_1 & T1_SOURCE_INT & T1_OSC1EN_OFF); // Servo pulse 1
OpenTimer3(TIMER_INT_ON & T3_PS_1_1 & T3_SOURCE_INT & T3_OSC1EN_OFF ); // Servo Pulse 2


//I2C
OpenI2C(MASTER, SLEW_OFF); // Activate bus I2C, Master mode 100 kbits
SSPADD =0x63;  //100kHz clock(63H) @40MHz (default)

T0CONbits.TMR0ON = 0;
T1CONbits.TMR1ON = 0;
T3CONbits.TMR3ON = 0;

TMR3H=TMR3L=0;
TMR1H=TMR1L=0; 
TMR0H = 0x3C; // 20 ms
TMR0L = 0xB0; 


S1=S2=1500;
S1_offset=S2_offset=0;
S1_old=S2_old=1500;

last_pos_counter=0;
pos_counter=0;
last_key_pressed=0;

valid_row = -2;
TXcount = 0;
i=0;
height_mode = HEIGHT_MODE_VARIABLE;


/* Enable interrupt priority */
RCONbits.IPEN = 1;

/* Make receive interrupt high priority */
IPR1bits.RCIP = 1;

// interrupt
INTCON = 0;
INTCONbits.GIE = 1;
INTCONbits.PEIE = 1;

LCDClearScreen();
LCDprint2((const rom char far *)"Starting....");

Servo();

state=0;
manual_coords = 0;

// wait until GPS has acquired the current position and then turn on the LED. As long as we are in this mode and a key was pressed
// we treat that as signal to store the current GPS position and flash the LED 10 times as visual indicator.
// LED off means GPS is not ready yet
// LED on means GPS has a trusted position
while(i == 0) {
   if (valid_row == -2) {
     LCDClearScreen();
     LCDprint2((const rom char far *)"No Serial Data yet");
     LCDprintc(13);
     LCDprint2((const rom char far *)"program XBee 1: 1");
     LCDprintc(13);
     LCDprint2((const rom char far *)"program XBee 2: 2");
     valid_row = -1;
   } else if ((valid_row != -1) && (state == 0)) {
     LCDClearScreen();
     LCDprint2((const rom char far *)"Serial Data received");
     state=1;
   } else if ((pos_counter != 0) && (ind_quality == '0') && (state != 2)) {
     LCDClearScreen();
     LCDprint2((const rom char far *)"GPS Sentence found, no fix yet");
     state=2;
   } else if ((pos_counter != 0) && (ind_quality != '0') && (state < 3)) {
     LCDClearScreen();
     LCDprint2((const rom char far *)"Press 5 to lock pos");
     LCDprintc(13);
     LCDprint2((const rom char far *)"Satellites: ");
     state = 3;
   }
   key_pressed = LCDKeyboardSingleChar();
   if (key_pressed == '#') {
     EnterCoords();
     state = 2;
   } else if (valid_row == -1) {
     if (key_pressed == '1') {
       ProgramXBee((const rom char far *)"4E3A", (const rom char far *)"4E3B");
     } else if (key_pressed == '2') {
       ProgramXBee((const rom char far *)"4E3B", (const rom char far *)"4E3A");
     }
   }
   if (last_pos_counter != pos_counter || manual_coords) {
     if (manual_coords == 0) GPSParse();
     manual_coords = 0;
     LCDprintc(' ');
     LCDprintc(' ');
     LCDprintc(sat_in_view0);
     LCDprintc(sat_in_view1);
     last_pos_counter = pos_counter;
   }
   if (ind_quality != '0') {
     if (key_pressed == '5') {
       i=1;
     } else {
       LC=1;
     }
   } else {
     LC=0;
   }
}

latitude_d = latitude_dc;
latitude_m = latitude_mc;
longitude_d = longitude_dc;
longitude_m = longitude_mc;
h = hc;



LCDClearScreen();
LCDprint2((const rom char far *)"Waiting for remote  position...");


cos100_longitude = (long)(cos((((double)latitude_d) + ((double)latitude_m)/60000.00) * 3.14159265f / 180.00f)*100.00f);

S1_zero = 1500;
S2_zero = 1500;
w = 9999;
v = 9999;
w_zero = w;
v_zero = v;
counter = 1;

key_pressed=0;

T0CONbits.TMR0ON = 1;
T1CONbits.TMR1ON = 1;
T3CONbits.TMR3ON = 1;


// No we are in tracking and offset mode. You can press the keys and move the servos freely.
// The first time the GPS position is considered trusted and more than 10 meters away, the servos
// start moving automatically. If it is the first time the remote location is valid, we initialize
// the angle offset assuming the current servo position already points to the remote position.
// While the servos try to follow the GPS position you can still press the buttons to move the head manually.
while(1) {
  if ((pos_counter != last_pos_counter) || manual_coords) {
    if (manual_coords == 0) GPSParse();
    manual_coords = 0;
    last_pos_counter = pos_counter;
    if (ind_quality != '0') {
      dx = getDeltaLongitude();
      dy = getDeltaLatitude();
      if (height_mode == HEIGHT_MODE_VARIABLE) {
        dz = hc - h;
      } else {
        dz = 0;
      }

      LCDClearScreen();
      sprintf (buf, "dx=%ld   ", dx/10);
      LCDprint(buf);
      sprintf (buf, "dy=%ld", dy/10);
      LCDprint(buf);
      LCDprintc(13);
      sprintf (buf, "dz=%ld   Sats: ", dz/10);
      LCDprint(buf);
      LCDprintc(sat_in_view0);
      LCDprintc(sat_in_view1);
      LCDprintc(13);
      sprintf (buf, "counter=%d", pos_counter);
      LCDprint(buf);
      LCDprintc(13);

      if ((dx*dx + dy*dy) > 10000) { // the remote position is more than 10m away, we start updating the servo position
        w = approx_atan2(dy, dx);
        v = approx_atan2(dz, isqrt(dy*dy + dx * dx));
        if (w_zero == 9999) {
          w_zero = w;
          v_zero = v;
        }
        S1_old = S1;
        S2_old = S2;
        dw = w_zero-w;
        if (dw > 18000) dw-=36000;
        if (dw < -18000) dw+=36000;
        S1_offset=(long)((dw*86)/1000); // we have degree*100, one degree is about 7us servo impuls
        S2_offset=(long)(((v-v_zero)*86)/1000);
        counter = 1;
        LC=1;
       LCDprint2((const rom char far *)"Tracking...");
      } else {
        LC=0;
      }
    } else {
      LC = 0;
    }
  } else {
    LC=0;
  }

  // instead of setting the new servo position, we let it move slowly towards the S1 = S1_zero + S1_offset position
  if (INTCONbits.TMR0IF) {
    T0CONbits.TMR0ON = 0;
    TMR0H = 0x3C; // 5 ms
    TMR0L = 0xB0; 
    INTCONbits.TMR0IF = 0;
    T0CONbits.TMR0ON = 1;

    key_pressed = LCDKeyboard();
    if (key_pressed == '6') {
      S1_zero++;
      counter = 200; // to make sure we have an immediate movement
    } else if (key_pressed == '4') {
      S1_zero--;
      counter = 200;
    } else if (key_pressed == '2') {
      S2_zero++;
      counter = 200;
    } else if (key_pressed == '8') {
      S2_zero--;
      counter = 200;
    } else if (key_pressed == '5') { // reset pos to center
      S1_zero=1500;
      S2_zero=1500;
      w_zero = w;
      v_zero = v;
      S1_offset = 0;
      S2_offset = 0;
      counter = 200;
    } else if (key_pressed == '0' && last_key_pressed != '0') { // switch height mode
      if (height_mode == HEIGHT_MODE_VARIABLE) {
        height_mode = HEIGHT_MODE_FIX;
        LCDClearScreen();
        LCDprint2((const rom char far *)"Height is constant 0");
      } else {
        height_mode = HEIGHT_MODE_VARIABLE;
        LCDClearScreen();
        LCDprint2((const rom char far *)"Height set variable");
      }
      flashLED(10);
    } else if (key_pressed == '#') {
      EnterCoords();
    }
    if (S1_zero < 900) S1_zero = 900;
    if (S1_zero > 2100) S1_zero = 2100;
    if (S2_zero < 1250) S2_zero = 1250;
    if (S2_zero > 2100) S2_zero = 2100;
    last_key_pressed = key_pressed;


    // Timer0 fires every 0.005s, every one sec we will know the new GPS position.
    // Therefore the servo should make the enitre movement in 200 steps

    S1 = (int)(S1_old - (((S1_old - (S1_zero + S1_offset))*counter)/200));
    if (S1 < 900) S1 = 900;
    if (S1 > 2100) S1 = 2100;

    S2 = (int)(S2_old - (((S2_old - (S2_zero - S2_offset))*counter)/200));
    if (S2 < 1200) S2 = 1200; // the z-Axis can move just little
    if (S2 > 2100) S2 = 2100;

    if ((PIR1bits.TMR1IF == 0) && (PIR2bits.TMR3IF == 0) && ((counter % 4) == 0)) {
      Servo();
    }

    counter++;
    if (counter > 200) {
      counter = 1;
      S1_old = S1_zero + S1_offset;
      S2_old = S2_zero - S2_offset;
      if (S1_old < 900) S1_old = 900;
      if (S1_old > 2100) S1_old = 2100;
      if (S2_old < 1250) S2_old = 1250;
      if (S2_old > 2100) S2_old = 2100;
    }
  }
} // while
}

long approx_atan2(long dist_y, long dist_x) { // returns degree times 100, so the number 6234 means 62.34°
  // Watchout, it is rotated so that north=0°, east=90°, west=-90°
  long rad100;
  long offset;

  if (dist_x == 0) {
    if (dist_y > 0) return 0;
    else return 18000;
  } else {
    rad100 = dist_y*100/dist_x;
    if (rad100 < 0) rad100 = -rad100;

    if (rad100 > 100) {
       // atan = (PI()/2-r/(0,28+r*r))/pi*180
   // offset = ((5700*rad100/(28+(rad100*rad100/100))))/100;
   offset = ((5700*rad100/(28+(rad100*rad100/100))));
    } else {
      offset = 9000-((rad100*570000)/(10000+(28*rad100*rad100/100)));
    }

    if ((dist_x > 0) && (dist_y >= 0)) return offset;
    else if ((dist_x < 0) && (dist_y >= 0)) return -offset;
    else if ((dist_x > 0) && (dist_y < 0)) return 18000-offset;
    else return offset-18000;
  }
}

long isqrt(long x) {
  long   squaredbit, remainder, root;

   if (x<1) return 0;
  
   /* Load the binary constant 01 00 00 ... 00, where the number
    * of zero bits to the right of the single one bit
    * is even, and the one bit is as far left as is consistant
    * with that condition.)
    */
   squaredbit  = (long) ((((unsigned long) ~0L) >> 1) & 
                        ~(((unsigned long) ~0L) >> 2));
   /* This portable load replaces the loop that used to be 
    * here, and was donated by  legalize@xmission.com 
    */

   /* Form bits of the answer. */
   remainder = x;  root = 0;
   while (squaredbit > 0) {
     if (remainder >= (squaredbit | root)) {
         remainder -= (squaredbit | root);
         root >>= 1; root |= squaredbit;
     } else {
         root >>= 1;
     }
     squaredbit >>= 2; 
   }

   return root;
}

void flashLED(char times) {
  for (i=0;i<times;i++) {
    LC=!LC;
    DelayMilliSeconds(500);
  }
}

long getDeltaLatitude(void) { // returns the distance in 0.1m
  // return ((latitude_m - atol(str_latitude_minute))*pi* 6378137/180/60000 + (latitude_d - atol(str_latitude_degree)) * pi * 6378137 / 180) 
  return ((latitude_m - latitude_mc)*186 + (latitude_d - latitude_dc) * 11131949)/100;
}

long getDeltaLongitude(void) { // returns the distance in 0.1m
  // return ((longitude_m - atol(str_longitude_minute))*pi* dist/180/60000 + (longitude_d - atol(str_longitude_degree)) * pi * dist / 180) 
  return ((longitude_m - longitude_mc)*186 + (longitude_d - longitude_dc) * 11131949) * cos100_longitude / 10000;
}

void Servo (void)
{
  TMR1H = (65536 - S1*10) >> 8;
  TMR1L = (65536 - S1*10); 
  LATBbits.LATB0 = 1;
 
  TMR3H = (65536 - S2*10) >> 8;
  TMR3L = (65536 - S2*10);
  LATBbits.LATB1 = 1;

  T1CONbits.TMR1ON = 1;
  T3CONbits.TMR3ON = 1;
}



void ISRgest(void) { // I.S.R.
  char data; // buffer

  if (PIR1bits.TMR1IF) {
    LATBbits.LATB0 = 0;
   
    T1CONbits.TMR1ON = 0; 
    PIR1bits.TMR1IF = 0;
  }
  if (PIR2bits.TMR3IF) {
    LATBbits.LATB1 = 0;
   
    T3CONbits.TMR3ON = 0; 
    PIR2bits.TMR3IF = 0;
  }
  if (PIR1bits.RCIF) {
    data = RCREG;
    if (data == '$') {
      TXcount = 0;
      valid_row = 1;
      checksum = 0;
      ignore_for_checksum = 0;
    } else if (valid_row == 1) {
      if (TXcount == 4) {
        if ((strcom[0] != 'G') || (strcom[1] != 'P') || (strcom[2] != 'G') || (strcom[3] != 'G') || (data != 'A')) {
          valid_row = 0;
        } else {
          strcom[TXcount] = data;
          checksum ^= data;
          TXcount++;
        }
      } else if ((data == '\n') || (data == '\r')) {
        valid_row = 0;
        if ((((strcom[TXcount-2] - '0') << 4) | (strcom[TXcount-1] - '0')) == checksum)
          pos_counter++;
      } else if (data == '*') {
        ignore_for_checksum = 1;
      } else {

        strcom[TXcount] = data;
        if (ignore_for_checksum == 0) {
          checksum ^= data;
        }
        TXcount++;
        if ( TXcount > 79 ) valid_row = 0;
      }

    } else {  // this is used to capture input received, e.g. during XBee programming
      strcom[TXcount] = data;
      TXcount++;
      if ( TXcount > 79 ) valid_row = 79;
    }
   /* Clear the interrupt flag */
    PIR1bits.RCIF = 0;
  }
}


void I2CW(char ADDS, char N1, char d1, char d2, char d3)
{

 // N1..... Bytes to send
 // ADDS... I2C Address
 // d1..... High byte
 // d2..... Low byte
 // d3..... ???

  if (N1 == 1)
  {    
   EEByteWrite(ADDS, 0,d2); // Write byte
   EEAckPolling(ADDS); // attende per ACK
  }

 if (N1 == 2)
  {    
   EEByteWrite(ADDS, d1,d2); // Write byte
   EEAckPolling(ADDS); // attende per ACK
  }

 if (N1 == 3)
  {    
   EEByteWrite(ADDS,d1,d2); // Write byte
   EEAckPolling(ADDS); // attende per ACK
    
   EEByteWrite(ADDS,(d1+1),d2); // Write byte
   EEAckPolling(ADDS); // attende per ACK
  }
}

void LCDprint(char * string)
 {
  char char_pos;
  for (char_pos=0; (string[char_pos] != 0) && (char_pos < 80); char_pos++)
   {
    I2CW(0xc6, 1, 0, string[char_pos], 0);
   }
 }

void LCDprint2(const rom char far * string)
 {
  char char_pos;
  for (char_pos=0; (string[char_pos] != 0) && (char_pos < 80); char_pos++)
   {
    I2CW(0xc6, 1, 0, string[char_pos], 0);
   }
 }

void LCDprintc(char string)
 {
  I2CW(0xc6, 1, 0, string, 0);
 }
void LCDClearScreen()
 {
  I2CW(0xc6, 1, 0, 12, 0); // clear screen
 }

char LCDKeyboard()
 {
  unsigned char let_b[16];
  EESequentialRead(0xC6,1,let_b,2);
  EEAckPolling(0xC6);

    if (let_b[0] & 0b10000000) return '8';
    else if (let_b[0] & 0b01000000) return '7';
    else if (let_b[0] & 0b00100000) return '6';
    else if (let_b[0] & 0b00010000) return '5';
    else if (let_b[0] & 0b00001000) return '4';
    else if (let_b[0] & 0b00000100) return '3';
    else if (let_b[0] & 0b00000010) return '2';
    else if (let_b[0] & 0b00000001) return '1';
    else if (let_b[1] & 0b00000001) return '9';
    else if (let_b[1] & 0b00000100) return '0';
    else if (let_b[1] & 0b00001000) return '#';
//    else if (let_b[1] & 0b00000010) return '*';
    else return 0;
 }

char LCDKeyboardSingleChar() {
  key_pressed = LCDKeyboard();
  if ((key_pressed != 0) && (key_pressed != last_key_pressed)) {
    last_key_pressed = key_pressed;
    return key_pressed;
  } else {
    last_key_pressed = key_pressed;
    return 0;
  }
}

void I2CR()
{
 char ADDS,N1,d1,d2,i;
 unsigned char let_b[16];

 N1 = strcom[3];  // numero byte da leggere
 ADDS = strcom[2];  // I2C Address
 d1 = strcom[4]; // option

 EESequentialRead(ADDS,d1,let_b,N1);
 EEAckPolling(ADDS);

 TXREG = '@';
 while(!TXSTAbits.TRMT); 
 TXREG = 'I';
 while(!TXSTAbits.TRMT); 
 TXREG = ADDS;
 while(!TXSTAbits.TRMT); 

for (i=0;i<N1;i++)
  {
   TXREG = let_b[i];
   while(!TXSTAbits.TRMT); 
  }
}

char GPIO_read(void)
{
 return PORTB;
}

void GPIO_write(char v)
{
 LATB = v;
}


void EnterCoords() {
  char c;
  char pos;
  char bufpos;

  LCDClearScreen();
  LCDprint2((const rom char far *)"Lat: ");

  c = LCDKeyboard();
  while (c == '#') { // if the # key is still pressed, wait until it is released
    DelayMilliSeconds(100);
    c = LCDKeyboard();
  }
  pos = 0;
  bufpos = 0;
  while(c != '*') {
    if (c != 0) {
      if (pos == 0) {
        LCDprintc(c);
        buf[bufpos++] = c;
      } else if (pos == 1) {
        LCDprintc(c);
        buf[bufpos] = c;
        buf[bufpos+1] = 0;
        latitude_dc = atol(buf);
        LCDprintc(' ');
        bufpos = 0;
      } else if ((pos >= 2) && (pos < 7)) {
        buf[bufpos++] = c;
        LCDprintc(c);
        if (pos == 3) LCDprintc('.');
      } else if (pos == 7) {
        LCDprintc(c);
        buf[bufpos] = c;
        buf[bufpos+1] = 0;
        latitude_mc = atol(buf);
        bufpos = 0;
      } else if (pos == 8) {
        if (c == '2') {
          LCDprintc('N');
        } else {
          LCDprintc('S');
          latitude_dc = -latitude_dc;
          latitude_mc = -latitude_mc;
        }
        bufpos = 0;
        LCDprintc(13);
        LCDprint2((const rom char far *)"Lon:");
      } else if ((pos >= 9) && (pos < 11)) {
        LCDprintc(c);
        buf[bufpos++] = c;
      } else if (pos == 11) {
        LCDprintc(c);
        buf[bufpos] = c;
        buf[bufpos+1] = 0;
        longitude_dc = atol(buf);
        LCDprintc(' ');
        bufpos = 0;
      } else if ((pos >= 12) && (pos < 17)) {
        LCDprintc(c);
        buf[bufpos++] = c;
        if (pos == 13) LCDprintc('.');
      } else if (pos == 17) {
        LCDprintc(c);
        buf[bufpos] = c;
        buf[bufpos+1] = 0;
        longitude_mc = atol(buf);
        bufpos = 0;
      } else if (pos == 18) {
        if (c == '6') {
          LCDprintc('E');
          longitude_mc = -longitude_mc;
          longitude_dc = -longitude_dc;
        } else {
          LCDprintc('W');
        }
        LCDprintc(13);
        LCDprint2((const rom char far *)"Height:");
      } else { // Height
        LCDprintc(c);
        buf[bufpos++] = c;
        if (pos == 23) {
          buf[bufpos] = '0';
          buf[bufpos+1] = 0;
          hc = atol(buf);

          ind_quality = '1';
          sat_in_view0='X';
          sat_in_view1='X';
          valid_row = 0;

          manual_coords = 1;
          return;
        }
      }
      pos++;
    }
    c = LCDKeyboardSingleChar();
  }
}


void ProgramXBee(const rom char far * local_address, const rom char far * remote_address) {
  char c;
  LCDClearScreen();
  LCDprint2((const rom char far *)"Are you sure?\r press # for yes");
  c = LCDKeyboardSingleChar();
  while (c == 0) {
    c = LCDKeyboardSingleChar();
  }
  if (c == '#') {
    write_s_UART2((const rom char far *)"Programming...");
    DelayMilliSeconds(4000);
    if (XBeeProcessCommand("+++", 0, 0))
    if (XBeeProcessCommand("ATMY", local_address, 13))
    if (XBeeProcessCommand("ATDH", "0000", 13))
    if (XBeeProcessCommand("ATDL", remote_address, 13))
    if (XBeeProcessCommand("ATWR", 0, 13))
    if (XBeeProcessCommand("ATCN", 0, 13)) {
      LCDprint2((const rom char far *)"XBEE programmed!");
    }
  } else {
    LCDClearScreen();
  }
}

char XBeeProcessCommand(const rom char far * command1, const rom char far * command2, char command3) {
  write_s_UART2(command1);
  if (command2 != 0) {
    write_s_UART2(command2);
  }
  if (command3 != 0) {
    write_c_UART(command3);
  }
  switch (XBeeResponse()) {
    case 1:
      LCDprint2((const rom char far *)"Command ");
      LCDprint2(command1);
      if (command2 != 0) {
        LCDprint2(command2);
      }
      LCDprint2((const rom char far *)" okay\r");
      return 1;
    break;

    case 2:
      LCDprintc(13);
      LCDprint2((const rom char far *)"XBEE said: ERROR");
    break;

    case 0:
      LCDprintc(13);
      LCDprint2((const rom char far *)"XBEE did not respond");
    break;
  }
  return 0;
}

char XBeeResponse() {
  int i;
  DelayMilliSeconds(1200); // check if the XBee responds after one second
  if (TXcount != 0) {
    if (strcom[0] == 'O' && strcom[1] == 'K') {
      TXcount = 0;
      return 1;
    } else if (strcom[0] == 'E' && strcom[1] == 'R' && strcom[2] == 'R' && strcom[3] == 'O' && strcom[3] == 'R') {
      TXcount = 0;
      return 2;
    }
  }
  TXcount = 0;
  return 0;
}

void write_c_UART(char c) {
 TXREG = c;
 while(!TXSTAbits.TRMT); 
}

void write_s_UART(char * c) {
 int pos;
 for(pos=0; (pos < 10) && (c[pos] != 0); pos++) {
   write_c_UART(c[pos]);
 }
}

void write_s_UART2(const rom char far * c) {
  int pos;
  for(pos=0; (pos < 10) && (c[pos] != 0); pos++) {
    write_c_UART(c[pos]);
  }
}

void DelayMilliSeconds(int ms) {
  int x;
  for(x=0; x < ms; x++) {
    Delay10KTCYx(1);
  }
}

void GPSParse() {
  char FLDcounter;
  char curr_pos; 
  char FLDpos;
  curr_pos = 6;
  FLDcounter=1;
  for (curr_pos=6; curr_pos < 76; curr_pos++) {
    if (strcom[curr_pos] == ',') {
      switch (FLDcounter) {
        case 2:
          buf[2] = strcom[curr_pos-4];
          buf[3] = strcom[curr_pos-3];
          buf[4] = strcom[curr_pos-2];
          buf[5] = strcom[curr_pos-1];
          buf[6] = 0;
          latitude_mc = atol(buf);
        break;
        case 3:
          if (strcom[curr_pos-1] == 'S') {
            latitude_dc = -latitude_dc;
            latitude_mc = -latitude_mc;
          }
        break;
        case 4:
          buf[2] = strcom[curr_pos-4];
          buf[3] = strcom[curr_pos-3];
          buf[4] = strcom[curr_pos-2];
          buf[5] = strcom[curr_pos-1];
          buf[6] = 0;
          longitude_mc = atol(buf);
        break;
        case 5:
          if (strcom[curr_pos-1] == 'E') {
            longitude_dc = -longitude_dc;
            longitude_mc = -longitude_mc;
          }
        break;
        case 6:
          ind_quality = strcom[curr_pos-1];
        break;
        case 7:
          sat_in_view0 = strcom[curr_pos-2];
          sat_in_view1 = strcom[curr_pos-1];
        break;
        case 9:
          if (FLDpos < 10) {
            buf[FLDpos] = '0';
            buf[FLDpos+1] = 0;
          }
          hc = atol(buf);
          curr_pos = 99; // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! exit loop !!!!!!!!!!!!!!!!!!!
        break;
      }
      FLDcounter++;
      FLDpos = 0;
    } else if (strcom[curr_pos] == '.') {
      switch (FLDcounter) {
        case 2:
          buf[0] = strcom[curr_pos-4];
          buf[1] = strcom[curr_pos-3];
          buf[2] = 0;
          latitude_dc = atol(buf);

          buf[0] = strcom[curr_pos-2];
          buf[1] = strcom[curr_pos-1];
        break;
        case 4:
          buf[0] = strcom[curr_pos-5];
          buf[1] = strcom[curr_pos-4];
          buf[2] = strcom[curr_pos-3];
          buf[3] = 0;
          longitude_dc = atol(buf);

          buf[0] = strcom[curr_pos-2];
          buf[1] = strcom[curr_pos-1];
        break;
        case 9:
          buf[FLDpos] = strcom[curr_pos+1];
          buf[FLDpos+1] = 0;
          FLDpos = 10;
        break;
      }
      FLDpos++;
    } else if (FLDcounter == 9) {
      buf[FLDpos] = strcom[curr_pos];
      FLDpos++;
    }
  }
}
Post a Comment