dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

gps: remove the old driver files

This commit is contained in:
Beat Küng 2016-04-19 17:05:37 +02:00 committed by Lorenz Meier
parent 7ef718912a
commit 1085f33f88
9 changed files with 1 additions and 3275 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

683
src/drivers/gps/ashtech.cpp
View File

@ -1,683 +0,0 @@
#include "ashtech.h"
#include <unistd.h>
#include <stdlib.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 <px4_time.h>
#include <px4_defines.h>
#ifdef __PX4_QURT
#include <dspal_time.h>
#endif
#include <fcntl.h>
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)
{
char *endp;
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.
*/
double ashtech_time = 0.0;
int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0,
local_time_off_min __attribute__((unused)) = 0;
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { day = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { month = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { year = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { local_time_off_hour = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { local_time_off_min = strtol(bufptr, &endp, 10); bufptr = endp; }
int ashtech_hour = static_cast<int>(ashtech_time / 10000);
int ashtech_minute = static_cast<int>((ashtech_time - ashtech_hour * 10000) / 100);
double ashtech_sec = static_cast<float>(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);
// TODO: this functionality is not available on the Snapdragon yet
#ifndef __PX4_QURT
time_t epoch = mktime(&timeinfo);
if (epoch > GPS_EPOCH_SECS) {
uint64_t usecs = static_cast<uint64_t>((ashtech_sec - static_cast<uint64_t>(ashtech_sec))) * 1000000;
// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
// and control its drift. Since we rely on the HRT for our monotonic
// clock, updating it from time to time is safe.
timespec ts;
ts.tv_sec = epoch;
ts.tv_nsec = usecs * 1000;
if (px4_clock_settime(CLOCK_REALTIME, &ts)) {
warn("failed setting clock");
}
_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
_gps_position->time_utc_usec += usecs;
} else {
_gps_position->time_utc_usec = 0;
}
#else
_gps_position->time_utc_usec = 0;
#endif
_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
*/
double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
int num_of_sv __attribute__((unused)) = 0, fix_quality = 0;
double hdop __attribute__((unused)) = 99.9;
char ns = '?', ew = '?';
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; }
if (ns == 'S') {
lat = -lat;
}
if (ew == 'W') {
lon = -lon;
}
/* convert from degrees, minutes and seconds to degrees * 1e7 */
_gps_position->lat = static_cast<int>((int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000);
_gps_position->lon = static_cast<int>((int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000);
_gps_position->alt = static_cast<int>(alt * 1000);
_rate_count_lat_lon++;
if (fix_quality <= 0) {
_gps_position->fix_type = 0;
} else {
/*
* in this NMEA message float integers (value 5) mode has higher value than fixed integers (value 4), whereas it provides lower quality,
* and since value 3 is not being used, I "moved" value 5 to 3 to add it to _gps_position->fix_type
*/
if (fix_quality == 5) { fix_quality = 3; }
/*
* fix quality 1 means just a normal 3D fix, so I'm subtracting 1 here. This way we'll have 3 for auto, 4 for DGPS, 5 for floats, 6 for fixed.
*/
_gps_position->fix_type = 3 + fix_quality - 1;
}
_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.1f;
_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);
/*
* Ashtech would return empty space as coordinate (lat, lon or alt) if it doesn't have a fix yet
*/
int coordinatesFound = 0;
double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
int num_of_sv __attribute__((unused)) = 0, fix_quality = 0;
double track_true = 0.0, ground_speed = 0.0 , age_of_corr __attribute__((unused)) = 0.0;
double hdop = 99.9, vdop = 99.9, pdop __attribute__((unused)) = 99.9,
tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0;
char ns = '?', ew = '?';
if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') {
/*
* if a coordinate is skipped (i.e. no fix), it either won't get into this block (two commas in a row)
* or strtod won't find anything and endp will point exactly where bufptr is. The same is for lon and alt.
*/
lat = strtod(bufptr, &endp);
if (bufptr != endp) {coordinatesFound++;}
bufptr = endp;
}
if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
if (bufptr && *(++bufptr) != ',') {
lon = strtod(bufptr, &endp);
if (bufptr != endp) {coordinatesFound++;}
bufptr = endp;
}
if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
if (bufptr && *(++bufptr) != ',') {
alt = strtod(bufptr, &endp);
if (bufptr != endp) {coordinatesFound++;}
bufptr = endp;
}
if (bufptr && *(++bufptr) != ',') { age_of_corr = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { track_true = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { ground_speed = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { vertic_vel = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { pdop = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { vdop = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { tdop = strtod(bufptr, &endp); bufptr = endp; }
if (ns == 'S') {
lat = -lat;
}
if (ew == 'W') {
lon = -lon;
}
_gps_position->lat = static_cast<int>((int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000);
_gps_position->lon = static_cast<int>((int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000);
_gps_position->alt = static_cast<int>(alt * 1000);
_gps_position->hdop = (float)hdop / 100.0f;
_gps_position->vdop = (float)vdop / 100.0f;
_rate_count_lat_lon++;
if (coordinatesFound < 3) {
_gps_position->fix_type = 0;
} else {
_gps_position->fix_type = 3 + fix_quality;
}
_gps_position->timestamp_position = hrt_absolute_time();
float track_rad = static_cast<float>(track_true) * M_PI_F / 180.0f;
float velocity_ms = static_cast<float>(ground_speed) / 1.9438445f; /** knots to m/s */
float velocity_north = static_cast<float>(velocity_ms) * cosf(track_rad);
float velocity_east = static_cast<float>(velocity_ms) * sinf(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 = static_cast<float>(-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.1f;
_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 *
*/
double ashtech_time __attribute__((unused)) = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0;
double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0,
deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0;
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { rms_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { maj_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { min_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { deg_from_north = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { lat_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { lon_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { alt_err = strtod(bufptr, &endp); bufptr = endp; }
_gps_position->eph = sqrtf(static_cast<float>(lat_err) * static_cast<float>(lat_err)
+ static_cast<float>(lon_err) * static_cast<float>(lon_err));
_gps_position->epv = static_cast<float>(alt_err);
_gps_position->s_variance_m_s = 0;
_gps_position->timestamp_variance = hrt_absolute_time();
} 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 = 0, this_msg_num = 0, tot_sv_visible = 0;
struct gsv_sat {
int svid;
int elevation;
int azimuth;
int snr;
} sat[4];
memset(sat, 0, sizeof(sat));
if (bufptr && *(++bufptr) != ',') { all_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { this_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { tot_sv_visible = strtol(bufptr, &endp, 10); bufptr = endp; }
if ((this_msg_num < 1) || (this_msg_num > all_msg_num)) {
return 0;
}
if (this_msg_num == 0 && bGPS && _satellite_info) {
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;
if (_satellite_info) {
_satellite_info->count = satellite_info_s::SAT_INFO_MAX_SATELLITES;
_satellite_info->timestamp = hrt_absolute_time();
}
}
if (_satellite_info) {
for (int y = 0 ; y < end ; y++) {
if (bufptr && *(++bufptr) != ',') { sat[y].svid = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { sat[y].elevation = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { sat[y].azimuth = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { sat[y].snr = strtol(bufptr, &endp, 10); bufptr = endp; }
_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)
{
{
uint8_t buf[32];
/* timeout additional to poll */
uint64_t time_started = hrt_absolute_time();
int j = 0;
ssize_t bytes_count = 0;
while (true) {
/* pass received bytes to the packet decoder */
while (j < bytes_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;
}
}
j++;
}
/* everything is read */
j = bytes_count = 0;
/* then poll or read for new data */
int ret = poll_or_read(_fd, buf, sizeof(buf), timeout * 2);
if (ret < 0) {
/* something went wrong when polling */
return -1;
} else if (ret == 0) {
/* Timeout while polling or just nothing read if reading, let's
* stay here, and use timeout below. */
} else if (ret > 0) {
/* if we have new data from GPS, go handle it */
bytes_count = ret;
}
/* in case we get crap from GPS or time out */
if (time_started + timeout * 1000 * 2 < hrt_absolute_time()) {
return -1;
}
}
}
}
#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 board configuration script
*/
const 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";
int ASHTECH::configure(unsigned &baudrate)
{
/* try different baudrates */
const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
for (unsigned 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);
if (write(_fd, comm, sizeof(comm)) != sizeof(comm)) {
return 1;
}
}
set_baudrate(_fd, 115200);
return 0;
}

96
src/drivers/gps/ashtech.h
View File

@ -1,96 +0,0 @@
/****************************************************************************
*
* 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
#endif
#include <uORB/topics/satellite_info.h>
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;
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[satellite_info_s::SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */
uint8_t used[satellite_info_s::SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */
uint8_t elevation[satellite_info_s::SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
uint8_t azimuth[satellite_info_s::SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
uint8_t snr[satellite_info_s::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_ */

1
src/drivers/gps/gps.cpp
View File

@ -639,6 +639,7 @@ GPS::task_main()
} else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
last_rate_count++;
}

210
src/drivers/gps/gps_helper.cpp
View File

@ -1,210 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
*
* 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.
*
****************************************************************************/
#ifndef __PX4_QURT
#include <termios.h>
#include <poll.h>
#else
#include <sys/ioctl.h>
#include <dev_fs_lib_serial.h>
#endif
#include <unistd.h>
#include <errno.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include "gps_helper.h"
/**
* @file gps_helper.cpp
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/
#define GPS_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls
float
GPS_Helper::get_position_update_rate()
{
return _rate_lat_lon;
}
float
GPS_Helper::get_velocity_update_rate()
{
return _rate_vel;
}
void
GPS_Helper::reset_update_rates()
{
_rate_count_vel = 0;
_rate_count_lat_lon = 0;
_interval_rate_start = hrt_absolute_time();
}
void
GPS_Helper::store_update_rates()
{
_rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
_rate_lat_lon = _rate_count_lat_lon / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
}
int
GPS_Helper::set_baudrate(const int &fd, unsigned baud)
{
#if __PX4_QURT
// TODO: currently QURT does not support configuration with termios.
dspal_serial_ioctl_data_rate data_rate;
switch (baud) {
case 9600: data_rate.bit_rate = DSPAL_SIO_BITRATE_9600; break;
case 19200: data_rate.bit_rate = DSPAL_SIO_BITRATE_19200; break;
case 38400: data_rate.bit_rate = DSPAL_SIO_BITRATE_38400; break;
case 57600: data_rate.bit_rate = DSPAL_SIO_BITRATE_57600; break;
case 115200: data_rate.bit_rate = DSPAL_SIO_BITRATE_115200; break;
default:
PX4_ERR("ERR: unknown baudrate: %d", baud);
return -EINVAL;
}
int ret = ::ioctl(fd, SERIAL_IOCTL_SET_DATA_RATE, (void *)&data_rate);
if (ret != 0) {
return ret;
}
#else
/* process baud rate */
int speed;
switch (baud) {
case 9600: speed = B9600; break;
case 19200: speed = B19200; break;
case 38400: speed = B38400; break;
case 57600: speed = B57600; break;
case 115200: speed = B115200; break;
default:
PX4_ERR("ERR: unknown baudrate: %d", baud);
return -EINVAL;
}
struct termios uart_config;
int termios_state;
/* fill the struct for the new configuration */
tcgetattr(fd, &uart_config);
/* clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* no parity, one stop bit */
uart_config.c_cflag &= ~(CSTOPB | PARENB);
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
warnx("ERR: %d (cfsetispeed)\n", termios_state);
return -1;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
warnx("ERR: %d (cfsetospeed)\n", termios_state);
return -1;
}
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR: %d (tcsetattr)\n", termios_state);
return -1;
}
#endif
return 0;
}
int
GPS_Helper::poll_or_read(int fd, uint8_t *buf, size_t buf_length, uint64_t timeout)
{
#ifndef __PX4_QURT
/* For non QURT, use the usual polling. */
pollfd fds[1];
fds[0].fd = fd;
fds[0].events = POLLIN;
/* Poll for new data, */
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
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. But don't read immediately
* by 1-2 bytes, wait for some more data to save expensive read() calls.
* If more bytes are available, we'll go back to poll() again.
*/
usleep(GPS_WAIT_BEFORE_READ * 1000);
return ::read(fd, buf, buf_length);
} else {
return -1;
}
} else {
return ret;
}
#else
/* For QURT, just use read for now, since this doesn't block, we need to slow it down
* just a bit. */
usleep(10000);
return ::read(fd, buf, buf_length);
#endif
}

88
src/drivers/gps/gps_helper.h
View File

@ -1,88 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* 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 gps_helper.h
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
#ifndef GPS_HELPER_H
#define GPS_HELPER_H
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
// TODO: this number seems wrong
#define GPS_EPOCH_SECS 1234567890ULL
class GPS_Helper
{
public:
GPS_Helper() {};
virtual ~GPS_Helper() {};
virtual int configure(unsigned &baud) = 0;
virtual int receive(unsigned timeout) = 0;
int set_baudrate(const int &fd, unsigned baud);
float get_position_update_rate();
float get_velocity_update_rate();
void reset_update_rates();
void store_update_rates();
/* This is an abstraction for the poll on serial used. The
* implementation is different for QURT than for POSIX and
* NuttX.
*
* @param fd: serial file descriptor
* @param buf: pointer to read buffer
* @param buf_length: size of read buffer
* @param timeout: timeout time in us
* @return: 0 for nothing read, or poll timed out
* < 0 for error
* > 0 number of bytes read
*/
int poll_or_read(int fd, uint8_t *buf, size_t buf_length, uint64_t timeout);
protected:
uint8_t _rate_count_lat_lon;
uint8_t _rate_count_vel;
float _rate_lat_lon = 0.0f;
float _rate_vel = 0.0f;
uint64_t _interval_rate_start;
};
#endif /* GPS_HELPER_H */

308
src/drivers/gps/mtk.cpp
View File

@ -1,308 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* 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 mtk.cpp
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
#include <px4_defines.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <math.h>
#include <string.h>
#include <assert.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include "mtk.h"
MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
_fd(fd),
_gps_position(gps_position),
_mtk_revision(0)
{
decode_init();
}
MTK::~MTK()
{
}
int
MTK::configure(unsigned &baudrate)
{
/* set baudrate first */
if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0) {
return -1;
}
baudrate = MTK_BAUDRATE;
/* Write config messages, don't wait for an answer */
if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
goto errout;
}
usleep(10000);
if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
goto errout;
}
usleep(10000);
if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
goto errout;
}
usleep(10000);
if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
goto errout;
}
usleep(10000);
if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
goto errout;
}
return 0;
errout:
warnx("mtk: config write failed");
return -1;
}
int
MTK::receive(unsigned timeout)
{
uint8_t buf[32];
gps_mtk_packet_t packet;
/* timeout additional to poll */
uint64_t time_started = hrt_absolute_time();
int j = 0;
while (true) {
int ret = poll_or_read(_fd, buf, sizeof(buf), timeout);
if (ret > 0) {
/* first read whatever is left */
if (j < ret) {
/* pass received bytes to the packet decoder */
while (j < ret) {
if (parse_char(buf[j], packet) > 0) {
handle_message(packet);
return 1;
}
j++;
}
/* everything is read */
j = 0;
}
} else {
usleep(20000);
}
/* in case we keep trying but only get crap from GPS */
if (time_started + timeout * 1000 < hrt_absolute_time()) {
return -1;
}
}
}
void
MTK::decode_init(void)
{
_rx_ck_a = 0;
_rx_ck_b = 0;
_rx_count = 0;
_decode_state = MTK_DECODE_UNINIT;
}
int
MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
{
int ret = 0;
if (_decode_state == MTK_DECODE_UNINIT) {
if (b == MTK_SYNC1_V16) {
_decode_state = MTK_DECODE_GOT_CK_A;
_mtk_revision = 16;
} else if (b == MTK_SYNC1_V19) {
_decode_state = MTK_DECODE_GOT_CK_A;
_mtk_revision = 19;
}
} else if (_decode_state == MTK_DECODE_GOT_CK_A) {
if (b == MTK_SYNC2) {
_decode_state = MTK_DECODE_GOT_CK_B;
} else {
// Second start symbol was wrong, reset state machine
decode_init();
}
} else if (_decode_state == MTK_DECODE_GOT_CK_B) {
// Add to checksum
if (_rx_count < 33) {
add_byte_to_checksum(b);
}
// Fill packet buffer
((uint8_t *)(&packet))[_rx_count] = b;
_rx_count++;
/* Packet size minus checksum, XXX ? */
if (_rx_count >= sizeof(packet)) {
/* Compare checksum */
if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
ret = 1;
} else {
ret = -1;
}
// Reset state machine to decode next packet
decode_init();
}
}
return ret;
}
void
MTK::handle_message(gps_mtk_packet_t &packet)
{
if (_mtk_revision == 16) {
_gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
_gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
} else if (_mtk_revision == 19) {
_gps_position->lat = packet.latitude; // both degrees*1e7
_gps_position->lon = packet.longitude; // both degrees*1e7
} else {
warnx("mtk: unknown revision");
_gps_position->lat = 0;
_gps_position->lon = 0;
// Indicate this data is not usable and bail out
_gps_position->eph = 1000.0f;
_gps_position->epv = 1000.0f;
_gps_position->fix_type = 0;
return;
}
_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
_gps_position->fix_type = packet.fix_type;
_gps_position->eph = packet.hdop / 100.0f; // from cm to m
_gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph
_gps_position->hdop = packet.hdop / 100.0f;
_gps_position->vdop = _gps_position->hdop;
_gps_position->vel_m_s = ((float)packet.ground_speed) / 100.0f; // from cm/s to m/s
_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
_gps_position->satellites_used = packet.satellites;
/* convert time and date information to unix timestamp */
struct tm timeinfo;
uint32_t timeinfo_conversion_temp;
timeinfo.tm_mday = packet.date / 10000;
timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 10000;
timeinfo.tm_mon = (timeinfo_conversion_temp / 100) - 1;
timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 100) + 100;
timeinfo.tm_hour = (packet.utc_time / 10000000);
timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 10000000;
timeinfo.tm_min = timeinfo_conversion_temp / 100000;
timeinfo_conversion_temp -= timeinfo.tm_min * 100000;
timeinfo.tm_sec = timeinfo_conversion_temp / 1000;
timeinfo_conversion_temp -= timeinfo.tm_sec * 1000;
// TODO: this functionality is not available on the Snapdragon yet
#ifndef __PX4_QURT
time_t epoch = mktime(&timeinfo);
if (epoch > GPS_EPOCH_SECS) {
// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
// and control its drift. Since we rely on the HRT for our monotonic
// clock, updating it from time to time is safe.
timespec ts;
ts.tv_sec = epoch;
ts.tv_nsec = timeinfo_conversion_temp * 1000000ULL;
if (px4_clock_settime(CLOCK_REALTIME, &ts)) {
warn("failed setting clock");
}
_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
_gps_position->time_utc_usec += timeinfo_conversion_temp * 1000ULL;
} else {
_gps_position->time_utc_usec = 0;
}
#else
_gps_position->time_utc_usec = 0;
#endif
_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
// Position and velocity update always at the same time
_rate_count_vel++;
_rate_count_lat_lon++;
return;
}
void
MTK::add_byte_to_checksum(uint8_t b)
{
_rx_ck_a = _rx_ck_a + b;
_rx_ck_b = _rx_ck_b + _rx_ck_a;
}

126
src/drivers/gps/mtk.h
View File

@ -1,126 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
*
* 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 mtk.cpp
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/
#ifndef MTK_H_
#define MTK_H_
#include "gps_helper.h"
#define MTK_SYNC1_V16 0xd0
#define MTK_SYNC1_V19 0xd1
#define MTK_SYNC2 0xdd
#define MTK_OUTPUT_5HZ "$PMTK220,200*2C\r\n"
#define MTK_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n"
#define SBAS_ON "$PMTK313,1*2E\r\n"
#define WAAS_ON "$PMTK301,2*2E\r\n"
#define MTK_NAVTHRES_OFF "$PMTK397,0*23\r\n"
#define MTK_TIMEOUT_5HZ 400
#define MTK_BAUDRATE 38400
typedef enum {
MTK_DECODE_UNINIT = 0,
MTK_DECODE_GOT_CK_A = 1,
MTK_DECODE_GOT_CK_B = 2
} mtk_decode_state_t;
/** the structures of the binary packets */
#pragma pack(push, 1)
typedef struct {
uint8_t payload; ///< Number of payload bytes
int32_t latitude; ///< Latitude in degrees * 10^7
int32_t longitude; ///< Longitude in degrees * 10^7
uint32_t msl_altitude; ///< MSL altitude in meters * 10^2
uint32_t ground_speed; ///< velocity in m/s
int32_t heading; ///< heading in degrees * 10^2
uint8_t satellites; ///< number of satellites used
uint8_t fix_type; ///< fix type: XXX correct for that
uint32_t date;
uint32_t utc_time;
uint16_t hdop; ///< horizontal dilution of position (without unit)
uint8_t ck_a;
uint8_t ck_b;
} gps_mtk_packet_t;
#pragma pack(pop)
#define MTK_RECV_BUFFER_SIZE 40
class MTK : public GPS_Helper
{
public:
MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
~MTK();
int receive(unsigned timeout);
int configure(unsigned &baudrate);
private:
/**
* Parse the binary MTK packet
*/
int parse_char(uint8_t b, gps_mtk_packet_t &packet);
/**
* Handle the package once it has arrived
*/
void handle_message(gps_mtk_packet_t &packet);
/**
* Reset the parse state machine for a fresh start
*/
void decode_init(void);
/**
* While parsing add every byte (except the sync bytes) to the checksum
*/
void add_byte_to_checksum(uint8_t);
int _fd;
struct vehicle_gps_position_s *_gps_position;
mtk_decode_state_t _decode_state;
uint8_t _mtk_revision;
unsigned _rx_count;
uint8_t _rx_ck_a;
uint8_t _rx_ck_b;
};
#endif /* MTK_H_ */

1189
src/drivers/gps/ubx.cpp
View File

File diff suppressed because it is too large Load Diff

575
src/drivers/gps/ubx.h
View File

@ -1,575 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
*
* 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 ubx.h
*
* U-Blox protocol definition. Following u-blox 6/7/8 Receiver Description
* including Prototol Specification.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* @author Hannes Delago
* (rework, add ubx7+ compatibility)
*
*/
#ifndef UBX_H_
#define UBX_H_
#include "gps_helper.h"
#define UBX_SYNC1 0xB5
#define UBX_SYNC2 0x62
/* Message Classes */
#define UBX_CLASS_NAV 0x01
#define UBX_CLASS_ACK 0x05
#define UBX_CLASS_CFG 0x06
#define UBX_CLASS_MON 0x0A
/* Message IDs */
#define UBX_ID_NAV_POSLLH 0x02
#define UBX_ID_NAV_DOP 0x04
#define UBX_ID_NAV_SOL 0x06
#define UBX_ID_NAV_PVT 0x07
#define UBX_ID_NAV_VELNED 0x12
#define UBX_ID_NAV_TIMEUTC 0x21
#define UBX_ID_NAV_SVINFO 0x30
#define UBX_ID_ACK_NAK 0x00
#define UBX_ID_ACK_ACK 0x01
#define UBX_ID_CFG_PRT 0x00
#define UBX_ID_CFG_MSG 0x01
#define UBX_ID_CFG_RATE 0x08
#define UBX_ID_CFG_NAV5 0x24
#define UBX_ID_CFG_SBAS 0x16
#define UBX_ID_MON_VER 0x04
#define UBX_ID_MON_HW 0x09
/* Message Classes & IDs */
#define UBX_MSG_NAV_POSLLH ((UBX_CLASS_NAV) | UBX_ID_NAV_POSLLH << 8)
#define UBX_MSG_NAV_SOL ((UBX_CLASS_NAV) | UBX_ID_NAV_SOL << 8)
#define UBX_MSG_NAV_DOP ((UBX_CLASS_NAV) | UBX_ID_NAV_DOP << 8)
#define UBX_MSG_NAV_PVT ((UBX_CLASS_NAV) | UBX_ID_NAV_PVT << 8)
#define UBX_MSG_NAV_VELNED ((UBX_CLASS_NAV) | UBX_ID_NAV_VELNED << 8)
#define UBX_MSG_NAV_TIMEUTC ((UBX_CLASS_NAV) | UBX_ID_NAV_TIMEUTC << 8)
#define UBX_MSG_NAV_SVINFO ((UBX_CLASS_NAV) | UBX_ID_NAV_SVINFO << 8)
#define UBX_MSG_ACK_NAK ((UBX_CLASS_ACK) | UBX_ID_ACK_NAK << 8)
#define UBX_MSG_ACK_ACK ((UBX_CLASS_ACK) | UBX_ID_ACK_ACK << 8)
#define UBX_MSG_CFG_PRT ((UBX_CLASS_CFG) | UBX_ID_CFG_PRT << 8)
#define UBX_MSG_CFG_MSG ((UBX_CLASS_CFG) | UBX_ID_CFG_MSG << 8)
#define UBX_MSG_CFG_RATE ((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8)
#define UBX_MSG_CFG_NAV5 ((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8)
#define UBX_MSG_CFG_SBAS ((UBX_CLASS_CFG) | UBX_ID_CFG_SBAS << 8)
#define UBX_MSG_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8)
#define UBX_MSG_MON_VER ((UBX_CLASS_MON) | UBX_ID_MON_VER << 8)
/* RX NAV-PVT message content details */
/* Bitfield "valid" masks */
#define UBX_RX_NAV_PVT_VALID_VALIDDATE 0x01 /**< validDate (Valid UTC Date) */
#define UBX_RX_NAV_PVT_VALID_VALIDTIME 0x02 /**< validTime (Valid UTC Time) */
#define UBX_RX_NAV_PVT_VALID_FULLYRESOLVED 0x04 /**< fullyResolved (1 = UTC Time of Day has been fully resolved (no seconds uncertainty)) */
/* Bitfield "flags" masks */
#define UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK 0x01 /**< gnssFixOK (A valid fix (i.e within DOP & accuracy masks)) */
#define UBX_RX_NAV_PVT_FLAGS_DIFFSOLN 0x02 /**< diffSoln (1 if differential corrections were applied) */
#define UBX_RX_NAV_PVT_FLAGS_PSMSTATE 0x1C /**< psmState (Power Save Mode state (see Power Management)) */
#define UBX_RX_NAV_PVT_FLAGS_HEADVEHVALID 0x20 /**< headVehValid (Heading of vehicle is valid) */
/* RX NAV-TIMEUTC message content details */
/* Bitfield "valid" masks */
#define UBX_RX_NAV_TIMEUTC_VALID_VALIDTOW 0x01 /**< validTOW (1 = Valid Time of Week) */
#define UBX_RX_NAV_TIMEUTC_VALID_VALIDKWN 0x02 /**< validWKN (1 = Valid Week Number) */
#define UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC 0x04 /**< validUTC (1 = Valid UTC Time) */
#define UBX_RX_NAV_TIMEUTC_VALID_UTCSTANDARD 0xF0 /**< utcStandard (0..15 = UTC standard identifier) */
/* TX CFG-PRT message contents */
#define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */
#define UBX_TX_CFG_PRT_PORTID_USB 0x03 /**< USB */
#define UBX_TX_CFG_PRT_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
#define UBX_TX_CFG_PRT_BAUDRATE 38400 /**< choose 38400 as GPS baudrate */
#define UBX_TX_CFG_PRT_INPROTOMASK 0x01 /**< UBX in */
#define UBX_TX_CFG_PRT_OUTPROTOMASK 0x01 /**< UBX out */
/* TX CFG-RATE message contents */
#define UBX_TX_CFG_RATE_MEASINTERVAL 200 /**< 200ms for 5Hz */
#define UBX_TX_CFG_RATE_NAVRATE 1 /**< cannot be changed */
#define UBX_TX_CFG_RATE_TIMEREF 0 /**< 0: UTC, 1: GPS time */
/* TX CFG-NAV5 message contents */
#define UBX_TX_CFG_NAV5_MASK 0x0005 /**< Only update dynamic model and fix mode */
#define UBX_TX_CFG_NAV5_DYNMODEL 7 /**< 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
#define UBX_TX_CFG_NAV5_FIXMODE 2 /**< 1 2D only, 2 3D only, 3 Auto 2D/3D */
/* TX CFG-SBAS message contents */
#define UBX_TX_CFG_SBAS_MODE_ENABLED 1 /**< SBAS enabled */
#define UBX_TX_CFG_SBAS_MODE_DISABLED 0 /**< SBAS disabled */
#define UBX_TX_CFG_SBAS_MODE UBX_TX_CFG_SBAS_MODE_DISABLED /**< SBAS enabled or disabled */
/* TX CFG-MSG message contents */
#define UBX_TX_CFG_MSG_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_TX_CFG_MSG_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_TX_CFG_MSG_RATE1_05HZ 10
/*** u-blox protocol binary message and payload definitions ***/
#pragma pack(push, 1)
/* General: Header */
typedef struct {
uint8_t sync1;
uint8_t sync2;
uint16_t msg;
uint16_t length;
} ubx_header_t;
/* General: Checksum */
typedef struct {
uint8_t ck_a;
uint8_t ck_b;
} ubx_checksum_t ;
/* Rx NAV-POSLLH */
typedef struct {
uint32_t iTOW; /**< GPS Time of Week [ms] */
int32_t lon; /**< Longitude [1e-7 deg] */
int32_t lat; /**< Latitude [1e-7 deg] */
int32_t height; /**< Height above ellipsoid [mm] */
int32_t hMSL; /**< Height above mean sea level [mm] */
uint32_t hAcc; /**< Horizontal accuracy estimate [mm] */
uint32_t vAcc; /**< Vertical accuracy estimate [mm] */
} ubx_payload_rx_nav_posllh_t;
/* Rx NAV-DOP */
typedef struct {
uint32_t iTOW; /**< GPS Time of Week [ms] */
uint16_t gDOP; /**< Geometric DOP [0.01] */
uint16_t pDOP; /**< Position DOP [0.01] */
uint16_t tDOP; /**< Time DOP [0.01] */
uint16_t vDOP; /**< Vertical DOP [0.01] */
uint16_t hDOP; /**< Horizontal DOP [0.01] */
uint16_t nDOP; /**< Northing DOP [0.01] */
uint16_t eDOP; /**< Easting DOP [0.01] */
} ubx_payload_rx_nav_dop_t;
/* Rx NAV-SOL */
typedef struct {
uint32_t iTOW; /**< GPS Time of Week [ms] */
int32_t fTOW; /**< Fractional part of iTOW (range: +/-500000) [ns] */
int16_t week; /**< GPS week */
uint8_t gpsFix; /**< GPSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */
uint8_t flags;
int32_t ecefX;
int32_t ecefY;
int32_t ecefZ;
uint32_t pAcc;
int32_t ecefVX;
int32_t ecefVY;
int32_t ecefVZ;
uint32_t sAcc;
uint16_t pDOP; /**< Position DOP [0.01] */
uint8_t reserved1;
uint8_t numSV; /**< Number of SVs used in Nav Solution */
uint32_t reserved2;
} ubx_payload_rx_nav_sol_t;
/* Rx NAV-PVT (ubx8) */
typedef struct {
uint32_t iTOW; /**< GPS Time of Week [ms] */
uint16_t year; /**< Year (UTC)*/
uint8_t month; /**< Month, range 1..12 (UTC) */
uint8_t day; /**< Day of month, range 1..31 (UTC) */
uint8_t hour; /**< Hour of day, range 0..23 (UTC) */
uint8_t min; /**< Minute of hour, range 0..59 (UTC) */
uint8_t sec; /**< Seconds of minute, range 0..60 (UTC) */
uint8_t valid; /**< Validity flags (see UBX_RX_NAV_PVT_VALID_...) */
uint32_t tAcc; /**< Time accuracy estimate (UTC) [ns] */
int32_t nano; /**< Fraction of second (UTC) [-1e9...1e9 ns] */
uint8_t fixType; /**< GNSSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GNSS + dead reckoning, 5 = time only fix */
uint8_t flags; /**< Fix Status Flags (see UBX_RX_NAV_PVT_FLAGS_...) */
uint8_t reserved1;
uint8_t numSV; /**< Number of SVs used in Nav Solution */
int32_t lon; /**< Longitude [1e-7 deg] */
int32_t lat; /**< Latitude [1e-7 deg] */
int32_t height; /**< Height above ellipsoid [mm] */
int32_t hMSL; /**< Height above mean sea level [mm] */
uint32_t hAcc; /**< Horizontal accuracy estimate [mm] */
uint32_t vAcc; /**< Vertical accuracy estimate [mm] */
int32_t velN; /**< NED north velocity [mm/s]*/
int32_t velE; /**< NED east velocity [mm/s]*/
int32_t velD; /**< NED down velocity [mm/s]*/
int32_t gSpeed; /**< Ground Speed (2-D) [mm/s] */
int32_t headMot; /**< Heading of motion (2-D) [1e-5 deg] */
uint32_t sAcc; /**< Speed accuracy estimate [mm/s] */
uint32_t headAcc; /**< Heading accuracy estimate (motion and vehicle) [1e-5 deg] */
uint16_t pDOP; /**< Position DOP [0.01] */
uint16_t reserved2;
uint32_t reserved3;
int32_t headVeh; /**< (ubx8+ only) Heading of vehicle (2-D) [1e-5 deg] */
uint32_t reserved4; /**< (ubx8+ only) */
} ubx_payload_rx_nav_pvt_t;
#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7 (sizeof(ubx_payload_rx_nav_pvt_t) - 8)
#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8 (sizeof(ubx_payload_rx_nav_pvt_t))
/* Rx NAV-TIMEUTC */
typedef struct {
uint32_t iTOW; /**< GPS Time of Week [ms] */
uint32_t tAcc; /**< Time accuracy estimate (UTC) [ns] */
int32_t nano; /**< Fraction of second, range -1e9 .. 1e9 (UTC) [ns] */
uint16_t year; /**< Year, range 1999..2099 (UTC) */
uint8_t month; /**< Month, range 1..12 (UTC) */
uint8_t day; /**< Day of month, range 1..31 (UTC) */
uint8_t hour; /**< Hour of day, range 0..23 (UTC) */
uint8_t min; /**< Minute of hour, range 0..59 (UTC) */
uint8_t sec; /**< Seconds of minute, range 0..60 (UTC) */
uint8_t valid; /**< Validity Flags (see UBX_RX_NAV_TIMEUTC_VALID_...) */
} ubx_payload_rx_nav_timeutc_t;
/* Rx NAV-SVINFO Part 1 */
typedef struct {
uint32_t iTOW; /**< GPS Time of Week [ms] */
uint8_t numCh; /**< Number of channels */
uint8_t globalFlags;
uint16_t reserved2;
} ubx_payload_rx_nav_svinfo_part1_t;
/* Rx NAV-SVINFO Part 2 (repeated) */
typedef struct {
uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */
uint8_t svid; /**< Satellite ID */
uint8_t flags;
uint8_t quality;
uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength) [dbHz] */
int8_t elev; /**< Elevation [deg] */
int16_t azim; /**< Azimuth [deg] */
int32_t prRes; /**< Pseudo range residual [cm] */
} ubx_payload_rx_nav_svinfo_part2_t;
/* Rx NAV-VELNED */
typedef struct {
uint32_t iTOW; /**< GPS Time of Week [ms] */
int32_t velN; /**< North velocity component [cm/s]*/
int32_t velE; /**< East velocity component [cm/s]*/
int32_t velD; /**< Down velocity component [cm/s]*/
uint32_t speed; /**< Speed (3-D) [cm/s] */
uint32_t gSpeed; /**< Ground speed (2-D) [cm/s] */
int32_t heading; /**< Heading of motion 2-D [1e-5 deg] */
uint32_t sAcc; /**< Speed accuracy estimate [cm/s] */
uint32_t cAcc; /**< Course / Heading accuracy estimate [1e-5 deg] */
} ubx_payload_rx_nav_velned_t;
/* Rx MON-HW (ubx6) */
typedef struct {
uint32_t pinSel;
uint32_t pinBank;
uint32_t pinDir;
uint32_t pinVal;
uint16_t noisePerMS;
uint16_t agcCnt;
uint8_t aStatus;
uint8_t aPower;
uint8_t flags;
uint8_t reserved1;
uint32_t usedMask;
uint8_t VP[25];
uint8_t jamInd;
uint16_t reserved3;
uint32_t pinIrq;
uint32_t pullH;
uint32_t pullL;
} ubx_payload_rx_mon_hw_ubx6_t;
/* Rx MON-HW (ubx7+) */
typedef struct {
uint32_t pinSel;
uint32_t pinBank;
uint32_t pinDir;
uint32_t pinVal;
uint16_t noisePerMS;
uint16_t agcCnt;
uint8_t aStatus;
uint8_t aPower;
uint8_t flags;
uint8_t reserved1;
uint32_t usedMask;
uint8_t VP[17];
uint8_t jamInd;
uint16_t reserved3;
uint32_t pinIrq;
uint32_t pullH;
uint32_t pullL;
} ubx_payload_rx_mon_hw_ubx7_t;
/* Rx MON-VER Part 1 */
typedef struct {
uint8_t swVersion[30];
uint8_t hwVersion[10];
} ubx_payload_rx_mon_ver_part1_t;
/* Rx MON-VER Part 2 (repeated) */
typedef struct {
uint8_t extension[30];
} ubx_payload_rx_mon_ver_part2_t;
/* Rx ACK-ACK */
typedef union {
uint16_t msg;
struct {
uint8_t clsID;
uint8_t msgID;
};
} ubx_payload_rx_ack_ack_t;
/* Rx ACK-NAK */
typedef union {
uint16_t msg;
struct {
uint8_t clsID;
uint8_t msgID;
};
} ubx_payload_rx_ack_nak_t;
/* Tx CFG-PRT */
typedef struct {
uint8_t portID;
uint8_t reserved0;
uint16_t txReady;
uint32_t mode;
uint32_t baudRate;
uint16_t inProtoMask;
uint16_t outProtoMask;
uint16_t flags;
uint16_t reserved5;
} ubx_payload_tx_cfg_prt_t;
/* Tx CFG-RATE */
typedef struct {
uint16_t measRate; /**< Measurement Rate, GPS measurements are taken every measRate milliseconds */
uint16_t navRate; /**< Navigation Rate, in number of measurement cycles. This parameter cannot be changed, and must be set to 1 */
uint16_t timeRef; /**< Alignment to reference time: 0 = UTC time, 1 = GPS time */
} ubx_payload_tx_cfg_rate_t;
/* Tx CFG-NAV5 */
typedef struct {
uint16_t mask;
uint8_t dynModel; /**< Dynamic Platform model: 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
uint8_t fixMode; /**< Position Fixing Mode: 1 2D only, 2 3D only, 3 Auto 2D/3D */
int32_t fixedAlt;
uint32_t fixedAltVar;
int8_t minElev;
uint8_t drLimit;
uint16_t pDop;
uint16_t tDop;
uint16_t pAcc;
uint16_t tAcc;
uint8_t staticHoldThresh;
uint8_t dgpsTimeOut;
uint8_t cnoThreshNumSVs; /**< (ubx7+ only, else 0) */
uint8_t cnoThresh; /**< (ubx7+ only, else 0) */
uint16_t reserved;
uint16_t staticHoldMaxDist; /**< (ubx8+ only, else 0) */
uint8_t utcStandard; /**< (ubx8+ only, else 0) */
uint8_t reserved3;
uint32_t reserved4;
} ubx_payload_tx_cfg_nav5_t;
/* tx cfg-sbas */
typedef struct {
uint8_t mode;
uint8_t usage;
uint8_t maxSBAS;
uint8_t scanmode2;
uint32_t scanmode1;
} ubx_payload_tx_cfg_sbas_t;
/* Tx CFG-MSG */
typedef struct {
union {
uint16_t msg;
struct {
uint8_t msgClass;
uint8_t msgID;
};
};
uint8_t rate;
} ubx_payload_tx_cfg_msg_t;
/* General message and payload buffer union */
typedef union {
ubx_payload_rx_nav_pvt_t payload_rx_nav_pvt;
ubx_payload_rx_nav_posllh_t payload_rx_nav_posllh;
ubx_payload_rx_nav_sol_t payload_rx_nav_sol;
ubx_payload_rx_nav_dop_t payload_rx_nav_dop;
ubx_payload_rx_nav_timeutc_t payload_rx_nav_timeutc;
ubx_payload_rx_nav_svinfo_part1_t payload_rx_nav_svinfo_part1;
ubx_payload_rx_nav_svinfo_part2_t payload_rx_nav_svinfo_part2;
ubx_payload_rx_nav_velned_t payload_rx_nav_velned;
ubx_payload_rx_mon_hw_ubx6_t payload_rx_mon_hw_ubx6;
ubx_payload_rx_mon_hw_ubx7_t payload_rx_mon_hw_ubx7;
ubx_payload_rx_mon_ver_part1_t payload_rx_mon_ver_part1;
ubx_payload_rx_mon_ver_part2_t payload_rx_mon_ver_part2;
ubx_payload_rx_ack_ack_t payload_rx_ack_ack;
ubx_payload_rx_ack_nak_t payload_rx_ack_nak;
ubx_payload_tx_cfg_prt_t payload_tx_cfg_prt;
ubx_payload_tx_cfg_rate_t payload_tx_cfg_rate;
ubx_payload_tx_cfg_nav5_t payload_tx_cfg_nav5;
ubx_payload_tx_cfg_sbas_t payload_tx_cfg_sbas;
ubx_payload_tx_cfg_msg_t payload_tx_cfg_msg;
} ubx_buf_t;
#pragma pack(pop)
/*** END OF u-blox protocol binary message and payload definitions ***/
/* Decoder state */
typedef enum {
UBX_DECODE_SYNC1 = 0,
UBX_DECODE_SYNC2,
UBX_DECODE_CLASS,
UBX_DECODE_ID,
UBX_DECODE_LENGTH1,
UBX_DECODE_LENGTH2,
UBX_DECODE_PAYLOAD,
UBX_DECODE_CHKSUM1,
UBX_DECODE_CHKSUM2
} ubx_decode_state_t;
/* Rx message state */
typedef enum {
UBX_RXMSG_IGNORE = 0,
UBX_RXMSG_HANDLE,
UBX_RXMSG_DISABLE,
UBX_RXMSG_ERROR_LENGTH
} ubx_rxmsg_state_t;
/* ACK state */
typedef enum {
UBX_ACK_IDLE = 0,
UBX_ACK_WAITING,
UBX_ACK_GOT_ACK,
UBX_ACK_GOT_NAK
} ubx_ack_state_t;
class UBX : public GPS_Helper
{
public:
UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info);
~UBX();
int receive(const unsigned timeout);
int configure(unsigned &baudrate);
private:
/**
* Parse the binary UBX packet
*/
int parse_char(const uint8_t b);
/**
* Start payload rx
*/
int payload_rx_init(void);
/**
* Add payload rx byte
*/
int payload_rx_add(const uint8_t b);
int payload_rx_add_nav_svinfo(const uint8_t b);
int payload_rx_add_mon_ver(const uint8_t b);
/**
* Finish payload rx
*/
int payload_rx_done(void);
/**
* Reset the parse state machine for a fresh start
*/
void decode_init(void);
/**
* While parsing add every byte (except the sync bytes) to the checksum
*/
void add_byte_to_checksum(const uint8_t);
/**
* Send a message
* @return true on success, false on write error (errno set)
*/
bool send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length);
/**
* Configure message rate
* @return true on success, false on write error
*/
bool configure_message_rate(const uint16_t msg, const uint8_t rate);
/**
* Calculate & add checksum for given buffer
*/
void calc_checksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum);
/**
* Wait for message acknowledge
*/
int wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report);
/**
* Calculate FNV1 hash
*/
uint32_t fnv1_32_str(uint8_t *str, uint32_t hval);
int _fd;
struct vehicle_gps_position_s *_gps_position;
struct satellite_info_s *_satellite_info;
bool _configured;
ubx_ack_state_t _ack_state;
bool _got_posllh;
bool _got_velned;
ubx_decode_state_t _decode_state;
uint16_t _rx_msg;
ubx_rxmsg_state_t _rx_state;
uint16_t _rx_payload_length;
uint16_t _rx_payload_index;
uint8_t _rx_ck_a;
uint8_t _rx_ck_b;
hrt_abstime _disable_cmd_last;
uint16_t _ack_waiting_msg;
ubx_buf_t _buf;
uint32_t _ubx_version;
bool _use_nav_pvt;
};
#endif /* UBX_H_ */