/**
* Finish payload rx
*/
int // 0 = no message handled, 1 = message handled, 2 = sat info message handled
UBX::payload_rx_done(void)
{
int ret = 0;
// return if no message handled
if (_rx_state != UBX_RXMSG_HANDLE) {
return ret;
}
// handle message
switch (_rx_msg) {
case UBX_MSG_NAV_PVT:
UBX_TRACE_RXMSG("Rx NAV-PVT\n");
//Check if position fix flag is good
if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1)
{
_gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType;
_gps_position->vel_ned_valid = true;
}
else
{
_gps_position->fix_type = 0;
_gps_position->vel_ned_valid = false;
}
_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->eph = (float)_buf.payload_rx_nav_pvt.hAcc * 1e-3f;
_gps_position->epv = (float)_buf.payload_rx_nav_pvt.vAcc * 1e-3f;
_gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_pvt.sAcc * 1e-3f;
_gps_position->vel_m_s = (float)_buf.payload_rx_nav_pvt.gSpeed * 1e-3f;
_gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_pvt.velN * 1e-3f;
_gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_pvt.velE * 1e-3f;
_gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_pvt.velD * 1e-3f;
_gps_position->cog_rad = (float)_buf.payload_rx_nav_pvt.headMot * M_DEG_TO_RAD_F * 1e-5f;
_gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f;
//Check if time and date fix flags are good
if( (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE)
&& (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME)
&& (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED))
{
/* convert to unix timestamp */
struct tm timeinfo;
timeinfo.tm_year = _buf.payload_rx_nav_pvt.year - 1900;
timeinfo.tm_mon = _buf.payload_rx_nav_pvt.month - 1;
timeinfo.tm_mday = _buf.payload_rx_nav_pvt.day;
timeinfo.tm_hour = _buf.payload_rx_nav_pvt.hour;
timeinfo.tm_min = _buf.payload_rx_nav_pvt.min;
timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec;
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 = _buf.payload_rx_nav_pvt.nano;
if (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 += _buf.payload_rx_nav_timeutc.nano / 1000;
} else {
_gps_position->time_utc_usec = 0;
}
}
_gps_position->timestamp_time = hrt_absolute_time();
_gps_position->timestamp_velocity = hrt_absolute_time();
_gps_position->timestamp_variance = hrt_absolute_time();
_gps_position->timestamp_position = hrt_absolute_time();
_rate_count_vel++;
_rate_count_lat_lon++;
_got_posllh = true;
_got_velned = true;
ret = 1;
break;
case UBX_MSG_NAV_POSLLH:
UBX_TRACE_RXMSG("Rx NAV-POSLLH\n");
_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->eph = (float)_buf.payload_rx_nav_posllh.hAcc * 1e-3f; // from mm to m
_gps_position->epv = (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_position = hrt_absolute_time();
_rate_count_lat_lon++;
_got_posllh = true;
ret = 1;
break;
case UBX_MSG_NAV_SOL:
UBX_TRACE_RXMSG("Rx NAV-SOL\n");
_gps_position->fix_type = _buf.payload_rx_nav_sol.gpsFix;
_gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f; // from cm to m
_gps_position->satellites_used = _buf.payload_rx_nav_sol.numSV;
_gps_position->timestamp_variance = hrt_absolute_time();
ret = 1;
break;
case UBX_MSG_NAV_TIMEUTC:
UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n");
if(_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC)
{
// convert to unix timestamp
struct tm timeinfo;
timeinfo.tm_year = _buf.payload_rx_nav_timeutc.year - 1900;
timeinfo.tm_mon = _buf.payload_rx_nav_timeutc.month - 1;
timeinfo.tm_mday = _buf.payload_rx_nav_timeutc.day;
timeinfo.tm_hour = _buf.payload_rx_nav_timeutc.hour;
timeinfo.tm_min = _buf.payload_rx_nav_timeutc.min;
timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec;
time_t epoch = mktime(&timeinfo);
// only set the time if it makes sense
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 = _buf.payload_rx_nav_timeutc.nano;
if (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 += _buf.payload_rx_nav_timeutc.nano / 1000;
} else {
_gps_position->time_utc_usec = 0;
}
}
_gps_position->timestamp_time = hrt_absolute_time();
ret = 1;
break;
case UBX_MSG_NAV_SVINFO:
UBX_TRACE_RXMSG("Rx NAV-SVINFO\n");
// _satellite_info already populated by payload_rx_add_svinfo(), just add a timestamp
_satellite_info->timestamp = hrt_absolute_time();
ret = 2;
break;
case UBX_MSG_NAV_VELNED:
UBX_TRACE_RXMSG("Rx NAV-VELNED\n");
_gps_position->vel_m_s = (float)_buf.payload_rx_nav_velned.speed * 1e-2f;
_gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_velned.velN * 1e-2f; /* NED NORTH velocity */
_gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_velned.velE * 1e-2f; /* NED EAST velocity */
_gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_velned.velD * 1e-2f; /* NED DOWN velocity */
_gps_position->cog_rad = (float)_buf.payload_rx_nav_velned.heading * M_DEG_TO_RAD_F * 1e-5f;
_gps_position->c_variance_rad = (float)_buf.payload_rx_nav_velned.cAcc * M_DEG_TO_RAD_F * 1e-5f;
_gps_position->vel_ned_valid = true;
_gps_position->timestamp_velocity = hrt_absolute_time();
_rate_count_vel++;
_got_velned = true;
ret = 1;
break;
case UBX_MSG_MON_VER:
UBX_TRACE_RXMSG("Rx MON-VER\n");
ret = 1;
break;
case UBX_MSG_MON_HW:
UBX_TRACE_RXMSG("Rx MON-HW\n");
switch (_rx_payload_length) {
case sizeof(ubx_payload_rx_mon_hw_ubx6_t): /* u-blox 6 msg format */
_gps_position->noise_per_ms = _buf.payload_rx_mon_hw_ubx6.noisePerMS;
_gps_position->jamming_indicator = _buf.payload_rx_mon_hw_ubx6.jamInd;
ret = 1;
break;
case sizeof(ubx_payload_rx_mon_hw_ubx7_t): /* u-blox 7+ msg format */
_gps_position->noise_per_ms = _buf.payload_rx_mon_hw_ubx7.noisePerMS;
_gps_position->jamming_indicator = _buf.payload_rx_mon_hw_ubx7.jamInd;
ret = 1;
break;
default: // unexpected payload size:
ret = 0; // don't handle message
break;
}
break;
case UBX_MSG_ACK_ACK:
UBX_TRACE_RXMSG("Rx ACK-ACK\n");
if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) {
_ack_state = UBX_ACK_GOT_ACK;
}
ret = 1;
break;
case UBX_MSG_ACK_NAK:
UBX_TRACE_RXMSG("Rx ACK-NAK\n");
if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) {
_ack_state = UBX_ACK_GOT_NAK;
}
ret = 1;
break;
default:
break;
}
return ret;
}
/**
* Finish payload rx
*/
int // 0 = no message handled, 1 = message handled, 2 = sat info message handled
UBX::payload_rx_done(void)
{
int ret = 0;
// return if no message handled
if (_rx_state != UBX_RXMSG_HANDLE) {
return ret;
}
// handle message
switch (_rx_msg) {
case UBX_MSG_NAV_PVT:
UBX_TRACE_RXMSG("Rx NAV-PVT\n");
_gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType;
_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->eph = (float)_buf.payload_rx_nav_pvt.hAcc * 1e-3f;
_gps_position->epv = (float)_buf.payload_rx_nav_pvt.vAcc * 1e-3f;
_gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_pvt.sAcc * 1e-3f;
_gps_position->vel_m_s = (float)_buf.payload_rx_nav_pvt.gSpeed * 1e-3f;
_gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_pvt.velN * 1e-3f;
_gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_pvt.velE * 1e-3f;
_gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_pvt.velD * 1e-3f;
_gps_position->vel_ned_valid = true;
_gps_position->cog_rad = (float)_buf.payload_rx_nav_pvt.headMot * M_DEG_TO_RAD_F * 1e-5f;
_gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f;
{
/* convert to unix timestamp */
struct tm timeinfo;
timeinfo.tm_year = _buf.payload_rx_nav_pvt.year - 1900;
timeinfo.tm_mon = _buf.payload_rx_nav_pvt.month - 1;
timeinfo.tm_mday = _buf.payload_rx_nav_pvt.day;
timeinfo.tm_hour = _buf.payload_rx_nav_pvt.hour;
timeinfo.tm_min = _buf.payload_rx_nav_pvt.min;
timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec;
time_t epoch = mktime(&timeinfo);
#ifndef CONFIG_RTC
//Since we lack a hardware RTC, set the system time clock based on GPS UTC
//TODO generalize this by moving into gps.cpp?
timespec ts;
ts.tv_sec = epoch;
ts.tv_nsec = _buf.payload_rx_nav_pvt.nano;
clock_settime(CLOCK_REALTIME, &ts);
#endif
_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
_gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_pvt.nano * 1e-3f);
}
_gps_position->timestamp_time = hrt_absolute_time();
_gps_position->timestamp_velocity = hrt_absolute_time();
_gps_position->timestamp_variance = hrt_absolute_time();
_gps_position->timestamp_position = hrt_absolute_time();
_rate_count_vel++;
_rate_count_lat_lon++;
_got_posllh = true;
_got_velned = true;
ret = 1;
break;
case UBX_MSG_NAV_POSLLH:
UBX_TRACE_RXMSG("Rx NAV-POSLLH\n");
_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->eph = (float)_buf.payload_rx_nav_posllh.hAcc * 1e-3f; // from mm to m
_gps_position->epv = (float)_buf.payload_rx_nav_posllh.vAcc * 1e-3f; // from mm to m
_gps_position->timestamp_position = hrt_absolute_time();
_rate_count_lat_lon++;
_got_posllh = true;
ret = 1;
break;
case UBX_MSG_NAV_SOL:
UBX_TRACE_RXMSG("Rx NAV-SOL\n");
_gps_position->fix_type = _buf.payload_rx_nav_sol.gpsFix;
_gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f; // from cm to m
_gps_position->satellites_used = _buf.payload_rx_nav_sol.numSV;
_gps_position->timestamp_variance = hrt_absolute_time();
ret = 1;
break;
case UBX_MSG_NAV_TIMEUTC:
UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n");
{
/* convert to unix timestamp */
struct tm timeinfo;
timeinfo.tm_year = _buf.payload_rx_nav_timeutc.year - 1900;
timeinfo.tm_mon = _buf.payload_rx_nav_timeutc.month - 1;
timeinfo.tm_mday = _buf.payload_rx_nav_timeutc.day;
timeinfo.tm_hour = _buf.payload_rx_nav_timeutc.hour;
timeinfo.tm_min = _buf.payload_rx_nav_timeutc.min;
timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec;
time_t epoch = mktime(&timeinfo);
#ifndef CONFIG_RTC
//Since we lack a hardware RTC, set the system time clock based on GPS UTC
//TODO generalize this by moving into gps.cpp?
timespec ts;
ts.tv_sec = epoch;
ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano;
clock_settime(CLOCK_REALTIME, &ts);
#endif
_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
_gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_timeutc.nano * 1e-3f);
}
_gps_position->timestamp_time = hrt_absolute_time();
ret = 1;
break;
case UBX_MSG_NAV_SVINFO:
UBX_TRACE_RXMSG("Rx NAV-SVINFO\n");
// _satellite_info already populated by payload_rx_add_svinfo(), just add a timestamp
_satellite_info->timestamp = hrt_absolute_time();
ret = 2;
break;
case UBX_MSG_NAV_VELNED:
UBX_TRACE_RXMSG("Rx NAV-VELNED\n");
_gps_position->vel_m_s = (float)_buf.payload_rx_nav_velned.speed * 1e-2f;
_gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_velned.velN * 1e-2f; /* NED NORTH velocity */
_gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_velned.velE * 1e-2f; /* NED EAST velocity */
_gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_velned.velD * 1e-2f; /* NED DOWN velocity */
_gps_position->cog_rad = (float)_buf.payload_rx_nav_velned.heading * M_DEG_TO_RAD_F * 1e-5f;
_gps_position->c_variance_rad = (float)_buf.payload_rx_nav_velned.cAcc * M_DEG_TO_RAD_F * 1e-5f;
_gps_position->vel_ned_valid = true;
_gps_position->timestamp_velocity = hrt_absolute_time();
_rate_count_vel++;
_got_velned = true;
ret = 1;
break;
case UBX_MSG_MON_VER:
UBX_TRACE_RXMSG("Rx MON-VER\n");
ret = 1;
break;
case UBX_MSG_MON_HW:
UBX_TRACE_RXMSG("Rx MON-HW\n");
switch (_rx_payload_length) {
case sizeof(ubx_payload_rx_mon_hw_ubx6_t): /* u-blox 6 msg format */
_gps_position->noise_per_ms = _buf.payload_rx_mon_hw_ubx6.noisePerMS;
_gps_position->jamming_indicator = _buf.payload_rx_mon_hw_ubx6.jamInd;
ret = 1;
break;
case sizeof(ubx_payload_rx_mon_hw_ubx7_t): /* u-blox 7+ msg format */
_gps_position->noise_per_ms = _buf.payload_rx_mon_hw_ubx7.noisePerMS;
_gps_position->jamming_indicator = _buf.payload_rx_mon_hw_ubx7.jamInd;
ret = 1;
break;
default: // unexpected payload size:
ret = 0; // don't handle message
break;
}
break;
case UBX_MSG_ACK_ACK:
UBX_TRACE_RXMSG("Rx ACK-ACK\n");
if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) {
_ack_state = UBX_ACK_GOT_ACK;
}
ret = 1;
break;
case UBX_MSG_ACK_NAK:
UBX_TRACE_RXMSG("Rx ACK-NAK\n");
if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) {
_ack_state = UBX_ACK_GOT_NAK;
}
ret = 1;
break;
default:
break;
}
return ret;
}
