void MavlinkBridge::readFromStream() {
int counter = 0;
while (running && connected && read(bridge_tty_fd, &bridge_c, 1) > 0 && counter++ < 1000) {
if (mavlink_parse_char(MAVLINK_COMM_0, bridge_c, &msg, &status)) {
if (msg.msgid == MAVLINK_MSG_ID_STATUSTEXT) {
mavlink_statustext_t s;
mavlink_msg_statustext_decode(&msg, &s);
if (strstr(s.text, "recieved") == 0){
printf("status %s\n", s.text);
}
}
if (msg.msgid == MAVLINK_MSG_ID_ATTITUDE) {
mavlink_attitude_t at;
mavlink_msg_attitude_decode(&msg, &at);
attitude = at;
}
if (msg.msgid == MAVLINK_MSG_ID_LOCAL_POSITION_NED) {
mavlink_local_position_ned_t localPosition_t;
mavlink_msg_local_position_ned_decode(&msg, &localPosition_t);
localPosition = localPosition_t;
}
if (msg.msgid == MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED) {
mavlink_position_target_local_ned_t localPositionTarget_t;
mavlink_msg_position_target_local_ned_decode(&msg, &localPositionTarget_t);
localPositionTarget = localPositionTarget_t;
}
}
}
}
void handle_local_position_ned(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_local_position_ned_t pos_ned;
mavlink_msg_local_position_ned_decode(msg, &pos_ned);
auto position = UAS::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.x, pos_ned.y, pos_ned.z));
auto orientation = uas->get_attitude_orientation();
auto pose = boost::make_shared<geometry_msgs::PoseStamped>();
pose->header = uas->synchronized_header(frame_id, pos_ned.time_boot_ms);
tf::pointEigenToMsg(position, pose->pose.position);
// XXX FIX ME #319
tf::quaternionTFToMsg(orientation, pose->pose.orientation);
local_position.publish(pose);
if (tf_send) {
geometry_msgs::TransformStamped transform;
transform.header.stamp = pose->header.stamp;
transform.header.frame_id = tf_frame_id;
transform.child_frame_id = tf_child_frame_id;
transform.transform.rotation = pose->pose.orientation;
tf::vectorEigenToMsg(position, transform.transform.translation);
tf2_broadcaster.sendTransform(transform);
}
}
void handle_local_position_ned(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_local_position_ned_t state;
mavlink_msg_local_position_ned_decode(msg, &state);
auto position_msg = boost::make_shared<geometry_msgs::Vector3Stamped>();
position_msg->header.stamp = ros::Time::now();
position_msg->vector.x = state.x;
position_msg->vector.y = state.y;
position_msg->vector.z = state.z;
position_pub.publish(position_msg);
auto velocity_msg = boost::make_shared<geometry_msgs::Vector3Stamped>();
velocity_msg->header.stamp = ros::Time::now();
velocity_msg->vector.x = state.vx;
velocity_msg->vector.y = state.vy;
velocity_msg->vector.z = state.vz;
velocity_pub.publish(velocity_msg);
}
void handle_local_position_ned(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_local_position_ned_t pos_ned;
mavlink_msg_local_position_ned_decode(msg, &pos_ned);
tf::Transform transform;
transform.setOrigin(tf::Vector3(pos_ned.y, pos_ned.x, -pos_ned.z));
transform.setRotation(uas->get_attitude_orientation());
geometry_msgs::PoseStampedPtr pose = boost::make_shared<geometry_msgs::PoseStamped>();
tf::poseTFToMsg(transform, pose->pose);
pose->header.frame_id = fixed_frame_id;
pose->header.stamp = ros::Time();
publish_vehicle_marker();
publish_vis_marker(pose);
}
void _MavlinkInterface::handleMessages()
{
Time_Stamps this_timestamps;
mavlink_message_t message;
int nMsgHandled;
nMsgHandled = 0;
//Handle Message while new message is received
while (readMessage(message))
{
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
system_id = current_messages.sysid;
autopilot_id = current_messages.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
// printf("MAVLINK_MSG_ID_HEARTBEAT\n");
mavlink_msg_heartbeat_decode(&message,
&(current_messages.heartbeat));
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
// printf("MAVLINK_MSG_ID_SYS_STATUS\n");
mavlink_msg_sys_status_decode(&message,
&(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status =
current_messages.time_stamps.sys_status;
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
// printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
mavlink_msg_battery_status_decode(&message,
&(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status =
current_messages.time_stamps.battery_status;
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
// printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
mavlink_msg_radio_status_decode(&message,
&(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status =
current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
// printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
mavlink_msg_local_position_ned_decode(&message,
&(current_messages.local_position_ned));
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned =
current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
// printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
mavlink_msg_global_position_int_decode(&message,
&(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int =
current_messages.time_stamps.global_position_int;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
// printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
mavlink_msg_position_target_local_ned_decode(&message,
&(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned =
get_time_usec();
this_timestamps.position_target_local_ned =
current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
// printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
mavlink_msg_position_target_global_int_decode(&message,
&(current_messages.position_target_global_int));
current_messages.time_stamps.position_target_global_int =
get_time_usec();
this_timestamps.position_target_global_int =
current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
// printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
mavlink_msg_highres_imu_decode(&message,
&(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu =
current_messages.time_stamps.highres_imu;
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
// printf("MAVLINK_MSG_ID_ATTITUDE\n");
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
break;
}
default:
{
// printf("Warning, did not handle message id %i\n", message.msgid);
break;
}
} // end: switch msgid
if(++nMsgHandled >= NUM_MSG_HANDLE)return;
}
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages(FILE *fd)
{
bool success; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
// fprintf(fd," xacc yacc zacc xgyro ygyro zgyro xmag ymag zmag altitude temperature\n");
// Blocking wait for new data
while ( !received_all and !time_to_exit )
{
// ----------------------------------------------------------------------
// READ MESSAGE
// ----------------------------------------------------------------------
mavlink_message_t message;
success = serial_port->read_message(message);
// printf("messageid = %d\n", message.msgid);
printf("flag = %d ch = %d \n",receive_flag,receive_ch);
// ----------------------------------------------------------------------
// HANDLE MESSAGE
// ----------------------------------------------------------------------
if( success )
{
// Store message sysid and compid.
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
//printf("MAVLINK_MSG_ID_HEARTBEAT\n");
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
//printf("MAVLINK_MSG_ID_SYS_STATUS\n");
mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status = current_messages.time_stamps.sys_status;
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
//printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status = current_messages.time_stamps.battery_status;
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
//printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status = current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
//printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
//fprintf(fd,"x = %f y = %f z = %f \n",current_messages.local_position_ned.x,current_messages.local_position_ned.y,current_messages.local_position_ned.z);
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
//printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned = get_time_usec();
this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
fprintf(fd, "lat_int = %d\n", current_messages.position_target_global_int.lat_int);
current_messages.time_stamps.position_target_global_int = get_time_usec();
this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
//printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
// fprintf(fd,"acc: %f %f %f ",current_messages.highres_imu.xacc,current_messages.highres_imu.yacc,current_messages.highres_imu.zacc);
// fprintf(fd,"gyro: %f %f %f ",current_messages.highres_imu.xgyro,current_messages.highres_imu.ygyro,current_messages.highres_imu.zgyro);
// fprintf(fd,"mag: %f %f %f ",current_messages.highres_imu.xmag,current_messages.highres_imu.ymag,current_messages.highres_imu.zmag);
// fprintf(fd,"alt: %f %f\n",current_messages.highres_imu.pressure_alt,current_messages.highres_imu.temperature);
if (receive_flag && receive_ch == 10) {
fprintf(fd,"acc: %f %f %f ",current_messages.highres_imu.xacc,current_messages.highres_imu.yacc,current_messages.highres_imu.zacc);
fprintf(fd,"gyro: %f %f %f ",current_messages.highres_imu.xgyro,current_messages.highres_imu.ygyro,current_messages.highres_imu.zgyro);
fprintf(fd,"mag: %f %f %f ",current_messages.highres_imu.xmag,current_messages.highres_imu.ymag,current_messages.highres_imu.zmag);
fprintf(fd,"alt: %f %f\n",current_messages.highres_imu.pressure_alt,current_messages.highres_imu.temperature);
receive_flag = false;
}
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
mavlink_msg_gps_raw_int_decode(&message, &(current_messages.gps_raw_int));
fprintf(fd, "lat = %d lon = %d alt = %d ", current_messages.gps_raw_int.lat,current_messages.gps_raw_int.lon,current_messages.gps_raw_int.alt);
fprintf(fd, "vel = %d cog = %d satellites_visible = %d\n", current_messages.gps_raw_int.vel,current_messages.gps_raw_int.cog,current_messages.gps_raw_int.satellites_visible);
if (receive_flag && receive_ch == 10) {
fprintf(fd, "lat = %d lon = %d alt = %d ", current_messages.gps_raw_int.lat,current_messages.gps_raw_int.lon,current_messages.gps_raw_int.alt);
fprintf(fd, "vel = %d cog = %d satellites_visible = %d\n", current_messages.gps_raw_int.vel,current_messages.gps_raw_int.cog,current_messages.gps_raw_int.satellites_visible);
receive_flag = false;
}
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
//printf("MAVLINK_MSG_ID_ATTITUDE\n");
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
break;
}
default:
{
// printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
} // end: if read message
// Check for receipt of all items
received_all =
this_timestamps.heartbeat &&
// this_timestamps.battery_status &&
// this_timestamps.radio_status &&
// this_timestamps.local_position_ned &&
// this_timestamps.global_position_int &&
// this_timestamps.position_target_local_ned &&
// this_timestamps.position_target_global_int &&
// this_timestamps.highres_imu &&
// this_timestamps.attitude &&
this_timestamps.sys_status
;
// give the write thread time to use the port
if ( writing_status > false ) {
usleep(100); // look for components of batches at 10kHz
}
} // end: while not received all
return;
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages()
{
bool success; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
// Blocking wait for new data
while ( not received_all and not time_to_exit )
{
// ----------------------------------------------------------------------
// READ MESSAGE
// ----------------------------------------------------------------------
mavlink_message_t message;
success = serial_port->read_message(message);
// ----------------------------------------------------------------------
// HANDLE MESSAGE
// ----------------------------------------------------------------------
if( success )
{
// Store message sysid and compid.
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
//printf("MAVLINK_MSG_ID_HEARTBEAT\n");
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
//printf("MAVLINK_MSG_ID_SYS_STATUS\n");
mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status = current_messages.time_stamps.sys_status;
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
//printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status = current_messages.time_stamps.battery_status;
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
//printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status = current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
//printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
//printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned = get_time_usec();
this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
current_messages.time_stamps.position_target_global_int = get_time_usec();
this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
//printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
//printf("MAVLINK_MSG_ID_ATTITUDE\n");
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
break;
}
default:
{
// printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
} // end: if read message
// Check for receipt of all items
received_all =
this_timestamps.heartbeat &&
this_timestamps.sys_status &&
// this_timestamps.battery_status &&
// this_timestamps.radio_status &&
this_timestamps.local_position_ned &&
// this_timestamps.global_position_int &&
// this_timestamps.position_target_local_ned &&
this_timestamps.position_target_global_int &&
this_timestamps.highres_imu &&
this_timestamps.attitude ;
// give the write thread time to use the port
if ( writing_status > false )
usleep(100); // look for components of batches at 10kHz
} // end: while not received all
return;
}
void mavlink_wpm_message_handler(const mavlink_message_t* msg)
{
uint64_t now = mavlink_missionlib_get_system_timestamp();
switch(msg->msgid)
{
case MAVLINK_MSG_ID_ATTITUDE:
{
if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size)
{
mavlink_mission_item_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]);
if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED)
{
mavlink_attitude_t att;
mavlink_msg_attitude_decode(msg, &att);
float yaw_tolerance = wpm.accept_range_yaw;
//compare current yaw
if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI)
{
if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4)
wpm.yaw_reached = true;
}
else if(att.yaw - yaw_tolerance < 0.0f)
{
float lowerBound = 360.0f + att.yaw - yaw_tolerance;
if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance)
wpm.yaw_reached = true;
}
else
{
float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI;
if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound)
wpm.yaw_reached = true;
}
}
}
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size)
{
mavlink_mission_item_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]);
if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED)
{
mavlink_local_position_ned_t pos;
mavlink_msg_local_position_ned_decode(msg, &pos);
//// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
wpm.pos_reached = false;
// compare current position (given in message) with current waypoint
float orbit = wp->param1;
float dist;
// if (wp->param2 == 0)
// {
// // FIXME segment distance
// //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z);
// }
// else
// {
dist = mavlink_wpm_distance_to_point(wpm.current_active_wp_id, pos.x, pos.y, pos.z);
// }
if (dist >= 0.f && dist <= orbit && wpm.yaw_reached)
{
wpm.pos_reached = true;
}
}
}
break;
}
// case MAVLINK_MSG_ID_CMD: // special action from ground station
// {
// mavlink_cmd_t action;
// mavlink_msg_cmd_decode(msg, &action);
// if(action.target == mavlink_system.sysid)
// {
// // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl;
// switch (action.action)
// {
// // case MAV_ACTION_LAUNCH:
// // // if (verbose) std::cerr << "Launch received" << std::endl;
// // current_active_wp_id = 0;
// // if (wpm.size>0)
// // {
// // setActive(waypoints[current_active_wp_id]);
// // }
// // else
// // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
// // break;
//
// // case MAV_ACTION_CONTINUE:
// // // if (verbose) std::c
// // err << "Continue received" << std::endl;
// // idle = false;
// // setActive(waypoints[current_active_wp_id]);
// // break;
//
// // case MAV_ACTION_HALT:
// // // if (verbose) std::cerr << "Halt received" << std::endl;
// // idle = true;
// // break;
//
// // default:
// // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
// // break;
// }
// }
// break;
// }
case MAVLINK_MSG_ID_MISSION_ACK:
{
mavlink_mission_ack_t wpa;
mavlink_msg_mission_ack_decode(msg, &wpa);
if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/))
{
wpm.timestamp_lastaction = now;
if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)
{
if (wpm.current_wp_id == wpm.size-1)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
#else
if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n");
#endif
wpm.current_state = MAVLINK_WPM_STATE_IDLE;
wpm.current_wp_id = 0;
}
}
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
#else
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#endif
}
break;
}
case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
{
mavlink_mission_set_current_t wpc;
mavlink_msg_mission_set_current_decode(msg, &wpc);
if(wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/)
{
wpm.timestamp_lastaction = now;
if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
{
if (wpc.seq < wpm.size)
{
// if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n");
wpm.current_active_wp_id = wpc.seq;
uint32_t i;
for(i = 0; i < wpm.size; i++)
{
if (i == wpm.current_active_wp_id)
{
wpm.waypoints[i].current = true;
}
else
{
wpm.waypoints[i].current = false;
}
}
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("NEW WP SET");
#else
if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm.current_active_wp_id);
#endif
wpm.yaw_reached = false;
wpm.pos_reached = false;
mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
wpm.timestamp_firstinside_orbit = 0;
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n");
#endif
}
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
#else
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n");
#endif
}
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
#else
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#endif
}
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
{
mavlink_mission_request_list_t wprl;
mavlink_msg_mission_request_list_decode(msg, &wprl);
if(wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/)
{
wpm.timestamp_lastaction = now;
if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
{
if (wpm.size > 0)
{
//if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
wpm.current_state = MAVLINK_WPM_STATE_SENDLIST;
wpm.current_wp_id = 0;
wpm.current_partner_sysid = msg->sysid;
wpm.current_partner_compid = msg->compid;
}
else
{
// if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
}
wpm.current_count = wpm.size;
mavlink_wpm_send_waypoint_count(msg->sysid,msg->compid, wpm.current_count);
}
else
{
// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state);
}
}
else
{
// if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n");
}
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST:
{
mavlink_mission_request_t wpr;
mavlink_msg_mission_request_decode(msg, &wpr);
if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/)
{
wpm.timestamp_lastaction = now;
//ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
if ((wpm.current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm.current_wp_id || wpr.seq == wpm.current_wp_id + 1) && wpr.seq < wpm.size))
{
if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("GOT WP REQ, state -> SEND");
#else
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
#endif
}
if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ");
#else
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
#endif
}
if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ");
#else
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
#endif
}
wpm.current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
wpm.current_wp_id = wpr.seq;
mavlink_wpm_send_waypoint(wpm.current_partner_sysid, wpm.current_partner_compid, wpr.seq);
}
else
{
// if (verbose)
{
if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS))
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state);
#endif
break;
}
else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
{
if (wpr.seq != 0)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
#endif
}
}
else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)
{
if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1);
#endif
}
else if (wpr.seq >= wpm.size)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
#endif
}
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n");
#endif
}
}
}
}
else
{
//we we're target but already communicating with someone else
if((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid))
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid);
#endif
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
#else
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#endif
}
}
break;
}
case MAVLINK_MSG_ID_MISSION_COUNT:
{
mavlink_mission_count_t wpc;
mavlink_msg_mission_count_decode(msg, &wpc);
if(wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/)
{
wpm.timestamp_lastaction = now;
if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id == 0))
{
if (wpc.count > 0)
{
if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("WP CMD OK: state -> GETLIST");
#else
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid);
#endif
}
if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
#else
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
#endif
}
wpm.current_state = MAVLINK_WPM_STATE_GETLIST;
wpm.current_wp_id = 0;
wpm.current_partner_sysid = msg->sysid;
wpm.current_partner_compid = msg->compid;
wpm.current_count = wpc.count;
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY");
#else
if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n");
#endif
wpm.rcv_size = 0;
//while(waypoints_receive_buffer->size() > 0)
// {
// delete waypoints_receive_buffer->back();
// waypoints_receive_buffer->pop_back();
// }
mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id);
}
else if (wpc.count == 0)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("COUNT 0");
#else
if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n");
#endif
wpm.rcv_size = 0;
//while(waypoints_receive_buffer->size() > 0)
// {
// delete waypoints->back();
// waypoints->pop_back();
// }
wpm.current_active_wp_id = -1;
wpm.yaw_reached = false;
wpm.pos_reached = false;
break;
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("IGN WP CMD");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_MISSION_ITEM_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
#endif
}
}
else
{
if (!(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST))
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state);
#endif
}
else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id);
#endif
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT - FIXME: missed error description\n");
#endif
}
}
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
#else
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#endif
}
}
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
{
mavlink_mission_item_t wp;
mavlink_msg_mission_item_decode(msg, &wp);
mavlink_missionlib_send_gcs_string("GOT WP");
if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/))
{
wpm.timestamp_lastaction = now;
//ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
if ((wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count))
{
// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid);
// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid);
// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid);
//
wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
mavlink_mission_item_t* newwp = &(wpm.rcv_waypoints[wp.seq]);
memcpy(newwp, &wp, sizeof(mavlink_mission_item_t));
wpm.current_wp_id = wp.seq + 1;
// if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)
{
mavlink_missionlib_send_gcs_string("GOT ALL WPS");
// if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count);
mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0);
if (wpm.current_active_wp_id > wpm.rcv_size-1)
{
wpm.current_active_wp_id = wpm.rcv_size-1;
}
// switch the waypoints list
// FIXME CHECK!!!
for (int i = 0; i < wpm.current_count; ++i)
{
wpm.waypoints[i] = wpm.rcv_waypoints[i];
}
wpm.size = wpm.current_count;
//get the new current waypoint
uint32_t i;
for(i = 0; i < wpm.size; i++)
{
if (wpm.waypoints[i].current == 1)
{
wpm.current_active_wp_id = i;
//// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id);
wpm.yaw_reached = false;
wpm.pos_reached = false;
mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
wpm.timestamp_firstinside_orbit = 0;
break;
}
}
if (i == wpm.size)
{
wpm.current_active_wp_id = -1;
wpm.yaw_reached = false;
wpm.pos_reached = false;
wpm.timestamp_firstinside_orbit = 0;
}
wpm.current_state = MAVLINK_WPM_STATE_IDLE;
}
else
{
mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id);
}
}
else
{
if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
{
//we're done receiving waypoints, answer with ack.
mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0);
// printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n");
}
// if (verbose)
{
if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS))
{
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state);
break;
}
else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST)
{
if(!(wp.seq == 0))
{
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq);
}
else
{
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
}
}
else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)
{
if (!(wp.seq == wpm.current_wp_id))
{
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id);
}
else if (!(wp.seq < wpm.current_count))
{
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq);
}
else
{
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
}
}
else
{
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
}
}
}
}
else
{
//we we're target but already communicating with someone else
if((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
{
// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid);
}
else if(wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/)
{
// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
}
}
break;
}
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
{
mavlink_mission_clear_all_t wpca;
mavlink_msg_mission_clear_all_decode(msg, &wpca);
if(wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm.current_state == MAVLINK_WPM_STATE_IDLE)
{
wpm.timestamp_lastaction = now;
// if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
// Delete all waypoints
wpm.size = 0;
wpm.current_active_wp_id = -1;
wpm.yaw_reached = false;
wpm.pos_reached = false;
}
else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
{
// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state);
}
break;
}
default:
{
// if (debug) // printf("Waypoint: received message of unknown type");
break;
}
}
//check if the current waypoint was reached
if (wpm.pos_reached /*wpm.yaw_reached &&*/ && !wpm.idle)
{
if (wpm.current_active_wp_id < wpm.size)
{
mavlink_mission_item_t *cur_wp = &(wpm.waypoints[wpm.current_active_wp_id]);
if (wpm.timestamp_firstinside_orbit == 0)
{
// Announce that last waypoint was reached
// if (verbose) // printf("*** Reached waypoint %u ***\n", cur_wp->seq);
mavlink_wpm_send_waypoint_reached(cur_wp->seq);
wpm.timestamp_firstinside_orbit = now;
}
// check if the MAV was long enough inside the waypoint orbit
//if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
if(now-wpm.timestamp_firstinside_orbit >= cur_wp->param2*1000)
{
if (cur_wp->autocontinue)
{
cur_wp->current = 0;
if (wpm.current_active_wp_id == wpm.size - 1 && wpm.size > 1)
{
//the last waypoint was reached, if auto continue is
//activated restart the waypoint list from the beginning
wpm.current_active_wp_id = 1;
}
else
{
if ((uint16_t)(wpm.current_active_wp_id + 1) < wpm.size)
wpm.current_active_wp_id++;
}
// Fly to next waypoint
wpm.timestamp_firstinside_orbit = 0;
mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
wpm.waypoints[wpm.current_active_wp_id].current = true;
wpm.pos_reached = false;
wpm.yaw_reached = false;
// if (verbose) // printf("Set new waypoint (%u)\n", wpm.current_active_wp_id);
}
}
}
}
else
{
wpm.timestamp_lastoutside_orbit = now;
}
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages()
{
int success = 0; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
int err_counter = 0;
struct timespec diff_r;
struct timespec aft_read;
struct timespec bef_read;
struct timespec receive_time;
struct timespec longRelNanoSleep;
longRelNanoSleep.tv_sec = 0;
longRelNanoSleep.tv_nsec = 100000;
struct timespec RelNanoSleep;
RelNanoSleep.tv_sec = 0;
RelNanoSleep.tv_nsec = 10;
// Blocking wait for new data
// pthread_mutex_lock(&mut);
// if (blocked == 0 && writing_status)
// {
// blocked = 1;
// pthread_cond_wait(&cond,&mut);
// blocked = 0;
// }
// else
// {
// pthread_cond_signal(&cond);
// }
// clock_gettime(CLOCK_REALTIME,&bef_read);
// clock_gettime(CLOCK_REALTIME,&aft_read);
// timespec_sub(&diff_r,&aft_read,&bef_read);
// int64_t diff_r_us = diff_r.tv_sec*1e6 + diff_r.tv_nsec/1e3;
// printf("Read Time: %ld \n",diff_r_us);
//
// pthread_mutex_unlock(&mut);
mavlink_message_t message;
while ( (success != 1) && (!time_to_exit) )
{
// ----------------------------------------------------------------------
// READ ONE BYTE AT TIME UNTIL COMPLETE MESSAGE IS RECEIVED
// ----------------------------------------------------------------------
success = serial_port->read_message(message); //Have I obtained a complete message?
if ( success == -1) // Errors...retry...
{
err_counter++;
if (err_counter == 3) // To many void read
{
fprintf(stderr,"ERRORS!\n");
break;
}
}
}
err_counter = 0;
if (success == -1) return;
// ----------------------------------------------------------------------
// HANDLE MESSAGE
// ----------------------------------------------------------------------
//if( success )
// Store message sysid and compid.
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
//printf("MAVLINK_MSG_ID_HEARTBEAT\n");
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
base_mode = current_messages.heartbeat.base_mode;
mav_type = current_messages.heartbeat.type;
system_status = current_messages.heartbeat.system_status;
custom_mode = current_messages.heartbeat.custom_mode;
//printf("base_mode = %u\n",base_mode);
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
heartbeat_count++;
//printf("IMU Sensors Timestamp %u\n",current_messages.time_stamps.highres_imu/1000);
if ( (current_messages.time_stamps.heartbeat - read_heartbeat_old) > 10000000 )
{
printf("HEARTBEAT frequency : %d \n",heartbeat_count/10);
heartbeat_count = 0;
read_heartbeat_old = current_messages.time_stamps.heartbeat;
}
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
//printf("MAVLINK_MSG_ID_SYS_STATUS\n");
mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status = current_messages.time_stamps.sys_status;
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
//printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status = current_messages.time_stamps.battery_status;
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
//printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status = current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
//printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
//printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned = get_time_usec();
this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
current_messages.time_stamps.position_target_global_int = get_time_usec();
this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
//printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
clock_gettime(CLOCK_REALTIME,&receive_time);
mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
highres_imu_count++;
//printf("IMU Sensors Timestamp %u\n",current_messages.time_stamps.highres_imu/1000);
if ( (current_messages.time_stamps.highres_imu - read_highres_imu_old) > 10000000 )
{
printf("HIGHRES_IMU frequency : %d Hz\n",highres_imu_count/10);
highres_imu_count = 0;
read_highres_imu_old = current_messages.time_stamps.highres_imu;
}
//fprintf(logfile_rd,"HIGHRES_IMU %d, %ld \n",receive_time.tv_sec, receive_time.tv_nsec);
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
//printf("MAVLINK_MSG_ID_ATTITUDE\n");
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
break;
}
case MAVLINK_MSG_ID_HIL_CONTROLS:
{
//printf("MAVLINK_MSG_ID_HIL_CONTROLS\n");
mavlink_msg_hil_controls_decode(&message, &(current_messages.hil_controls));
current_messages.time_stamps.hil_controls = get_time_usec();
this_timestamps.hil_controls = current_messages.time_stamps.hil_controls;
float data[4] = {current_messages.hil_controls.roll_ailerons,
current_messages.hil_controls.pitch_elevator,
current_messages.hil_controls.yaw_rudder,
current_messages.hil_controls.throttle};
int bsent = udp_mtl->send_bytes((char *)data,sizeof(float[4]));
hil_controls_count++;
//printf("IMU Sensors Timestamp %u\n",current_messages.time_stamps.highres_imu/1000);
if ( (current_messages.time_stamps.hil_controls - read_hil_controls_old) > 10000000 )
{
printf("HIL_CONTROLS frequency : %d Hz\n",hil_controls_count/10);
hil_controls_count = 0;
read_hil_controls_old = current_messages.time_stamps.hil_controls;
printf("%0.2f | ",current_messages.hil_controls.roll_ailerons);
printf("%0.2f | ",current_messages.hil_controls.pitch_elevator);
printf("%0.2f | ",current_messages.hil_controls.yaw_rudder);
printf("%0.2f | \n",current_messages.hil_controls.throttle);
}
//fprintf(logfile_rd,"HIL_CONTROLS %d, %ld \n",receive_time.tv_sec, receive_time.tv_nsec);
break;
}
default:
{
// printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
return;
}
void _Mavlink::handleMessages()
{
mavlink_message_t message;
int nMsgHandled = 0;
//Handle Message while new message is received
while (readMessage(message))
{
// Note this doesn't handle multiple message sources.
m_msg.sysid = message.sysid;
m_msg.compid = message.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
LOG(INFO)<<"-> MAVLINK_MSG_ID_HEARTBEAT";
mavlink_msg_heartbeat_decode(&message, &(m_msg.heartbeat));
m_msg.time_stamps.heartbeat = get_time_usec();
if (m_msg.heartbeat.type != MAV_TYPE_GCS)
{
m_systemID = m_msg.sysid;
m_targetComponentID = m_msg.compid;
LOG_I("-> SYSTEM_ID:"<<m_systemID
<<" COMPONENT_ID:"<<m_componentID
<<" TARGET_COMPONENT_ID:"<<m_targetComponentID);
}
else
{
LOG_I("->HEARTBEAT FROM MAV_TYPE_GCS");
}
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
LOG_I("-> MAVLINK_MSG_ID_SYS_STATUS");
mavlink_msg_sys_status_decode(&message,
&(m_msg.sys_status));
m_msg.time_stamps.sys_status = get_time_usec();
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
LOG_I("-> MAVLINK_MSG_ID_BATTERY_STATUS");
mavlink_msg_battery_status_decode(&message,
&(m_msg.battery_status));
m_msg.time_stamps.battery_status = get_time_usec();
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
LOG_I("-> MAVLINK_MSG_ID_RADIO_STATUS");
mavlink_msg_radio_status_decode(&message,
&(m_msg.radio_status));
m_msg.time_stamps.radio_status = get_time_usec();
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
LOG_I("-> MAVLINK_MSG_ID_LOCAL_POSITION_NED");
mavlink_msg_local_position_ned_decode(&message,
&(m_msg.local_position_ned));
m_msg.time_stamps.local_position_ned = get_time_usec();
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
LOG_I("-> MAVLINK_MSG_ID_GLOBAL_POSITION_INT");
mavlink_msg_global_position_int_decode(&message,
&(m_msg.global_position_int));
m_msg.time_stamps.global_position_int = get_time_usec();
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
LOG_I("-> MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED");
mavlink_msg_position_target_local_ned_decode(&message,
&(m_msg.position_target_local_ned));
m_msg.time_stamps.position_target_local_ned =
get_time_usec();
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
LOG_I("-> MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT");
mavlink_msg_position_target_global_int_decode(&message,
&(m_msg.position_target_global_int));
m_msg.time_stamps.position_target_global_int =
get_time_usec();
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
LOG_I("-> MAVLINK_MSG_ID_HIGHRES_IMU");
mavlink_msg_highres_imu_decode(&message,
&(m_msg.highres_imu));
m_msg.time_stamps.highres_imu = get_time_usec();
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
LOG_I("-> MAVLINK_MSG_ID_ATTITUDE");
mavlink_msg_attitude_decode(&message, &(m_msg.attitude));
m_msg.time_stamps.attitude = get_time_usec();
break;
}
case MAVLINK_MSG_ID_COMMAND_ACK:
{
mavlink_msg_command_ack_decode(&message,
&(m_msg.command_ack));
m_msg.time_stamps.attitude = get_time_usec();
LOG_I("-> MAVLINK_MSG_ID_COMMAND_ACK:"<<m_msg.command_ack.result);
break;
}
default:
{
LOG_I("-> UNKNOWN MSG_ID:"<<message.msgid);
break;
}
}
if (++nMsgHandled >= NUM_MSG_HANDLE)
return;
}
}
//32
void MavSerialPort::local_position_ned_handler(){
// qDebug() << "MAVLINK_MSG_ID_LOCAL_POSITION_NED\n";
mavlink_msg_local_position_ned_decode(&message, &local_position_ned);
// emit localChanged();
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages()
{
bool success; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
// Blocking wait for new data
while ( not received_all and not time_to_exit )
{
// ----------------------------------------------------------------------
// READ MESSAGE
// ----------------------------------------------------------------------
mavlink_message_t message;
success = serial_port->read_message(message);
// ----------------------------------------------------------------------
// HANDLE MESSAGE
// ----------------------------------------------------------------------
if( success )
{
// Store message sysid and compid.
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
std::cout << "MAVLINK_MSG_ID_HEARTBEAT" << std::endl;
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
//std::cout << "MAVLINK_MSG_ID_SYS_STATUS" << std::endl;
mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status = current_messages.time_stamps.sys_status;
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
std::cout << "MAVLINK_MSG_ID_BATTERY_STATUS" << std::endl;
mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status = current_messages.time_stamps.battery_status;
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
std::cout << "MAVLINK_MSG_ID_RADIO_STATUS" << std::endl;
mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status = current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
std::cout << "MAVLINK_MSG_ID_LOCAL_POSITION_NED" << std::endl;
mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
std::cout << "MAVLINK_MSG_ID_GLOBAL_POSITION_INT" << std::endl;
mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
std::cout << "MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED" << std::endl;
mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned = get_time_usec();
this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
std::cout << "MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT" << std::endl;
mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
current_messages.time_stamps.position_target_global_int = get_time_usec();
this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
std::cout << "MAVLINK_MSG_ID_HIGHRES_IMU" << std::endl;
mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
vector4d quaternion;
std::cout << "MAVLINK_MSG_ID_ATTITUDE" << std::endl;
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
quaternion_from_euler(&quaternion, current_messages.attitude.roll,
current_messages.attitude.pitch, current_messages.attitude.yaw);
std::cout <<
"\troll: " << current_messages.attitude.roll <<
"\tpitch: " << current_messages.attitude.pitch <<
"\tyaw: " << current_messages.attitude.yaw <<
std::endl;
std::cout <<
"\trollspeed: " << current_messages.attitude.rollspeed <<
"\tpitchspeed: " << current_messages.attitude.pitchspeed <<
"\tyawspeed: " << current_messages.attitude.yawspeed <<
std::endl;
std::cout <<
"\tqx: " << quaternion.x <<
"\tqy: " << quaternion.y <<
"\tqz: " << quaternion.z <<
"\tqw: " << quaternion.w <<
std::endl;
break;
}
case MAVLINK_MSG_ID_DEBUG:
{
std::cout << "MAVLINK_MSG_ID_DEBUG" << std::endl;
/* mavlink_msg_debug_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;*/
break;
}
case MAVLINK_MSG_ID_STATUSTEXT:
{
std::cout << "MAVLINK_MSG_ID_STATUSTEXT: ";
mavlink_msg_statustext_decode(&message, &(current_messages.statustext));
current_messages.statustext.text[50] = 0;
printf("%d - '%s'\n", current_messages.statustext.severity, current_messages.statustext.text);
current_messages.time_stamps.statustext = get_time_usec();
this_timestamps.statustext = current_messages.time_stamps.statustext;
break;
}
case MAVLINK_MSG_ID_RAW_IMU:
{
std::cout << "MAVLINK_MSG_ID_RAW_IMU:" << std::endl;
mavlink_msg_raw_imu_decode(&message, &(current_messages.raw_imu));
std::cout << "\tacc :\t" << current_messages.raw_imu.xacc <<
"\t" << current_messages.raw_imu.yacc <<
"\t" << current_messages.raw_imu.zacc <<
std::endl;
std::cout << "\tgyro:\t" << current_messages.raw_imu.xgyro <<
"\t" << current_messages.raw_imu.ygyro <<
"\t" << current_messages.raw_imu.zgyro <<
std::endl;
std::cout << "\tmag:\t" << current_messages.raw_imu.xmag <<
"\t" << current_messages.raw_imu.ymag <<
"\t" << current_messages.raw_imu.zmag <<
std::endl;
current_messages.time_stamps.raw_imu = get_time_usec();
this_timestamps.raw_imu = current_messages.time_stamps.raw_imu;
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
std::cout << "MAVLINK_MSG_ID_GPS_RAW_INT:" << std::endl;
mavlink_msg_gps_raw_int_decode(&message, &(current_messages.gps_raw_int));
current_messages.time_stamps.gps_raw_int = get_time_usec();
this_timestamps.gps_raw_int = current_messages.time_stamps.gps_raw_int;
std::cout <<
"\tlat: " << current_messages.gps_raw_int.lat <<
"\tlon: " << current_messages.gps_raw_int.lon <<
"\talt: " << current_messages.gps_raw_int.alt <<
std::endl;
std::cout <<
"\teph: " << current_messages.gps_raw_int.eph <<
"\tepv: " << current_messages.gps_raw_int.epv <<
std::endl;
std::cout <<
"\tvel: " << current_messages.gps_raw_int.vel <<
"\tcog: " << current_messages.gps_raw_int.cog <<
std::endl;
std::cout <<
"\tfix: " << (int)current_messages.gps_raw_int.fix_type <<
"\tsat: " << (int)current_messages.gps_raw_int.satellites_visible <<
std::endl;
break;
}
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
{
vector4d quaternion;
std::cout << "MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:" << std::endl;
mavlink_msg_nav_controller_output_decode(&message, &(current_messages.nav_controller_output));
current_messages.time_stamps.nav_controller_output = get_time_usec();
this_timestamps.nav_controller_output = current_messages.time_stamps.nav_controller_output;
quaternion_from_euler(&quaternion, current_messages.nav_controller_output.nav_roll,
current_messages.nav_controller_output.nav_pitch, current_messages.nav_controller_output.nav_bearing);
std::cout <<
"\tnav_roll: " << current_messages.nav_controller_output.nav_roll <<
"\tnav_pitch: " << current_messages.nav_controller_output.nav_pitch <<
"\tnav_bearing: " << current_messages.nav_controller_output.nav_bearing <<
std::endl;
std::cout <<
"\tqx: " << quaternion.x <<
"\tqy: " << quaternion.y <<
"\tqz: " << quaternion.z <<
"\tqw: " << quaternion.w <<
std::endl;
std::cout <<
"\ttarget_bearing: " << current_messages.nav_controller_output.target_bearing <<
"\twp_dist: " << current_messages.nav_controller_output.wp_dist <<
std::endl;
std::cout <<
"\talt_error: " << current_messages.nav_controller_output.alt_error <<
"\taspd_error: " << current_messages.nav_controller_output.aspd_error <<
"\txtrack_error: " << current_messages.nav_controller_output.xtrack_error <<
std::endl;
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE:
{
std::cout << "MAVLINK_MSG_ID_SCALED_PRESSURE:" << std::endl;
mavlink_msg_scaled_pressure_decode(&message, &(current_messages.scaled_pressure));
current_messages.time_stamps.scaled_pressure = get_time_usec();
this_timestamps.scaled_pressure = current_messages.time_stamps.scaled_pressure;
std::cout <<
"\tpress_abs: " << current_messages.scaled_pressure.press_abs <<
"\tpress_diff: " << current_messages.scaled_pressure.press_diff <<
"\ttemperature: " << ((double)current_messages.scaled_pressure.temperature / 100.0) <<
std::endl;
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
{
std::cout << "MAVLINK_MSG_ID_RC_CHANNELS_RAW:" << std::endl;
mavlink_msg_rc_channels_raw_decode(&message, &(current_messages.rc_channels_raw));
current_messages.time_stamps.rc_channels_raw = get_time_usec();
this_timestamps.rc_channels_raw = current_messages.time_stamps.rc_channels_raw;
std::cout <<
"\tport: " << (int)current_messages.rc_channels_raw.port <<
"\tchan1_raw: " << current_messages.rc_channels_raw.chan1_raw <<
"\tchan2_raw: " << current_messages.rc_channels_raw.chan2_raw <<
"\tchan3_raw: " << current_messages.rc_channels_raw.chan3_raw <<
"\tchan4_raw: " << current_messages.rc_channels_raw.chan4_raw <<
"\tchan5_raw: " << current_messages.rc_channels_raw.chan5_raw <<
"\tchan6_raw: " << current_messages.rc_channels_raw.chan6_raw <<
"\tchan7_raw: " << current_messages.rc_channels_raw.chan7_raw <<
"\tchan8_raw: " << current_messages.rc_channels_raw.chan8_raw <<
"\trssi: " << (int)current_messages.rc_channels_raw.rssi <<
std::endl;
break;
}
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
{
std::cout << "MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:" << std::endl;
mavlink_msg_servo_output_raw_decode(&message, &(current_messages.servo_output_raw));
current_messages.time_stamps.servo_output_raw = get_time_usec();
this_timestamps.servo_output_raw = current_messages.time_stamps.servo_output_raw;
std::cout <<
"\tport: " << (int)current_messages.servo_output_raw.port <<
"\tservo1_raw: " << current_messages.servo_output_raw.servo1_raw <<
"\tservo2_raw: " << current_messages.servo_output_raw.servo2_raw <<
"\tservo3_raw: " << current_messages.servo_output_raw.servo3_raw <<
"\tservo4_raw: " << current_messages.servo_output_raw.servo4_raw <<
"\tservo5_raw: " << current_messages.servo_output_raw.servo5_raw <<
"\tservo6_raw: " << current_messages.servo_output_raw.servo6_raw <<
"\tservo7_raw: " << current_messages.servo_output_raw.servo7_raw <<
"\tservo8_raw: " << current_messages.servo_output_raw.servo8_raw <<
std::endl;
break;
}
case MAVLINK_MSG_ID_VFR_HUD:
{
std::cout << "MAVLINK_MSG_ID_VFR_HUD:" << std::endl;
mavlink_msg_vfr_hud_decode(&message, &(current_messages.vfr_hud));
current_messages.time_stamps.vfr_hud = get_time_usec();
this_timestamps.vfr_hud = current_messages.time_stamps.vfr_hud;
std::cout <<
"\tairspeed: " << current_messages.vfr_hud.airspeed <<
"\tgroundspeed: " << current_messages.vfr_hud.groundspeed <<
"\theading: " << current_messages.vfr_hud.heading <<
std::endl;
std::cout <<
"\tthrottle: " << current_messages.vfr_hud.throttle <<
"\talt: " << current_messages.vfr_hud.alt <<
"\tclimb: " << current_messages.vfr_hud.climb <<
std::endl;
break;
}
case MAVLINK_MSG_ID_MISSION_CURRENT:
{
std::cout << "MAVLINK_MSG_ID_MISSION_CURRENT: ";
mavlink_msg_mission_current_decode(&message, &(current_messages.mission_current));
current_messages.time_stamps.mission_current = get_time_usec();
this_timestamps.mission_current = current_messages.time_stamps.mission_current;
std::cout <<
"seq: " << current_messages.mission_current.seq <<
std::endl;
break;
}
default:
{
printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
} // end: if read message
// Check for receipt of all items
received_all =
this_timestamps.heartbeat &&
this_timestamps.sys_status &&
// this_timestamps.battery_status &&
// this_timestamps.radio_status &&
this_timestamps.local_position_ned &&
// this_timestamps.global_position_int &&
// this_timestamps.position_target_local_ned &&
this_timestamps.position_target_global_int &&
this_timestamps.highres_imu &&
this_timestamps.attitude ;
// give the write thread time to use the port
if ( writing_status > false )
usleep(100); // look for components of batches at 10kHz
} // end: while not received all
return;
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void Autopilot_Interface::read_messages() {
bool success; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
// Blocking wait for new data
while (!received_all and !time_to_exit) {
// ----------------------------------------------------------------------
// READ MESSAGE
// ----------------------------------------------------------------------
mavlink_message_t message;
success = serial_port->read_message(message);
// ----------------------------------------------------------------------
// HANDLE MESSAGE
// ----------------------------------------------------------------------
if (success) {
// Store message sysid and compid.
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
// Handle Message ID
switch (message.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: {
printf("MAVLINK_MSG_ID_HEARTBEAT\n");
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
break;
}
case MAVLINK_MSG_ID_SYS_STATUS: {
printf("MAVLINK_MSG_ID_SYS_STATUS\n");
mavlink_msg_sys_status_decode(&message,
&(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status = current_messages.time_stamps.sys_status;
printf("Battery life remaining: %i\n",
current_messages.sys_status.battery_remaining);
break;
}
// this is not being received
case MAVLINK_MSG_ID_BATTERY_STATUS: {
printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
mavlink_msg_battery_status_decode(&message,
&(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status =
current_messages.time_stamps.battery_status;
printf("Battery life remaining: %i\n",
current_messages.battery_status.battery_remaining);
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS: {
printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
mavlink_msg_radio_status_decode(&message,
&(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status =
current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED: {
printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
mavlink_msg_local_position_ned_decode(
&message, &(current_messages.local_position_ned));
printf(" pos (GPS): %f %f %f (m)\n",
current_messages.local_position_ned.x,
current_messages.local_position_ned.y,
current_messages.local_position_ned.z);
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned =
current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
mavlink_msg_global_position_int_decode(
&message, &(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int =
current_messages.time_stamps.global_position_int;
mavlink_global_position_int_t gpos =
current_messages.global_position_int;
printf(" pos (GPS): %f %f %f (m)\n", gpos.lat, gpos.lon,
gpos.alt);
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED: {
printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
mavlink_msg_position_target_local_ned_decode(
&message, &(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned =
get_time_usec();
this_timestamps.position_target_local_ned =
current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT: {
printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
mavlink_msg_position_target_global_int_decode(
&message, &(current_messages.position_target_global_int));
current_messages.time_stamps.position_target_global_int =
get_time_usec();
this_timestamps.position_target_global_int =
current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU: {
printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
mavlink_msg_highres_imu_decode(&message,
&(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu =
current_messages.time_stamps.highres_imu;
mavlink_highres_imu_t imu = current_messages.highres_imu;
printf(" ap time: %llu \n", imu.time_usec);
printf(" acc (NED): % f % f % f (m/s^2)\n", imu.xacc, imu.yacc,
imu.zacc);
printf(" gyro (NED): % f % f % f (rad/s)\n", imu.xgyro, imu.ygyro,
imu.zgyro);
printf(" mag (NED): % f % f % f (Ga)\n", imu.xmag, imu.ymag,
imu.zmag);
printf(" baro: %f (mBar) \n", imu.abs_pressure);
printf(" altitude: %f (m) \n", imu.pressure_alt);
printf(" temperature: %f C \n", imu.temperature);
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
printf("### MAVLINK_MSG_ID_ATTITUDE\n");
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
mavlink_attitude_t att = current_messages.attitude;
printf("### roll: %f pitch: %f yaw: %f\n", att.roll, att.pitch,
att.yaw);
break;
}
default: {
// printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
} // end: if read message
// Check for receipt of all items
received_all =
this_timestamps.heartbeat &&
// this_timestamps.battery_status &&
// this_timestamps.radio_status &&
// this_timestamps.local_position_ned &&
// this_timestamps.global_position_int &&
// this_timestamps.position_target_local_ned
//&&
// this_timestamps.position_target_global_int
//&&
// this_timestamps.highres_imu &&
// this_timestamps.attitude &&
this_timestamps.sys_status;
// give the write thread time to use the port
if (writing_status > false) {
usleep(100); // look for components of batches at 10kHz
}
} // end: while not received all
return;
}
