public bool Goto(float x, float y, float z)
{
if (selected)
{
if (apm_mode != (int)MAVLink.COPTER_MODE.GUIDED)
{
MAVLink.mavlink_set_mode_t cmd = new MAVLink.mavlink_set_mode_t
{
base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED,
target_system = 0,
custom_mode = (uint)MAVLink.COPTER_MODE.GUIDED
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.SET_MODE, cmd);
sock.SendTo(data, myproxy);
}
{
MAVLink.mavlink_set_position_target_local_ned_t cmd = new MAVLink.mavlink_set_position_target_local_ned_t
{
target_system = 0,
target_component = 0,
coordinate_frame = (byte)MAVLink.MAV_FRAME.LOCAL_NED,
type_mask = 0x0DF8,
x = x,
y = y,
z = z
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.SET_POSITION_TARGET_LOCAL_NED, cmd);
sock.SendTo(data, myproxy);
}
return(true);
}
return(false);
}
public static bool translateMode(string modein, ref MAVLink.mavlink_set_mode_t mode)
{
//MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t();
mode.target_system = MainV2.comPort.sysid;
try
{
if (Common.getModes() == typeof(Common.apmmodes))
{
switch ((int)Enum.Parse(Common.getModes(), modein))
{
case (int)Common.apmmodes.MANUAL:
case (int)Common.apmmodes.CIRCLE:
case (int)Common.apmmodes.STABILIZE:
case (int)Common.apmmodes.AUTO:
case (int)Common.apmmodes.RTL:
case (int)Common.apmmodes.LOITER:
case (int)Common.apmmodes.FLY_BY_WIRE_A:
case (int)Common.apmmodes.FLY_BY_WIRE_B:
mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
mode.custom_mode = (uint)(int)Enum.Parse(Common.getModes(), modein);
break;
default:
MessageBox.Show("No Mode Changed " + (int)Enum.Parse(Common.getModes(), modein));
return(false);
}
}
else if (Common.getModes() == typeof(Common.ac2modes))
{
switch ((int)Enum.Parse(Common.getModes(), modein))
{
case (int)Common.ac2modes.STABILIZE:
case (int)Common.ac2modes.AUTO:
case (int)Common.ac2modes.RTL:
case (int)Common.ac2modes.LOITER:
case (int)Common.ac2modes.ACRO:
case (int)Common.ac2modes.ALT_HOLD:
case (int)Common.ac2modes.CIRCLE:
case (int)Common.ac2modes.POSITION:
mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
mode.custom_mode = (uint)(int)Enum.Parse(Common.getModes(), modein);
break;
default:
MessageBox.Show("No Mode Changed " + (int)Enum.Parse(Common.getModes(), modein));
return(false);
}
}
}
catch { System.Windows.Forms.MessageBox.Show("Failed to find Mode"); return(false); }
return(true);
}
private void button11_Click(object sender, EventArgs e)
{
MAVLink.mavlink_set_mode_t cmd = new MAVLink.mavlink_set_mode_t();
cmd.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
cmd.custom_mode = 4;
cmd.target_system = 1;
DroneData drone = drones["bebop2"];
byte[] pkt = mavlinkParse.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.SET_MODE, cmd);
mavSock.SendTo(pkt, drone.ep);
}
public void LandClick()
{
MAVLink.mavlink_set_mode_t cmd = new MAVLink.mavlink_set_mode_t
{
base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED,
target_system = 0,
custom_mode = 9
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.SET_MODE, cmd);
sock.SendTo(data, ep);
}
public void Guided()
{
MAVLink.mavlink_set_mode_t cmd = new MAVLink.mavlink_set_mode_t
{
base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED,
target_system = 0,
custom_mode = (uint)MAVLink.COPTER_MODE.GUIDED
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.SET_MODE, cmd);
sock.SendTo(data, myproxy);
}
public static void ChangeFlightMode(APMModes.Quadrotor mode)
{
MAVLink.mavlink_set_mode_t cmd = new MAVLink.mavlink_set_mode_t();
cmd.target_system = sysID;
cmd.base_mode = 1; // Not sure what this is
cmd.custom_mode = (uint)mode;
byte[] commandBytes = mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.SET_MODE, cmd);
SendPacket(commandBytes);
System.Threading.Thread.Sleep(10);
SendPacket(commandBytes);
}
public void Stabilize()
{
if (selected)
{
MAVLink.mavlink_set_mode_t cmd = new MAVLink.mavlink_set_mode_t
{
base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED,
target_system = (byte)DroneID,
custom_mode = (uint)MAVLink.COPTER_MODE.STABILIZE
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.SET_MODE, cmd);
sock.SendTo(data, myproxy);
}
}
public void GuidedClick()
{
//Debug.Log("clicked");
MAVLink.mavlink_set_mode_t cmd = new MAVLink.mavlink_set_mode_t
{
base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED,
target_system = 0,
custom_mode = 4
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.SET_MODE, cmd);
sock.SendTo(data, ep);
StartCoroutine(SetGPSOrigin());
}
public void setMode(string modein)
{
try
{
MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t();
if (translateMode(modein, ref mode))
{
setMode(mode);
}
}
catch
{
System.Windows.Forms.MessageBox.Show("Failed to change Modes");
}
}
private void autoFly_Click(object sender, EventArgs e)
{
MAVLink.mavlink_set_mode_t mode1 = new MAVLink.mavlink_set_mode_t();
byte base_mode1 = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
byte custom_mode1 = 1;
byte target_system1 = sysid;
mode1.custom_mode = custom_mode1;
mode1.base_mode = base_mode1;
mode1.target_system = target_system1;
System.Threading.Thread.Sleep(100);
MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t();//定义req为 MAVLink.mavlink_command_long_t类，填充常命令的数据包
req.target_system = 1;
req.target_component = 1;
req.command = (ushort)MAVLink.MAV_CMD.COMPONENT_ARM_DISARM; //‘COMPONENT_ARM_DISARM’代表你要发送什么类的信息（此处为解锁信息），可自己选择（看类里有多少种）
//req.command = (ushort)MAVLink.MAV_CMD
req.param1 = 1; //此处发送解锁信息，只需用param1。其余param2—7注释掉
MAVLink.mavlink_set_mode_t mode4 = new MAVLink.mavlink_set_mode_t();
byte base_mode4 = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
byte custom_mode4 = 4;
byte target_system4 = sysid;
mode4.custom_mode = custom_mode4;
mode4.base_mode = base_mode4;
mode4.target_system = target_system4;
MAVLink.mavlink_command_long_t req2 = new MAVLink.mavlink_command_long_t();//定义req为 MAVLink.mavlink_command_long_t类，填充常命令的数据包
req2.target_system = 1;
req2.target_component = 1;
req2.command = (ushort)MAVLink.MAV_CMD.TAKEOFF;
req2.param1 = 0;
req2.param2 = 0;
req2.param3 = 0;
req2.param4 = 0;
req2.param5 = 0;
req2.param6 = 0;
req2.param7 = 2;
byte[] packetMode1 = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.SET_MODE, mode1);
byte[] packet = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, req);
byte[] packetMode4 = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.SET_MODE, mode4);
//把req数据包，按照mavlink协议打包成（定义了临时变量packet）
byte[] packet2 = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, req2);
serialPort1.Write(packetMode1, 0, packetMode1.Length);//发送数据包
System.Threading.Thread.Sleep(50);
try
{
var ack1 = readsomedata <MAVLink.mavlink_command_ack_t>(sysid, compid);//读取反馈信号
if (ack1.result == (byte)MAVLink.MAV_RESULT.ACCEPTED)
{
Console.WriteLine("切换到自稳模式");
}
if (ack1.result == (byte)MAVLink.MAV_RESULT.TEMPORARILY_REJECTED)
{
Console.WriteLine("---切换到自稳模式失败---");
}
}
catch
{
}
System.Threading.Thread.Sleep(20);
serialPort1.Write(packet, 0, packet.Length);
System.Threading.Thread.Sleep(50);
try
{
var ack2 = readsomedata <MAVLink.mavlink_command_ack_t>(sysid, compid);//读取反馈信号
if (ack2.result == (byte)MAVLink.MAV_RESULT.ACCEPTED)
{
Console.WriteLine("解锁完成");
}
if (ack2.result == (byte)MAVLink.MAV_RESULT.TEMPORARILY_REJECTED)
{
Console.WriteLine("---解锁失败---");
}
}
catch
{
}
System.Threading.Thread.Sleep(1500);
serialPort1.Write(packetMode4, 0, packetMode4.Length);
System.Threading.Thread.Sleep(50);
try
{
var ack3 = readsomedata <MAVLink.mavlink_command_ack_t>(sysid, compid);//读取反馈信号
if (ack3.result == (byte)MAVLink.MAV_RESULT.ACCEPTED)
{
Console.WriteLine("切换到引导模式");
}
if (ack3.result == (byte)MAVLink.MAV_RESULT.TEMPORARILY_REJECTED)
{
Console.WriteLine("---切换到引导模式失败---");
}
}
catch
{
}
System.Threading.Thread.Sleep(100);
serialPort1.Write(packet2, 0, packet2.Length);
System.Threading.Thread.Sleep(100);
try
{
var ack4 = readsomedata <MAVLink.mavlink_command_ack_t>(sysid, compid);//读取反馈信号
if (ack4.result == (byte)MAVLink.MAV_RESULT.ACCEPTED)
{
Console.WriteLine("-----起飞！！！-----");
fly.Text = "已起飞";
}
if (ack4.result == (byte)MAVLink.MAV_RESULT.TEMPORARILY_REJECTED)
{
Console.WriteLine("--起飞失败，请检查飞机状态--");
}
}
catch
{
}
}
public static bool translateMode(string modein, ref MAVLink.mavlink_set_nav_mode_t navmode, ref MAVLink.mavlink_set_mode_t mode)
{
//MAVLink.mavlink_set_nav_mode_t navmode = new MAVLink.mavlink_set_nav_mode_t();
navmode.target = MainV2.comPort.sysid;
navmode.nav_mode = 255;
//MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t();
mode.target = MainV2.comPort.sysid;
try
{
if (Common.getModes() == typeof(Common.apmmodes))
{
switch ((int)Enum.Parse(Common.getModes(), modein))
{
case (int)Common.apmmodes.MANUAL:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_MANUAL;
break;
case (int)Common.apmmodes.GUIDED:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_GUIDED;
break;
case (int)Common.apmmodes.STABILIZE:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_TEST1;
break;
// AUTO MODES
case (int)Common.apmmodes.AUTO:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_WAYPOINT;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
case (int)Common.apmmodes.RTL:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_RETURNING;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
case (int)Common.apmmodes.LOITER:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_LOITER;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
// FBW
case (int)Common.apmmodes.FLY_BY_WIRE_A:
navmode.nav_mode = (byte)1;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_TEST2;
break;
case (int)Common.apmmodes.FLY_BY_WIRE_B:
navmode.nav_mode = (byte)2;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_TEST2;
break;
default:
CustomMessageBox.Show("No Mode Changed " + (int)Enum.Parse(Common.getModes(), modein));
return(false);
}
}
else if (Common.getModes() == typeof(Common.ac2modes))
{
switch ((int)Enum.Parse(Common.getModes(), modein))
{
case (int)Common.ac2modes.GUIDED:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_GUIDED;
break;
case (int)Common.ac2modes.STABILIZE:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_MANUAL;
break;
// AUTO MODES
case (int)Common.ac2modes.AUTO:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_WAYPOINT;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
case (int)Common.ac2modes.RTL:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_RETURNING;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
case (int)Common.ac2modes.LOITER:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_LOITER;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
default:
CustomMessageBox.Show("No Mode Changed " + (int)Enum.Parse(Common.getModes(), modein));
return(false);
}
}
}
catch { System.Windows.Forms.CustomMessageBox.Show("Failed to find Mode"); return(false); }
return(true);
}
