skip to main |
skip to sidebar
#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++;
}
}
}
