void ofxMavlink::sendMissionLand()
{
//cout << "Send mission land" << endl;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
mavlink_mission_item_t mission;
mavlink_message_t message;
mission.autocontinue = 0;
mission.current = 2; //2 for guided mode
mission.param1 = 0;
mission.param2 = 0;
mission.param3 = 0;
mission.param4 = 0;
mission.frame = 0;// MAV_FRAME_GLOBAL;
mission.command = MAV_CMD_NAV_LAND;
mission.seq = 0; // don't read out the sequence number of the waypoint class
mission.x = 0; //latitude
mission.y = 0; //longitude
mission.z = 3; //altitude
mission.target_system = 1;
mission.target_component = MAV_COMP_ID_MISSIONPLANNER;
mavlink_msg_mission_item_encode(sysId, compId, &message, &mission);
unsigned len = mavlink_msg_to_send_buffer(buf, &message);
/* write packet via serial link */
int returnval = write(fd, buf, len);
if(returnval < 0) ofLogError("ofxMavlink") << "Error writing to serial port";
/* wait until all data has been written */
tcdrain(fd);
}
COMM_MAV_RESULT navigation_tx_mission_item(COMM_MAV_MSG_TARGET *target, mavlink_message_t *msg, mavlink_message_t *answer_msg)
{
uint16_t seq_id = mavlink_msg_mission_request_get_seq(msg);
if(seq_id < N_TARGETS_MAX)
{
// this function packs the already available struct.
mavlink_msg_mission_item_encode(mavlink_system.sysid, target->component, answer_msg, &(mission_items.item[seq_id]));
return REPLY_TO_SENDER;
}
else //requested item outside buffer
{
//TODO: send NACK
return NO_ANSWER;
}
}
void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
{
if (seq < wpm->size) {
mavlink_message_t msg;
mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
wp->target_system = wpm->current_partner_sysid;
wp->target_component = wpm->current_partner_compid;
mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp);
mavlink_missionlib_send_message(&msg);
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system);
// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
} else {
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\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;
}
}
