Subversion Repositories HomeAutomation

Rev

Rev 1580 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | SVN | RSS feed

  1.  
  2. #include "sns_nmea.h"
  3. #include <drivers/uart/serial.h>
  4. #include <drivers/uart/uart.h>
  5.  
  6. void nmea_send_can(uint8_t timer);
  7.  
  8. uint8_t nmea_buffer[80];
  9. uint8_t nmea_comindex[25];
  10. uint8_t nmea_buffer_index, nmea_buflen, nmea_comlen;
  11.  
  12. uint8_t a2d (uint8_t chr) { // Convert ascii to decimal
  13.     return (chr-48);
  14. }
  15.  
  16. void nmea_clear_comindex(void) {
  17.     uint8_t i;
  18.    
  19.     for (i=0;i<25;i++) {
  20.         nmea_comindex[i]=0;
  21.     }
  22. }
  23.  
  24. void nmea_parse_GGA(void) {
  25.     if (nmea_buffer[nmea_comindex[1]+1] != 0x2C)
  26.     {
  27.        
  28.         //Get time in UTC
  29.         NMEA_data.time_h = 10*a2d(nmea_buffer[6]) + a2d(nmea_buffer[7]);
  30.         NMEA_data.time_m = 10*a2d(nmea_buffer[8]) + a2d(nmea_buffer[9]);
  31.         NMEA_data.time_s = 10*a2d(nmea_buffer[10]) + a2d(nmea_buffer[11]);
  32.        
  33.         //Get fixtype
  34.         NMEA_data.fixvalid=a2d(nmea_buffer[nmea_comindex[5]+1]);
  35.        
  36.         //Get number of satellites used in fix
  37.         NMEA_data.usedsat = 10*a2d(nmea_buffer[nmea_comindex[6]+1]) + a2d(nmea_buffer[nmea_comindex[6]+2]);
  38.        
  39.         // Get latitude
  40.         if (nmea_comindex[2]-nmea_comindex[1] == LAT_LENGTH) {
  41.             NMEA_data.lat_deg = 10*a2d(nmea_buffer[nmea_comindex[1]+1])
  42.             + a2d(nmea_buffer[nmea_comindex[1]+2]);
  43.            
  44.             NMEA_data.lat_min = 10*a2d(nmea_buffer[nmea_comindex[1]+3])
  45.             + a2d(nmea_buffer[nmea_comindex[1]+4]);
  46.  
  47.             NMEA_data.lat_sec_h = 10*a2d(nmea_buffer[nmea_comindex[1]+6]);
  48.             NMEA_data.lat_sec_h += a2d(nmea_buffer[nmea_comindex[1]+7]);
  49.             NMEA_data.lat_sec_l = 10*a2d(nmea_buffer[nmea_comindex[1]+8]);
  50.             NMEA_data.lat_sec_l += a2d(nmea_buffer[nmea_comindex[1]+9]);
  51.             NMEA_data.lat_sec_l = NMEA_data.lat_sec_l << 1;
  52.            
  53.             //Last bit contains cardinal direction, N=0, S=1
  54.             if (nmea_buffer[nmea_comindex[2]+1] == 'N') {
  55.                 NMEA_data.lat_sec_l &=0b11111110;
  56.             } else {
  57.                 NMEA_data.lat_sec_l |=1;
  58.             }
  59.         }
  60.        
  61.        
  62.         // Get longitude
  63.         if (nmea_comindex[4]-nmea_comindex[3] == LON_LENGTH) {
  64.             NMEA_data.lon_deg = 100*a2d(nmea_buffer[nmea_comindex[3]+1])
  65.             + 10*a2d(nmea_buffer[nmea_comindex[3]+2])
  66.             + a2d(nmea_buffer[nmea_comindex[3]+3]);
  67.            
  68.             NMEA_data.lon_min = 10*a2d(nmea_buffer[nmea_comindex[3]+4])
  69.             + a2d(nmea_buffer[nmea_comindex[3]+5]);
  70.            
  71.             NMEA_data.lon_sec_h = 10*a2d(nmea_buffer[nmea_comindex[3]+7]);
  72.             NMEA_data.lon_sec_h += a2d(nmea_buffer[nmea_comindex[3]+8]);
  73.             NMEA_data.lon_sec_l = 10*a2d(nmea_buffer[nmea_comindex[3]+9]);
  74.             NMEA_data.lon_sec_l += a2d(nmea_buffer[nmea_comindex[3]+10]);
  75.             NMEA_data.lon_sec_l = NMEA_data.lon_sec_l << 1;
  76.            
  77.             //Last bit contains cardinal direction, E=0, W=1
  78.             if (nmea_buffer[nmea_comindex[4]+1] == 'E') {
  79.                 NMEA_data.lon_sec_l &=0b11111110;
  80.             } else {
  81.                 NMEA_data.lon_sec_l |=1;
  82.             }
  83.         }
  84.     }
  85. }
  86.  
  87. void nmea_parse_GSA(void) {
  88.    /*
  89.       Gets fixtype only, 1=No fix, 2=2D fix, 3=3D fix
  90.    */
  91.     NMEA_data.fixtype=a2d(nmea_buffer[nmea_comindex[1]+1]);
  92. }
  93.  
  94. void nmea_parse_GSV(void) {
  95.     //Information about tracked satellites, angle, snr etc. Ignore for now.
  96. }
  97.  
  98. void nmea_parse_RMC(void) {
  99.     //TODO
  100.     /*  Follows comindex    length  info
  101.          0          6   time of fix
  102.          1          1   validity, A=ok, V=invalid
  103.          2          X   latitude
  104.          3          1   lat CD
  105.          4          X   longitude
  106.          5          1   lon CD
  107.          6          5   Speed over ground, knots, NNN.N
  108.          7          5   Course made good, true
  109.          8          6   Date of fix, DDMMYY
  110.          9          5   Magnetic variation, degrees
  111.          10         1   Magnetic variation, CD
  112.      
  113.     */
  114.     uint8_t varlen=0;
  115.     uint8_t validity=0;
  116.     if (nmea_buffer[nmea_comindex[1]+1]==0x41) //0x41='A', valid fix
  117.      validity=1;
  118.     NMEA_data.speed=0;
  119.     NMEA_data.cmg_h=0;
  120.     NMEA_data.cmg_l=0;
  121.  
  122.     if (validity==1) {
  123.        NMEA_data.speed=0;
  124.        varlen = nmea_comindex[7]-nmea_comindex[6];
  125.        switch(varlen-4) {
  126.           case 3:
  127.          NMEA_data.speed = 100*a2d(nmea_buffer[nmea_comindex[6]+1])+
  128.                    10*a2d(nmea_buffer[nmea_comindex[6]+2])+
  129.                    a2d(nmea_buffer[nmea_comindex[6]+3]);
  130.         break;
  131.           case 2:
  132.          NMEA_data.speed = 10*a2d(nmea_buffer[nmea_comindex[6]+1])+
  133.                    a2d(nmea_buffer[nmea_comindex[6]+2]);
  134.         break;
  135.           case 1:
  136.          NMEA_data.speed = a2d(nmea_buffer[nmea_comindex[6]+1]);
  137.          break;
  138.        }
  139.  
  140.        varlen = nmea_comindex[8]-nmea_comindex[7];
  141.        switch (varlen-4) {
  142.         case 3:
  143.            NMEA_data.cmg_h = a2d(nmea_buffer[nmea_comindex[7]+1]);
  144.            NMEA_data.cmg_l = 10*a2d(nmea_buffer[nmea_comindex[7]+2])+
  145.                  a2d(nmea_buffer[nmea_comindex[7]+3]);
  146.            break;
  147.         case 2:
  148.         NMEA_data.cmg_l = 10*a2d(nmea_buffer[nmea_comindex[7]+1])+
  149.                   a2d(nmea_buffer[nmea_comindex[7]+2]);
  150.            break;
  151.         case 1:
  152.            NMEA_data.cmg_l = a2d(nmea_buffer[nmea_comindex[7]+1]);
  153.            break;
  154.        }
  155.           NMEA_data.date_d = 10*a2d(nmea_buffer[nmea_comindex[8]+1])+a2d(nmea_buffer[nmea_comindex[8]+2]);
  156.           NMEA_data.date_m = 10*a2d(nmea_buffer[nmea_comindex[8]+3])+a2d(nmea_buffer[nmea_comindex[8]+4]);
  157.           NMEA_data.date_y = 10*a2d(nmea_buffer[nmea_comindex[8]+5])+a2d(nmea_buffer[nmea_comindex[8]+6]);
  158.     }
  159.     #if CAN_PRINTF==1
  160.         //printf("%02d-%02d-%02d",NMEA_data.date_y,NMEA_data.date_m,NMEA_data.date_d);
  161.     #endif
  162. }
  163.  
  164. void nmea_make_index(void) {
  165.  
  166.     nmea_clear_comindex();
  167.    
  168.     uint8_t i;
  169.     uint8_t j=0;
  170.    
  171.     for (i=0;i<nmea_buflen;i++) {
  172.         if (nmea_buffer[i] == ',') {
  173.             nmea_comindex[j] = i;
  174.             j++;
  175.         }
  176.     }
  177.     nmea_comlen = j;
  178. }
  179.  
  180. void nmea_parse(void) {
  181.     // Supported messages: GSA, GSV, GGA, RMC
  182.     nmea_make_index();
  183.        
  184.     if (
  185.         nmea_buffer[2] == 'G' &&
  186.         nmea_buffer[3] == 'S' &&
  187.         nmea_buffer[4] == 'A')
  188.     {
  189.         nmea_parse_GSA();
  190.     } else if (
  191.         nmea_buffer[2] == 'G' &&
  192.         nmea_buffer[3] == 'S' &&
  193.         nmea_buffer[4] == 'V') {
  194.         nmea_parse_GSV();
  195.     } else if (
  196.         nmea_buffer[2] == 'G' &&
  197.         nmea_buffer[3] == 'G' &&
  198.         nmea_buffer[4] == 'A') {
  199.         nmea_parse_GGA();
  200.     } else if (
  201.         nmea_buffer[2] == 'R' &&
  202.         nmea_buffer[3] == 'M' &&
  203.         nmea_buffer[4] == 'C') {
  204.         nmea_parse_RMC();
  205.     }      
  206.    
  207. }
  208.  
  209. void sns_nmea_Init(void)
  210. {
  211.     Serial_Init(); 
  212.     Timer_SetTimeout(sns_nmea_cansend_TIMER, sns_nmea_cansend_SEND_PERIOD , TimerTypeFreeRunning, &nmea_send_can);
  213.  
  214. #ifdef TSIP_AT_STARTUP
  215.     // Tell GPS to switch to NMEA and 9600 baud
  216.     //TODO: put in progmem to save space
  217.     //TODO: do not send this directly at power on, the GPS module is quite slow at startup
  218.     uint8_t tsip_command[] = {0x10, 0xBC, 0x00, 0x07, 0x07, 0x03, 0x00, 0x00, 0x00, 0x02, 0x04, 0x00, 0x10, 0x03};
  219.     for(i = 0; i<14; i++){
  220.         uart_putc(tsip_command[i]);
  221.     }
  222. #endif
  223. }
  224.  
  225. void sns_nmea_Process(void)
  226. {
  227.     uint16_t rx_uart;
  228.     uint8_t rx_char;
  229.    
  230.     rx_uart = uart_getc();
  231.    
  232.     if (rx_uart != UART_NO_DATA) {
  233.         rx_char = (uint8_t) rx_uart & 0x00ff;
  234.        
  235.         if (rx_char == 0x24) {
  236.             nmea_buffer_index=0;
  237.         } else if (rx_char == '\n') {
  238.             nmea_buflen = nmea_buffer_index+1;
  239.             nmea_parse();
  240.         } else {           
  241.             nmea_buffer[nmea_buffer_index] = rx_char;
  242.             nmea_buffer_index++;
  243.         }
  244.     }
  245. }
  246.  
  247. void sns_nmea_HandleMessage(StdCan_Msg_t *rxMsg)
  248. {
  249. /*
  250.     if (    StdCan_Ret_class(rxMsg->Header) == CAN_MODULE_CLASS_SNS &&
  251.         StdCan_Ret_direction(rxMsg->Header) == DIRECTIONFLAG_FROM_OWNER &&
  252.         rxMsg->Header.ModuleType == CAN_MODULE_TYPE_SNS_NMEA &&
  253.         rxMsg->Header.ModuleId == sns_nmea_ID)
  254.     {
  255.         switch (rxMsg->Header.Command)
  256.         {
  257.         case CAN_CMD_MODULE_DUMMY:
  258.         ///TODO: Do something dummy
  259.         break;
  260.         }
  261.     }
  262. */
  263. }
  264.  
  265. void sns_nmea_List(uint8_t ModuleSequenceNumber)
  266. {
  267.     StdCan_Msg_t txMsg;
  268.    
  269.     StdCan_Set_class(txMsg.Header, CAN_MODULE_CLASS_SNS); ///TODO: Change this to the actual class type
  270.     StdCan_Set_direction(txMsg.Header, DIRECTIONFLAG_FROM_OWNER);
  271.     txMsg.Header.ModuleType = CAN_MODULE_TYPE_SNS_NMEA; ///TODO: Change this to the actual module type
  272.     txMsg.Header.ModuleId = sns_nmea_ID;
  273.     txMsg.Header.Command = CAN_MODULE_CMD_GLOBAL_LIST;
  274.     txMsg.Length = 6;
  275.  
  276.     txMsg.Data[0] = NODE_HW_ID_BYTE0;
  277.     txMsg.Data[1] = NODE_HW_ID_BYTE1;
  278.     txMsg.Data[2] = NODE_HW_ID_BYTE2;
  279.     txMsg.Data[3] = NODE_HW_ID_BYTE3;
  280.    
  281.     txMsg.Data[4] = NUMBER_OF_MODULES;
  282.     txMsg.Data[5] = ModuleSequenceNumber;
  283.    
  284.     while (StdCan_Put(&txMsg) != StdCan_Ret_OK);
  285. }
  286.  
  287. void nmea_send_can(uint8_t timer)
  288. {
  289.     StdCan_Msg_t txMsg;
  290.  
  291.     StdCan_Set_class(txMsg.Header, CAN_MODULE_CLASS_SNS);
  292.     StdCan_Set_direction(txMsg.Header, DIRECTIONFLAG_FROM_OWNER);
  293.     txMsg.Header.ModuleType = CAN_MODULE_TYPE_SNS_NMEA;
  294.     txMsg.Header.ModuleId = sns_nmea_ID;
  295.     txMsg.Header.Command = CAN_MODULE_CMD_NMEA_POSITION; //definiera nåt vettigt för det sen
  296.    
  297.     txMsg.Length = 8;
  298.     txMsg.Data[0] = NMEA_data.lat_deg;
  299.     txMsg.Data[1] = NMEA_data.lat_min;
  300.     txMsg.Data[2] = NMEA_data.lat_sec_h;   
  301.     txMsg.Data[3] = NMEA_data.lat_sec_l;
  302.  
  303.     txMsg.Data[4] = NMEA_data.lon_deg;
  304.     txMsg.Data[5] = NMEA_data.lon_min;
  305.     txMsg.Data[6] = NMEA_data.lon_sec_h;   
  306.     txMsg.Data[7] = NMEA_data.lon_sec_l;
  307.    
  308.     while (StdCan_Put(&txMsg) != StdCan_Ret_OK);
  309.  
  310.     txMsg.Header.Command = CAN_MODULE_CMD_GLOBAL_GPSTIME;
  311.     txMsg.Length = 4;
  312.     txMsg.Data[0] = NMEA_data.time_h;
  313.     txMsg.Data[1] = NMEA_data.time_m;
  314.     txMsg.Data[2] = NMEA_data.time_s;
  315.     txMsg.Data[3] = 0; //Timezone diff from GMT
  316.  
  317.     while (StdCan_Put(&txMsg) != StdCan_Ret_OK);
  318.  
  319.     txMsg.Header.Command = CAN_MODULE_CMD_GLOBAL_GPSDATE;
  320.     txMsg.Length = 3;
  321.     txMsg.Data[0] = NMEA_data.date_y;
  322.     txMsg.Data[1] = NMEA_data.date_m;
  323.     txMsg.Data[2] = NMEA_data.date_d;
  324.  
  325.     while (StdCan_Put(&txMsg) != StdCan_Ret_OK);
  326.  
  327.     txMsg.Header.Command = CAN_MODULE_CMD_NMEA_HEADING;
  328.     txMsg.Length = 6;
  329.     txMsg.Data[0] = NMEA_data.fixvalid;
  330.     txMsg.Data[1] = NMEA_data.fixtype;
  331.     txMsg.Data[2] = NMEA_data.usedsat;
  332.     txMsg.Data[3] = NMEA_data.speed;
  333.     txMsg.Data[4] = NMEA_data.cmg_h;
  334.     txMsg.Data[5] = NMEA_data.cmg_l;
  335.    
  336.     while (StdCan_Put(&txMsg) != StdCan_Ret_OK);
  337.    
  338.     #if CAN_PRINTF==1
  339.       //printf("Lat: %02d %02d.%02d%02d, Lon: %03d %02d.%02d%02d\n",NMEA_data.lat_deg, NMEA_data.lat_min, NMEA_data.lat_sec_h, NMEA_data.lat_sec_l >> 1,NMEA_data.lon_deg, NMEA_data.lon_min, NMEA_data.lon_sec_h, NMEA_data.lon_sec_l >> 1);
  340.       //printf("Time: %01d:%01d:%01d\n",NMEA_data.time_h,NMEA_data.time_m,NMEA_data.time_s);
  341.       //printf("Fixvalid: %01d\n",NMEA_data.fixvalid);
  342.       //printf("Sat: %02d\n",NMEA_data.usedsat);
  343.       //printf("Type:%01d\n",NMEA_data.fixtype);
  344.     #endif
  345.  
  346.     /*
  347.      Add to msg:
  348.      Valid fix,     NMEA_data.fixvalid
  349.      # of satellites,   NMEA_data.numsat
  350.      fixtype (no/2d/3d) NMEA_data.fixtype
  351.      Speed          NMEA_data.speed
  352.      Course made good   NMEA_data.cmg_h, NMEA_data.cmg_l
  353.      
  354.     */
  355. }
  356.  
  357.