void handle_system_time(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) { mavlink_system_time_t mtime; mavlink_msg_system_time_decode(msg, &mtime); // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 const bool fcu_time_valid = mtime.time_unix_usec > 1234567890ULL * 1000000; if (fcu_time_valid) { // continious publish for ntpd auto time_unix = boost::make_shared<sensor_msgs::TimeReference>(); ros::Time time_ref( mtime.time_unix_usec / 1000000, // t_sec (mtime.time_unix_usec % 1000000) * 1000); // t_nsec time_unix->header.stamp = ros::Time::now(); time_unix->time_ref = time_ref; time_unix->source = time_ref_source; time_ref_pub.publish(time_unix); } else { ROS_WARN_THROTTLE_NAMED(60, "time", "TM: Wrong FCU time."); } }
void message_rx_cb(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) { switch (msg->msgid) { case MAVLINK_MSG_ID_GPS_RAW_INT: { mavlink_gps_raw_int_t raw_gps; mavlink_msg_gps_raw_int_decode(msg, &raw_gps); sensor_msgs::NavSatFixPtr fix(new sensor_msgs::NavSatFix); geometry_msgs::TwistStampedPtr vel(new geometry_msgs::TwistStamped); gps_diag.set_gps_raw(raw_gps); if (raw_gps.fix_type < 2) { ROS_WARN_THROTTLE_NAMED(60, "gps", "GPS: no fix"); return; } fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS; if (raw_gps.fix_type == 2 || raw_gps.fix_type == 3) fix->status.status = sensor_msgs::NavSatStatus::STATUS_FIX; else fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX; fix->latitude = raw_gps.lat / 1E7; // deg fix->longitude = raw_gps.lon / 1E7; // deg fix->altitude = raw_gps.alt / 1E3; // m if (raw_gps.eph != UINT16_MAX) { double hdop = raw_gps.eph / 1E2; double hdop2 = std::pow(hdop, 2); // TODO: Check // From nmea_navsat_driver fix->position_covariance[0] = hdop2; fix->position_covariance[4] = hdop2; fix->position_covariance[8] = pow(2 * hdop, 2); fix->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED; } else { fix->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN; } fix->header.frame_id = frame_id; fix->header.stamp = ros::Time::now(); fix_pub.publish(fix); if (raw_gps.vel != UINT16_MAX && raw_gps.cog != UINT16_MAX) { double speed = raw_gps.vel / 1E2; // m/s double course = raw_gps.cog / 1E2; // deg // From nmea_navsat_driver vel->twist.linear.x = speed * std::sin(course); vel->twist.linear.y = speed * std::cos(course); vel->header.frame_id = frame_id; vel->header.stamp = fix->header.stamp; vel_pub.publish(vel); } } break; case MAVLINK_MSG_ID_GPS_STATUS: { // TODO: not supported by APM:Plane, // no standard ROS messages mavlink_gps_status_t gps_stat; mavlink_msg_gps_status_decode(msg, &gps_stat); ROS_DEBUG_NAMED("gps", "GPS stat sat visible: %d", gps_stat.satellites_visible); } break; case MAVLINK_MSG_ID_SYSTEM_TIME: { mavlink_system_time_t mtime; mavlink_msg_system_time_decode(msg, &mtime); if (mtime.time_unix_usec == 0) { ROS_WARN_THROTTLE_NAMED(60, "gps", "Wrong system time. Is GPS Ok? (boot_ms: %u)", mtime.time_boot_ms); return; } sensor_msgs::TimeReferencePtr time(new sensor_msgs::TimeReference); ros::Time time_ref( mtime.time_unix_usec / 1000000, // t_sec (mtime.time_unix_usec % 1000000) * 1000); // t_nsec time->source = time_ref_source; time->time_ref = time_ref; time->header.frame_id = time_ref_source; time->header.stamp = ros::Time::now(); time_ref_pub.publish(time); } break; }; }