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