group/Plane.Sdk3
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
f0a4484cfc
Plane.Sdk3/PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs

1534 lines
56 KiB
C#
Raw Normal View History

紧急返航功能，策略不对，临时保存
2024-01-17 22:37:53 +08:00
using Plane.Copters;
using Plane.Geography;
using Plane.Protocols;
using System;
using System.Collections.Generic;
using System.IO.Ports;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Media;
using static Plane.Copters.PlaneCopter;
using static Plane.Protocols.MAVLink;
namespace Plane.CommunicationManagement
{
public partial class CommModuleManager
{
public int packetcount = 0;
private static object lock_rtcm = new object();//互锁量
private List<byte[]> rtcm_packets = new List<byte[]>();
private DateTime waitRtcmTime = DateTime.Now;
private bool starttime = false;
private bool rtcm_run = false;
private SerialPort RecomPort;
private bool Recomisopen = false;
private IEnumerable<ICopter> _allcopters;
//是否使用专用传输模块
public bool UseTransModule = true;
private byte[] SetChannels(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null)
{
MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t();
rc.target_component = 1;
rc.target_system = 1;
rc.chan1_raw = ch1 ?? 1500;
rc.chan2_raw = ch2 ?? 1500;
rc.chan3_raw = ch3 ?? 1500;
rc.chan4_raw = ch4 ?? 1500;
rc.chan5_raw = 1500;
rc.chan6_raw = 1500;
rc.chan7_raw = 1500;
rc.chan8_raw = 1500;
return GeneratePacket(MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, rc);
}
private byte[] SetModeAsync(FlightMode mode)
{
return SetModeAsync(mode.ToAC2Mode());
}
private byte[] DoARMAsync(bool armit)
{
return DoCommandAsync(MAVLink.MAV_CMD.COMPONENT_ARM_DISARM, armit ? 1 : 0, 21196, 0, 0, 0, 0, 0);
}
private byte[] DoMotorTestAsync(int motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE thr_type, int throttle, int timeout)
{
return DoCommandAsync(MAVLink.MAV_CMD.DO_MOTOR_TEST, (float)motor, (float)(byte)thr_type, (float)throttle, (float)timeout, 0, 0, 0);
}
private byte[] DoCommandAsync(MAVLink.MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
{
MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t();
req.target_system = 1;
req.target_component = 1;
if (actionid == MAVLink.MAV_CMD.COMPONENT_ARM_DISARM)
{
req.target_component = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;
}
req.command = (ushort)actionid;
req.param1 = p1;
req.param2 = p2;
req.param3 = p3;
req.param4 = p4;
req.param5 = p5;
req.param6 = p6;
req.param7 = p7;
return GeneratePacket(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req); ;
}
private byte[] GeneratePacket<TMavlinkPacket>(byte messageType, TMavlinkPacket indata)
{
byte[] data;
data = MavlinkUtil.StructureToByteArray(indata);
byte[] packet = new byte[data.Length + 6 + 2];
packet[0] = MAVLink.MAVLINK_STX;
packet[1] = (byte)(data.Length);
packet[2] = (byte)packetcount;
packetcount++;
packet[3] = 255; // this is always 255 - MYGCS
packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
packet[5] = messageType;
int i = 6;
foreach (byte b in data)
{
packet[i] = b;
i++;
}
ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);
checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum);
byte ck_a = (byte)(checksum & 0xFF); ///< High byte
byte ck_b = (byte)(checksum >> 8); ///< Low byte
packet[i] = ck_a;
i += 1;
packet[i] = ck_b;
i += 1;
return packet;
}
private byte[] SetModeAsync(ac2modes modein)
{
try
{
MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t();
if (TranslateMode(modein, ref mode))
{
return SetModeAsync(mode);
}
}
catch { }
return null;
}
private byte[] SetModeAsync(MAVLink.mavlink_set_mode_t mode, MAVLink.MAV_MODE_FLAG base_mode = 0)
{
mode.base_mode |= (byte)base_mode;
return GeneratePacket(MAVLink.MAVLINK_MSG_ID_SET_MODE, mode);
}
private bool TranslateMode(ac2modes modein, ref MAVLink.mavlink_set_mode_t mode)
{
mode.target_system = 1;
switch (modein)
{
case ac2modes.STABILIZE:
case ac2modes.AUTO:
case ac2modes.RTL:
case ac2modes.LOITER:
case ac2modes.ACRO:
case ac2modes.ALT_HOLD:
case ac2modes.CIRCLE:
case ac2modes.POSITION:
case ac2modes.LAND:
case ac2modes.OF_LOITER:
case ac2modes.GUIDED:
mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
mode.custom_mode = (uint)modein;
break;
default:
return false;
}
return true;
}
private byte[] SetParam2Async(string paramname, float value)
{
// param type is set here, however it is always sent over the air as a float 100int = 100f.
var req = new MAVLink.mavlink_param_set_t { target_system = 1, target_component = 1, param_type = 4 };
byte[] temp = Encoding.GetEncoding("ASCII").GetBytes(paramname);
Array.Resize(ref temp, 16);
req.param_id = temp;
req.param_value = (value);
return GeneratePacket((byte)MAVLink.MAVLINK_MSG_ID_PARAM_SET, req);
}
private byte[] GetParam2Async(string paramname)
{
var paramId = Encoding.ASCII.GetBytes(paramname);
Array.Resize(ref paramId, 16);
var req = new MAVLink.mavlink_param_request_read_t
{
param_index = -1,
target_system = 1,
target_component = 1,
param_id = paramId
};
return GeneratePacket(MAVLink.MAVLINK_MSG_ID_PARAM_REQUEST_READ, req);
}
private byte[] DoLEDCommandAsync()
{
MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
led.target_system = 1;
led.target_component = 1;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;;
led.instance = 0;
led.pattern = 0;
led.custom_len = 4;
led.custom_bytes = new byte[24];
led.custom_bytes[0] = 255;
led.custom_bytes[1] = 255;
led.custom_bytes[2] = 255;
led.custom_bytes[3] = 3;
return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led);
}
private byte[] DoThrowoutCommandAsync()
{
MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
led.target_system = 1;
led.target_component = 1;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;;
led.instance = 0;
led.pattern = 0;
led.custom_len = 1; //长度是1个字节
led.custom_bytes = new byte[24];
led.custom_bytes[0] = (byte)10; //10代表抛物
return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led);
}
private byte[] DoTaskLEDCommandAsync(int ledmode, float ledInterval, int ledtimes, byte cR,byte cG,Byte cB)
{
//新版固件ledInterval间隔单位秒，传输为一个字节无法传输小数，所以*10改为100ms，可传输一位小数
byte LEDInterval = (byte)(ledInterval * 10);// (byte)(10 / ledrate);
MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
led.target_system = 1;
led.target_component = 1;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;;
led.instance = 0;
led.pattern = 0;
led.custom_len = 6; //测试LED灯光
led.custom_bytes = new byte[24];
led.custom_bytes[0] = cR;
led.custom_bytes[1] = cG;
led.custom_bytes[2] = cB;
led.custom_bytes[3] =(byte)ledmode;
led.custom_bytes[4] = LEDInterval;
led.custom_bytes[5] = (byte)ledtimes;
return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led);
}
private byte[] BatchCopterIdData(IEnumerable<ICopter> copters)
{
byte[] BatchByte = null;
List<ICopter> copterList = copters.OrderBy(c => int.Parse(c.Id)).ToList();
for (int i = 0; i < copterList.Count; i++)
{
byte[] tempByte;
int id = int.Parse(copterList[i].Id);
int count = 1;
while (i + 1 < copterList.Count && int.Parse(copterList[i + 1].Id) == id + count)
{
count++;
i++;
}
if (count == 1)
{
short curId = (short)(0 << 12 ^ id);
tempByte = BitConverter.GetBytes(curId);
}
else
{
short beginId = (short)(2 << 12 ^ id);
tempByte = BitConverter.GetBytes(beginId);
short numCount = (short)(4 << 12 ^ count);
tempByte = tempByte.Concat(BitConverter.GetBytes(numCount)).ToArray();
}
if (BatchByte == null)
BatchByte = tempByte;
else
BatchByte = BatchByte.Concat(tempByte).ToArray();
}
return BatchByte;
}
private void GetCopterIds(IEnumerable<ICopter> copters, out short copterId, out byte[] batchPacket)
{
copterId = 0;
batchPacket = null;
if (copters != null)
{
if (copters.Count() == 1)
copterId = short.Parse(copters.FirstOrDefault().Id);
else
{
batchPacket = BatchCopterIdData(copters);
}
}
}
private async Task RtcmLoop()
{
while (rtcm_run)
{
byte[] sendDate = new byte[0];
lock (lock_rtcm)
{
if (rtcm_packets.Count >= 3 ||
(starttime && waitRtcmTime.AddMilliseconds(200) <= DateTime.Now))
{
if (rtcm_packets.Count > 0)
{
Windows.Messages.Message.Show($"rtcm_packets.Count = {rtcm_packets.Count}");
for (int i = 0; i < rtcm_packets.Count; i++)
{
sendDate = sendDate.Concat(rtcm_packets[i]).ToArray();
}
rtcm_packets.Clear();
starttime = false;
}
}
}
if (sendDate != null && sendDate.Length > 0)
{
Windows.Messages.Message.Show($"{DateTime.Now.ToString("HH:mm:ss:fff")} 通信数据长度 = {sendDate.Length}");
await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, sendDate).ConfigureAwait(false);
}
await Task.Delay(10);
}
}
/////////////////////////////////////////////////////////////公开输出函数
/// <summary>
/// 设置通道
/// </summary>
/// <param name="id"></param>
/// <param name="ch1"></param>
/// <param name="ch2"></param>
/// <param name="ch3"></param>
/// <param name="ch4"></param>
/// <returns></returns>
public async Task SetChannelsAsync(IEnumerable<ICopter> copters = null, ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null)
{
if (UseTransModule)
await SetChannelsAsync_CMod(copters, ch1, ch2, ch3, ch4);
else await SetChannelsAsync_Single(copters, ch1, ch2, ch3, ch4);
}
public async Task SetChannelsAsync_Single(IEnumerable<ICopter> copters = null, ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null)
{
await Task.WhenAll(copters.Select(async copter =>
await copter.SetChannelsAsync(ch2:ch2))).ConfigureAwait(false);
}
public async Task SetChannelsAsync_CMod(IEnumerable<ICopter> copters = null, ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null)
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
byte[] packet = SetChannels(ch1, ch2, ch3, ch4);
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
}
/// <summary>
/// 开始写任务
/// </summary>
/// <param name="id">飞机ID</param>
/// <param name="missions">任务列表</param>
/// <returns></returns>
///
public async Task<bool> WriteMissionListAsync(ICopter copter, List<IMission> missions)
{
if (UseTransModule)
return await WriteMissionListAsync_CMod(short.Parse(copter.Id), missions);
else return await WriteMissionListAsync_Single(copter, missions);
}
public async Task<bool> WriteMissionListAsync_Single(ICopter copter, List<IMission> missions)
{
return await copter.WriteMissionListAsync(missions);
}
public async Task<bool> WriteMissionListAsync_CMod(short id, List<IMission> missions)
{
missions.Insert(0, PLCopter.PRE_TAKE_OFF_MISSION);
List<MAVLink.mavlink_mission_item_t> mission_list = new List<MAVLink.mavlink_mission_item_t>();
foreach (IMission mission in missions)
{
var frame = mission.Command == FlightCommand.Waypoint && mission.Sequence == 0 ? MAVLink.MAV_FRAME.GLOBAL : MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;
var req = new MAVLink.mavlink_mission_item_t();
req.target_system = 1;
req.target_component = 1;
req.command = (byte)mission.Command;
req.current = 0;
req.autocontinue = 1;
req.frame = (byte)frame;
if (mission.Command == FlightCommand.LEDControl)
{
req.x = mission.R;
req.y = mission.G;
req.z = mission.B;
}
else
{
req.x = (float)mission.Latitude;
req.y = (float)mission.Longitude;
req.z = mission.Altitude;
}
req.param1 = mission.Param1;
req.param2 = mission.Param2;
req.param3 = mission.Param3;
req.param4 = mission.Param4;
req.seq = mission.Sequence;
mission_list.Add(req);
}
bool result = await MissionPacket(id, (byte)MavComm.COMM_DOWNLOAD_MODE, mission_list.ToArray());
lock(MissinLocker)
{
if (missionWriteState.ContainsKey(id))
{
missionWriteState[id].StateReturn = MissionErrorId;
missionWriteState[id].ErrorCode = ErrorCode;
missionWriteState[id].SendAchieved = result;
missionWriteState[id].WriteSucceed = false;
}
else
{
CommWriteMissinState state = new CommWriteMissinState(result);
state.StateReturn = MissionErrorId;
state.ErrorCode = ErrorCode;
missionWriteState.Add(id, state);
}
return result;
}
}
/// <summary>
/// 开始执行任务
/// </summary>
/// <param name="hour_utc">开始执行的小时</param>
/// <param name="minute_utc">开始执行的分钟</param>
/// <param name="second_utc">开始执行的秒</param>
/// <param name="Missionlng">任务原点经度</param>
/// <param name="Missionlat">任务原点纬度</param>
/// <returns></returns>
public async Task DoMissionStartAsync(IEnumerable<ICopter> copters,int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
{
if (UseTransModule)
await DoMissionStartAsync_CMod(copters,hour_utc, minute_utc, second_utc, Missionlng, Missionlat);
else await DoMissionStartAsync_Single(copters, hour_utc, minute_utc, second_utc, Missionlng, Missionlat);
}
public async Task DoMissionStartAsync_Single(IEnumerable<ICopter> copters,int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
{
foreach (var vcopter in copters)
{
await vcopter.MissionStartAsync(hour_utc,
minute_utc,
second_utc,
Missionlng,
Missionlat
);
}
}
public async Task DoMissionStartAsync_CMod(IEnumerable<ICopter> copters,int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
{
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, (float)Missionlng, (float)Missionlat, 0, -1);
short copterId = 0;
byte[] batchPacket = null;
//部分开始任务
if (copters!=null)
GetCopterIds(copters, out copterId, out batchPacket);
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
}
/// <summary>
/// 暂停任务
/// </summary>
public async Task DoMissionPauseAsync()
{
if (UseTransModule)
{
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, 0, 0, 0, 0, 0, 1, 0);
await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
}else
Windows.Messages.Message.Show($"未开发完成！！");
}
/// <summary>
/// 恢复任务
/// </summary>
public async Task DoMissionResumeAsync(int hour_utc, int minute_utc, int second_utc)
{
if (UseTransModule)
{
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, 0, 0, 2, 0);
await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
}else
Windows.Messages.Message.Show($"未开发完成！！");
}
/// <summary>
/// 解锁
/// </summary>
/// <param name="copters">要解锁的飞机</param>
/// <returns></returns>
public async Task UnlockAsync(IEnumerable<ICopter> copters = null)
{
if (UseTransModule)
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
byte[] packet1 = SetChannels(1500, 1500, 1100, 1500);
byte[] packet2 = SetModeAsync(FlightMode.ALT_HOLD); //ALT_HOLD LOITER
byte[] packet3 = DoARMAsync(true);
byte[] data = packet1.Concat(packet2).Concat(packet3).ToArray();
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false);
}
else
{
IEnumerable<ICopter> vcopters = copters;
if (vcopters == null) vcopters = _allcopters;
if (vcopters != null)
{
foreach (var vcopter in vcopters)
await vcopter.UnlockAsync();
}
}
}
/// <summary>
/// 抛物
/// </summary>
/// <param name="copters">要解锁的飞机</param>
/// <returns></returns>
public void ThrowoutAsync(IEnumerable<ICopter> copters = null)
{
if (UseTransModule)
{
Task.Run(async () =>
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
byte[] packet = DoThrowoutCommandAsync();
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
});
}
else
{
/*
Task.Run(async () =>
{
foreach (var vcopter in copters)
await vcopter.LEDAsync();
});
*/
}
}
/// <summary>
/// LED闪烁白灯
/// </summary>
/// <param name="copters">要闪烁的飞机</param>
/// <returns></returns>
public void LED_FlickerAsync(IEnumerable<ICopter> copters = null)
{
if (UseTransModule)
{
Task.Run(async () =>
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
byte[] packet = DoLEDCommandAsync();
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
});
}
else
{
Task.Run(async () =>
{
foreach (var vcopter in copters)
await vcopter.LEDAsync();
});
}
}
/*
private int ledmode = 0; //灯光模式 0常亮 1闪烁 2随机闪烁(RGB无意义)
private float ledInterval = 0; //间隔 单位秒 最小0.1
private int ledtimes = 0; //次数 (预留、暂取消其意义)
private string ledRGB = "FFFFFF"; //灯光颜色
*/
public void LED_TaskAsync( int ledmode, float ledInterval, int ledtimes, string ledRGB, ICopter copter,bool oldver=true )
{
if (UseTransModule)
{
Task.Run(async () =>
{
Color rightColor = (Color)ColorConverter.ConvertFromString("#" + ledRGB);
byte[] packet = DoTaskLEDCommandAsync(ledmode, ledInterval, ledtimes, (byte)rightColor.R, (byte)rightColor.G, (byte)rightColor.B);
await WriteCommPacketAsync(short.Parse(copter.Id), MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
});
}
}
/// <summary>
/// 设置参数
/// </summary>
/// <param name="paramname"></param>
/// <param name="value"></param>
/// <param name="copters"></param>
/// <returns></returns>
public async Task<int> SetParamAsync(string paramname, float value, IEnumerable<ICopter> copters = null)
{
if (UseTransModule)
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
byte[] packet = SetParam2Async(paramname, value);
int packetNum = packet[2];
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
return packetNum;
}
else
{
IEnumerable<ICopter> vcopters = copters;
if (vcopters == null) vcopters = _allcopters;
if (vcopters != null)
{
foreach (var vcopter in vcopters)
await vcopter.SetParamAsync(paramname, value);
}
return 0;
}
}
/// <summary>
/// 广播设置多个参数
/// </summary>
/// <param name="param"></param>
/// <returns></returns>
public async Task<int> SetMultipleParamAsync(params string[] param)
{
if (param.Length % 2 == 1)
return 0;
if (UseTransModule)
{
byte[] packet = null;
int packetNum = 0;
for (int i = 0; i < param.Length; i += 2)
{
string paramname = param[i];
float value = float.Parse(param[i + 1]);
byte[] data = SetParam2Async(paramname, value);
packetNum = data[2];
packet = packet == null ? data : packet.Concat(data).ToArray();
}
await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
return packetNum;
}
else
{
byte[] packet = null;
int packetNum = 0;
for (int i = 0; i < param.Length; i += 2)
{
string paramname = param[i];
float value = float.Parse(param[i + 1]);
// foreach (var vcopter in Copters )
// await vcopter.SetParamAsync(paramname, value);
}
return 0;
}
}
public void SetAllCoptersForWifi(IEnumerable<ICopter> copters )
{
_allcopters = copters;
}
/// <summary>
/// 读取参数
/// </summary>
/// <param name="paramname"></param>
/// <param name="copters"></param>
/// <returns></returns>
///
public async Task ReadParamAsnyc(string paramname, IEnumerable<ICopter> copters = null)
{
if (UseTransModule)
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
byte[] packet = GetParam2Async(paramname);
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
}
else
{
Windows.Messages.Message.Show($"未开发完成！！");
}
}
/// <summary>
/// 起飞测试
/// </summary>
/// <param name="alt">起飞高度</param>
/// <param name="copters"></param>
/// <returns></returns>
public async Task TakeOffAsync(float alt, IEnumerable<ICopter> copters = null)
{
if (UseTransModule)
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
byte[] packet1 = SetModeAsync(FlightMode.GUIDED);
byte[] packet2 = SetChannels(1500, 1500, 1500, 1500);
byte[] packet3 = DoCommandAsync(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0, 0, alt);
byte[] data = packet1.Concat(packet2).Concat(packet3).ToArray();
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false);
}
else
{
foreach (var vcopter in copters)
await vcopter.TakeOffAsync();
}
}
/// <summary>
/// 返航
/// </summary>
/// <param name="copters"></param>
/// <returns></returns>
public async Task ReturnToLaunchAsync(IEnumerable<ICopter> copters = null)
{
if (UseTransModule)
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
byte[] packet = SetModeAsync(FlightMode.RTL);
await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
}
else
{
foreach (var vcopter in copters)
await vcopter.ReturnToLaunchAsync ();
}
}
/// <summary>
/// 获取飞机版本
/// </summary>
/// <param name="copters"></param>
/// <returns></returns>
public async Task GetVersionsAsync(IEnumerable<ICopter> copters = null)
{
if (UseTransModule)
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
MAVLink.mavlink_sversion_request req = new MAVLink.mavlink_sversion_request();
req.version = 0;
byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, req);
await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
}
else
{
Windows.Messages.Message.Show($"未开发完成！！");
}
}
public async Task DoMissionEmergencyRetAsync(PLLocation takeoffcentloc, PLLocation taskcentloc, float mindistance,int rettime,bool descending)
{
Int32 param7 = rettime << 16;
//低16位中的第一位表示是否直接降低高度
if (descending)
param7 = param7 | 1;
if (UseTransModule)
{
支持紧急返航 支持返回飞机错误
2024-03-26 21:58:48 +08:00
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.RETURN_TO_LAUNCH,
(float)takeoffcentloc.Latitude * 10000000,
(float)takeoffcentloc.Longitude * 10000000,//起飞高度不用传=0
mindistance*100, 0, 0,0,0);
紧急返航功能，策略不对，临时保存
2024-01-17 22:37:53 +08:00
short copterId = 0;
byte[] batchPacket = null;
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
}
else
{
Windows.Messages.Message.Show($"未开发完成！！");
}
}
public async Task GetCommsumAsync(IEnumerable<ICopter> copters = null)
{
if (UseTransModule)
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
MAVLink.mavlink_sversion_request req = new MAVLink.mavlink_sversion_request();
req.version = 1;
byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, req);
await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
}
else
{
Windows.Messages.Message.Show($"未开发完成！！");
}
}
/// <summary>
/// 降落
/// </summary>
/// <returns></returns>
public async Task LandAsync(IEnumerable<ICopter> copters = null)
{
if (UseTransModule)
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
byte[] packet = SetModeAsync(FlightMode.LAND);
await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
}
else
{
IEnumerable<ICopter> vcopters = copters;
if (vcopters == null) vcopters = _allcopters;
if (vcopters != null)
{
foreach (var vcopter in vcopters)
await vcopter.LandAsync();
}
}
}
/// <summary>
/// 加锁
/// </summary>
/// <param name="armit"></param>
/// <returns></returns>
public async Task LockAsync(IEnumerable<ICopter> copters = null)
{
if (UseTransModule)
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
byte[] packet1 = SetChannels(1500, 1500, 1100, 1500);
byte[] packet2 = DoARMAsync(false);
byte[] data = packet1.Concat(packet2).ToArray();
await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false);
}
else
{
IEnumerable<ICopter> vcopters = copters;
if (vcopters == null) vcopters = _allcopters;
if (vcopters != null)
{
foreach (var vcopter in vcopters)
await vcopter.LockAsync();
}
}
}
/// <summary>
/// 测试电机
/// </summary>
/// <param name="motor"></param>
/// <returns></returns>
public async Task MotorTestAsync(IEnumerable<ICopter> copters = null)
{
if (UseTransModule)
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
//1-4代表1-4电机
byte[] data1 = DoMotorTestAsync(1, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5);
byte[] data2 = DoMotorTestAsync(2, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5);
byte[] data3 = DoMotorTestAsync(3, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5);
byte[] data4 = DoMotorTestAsync(4, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5);
byte[] data = data1.Concat(data2).Concat(data3).Concat(data4).ToArray();
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket);
}
else
{
if (copters != null)
foreach (var vcopter in copters)
{
await vcopter.MotorTestAsync(1);
await vcopter.MotorTestAsync(2);
await vcopter.MotorTestAsync(3);
await vcopter.MotorTestAsync(4);
}
}
}
/// <summary>
/// 切换悬停模式
/// </summary>
/// <returns></returns>
public async Task HoverAsync(IEnumerable<ICopter> copters = null)
{
if (UseTransModule)
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
byte[] packet1 = SetChannels(1500, 1500, 1500, 1500);
byte[] packet2 = SetModeAsync(FlightMode.LOITER);
byte[] data = packet1.Concat(packet2).ToArray();
await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false);
}
else
{
if (copters != null)
foreach (var vcopter in copters)
await vcopter.HoverAsync();
}
}
/// <summary>
/// 切换手动模式
/// </summary>
/// <param name="copters"></param>
/// <returns></returns>
public async Task FloatAsync(IEnumerable<ICopter> copters = null)
{
if (UseTransModule)
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
byte[] packet = SetModeAsync(FlightMode.ALT_HOLD);
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
}
else
{
if (copters!=null)
foreach (var vcopter in copters)
await vcopter.FloatAsync();
}
}
/// <summary>
/// 校准加速计
/// </summary>
/// <param name="copterId"></param>
/// <returns></returns>
public async Task DoStartPreflightCompassAsync(IEnumerable<ICopter> copters = null)
{
if (UseTransModule)
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION, 0, 0, 0, 0, 1, 0, 0);
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
}
else
{
if (copters != null)
foreach (var vcopter in copters)
await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION, 0, 0, 0, 0, 1, 0, 0);
}
}
/// <summary>
/// 校准加速计下一步
/// </summary>
/// <param name="copterId"></param>
/// <returns></returns>
public async Task DoNextPreflightCompassAsync(IEnumerable<ICopter> copters = null)
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
MAVLink.mavlink_command_ack_t req = new MAVLink.mavlink_command_ack_t();
req.command = 1;
req.result = 1;
if (UseTransModule)
{
byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_COMMAND_ACK, req);
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
}
else
{
if (copters != null)
foreach (var vcopter in copters)
{
await vcopter.DoCommandAckAsync(1, 1);
}
}
}
支持紧急返航 支持返回飞机错误
2024-03-26 21:58:48 +08:00
/// <summary>
/// 重启飞控
/// </summary>
/// <param name="copterId"></param>
/// <returns></returns>
public async Task DoRestartFCAsync(IEnumerable<ICopter> copters = null)
{
if (UseTransModule)
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
//只校陀螺仪，详细说明见;https://mavlink.io/en/messages/common.html#MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, 1, 1,1, 0, 0, 0, 0);
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
}
else
{
if (copters != null)
foreach (var vcopter in copters)
await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, 1, 1, 1, 0, 0, 0, 0);
}
}
/// <summary>
/// 校准陀螺仪
/// </summary>
/// <param name="copterId"></param>
/// <returns></returns>
public async Task DoCalibrationPreflightAsync(IEnumerable<ICopter> copters = null)
{
if (UseTransModule)
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
//只校陀螺仪，详细说明见;https://mavlink.io/en/messages/common.html#MAV_CMD_PREFLIGHT_CALIBRATION
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION, 1, 0, 0, 0, 0, 0, 0);
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
}
else
{
if (copters != null)
foreach (var vcopter in copters)
await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION, 1, 0, 0, 0, 0, 0, 0);
}
}
紧急返航功能，策略不对，临时保存
2024-01-17 22:37:53 +08:00
/// <summary>
/// 校准指南针
/// </summary>
/// <param name="copterId"></param>
/// <returns></returns>
public async Task DoCalibrationCompassAsync(IEnumerable<ICopter> copters = null)
{
if (UseTransModule)
{
short copterId = 0;
byte[] batchPacket = null;
GetCopterIds(copters, out copterId, out batchPacket);
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.DO_START_MAG_CAL, 0, 1, 1, 0, 0, 0, 0);
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
}
else
{
if (copters != null)
foreach (var vcopter in copters)
await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.DO_START_MAG_CAL, 0, 1, 1, 0, 0, 0, 0);
}
}
/// <summary>
/// 放弃校准指南针
/// </summary>
/// <param name="copterId"></param>
/// <returns></returns>
public async Task DoCancelCalibrationCompassAsync(short copterId)
{
if (UseTransModule)
{
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.DO_CANCEL_MAG_CAL, 0, 0, 1, 0, 0, 0, 0);
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
}
else
{
Windows.Messages.Message.Show($"未开发完成！！");
}
}
/// <summary>
/// 确认校准磁罗盘
/// </summary>
/// <param name="copterId"></param>
/// <returns></returns>
public async Task DoAcceptCalibrationCompassAsync(short copterId)
{
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.DO_ACCEPT_MAG_CAL, 0, 0, 1, 0, 0, 0, 0);
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
}
byte rtcm_tmpser = 0;
public void BroadcastGpsDataAsync(byte[] data, ushort length)
{
//广播发送RTCM数据采用专用数据可以一次发180个字节
if (!Recomisopen)
return;
MAVLink.mavlink_gps_rtcm_data_t gps = new mavlink_gps_rtcm_data_t();
var msglen = 180;
if (length > msglen * 4)
{
Windows.Messages.Message.Show($"RTCM数据太长:" + length);
return;
}
// number of packets we need, including a termination packet if needed
var nopackets = (length % msglen) == 0 ? length / msglen + 1 : (length / msglen) + 1;
if (nopackets >= 4)
nopackets = 4;
// flags = isfrag(1)/frag(2)/seq(5)
for (int a = 0; a < nopackets; a++)
{
// check if its a fragment
if (nopackets > 1)
gps.flags = 1;
else
gps.flags = 0;
// add fragment number
gps.flags += (byte)((a & 0x3) << 1);
// add seq number
gps.flags += (byte)((inject_seq_no & 0x1f) << 3);
// create the empty buffer
gps.data = new byte[msglen];
// calc how much data we are copying
int copy = Math.Min(length - a * msglen, msglen);
// copy the data
Array.Copy(data, a * msglen, gps.data, 0, copy);
// set the length
gps.len = (byte)copy;
byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_RTCM_DATA, gps, gps.len);
//Console.WriteLine(string.Format("{0:T} rtcm send :{1:D}", DateTime.Now, packet.Length));
try
{
RecomPort.Write(packet, 0, packet.Length);
}
catch (Exception ex)
{
Windows.Messages.Message.Show("转发端口发送失败" + ex.Message);
}
}
inject_seq_no++;
}
支持紧急返航 支持返回飞机错误
2024-03-26 21:58:48 +08:00
// get sum crc 计算数据校验和
public byte checkrtrcmsum(byte[] data, ushort length)
{
byte rtcm_sumcrc = 0;
for ( int i=0; i< length; i++)
{
rtcm_sumcrc += data[i];
}
return rtcm_sumcrc;
}
紧急返航功能，策略不对，临时保存
2024-01-17 22:37:53 +08:00
/// <summary>
///广播Rtk //-------------使用中的RTK广播函数------------
/// </summary>
/// <param name="data"></param>
/// <param name="length"></param>
/// <returns></returns>
public async Task InjectGpsDataAsync(byte[] data, ushort length)
{
//由于通讯模块限制一次只能发110个字节
if (UseTransModule)
{
MAVLink.mavlink_gps_inject_data_t gps = new MAVLink.mavlink_gps_inject_data_t();
var msglen = 110;
int datalen = 0;
var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1;
for (int a = 0; a < len; a++)
{
datalen = Math.Min(length - a * msglen, msglen);
gps.data = new byte[msglen];
Array.Copy(data, a * msglen, gps.data, 0, datalen);
//gps.data[0] = rtcm_tmpser++;
gps.len = (byte)datalen;
gps.target_component = rtcm_tmpser++;
支持紧急返航 支持返回飞机错误
2024-03-26 21:58:48 +08:00
//实测一旦收到数据都是正确的，张伟已经做过crc检验了，为了兼容性不再做了
//如果需要做，飞控里面需要把注释的代码放开，并且不再兼容之前的固件
gps.target_system = 1;// checkrtrcmsum(gps.data, (ushort)datalen);
紧急返航功能，策略不对，临时保存
2024-01-17 22:37:53 +08:00
支持紧急返航 支持返回飞机错误
2024-03-26 21:58:48 +08:00
// Console.WriteLine(" ["+ gps.target_component + "]总长: "+ length +" 发送第 "+ (a+1) + " 个包,长度: " + (ushort)datalen );
紧急返航功能，策略不对，临时保存
2024-01-17 22:37:53 +08:00
byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len);
await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
支持紧急返航 支持返回飞机错误
2024-03-26 21:58:48 +08:00
await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
紧急返航功能，策略不对，临时保存
2024-01-17 22:37:53 +08:00
//重发一次，有序列号(target_component)飞机可以检测出来重复接收的
//需要新固件支持
支持紧急返航 支持返回飞机错误
2024-03-26 21:58:48 +08:00
// await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
// await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
紧急返航功能，策略不对，临时保存
2024-01-17 22:37:53 +08:00
支持紧急返航 支持返回飞机错误
2024-03-26 21:58:48 +08:00
紧急返航功能，策略不对，临时保存
2024-01-17 22:37:53 +08:00
//重发一次，有序列号(target_component)飞机可以检测出来重复接收的--需要新固件支持
支持紧急返航 支持返回飞机错误
2024-03-26 21:58:48 +08:00
//await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
// await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
紧急返航功能，策略不对，临时保存
2024-01-17 22:37:53 +08:00
}
}
else
{
if (_allcopters != null)
foreach (var vcopter in _allcopters)
await vcopter.InjectGpsDataAsync(data, length);
}
}
int inject_seq_no = 0;
/// <summary>
/// 插入RTK数据
/// </summary>
/// <param name="data"></param>
/// <param name="length"></param>
/// <returns></returns>
public async Task InjectGpsRTCMDataAsync(byte[] data, int length)
{
MAVLink.mavlink_gps_rtcm_data_t gps = new MAVLink.mavlink_gps_rtcm_data_t();
var msglen = 180; //飞机端是180，之前也是180和通讯盒可能也有关系
// if (length > msglen * 4)
// log.Error("Message too large " + length);
// number of packets we need, including a termination packet if needed
var nopackets = (length % msglen) == 0 ? length / msglen + 1 : (length / msglen) + 1;
// if (nopackets>1)
// Plane.Windows.Messages.Message.Show(" RTCM_Mut: " + length);
if (nopackets >= 4)
nopackets = 4;
// flags = isfrag(1)/frag(2)/seq(5)
for (int a = 0; a < nopackets; a++)
{
// check if its a fragment
if (nopackets > 1)
gps.flags = 1;
else
gps.flags = 0;
// add fragment number
gps.flags += (byte)((a & 0x3) << 1);
// add seq number
gps.flags += (byte)((inject_seq_no & 0x1f) << 3);
// create the empty buffer
gps.data = new byte[msglen];
// calc how much data we are copying
int copy = Math.Min(length - a * msglen, msglen);
// copy the data
Array.Copy(data, a * msglen, gps.data, 0, copy);
// set the length
gps.len = (byte)copy;
byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_GPS_RTCM_DATA, gps);
// packet[2]
//Plane.Windows.Messages.Message.Show("["+ packet[2]+"]RTCM:"+ gps.len);
// Console.WriteLine("RTCM:" + gps.len+ "[" + data[0]+","+ data[1] + "," + data[2] + "," + data[3] + "," + data[4]+"]");
//Windows.Messages.Message.Show($"{DateTime.Now.ToString("HH:mm:ss:fff")} 单次长度 = {packet.Length}");
//老版本在这发送，新版本在StartRtcmLoop里面一次性打包4个180字节发送了
await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
/* //长RTCP数据发送还没开发完
lock (lock_rtcm)
{
rtcm_packets.Add(packet);
if (rtcm_packets.Count == 1)
{
waitRtcmTime = DateTime.Now;
starttime = true;
}
}
*/
}
}
/// <summary>
/// 开始循环检测Rtcm数量
/// </summary>
/// <returns></returns>
public async Task StartRtcmLoop()
{
rtcm_run = true;
waitRtcmTime = DateTime.Now;
await Task.Factory.StartNew(RtcmLoop);
}
/// <summary>
/// 结束循环检测Rtcm数量
/// </summary>
/// <returns></returns>
public async Task CloseRtcmLoop()
{
rtcm_run = false;
await Task.Delay(1);
}
public void CloseResendRtcmserial()
{
if (Recomisopen)
{
RecomPort.Close();
RecomPort.Dispose();
}
Recomisopen = false;
}
public bool OpenResendRtcmserial(string reserialport)
{
RecomPort= new SerialPort(reserialport);
RecomPort.BaudRate = 57600;
RecomPort.Parity = Parity.None;
RecomPort.StopBits = StopBits.One;
RecomPort.DataBits = 8;
RecomPort.Handshake = Handshake.None;
try
{
RecomPort.Open();
Recomisopen = true;
}
catch (Exception ex)
{
// Alert.Show("转发端口打开失败" + ex.Message);
}
return Recomisopen;
}
/// <summary>
/// 产生RTK包
/// </summary>
/// <typeparam name="TMavlinkPacket"></typeparam>
/// <param name="messageType"></param>
/// <param name="indata"></param>
/// <param name="rtklength"></param>
/// <returns></returns>
public byte[] GenerateRTKPacketAsync<TMavlinkPacket>(byte messageType, TMavlinkPacket indata, int rtklength)
{
byte[] data;
int rtkdataleng = rtklength + 3;
data = MavlinkUtil.StructureToByteArray(indata);
byte[] packet = new byte[rtkdataleng + 6 + 2];
packet[0] = MAVLink.MAVLINK_STX;
packet[1] = (byte)(rtkdataleng);
packet[2] = (byte)packetcount;
packetcount++;
packet[3] = 255; // this is always 255 - MYGCS
packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
packet[5] = messageType;
int i = 6;
foreach (byte b in data)
{
if (i < rtkdataleng + 6)
packet[i] = b;
else break;
i++;
}
ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);
checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum);
byte ck_a = (byte)(checksum & 0xFF); ///< High byte
byte ck_b = (byte)(checksum >> 8); ///< Low byte
packet[i] = ck_a;
i += 1;
packet[i] = ck_b;
i += 1;
return packet;
}
}
}
Reference in New Issue Copy Permalink