aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/drivers/drv_gps.h3
-rw-r--r--src/drivers/gps/ashtech.cpp616
-rw-r--r--src/drivers/gps/ashtech.h96
-rw-r--r--src/drivers/gps/gps.cpp18
-rw-r--r--src/drivers/gps/module.mk1
-rw-r--r--src/modules/uORB/topics/vehicle_gps_position.h2
6 files changed, 734 insertions, 2 deletions
diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h
index e14f4e00d..76a211000 100644
--- a/src/drivers/drv_gps.h
+++ b/src/drivers/drv_gps.h
@@ -57,7 +57,8 @@
typedef enum {
GPS_DRIVER_MODE_NONE = 0,
GPS_DRIVER_MODE_UBX,
- GPS_DRIVER_MODE_MTK
+ GPS_DRIVER_MODE_MTK,
+ GPS_DRIVER_MODE_ASHTECH
} gps_driver_mode_t;
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;
+}
diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h
new file mode 100644
index 000000000..e8c5ffb4c
--- /dev/null
+++ b/src/drivers/gps/ashtech.h
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2013. All rights reserved.
+ * Author: Boriskin Aleksey <a.d.boriskin@gmail.com>
+ * Kistanov Alexander <akistanov@gramant.ru>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* @file ASHTECH protocol definitions */
+
+#ifndef ASHTECH_H_
+#define ASHTECH_H_
+
+#include "gps_helper.h"
+
+#ifndef RECV_BUFFER_SIZE
+#define RECV_BUFFER_SIZE 512
+
+#define SAT_INFO_MAX_SATELLITES 20
+#endif
+
+
+class ASHTECH : public GPS_Helper
+{
+ enum ashtech_decode_state_t {
+ NME_DECODE_UNINIT,
+ NME_DECODE_GOT_SYNC1,
+ NME_DECODE_GOT_ASTERIKS,
+ NME_DECODE_GOT_FIRST_CS_BYTE
+ };
+
+ int _fd;
+ struct satellite_info_s *_satellite_info;
+ struct vehicle_gps_position_s *_gps_position;
+ int ashtechlog_fd;//miklm
+
+ ashtech_decode_state_t _decode_state;
+ uint8_t _rx_buffer[RECV_BUFFER_SIZE];
+ uint16_t _rx_buffer_bytes;
+ 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.
+ 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 */
+ uint8_t elevation[SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
+ uint8_t azimuth[SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
+ uint8_t snr[SAT_INFO_MAX_SATELLITES]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
+
+public:
+ ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info);
+ ~ASHTECH();
+ int receive(unsigned timeout);
+ int configure(unsigned &baudrate);
+ void decode_init(void);
+ int handle_message(int len);
+ int parse_char(uint8_t b);
+ /** Read int ASHTECH parameter */
+ int32_t read_int();
+ /** Read float ASHTECH parameter */
+ double read_float();
+ /** Read char ASHTECH parameter */
+ char read_char();
+
+};
+
+#endif /* ASHTECH_H_ */
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 34dd63086..2547cda97 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -69,6 +69,7 @@
#include "ubx.h"
#include "mtk.h"
+#include "ashtech.h"
#define TIMEOUT_5HZ 500
@@ -341,6 +342,11 @@ GPS::task_main()
_Helper = new MTK(_serial_fd, &_report_gps_pos);
break;
+ case GPS_DRIVER_MODE_ASHTECH:
+
+ _Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info);
+ break;
+
default:
break;
}
@@ -402,6 +408,10 @@ GPS::task_main()
mode_str = "MTK";
break;
+ case GPS_DRIVER_MODE_ASHTECH:
+ mode_str = "ASHTECH";
+ break;
+
default:
break;
}
@@ -429,6 +439,10 @@ GPS::task_main()
break;
case GPS_DRIVER_MODE_MTK:
+ _mode = GPS_DRIVER_MODE_ASHTECH;
+ break;
+
+ case GPS_DRIVER_MODE_ASHTECH:
_mode = GPS_DRIVER_MODE_UBX;
break;
@@ -475,6 +489,10 @@ GPS::print_info()
warnx("protocol: MTK");
break;
+ case GPS_DRIVER_MODE_ASHTECH:
+ warnx("protocol: ASHTECH");
+ break;
+
default:
break;
}
diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk
index b00818424..4f99b0d3b 100644
--- a/src/drivers/gps/module.mk
+++ b/src/drivers/gps/module.mk
@@ -40,6 +40,7 @@ MODULE_COMMAND = gps
SRCS = gps.cpp \
gps_helper.cpp \
mtk.cpp \
+ ashtech.cpp \
ubx.cpp
MODULE_STACKSIZE = 1200
diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index 80d65cd69..43909e2fa 100644
--- a/src/modules/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -62,7 +62,7 @@ struct vehicle_gps_position_s {
uint64_t timestamp_variance;
float s_variance_m_s; /**< speed accuracy estimate m/s */
float c_variance_rad; /**< course accuracy estimate rad */
- uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
+ uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: Real-Time Kinematic, fixed integers, 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
float eph; /**< GPS HDOP horizontal dilution of position in m */
float epv; /**< GPS VDOP horizontal dilution of position in m */
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-03 22:15:35 +0000