private ushort getWPCount() //获取飞行器航点数量 { MAVLink.mavlink_mission_request_list_t req = new MAVLink.mavlink_mission_request_list_t(); req.target_system = TARGET_SYSTEM_ID; req.target_component = TARGET_SYS_COMPID; bool success = false; for (int i = 0; i < 3; i++) { Program.mav_msg_handler.mission_count_update = false; Program.mav_msg_handler.SetTimeout(3); generatePacket((byte)MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST_LIST, req); while (Program.mav_msg_handler.mission_count_update == false) { if (Program.mav_msg_handler.Wait()) { success = true; this.ListMessage("Read WayPoint count timeout...." + (i + 1).ToString()); break; } Application.DoEvents(); } } if (success) { this.ListMessage("读取航点数量失败..."); return(0); } else { this.ListMessage("读取航点数量成功..."); } ushort mis_count = (ushort)Program.mav_msg_handler.mission_count; return(mis_count); }
// Update is called once per frame void Update() { if (sock.Available > 0) { int recvBytes = 0; try { recvBytes = sock.Receive(buf); } catch (SocketException e) { Debug.LogWarning("socket err " + e.ErrorCode); } if (recvBytes > 0) { MAVLink.MAVLinkMessage msg = mavlinkParse.ReadPacket(buf); if (msg != null) { if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.STATUSTEXT) { var status_txt = (MAVLink.mavlink_statustext_t)msg.data; Debug.Log(System.Text.Encoding.ASCII.GetString(status_txt.text)); } else if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.HEARTBEAT) { var heartbeat = (MAVLink.mavlink_heartbeat_t)msg.data; apm_mode = (int)heartbeat.custom_mode; if (apm_mode != (int)MAVLink.COPTER_MODE.GUIDED) { wp.SetActive(false); } if ((heartbeat.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == 0) { if (armed) { MyDroneModel.GetComponent <DroneAnime>().PropellerRun = false; } armed = false; } else { if (!armed) { MyDroneModel.GetComponent <DroneAnime>().PropellerRun = true; } armed = true; } system_status = heartbeat.system_status; if (!pos_tgt_local_rcved && (apm_mode == (int)MAVLink.COPTER_MODE.GUIDED)) { MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t { command = (ushort)MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL, param1 = (float)MAVLink.MAVLINK_MSG_ID.POSITION_TARGET_LOCAL_NED, param2 = 1000000 }; byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd); sock.SendTo(data, myproxy); } if (!mis_cur_rcved && (apm_mode == (int)MAVLink.COPTER_MODE.AUTO)) { MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t { command = (ushort)MAVLink.MAV_CMD.SET_MESSAGE_INTERVAL, param1 = (float)MAVLink.MAVLINK_MSG_ID.MISSION_CURRENT, param2 = 1000000 }; byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd); sock.SendTo(data, myproxy); } if (mission_count < 0) { MAVLink.mavlink_mission_request_list_t cmd = new MAVLink.mavlink_mission_request_list_t { target_system = 0, target_component = 0, mission_type = 0 }; byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST_LIST, cmd); sock.SendTo(data, myproxy); Debug.Log("send MISSION_REQUEST_LIST"); } if ((wait_mission_seq >= 0) && (wait_mission_seq < mission_count)) { MAVLink.mavlink_mission_request_int_t cmd = new MAVLink.mavlink_mission_request_int_t { target_system = 0, target_component = 0, seq = (ushort)wait_mission_seq, mission_type = 0 }; byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST_INT, cmd); sock.SendTo(data, myproxy); Debug.Log("send MISSION_REQUEST_INT " + wait_mission_seq); } } else if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.POSITION_TARGET_LOCAL_NED) { pos_tgt_local_rcved = true; var pos_tgt = (MAVLink.mavlink_position_target_local_ned_t)msg.data; if (((pos_tgt.type_mask & 0x1000) == 0x1000) || system_status != (byte)MAVLink.MAV_STATE.ACTIVE) { wp.SetActive(false); } else { wp.transform.position = new Vector3(-pos_tgt.x, -pos_tgt.z, pos_tgt.y); wp.SetActive(true); } } else if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.MISSION_CURRENT) { mis_cur_rcved = true; cur_mis_seq = ((MAVLink.mavlink_mission_current_t)msg.data).seq; //Debug.Log("rcv MISSION_CURRENT " + cur_mis_seq); if (cur_mis_seq < nxt_wp_seq) { MAVLink.mavlink_mission_set_current_t cmd = new MAVLink.mavlink_mission_set_current_t { target_system = 0, target_component = 0, seq = (ushort)nxt_wp_seq }; byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_SET_CURRENT, cmd); sock.SendTo(data, myproxy); } else if (mission_chk_points.Contains(cur_mis_seq)) { waiting_in_chk_point = true; } else { waiting_in_chk_point = false; } } else if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.MISSION_COUNT) { mission_count = ((MAVLink.mavlink_mission_count_t)msg.data).count; if (mission_count > 0) { MAVLink.mavlink_mission_request_int_t cmd = new MAVLink.mavlink_mission_request_int_t { target_system = 0, target_component = 0, seq = 0, mission_type = 0 }; byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST_INT, cmd); sock.SendTo(data, myproxy); wait_mission_seq = 0; Debug.Log("send MISSION_REQUEST_INT 0"); } } else if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.MISSION_ITEM_INT) { var item_int = (MAVLink.mavlink_mission_item_int_t)msg.data; if (item_int.seq == wait_mission_seq) { if (item_int.command == 93) //MAV_CMD_NAV_DELAY { mission_chk_points.Add(item_int.seq); } wait_mission_seq += 1; if (wait_mission_seq < mission_count) { MAVLink.mavlink_mission_request_int_t cmd = new MAVLink.mavlink_mission_request_int_t { target_system = 0, target_component = 0, seq = (ushort)wait_mission_seq, mission_type = 0 }; byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST_INT, cmd); sock.SendTo(data, myproxy); Debug.Log("send MISSION_REQUEST_INT " + wait_mission_seq); } else { MAVLink.mavlink_mission_ack_t cmd = new MAVLink.mavlink_mission_ack_t { target_system = 0, target_component = 0, type = 0, mission_type = 0 }; byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_ACK, cmd); sock.SendTo(data, myproxy); Debug.Log("send MISSION_ACK"); } } } /*else if (msg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.MISSION_ITEM_REACHED) * { * Debug.Log("rcv MISSION_ITEM_REACHED "+((MAVLink.mavlink_mission_item_reached_t)msg.data).seq); * int seq = ((MAVLink.mavlink_mission_item_reached_t)msg.data).seq; * if (mis_chk_points.Contains(seq)) * { * MAVLink.mavlink_mission_set_current_t cmd = new MAVLink.mavlink_mission_set_current_t * { * target_system = 0, * target_component = 0, * seq = (ushort)(seq + 2) * }; * byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_SET_CURRENT, cmd); * sock.SendTo(data, myproxy); * } * }*/ } } } if (lastPos.Equals(transform.localPosition)) { tracking = false; } else { tracking = true; vel_update_cd -= Time.deltaTime; if (vel_update_cd < 0) { vel_update_cd = 0.5f; vel = (transform.localPosition - lastPos) / Time.deltaTime; } } lastPos = transform.localPosition; if (!tracking) { if (MyDroneModel.activeSelf) { MyDroneModel.SetActive(false); GetComponent <BoxCollider>().enabled = false; } } else if (!MyDroneModel.activeSelf) { MyDroneModel.SetActive(true); GetComponent <BoxCollider>().enabled = true; } }
public static void GetMission() { MAVLink.mavlink_mission_request_list_t req = new MAVLink.mavlink_mission_request_list_t(); req.target_system = sysID; req.target_component = compID; byte[] bytes = mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.MISSION_REQUEST_LIST, req); if (SendWaitRetry(bytes, typeof(MAVLink.mavlink_mission_count_t))) { if (OnMissionUpdated != null) { OnMissionUpdated(0, missionWaypointsCount); } for (int i = 0; i < missionWaypointsCount; i++) { if (!GetMissionItem(i)) { // Getting mission item failed if (OnMissionUpdated != null) { OnMissionUpdated(-1, missionWaypointsCount); } threadBusy = false; return; } // Getting mission item succeeded if (OnMissionUpdated != null) { OnMissionUpdated(i, missionWaypointsCount); } } // Send Ack MAVLink.mavlink_mission_ack_t ack = new MAVLink.mavlink_mission_ack_t(); ack.target_system = 1; ack.target_component = 0; ack.type = (byte)MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED; byte[] ackBytes = mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.MISSION_ACK, ack); SendPacket(ackBytes); // Notify everyone if (OnMissionUpdated != null) { OnMissionUpdated(missionWaypointsCount, missionWaypointsCount); } } else { // Loading mission failed if (OnMissionUpdated != null) { OnMissionUpdated(-1, missionWaypointsCount); } } threadBusy = false; }