Code style fix

3 files changed, 605 insertions, 515 deletions

diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp
index d40b12721..7342d01d1 100644
--- a/src/drivers/gps/ashtech.cpp
+++ b/src/drivers/gps/ashtech.cpp
@@ -18,282 +18,343 @@
 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)
+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);
+	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)
+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);
+	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)
+	_fd(fd),
+	_satellite_info(satellite_info),
+	_gps_position(gps_position)
 {
-    decode_init();  
-    _decode_state = NME_DECODE_UNINIT;
-    _rx_buffer_bytes = 0;
+	decode_init();
+	_decode_state = NME_DECODE_UNINIT;
+	_rx_buffer_bytes = 0;
 }
 
-ASHTECH::~ASHTECH(){
+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  / 1.9438445;			/** knots to m/s */
+
+/*
+ * 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  / 1.9438445;			/** knots to m/s */
 		double velocity_north = velocity_ms * cos(track_rad);
 		double velocity_east  = velocity_ms * sin(track_rad);
 
@@ -301,230 +362,255 @@ int ASHTECH::handle_message(int len){
 		_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->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;
+		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){
+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));
-            }
-        }
-    }
-}
+	{
+		/* 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)))
@@ -557,8 +643,9 @@ int ASHTECH::parse_char(uint8_t b)
 			_decode_state = NME_DECODE_UNINIT;
 			_rx_buffer_bytes = 0;
 
-		} else
+		} else {
 			_rx_buffer[_rx_buffer_bytes++] = b;
+		}
 
 		break;
 
@@ -573,7 +660,7 @@ int ASHTECH::parse_char(uint8_t b)
 		uint8_t *buffer = _rx_buffer + 1;
 		uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3;
 
-		for (; buffer < bufend; buffer++) checksum ^= *buffer;
+		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))) {
@@ -588,32 +675,36 @@ int ASHTECH::parse_char(uint8_t b)
 	return iRet;
 }
 
-void ASHTECH::decode_init(void){
+void ASHTECH::decode_init(void)
+{
 
 }
 
-//ashtech boad configuration script 
-//char comm[] = "$PASHS,NME,GGA,A,ON,0.1\r\n"
+/* 
+ * ashtech boad configuration script 
+ */
+
 char comm[] = "$PASHS,POP,20\r\n"\
-              "$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.05\r\n"\
-              "$PASHS,NME,GSV,B,ON,3\r\n"\
-              "$PASHS,SPD,A,8\r\n"\
-              "$PASHS,SPD,B,9\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, 115200);
-    return 0;
+	      "$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.05\r\n"\
+	      "$PASHS,NME,GSV,B,ON,3\r\n"\
+	      "$PASHS,SPD,A,8\r\n"\
+	      "$PASHS,SPD,B,9\r\n";
+
+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, 115200);
+	return 0;
 }
diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h
index e8c5ffb4c..56f8571a4 100644
--- a/src/drivers/gps/ashtech.h
+++ b/src/drivers/gps/ashtech.h
@@ -67,8 +67,8 @@ class ASHTECH : public GPS_Helper
 	bool                  _parse_error; // parse error flag
 	char                 *_parse_pos; // parse position
 
-    bool	_gsv_in_progress;					// Indicates that gsv data parsing is in progress
-	//int     _satellites_count; 				// Number of satellites info parsed.
+	bool	_gsv_in_progress;			/**< Indicates that gsv data parsing is in progress */
+	//int     _satellites_count; 			/**< Number of satellites info parsed. */
 	uint8_t count;					/**< Number of satellites in satellite info */
 	uint8_t svid[SAT_INFO_MAX_SATELLITES]; 		/**< Space vehicle ID [1..255], see scheme below  */
 	uint8_t used[SAT_INFO_MAX_SATELLITES];		/**< 0: Satellite not used, 1: used for navigation */
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 2547cda97..5fb4b9ff8 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -343,7 +343,6 @@ GPS::task_main()
 				break;
 
 			case GPS_DRIVER_MODE_ASHTECH:
-
 				_Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info);
 				break;
 
@@ -408,9 +407,9 @@ GPS::task_main()
 							mode_str = "MTK";
 							break;
 
-					case GPS_DRIVER_MODE_ASHTECH:
-						mode_str = "ASHTECH";
-						break;
+						case GPS_DRIVER_MODE_ASHTECH:
+							mode_str = "ASHTECH";
+							break;
 
 						default:
 							break;
@@ -439,10 +438,10 @@ GPS::task_main()
 				break;
 
 			case GPS_DRIVER_MODE_MTK:
-			_mode = GPS_DRIVER_MODE_ASHTECH;
-			break;
+				_mode = GPS_DRIVER_MODE_ASHTECH;
+				break;
 
-		case GPS_DRIVER_MODE_ASHTECH:
+			case GPS_DRIVER_MODE_ASHTECH:
 				_mode = GPS_DRIVER_MODE_UBX;
 				break;