void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq)
{
if (seq < wpm->max_size) {
mavlink_message_t msg;
mavlink_mission_request_t wpr;
wpr.target_system = wpm->current_partner_sysid;
wpr.target_component = wpm->current_partner_compid;
wpr.seq = seq;
mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr);
mavlink_missionlib_send_message(&msg);
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system);
// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
} else {
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n");
}
}
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));
}
