//指令发送模块
void flight_control_commond_sender(ushort cmd4sender)
{
MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t();
req.target_system = sysid;
req.target_component = compid;
req.command = cmd4sender;
/*
* req.param1 = p1;
* req.param2 = p2;
* req.param3 = p3;
* req.param4 = p4;
* req.param5 = p5;
* req.param6 = p6;
* req.param7 = p7;
*/
byte[] packet = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, req);
serialPort1.Write(packet, 0, packet.Length);
/*回执包未准备好
* try
* {//200ms内，收信息。若收到成功动作的信息，结束收取。若未收到其他类型信息，输出而后继续收。若200ms后依然没收到，那么直接结束。
* var ack = readsomedata<MAVLink.mavlink_command_ack_t>(sysid, compid);
* //if (ack.result == (byte)MAVLink.MAV_RESULT.ACCEPTED)
* }
* catch
* {
* }
*/
}
private void button_OK_Click(object sender, EventArgs e)
{
MAVLink.mavlink_command_long_t dd = new MAVLink.mavlink_command_long_t();
try
{
dd.param1 = float.Parse(this.textBox_missionSeconds.Text);
dd.param2 = float.Parse(this.comboBox_missionType.Text);
dd.param3 = float.Parse(this.textBox_missonRadius.Text);
dd.param4 = float.Parse(this.textBox_missionyaw.Text);
dd.param5 = float.Parse(this.textBox_missionlat.Text);
dd.param6 = float.Parse(this.textBox_missionlng.Text);
dd.param7 = float.Parse(this.textBox_missionalt.Text);
if (GetData != null)
{
GetData(dd);
}
}
catch {
MessageBox.Show("数据出错...");
return;
}
this.Close();
}
public byte[] takeoff(float altitude)
{
float temp = (float)(((float)((1e+300) * (1e+300))) * 0.0F);
//tmp = (float)((tmp[0] << 24) && (tmp[1] << 16) && (tmp[2] << 8) && (tmp[3] << 0));
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.TAKEOFF; //‘COMPONENT_ARM_DISARM’代表你要发送什么类的信息（此处为解锁信息），可自己选择（看类里有多少种）
//req.command = (ushort)MAVLink.MAV_CMD
//req.param1 = armed ? 0 : 1;//此处发送解锁信息，只需用param1。其余param2—7注释掉
req.param1 = -1;
req.param2 = 0;
req.param3 = 0;
req.param4 = temp;
req.param5 = temp;
req.param6 = temp;
req.param7 = altitude;
byte[] packet = mavlink.GenerateMAVLinkPacket_PX4(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, req); //把req数据包，按照mavlink协议打包成（定义了临时变量packet）
// byte[] packet = mavlink.GenerateMAVLinkPacket_PX4(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, req);//把req数据包，按照mavlink协议打包成（定义了临时变量packet）
// mavlink.GenerateMAVLinkPacket_PX4(MAVLink.MAVLINK_MSG_ID.SET_POSITION_TARGET_LOCAL_NED, req);
//foreach (byte i in packet)
//{
// Console.Write("{0:X} ", i);
//}
return(packet);
}
public byte[] pause()
{
float NAN = (((float)((1e+300) * (1e+300))) * 0.0F);
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.DO_REPOSITION; //‘COMPONENT_ARM_DISARM’代表你要发送什么类的信息（此处为解锁信息），可自己选择（看类里有多少种）
//req.command = (ushort)MAVLink.MAV_CMD
//req.param1 = armed ? 0 : 1;//此处发送解锁信息，只需用param1。其余param2—7注释掉
req.param1 = -1.0f;
req.param2 = 1;
req.param3 = 0.0f;
req.param4 = NAN;
req.param5 = NAN;
req.param6 = NAN;
req.param7 = NAN;
byte[] packet = mavlink.GenerateMAVLinkPacket_PX4(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, req); //把req数据包，按照mavlink协议打包成（定义了临时变量packet）
// byte[] packet = mavlink.GenerateMAVLinkPacket_PX4(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, req);//把req数据包，按照mavlink协议打包成（定义了临时变量packet）
// mavlink.GenerateMAVLinkPacket_PX4(MAVLink.MAVLINK_MSG_ID.SET_POSITION_TARGET_LOCAL_NED, req);
//foreach (byte i in packet)
//{
// Console.Write("{0:X} ", i);
//}
return(packet);
}
public byte[] disarm()
{
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 = 0;
/*
* req.param2 = p2;//具体想要发送什么类型的数据,param如何设置，需要查看阿木社区中的MAV_CMD
* req.param3 = p3;//跟踪的可以参考里面的fellow
* req.param4 = p4;
* req.param5 = p5;
* req.param6 = p6;
* req.param7 = p7;
*/
byte[] packet = mavlink.GenerateMAVLinkPacket_PX4(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, req); //把req数据包，按照mavlink协议打包成（定义了临时变量packet）
//foreach (byte i in packet)
//{
// Console.Write("{0:X} ", i);
//}
return(packet);
}
public void SendToLand()
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t();
cmd.command = (ushort)MAVLink.MAV_CMD.RETURN_TO_LAUNCH;
generatePacket((byte)MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
}
public MAVLink.mavlink_command_long_t get_command_long()
{
MAVLink.mavlink_command_long_t temp = new MAVLink.mavlink_command_long_t();
//temp.command = actionid;
temp.param1 = p1;
temp.param2 = p2;
return(temp);
}
public void SendTakeOff()
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t();
cmd.command = (ushort)MAVLink.MAV_CMD.TAKEOFF;
generatePacket((byte)MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
}
public static void ArmDisarm(bool arm)
{
MAVLink.mavlink_command_long_t command = new MAVLink.mavlink_command_long_t();
command.target_system = 1;
command.target_component = 0;
command.command = (ushort)MAVLink.MAV_CMD.COMPONENT_ARM_DISARM;
command.param1 = arm ? 1 : 0;
byte[] commandBytes = mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, command);
SendPacket(commandBytes);
}
public void TakeOff()
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t
{
command = (ushort)MAVLink.MAV_CMD.TAKEOFF,
param7 = 1f
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
sock.SendTo(data, myproxy);
}
public void Arm()
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t
{
command = (ushort)MAVLink.MAV_CMD.COMPONENT_ARM_DISARM,
param1 = 1
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
sock.SendTo(data, myproxy);
}
public void Disarm()
{
MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t();
req.target_system = 1;
req.target_component = 1;
req.command = (ushort)MAVLink.MAV_CMD.COMPONENT_ARM_DISARM;
req.param1 = _armed ? 0 : 1;
_armed = !_armed;
byte[] packet = _mavlink_parse.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, req);
}
public void Disarm()
{
MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t();
req.target_system = 1;
req.target_component = 1;
req.command = (ushort)MAVLink.MAV_CMD.COMPONENT_ARM_DISARM;
req.param1 = Vehicle.IsArmed ? 0 : 1;
Vehicle.IsArmed = !Vehicle.IsArmed;
byte[] packet = _mavlinkparse.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, req);
_datastream.Write(packet, 0, packet.Length);
}
public void SendHome()
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t();
cmd.command = (ushort)MAVLink.MAV_CMD.DO_SET_HOME;
unsafe
{
*(int *)&cmd.param1 = 1;
}
generatePacket((byte)MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
}
public static void Takeoff(double altitudeInMeters)
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t();
cmd.target_system = sysID;
cmd.target_component = compID;
cmd.command = (ushort)MAVLink.MAV_CMD.TAKEOFF;
cmd.param7 = (float)altitudeInMeters;
byte[] commandBytes = mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
SendPacket(commandBytes);
}
public void TakeoffClick()
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t
{
target_system = (byte)gameWorld.TargetID,
command = (ushort)MAVLink.MAV_CMD.TAKEOFF,
param7 = 0.7f
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
sock.SendTo(data, ep);
}
public void ArmClick()
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t
{
target_system = (byte)gameWorld.TargetID,
command = (ushort)MAVLink.MAV_CMD.COMPONENT_ARM_DISARM,
param1 = 1
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
sock.SendTo(data, ep);
}
public void SendCalCompass2()
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t();
cmd.command = (ushort)40000; //cal command
unsafe {
*(int *)&cmd.param1 = 2; //cal compass1
}
generatePacket((byte)MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
}
private void SendHome()
{
var cl = new MAVLink.mavlink_command_long_t(0, 0, 0, 0, (float)(HomeLoc.Lat * 1e7),
(float)(HomeLoc.Lng * 1e7),
(float)(HomeLoc.Alt * 1e2),
(ushort)MAVLink.MAV_CMD.DO_SET_HOME, 1, 1, 1);
var home = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cl, 255, 190, sequence++);
ATStream.Write(home, 0, home.Length);
}
public void SendToCamera(int mode)
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t();
cmd.command = (ushort)MAVLink.MAV_CMD.DO_DIGICAM_CONTROL;
unsafe
{
*(int *)&cmd.param1 = 0; //REC ON/OFF
*(int *)&cmd.param5 = mode;
}
generatePacket((byte)MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
}
public void SendToMode(int mode)
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t();
cmd.command = (ushort)MAVLink.MAV_CMD.DO_SET_MODE;
unsafe
{
*(int *)&cmd.param1 = mode;
// *(int*)&cmd.param2 =
}
generatePacket((byte)MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
}
public void SendCamZoom(int mode)
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t();
cmd.command = (ushort)MAVLink.MAV_CMD.DO_DIGICAM_CONTROL;
unsafe
{
*(int *)&cmd.param1 = 1;
*(int *)&cmd.param5 = mode; //Zoom in/out (0/1)
}
generatePacket((byte)MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
}
public void DisarmClick()
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t
{
command = (ushort)MAVLink.MAV_CMD.COMPONENT_ARM_DISARM,
target_system = 0,
param1 = 0,
param2 = 21196
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
sock.SendTo(data, ep);
}
private void button9_Click(object sender, EventArgs e)
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t();
cmd.command = (ushort)MAVLink.MAV_CMD.TAKEOFF;
cmd.target_system = 1;
//cmd.target_component = 250;
cmd.param7 = 1.2f;
DroneData drone = drones["bebop2"];
byte[] pkt = mavlinkParse.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
mavSock.SendTo(pkt, drone.ep);
}
private void button8_Click(object sender, EventArgs e)
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t();
cmd.command = (ushort)MAVLink.MAV_CMD.COMPONENT_ARM_DISARM;
cmd.target_system = 1;
cmd.param1 = 0;
cmd.param2 = 21196;
byte[] pkt = mavlinkParse.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
DroneData drone = drones["bebop2"];
mavSock.SendTo(pkt, drone.ep);
}
public static void RebootAutopilot(bool bootloaderMode = false)
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t();
cmd.target_system = sysID;
cmd.target_component = compID;
cmd.command = (ushort)MAVLink.MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN;
cmd.param1 = (float)(bootloaderMode ? 3 : 1);
cmd.param2 = (float)1;
byte[] commandBytes = mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
SendPacket(commandBytes);
}
public void Arm()
{
if (selected && ((apm_mode == (int)MAVLink.COPTER_MODE.AUTO) || (apm_mode == (int)MAVLink.COPTER_MODE.GUIDED)))
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t
{
command = (ushort)MAVLink.MAV_CMD.COMPONENT_ARM_DISARM,
param1 = 1
};
byte[] data = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
sock.SendTo(data, myproxy);
}
}
public void SendLotiter(float yaw, PointLatLngAlt point, int type, float seconds)
{
MAVLink.mavlink_command_long_t dd = new MAVLink.mavlink_command_long_t();
dd.command = (ushort)MAVLink.MAV_CMD.LOITER_TIME;
dd.param1 = seconds;
dd.param2 = type;
dd.param4 = yaw;
dd.param5 = (float)point.Lat;
dd.param6 = (float)point.Lng;
dd.param7 = (float)point.Alt;
generatePacket((byte)MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, dd);
}
public void SendMissionStart(int height, PointLatLngAlt point, int type, float seconds)
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t();
cmd.command = (ushort)MAVLink.MAV_CMD.MISSION_START;
unsafe
{
*(int *)&cmd.param1 = height;
}
cmd.param2 = 0;
generatePacket((byte)MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
}
public void SendGimbal(float pitch)
{
MAVLink.mavlink_command_long_t cmd = new MAVLink.mavlink_command_long_t();
cmd.command = (ushort)MAVLink.MAV_CMD.DO_MOUNT_CONTROL;
unsafe
{
*(int *)&cmd.param1 = 1; //pitch
//cmd.param3 is useless here
}
cmd.param2 = pitch;
generatePacket((byte)MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
}
