public void NextWP() { if (selected && (apm_mode == (int)MAVLink.COPTER_MODE.AUTO)) { MAVLink.mavlink_mission_set_current_t cmd = new MAVLink.mavlink_mission_set_current_t { target_system = 0, target_component = 0, seq = (ushort)(cur_mis_seq + 1) }; byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_SET_CURRENT, cmd); sock.SendTo(data, myproxy); nxt_wp_seq = cur_mis_seq + 1; Debug.Log("NextWP " + nxt_wp_seq); } }
// 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; } }