Skip to content

Commit

Permalink
adding MSG_NAV_HPPOSLLH for high precision RTK positioning + float64 …
Browse files Browse the repository at this point in the history 
…lon/lat/alt (PX4#136)
  • Loading branch information
@slgrobotics
slgrobotics authored Jul 11, 2023
1 parent 1f850de commit 261480c
Show file tree
Hide file tree
Showing 8 changed files with 120 additions and 67 deletions.
16 changes: 8 additions & 8 deletions src/ashtech.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -241,9 +241,10 @@ int GPSDriverAshtech::handleMessage(int len)
}

/* 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);
_gps_position->latitude_deg = static_cast<int>(lat * 0.01) + (lat * 0.01 - static_cast<int>(lat * 0.01)) * 100.0 / 60.0;
_gps_position->longitude_deg = static_cast<int>(lon * 0.01) + (lon * 0.01 - static_cast<int>
(lon * 0.01)) * 100.0 / 60.0;
_gps_position->altitude_msl_m = alt;
_rate_count_lat_lon++;

if (fix_quality <= 0) {
Expand Down Expand Up @@ -411,11 +412,10 @@ int GPSDriverAshtech::handleMessage(int len)
lon = -lon;
}

_gps_position->lat = static_cast<int>((static_cast<int>(lat * 0.01) + (lat * 0.01 - static_cast<int>(
lat * 0.01)) * 100.0 / 60.0) * 10000000);
_gps_position->lon = static_cast<int>((static_cast<int>(lon * 0.01) + (lon * 0.01 - static_cast<int>(
lon * 0.01)) * 100.0 / 60.0) * 10000000);
_gps_position->alt = static_cast<int>(alt * 1000);
_gps_position->latitude_deg = static_cast<int>(lat * 0.01) + (lat * 0.01 - static_cast<int>(lat * 0.01)) * 100.0 / 60.0;
_gps_position->longitude_deg = static_cast<int>(lon * 0.01) + (lon * 0.01 - static_cast<int>
(lon * 0.01)) * 100.0 / 60.0;
_gps_position->altitude_msl_m = alt;
_gps_position->hdop = static_cast<float>(hdop);
_gps_position->vdop = static_cast<float>(vdop);
_rate_count_lat_lon++;
Expand Down
8 changes: 4 additions & 4 deletions src/emlid_reach.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -285,10 +285,10 @@ GPSDriverEmlidReach::handleErbSentence()
_gps_position->timestamp = gps_absolute_time();

_last_POS_timeGPS = _erb_buff.payload.geodic_position.timeGPS;
_gps_position->lon = round(_erb_buff.payload.geodic_position.longitude * 1e7);
_gps_position->lat = round(_erb_buff.payload.geodic_position.latitude * 1e7);
_gps_position->alt_ellipsoid = round(_erb_buff.payload.geodic_position.altElipsoid * 1e3);
_gps_position->alt = round(_erb_buff.payload.geodic_position.altMeanSeaLevel * 1e3);
_gps_position->longitude_deg = _erb_buff.payload.geodic_position.longitude;
_gps_position->latitude_deg = _erb_buff.payload.geodic_position.latitude;
_gps_position->altitude_ellipsoid_m = _erb_buff.payload.geodic_position.altElipsoid;
_gps_position->altitude_msl_m = _erb_buff.payload.geodic_position.altMeanSeaLevel;

_rate_count_lat_lon++;

Expand Down
8 changes: 4 additions & 4 deletions src/femtomes.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,10 +98,10 @@ int GPSDriverFemto::handleMessage(int len)
memcpy(&_femto_uav_gps, _femto_msg.data, sizeof(femto_uav_gps_t));

_gps_position->time_utc_usec = _femto_uav_gps.time_utc_usec;
_gps_position->lat = _femto_uav_gps.lat;
_gps_position->lon = _femto_uav_gps.lon;
_gps_position->alt = _femto_uav_gps.alt;
_gps_position->alt_ellipsoid = _femto_uav_gps.alt_ellipsoid;
_gps_position->latitude_deg = _femto_uav_gps.lat / 1e7;
_gps_position->longitude_deg = _femto_uav_gps.lon / 1e7;
_gps_position->altitude_msl_m = _femto_uav_gps.alt / 1e3;
_gps_position->altitude_ellipsoid_m = _femto_uav_gps.alt_ellipsoid / 1e3;
_gps_position->s_variance_m_s = _femto_uav_gps.s_variance_m_s;
_gps_position->c_variance_rad = _femto_uav_gps.c_variance_rad;
_gps_position->eph = _femto_uav_gps.eph;
Expand Down
14 changes: 7 additions & 7 deletions src/mtk.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -217,17 +217,17 @@ void
GPSDriverMTK::handleMessage(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
_gps_position->latitude_deg = packet.latitude / 1e6; // from degrees*1e6 to degrees
_gps_position->longitude_deg = packet.longitude / 1e6; // from degrees*1e6 to degrees

} else if (_mtk_revision == 19) {
_gps_position->lat = packet.latitude; // both degrees*1e7
_gps_position->lon = packet.longitude; // both degrees*1e7
_gps_position->latitude_deg = packet.latitude / 1e7; // from degrees*1e7 to degrees
_gps_position->longitude_deg = packet.longitude / 1e7; // from degrees*1e7 to degrees

} else {
GPS_WARN("mtk: unknown revision");
_gps_position->lat = 0;
_gps_position->lon = 0;
_gps_position->latitude_deg = 0.0;
_gps_position->longitude_deg = 0.0;

// Indicate this data is not usable and bail out
_gps_position->eph = 1000.0f;
Expand All @@ -236,7 +236,7 @@ GPSDriverMTK::handleMessage(gps_mtk_packet_t &packet)
return;
}

_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
_gps_position->altitude_msl_m = packet.msl_altitude / 100.0; // from cm to meters
_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
Expand Down
18 changes: 9 additions & 9 deletions src/nmea.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -266,11 +266,11 @@ int GPSDriverNMEA::handleMessage(int len)
}

/* convert from degrees, minutes and seconds to degrees */
_gps_position->lon = static_cast<int>((int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000);
_gps_position->lat = static_cast<int>((int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000);
_gps_position->longitude_deg = int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0;
_gps_position->latitude_deg = int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0;
_gps_position->hdop = hdop;
_gps_position->alt = static_cast<int>(alt * 1000);
_gps_position->alt_ellipsoid = _gps_position->alt + static_cast<int>(geoid_h * 1000);
_gps_position->altitude_msl_m = (double)alt;
_gps_position->altitude_ellipsoid_m = (double)(alt + geoid_h);
_sat_num_gga = static_cast<int>(num_of_sv);


Expand Down Expand Up @@ -392,10 +392,10 @@ int GPSDriverNMEA::handleMessage(int len)
}

/* convert from degrees, minutes and seconds to degrees */
_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->latitude_deg = int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0;
_gps_position->longitude_deg = int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0;
_gps_position->hdop = hdop;
_gps_position->alt = static_cast<int>(alt * 1000);
_gps_position->altitude_msl_m = (double)alt;
_sat_num_gns = static_cast<int>(num_of_sv);

if (!_POS_received && (_last_POS_timeUTC < utc_time)) {
Expand Down Expand Up @@ -488,8 +488,8 @@ int GPSDriverNMEA::handleMessage(int len)
int nmea_mth = static_cast<int>((nmea_date - nmea_day * 10000) / 100);
int nmea_year = static_cast<int>(nmea_date - nmea_day * 10000 - nmea_mth * 100);
/* convert from degrees, minutes and seconds to degrees */
_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->latitude_deg = int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0;
_gps_position->longitude_deg = int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0;

_gps_position->vel_m_s = velocity_ms;
_gps_position->vel_n_m_s = velocity_north;
Expand Down
10 changes: 5 additions & 5 deletions src/sbf.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -525,11 +525,11 @@ int GPSDriverSBF::payloadRxDone()
_gps_position->satellites_used = 0;
}

_gps_position->lat = static_cast<int>(round(_buf.payload_pvt_geodetic.latitude * M_RAD_TO_DEG * 1e7));
_gps_position->lon = static_cast<int>(round(_buf.payload_pvt_geodetic.longitude * M_RAD_TO_DEG * 1e7));
_gps_position->alt_ellipsoid = static_cast<int>(round(_buf.payload_pvt_geodetic.height * 1000));
_gps_position->alt = static_cast<int>(round((_buf.payload_pvt_geodetic.height - static_cast<double>
(_buf.payload_pvt_geodetic.undulation)) * 1000));
_gps_position->latitude_deg = _buf.payload_pvt_geodetic.latitude * M_RAD_TO_DEG;
_gps_position->longitude_deg = _buf.payload_pvt_geodetic.longitude * M_RAD_TO_DEG;
_gps_position->altitude_ellipsoid_m = _buf.payload_pvt_geodetic.height;
_gps_position->altitude_msl_m = _buf.payload_pvt_geodetic.height - static_cast<double>
(_buf.payload_pvt_geodetic.undulation);

/* H and V accuracy are reported in 2DRMS, but based off the uBlox reporting we expect RMS.
* Devide by 100 from cm to m and in addition divide by 2 to get RMS. */
Expand Down
83 changes: 59 additions & 24 deletions src/ubx.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -664,6 +664,10 @@ int GPSDriverUBX::configureDevice(const GPSConfig &config, const int32_t uart2_b
// Send a new CFG-VALSET message to make sure it does not get too large
cfg_valset_msg_size = initCfgValset();
cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_NAV_PVT_I2C, 1, cfg_valset_msg_size);
cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_NAV_HPPOSLLH_I2C, 1, cfg_valset_msg_size);
cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_I2C,
_mode == UBXMode::RoverWithMovingBase || _mode == UBXMode::RoverWithMovingBaseUART1 ? 1 : 0,
cfg_valset_msg_size);
_use_nav_pvt = true;
cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_NAV_DOP_I2C, 1, cfg_valset_msg_size);
cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_NAV_SAT_I2C, (_satellite_info != nullptr) ? 10 : 0, cfg_valset_msg_size);
Expand Down Expand Up @@ -722,9 +726,6 @@ int GPSDriverUBX::configureDevice(const GPSConfig &config, const int32_t uart2_b
cfg_valset_msg_size = initCfgValset();
cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_UBX, 1, cfg_valset_msg_size);
cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_RTCM3X, 0, cfg_valset_msg_size);
// heading output period 1 second
cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART1, 1, cfg_valset_msg_size);
cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_USB, 1, cfg_valset_msg_size);
// enable RTCM input on uart2 + set baudrate
cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2_STOPBITS, 1, cfg_valset_msg_size);
cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2_DATABITS, 0, cfg_valset_msg_size);
Expand Down Expand Up @@ -783,8 +784,6 @@ int GPSDriverUBX::configureDevice(const GPSConfig &config, const int32_t uart2_b
cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_NMEA, 0, cfg_valset_msg_size);
cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_UBX, 1, cfg_valset_msg_size);
cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_RTCM3X, 0, cfg_valset_msg_size);
cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART1, 1, cfg_valset_msg_size);
cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_USB, 1, cfg_valset_msg_size);

