void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) { mavlink_message_t msg; mavlink_mission_count_t wpc; wpc.target_system = wpm.current_partner_sysid; wpc.target_component = wpm.current_partner_compid; wpc.count = count; mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); }
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; } }
/** * This callback is executed each time a waypoint changes. * * It publishes the vehicle_global_position_setpoint_s or the * vehicle_local_position_setpoint_s topic, depending on the type of waypoint */ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, float param2, float param3, float param4, float param5_lat_x, float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command) { static orb_advert_t global_position_setpoint_pub = -1; static orb_advert_t global_position_set_triplet_pub = -1; static orb_advert_t local_position_setpoint_pub = -1; static unsigned last_waypoint_index = -1; char buf[50] = {0}; // XXX include check if WP is supported, jump to next if not /* Update controller setpoints */ if (frame == (int)MAV_FRAME_GLOBAL) { /* global, absolute waypoint */ struct vehicle_global_position_setpoint_s sp; sp.lat = param5_lat_x * 1e7f; sp.lon = param6_lon_y * 1e7f; sp.altitude = param7_alt_z; sp.altitude_is_relative = false; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; set_special_fields(param1, param2, param3, param4, command, &sp); /* Initialize setpoint publication if necessary */ if (global_position_setpoint_pub < 0) { global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); } else { orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); } /* fill triplet: previous, current, next waypoint */ struct vehicle_global_position_set_triplet_s triplet; /* current waypoint is same as sp */ memcpy(&(triplet.current), &sp, sizeof(sp)); /* * Check if previous WP (in mission, not in execution order) * is available and identify correct index */ int last_setpoint_index = -1; bool last_setpoint_valid = false; if (index > 0) { last_setpoint_index = index - 1; } while (last_setpoint_index >= 0) { if (wpm->waypoints[last_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME || wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) { last_setpoint_valid = true; break; } last_setpoint_index--; } /* * Check if next WP (in mission, not in execution order) * is available and identify correct index */ int next_setpoint_index = -1; bool next_setpoint_valid = false; /* next waypoint */ if (wpm->size > 1) { next_setpoint_index = index + 1; } while (next_setpoint_index < wpm->size - 1) { if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME || wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) { next_setpoint_valid = true; break; } next_setpoint_index++; } /* populate last and next */ triplet.previous_valid = false; triplet.next_valid = false; if (last_setpoint_valid) { triplet.previous_valid = true; struct vehicle_global_position_setpoint_s sp; sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f; sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f; sp.altitude = wpm->waypoints[last_setpoint_index].z; sp.altitude_is_relative = false; sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; set_special_fields(wpm->waypoints[last_setpoint_index].param1, wpm->waypoints[last_setpoint_index].param2, wpm->waypoints[last_setpoint_index].param3, wpm->waypoints[last_setpoint_index].param4, wpm->waypoints[last_setpoint_index].command, &sp); memcpy(&(triplet.previous), &sp, sizeof(sp)); } if (next_setpoint_valid) { triplet.next_valid = true; struct vehicle_global_position_setpoint_s sp; sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f; sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f; sp.altitude = wpm->waypoints[next_setpoint_index].z; sp.altitude_is_relative = false; sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; set_special_fields(wpm->waypoints[next_setpoint_index].param1, wpm->waypoints[next_setpoint_index].param2, wpm->waypoints[next_setpoint_index].param3, wpm->waypoints[next_setpoint_index].param4, wpm->waypoints[next_setpoint_index].command, &sp); memcpy(&(triplet.next), &sp, sizeof(sp)); } /* Initialize triplet publication if necessary */ if (global_position_set_triplet_pub < 0) { global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet); } else { orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet); } sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { /* global, relative alt (in relation to HOME) waypoint */ struct vehicle_global_position_setpoint_s sp; sp.lat = param5_lat_x * 1e7f; sp.lon = param6_lon_y * 1e7f; sp.altitude = param7_alt_z; sp.altitude_is_relative = true; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; set_special_fields(param1, param2, param3, param4, command, &sp); /* Initialize publication if necessary */ if (global_position_setpoint_pub < 0) { global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); } else { orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); } sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) { /* local, absolute waypoint */ struct vehicle_local_position_setpoint_s sp; sp.x = param5_lat_x; sp.y = param6_lon_y; sp.z = param7_alt_z; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; /* Initialize publication if necessary */ if (local_position_setpoint_pub < 0) { local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp); } else { orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); } sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else { warnx("non-navigation WP, ignoring"); mavlink_missionlib_send_gcs_string("[mp] Unknown waypoint type, ignoring."); return; } /* only set this for known waypoint types (non-navigation types would have returned earlier) */ last_waypoint_index = index; mavlink_missionlib_send_gcs_string(buf); printf("%s\n", buf); //printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); }
/** * 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); }
/** * This callback is executed each time a waypoint changes. * * It publishes the vehicle_global_position_setpoint_s or the * vehicle_local_position_setpoint_s topic, depending on the type of waypoint */ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, float param2, float param3, float param4, float param5_lat_x, float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command) { static orb_advert_t global_position_setpoint_pub = -1; static orb_advert_t local_position_setpoint_pub = -1; char buf[50] = {0}; /* Update controller setpoints */ if (frame == (int)MAV_FRAME_GLOBAL) { /* global, absolute waypoint */ struct vehicle_global_position_setpoint_s sp; sp.lat = param5_lat_x * 1e7f; sp.lon = param6_lon_y * 1e7f; sp.altitude = param7_alt_z; sp.altitude_is_relative = false; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; /* Initialize publication if necessary */ if (global_position_setpoint_pub < 0) { global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); } else { orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); } sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { /* global, relative alt (in relation to HOME) waypoint */ struct vehicle_global_position_setpoint_s sp; sp.lat = param5_lat_x * 1e7f; sp.lon = param6_lon_y * 1e7f; sp.altitude = param7_alt_z; sp.altitude_is_relative = true; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; /* Initialize publication if necessary */ if (global_position_setpoint_pub < 0) { global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); } else { orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); } sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) { /* local, absolute waypoint */ struct vehicle_local_position_setpoint_s sp; sp.x = param5_lat_x; sp.y = param6_lon_y; sp.z = param7_alt_z; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; /* Initialize publication if necessary */ if (local_position_setpoint_pub < 0) { local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp); } else { orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); } sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } mavlink_missionlib_send_gcs_string(buf); printf("%s\n", buf); //printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); }
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { /* Start sending parameters */ mavlink_pm_start_queued_send(); mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); } break; case MAVLINK_MSG_ID_PARAM_SET: { /* Handle parameter setting */ if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { mavlink_param_set_t mavlink_param_set; mavlink_msg_param_set_decode(msg, &mavlink_param_set); if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN+1]; strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter, set and send it */ param_t param = param_find(name); if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[mavlink pm] unknown: %s", name); mavlink_missionlib_send_gcs_string(buf); } else { /* set and send parameter */ param_set(param, &(mavlink_param_set.param_value)); mavlink_pm_send_param(param); } } } } break; case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { mavlink_param_request_read_t mavlink_param_request_read; mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { /* when no index is given, loop through string ids and compare them */ if (mavlink_param_request_read.param_index == -1) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN+1]; strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter and send it */ mavlink_pm_send_param_for_name(name); } else { /* when index is >= 0, send this parameter again */ mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index); } } } break; case MAVLINK_MSG_ID_COMMAND_LONG: { mavlink_command_long_t cmd_mavlink; mavlink_msg_command_long_decode(msg, &cmd_mavlink); uint8_t result = MAV_RESULT_UNSUPPORTED; if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) ||(cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { /* preflight parameter load / store */ if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_STORAGE) { /* Read all parameters from EEPROM to RAM */ if (((int)(cmd_mavlink.param1)) == 0) { /* read all parameters from EEPROM to RAM */ int read_ret = mavlink_pm_load_eeprom(); if (read_ret == OK) { //printf("[mavlink pm] Loaded EEPROM params in RAM\n"); mavlink_missionlib_send_gcs_string("[mavlink pm] OK loaded EEPROM params"); result = MAV_RESULT_ACCEPTED; } else if (read_ret == 1) { mavlink_missionlib_send_gcs_string("[mavlink pm] No stored parameters to load"); result = MAV_RESULT_ACCEPTED; } else { if (read_ret < -1) { mavlink_missionlib_send_gcs_string("[mavlink pm] ERR loading params from EEPROM"); } else { mavlink_missionlib_send_gcs_string("[mavlink pm] ERR loading params, no EEPROM found"); } result = MAV_RESULT_FAILED; } } else if (((int)(cmd_mavlink.param1)) == 1) { /* write all parameters from RAM to EEPROM */ int write_ret = mavlink_pm_save_eeprom(); if (write_ret == OK) { mavlink_missionlib_send_gcs_string("[mavlink pm] OK params written to EEPROM"); result = MAV_RESULT_ACCEPTED; } else { if (write_ret < -1) { mavlink_missionlib_send_gcs_string("[mavlink pm] ERR writing params to EEPROM"); } else { mavlink_missionlib_send_gcs_string("[mavlink pm] ERR writing params, no EEPROM found"); } result = MAV_RESULT_FAILED; } } else { //fprintf(stderr, "[mavlink pm] refusing unsupported storage request\n"); mavlink_missionlib_send_gcs_string("[mavlink pm] refusing unsupported STOR request"); result = MAV_RESULT_UNSUPPORTED; } } } /* send back command result */ //mavlink_msg_command_ack_send(chan, cmd.command, result); } break; } }
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { /* Start sending parameters */ global_data_parameter_storage->pm.next_param = 0; mavlink_missionlib_send_gcs_string("[pm] sending list"); } break; case MAVLINK_MSG_ID_PARAM_SET: { /* Handle parameter setting */ if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { mavlink_param_set_t mavlink_param_set; mavlink_msg_param_set_decode(msg, &mavlink_param_set); if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { uint16_t i; //parameters uint16_t j; //chars bool match; for (i = 0; i < PARAM_MAX_COUNT; i++) { match = true; for (j = 0; j < MAX_PARAM_NAME_LEN; j++) { /* Compare char by char */ if (global_data_parameter_storage->pm.param_names[i][j] != mavlink_param_set.param_id[j]) { match = false; } /* End matching if null termination is reached */ if (global_data_parameter_storage->pm.param_names[i][j] == '\0') { break; } } /* Check if matched */ if (match) { // XXX handle param type as well, assuming float here global_data_parameter_storage->pm.param_values[i] = mavlink_param_set.param_value; mavlink_pm_send_one_parameter(i); } } } } } break; case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { mavlink_param_request_read_t mavlink_param_request_read; mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { /* when no index is given, loop through string ids and compare them */ if (mavlink_param_request_read.param_index == -1) { uint16_t i; //parameters uint16_t j; //chars bool match; for (i = 0; i < PARAM_MAX_COUNT; i++) { match = true; for (j = 0; j < MAX_PARAM_NAME_LEN; j++) { /* Compare char by char */ if (global_data_parameter_storage->pm.param_names[i][j] != mavlink_param_request_read.param_id[j]) { match = false; } /* End matching if null termination is reached */ if (global_data_parameter_storage->pm.param_names[i][j] == '\0') { break; } } /* Check if matched */ if (match) { mavlink_pm_send_one_parameter(i); } } } else { /* when index is >= 0, send this parameter again */ mavlink_pm_send_one_parameter(mavlink_param_request_read.param_index); } } } 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); }
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t* msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { // Start sending parameters pm.next_param = 0; mavlink_missionlib_send_gcs_string("PM SENDING LIST"); } break; case MAVLINK_MSG_ID_PARAM_SET: { mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); // Check if this message is for this system if (set.target_system == mavlink_system.sysid && set.target_component == mavlink_system.compid) { char* key = set.param_id; for (uint16_t i = 0; i < MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN; i++) { bool match = true; for (uint16_t j = 0; j < MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN; j++) { // Compare if (pm.param_names[i][j] != key[j]) { match = false; } // End matching if null termination is reached if (pm.param_names[i][j] == '\0') { break; } } // Check if matched if (match) { // 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 (pm.param_values[i] != set.param_value && !isnan(set.param_value) && !isinf(set.param_value)) { pm.param_values[i] = set.param_value; // Report back new value #ifndef MAVLINK_USE_CONVENIENCE_FUNCTIONS mavlink_message_t tx_msg; mavlink_msg_param_value_pack_chan(mavlink_system.sysid, mavlink_system.compid, MAVLINK_COMM_0, &tx_msg, pm.param_names[i], pm.param_values[i], MAVLINK_TYPE_FLOAT, pm.size, i); mavlink_missionlib_send_message(&tx_msg); #else mavlink_msg_param_value_send(MAVLINK_COMM_0, pm.param_names[i], pm.param_values[i], MAVLINK_TYPE_FLOAT, pm.size, i); #endif mavlink_missionlib_send_gcs_string("PM received param"); } // End valid value check } // End match check } // End for loop } // End system ID check } // End case break; } // End switch }
/** * Telemetry transmit task. Processes queue events and periodic updates. */ static void telemetryRxTask(void *parameters) { uint32_t inputPort; uint8_t c; // Task loop while (1) { #if defined(PIOS_INCLUDE_USB_HID) // Determine input port (USB takes priority over telemetry port) if (PIOS_USB_HID_CheckAvailable(0)) { inputPort = PIOS_COM_TELEM_USB; } else #endif /* PIOS_INCLUDE_USB_HID */ { inputPort = telemetryPort; } mavlink_channel_t mavlink_chan = MAVLINK_COMM_0; // Block until a byte is available PIOS_COM_ReceiveBuffer(inputPort, &c, 1, portMAX_DELAY); // And process it if (mavlink_parse_char(mavlink_chan, c, &rx_msg, &rx_status)) { // Handle packet with waypoint component mavlink_wpm_message_handler(&rx_msg); // Handle packet with parameter component mavlink_pm_message_handler(mavlink_chan, &rx_msg); switch (rx_msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: { // Check if this is the gcs mavlink_heartbeat_t beat; mavlink_msg_heartbeat_decode(&rx_msg, &beat); if (beat.type == MAV_TYPE_GCS) { // Got heartbeat from the GCS, we're good! lastOperatorHeartbeat = xTaskGetTickCount() * portTICK_RATE_MS; } } break; case MAVLINK_MSG_ID_SET_MODE: { mavlink_set_mode_t mode; mavlink_msg_set_mode_decode(&rx_msg, &mode); // Check if this system should change the mode if (mode.target_system == mavlink_system.sysid) { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); switch (mode.base_mode) { case MAV_MODE_MANUAL_ARMED: { flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL; flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED; } break; case MAV_MODE_MANUAL_DISARMED: { flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL; flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; } break; case MAV_MODE_PREFLIGHT: { flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; } break; case MAV_MODE_STABILIZE_ARMED: { flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED1; flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED; } break; case MAV_MODE_GUIDED_ARMED: { flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED2; flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED; } break; case MAV_MODE_AUTO_ARMED: { flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED3; flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED; } break; } bool newHilEnabled = (mode.base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL); if (newHilEnabled != hilEnabled) { if (newHilEnabled) { // READ-ONLY flag write to ActuatorCommand UAVObjMetadata meta; UAVObjHandle handle = ActuatorCommandHandle(); UAVObjGetMetadata(handle, &meta); meta.access = ACCESS_READONLY; UAVObjSetMetadata(handle, &meta); mavlink_missionlib_send_gcs_string("ENABLING HIL SIMULATION"); mavlink_missionlib_send_gcs_string("+++++++++++++++++++++++"); mavlink_missionlib_send_gcs_string("BLOCKING ALL ACTUATORS"); } else { // READ-ONLY flag write to ActuatorCommand UAVObjMetadata meta; UAVObjHandle handle = ActuatorCommandHandle(); UAVObjGetMetadata(handle, &meta); meta.access = ACCESS_READWRITE; UAVObjSetMetadata(handle, &meta); mavlink_missionlib_send_gcs_string("DISABLING HIL SIMULATION"); mavlink_missionlib_send_gcs_string("+++++++++++++++++++++++"); mavlink_missionlib_send_gcs_string("ACTIVATING ALL ACTUATORS"); } } hilEnabled = newHilEnabled; FlightStatusSet(&flightStatus); // Check HIL bool hilEnabled = (mode.base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL); enableHil(hilEnabled); } } break; case MAVLINK_MSG_ID_HIL_STATE: { if (hilEnabled) { mavlink_hil_state_t hil; mavlink_msg_hil_state_decode(&rx_msg, &hil); // Write GPSPosition GPSPositionData gps; GPSPositionGet(&gps); gps.Altitude = hil.alt/10; gps.Latitude = hil.lat/10; gps.Longitude = hil.lon/10; GPSPositionSet(&gps); // Write PositionActual PositionActualData pos; PositionActualGet(&pos); // FIXME WRITE POSITION HERE PositionActualSet(&pos); // Write AttitudeActual AttitudeActualData att; AttitudeActualGet(&att); att.Roll = hil.roll; att.Pitch = hil.pitch; att.Yaw = hil.yaw; // FIXME //att.RollSpeed = hil.rollspeed; //att.PitchSpeed = hil.pitchspeed; //att.YawSpeed = hil.yawspeed; // Convert to quaternion formulation RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1); // Write AttitudeActual AttitudeActualSet(&att); // Write AttitudeRaw AttitudeRawData raw; AttitudeRawGet(&raw); raw.gyros[0] = hil.rollspeed; raw.gyros[1] = hil.pitchspeed; raw.gyros[2] = hil.yawspeed; raw.accels[0] = hil.xacc; raw.accels[1] = hil.yacc; raw.accels[2] = hil.zacc; // raw.magnetometers[0] = hil.xmag; // raw.magnetometers[0] = hil.ymag; // raw.magnetometers[0] = hil.zmag; AttitudeRawSet(&raw); } } break; case MAVLINK_MSG_ID_COMMAND_LONG: { // FIXME Implement } break; } } } }