/* * @brief Sends an waypoint ack message */ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) { mavlink_message_t msg; mavlink_mission_ack_t wpa; wpa.target_system = wpm->current_partner_sysid; wpa.target_component = wpm->current_partner_compid; wpa.type = type; mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa); mavlink_missionlib_send_message(&msg); // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); if (MAVLINK_WPM_TEXT_FEEDBACK) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); #else if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); #endif mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); } }
void mavlink_connector::handle_missions(mavlink_message_t *msg) { mavlink_message_t resp; switch (msg->msgid) { case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { mavlink_mission_count_t count; count.count = dji_variable::wp_m.missions[0].size(); count.target_component = msg->compid; count.target_system = msg->sysid; mavlink_msg_mission_count_encode(0, 200, &resp, &count); this->send_msg(&resp); break; } case MAVLINK_MSG_ID_MISSION_REQUEST: { mavlink_mission_request_t req; mavlink_msg_mission_request_decode(msg, &req); // printf("ground is trying to request mission %d \n",req.seq); std::vector<mavlink_mission_item_t> mission_msgs = dji_variable::wp_m.missions[0].to_mission_items(); mavlink_mission_item_t a = mission_msgs[req.seq]; a.target_system = msg->sysid; a.target_component = msg->compid; a.current = (a.seq == dji_variable::wp_m.waypoint_ptr && dji_variable::wp_m.mission_id == 0)? 1:0; mavlink_msg_mission_item_encode(0,200, &resp, &a); this->send_msg(&resp); break; } case MAVLINK_MSG_ID_MISSION_COUNT: { mavlink_mission_count_t mission_count_t; mavlink_msg_mission_count_decode(msg, &mission_count_t); if (MISSION_REC_STATE == MISSION_REC_STANDBY) { printf("Ground is trying to set a waypoint long as %d\n", mission_count_t.count); waypoint_length_size = mission_count_t.count; waypoint_waitfor = 0; dji_variable::wp_m.missions[0].clear(); MISSION_REC_STATE = MISSION_REC_WAIT_FOR; mavlink_mission_request_t request_t; request_t.target_component = msg->compid; request_t.target_system = msg->sysid; request_t.seq = 0; mavlink_msg_mission_request_encode(0,200,&resp,&request_t); this->send_msg(&resp); break; } }; case MAVLINK_MSG_ID_MISSION_ITEM: { if (MISSION_REC_STATE != MISSION_REC_WAIT_FOR) break; mavlink_mission_item_t mission_item_t; mavlink_msg_mission_item_decode(msg,&mission_item_t); printf("seq:%d %f %f cmd: %d\n",mission_item_t.seq,mission_item_t.x,mission_item_t.y,mission_item_t.command); api_pos_custom_data_t wp; wp.lati = mission_item_t.x /180 * M_PI; wp.longti = mission_item_t.y /180 * M_PI; wp.alti = mission_item_t.z; wp.cmd = mission_item_t.command; // wp.uncertain = mission_item_t. wp.uncertain = 10 ; mavlink_mission_request_t request_t; request_t.target_component = msg->compid; request_t.target_system = msg->sysid; if (mission_item_t.seq == waypoint_waitfor) { waypoint_waitfor ++; request_t.seq = waypoint_waitfor; dji_variable::wp_m.missions[0].waypoints.push_back(wp); if (request_t.seq < waypoint_length_size) { mavlink_msg_mission_request_encode(0, 200, &resp, &request_t); this->send_msg(&resp); } else{ std::cout << dji_variable::wp_m.missions[0]; waypoint_waitfor = 0; waypoint_length_size = 0; MISSION_REC_STATE = MISSION_REC_STANDBY; mavlink_mission_ack_t ack_t; ack_t.target_component = msg->compid; ack_t.target_system = msg->sysid; ack_t.type = MAV_MISSION_RESULT::MAV_MISSION_ACCEPTED; mavlink_msg_mission_ack_encode(0,200,&resp,&ack_t); this->send_msg(&resp); } } else{ request_t.seq = waypoint_waitfor; mavlink_msg_mission_request_encode(0, 200, &resp, &request_t); this->send_msg(&resp); } break; }; case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { mavlink_mission_set_current_t set_current_t; mavlink_msg_mission_set_current_decode(msg,&set_current_t); int ptr = set_current_t.seq; printf("say system is trying to set current %d \n",ptr); dji_variable::wp_m.mission_id = 0; dji_variable::wp_m.waypoint_ptr = ptr; dji_variable::wp_m.begin_fly_waypoints(0,ptr); mavlink_mission_ack_t ack_t; ack_t.target_component = msg->compid; ack_t.target_system = msg->sysid; ack_t.type = MAV_MISSION_RESULT::MAV_MISSION_ACCEPTED; mavlink_msg_mission_ack_encode(0,200,&resp,&ack_t); this->send_msg(&resp); }; default: break; } }
void MAVLinkProxy::dataReceived(const boost::system::error_code &error, size_t received_bytes) { static mavlink_message_t msg; static mavlink_status_t status; // Response message and buffer static mavlink_message_t msg_response; static uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // Waypoint reception variables static int waypoints_to_receive = 0; static uint16_t waypoints_received = 0; static int waypoint_timeout = 200; static unsigned long last_waypoint_time = 0; for(int i = 0; i < (int) received_bytes; i++) { if(mavlink_parse_char(MAVLINK_COMM_0, _received_msg_buf[i], &msg, &status)) { // Handle received messages msg_response.magic = 0; // Waypoint reception if(waypoints_to_receive > 0 && (last_waypoint_time + waypoint_timeout <= (std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1)))) { // Timeout printf("Timeout receiving waypoints!\n"); waypoints_to_receive = 0; last_waypoint_time = 0; } if(waypoints_to_receive > 0 && msg.msgid == MAVLINK_MSG_ID_MISSION_ITEM) { // Got waypoint waypoints_received++; waypoints_to_receive--; mavlink_mission_item_t waypoint; mavlink_msg_mission_item_decode(&msg, &waypoint); handleWaypoint(&waypoint); last_waypoint_time = std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1); if(waypoints_to_receive > 0) { mavlink_mission_request_t request; request.seq = waypoints_received; request.target_system = msg.sysid; request.target_component = msg.compid; mavlink_msg_mission_request_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, &request); } else { mavlink_mission_ack_t ack; ack.target_system = msg.sysid; ack.target_component = msg.compid; ack.type = MAV_MISSION_ACCEPTED; mavlink_msg_mission_ack_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, &ack); processPartialMission(); } } else if(msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { printf("Waypoint count request\n"); uint16_t mission_length = 0; mavlink_msg_mission_count_pack(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, msg.sysid, msg.compid, mission_length); } else if(msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { printf("Parameter request\n"); mavlink_param_value_t param; param.param_count = 0; param.param_id[0] = '\0'; param.param_index = 0; param.param_type = MAV_PARAM_TYPE_UINT8; param.param_value = 0; mavlink_msg_param_value_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, ¶m); } else if(msg.msgid == MAVLINK_MSG_ID_MISSION_COUNT) { mavlink_mission_count_t mission_count; mavlink_msg_mission_count_decode(&msg, &mission_count); printf("%d waypoints available\n", mission_count.count); _mission_ready = false; _partial_mission.clear(); waypoints_to_receive = mission_count.count; waypoints_received = 0; last_waypoint_time = std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1); mavlink_mission_request_t request; request.seq = 0; request.target_system = msg.sysid; request.target_component = msg.compid; mavlink_msg_mission_request_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, &request); } else if(msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) { // Heartbeat from MAVLink ground station } else { // Unhandled message type printf("Unknown MAVLink packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid); } // Send a response if the message has been populated if(msg_response.magic != 0) { mavlink_msg_to_send_buffer(buf, &msg_response); _socket.send_to(boost::asio::buffer(buf), _endpoint); } } } // Listen for next packet _socket.async_receive_from(boost::asio::buffer(_received_msg_buf), _endpoint, boost::bind(&MAVLinkProxy::dataReceived, this, boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred)); }