void mavlink_wpm_loop() { //check for timed-out operations uint64_t now = mavlink_missionlib_get_system_timestamp(); if (now-wpm.timestamp_lastaction > wpm.timeout && wpm.current_state != MAVLINK_WPM_STATE_IDLE) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("Operation timeout switching -> IDLE"); #else if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state); #endif wpm.current_state = MAVLINK_WPM_STATE_IDLE; wpm.current_count = 0; wpm.current_partner_sysid = 0; wpm.current_partner_compid = 0; wpm.current_wp_id = -1; if(wpm.size == 0) { wpm.current_active_wp_id = -1; } } if(now-wpm.timestamp_last_send_setpoint > wpm.delay_setpoint && wpm.current_active_wp_id < wpm.size) { mavlink_wpm_send_setpoint(wpm.current_active_wp_id); } }
/* * @brief Directs the MAV to fly to a position * * Sends a message to the controller, advising it to fly to the coordinates * of the waypoint with a given orientation * * @param seq The waypoint sequence number the MAV should fly to. */ void mavlink_wpm_send_setpoint(uint16_t seq) { if (seq < wpm->size) { mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); mavlink_missionlib_current_waypoint_changed(cur->seq, cur->param1, cur->param2, cur->param3, cur->param4, cur->x, cur->y, cur->z, cur->frame, cur->command); wpm->timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp(); } else { if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n"); } }
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; } }
/** * MAVLink Protocol main function. */ int mavlink_thread_main(int argc, char *argv[]) { /* initialize mavlink text message buffering */ mavlink_logbuffer_init(&lb, 5); int ch; char *device_name = "/dev/ttyS1"; baudrate = 57600; /* work around some stupidity in task_create's argv handling */ argc -= 2; argv += 2; while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { switch (ch) { case 'b': baudrate = strtoul(optarg, NULL, 10); if (baudrate == 0) errx(1, "invalid baud rate '%s'", optarg); break; case 'd': device_name = optarg; break; case 'e': mavlink_link_termination_allowed = true; break; case 'o': mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD; break; default: usage(); } } struct termios uart_config_original; bool usb_uart; /* print welcome text */ warnx("MAVLink v1.0 serial interface starting..."); /* inform about mode */ warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); /* Flush stdout in case MAVLink is about to take it over */ fflush(stdout); /* default values for arguments */ uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart); if (uart < 0) err(1, "could not open %s", device_name); /* create the device node that's used for sending text log messages, etc. */ register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL); /* Initialize system properties */ mavlink_update_system(); /* start the MAVLink receiver */ receive_thread = receive_start(uart); /* start the ORB receiver */ uorb_receive_thread = uorb_receive_start(); /* initialize waypoint manager */ mavlink_wpm_init(wpm); /* all subscriptions are now active, set up initial guess about rate limits */ if (baudrate >= 230400) { /* 200 Hz / 5 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20); /* 50 Hz / 20 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30); /* 20 Hz / 50 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); /* 10 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); /* 10 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); } else if (baudrate >= 115200) { /* 20 Hz / 50 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); /* 5 Hz / 200 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); /* 5 Hz / 200 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); } else if (baudrate >= 57600) { /* 10 Hz / 100 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); /* 10 Hz / 100 ms ATTITUDE */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200); /* 5 Hz / 200 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); /* 5 Hz / 200 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); } else { /* very low baud rate, limit to 1 Hz / 1000 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000); /* 1 Hz / 1000 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); /* 0.5 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); /* 0.1 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); } thread_running = true; /* arm counter to go off immediately */ unsigned lowspeed_counter = 10; while (!thread_should_exit) { /* 1 Hz */ if (lowspeed_counter == 10) { mavlink_update_system(); /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); /* switch HIL mode if required */ set_hil_on_off(v_status.flag_hil_enabled); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, v_status.load, v_status.voltage_battery * 1000.0f, v_status.current_battery * 1000.0f, v_status.battery_remaining, v_status.drop_rate_comm, v_status.errors_comm, v_status.errors_count1, v_status.errors_count2, v_status.errors_count3, v_status.errors_count4); lowspeed_counter = 0; } lowspeed_counter++; /* sleep quarter the time */ usleep(25000); /* check if waypoint has been reached against the last positions */ mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); /* sleep quarter the time */ usleep(25000); /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); /* sleep quarter the time */ usleep(25000); if (baudrate > 57600) { mavlink_pm_queued_send(); } /* sleep 10 ms */ usleep(10000); /* send one string at 10 Hz */ if (!mavlink_logbuffer_is_empty(&lb)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&lb, &msg); if (lb_ret == OK) { mavlink_missionlib_send_gcs_string(msg.text); } } /* sleep 15 ms */ usleep(15000); } /* wait for threads to complete */ pthread_join(receive_thread, NULL); pthread_join(uorb_receive_thread, NULL); /* Reset the UART flags to original state */ if (!usb_uart) tcsetattr(uart, TCSANOW, &uart_config_original); thread_running = false; exit(0); }
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); }