void
MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
{
mavlink_mission_ack_t wpa;
mavlink_msg_mission_ack_decode(msg, &wpa);
if (CHECK_SYSID_COMPID_MISSION(wpa)) {
if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) {
if (_state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
if (_transfer_seq == _count) {
if (_verbose) { warnx("WPM: MISSION_ACK OK all items sent, switch to state IDLE"); }
} else {
_mavlink->send_statustext_critical("WPM: ERR: not all items sent -> IDLE");
if (_verbose) { warnx("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway"); }
}
_state = MAVLINK_WPM_STATE_IDLE;
}
} else {
_mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch");
if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); }
}
}
}
static inline void MissionAck(mavlink_message_t* handle_msg)
{
mavlink_mission_ack_t packet;
//send_text((uint8_t*)"waypoint ack\r\n");
DPRINT("mission ack\r\n");
// decode
mavlink_msg_mission_ack_decode(handle_msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) return;
// parse for error - although we do nothing about an error.
//uint8_t type = packet.type; // ok (0), error(1)
// turn off waypoint send
mavlink_flags.mavlink_sending_waypoints = false;
mavlink_waypoint_timeout = 0;
}
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;
}
}
void protDecodeMavlink(void) {
uint8_t indx, writeSuccess, commChannel = 1;
mavlink_param_set_t set;
mavlink_message_t msg;
mavlink_status_t status;
// uint8_t* dataIn;
// fix the data length so if the interrupt adds data
// during execution of this block, it will be read
// until the next gsRead
unsigned int tmpLen = getLength(uartMavlinkInBuffer), i = 0;
// if the buffer has more data than the max size, set it to max,
// otherwise set it to the length
//DatafromGSmavlink[0] = (tmpLen > MAXINLEN) ? MAXINLEN : tmpLen;
// read the data
//for (i = 1; i <= DatafromGSmavlink[0]; i += 1) {
//mavlink_parse_char(commChannel, readFront(uartBufferInMavlink), &msg, &status);
//DatafromGSmavlink[i] = readFront(uartMavlinkInBuffer);
//}
//dataIn = DatafromGSmavlink;
// increment the age of heartbeat
mlPending.heartbeatAge++;
for (i = 0; i <= tmpLen; i++) {
// Try to get a new message
if (mavlink_parse_char(commChannel, readFront(uartMavlinkInBuffer), &msg, &status)) {
// Handle message
switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
mavlink_msg_heartbeat_decode(&msg, &mlHeartbeat);
// Reset the heartbeat
mlPending.heartbeatAge = 0;
break;
//AM DBG
case MAVLINK_MSG_ID_MISSION_COUNT:
if (!mlPending.wpTransaction && (mlPending.wpProtState == WP_PROT_IDLE)) {
mavlink_msg_mission_count_decode(&msg, &mlWpCount);
// Start the transaction
mlPending.wpTransaction = 1;
// change the state
mlPending.wpProtState = WP_PROT_GETTING_WP_IDLE;
// reset the rest of the state machine
mlPending.wpTotalWps = mlWpCount.count;
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTimeOut = 0;
}
break;
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
// if there is no transaction going on
if (!mlPending.wpTransaction && (mlPending.wpProtState == WP_PROT_IDLE)) {
// Start the transaction
mlPending.wpTransaction = 1;
// change the state
mlPending.wpProtState = WP_PROT_LIST_REQUESTED;
// reset the rest of the state machine
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTimeOut = 0;
}
break;
case MAVLINK_MSG_ID_MISSION_REQUEST:
mavlink_msg_mission_request_decode(&msg, &mlWpRequest);
if (mlPending.wpTransaction && (mlWpRequest.seq < mlWpValues.wpCount)) {
// change the state
mlPending.wpProtState = WP_PROT_TX_WP;
// reset the rest of the state machine
mlPending.wpCurrentWpInTransaction = mlWpRequest.seq;
mlPending.wpTimeOut = 0;
} else {
// TODO: put here a report for a single WP, i.e. not inside a transaction
}
break;
case MAVLINK_MSG_ID_MISSION_ACK:
mavlink_msg_mission_ack_decode(&msg, &mlWpAck);
if (mlPending.wpTransaction) {
// End the transaction
mlPending.wpTransaction = 0;
// change the state
mlPending.wpProtState = WP_PROT_IDLE;
// reset the rest of the state machine
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTimeOut = 0;
// send current waypoint index
mlPending.wpSendCurrent = TRUE;
}
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
writeSuccess = SUCCESS;
mavlink_msg_mission_item_decode(&msg, &mlSingleWp);
if (mlPending.wpTransaction && (mlPending.wpProtState == WP_PROT_RX_WP)) {
mlPending.wpProtState = WP_PROT_GETTING_WP_IDLE;
}
indx = (uint8_t) mlSingleWp.seq;
mlWpValues.lat[indx] = mlSingleWp.x;
mlWpValues.lon[indx] = mlSingleWp.y;
mlWpValues.alt[indx] = mlSingleWp.z;
mlWpValues.type[indx] = mlSingleWp.command;
mlWpValues.orbit[indx] = (uint16_t) mlSingleWp.param3;
/*
// Record the data in EEPROM
writeSuccess = storeWaypointInEeprom(&mlSingleWp);
// Set the flag of Aknowledge for the AKN Message
// if the write was not successful
if (writeSuccess != SUCCESS) {
mlPending.wpAck++;
mlWpAck.target_component = MAV_COMP_ID_MISSIONPLANNER;
mlWpAck.type = MAV_MISSION_ERROR;
}
*/
break;
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
writeSuccess = SUCCESS;
// clear the WP values in memory;
memset(&mlWpValues, 0, sizeof (mavlink_mission_item_values_t));
/*
writeSuccess = clearWaypointsFrom(0);
// Set the flag of Aknowledge fail
// if the write was unsuccessful
if (writeSuccess != SUCCESS) {
mlPending.statustext++;
mlStatustext.severity = MAV_SEVERITY_ERROR;
strncpy(mlStatustext.text, "Failed to clear waypoints from EEPROM.", 49);
}
*/
// Update the waypoint count
mlWpValues.wpCount = 0;
// Set the state machine ready to send the WP akn
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTotalWps = 0;
mlPending.wpTransaction = 1;
mlPending.wpProtState = WP_PROT_GETTING_WP_IDLE;
break;
case MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN:
writeSuccess = SUCCESS;
memset(&mlSingleWp, 0, sizeof (mavlink_mission_item_t));
mavlink_msg_set_gps_global_origin_decode(&msg, &mlGSLocation);
mlSingleWp.x = (float) (mlGSLocation.latitude);
mlSingleWp.y = (float) (mlGSLocation.longitude);
mlSingleWp.z = (float) (mlGSLocation.altitude);
indx = (uint8_t) MAX_NUM_WPS - 1;
mlWpValues.lat[indx] = mlSingleWp.x;
mlWpValues.lon[indx] = mlSingleWp.y;
mlWpValues.alt[indx] = mlSingleWp.z;
mlWpValues.type[indx] = MAV_CMD_NAV_LAND;
mlWpValues.orbit[indx] = 0;
// Record the data in EEPROM
/*
writeSuccess = storeWaypointInEeprom(&mlSingleWp);
if (writeSuccess != SUCCESS) {
mlPending.statustext++;
mlStatustext.severity = MAV_SEVERITY_ERROR;
strncpy(mlStatustext.text, "Failed to write origin to EEPROM.", 49);
}
else {
mlPending.statustext++;
mlStatustext.severity = MAV_SEVERITY_INFO;
strncpy(mlStatustext.text, "Control DSC GPS origin set.", 49);
}
*/
break;
//AM DBG
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
mlPending.piTransaction = 1;
mlPending.piProtState = PI_SEND_ALL_PARAM;
mlPending.piCurrentParamInTransaction = 0;
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
// If it was in the middle of a list transmission or there is already a param enqueued
mlPending.piTransaction = 1;
switch (mlPending.piProtState) {
case PI_IDLE:
mlPending.piBackToList = 0; // no need to go back
mlPending.piQIdx = -1; // no Index
mlPending.piCurrentParamInTransaction = mavlink_msg_param_request_read_get_param_index(&msg); // assign directly
mlPending.piProtState = PI_SEND_ONE_PARAM;
break;
case PI_SEND_ALL_PARAM:
mlPending.piBackToList = 1; // mark to go back
mlPending.piQIdx++; // done like this because when empty index = -1
mlPending.piQueue[mlPending.piQIdx] = mavlink_msg_param_request_read_get_param_index(&msg); // put in in queue
mlPending.piProtState = PI_SEND_ONE_PARAM;
break;
case PI_SEND_ONE_PARAM:
if (mlPending.piBackToList) {
mlPending.piQIdx++; // done like this because when empty index = -1
mlPending.piQueue[mlPending.piQIdx] = mavlink_msg_param_request_read_get_param_index(&msg); // put in in queue
}
mlPending.piProtState = PI_SEND_ONE_PARAM;
break;
}
break;
case MAVLINK_MSG_ID_PARAM_SET:
mavlink_msg_param_set_decode(&msg, &set);
if ((uint8_t) set.target_system == (uint8_t) SYSTEMID &&
(uint8_t) set.target_component == (uint8_t) COMPID) {
char* key = (char*) set.param_id;
uint8_t i, j;
uint8_t match;
for (i = 0; i < PAR_PARAM_COUNT; i++) {
match = 1;
for (j = 0; j < PARAM_NAME_LENGTH; j++) {
// Compare
if (((char) (mlParamInterface.param_name[i][j]))
!= (char) (key[j])) {
match = 0;
} // if
// End matching if null termination is reached
if (((char) mlParamInterface.param_name[i][j]) == '\0') {
break;
} // if
}// for j
// Check if matched
if (match) {
//sw_debug = 1;
// Only write and emit changes if there is actually a difference
// AND only write if new value is NOT "not-a-number"
// AND is NOT infinity
if (isFinite(set.param_value)) {
mlParamInterface.param[i] = set.param_value;
// Report back new value
mlPending.piBackToList = 0; // no need to go back
mlPending.piQIdx = -1; // no Index
mlPending.piCurrentParamInTransaction = i; // assign directly
mlPending.piProtState = PI_SEND_ONE_PARAM;
mlPending.piTransaction = 1;
} // if different and not nan and not inf
} // if match
}// for i
} // if addressed to this
break;
default:
break;
}
}
}
}
void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos)
{
uint64_t now = mavlink_missionlib_get_system_timestamp();
switch (msg->msgid) {
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) {
mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
wpm->current_state = MAVLINK_WPM_STATE_IDLE;
wpm->current_wp_id = 0;
}
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
}
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;
}
}
mavlink_missionlib_send_gcs_string("NEW WP SET");
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 {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
}
} else {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
}
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)) {
// printf("wpc count in: %d\n",wpc.count);
// printf("Comp id: %d\n",msg->compid);
// printf("Current partner sysid: %d\n",wpm->current_partner_sysid);
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");
// printf("sysid=%d, current_partner_sysid=%d\n", msg->sysid, wpm->current_partner_sysid);
// printf("compid=%d, current_partner_compid=%d\n", msg->compid, wpm->current_partner_compid);
// 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*/))
if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) {
wpm->timestamp_lastaction = now;
// printf("wpm->current_state=%u, wp.seq = %d, wpm->current_wp_id=%d\n", wpm->current_state, wp.seq, wpm->current_wp_id);
// wpm->current_state = MAVLINK_WPM_STATE_GETLIST;//removeme debug XXX TODO
//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)) {
//mavlink_missionlib_send_gcs_string("DEBUG 2");
// 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));
// printf("WP seq: %d\n",wp.seq);
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);
// 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);
// printf ("wpm->current_wp_id =%d, wpm->current_count=%d\n\n", wpm->current_wp_id, wpm->current_count);
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!!!
uint32_t i;
for (i = 0; i < wpm->current_count; ++i) {
wpm->waypoints[i] = wpm->rcv_waypoints[i];
}
wpm->size = wpm->current_count;
//get the new current waypoint
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);
mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, 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_waypoints_reached(now, global_pos, local_pos);
}