diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h index 8d7b5c1df..5ea16426b 100644 --- a/src/drivers/drv_gps.h +++ b/src/drivers/drv_gps.h @@ -53,8 +53,9 @@ typedef enum { GPS_DRIVER_MODE_NONE = 0, + GPS_DRIVER_MODE_OEM615, GPS_DRIVER_MODE_UBX, - GPS_DRIVER_MODE_MTK, - GPS_DRIVER_MODE_ASHTECH + GPS_DRIVER_MODE_MTK +// GPS_DRIVER_MODE_ASHTECH } gps_driver_mode_t; diff --git a/src/drivers/gps/CMakeLists.txt b/src/drivers/gps/CMakeLists.txt index 8e1ec4ece..26363faa4 100644 --- a/src/drivers/gps/CMakeLists.txt +++ b/src/drivers/gps/CMakeLists.txt @@ -42,6 +42,7 @@ px4_add_module( devices/src/mtk.cpp devices/src/ashtech.cpp devices/src/ubx.cpp + dgps/oem615.cpp DEPENDS platforms__common ) diff --git a/src/drivers/gps/dgps/oem615.cpp b/src/drivers/gps/dgps/oem615.cpp new file mode 100755 index 000000000..6135a0f41 --- /dev/null +++ b/src/drivers/gps/dgps/oem615.cpp @@ -0,0 +1,795 @@ +/* +* file : ome615.cpp +* author : bdai +* time : Jul 18, 2016 +* ref : OEM6 Family Firmware Reference Manual.pdf +*/ + +#include +#include +#include +#include +#include +#include + +#include "oem615.h" + +GPSDriverOEM615::GPSDriverOEM615(GPSCallbackPtr callback, void *callback_user, struct vehicle_gps_position_s *gps_position, + struct satellite_info_s *satellite_info): + GPSHelper(callback, callback_user), + _satellite_info(satellite_info), + _gps_position(gps_position), + _last_timestamp_time(0) +{ + decodeInit(); + _decode_state = OEM615_DECODE_UNINIT; + _rx_buffer_bytes = 0; +} + +GPSDriverOEM615::~GPSDriverOEM615(){ + +} + +int GPSDriverOEM615::handleMessage(int len){ + char * endp; + + if (len < 7) { return 0; } + + int uiCalcComma = 0; /* the number of msg's "," */ + + for (int i = 0 ; i < len; i++) { /* count the "," */ + if (_rx_buffer[i] == ',') { uiCalcComma++; } + } + + char *bufptr = (char *)(_rx_buffer + 6); + +// PX4_INFO("Buf is: %s",_rx_buffer); + int ret = 0; + + 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 oem615_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) != ',') { oem615_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 oem615_hour = static_cast(oem615_time / 10000); + int oem615_minute = static_cast((oem615_time - oem615_hour * 10000) / 100); + double oem615_sec = static_cast(oem615_time - oem615_hour * 10000 - oem615_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 = oem615_hour; + timeinfo.tm_min = oem615_minute; + timeinfo.tm_sec = int(oem615_sec); + +#ifndef NO_OEM515 + time_t epoch = mktime(&timeinfo); + if (epoch > GPS_EPOCH_SECS) { + uint64_t usecs = static_cast((oem615_sec - static_cast(oem615_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; + setClock(ts); + + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += usecs; + } else { + _gps_position->time_utc_usec = 0; + } +#else + _gps_position->time_utc_usec = 0; +#endif + _last_timestamp_time = gps_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 OEM615 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 oem615_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 = '?'; + +// double alt_ellipsoid = 0.0; +// float age_diff_corr = 0.0; +// char units1 = '?', units2 = '?'; + + if (bufptr && *(++bufptr) != ',') { oem615_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 (bufptr && *(++bufptr) != ',') { units1 = *(bufptr++); } +// if (bufptr && *(++bufptr) != ',') { alt_ellipsoid = strtod(bufptr, &endp); bufptr = endp; } +// if (bufptr && *(++bufptr) != ',') { units2 = *(bufptr++);} +// +// if (bufptr && *(++bufptr) != ',') { age_diff_corr = strtod(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(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000); + _gps_position->lon = static_cast((int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000); + _gps_position->alt = static_cast(alt * 1000); +// _gps_position->lat = 41.0; +// _gps_position->lon = 123.0; +// _gps_position->alt = 1.0; + + _gps_position->satellites_used = num_of_sv; +// PX4_INFO("num_of_sv: %d",num_of_sv); + PX4_INFO("oem time is: %8.4f",hrt_absolute_time()*1e-6); + + _gps_position->hdop = hdop; + + _rate_count_lat_lon++; + if (fix_quality <= 0) { + _gps_position->fix_type = 0; + + } else { + /* + * in this OEM615 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 = gps_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->vel_ned_valid = true; + _gps_position->c_variance_rad = 0.1f; + + ret = 1; + + } else if ((memcmp(_rx_buffer, "#BESTVELA,", 9) == 0) && (uiCalcComma == 16)) { + +// bufptr = (char *)(_rx_buffer + 9); +// double hor_spd = 0.0; +// double trk_gnd = 0.0; +// double vert_spd = 0.0; +// while(bufptr && *(++bufptr) != ',')++bufptr; /* com1 */ +// if (bufptr && *(++bufptr) != ',') { strtod(bufptr, &endp); bufptr = endp; } /* 0 */ +// if (bufptr && *(++bufptr) != ',') { strtod(bufptr, &endp); bufptr = endp; } /* 61.0 */ +// while (bufptr && *(++bufptr) != ',')++bufptr; /* FIN...ING */ +// if (bufptr && *(++bufptr) != ',') { strtod(bufptr, &endp); bufptr = endp; } /* 1337 */ +// if (bufptr && *(++bufptr) != ',') { strtod(bufptr, &endp); bufptr = endp; } /* 3341.00*/ +// if (bufptr && *(++bufptr) != ',') { strtod(bufptr, &endp); bufptr = endp; } /*000*/ +// if (bufptr && *(++bufptr) != ',') { strtod(bufptr, &endp); bufptr = endp; } /*827B--*/ +// if (bufptr && *(++bufptr) != ',') { strtod(bufptr, &endp); bufptr = endp; } /*1984*/ +// while (bufptr && *(++bufptr) != ',')++bufptr; /* SOL_COMPUTED */ +// while (bufptr && *(++bufptr) != ',')++bufptr; /* DOPPLER_VELOCITY */ +// if (bufptr && *(++bufptr) != ',') { strtod(bufptr, &endp); bufptr = endp; } /* latency */ +// if (bufptr && *(++bufptr) != ',') { strtod(bufptr, &endp); bufptr = endp; } /* age */ +// if (bufptr && *(++bufptr) != ',') { hor_spd = strtod(bufptr, &endp); bufptr = endp; } /* hor spd -- m/s */ +// if (bufptr && *(++bufptr) != ',') { trk_gnd = strtod(bufptr, &endp); bufptr = endp; } /* trk gnd */ +// if (bufptr && *(++bufptr) != ',') { vert_spd = strtod(bufptr, &endp); bufptr = endp; } /* vert spd -- m/s */ +// +// float track_rad = static_cast(trk_gnd) * M_PI_F / 180.0f; +// +// float velocity_north = static_cast(hor_spd) * cosf(track_rad); +// float velocity_east = static_cast(hor_spd) * sinf(track_rad); +// +// _gps_position->vel_m_s = hor_spd; /** 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(-vert_spd); /** 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(); + ret = 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); + + /* + * oem615 would return empty space as coordinate (lat, lon or alt) if it doesn't have a fix yet + */ + int coordinatesFound = 0; + double oem615_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) != ',') { oem615_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(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000); + _gps_position->lon = static_cast((int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000); + _gps_position->alt = static_cast(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 = gps_absolute_time(); + + float track_rad = static_cast(track_true) * M_PI_F / 180.0f; + + float velocity_ms = static_cast(ground_speed) / 1.9438445f; /** knots to m/s */ + float velocity_north = static_cast(velocity_ms) * cosf(track_rad); + float velocity_east = static_cast(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(-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; + ret = 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 oem615_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) != ',') { oem615_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(lat_err) * static_cast(lat_err) + + static_cast(lon_err) * static_cast(lon_err)); + _gps_position->epv = static_cast(alt_err); + + _gps_position->s_variance_m_s = 0; + + + } 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 == true)&& _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 = gps_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; + } + } + } + if(ret > 0){ + _gps_position->timestamp_time_relative = (int32_t)(_last_timestamp_time - _gps_position->timestamp); + } + + return ret; +} + +int GPSDriverOEM615::receive(unsigned timeout){ + uint8_t buf[GPS_READ_BUFFER_SIZE]; + + /* timeout additional to poll */ + uint64_t time_started = gps_absolute_time(); + + int j = 0; + ssize_t bytes_count = 0; +// PX4_INFO("oem time is: %8.4f",hrt_absolute_time()*1e-6); + while (true) { + + /* pass received bytes to the packet decoder */ + while (j < bytes_count) { + int l = 0; + + if ((l = parseChar(buf[j])) > 0) { + /* return to configure during configuration or to the gps driver during normal work + * if a packet has arrived */ + if (handleMessage(l) > 0) { + return 1; + } + } + + j++; + } + + /* everything is read */ + j = bytes_count = 0; + + /* then poll or read for new data */ + int ret = read(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 < gps_absolute_time()) { + return -1; + } + } +} +#define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA))) +int GPSDriverOEM615::parseChar(uint8_t b){ + int iRet = 0; + + switch (_decode_state) { + case OEM615_DECODE_UNINIT: /* First, look for sync1 */ + if (b == '$') { + _decode_state = NME_DECODE_GOT_SYNC1; + _rx_buffer_bytes = 0; + _rx_buffer[_rx_buffer_bytes++] = b; + } + if (b == '#') { + _decode_state = OEM615_DECODE_GOT_SYNC1; + _rx_buffer_bytes = 0; + _rx_buffer[_rx_buffer_bytes++] = b; + } + break; + case OEM615_DECODE_GOT_SYNC1: + if (b == '#') { /* next msg */ + _decode_state = OEM615_DECODE_GOT_SYNC1; + _rx_buffer_bytes = 0; + } else if (b == '*') { /* end of msg */ + _decode_state = OEM615_DECODE_GOT_ASTERIKS; + _crc_pos = 0; + } + + if (_rx_buffer_bytes >= (sizeof(_rx_buffer) - 9)) { + _decode_state = OEM615_DECODE_UNINIT; + _rx_buffer_bytes = 0; + } else { /* get msg's field */ + _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 = OEM615_DECODE_UNINIT; + _rx_buffer_bytes = 0; + } else { + _rx_buffer[_rx_buffer_bytes++] = b; + } + break; + case OEM615_DECODE_GOT_ASTERIKS: + _rx_buffer[_rx_buffer_bytes++] = b; + _crc_pos++; + if (_crc_pos == 7) + { + _decode_state = OEM615_DECODE_GOT_FIRST_CS_BYTE; + } + break; + case NME_DECODE_GOT_ASTERIKS: + _rx_buffer[_rx_buffer_bytes++] = b; /* crc1 */ + _decode_state = NME_DECODE_GOT_FIRST_CS_BYTE; + break; + case OEM615_DECODE_GOT_FIRST_CS_BYTE: + _rx_buffer[_rx_buffer_bytes++] = b; + { + uint32_t checksum = 0; + uint8_t *buffer = _rx_buffer + 1; /* skip "#" */ + uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 9;/* skip *cccc */ + uint32_t buflen = bufend - buffer; + checksum = CalculateCRC32(buflen, buffer); + if ((HEXDIGIT_CHAR((checksum >> 4) & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 2)) && + (HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))){ + iRet = _rx_buffer_bytes; + } + } + _decode_state = OEM615_DECODE_UNINIT; + _rx_buffer_bytes = 0; + break; + case NME_DECODE_GOT_FIRST_CS_BYTE: + _rx_buffer[_rx_buffer_bytes++] = b; /* crc2 */ + { + uint8_t checksum = 0; + uint8_t *buffer = _rx_buffer + 1; /* skip $ */ + uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3; /* skip *cc */ + + for (; buffer < bufend; buffer++) { checksum ^= *buffer; } + + if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) && /* *(_rx_buffer + _rx_buffer_bytes - 2)-->crc1 */ + (HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) { /* *(_rx_buffer + _rx_buffer_bytes - 1)-->crc2 */ + iRet = _rx_buffer_bytes; + } + } + _decode_state = OEM615_DECODE_UNINIT; + _rx_buffer_bytes = 0; + break; + } + return iRet; +} + +uint32_t GPSDriverOEM615::CalculateCRC32(uint32_t ulcount, uint8_t *ucBuffer) +{ + uint32_t ulTemp1; + uint32_t ulTemp2; + uint32_t ulCRC = 0; + while (ulcount-- != 0) + { + ulTemp1 = (ulCRC >> 8) & 0x00FFFFFFL; + ulTemp2 = CRC32Value(((int)ulCRC ^ *ucBuffer++) & 0xff); + ulCRC = ulTemp1 ^ ulTemp2; + } + return(ulCRC); +} +uint32_t GPSDriverOEM615::CRC32Value(int i) +{ + int j; + uint32_t ulCRC; + ulCRC = i; + for (j = 8; j > 0; j--) + { + if (ulCRC & 1) + ulCRC = (ulCRC >> 1) ^ CRC32_POLYNOMIAL; + else + ulCRC >>= 1; + } + return ulCRC; +} + +const char comm[] = "#BESTVELA,ON\r\n"\ + "$GPZDA,B,ON,3\r\n"\ + "$GPGGA,B,OFF\r\n"\ + "$GPGST,B,ON,3\r\n"\ + "$GPGSV,B,ON,3\r\n"\ + "OEM615-GPS-MODE\r\n"; +/* + +"$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 GPSDriverOEM615::configure(unsigned &baudrate, OutputMode output_mode) +{ + if (output_mode != OutputMode::GPS) { + GPS_WARN("oem615: Unsupported Output Mode %i", (int)output_mode); + return -1; + } + + /* 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]; + setBaudrate(baudrate); + + if (write(comm, sizeof(comm)) != sizeof(comm)) { + return -1; + } + } + + return setBaudrate(115200); +} + + +void GPSDriverOEM615::decodeInit(void) +{ + +} + diff --git a/src/drivers/gps/dgps/oem615.h b/src/drivers/gps/dgps/oem615.h new file mode 100755 index 000000000..145bea1df --- /dev/null +++ b/src/drivers/gps/dgps/oem615.h @@ -0,0 +1,61 @@ +/* +* file : ome615.h +* author : bdai +* time : Jul 18, 2016 +* ref : OEM6 Family Firmware Reference Manual.pdf +*/ + +#include "../devices/src/gps_helper.h" +#include "../definitions.h" + +#define OEM615_RECV_BUFFER_SIZE 512 +#define CRC32_POLYNOMIAL 0xEDB88320L /* for CRC32 */ + +class GPSDriverOEM615 : public GPSHelper{ +public: + GPSDriverOEM615(GPSCallbackPtr callback, void *callback_user, struct vehicle_gps_position_s *gps_position, + struct satellite_info_s *satellite_info); + virtual ~GPSDriverOEM615(); + + int receive(unsigned timeout); + int configure(unsigned &baudrate, OutputMode output_mode); + +private: + void decodeInit(void); + int handleMessage(int len); + int parseChar(uint8_t b); + uint32_t CalculateCRC32(uint32_t ulcount, uint8_t *ucBuffer); + uint32_t CRC32Value(int i); + + /** Read int OEM615 parameter */ + int32_t read_int(); + /** Read float OEM615 parameter */ + double read_float(); + /** Read char OEM615 parameter */ + char read_char(); + + enum oem615_decode_state_t { + OEM615_DECODE_UNINIT, + OEM615_DECODE_GOT_SYNC1, + NME_DECODE_GOT_SYNC1, + OEM615_DECODE_GOT_ASTERIKS, + NME_DECODE_GOT_ASTERIKS, + OEM615_DECODE_GOT_FIRST_CS_BYTE, + NME_DECODE_GOT_FIRST_CS_BYTE + }; + + struct satellite_info_s *_satellite_info; + struct vehicle_gps_position_s *_gps_position; + + uint64_t _last_timestamp_time; + int oem615log_fd; + + oem615_decode_state_t _decode_state; + uint8_t _crc_pos; + uint8_t _rx_buffer[OEM615_RECV_BUFFER_SIZE]; + uint16_t _rx_buffer_bytes; + bool _parse_error; /** parse error flag */ + char *_parse_pos; /** parse position */ + + +}; diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index e0c057170..8bc35212e 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include #include @@ -85,12 +86,16 @@ #include "devices/src/ubx.h" #include "devices/src/mtk.h" #include "devices/src/ashtech.h" +#include "dgps/oem615.h" +#include #define TIMEOUT_5HZ 500 -#define RATE_MEASUREMENT_PERIOD 5000000 +#define RATE_MEASUREMENT_PERIOD 1000000 #define GPS_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls +//static const float OFFSET[3] = {0.08, 0, 0.08}; +//static float Rbn[9]; /* class for dynamic allocation of satellite info data */ class GPS_Sat_Info @@ -139,6 +144,7 @@ class GPS int _gps_num; ///< number of GPS connected int _orb_inject_data_fd; +// int _att_sub; orb_advert_t _dump_communication_pub; ///< if non-null, dump communication gps_dump_s *_dump_to_device; @@ -258,6 +264,7 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num _fake_gps(fake_gps), _gps_num(gps_num), _orb_inject_data_fd(-1), +// _att_sub(-1), _dump_communication_pub(nullptr), _dump_to_device(nullptr), _dump_from_device(nullptr) @@ -644,8 +651,9 @@ GPS::task_main() } _orb_inject_data_fd = orb_subscribe(ORB_ID(gps_inject_data)); +// _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - initializeCommunicationDump(); + initializeCommunicationDump();/*what is means? -bdai<30 Aug 2016>*/ uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; @@ -688,17 +696,22 @@ GPS::task_main() } switch (_mode) { + case GPS_DRIVER_MODE_UBX: _helper = new GPSDriverUBX(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info); break; + case GPS_DRIVER_MODE_OEM615: + _helper = new GPSDriverOEM615(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info); + break; + case GPS_DRIVER_MODE_MTK: _helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos); break; - case GPS_DRIVER_MODE_ASHTECH: - _helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info); - break; +// case GPS_DRIVER_MODE_ASHTECH: +// _helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info); +// break; default: break; @@ -738,7 +751,7 @@ GPS::task_main() int helper_ret; while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { - +// PX4_INFO("mode:%d \tmain time is: %8.4f",_mode,hrt_absolute_time()*1e-6); if (helper_ret & 1) { publish(); @@ -797,15 +810,24 @@ GPS::task_main() /* select next mode */ switch (_mode) { + case GPS_DRIVER_MODE_UBX: - _mode = GPS_DRIVER_MODE_MTK; + _mode = GPS_DRIVER_MODE_OEM615; break; - case GPS_DRIVER_MODE_MTK: - _mode = GPS_DRIVER_MODE_ASHTECH; + case GPS_DRIVER_MODE_OEM615: + _mode = GPS_DRIVER_MODE_MTK; break; - case GPS_DRIVER_MODE_ASHTECH: +// case GPS_DRIVER_MODE_MTK: +// _mode = GPS_DRIVER_MODE_ASHTECH; +// break; +// +// case GPS_DRIVER_MODE_ASHTECH: +// _mode = GPS_DRIVER_MODE_UBX; +// break; + + case GPS_DRIVER_MODE_MTK: _mode = GPS_DRIVER_MODE_UBX; break; @@ -864,13 +886,17 @@ GPS::print_info() PX4_WARN("protocol: UBX"); break; + case GPS_DRIVER_MODE_OEM615: + PX4_WARN("protocol: OEM615"); + break; + case GPS_DRIVER_MODE_MTK: PX4_WARN("protocol: MTK"); break; - case GPS_DRIVER_MODE_ASHTECH: - PX4_WARN("protocol: ASHTECH"); - break; +// case GPS_DRIVER_MODE_ASHTECH: +// PX4_WARN("protocol: ASHTECH"); +// break; default: break; @@ -904,7 +930,40 @@ GPS::print_info() void GPS::publish() { + +// double lat,lon,alt; +// +// lat = _report_gps_pos.lat * 1e-7; +// lon = _report_gps_pos.lon * 1e-7; +// alt = _report_gps_pos.alt * 1e-3; +// +//// PX4_INFO("GPS Pre: %12.9f\t%12.9f\t%12.9f",lat,lon,alt); +// +// struct map_projection_reference_s map_ref; +// struct vehicle_attitude_s att; +// float offset[3] = {0, 0, 0}; +// bool att_updated = false; +// orb_check(_att_sub, &att_updated); +// if (att_updated){ +// orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); +// memcpy(&Rbn, &(att.R), sizeof(Rbn)); +// } +// for (int i = 0; i < 3; i++){ +// offset[i] = Rbn[i*3]*OFFSET[0] + Rbn[i*3 + 1]*OFFSET[1] + Rbn[i*3 + 2]*OFFSET[2]; +// } +// +// map_projection_init(&map_ref, lat, lon); +// +// map_projection_reproject(&map_ref, offset[0], offset[1], &lat, &lon); +// _report_gps_pos.lat = (int)(lat * 1e7); +// _report_gps_pos.lon = (int)(lon * 1e7); +// _report_gps_pos.alt = (int)((alt - (double)offset[2]) * 1e3); +// +//// PX4_INFO("GPS Aft: %12.9f\t%12.9f\t%12.9f",lat,lon,alt); +// PX4_INFO("GPS int: %d\t%d\t%d",_report_gps_pos.lat,_report_gps_pos.lon,_report_gps_pos.alt); + if (_gps_num == 1) { +// _report_gps_pos.satellites_used = 10; orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_gps_pos_pub, &_report_gps_pos, &_gps_orb_instance, ORB_PRIO_DEFAULT); is_gps1_advertised = true; diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 43e901a0e..699585a84 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -70,7 +70,7 @@ PARAM_DEFINE_FLOAT(EKF2_BARO_DELAY, 0); * @unit ms * @decimal 1 */ -PARAM_DEFINE_FLOAT(EKF2_GPS_DELAY, 200); +PARAM_DEFINE_FLOAT(EKF2_GPS_DELAY, 100); /** * Optical flow measurement delay relative to IMU measurements @@ -317,7 +317,7 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_V_NOISE, 0.5f); * @unit m * @decimal 2 */ -PARAM_DEFINE_FLOAT(EKF2_GPS_P_NOISE, 0.5f); +PARAM_DEFINE_FLOAT(EKF2_GPS_P_NOISE, 0.01f); /** * Measurement noise for non-aiding position hold. @@ -518,7 +518,7 @@ PARAM_DEFINE_INT32(EKF2_AID_MASK, 1); * @value 3 Vision * */ -PARAM_DEFINE_INT32(EKF2_HGT_MODE, 0); +PARAM_DEFINE_INT32(EKF2_HGT_MODE, 1); /** * Measurement noise for range finder fusion diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 61bb6a982..7672ace9d 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -67,6 +67,8 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _gps_delay(this, "GPS_DELAY"), _gps_xy_stddev(this, "GPS_XY"), _gps_z_stddev(this, "GPS_Z"), + _dgps_xy_stddev(this, "DGPS_XY"), + _dgps_z_stddev(this, "DGPS_Z"), _gps_vxy_stddev(this, "GPS_VXY"), _gps_vz_stddev(this, "GPS_VZ"), _gps_eph_max(this, "EPH_MAX"), @@ -139,6 +141,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : // reference altitudes _altOrigin(0), + _gpsbaroHgtDiff(0), _altOriginInitialized(false), _baroAltOrigin(0), _gpsAltOrigin(0), diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 761251b0d..5250fed77 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -265,6 +265,8 @@ class BlockLocalPositionEstimator : public control::SuperBlock BlockParamFloat _gps_delay; BlockParamFloat _gps_xy_stddev; BlockParamFloat _gps_z_stddev; + BlockParamFloat _dgps_xy_stddev; + BlockParamFloat _dgps_z_stddev; BlockParamFloat _gps_vxy_stddev; BlockParamFloat _gps_vz_stddev; BlockParamFloat _gps_eph_max; @@ -344,6 +346,7 @@ class BlockLocalPositionEstimator : public control::SuperBlock // reference altitudes float _altOrigin; + float _gpsbaroHgtDiff; bool _altOriginInitialized; float _baroAltOrigin; float _gpsAltOrigin; diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c index 3543377a1..5eaa208e1 100644 --- a/src/modules/local_position_estimator/params.c +++ b/src/modules/local_position_estimator/params.c @@ -160,6 +160,28 @@ PARAM_DEFINE_FLOAT(LPE_GPS_XY, 1.0f); */ PARAM_DEFINE_FLOAT(LPE_GPS_Z, 3.0f); +/** + * Minimum DGPS xy standard deviation, uses reported EPH if greater. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 5 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(LPE_DGPS_XY, 0.1f); + +/** + * Minimum DGPS z standard deviation, uses reported EPV if greater. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 200 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(LPE_DGPS_Z, 0.12f); + /** * GPS xy velocity standard deviation. * EPV used if greater than this value. diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp index c41bfed60..89abd5089 100644 --- a/src/modules/local_position_estimator/sensors/baro.cpp +++ b/src/modules/local_position_estimator/sensors/baro.cpp @@ -55,7 +55,8 @@ void BlockLocalPositionEstimator::baroCorrect() if (baroMeasure(y) != OK) { return; } // subtract baro origin alt - y -= _baroAltOrigin; + y -= (_baroAltOrigin + _gpsbaroHgtDiff); +// y -= _baroAltOrigin; // baro measurement matrix Matrix C; diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp index ff331072b..4b09cf54b 100644 --- a/src/modules/local_position_estimator/sensors/gps.cpp +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -118,17 +118,25 @@ void BlockLocalPositionEstimator::gpsCorrect() R.setZero(); // default to parameter, use gps cov if provided - float var_xy = _gps_xy_stddev.get() * _gps_xy_stddev.get(); - float var_z = _gps_z_stddev.get() * _gps_z_stddev.get(); float var_vxy = _gps_vxy_stddev.get() * _gps_vxy_stddev.get(); float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get(); + float var_xy = _gps_xy_stddev.get() * _gps_xy_stddev.get(); + float var_z = _gps_z_stddev.get() * _gps_z_stddev.get(); + + + if (_sub_gps.get().fix_type == 6){ + var_xy = _dgps_xy_stddev.get() * _dgps_xy_stddev.get(); + var_z = _dgps_z_stddev.get() * _dgps_z_stddev.get(); + } + +// PX4_INFO("var_xy is: %8.4f", (double)var_xy); // if field is not below minimum, set it to the value provided - if (_sub_gps.get().eph > _gps_xy_stddev.get()) { + if (_sub_gps.get().eph > var_xy) { var_xy = _sub_gps.get().eph * _sub_gps.get().eph; } - if (_sub_gps.get().epv > _gps_z_stddev.get()) { + if (_sub_gps.get().epv > var_z) { var_z = _sub_gps.get().epv * _sub_gps.get().epv; }