Ashtech GPS driver

1 files changed, 616 insertions, 0 deletions

diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp
new file mode 100644
index 000000000..d338ff8e5
--- /dev/null
+++ b/src/drivers/gps/ashtech.cpp
@@ -0,0 +1,616 @@
+#include "ashtech.h"
+
+#include <unistd.h>
+#include <stdio.h>
+#include <poll.h>
+#include <math.h>
+#include <string.h>
+#include <assert.h>
+#include <systemlib/err.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/satellite_info.h>
+#include <drivers/drv_hrt.h>
+
+#include <fcntl.h>
+#include <math.h>
+
+typedef double float64_t;
+typedef float  float32_t;
+
+char*  str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result)
+{
+  int8_t n=0;
+  int32_t d=0;
+  int8_t neg = 0;
+
+  if(*pos == '-') { neg = 1; pos++; }
+  else if(*pos == '+')     { pos++; }
+  else if(sign) return(NULL);
+  while(*pos >= '0' && *pos <= '9')
+  {
+     d = d*10 + (*(pos++) - '0');
+     n++;
+     if(n_max_digit>0 && n==n_max_digit) break;
+  }
+  if(n==0 || n>10) return(NULL);
+  if(neg) *result = -d;
+  else    *result =  d;
+  return((char *)pos);
+}
+
+char*  scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac,
+                       float64_t *result)
+{
+  float64_t f=0.0, div=1.0;
+  int32_t d_int;
+  int8_t n=0, isneg= 0;
+
+  if( *pos == '-' ) isneg= 1;
+  if((pos = str_scanDec(pos, sign, n_max_int, &d_int)) == NULL) return(NULL);
+  if(*(pos) == '.')
+  {
+     pos++;
+     while(*pos >= '0' && *pos <= '9')
+     {
+        f = f*(10.0) + (float64_t)(*(pos++) - '0');
+        div *= (0.1);
+        n++;
+        if(n_max_frac>0 && n==n_max_frac) break;
+     }
+  }
+  else if(n_max_frac > 0) return(NULL);
+  if( isneg ) *result = (float64_t)d_int - f*div;
+  else        *result = (float64_t)d_int + f*div;
+  return((char *)pos);
+}
+
+
+
+ASHTECH::ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info):
+_fd(fd),
+_satellite_info(satellite_info),
+_gps_position(gps_position)
+{
+    decode_init();  
+    _decode_state = NME_DECODE_UNINIT;
+    _rx_buffer_bytes = 0;
+}
+
+ASHTECH::~ASHTECH(){
+}
+//All NMEA descriptions are taken from
+//http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html
+int ASHTECH::handle_message(int len){
+  if(len < 7) return 0;
+  int uiCalcComma = 0;
+  for(int i = 0 ; i < len; i++){
+    if(_rx_buffer[i] == ',')uiCalcComma++;
+  }
+  char* bufptr = (char*)( _rx_buffer+6);
+
+  if((memcmp(_rx_buffer+3, "ZDA,",3) == 0)&&(uiCalcComma == 6)){
+      /*
+      UTC day, month, and year, and local time zone offset
+      An example of the ZDA message string is:
+
+      $GPZDA,172809.456,12,07,1996,00,00*45
+
+      ZDA message fields
+      Field	Meaning
+      0	Message ID $GPZDA
+      1	UTC
+      2	Day, ranging between 01 and 31
+      3	Month, ranging between 01 and 12
+      4	Year
+      5	Local time zone offset from GMT, ranging from 00 through �13 hours
+      6	Local time zone offset from GMT, ranging from 00 through 59 minutes
+      7	The checksum data, always begins with *
+      Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT.
+      */
+      float64_t ashtech_time = 0.0;
+      int day = 0, month = 0,year = 0,local_time_off_hour = 0,local_time_off_min = 0;
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &day);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &month);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &year);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_hour);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_min);
+
+
+      int ashtech_hour = ashtech_time/10000;
+      int ashtech_minute = (ashtech_time - ashtech_hour*10000)/100;
+      float64_t ashtech_sec = ashtech_time - ashtech_hour*10000 - ashtech_minute*100;
+      //convert to unix timestamp
+      struct tm timeinfo;
+      timeinfo.tm_year = year - 1900;
+      timeinfo.tm_mon = month - 1;
+      timeinfo.tm_mday = day;
+      timeinfo.tm_hour = ashtech_hour;
+      timeinfo.tm_min = ashtech_minute;
+      timeinfo.tm_sec = int(ashtech_sec);
+      time_t epoch = mktime(&timeinfo);
+
+      _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
+      _gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6);
+      _gps_position->timestamp_time = hrt_absolute_time();
+    }
+
+  else if((memcmp(_rx_buffer+3, "GGA,",3) == 0)&&(uiCalcComma == 14)){
+  /* 
+    Time, position, and fix related data
+    An example of the GBS message string is:
+
+    $GPGGA,172814.0,3723.46587704,N,12202.26957864,W,2,6,1.2,18.893,M,-25.669,M,2.0,0031*4F
+
+    Note - The data string exceeds the ASHTECH standard length.
+    GGA message fields
+    Field   Meaning
+    0   Message ID $GPGGA
+    1   UTC of position fix
+    2   Latitude
+    3   Direction of latitude: 
+    N: North 
+    S: South
+    4   Longitude
+    5   Direction of longitude: 
+    E: East 
+    W: West
+    6   GPS Quality indicator: 
+    0: Fix not valid 
+    1: GPS fix 
+    2: Differential GPS fix, OmniSTAR VBS 
+    4: Real-Time Kinematic, fixed integers 
+    5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK
+    7   Number of SVs in use, range from 00 through to 24+
+    8   HDOP
+    9   Orthometric height (MSL reference)
+    10  M: unit of measure for orthometric height is meters
+    11  Geoid separation
+    12  M: geoid separation measured in meters
+    13  Age of differential GPS data record, Type 1 or Type 9. Null field when DGPS is not used.
+    14  Reference station ID, range 0000-4095. A null field when any reference station ID is selected and no corrections are received1.
+    15  
+    The checksum data, always begins with *
+    Note - If a user-defined geoid model, or an inclined
+  */
+      float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
+      int num_of_sv = 0, fix_quality=0;
+      float64_t hdop = 99.9;
+      char ns = '?', ew = '?';
+
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat);
+      if(bufptr && *(++bufptr) != ',') ns = *(bufptr++);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon);
+      if(bufptr && *(++bufptr) != ',') ew = *(bufptr++);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &fix_quality);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&hdop);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt);
+
+      if(ns == 'S')
+        lat = -lat;
+      if(ew == 'W')
+        lon = -lon;
+
+      _gps_position->lat = (int(lat*0.01) + (lat*0.01 - int(lat*0.01))*100.0/60.0)*10000000;
+      _gps_position->lon = (int(lon*0.01) + (lon*0.01 - int(lon*0.01))*100.0/60.0)*10000000;
+      _gps_position->alt = alt*1000;
+      _rate_count_lat_lon++;
+
+      if((lat == 0.0) && (lon == 0.0) && (alt == 0.0)){
+        _gps_position->fix_type = 0;
+      }
+      else
+        _gps_position->fix_type = 3 + fix_quality;
+
+	  _gps_position->timestamp_position = hrt_absolute_time();
+
+      _gps_position->vel_m_s = 0;                                  /**< GPS ground speed (m/s) */
+      _gps_position->vel_n_m_s = 0;                                /**< GPS ground speed in m/s */
+      _gps_position->vel_e_m_s = 0;                                /**< GPS ground speed in m/s */
+      _gps_position->vel_d_m_s = 0;                                /**< GPS ground speed in m/s */
+      _gps_position->cog_rad = 0;                                  /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
+      _gps_position->vel_ned_valid = true;                         /**< Flag to indicate if NED speed is valid */
+      _gps_position->c_variance_rad = 0.1;
+      _gps_position->timestamp_velocity = hrt_absolute_time();
+      return 1;
+  }
+  else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) {
+  /*
+	Example $PASHR,POS,2,10,125410.00,5525.8138702,N,03833.9587380,E,131.555,1.0,0.0,0.007,-0.001,2.0,1.0,1.7,1.0,*34
+
+      $PASHR,POS,d1,d2,m3,m4,c5,m6,c7,f8,f9,f10,f11,f12,f13,f14,f15,f16,s17*cc 
+      Parameter Description Range 
+        d1 Position mode 0: standalone 
+                         1: differential 
+                         2: RTK float 
+                         3: RTK fixed 
+                         5: Dead reckoning 
+                         9: SBAS (see NPT setting) 
+        d2 Number of satellite used in position fix 0-99
+        m3 Current UTC time of position fix (hhmmss.ss) 000000.00-235959.99 
+        m4 Latitude of position (ddmm.mmmmmm) 0-90 degrees 00-59.9999999 minutes 
+        c5 Latitude sector N, S
+        m6 Longitude of position (dddmm.mmmmmm) 0-180 degrees 00-59.9999999 minutes 
+        c7 Longitude sector E,W
+        f8 Altitude above ellipsoid +9999.000
+        f9 Differential age (data link age), seconds 0.0-600.0
+        f10 True track/course over ground in degrees 0.0-359.9
+        f11 Speed over ground in knots 0.0-999.9
+        f12 Vertical velocity in decimeters per second +999.9 
+        f13 PDOP 0-99.9 
+        f14 HDOP 0-99.9 
+        f15 VDOP 0-99.9 
+        f16 TDOP 0-99.9 
+        s17 Reserved no data 
+        *cc Checksum 
+      */
+      bufptr = (char*)( _rx_buffer+10);
+      float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
+      int num_of_sv = 0, fix_quality=0;
+      float64_t track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0;
+      float64_t hdop = 99.9, vdop = 99.9,  pdop = 99.9, tdop = 99.9,vertic_vel = 0.0;
+      char ns = '?', ew = '?';
+
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &fix_quality);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat);
+      if(bufptr && *(++bufptr) != ',') ns = *(bufptr++);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon);
+      if(bufptr && *(++bufptr) != ',') ew = *(bufptr++);      
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&age_of_corr);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&vertic_vel);      
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&pdop);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&hdop);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&vdop);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&tdop);
+
+      if(ns == 'S')
+        lat = -lat;
+      if(ew == 'W')
+        lon = -lon;
+
+      _gps_position->lat = (int(lat*0.01) + (lat*0.01 - int(lat*0.01))*100.0/60.0)*10000000;
+      _gps_position->lon = (int(lon*0.01) + (lon*0.01 - int(lon*0.01))*100.0/60.0)*10000000;
+      _gps_position->alt = alt*1000;
+      _rate_count_lat_lon++;
+
+      if((lat == 0.0) && (lon == 0.0) && (alt == 0.0)){
+    	  _gps_position->fix_type = 0;
+      }
+      else
+         _gps_position->fix_type = 3 + fix_quality;
+
+      _gps_position->timestamp_position = hrt_absolute_time();
+   
+      const float64_t m_pi  = 3.14159265;
+
+      double track_rad = track_true * m_pi / 180.0;
+
+	  	double velocity_ms = ground_speed  / 3.6;
+		double velocity_north = velocity_ms * cos(track_rad);
+		double velocity_east  = velocity_ms * sin(track_rad);
+
+		_gps_position->vel_m_s = velocity_ms;                            /**< GPS ground speed (m/s) */
+		_gps_position->vel_n_m_s = velocity_north;                       /**< GPS ground speed in m/s */
+		_gps_position->vel_e_m_s = velocity_east;                        /**< GPS ground speed in m/s */
+		_gps_position->vel_d_m_s = -vertic_vel;                      /**< GPS ground speed in m/s */
+		_gps_position->cog_rad = track_rad;                              /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
+		_gps_position->vel_ned_valid = true;                             /**< Flag to indicate if NED speed is valid */
+		_gps_position->c_variance_rad = 0.1;
+		_gps_position->timestamp_velocity = hrt_absolute_time();
+      return 1;
+
+  }
+  else if((memcmp(_rx_buffer+3, "GST,",3) == 0)&&(uiCalcComma == 8)){
+  /* 
+    Position error statistics
+    An example of the GST message string is:
+
+    $GPGST,172814.0,0.006,0.023,0.020,273.6,0.023,0.020,0.031*6A
+
+    The Talker ID ($--) will vary depending on the satellite system used for the position solution:
+
+    $GP - GPS only
+    $GL - GLONASS only
+    $GN - Combined
+    GST message fields
+    Field   Meaning
+    0   Message ID $GPGST
+    1   UTC of position fix
+    2   RMS value of the pseudorange residuals; includes carrier phase residuals during periods of RTK (float) and RTK (fixed) processing
+    3   Error ellipse semi-major axis 1 sigma error, in meters
+    4   Error ellipse semi-minor axis 1 sigma error, in meters
+    5   Error ellipse orientation, degrees from true north
+    6   Latitude 1 sigma error, in meters
+    7   Longitude 1 sigma error, in meters
+    8   Height 1 sigma error, in meters
+    9   The checksum data, always begins with *
+  */
+      float64_t ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0;
+      float64_t min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0;
+
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&rms_err);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&maj_err);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&min_err);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&deg_from_north);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat_err);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon_err);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt_err);
+
+      _gps_position->eph = sqrt(lat_err*lat_err +lon_err*lon_err);
+      _gps_position->epv = alt_err;
+
+      _gps_position->s_variance_m_s = 0;
+      _gps_position->timestamp_variance = hrt_absolute_time();
+  }
+  else if((memcmp(_rx_buffer+3, "VTG,",3) == 0)&&(uiCalcComma == 9)){
+  /*
+    Track made good and speed over ground
+    An example of the VTG message string is:
+
+    $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48
+
+    VTG message fields
+    Field   Meaning
+    0   Message ID $GPVTG
+    1   Track made good (degrees true)
+    2   T: track made good is relative to true north
+    3   Track made good (degrees magnetic)
+    4   M: track made good is relative to magnetic north
+    5   Speed, in knots
+    6   N: speed is measured in knots
+    7   Speed over ground in kilometers/hour (kph)
+    8   K: speed over ground is measured in kph
+    9   The checksum data, always begins with *
+  */
+     /*float64_t track_true = 0.0, speed = 0.0, track_magnetic = 0.0, ground_speed = 0.0;
+     char true_north = '?', magnetic_north = '?', speed_in_knots = '?', speed_in_kph  = '?';
+     
+     if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true);
+     if(bufptr && *(++bufptr) != ',') true_north = *(bufptr++);
+     if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_magnetic);
+     if(bufptr && *(++bufptr) != ',') magnetic_north = *(bufptr++);
+     if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&speed);
+     if(bufptr && *(++bufptr) != ',') speed_in_knots = *(bufptr++);
+     if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed);
+     if(bufptr && *(++bufptr) != ',') speed_in_kph = *(bufptr++);
+
+     const float64_t dPI  = 3.14159265;
+		   float64_t tan_C = tan(track_true * dPI / 180.0);
+		   float64_t lat_ = sqrt(ground_speed*ground_speed/ (1+tan_C)); //km/hour
+		   float64_t lon_ = lat_*tan_C; //		 	                        //km/hour*/
+  }
+  else if((memcmp(_rx_buffer+3, "GSV,",3) == 0)){
+  /*
+    The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is:
+
+    $GPGSV,4,1,13,02,02,213,,03,-3,000,,11,00,121,,14,13,172,05*67
+
+    GSV message fields
+    Field   Meaning
+    0   Message ID $GPGSV
+    1   Total number of messages of this type in this cycle
+    2   Message number
+    3   Total number of SVs visible
+    4   SV PRN number
+    5   Elevation, in degrees, 90� maximum
+    6   Azimuth, degrees from True North, 000� through 359�
+    7   SNR, 00 through 99 dB (null when not tracking)
+    8-11    Information about second SV, same format as fields 4 through 7
+    12-15   Information about third SV, same format as fields 4 through 7
+    16-19   Information about fourth SV, same format as fields 4 through 7
+    20  The checksum data, always begins with *
+  */
+// currently process only gps, because do not know what 
+// Global satellite ID I should use for non GPS sats
+    bool bGPS = false;
+    if(memcmp(_rx_buffer, "$GP",3) != 0) 
+      return 0;
+    else
+      bGPS = true;
+
+    int all_msg_num, this_msg_num, tot_sv_visible;
+    struct gsv_sat{
+      int svid;
+      int elevation;
+      int azimuth;
+      int snr;
+    } sat[4];
+    memset(sat, 0, sizeof(sat));
+    
+    if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &all_msg_num);
+    if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &this_msg_num);
+    if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &tot_sv_visible);
+    if((this_msg_num<1) || (this_msg_num>all_msg_num)){
+      return 0;
+    }
+    if((this_msg_num == 0)&&(bGPS == true)){
+      memset(_satellite_info->svid,     0,sizeof(_satellite_info->svid));
+      memset(_satellite_info->used,     0,sizeof(_satellite_info->used));
+      memset(_satellite_info->snr,      0,sizeof(_satellite_info->snr));
+      memset(_satellite_info->elevation,0,sizeof(_satellite_info->elevation));
+      memset(_satellite_info->azimuth,  0,sizeof(_satellite_info->azimuth));
+    }
+
+    int end = 4;
+    if(this_msg_num == all_msg_num){
+    	end =  tot_sv_visible - (this_msg_num-1)*4;
+    	_gps_position->satellites_used = tot_sv_visible;
+    	_satellite_info->count = SAT_INFO_MAX_SATELLITES;
+    	_satellite_info->timestamp = hrt_absolute_time();
+    }
+    for(int y = 0 ; y < end ;y++){
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].svid);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].elevation);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].azimuth);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].snr);
+    
+      _satellite_info->svid[y+(this_msg_num-1)*4]      = sat[y].svid;
+      _satellite_info->used[y+(this_msg_num-1)*4]      = ((sat[y].snr>0)? true: false);
+      _satellite_info->snr[y+(this_msg_num-1)*4]       = sat[y].snr;
+      _satellite_info->elevation[y+(this_msg_num-1)*4] = sat[y].elevation;
+      _satellite_info->azimuth[y+(this_msg_num-1)*4]   = sat[y].azimuth;
+    }
+  }
+
+  return 0;
+}
+
+
+int ASHTECH::receive(unsigned timeout){
+{
+    /* poll descriptor */
+    pollfd fds[1];
+    fds[0].fd = _fd;
+    fds[0].events = POLLIN;
+
+    uint8_t buf[32];
+
+    /* timeout additional to poll */
+    uint64_t time_started = hrt_absolute_time();
+
+    int j = 0;
+    ssize_t count = 0;
+
+    while (true) {
+
+        /* pass received bytes to the packet decoder */
+        while (j < count) {
+            int l=0;
+            if ((l = parse_char(buf[j])) > 0) {
+                /* return to configure during configuration or to the gps driver during normal work
+                 * if a packet has arrived */
+                 if (handle_message(l) > 0)
+                    return 1;
+            }
+            /* in case we keep trying but only get crap from GPS */
+            if (time_started + timeout*1000*2 < hrt_absolute_time() ) {
+                return -1;
+            }
+            j++;
+        }
+
+        /* everything is read */
+        j = count = 0;
+
+        /* then poll for new data */
+        int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout*2);
+
+        if (ret < 0) {
+            /* something went wrong when polling */
+            return -1;
+
+        } else if (ret == 0) {
+            /* Timeout */
+            return -1;
+
+        } else if (ret > 0) {
+            /* if we have new data from GPS, go handle it */
+            if (fds[0].revents & POLLIN) {
+                /*
+                 * We are here because poll says there is some data, so this
+                 * won't block even on a blocking device.  If more bytes are
+                 * available, we'll go back to poll() again...
+                 */
+                count = ::read(_fd, buf, sizeof(buf));
+            }
+        }
+    }
+}
+
+}
+#define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA)))
+
+int ASHTECH::parse_char(uint8_t b)
+{
+	int iRet = 0;
+
+	switch (_decode_state) {
+		/* First, look for sync1 */
+	case NME_DECODE_UNINIT:
+		if (b == '$') {
+			_decode_state = NME_DECODE_GOT_SYNC1;
+			_rx_buffer_bytes = 0;
+			_rx_buffer[_rx_buffer_bytes++] = b;
+		}
+
+		break;
+
+	case NME_DECODE_GOT_SYNC1:
+		if (b == '$') {
+			_decode_state = NME_DECODE_GOT_SYNC1;
+			_rx_buffer_bytes = 0;
+
+		} else if (b == '*') {
+			_decode_state = NME_DECODE_GOT_ASTERIKS;
+		}
+
+		if (_rx_buffer_bytes >= (sizeof(_rx_buffer) - 5)) {
+			_decode_state = NME_DECODE_UNINIT;
+			_rx_buffer_bytes = 0;
+
+		} else
+			_rx_buffer[_rx_buffer_bytes++] = b;
+
+		break;
+
+	case NME_DECODE_GOT_ASTERIKS:
+		_rx_buffer[_rx_buffer_bytes++] = b;
+		_decode_state = NME_DECODE_GOT_FIRST_CS_BYTE;
+		break;
+
+	case NME_DECODE_GOT_FIRST_CS_BYTE:
+		_rx_buffer[_rx_buffer_bytes++] = b;
+		uint8_t checksum = 0;
+		uint8_t *buffer = _rx_buffer + 1;
+		uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3;
+
+		for (; buffer < bufend; buffer++) checksum ^= *buffer;
+
+		if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) &&
+		    (HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) {
+			iRet = _rx_buffer_bytes;
+		}
+
+		_decode_state = NME_DECODE_UNINIT;
+		_rx_buffer_bytes = 0;
+		break;
+	}
+
+	return iRet;
+}
+
+void ASHTECH::decode_init(void){
+
+}
+
+//ashtech boad configuration script 
+//char comm[] = "$PASHS,NME,GGA,A,ON,0.1\r\n"
+char comm[] = "$PASHS,NME,ZDA,B,ON,3\r\n"\
+              "$PASHS,NME,GGA,B,OFF\r\n"\
+              "$PASHS,NME,GST,B,ON,3\r\n"\
+              "$PASHS,NME,POS,B,ON,0.1\r\n"\
+              "$PASHS,NME,GSV,B,ON,3\r\n"\
+              "$PASHS,SPD,A,8\r\n"\
+              "$PASHS,SPD,B,7\r\n"; // default baud is 7
+
+int ASHTECH::configure(unsigned &baudrate){
+    /* try different baudrates */
+    const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
+
+    for (int baud_i = 0; baud_i < sizeof(baudrates_to_try)/sizeof(baudrates_to_try[0]); baud_i++) {
+      baudrate = baudrates_to_try[baud_i];
+      set_baudrate(_fd, baudrate);
+      write(_fd, (uint8_t*)comm, sizeof(comm));
+    }
+    set_baudrate(_fd, 38400);
+    return 0;
+}