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;
};
}
void MAVLink_Message_Handler::handle_message(uint64_t timestamp, mavlink_message_t &msg)
{
// ::fprintf(stderr, "msg.msgid=%u\n", msg.msgid);
switch(msg.msgid) {
case MAVLINK_MSG_ID_AHRS2: {
mavlink_ahrs2_t decoded;
mavlink_msg_ahrs2_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
mavlink_attitude_t decoded;
mavlink_msg_attitude_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_EKF_STATUS_REPORT: {
mavlink_ekf_status_report_t decoded;
mavlink_msg_ekf_status_report_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
mavlink_global_position_int_t decoded;
mavlink_msg_global_position_int_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT: {
mavlink_gps_raw_int_t decoded;
mavlink_msg_gps_raw_int_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_heartbeat_t decoded;
mavlink_msg_heartbeat_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_MOUNT_STATUS: {
mavlink_mount_status_t decoded;
mavlink_msg_mount_status_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: {
mavlink_nav_controller_output_t decoded;
mavlink_msg_nav_controller_output_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_PARAM_VALUE: {
mavlink_param_value_t decoded;
mavlink_msg_param_value_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK: {
mavlink_remote_log_data_block_t decoded;
mavlink_msg_remote_log_data_block_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE: {
mavlink_scaled_pressure_t decoded;
mavlink_msg_scaled_pressure_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE2: {
mavlink_scaled_pressure2_t decoded;
mavlink_msg_scaled_pressure2_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: {
mavlink_servo_output_raw_t decoded;
mavlink_msg_servo_output_raw_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_STATUSTEXT: {
mavlink_statustext_t decoded;
mavlink_msg_statustext_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SYS_STATUS: {
mavlink_sys_status_t decoded;
mavlink_msg_sys_status_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SYSTEM_TIME: {
mavlink_system_time_t decoded;
mavlink_msg_system_time_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_VFR_HUD: {
mavlink_vfr_hud_t decoded;
mavlink_msg_vfr_hud_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
}
}
