static void mavlink_handler (const lcm_recv_buf_t *rbuf, const char * channel, const mavlink_message_t* msg, void * user)
{
if (verbose)
ROS_INFO("Received message #%d on channel \"%s\" (sys:%d|comp:%d):\n", msg->msgid, channel, msg->sysid, msg->compid);
/**
* Serialize the Mavlink-ROS-message
*/
lcm_mavlink_ros::Mavlink rosmavlink_msg;
createROSFromMavlink(msg,&rosmavlink_msg);
/**
* Mark the ROS-Message as coming from LCM so that it will not be sent back to LCM
*/
rosmavlink_msg.fromlcm = true;
/**
* Send the received MAVLink message to ROS (topic: mavlink, see main())
*/
mavlink_pub.publish(rosmavlink_msg);
switch(msg->msgid)
{
case MAVLINK_MSG_ID_ATTITUDE:
{
sensor_msgs::Imu imu_msg;
convertMavlinkAttitudeToROS(msg, imu_msg);
attitude_pub.publish(imu_msg);
if (verbose)
ROS_INFO("Published Imu message (sys:%d|comp:%d):\n", msg->sysid, msg->compid);
}
break;
}
}
static void
mavlink_handler (const lcm_recv_buf_t *rbuf, const char * channel,
const mavlink_message_t* msg, void * user)
{
if (verbose)
ROS_INFO("Received message #%d on channel \"%s\" (sys:%d|comp:%d):\n", msg->msgid, channel, msg->sysid, msg->compid);
/**
* Serialize the Mavlink-ROS-message
*/
lcm_mavlink_ros::Mavlink rosmavlink_msg;
createROSFromMavlink(msg,&rosmavlink_msg);
/**
* Mark the ROS-Message as coming from LCM so that it will not be sent back to LCM
*/
rosmavlink_msg.fromlcm = true;
/**
* Send the received MAVLink message to ROS (topic: mavlink, see main())
*/
mavlink_pub.publish(rosmavlink_msg);
}
void
ArduPilot::readData(){
mavlink_msgs::Mavlink ros_mav_msg;
mavlink_message_t msg;
mavlink_status_t status;
uint8_t byte;
while(my_serial_.available()>0){
my_serial_.read(&byte,1);
if(mavlink_parse_char(MAVLINK_COMM_0,byte,&msg,&status)){
clock_gettime(CLOCK_MONOTONIC, ¤t_time_);
createROSFromMavlink(&msg,&ros_mav_msg);
ros_mav_msg.header.stamp.sec = current_time_.tv_sec;
ros_mav_msg.header.stamp.nsec = current_time_.tv_nsec;
mav_pub_.publish(ros_mav_msg);
switch(msg.msgid){
case MAVLINK_MSG_ID_HEARTBEAT:
{
}
break;
case MAVLINK_MSG_ID_RAW_IMU: // msg ID: 27
{
if(imu_pub_.getNumSubscribers()>0){
mavlink_raw_imu_t imu;
mavlink_msg_raw_imu_decode(&msg,&imu);
ros_imu_msg_.header.stamp.sec = current_time_.tv_sec;
ros_imu_msg_.header.stamp.nsec = current_time_.tv_nsec;
//ros_imu_msg_.header.stamp = ros::Time.now();
ros_imu_msg_.linear_acceleration.x = 9.81*(imu.xacc/1000);
ros_imu_msg_.linear_acceleration.y = 9.81*(imu.yacc/1000);
ros_imu_msg_.linear_acceleration.z = 9.81*(imu.zacc/1000);
ros_imu_msg_.angular_velocity.x = imu.xgyro/1000;
ros_imu_msg_.angular_velocity.y = imu.ygyro/1000;
ros_imu_msg_.angular_velocity.z = imu.zgyro/1000;
imu_pub_.publish(ros_imu_msg_);
}
}
break;
case MAVLINK_MSG_ID_GPS_RAW_INT: //msg ID: 24
{
if(gps_pub_.getNumSubscribers()>0){
mavlink_gps_raw_int_t gps;
mavlink_msg_gps_raw_int_decode(&msg,&gps);
ros_gps_msg_.header.stamp.sec = current_time_.tv_sec;
ros_gps_msg_.header.stamp.nsec = current_time_.tv_nsec;
ros_gps_msg_.latitude = gps.lat/1E7;
ros_gps_msg_.longitude = gps.lon/1E7;
ros_gps_msg_.altitude = gps.alt/1E3;
gps_pub_.publish(ros_gps_msg_);
gps_common::LLtoUTM(gps.lat,gps.lon, north_, east_, utm_zone_);
ros_pose_msg_.position.x = north_;
ros_pose_msg_.position.y = east_;
ros_pose_msg_.position.z = -gps.alt/1E7;
}
}
break;
case MAVLINK_MSG_ID_ATTITUDE: //msg ID: 30
{
if(pose_pub_.getNumSubscribers()>0){
mavlink_attitude_t att;
mavlink_msg_attitude_decode(&msg,&att);
ros_att_msg_.header.stamp.sec = current_time_.tv_sec;
ros_att_msg_.header.stamp.nsec = current_time_.tv_nsec;
ros_att_msg_.orientation.x = att.roll;
ros_att_msg_.orientation.y = att.pitch;
ros_att_msg_.orientation.z = att.yaw;
att_pub_.publish(ros_att_msg_);
ros_pose_msg_.orientation.x = att.roll;
ros_pose_msg_.orientation.y = att.pitch;
ros_pose_msg_.orientation.z = att.yaw;
pose_pub_.publish(ros_pose_msg_);
}
}
break;
case MAVLINK_MSG_ID_AHRS: //msg ID: 163
{
mavlink_ahrs_t ahrs;
}
break;
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: //msg ID: 35
{
if(rc_pub_.getNumSubscribers()>0){
mavlink_rc_channels_raw_t rc;
ros_rc_msg_.time_boot_ms = rc.time_boot_ms;
ros_rc_msg_.chan1_raw = rc.chan1_raw;
ros_rc_msg_.chan2_raw = rc.chan2_raw;
ros_rc_msg_.chan3_raw = rc.chan3_raw;
ros_rc_msg_.chan4_raw = rc.chan4_raw;
ros_rc_msg_.chan5_raw = rc.chan5_raw;
ros_rc_msg_.chan6_raw = rc.chan6_raw;
ros_rc_msg_.chan7_raw = rc.chan7_raw;
ros_rc_msg_.chan8_raw = rc.chan8_raw;
ros_rc_msg_.rssi = rc.rssi;
rc_pub_.publish(ros_rc_msg_);
}
}
break;
/* case MAVLINK_MSG_ID_G: //msg ID: 28
{
mavlink_raw_pressure_t baro;
}
break; */
case MAVLINK_MSG_ID_RAW_PRESSURE: //msg ID: 28
{
mavlink_raw_pressure_t baro;
}
break;
case MAVLINK_MSG_ID_COMMAND_LONG:
// EXECUTE ACTION
break;
default:
//Do nothing
break;
}
}
}
// my_serial.flush();
}
