Subversion Repositories HomeAutomation

Rev

Rev 1000 | 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] != ",")
  26.     {
  27.         uint8_t i;
  28.        
  29.         //Get time in UTC
  30.         NMEA_data.time_h = 10*a2d(nmea_buffer[6]) + a2d(nmea_buffer[7]);
  31.         NMEA_data.time_m = 10*a2d(nmea_buffer[8]) + a2d(nmea_buffer[9]);
  32.         NMEA_data.time_s = 10*a2d(nmea_buffer[10]) + a2d(nmea_buffer[11]);
  33.        
  34.         //Get fixtype
  35.         NMEA_data.fixvalid=a2d(nmea_buffer[nmea_comindex[5]+1]);
  36.        
  37.         //Get number of satellites used in fix
  38.         NMEA_data.usedsat = 10*a2d(nmea_buffer[nmea_comindex[6]+1]) + a2d(nmea_buffer[nmea_comindex[6]+2]);
  39.        
  40.         // Get latitude
  41.         if (nmea_comindex[2]-nmea_comindex[1] == 10) {
  42.             NMEA_data.lat_deg = 10*a2d(nmea_buffer[nmea_comindex[1]+1])
  43.             + a2d(nmea_buffer[nmea_comindex[1]+2]);
  44.            
  45.             NMEA_data.lat_min = 10*a2d(nmea_buffer[nmea_comindex[1]+3])
  46.             + a2d(nmea_buffer[nmea_comindex[1]+4]);
  47.  
  48.             NMEA_data.lat_sec_h = 10*a2d(nmea_buffer[nmea_comindex[1]+6]);
  49.             NMEA_data.lat_sec_h += a2d(nmea_buffer[nmea_comindex[1]+7]);
  50.             NMEA_data.lat_sec_l = 10*a2d(nmea_buffer[nmea_comindex[1]+8]);
  51.             NMEA_data.lat_sec_l += a2d(nmea_buffer[nmea_comindex[1]+9]);
  52.             NMEA_data.lat_sec_l = NMEA_data.lat_sec_l << 1;
  53.            
  54.             //Last bit contains cardinal direction, N=0, S=1
  55.             if (nmea_buffer[nmea_comindex[2]+1] == 'N') {
  56.                 NMEA_data.lat_sec_l &=0b11111110;
  57.             } else {
  58.                 NMEA_data.lat_sec_l |=1;
  59.             }
  60.         }
  61.        
  62.        
  63.         // Get longitude
  64.         if (nmea_comindex[4]-nmea_comindex[3] == 11) {
  65.             NMEA_data.lon_deg = 100*a2d(nmea_buffer[nmea_comindex[3]+1])
  66.             + 10*a2d(nmea_buffer[nmea_comindex[3]+2])
  67.             + a2d(nmea_buffer[nmea_comindex[3]+3]);
  68.            
  69.             NMEA_data.lon_min = 10*a2d(nmea_buffer[nmea_comindex[3]+4])
  70.             + a2d(nmea_buffer[nmea_comindex[3]+5]);
  71.            
  72.             NMEA_data.lon_sec_h = 10*a2d(nmea_buffer[nmea_comindex[3]+7]);
  73.             NMEA_data.lon_sec_h += a2d(nmea_buffer[nmea_comindex[3]+8]);
  74.             NMEA_data.lon_sec_l = 10*a2d(nmea_buffer[nmea_comindex[3]+9]);
  75.             NMEA_data.lon_sec_l += a2d(nmea_buffer[nmea_comindex[3]+10]);
  76.             NMEA_data.lon_sec_l = NMEA_data.lon_sec_l << 1;
  77.            
  78.             //Last bit contains cardinal direction, E=0, W=1
  79.             if (nmea_buffer[nmea_comindex[4]+1] == 'E') {
  80.                 NMEA_data.lon_sec_l &=0b11111110;
  81.             } else {
  82.                 NMEA_data.lon_sec_l |=1;
  83.             }
  84.         }
  85.     }
  86. }
  87.  
  88. void nmea_parse_GSA(void) {
  89.     //TODO
  90. }
  91.  
  92. void nmea_parse_GSV(void) {
  93.     //TODO
  94. }
  95.  
  96. void nmea_parse_RMC(void) {
  97.     //TODO
  98. }
  99.  
  100. void nmea_make_index(void) {
  101.  
  102.     nmea_clear_comindex();
  103.    
  104.     uint8_t i;
  105.     uint8_t j=0;
  106.    
  107.     for (i=0;i<nmea_buflen;i++) {
  108.         if (nmea_buffer[i] == ',') {
  109.             nmea_comindex[j] = i;
  110.             j++;
  111.         }
  112.     }
  113.     nmea_comlen = j;
  114. }
  115.  
  116. void nmea_parse(void) {
  117.     // Supported messages: GSA, GSV, GGA, RMC
  118.     nmea_make_index();
  119.        
  120.     if (
  121.         nmea_buffer[2] == 'G' &&
  122.         nmea_buffer[3] == 'S' &&
  123.         nmea_buffer[4] == 'A')
  124.     {
  125.         nmea_parse_GSA();
  126.     } else if (
  127.         nmea_buffer[2] == 'G' &&
  128.         nmea_buffer[3] == 'S' &&
  129.         nmea_buffer[4] == 'V') {
  130.         nmea_parse_GSV();
  131.     } else if (
  132.         nmea_buffer[2] == 'G' &&
  133.         nmea_buffer[3] == 'G' &&
  134.         nmea_buffer[4] == 'A') {
  135.         nmea_parse_GGA();
  136.     } else if (
  137.         nmea_buffer[2] == 'R' &&
  138.         nmea_buffer[3] == 'M' &&
  139.         nmea_buffer[4] == 'C') {
  140.         nmea_parse_RMC();
  141.     }      
  142.    
  143. }
  144.  
  145. void sns_nmea_Init(void)
  146. {
  147.     Serial_Init(); 
  148.     Timer_SetTimeout(sns_nmea_cansend_TIMER, sns_nmea_cansend_SEND_PERIOD , TimerTypeFreeRunning, &nmea_send_can);
  149. }
  150.  
  151. void sns_nmea_Process(void)
  152. {
  153.     uint16_t rx_uart;
  154.     uint8_t rx_char;
  155.    
  156.     rx_uart = uart_getc();
  157.    
  158.     if (rx_uart != UART_NO_DATA) {
  159.         rx_char = (uint8_t) rx_uart & 0x00ff;
  160.        
  161.         if (rx_char == 0x24) {
  162.             nmea_buffer_index=0;
  163.         } else if (rx_char == '\n') {
  164.             nmea_buflen = nmea_buffer_index+1;
  165.             nmea_parse();
  166.         } else {           
  167.             nmea_buffer[nmea_buffer_index] = rx_char;
  168.             nmea_buffer_index++;
  169.         }
  170.     }
  171. }
  172.  
  173. void sns_nmea_HandleMessage(StdCan_Msg_t *rxMsg)
  174. {
  175. /*
  176.     if (    StdCan_Ret_class(rxMsg->Header) == CAN_MODULE_CLASS_SNS &&
  177.         StdCan_Ret_direction(rxMsg->Header) == DIRECTIONFLAG_FROM_OWNER &&
  178.         rxMsg->Header.ModuleType == CAN_MODULE_TYPE_SNS_NMEA &&
  179.         rxMsg->Header.ModuleId == sns_nmea_ID)
  180.     {
  181.         switch (rxMsg->Header.Command)
  182.         {
  183.         case CAN_CMD_MODULE_DUMMY:
  184.         ///TODO: Do something dummy
  185.         break;
  186.         }
  187.     }
  188. */
  189. }
  190.  
  191. void sns_nmea_List(uint8_t ModuleSequenceNumber)
  192. {
  193.     StdCan_Msg_t txMsg;
  194.    
  195.     StdCan_Set_class(txMsg.Header, CAN_MODULE_CLASS_SNS); ///TODO: Change this to the actual class type
  196.     StdCan_Set_direction(txMsg.Header, DIRECTIONFLAG_FROM_OWNER);
  197.     txMsg.Header.ModuleType = CAN_MODULE_TYPE_SNS_NMEA; ///TODO: Change this to the actual module type
  198.     txMsg.Header.ModuleId = sns_nmea_ID;
  199.     txMsg.Header.Command = CAN_MODULE_CMD_GLOBAL_LIST;
  200.     txMsg.Length = 6;
  201.  
  202.     txMsg.Data[0] = NODE_HW_ID_BYTE0;
  203.     txMsg.Data[1] = NODE_HW_ID_BYTE1;
  204.     txMsg.Data[2] = NODE_HW_ID_BYTE2;
  205.     txMsg.Data[3] = NODE_HW_ID_BYTE3;
  206.    
  207.     txMsg.Data[4] = NUMBER_OF_MODULES;
  208.     txMsg.Data[5] = ModuleSequenceNumber;
  209.    
  210.     while (StdCan_Put(&txMsg) != StdCan_Ret_OK);
  211. }
  212.  
  213. void nmea_send_can(uint8_t timer)
  214. {
  215.     StdCan_Msg_t txMsg;
  216.  
  217.     StdCan_Set_class(txMsg.Header, CAN_MODULE_CLASS_SNS);
  218.     StdCan_Set_direction(txMsg.Header, DIRECTIONFLAG_FROM_OWNER);
  219.     txMsg.Header.ModuleType = CAN_MODULE_TYPE_SNS_NMEA;
  220.     txMsg.Header.ModuleId = sns_nmea_ID;
  221.     txMsg.Header.Command = CAN_MODULE_CMD_NMEA_POSITION; //definiera nåt vettigt för det sen
  222.    
  223.     txMsg.Length = 8;
  224.     txMsg.Data[0] = NMEA_data.lat_deg;
  225.     txMsg.Data[1] = NMEA_data.lat_min;
  226.     txMsg.Data[2] = NMEA_data.lat_sec_h;   
  227.     txMsg.Data[3] = NMEA_data.lat_sec_l;
  228.  
  229.     txMsg.Data[4] = NMEA_data.lon_deg;
  230.     txMsg.Data[5] = NMEA_data.lon_min;
  231.     txMsg.Data[6] = NMEA_data.lon_sec_h;   
  232.     txMsg.Data[7] = NMEA_data.lon_sec_l;
  233.    
  234.     while (StdCan_Put(&txMsg) != StdCan_Ret_OK);
  235.    
  236.     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);
  237. }
  238.  
  239.