Jappe Lino
interface towards a Garmin plotter to make measurements come up as boats
Diff: main.cpp
- Revision:
- 0:70a1315acc03
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Jul 05 08:16:13 2011 +0000
@@ -0,0 +1,807 @@
+#include "mbed.h"
+#include "SerialBuffered.h"
+#include "garmin_test.h"
+#include "MODSERIAL.h"
+
+#define IDLE 0
+#define SEARCH_START 1
+#define CAPTURE 2
+#define FETCH_DATA 3
+
+#define U0RBRTHR 0x4000C000
+#define U0DLL U0RBRTHR
+#define U0RBR U0RBRTHR
+#define U0THR U0RBRTHR
+#define U0FCR 0x4000C008
+#define U0IER 0x4000C004
+#define U0DLM U0IER
+
+
+#define U0LCR 0x4000C00C
+#define U0LSR 0x4000C014
+#define U0SCR 0x4000C01C
+#define U0ACR 0x4000C020
+#define U0FDR 0x4000C028
+#define U0TER 0x4000C030
+#define U0FIFOLVL 0x4000C058
+
+#define CLKOUTCFG 0x400FC1C8
+#define PINSEL3 0x4002C00C
+
+
+DigitalOut mled0(LED1);
+DigitalOut mled1(LED2);
+DigitalOut mled2(LED3);
+DigitalOut mled3(LED4);
+
+ MODSERIAL bg(p9,p10);
+ MODSERIAL bg2(p13,p14);
+ //Serial pc(USBTX, USBRX); // tx, rx
+
+
+static volatile char garmin_buffer[50];
+static char ais_2_garmin[]={'!','A','I','V','D','M',',','1',',',',','1',',','A',',','1','3','u','?','e','t','P','v','2',';',\
+ '1','B','J','d','D','Q','t','U','M','1','U','1','C','b','0','6','9','D',',','0','*','7','B',0
+ }; int i;
+int item;
+
+#define Z_SIZE 50
+static char Zigbee_buffer[Z_SIZE];
+static int p_z = 0;
+
+int analyze_buffer(void);
+void packet_data(void);
+void print_bg(char *message);
+void debug_print(char *message);
+int iChecksum( char* message );
+void packet_data_no_printf(void);
+void print_pc(char *message);
+void rxCallback(void);
+void Rx_int();
+void rxCallback_Zigbee(void);
+void int_to_speed(int speed);
+void ais_crc(void);
+void adjust_boat_east(void);
+int find_solar_amphour(void);
+int find_fridge_amphour(void);
+int string_2_int(char *p_s);
+void init_usb_serial(void);
+
+volatile int p_rx=0;
+volatile int rec_state = SEARCH_START;
+volatile int ais_ready=0;
+int gi;
+int b;
+int speed;
+char *p_f;
+int z_sequence;
+
+
+char db[2];
+ static volatile char Rx_buffer[2000];
+ char crlf[]={0x0A,0x0D,0};
+
+int main() {
+
+
+ extern volatile int p_rx;
+ extern volatile char Rx_buffer[2000];
+ int p_s,p_f;
+ int degrees = 0x30;
+
+
+ p_s = 0;
+ p_f = 0;
+
+ z_sequence = 0;
+
+ init_usb_serial();
+
+ bg2.baud(38400);
+ bg.baud(38400);
+
+ bg.attach(&rxCallback_Zigbee, MODSERIAL::RxIrq);
+ bg2.attach(&rxCallback, MODSERIAL::RxIrq);
+
+ print_pc("tjosan");
+ print_pc(&crlf[0]);
+
+
+ for(;;){
+
+ while(ais_ready == 0); // wait for interrupt routine to fill up data
+
+ ais_ready = 0;
+
+ mled3 = 1;
+
+ b++;
+
+ ais_2_garmin[35] = degrees++;
+
+ int_to_speed(speed++);
+
+ if(speed > 170)
+ speed = 0;
+
+ if(degrees > 0x33)
+ degrees = 0x30;
+
+ if(b>2){
+
+ mled3 = 0;
+
+ b = 0;
+
+ }
+ ais_2_garmin[15] = 0x33;
+
+ print_pc(&Zigbee_buffer[0]);
+ print_pc(&crlf[0]);
+
+
+ if(p_s != 0){
+
+ int_to_speed(p_s);
+
+ ais_crc();
+ __disable_irq();
+ print_bg(&ais_2_garmin[0]);
+ __enable_irq();
+ print_pc(&ais_2_garmin[0]);
+ print_pc(&crlf[0]);
+
+ }
+
+ if(p_f != 0){
+
+ adjust_boat_east(); // prepere for the next "boat"
+
+ ais_2_garmin[15] = 0x34; // change boat identity
+
+ int_to_speed(p_f);
+
+ ais_crc();
+ __disable_irq();
+ print_bg(&ais_2_garmin[0]);
+ __enable_irq();
+ print_pc(&ais_2_garmin[0]);
+ print_pc(&crlf[0]);
+
+ }
+
+ if( z_sequence == 2){
+
+ p_s = find_solar_amphour();
+
+ p_f = find_fridge_amphour();
+
+ }
+
+ z_sequence++;
+ if(z_sequence > 2){
+
+ z_sequence = 0;
+
+ for(i = 0 ; i < Z_SIZE; i++) // erase data in zigbee buffer
+ Zigbee_buffer[i] = 0;
+
+ }
+ }
+
+ }
+void init_usb_serial(void){
+
+ int *p_reg;
+
+ p_reg = (int *) U0LCR;
+ *p_reg = 0x83;
+ p_reg = (int *) U0FCR;
+ *p_reg = 0x01;
+ p_reg = (int *) U0DLL;
+ *p_reg = 832;
+ p_reg = (int *) U0DLM;
+ *p_reg = 0;
+ p_reg = (int *) U0FDR;
+ p_reg = (int *) U0LCR;
+ *p_reg = (*p_reg) & 0x7F;
+
+}
+
+int string_2_int(char *p_s){
+
+int i;
+
+ i = (*p_s++ - 0x30) * 10;
+
+ i = i + (*p_s - 0x30);
+
+ return(i);
+
+}
+
+int find_fridge_amphour(void){
+
+ char *p_b;
+
+ int a;
+
+ int i = 0;
+
+ p_b = &Zigbee_buffer[0];
+
+ while(*p_b != 'A' && i < 50) // search for the identifying character
+ p_b++;
+
+ if( i < 50){
+
+ a = (*(p_b + 7) - 0x30) * 100 + (*(p_b + 8) - 0x30) * 10;
+
+ }
+ else
+ a = 0;
+
+ return(a);
+}
+int find_solar_amphour(void){
+
+ char *p_b;
+
+ int a;
+
+ int i = 0;
+
+ p_b = &Zigbee_buffer[0];
+
+ while(*p_b != 'D' && i < 50) // search for the identifying character
+ p_b++;
+
+ if( i < 50){
+
+ a = (*(p_b + 7) - 0x30) * 100 + (*(p_b + 8) - 0x30) * 10;
+
+ }
+ else
+ a = 0;
+
+ return(a);
+}
+
+void adjust_boat_east(void){
+
+ char a;
+
+ a = ais_2_garmin[26];
+
+ a = a + 4;
+
+ if(a > 87 && a < 98)
+ a = a + 8;
+
+ if(a > 119){
+
+ ais_2_garmin[25]++;
+
+ a = 0x30;
+
+ }
+
+ ais_2_garmin[26] = a;
+
+
+}
+
+void int_to_speed(int speed){
+
+int low_part,high_part;
+
+ high_part = (speed & 0x3C0) >> 6;
+
+ low_part = speed & 0x3F;
+
+ ais_2_garmin[22] = high_part + 0x30;
+
+ if(ais_2_garmin[22] > 87)
+ ais_2_garmin[22] = ais_2_garmin[22] + 8;
+
+ ais_2_garmin[23] = low_part + 0x30;
+
+ if(ais_2_garmin[23] > 87)
+ ais_2_garmin[23] = ais_2_garmin[23] + 8;
+
+
+
+
+}
+
+void rxCallback_Zigbee(void) {
+static int c,d;
+
+ d = bg.getc();
+
+ Zigbee_buffer[p_z++] = d;
+
+ if(p_z > Z_SIZE)
+ p_z = 0;
+
+ c++;
+
+ if(c == 10)
+ mled1 = 1;
+
+ if(c >= 20){
+
+ c = 0;
+ mled1 = 0;
+
+ }
+
+
+}
+void rxCallback(void) {
+
+static char nmea_in;
+extern volatile int rec_state;
+extern volatile char Rx_buffer[2000];
+extern volatile int p_rx;
+extern volatile int ais_ready;
+int head;
+
+
+// Rx_buffer[p_rx++] = bg2.getc();
+
+ mled0 = 1;
+
+ nmea_in= bg2.getc();
+
+ switch (rec_state) {
+
+ case IDLE:
+
+ mled0 = 1;
+
+ break;
+
+
+ case SEARCH_START:
+
+ if (nmea_in == '$') {
+
+ rec_state = CAPTURE;
+
+ }
+
+ break;
+
+ case CAPTURE:
+
+ garmin_buffer[item++] = nmea_in;
+
+ if(item >6){
+
+ head = analyze_buffer();
+
+ if(head == 17){
+
+ mled2 = 1;
+
+ gi++;
+
+ if(gi >= 2){
+
+ gi = 0;
+
+ mled0 = 0;
+ mled2 = 0;
+ mled3 = 0;
+
+ }
+
+ rec_state = FETCH_DATA; // we found our nmea sentence
+
+ }
+
+ else{
+
+ rec_state = SEARCH_START; // not rigth sentence, go back to search rigth header
+
+ item = 0;
+
+ }
+ }
+
+ break;
+
+ case FETCH_DATA:
+
+ garmin_buffer[item++] = nmea_in;
+
+ if(item > 40){
+
+ packet_data_no_printf();
+
+ item = 0;
+
+ ais_ready = 17;
+
+ rec_state = SEARCH_START;
+
+ }
+ break;
+
+ default:
+
+ mled0 = 1;
+
+ break;
+ }
+
+ if(p_rx > 1100){
+ p_rx = 1100;
+ mled2 = 0;
+ }
+
+}
+int analyze_buffer(void){
+
+ extern volatile char garmin_buffer[50];
+
+ char t_1,t_2,t_3,t_4,t_5;
+
+ t_1 = garmin_buffer[0];
+
+ t_2 = garmin_buffer[1];
+
+ t_3 = garmin_buffer[2];
+
+ t_4 = garmin_buffer[3];
+
+ t_5 = garmin_buffer[4];
+
+ if(t_1 == 'G' && t_2 == 'P' && t_3 == 'R' && t_4 == 'M' && t_5 == 'C')
+ return(17);
+ else
+ return(0);
+
+}
+
+void packet_data_no_printf(void){
+
+ extern volatile char garmin_buffer[50];
+
+ extern char ais_2_garmin[48];
+
+ char long_ais[7],lat_ais[7];
+
+ int i;
+
+ long long_1, long_2,lat_1,lat_2;
+
+ char longitude[8],latitude[8];
+
+ for(i = 0; i < 4; i++)
+ latitude[i] = garmin_buffer[i + 15];
+
+ for(i = 4; i < 8 ; i++)
+ latitude[i] = garmin_buffer[i + 16]; // don't copy the "."
+
+ for(i = 0; i < 5; i++)
+ longitude[i] = garmin_buffer[i + 27];
+
+ for(i = 5; i < 8; i++)
+ longitude[i] = garmin_buffer[i + 28];
+
+ // strip of 0x30 from all values
+
+ for(i = 0; i < 8 ; i++)
+ longitude[i] = longitude[i] - 0x30;
+
+ for(i = 0; i < 8 ; i++)
+ latitude[i] = latitude[i] - 0x30;
+
+ long_1 = longitude[3] * 100000 + longitude[4] * 10000 + longitude[5] * 1000 + longitude[6] * 100 + longitude[7] * 10;
+
+ long_1 = long_1 * 6;
+
+ long_1 = long_1 / 10;
+
+ long_2 = longitude[0] * 100 + longitude[1] * 10 + longitude[2];
+
+ long_2 = long_2 * 600000;
+
+ long_2 = long_2 + long_1;
+
+ lat_1 = latitude[2] * 100000 + latitude[3] * 10000 + latitude[4] * 1000 + latitude[5] * 100 + latitude[6] * 10 + latitude[7];
+
+ lat_2 = latitude[0] * 10 + latitude[1];
+
+ lat_2 = lat_2 * 600000;
+
+ lat_2 = lat_2 + lat_1;
+
+ // do the bit fiddling to pack 8 bits into 6 bit and back into 8 bit format according to the AIVDM format
+
+ long_ais[0] = ((long_2 >> 23) & 0x3F) + 0x30;
+
+ long_ais[1] = ((long_2 >> 17) & 0x3F) + 0x30;
+
+ long_ais[2] = ((long_2 >> 11) & 0x3F) + 0x30;
+
+ long_ais[3] = ((long_2 >> 5) & 0x38) + 0x30;
+
+ lat_ais[0] = ((lat_2 >> 20) & 0x3F) + 0x30;
+
+ lat_ais[1] = ((lat_2 >> 14) & 0x3F) + 0x30;
+
+// if(lat_ais[1] > 87)
+// lat_ais[1] = lat_ais[1] + 0x08;
+
+ lat_ais[2] = ((lat_2 >> 8) & 0x3F) + 0x30;
+
+ lat_ais[3] = ((lat_2 >> 2) & 0x3F) + 0x30;
+
+ ais_2_garmin[24] = long_ais[0];
+
+ ais_2_garmin[25] = long_ais[1];
+
+ ais_2_garmin[26] = long_ais[2];
+
+ ais_2_garmin[27] = long_ais[3];
+
+ ais_2_garmin[29] = lat_ais[0];
+
+ ais_2_garmin[30] = lat_ais[1];
+
+ ais_2_garmin[31] = lat_ais[2];
+
+ ais_2_garmin[32] = lat_ais[3];
+
+ for( i = 24; i < 33 ; i++){ // adjust for codes not used
+
+ if(ais_2_garmin[i] > 87)
+ ais_2_garmin[i] = ais_2_garmin[i] + 8;
+
+ }
+
+}
+
+void ais_crc(void){
+
+ int checksum,pos;
+
+ checksum = 0;
+
+ pos = 0;
+
+ while(ais_2_garmin[pos++] != '*');
+
+ checksum = iChecksum(&ais_2_garmin[0]);
+
+ ais_2_garmin[pos] = ((checksum & 0xF0) >> 4) + 0x30;
+
+ if(ais_2_garmin[pos] > 0x39)
+ ais_2_garmin[pos] = ais_2_garmin[pos] + 7;
+
+ ais_2_garmin[pos+1] = (checksum & 0x0F) + 0x30;
+
+ if(ais_2_garmin[pos+1] > 0x39)
+ ais_2_garmin[pos+1] = ais_2_garmin[pos+1] + 7;
+
+}
+
+void packet_data(void){
+
+ extern volatile char garmin_buffer[50];
+
+ extern char ais_2_garmin[48];
+
+ char long_ais[7],lat_ais[7];
+
+ char p_buffer[40];
+
+ int i;
+
+ long long_1, long_2,lat_1,lat_2;
+
+ char longitude[8],latitude[8];
+
+ char crlf[] = {0x0A,0x0D,0};
+
+ int checksum,pos;
+
+ for(i = 0; i < 4; i++)
+ latitude[i] = garmin_buffer[i + 15];
+
+ for(i = 4; i < 8 ; i++)
+ latitude[i] = garmin_buffer[i + 16]; // don't copy the "."
+
+ for(i = 0; i < 5; i++)
+ longitude[i] = garmin_buffer[i + 27];
+
+ for(i = 5; i < 8; i++)
+ longitude[i] = garmin_buffer[i + 28];
+
+ debug_print("from Garmin latitude: ");
+
+ for(i = 0; i < 8 ; i++)
+ p_buffer[i] = latitude[i];
+
+ p_buffer[8] = 0;
+
+ debug_print(&p_buffer[0]);
+
+ debug_print("from Garmin longitude: ");
+
+ for(i = 0 ; i < 8 ; i++)
+ p_buffer[i] = longitude[i];
+
+ p_buffer[8] = 0;
+
+ debug_print(&p_buffer[0]);
+
+ debug_print(&crlf[0]);
+
+ // strip of 0x30 from all values
+
+ for(i = 0; i < 8 ; i++)
+ longitude[i] = longitude[i] - 0x30;
+
+ for(i = 0; i < 8 ; i++)
+ latitude[i] = latitude[i] - 0x30;
+
+ //
+
+ long_1 = longitude[3] * 100000 + longitude[4] * 10000 + longitude[5] * 1000 + longitude[6] * 100 + longitude[7] * 10;
+
+// long_1 = long_1 * 5;
+
+// long_1 = long_1 / 3;
+
+ long_1 = long_1 * 6;
+
+ long_1 = long_1 / 10;
+
+ long_2 = longitude[0] * 100 + longitude[1] * 10 + longitude[2];
+
+ long_2 = long_2 * 600000;
+
+ long_2 = long_2 + long_1;
+
+ Serial pc(USBTX, USBRX); // tx, rx
+
+ pc.printf("longitude converted into minutes/10000: %d \n",long_2);
+
+ lat_1 = latitude[2] * 100000 + latitude[3] * 10000 + latitude[4] * 1000 + latitude[5] * 100 + latitude[6] * 10 + latitude[7];
+
+ // lat_1 = lat_1 ;
+
+ // lat_1 = lat_1 *10;
+
+ // lat_1 = lat_1 / 6 ;
+
+ lat_2 = latitude[0] * 10 + latitude[1];
+
+ lat_2 = lat_2 * 600000;
+
+ lat_2 = lat_2 + lat_1;
+
+ pc.printf("latitude converted into minutes/10000: %d \n\n",lat_2);
+
+ // do the bit fiddling to pack 8 bits into 6 bit and back into 8 bit format according to the AIVDM format
+
+ long_ais[0] = ((long_2 >> 23) & 0x3F) + 0x30;
+
+ long_ais[1] = ((long_2 >> 17) & 0x3F) + 0x30;
+
+ long_ais[2] = ((long_2 >> 11) & 0x3F) + 0x30;
+
+ long_ais[3] = ((long_2 >> 5) & 0x38) + 0x30;
+
+ pc.printf("longitude : %c%c%c%c\n",long_ais[0],long_ais[1],long_ais[2],long_ais[3]);
+
+ lat_ais[0] = ((lat_2 >> 20) & 0x3F) + 0x30;
+
+ lat_ais[1] = ((lat_2 >> 14) & 0x3F) + 0x30;
+
+ pc.printf("lat_1 %x \n",lat_ais[1]);
+ if(lat_ais[1] > 87)
+ lat_ais[1] = lat_ais[1] + 0x08;
+ pc.printf("lat_1 modified %x \n",lat_ais[1]);
+
+ lat_ais[2] = ((lat_2 >> 8) & 0x3F) + 0x30;
+
+ lat_ais[3] = ((lat_2 >> 2) & 0x3F) + 0x30;
+
+ pc.printf("latitude: %c%c%c%c\n\n",lat_ais[0],lat_ais[1],lat_ais[2],lat_ais[3]);
+
+ debug_print("original string to Garmin: ");
+
+ debug_print(&ais_2_garmin[0]);
+
+ pc.printf("\n");
+
+ ais_2_garmin[24] = long_ais[0];
+
+ ais_2_garmin[25] = long_ais[1];
+
+ ais_2_garmin[26] = long_ais[2];
+
+ ais_2_garmin[27] = long_ais[3];
+
+ ais_2_garmin[29] = lat_ais[0];
+
+ ais_2_garmin[30] = lat_ais[1];
+
+ ais_2_garmin[31] = lat_ais[2];
+
+ ais_2_garmin[32] = lat_ais[3];
+
+ pos = 0;
+
+ while(ais_2_garmin[pos++] != '*');
+
+ checksum = iChecksum(&ais_2_garmin[0]);
+
+ ais_2_garmin[pos] = ((checksum & 0xF0) >> 4) + 0x30;
+
+ if(ais_2_garmin[pos] > 0x39)
+ ais_2_garmin[pos] = ais_2_garmin[pos] + 7;
+
+ ais_2_garmin[pos+1] = (checksum & 0x0F) + 0x30;
+
+ if(ais_2_garmin[pos+1] > 0x39)
+ ais_2_garmin[pos+1] = ais_2_garmin[pos+1] + 7;
+
+
+} void debug_print(char *message){
+
+
+// Serial bg_d(p28,p27);
+
+// bg_d.printf(message);
+
+ Serial pc(USBTX, USBRX);
+
+ pc.printf(message);
+
+}
+void print_bg(char *message){
+
+
+
+ while(*message != 0){
+
+ while(bg.writeable() == 0);
+
+ bg.putc((int) *message++);
+
+ }
+
+}
+void print_pc(char *message){
+
+ int *p_reg;
+ int *p_lsr;
+
+ p_lsr = (int *) U0LSR;
+
+ p_reg = (int *) U0THR;
+
+ while(*message != 0){
+
+ while ((*p_lsr & 0x20) == 0); // wait until holding register empty
+
+ *p_reg = *message++;
+
+ }
+
+}
+int iChecksum( char* message )
+{
+int sum = 0;
+ for( char* c = message+1; *c!='*'; c++ )
+ {
+ sum^=*c;
+ }
+ return sum;
+}