if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
return -1;
Expand All @@ -803,10 +802,6 @@ int GPSDriverUBX::configureDevice(const GPSConfig &config, const int32_t uart2_b
cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_NMEA, 0, cfg_valset_msg_size);
cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_UBX, 1, cfg_valset_msg_size);
cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_RTCM3X, 1, cfg_valset_msg_size);

cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART1, 0, cfg_valset_msg_size);
cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART2, 0, cfg_valset_msg_size);

cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_0_UART1, 1, cfg_valset_msg_size);
cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_UART1, 1, cfg_valset_msg_size);
cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1074_UART1, 1, cfg_valset_msg_size);
Expand Down Expand Up @@ -1358,6 +1353,17 @@ GPSDriverUBX::payloadRxInit()

break;

case UBX_MSG_NAV_HPPOSLLH:
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_hpposllh_t)) {
_rx_state = UBX_RXMSG_ERROR_LENGTH;

} else if (!_configured) {
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured

}

break;

case UBX_MSG_NAV_TIMEUTC:
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t)) {
_rx_state = UBX_RXMSG_ERROR_LENGTH;
Expand Down Expand Up @@ -1413,8 +1419,8 @@ GPSDriverUBX::payloadRxInit()
break; // unconditionally handle this message

case UBX_MSG_MON_HW:
if ((_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t))
&& (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))
if ((_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */
&& (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t)) /* u-blox 7+ msg format */
&& (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_deprecated_t))) {
_rx_state = UBX_RXMSG_ERROR_LENGTH;

Expand Down Expand Up @@ -1882,13 +1888,20 @@ GPSDriverUBX::payloadRxDone()

_gps_position->satellites_used = _buf.payload_rx_nav_pvt.numSV;

_gps_position->lat = _buf.payload_rx_nav_pvt.lat;
_gps_position->lon = _buf.payload_rx_nav_pvt.lon;
_gps_position->alt = _buf.payload_rx_nav_pvt.hMSL;
_gps_position->alt_ellipsoid = _buf.payload_rx_nav_pvt.height;
if (_gps_position->fix_type < 6) {
// When RTK is active and solid (fix=6), these values will be filled by HPPOSLLH:
_gps_position->latitude_deg = _buf.payload_rx_nav_pvt.lat * 1e-7;
_gps_position->longitude_deg = _buf.payload_rx_nav_pvt.lon * 1e-7;
_gps_position->altitude_msl_m = _buf.payload_rx_nav_pvt.hMSL * 1e-3;
_gps_position->altitude_ellipsoid_m = _buf.payload_rx_nav_pvt.height * 1e-3;

_gps_position->eph = static_cast<float>(_buf.payload_rx_nav_pvt.hAcc) * 1e-3f;
_gps_position->epv = static_cast<float>(_buf.payload_rx_nav_pvt.vAcc) * 1e-3f;

_rate_count_lat_lon++;
_got_posllh = true;
}

_gps_position->eph = static_cast<float>(_buf.payload_rx_nav_pvt.hAcc) * 1e-3f;
_gps_position->epv = static_cast<float>(_buf.payload_rx_nav_pvt.vAcc) * 1e-3f;
_gps_position->s_variance_m_s = static_cast<float>(_buf.payload_rx_nav_pvt.sAcc) * 1e-3f;

_gps_position->vel_m_s = static_cast<float>(_buf.payload_rx_nav_pvt.gSpeed) * 1e-3f;
Expand Down Expand Up @@ -1943,9 +1956,6 @@ GPSDriverUBX::payloadRxDone()
_last_timestamp_time = _gps_position->timestamp;

_rate_count_vel++;
_rate_count_lat_lon++;

_got_posllh = true;
_got_velned = true;

ret = 1;
Expand All @@ -1970,12 +1980,12 @@ GPSDriverUBX::payloadRxDone()
case UBX_MSG_NAV_POSLLH:
UBX_TRACE_RXMSG("Rx NAV-POSLLH");

_gps_position->lat = _buf.payload_rx_nav_posllh.lat;
_gps_position->lon = _buf.payload_rx_nav_posllh.lon;
_gps_position->alt = _buf.payload_rx_nav_posllh.hMSL;
_gps_position->latitude_deg = _buf.payload_rx_nav_posllh.lat * 1e-7;
_gps_position->longitude_deg = _buf.payload_rx_nav_posllh.lon * 1e-7;
_gps_position->altitude_msl_m = _buf.payload_rx_nav_posllh.hMSL * 1e-3;
_gps_position->altitude_ellipsoid_m = _buf.payload_rx_nav_posllh.height * 1e-3;
_gps_position->eph = static_cast<float>(_buf.payload_rx_nav_posllh.hAcc) * 1e-3f; // from mm to m
_gps_position->epv = static_cast<float>(_buf.payload_rx_nav_posllh.vAcc) * 1e-3f; // from mm to m
_gps_position->alt_ellipsoid = _buf.payload_rx_nav_posllh.height;

_gps_position->timestamp = gps_absolute_time();

Expand All @@ -1985,6 +1995,31 @@ GPSDriverUBX::payloadRxDone()
ret = 1;
break;

case UBX_MSG_NAV_HPPOSLLH:
UBX_TRACE_RXMSG("Rx NAV-HPPOSLLH");

if (_buf.payload_rx_nav_hpposllh.flags == 0 && _gps_position->fix_type == 6) {
_gps_position->latitude_deg = _buf.payload_rx_nav_hpposllh.lat * 1e-7 + _buf.payload_rx_nav_hpposllh.latHp *
1e-9; // regular precision lat/lon (1e7), plus high precision (1e9)
_gps_position->longitude_deg = _buf.payload_rx_nav_hpposllh.lon * 1e-7 + _buf.payload_rx_nav_hpposllh.lonHp * 1e-9;
_gps_position->altitude_msl_m = _buf.payload_rx_nav_hpposllh.hMSL * 1e-3 + _buf.payload_rx_nav_hpposllh.hMSLHp *
1e-4; // regular precision altitude, mm, plus high precision components of altitude, 0.1 mm
_gps_position->altitude_ellipsoid_m = _buf.payload_rx_nav_hpposllh.height * 1e-3 + _buf.payload_rx_nav_hpposllh.heightHp
* 1e-4;
_gps_position->eph = static_cast<float>(_buf.payload_rx_nav_hpposllh.hAcc) *
1e-4f; // Accuracy estimates, convert from 0.1 mm to m
_gps_position->epv = static_cast<float>(_buf.payload_rx_nav_hpposllh.vAcc) * 1e-4f;

_gps_position->timestamp = gps_absolute_time();

_rate_count_lat_lon++;
_got_posllh = true;

ret = 1;
}

break;

case UBX_MSG_NAV_SOL:
UBX_TRACE_RXMSG("Rx NAV-SOL");

Expand Down
Loading

0 comments on commit 261480c

Please sign in to comment.