/** * 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; }