private void setWPACK()
{
MAVLink.mavlink_mission_ack_t req = new MAVLink.mavlink_mission_ack_t();
req.target_system = TARGET_SYSTEM_ID;
req.target_component = TARGET_SYS_COMPID;
req.type = 0;
generatePacket((byte)MAVLink.MAVLINK_MSG_ID.MISSION_ACK, req);
}
private void but_mission_Click(object sender, EventArgs e)
{
MAVLink.mavlink_mission_count_t req = new MAVLink.mavlink_mission_count_t();
req.target_system = 1;
req.target_component = 1;
// set wp count
req.count = 1;
byte[] packet = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_COUNT, req);
Console.WriteLine("MISSION_COUNT send");
serialPort1.Write(packet, 0, packet.Length);
var ack = readsomedata <MAVLink.mavlink_mission_request_t>(sysid, compid);
if (ack.seq == 0)
{
MAVLink.mavlink_mission_item_int_t req2 = new MAVLink.mavlink_mission_item_int_t();
req2.target_system = sysid;
req2.target_component = compid;
req2.command = (byte)MAVLink.MAV_CMD.WAYPOINT;
req2.current = 1;
req2.autocontinue = 0;
req2.frame = (byte)MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;
req2.y = (int)(115 * 1.0e7);
req2.x = (int)(-35 * 1.0e7);
req2.z = (float)(2.34);
req2.param1 = 0;
req2.param2 = 0;
req2.param3 = 0;
req2.param4 = 0;
req2.seq = 0;
packet = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_ITEM_INT, req2);
Console.WriteLine("MISSION_ITEM_INT send");
lock (readlock)
{
serialPort1.Write(packet, 0, packet.Length);
var ack2 = readsomedata <MAVLink.mavlink_mission_ack_t>(sysid, compid);
if ((MAVLink.MAV_MISSION_RESULT)ack2.type != MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
{
}
}
MAVLink.mavlink_mission_ack_t req3 = new MAVLink.mavlink_mission_ack_t();
req3.target_system = 1;
req3.target_component = 1;
req3.type = 0;
packet = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.MISSION_ACK, req3);
Console.WriteLine("MISSION_ACK send");
serialPort1.Write(packet, 0, packet.Length);
}
}
public void setWPACK()
{
MAVLink.mavlink_mission_ack_t req = new MAVLink.mavlink_mission_ack_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid;
req.type = 0;
generatePacket((byte) MAVLINK_MSG_ID.MISSION_ACK, req);
}
private void LogPacket(object packet, bool ingoing, int sysId, int compId)
{
if (threadDone)
{
return;
}
if (listViewMessages.InvokeRequired)
{
try { listViewMessages.Invoke(new Action <object, bool, int, int>(LogPacket), packet, ingoing, sysId, compId); }
catch { };
return;
}
List <string> fields = new List <string>();
fields.Add(sysId.ToString());
fields.Add(compId.ToString());
if ((ingoing && !checkBoxIngoing.Checked) || (!ingoing && !checkBoxOutgoing.Checked))
{
return;
}
string messageName = packet.ToString().Replace("MAVLink+mavlink_", "");
if (IsMessageFilteredOut(messageName))
{
return;
}
if (listViewMessages.IsDisposed)
{
return;
}
if (packet.GetType() == typeof(MAVLink.mavlink_gps_raw_int_t))
{
MAVLink.mavlink_gps_raw_int_t gps = (MAVLink.mavlink_gps_raw_int_t)packet;
fields.Add("GPS Raw Int");
fields.Add(((double)gps.lat / 10000000).ToString());
fields.Add(((double)gps.lon / 10000000).ToString());
fields.Add(gps.alt.ToString());
fields.Add(gps.vel.ToString());
fields.Add(gps.satellites_visible.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_global_position_int_t))
{
MAVLink.mavlink_global_position_int_t gps = (MAVLink.mavlink_global_position_int_t)packet;
fields.Add("GPS Global Position Int");
fields.Add(((double)gps.lat / 10000000).ToString());
fields.Add(((double)gps.lon / 10000000).ToString());
fields.Add(gps.alt.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_attitude_t))
{
MAVLink.mavlink_attitude_t att = (MAVLink.mavlink_attitude_t)packet;
fields.Add("Attitude");
fields.Add((att.pitch * 180.0 / Math.PI).ToString());
fields.Add((att.roll * 180.0 / Math.PI).ToString());
fields.Add((att.yaw * 180.0 / Math.PI).ToString());
fields.Add((att.pitchspeed * 180.0 / Math.PI).ToString());
fields.Add((att.rollspeed * 180.0 / Math.PI).ToString());
fields.Add((att.yawspeed * 180.0 / Math.PI).ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_scaled_imu_t))
{
MAVLink.mavlink_scaled_imu_t imu = (MAVLink.mavlink_scaled_imu_t)packet;
fields.Add("Scaled IMU");
fields.Add(imu.xmag.ToString());
fields.Add(imu.ymag.ToString());
fields.Add(imu.zmag.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_scaled_imu3_t))
{
MAVLink.mavlink_scaled_imu3_t imu = (MAVLink.mavlink_scaled_imu3_t)packet;
fields.Add("Scaled IMU3");
fields.Add(imu.xmag.ToString());
fields.Add(imu.ymag.ToString());
fields.Add(imu.zmag.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_scaled_imu2_t))
{
MAVLink.mavlink_scaled_imu2_t imu = (MAVLink.mavlink_scaled_imu2_t)packet;
fields.Add("Scaled IMU2");
fields.Add(imu.xmag.ToString());
fields.Add(imu.ymag.ToString());
fields.Add(imu.zmag.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_sys_status_t))
{
MAVLink.mavlink_sys_status_t status = (MAVLink.mavlink_sys_status_t)packet;
fields.Add("System Status");
fields.Add(status.voltage_battery.ToString());
}
// else if (packet.GetType() == typeof(MAVLink.mavlink_autopilot_version_t))
// {
// MAVLink.mavlink_autopilot_version_t ver = (MAVLink.mavlink_autopilot_version_t)packet;
// listViewMessages.Items.Add(new ListViewItem(new string[] {
// "Autopilot Version",
// ver.version.ToString(),
// ver.custom_version.ToString(),
// ver.capabilities.ToString()}));
// }
else if (packet.GetType() == typeof(MAVLink.mavlink_heartbeat_t))
{
MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)packet;
fields.Add("Heartbeat");
fields.Add(hb.autopilot.ToString());
fields.Add(hb.system_status.ToString());
fields.Add(hb.mavlink_version.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_statustext_t))
{
MAVLink.mavlink_statustext_t status = (MAVLink.mavlink_statustext_t)packet;
fields.Add("Status Text");
fields.Add(ASCIIEncoding.ASCII.GetString(status.text));
fields.Add(status.severity.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_param_value_t))
{
MAVLink.mavlink_param_value_t paramValue = (MAVLink.mavlink_param_value_t)packet;
fields.Add("Param Value");
fields.Add(ASCIIEncoding.ASCII.GetString(paramValue.param_id));
fields.Add(paramValue.param_value.ToString());
fields.Add(paramValue.param_count.ToString());
fields.Add(paramValue.param_index.ToString());
fields.Add(paramValue.param_type.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_param_request_read_t))
{
MAVLink.mavlink_param_request_read_t paramReq = (MAVLink.mavlink_param_request_read_t)packet;
fields.Add("Param Request Read");
fields.Add(ASCIIEncoding.ASCII.GetString(paramReq.param_id));
}
else if (packet.GetType() == typeof(MAVLink.mavlink_param_set_t))
{
MAVLink.mavlink_param_set_t paramSet = (MAVLink.mavlink_param_set_t)packet;
fields.Add("Param Set");
fields.Add(ASCIIEncoding.ASCII.GetString(paramSet.param_id));
fields.Add(paramSet.param_value.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_count_t))
{
MAVLink.mavlink_mission_count_t paramValue = (MAVLink.mavlink_mission_count_t)packet;
fields.Add("Mission Count");
fields.Add(paramValue.count.ToString());
fields.Add(paramValue.target_component.ToString());
fields.Add(paramValue.target_system.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_item_t))
{
MAVLink.mavlink_mission_item_t item = (MAVLink.mavlink_mission_item_t)packet;
fields.Add("Mission Item");
fields.Add(item.seq.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_request_t))
{
MAVLink.mavlink_mission_request_t item = (MAVLink.mavlink_mission_request_t)packet;
fields.Add("Mission Request Item");
fields.Add(item.seq.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_command_ack_t))
{
MAVLink.mavlink_command_ack_t paramValue = (MAVLink.mavlink_command_ack_t)packet;
fields.Add("Ack");
fields.Add(((MAVLink.MAV_CMD)paramValue.command).ToString());
fields.Add(((MAVLink.MAV_RESULT)paramValue.result).ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_mission_ack_t))
{
MAVLink.mavlink_mission_ack_t paramValue = (MAVLink.mavlink_mission_ack_t)packet;
fields.Add("Mission Ack");
fields.Add(paramValue.type.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_radio_status_t))
{
MAVLink.mavlink_radio_status_t radio = (MAVLink.mavlink_radio_status_t)packet;
fields.Add("Radio Status");
fields.Add(radio.rssi.ToString());
fields.Add(radio.remrssi.ToString());
}
else if (packet.GetType() == typeof(MAVLink.mavlink_ekf_status_report_t))
{
MAVLink.mavlink_ekf_status_report_t ekf = (MAVLink.mavlink_ekf_status_report_t)packet;
fields.Add("EKF Status");
fields.Add(ekf.flags.ToString());
fields.Add(ekf.velocity_variance.ToString());
fields.Add(ekf.pos_horiz_variance.ToString());
fields.Add(ekf.pos_vert_variance.ToString());
fields.Add(ekf.compass_variance.ToString());
fields.Add(ekf.terrain_alt_variance.ToString());
}
else
{
fields.Add(messageName);
//Log(packet.ToString());
}
if (ingoing)
{
listViewMessages.Items.Add(INGOING(new ListViewItem(fields.ToArray())));
}
else
{
listViewMessages.Items.Add(OUTGOING(new ListViewItem(fields.ToArray())));
}
}
// 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;
}
