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_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;
}
}
