group/Plane.Sdk3
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

支持紧急返航

Browse Source 
支持返回飞机错误
This commit is contained in:
pxzleo 2024-03-26 21:58:48 +08:00
parent 9c2238479f
commit f0a4484cfc
9 changed files with 4433 additions and 4239 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs
View File

@ -310,7 +310,7 @@ namespace Plane.CommunicationManagement
case (short)MavComm.MessageType.SCAN3:
AnalyzeComm3RetrunPacket(copterNum, dealData);
break;
case 4:
case 4: //版本13通讯模块收到飞机返回的数据
AnalyzeComm4RetrunPacket(copterNum, dealData);
break;
}

91
PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs
View File

@ -851,14 +851,10 @@ namespace Plane.CommunicationManagement
param7 = param7 | 1;
if (UseTransModule)
{
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START,
(float)takeoffcentloc.Latitude * 1000,
(float)takeoffcentloc.Longitude * 1000,//起飞高度不用传=0
(float)taskcentloc.Latitude * 1000,
(float)taskcentloc.Longitude * 1000,
taskcentloc.Altitude,
mindistance, param7
);
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.RETURN_TO_LAUNCH,
(float)takeoffcentloc.Latitude * 10000000,
(float)takeoffcentloc.Longitude * 10000000,//起飞高度不用传=0
mindistance*100, 0, 0,0,0);
short copterId = 0;
byte[] batchPacket = null;
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
@ -1104,6 +1100,55 @@ namespace Plane.CommunicationManagement
}
}
/// <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);
}
}
/// <summary>
/// 校准指南针
/// </summary>
@ -1227,6 +1272,16 @@ namespace Plane.CommunicationManagement
}
// 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;
}
/// <summary>
@ -1252,26 +1307,28 @@ namespace Plane.CommunicationManagement
//gps.data[0] = rtcm_tmpser++;
gps.len = (byte)datalen;
gps.target_component = rtcm_tmpser++;
gps.target_system = 1;
//实测一旦收到数据都是正确的，张伟已经做过crc检验了，为了兼容性不再做了
//如果需要做，飞控里面需要把注释的代码放开，并且不再兼容之前的固件
gps.target_system = 1;// checkrtrcmsum(gps.data, (ushort)datalen);
// Console.WriteLine(" ["+ gps.target_component + "]总长: "+ length +" 发送第 "+ (a+1) + " 个包,长度: " + (ushort)datalen );
// Console.WriteLine(" ["+ gps.target_component + "]总长: "+ length +" 发送第 "+ (a+1) + " 个包,长度: " + (ushort)datalen );
byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len);
await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
//重发一次，有序列号(target_component)飞机可以检测出来重复接收的
//需要新固件支持
// await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
// await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
// await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
// await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
/*
//重发一次，有序列号(target_component)飞机可以检测出来重复接收的--需要新固件支持
await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
*/
//await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
// await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
}

426
PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs
View File

@ -1,213 +1,213 @@
using Plane.Communication;
using Plane.Protocols;
using Plane.Windows.Messages;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace Plane.CommunicationManagement
{
public partial class CommModuleManager
{
private int MissionStartCopterId = 0;
private int MissionSendCopterId = 0;
private int MissionErrorId = -1;
private int ErrorCode = 0;
public event EventHandler<CommunicationReceiveCopterInfoEventArgs> CommunicationReceived;
public event EventHandler<CommunicationCopterDisconnectEventArgs> CommunicationCopterDisconnect;
public event EventHandler<CommunicationConnectEventArgs> CommunicationConnected;
private Dictionary<int, CommWriteMissinState> missionWriteState = new Dictionary<int, CommWriteMissinState>();
private static readonly object MissinLocker = new object();
public Dictionary<int, CommWriteMissinState> MissionWriteState
{
get {return missionWriteState; }
}
public void ClearMissionWriteState()
{
missionWriteState.Clear();
}
private void AnalyzeMissionStartPacket(short copterId, byte[] buffer)
{
short errorId = BitConverter.ToInt16(buffer, 0);
MissionStartCopterId = copterId;
MissionErrorId = errorId;
if(errorId != 0)
Message.Show($"{copterId}:返回 = {errorId}");
}
private void AnalyzeStringPacket(short copterId, byte[] buffer)
{
int count = Array.IndexOf(buffer, (byte)0);
string msg = System.Text.Encoding.Default.GetString(buffer, 0, count).Replace("tcpserver", "flicube");
Message.Show(msg);
}
private void AnalyzeComm2RetrunPacket(short copterId, byte[] buffer)
{
ushort ret = BitConverter.ToUInt16(buffer, 0);
switch (ret)
{
case MavComm.SEARCH_FINDCOPTER:
Message.Show(copterId.ToString() + "---飞机开始相应");
break;
case MavComm.SEARCH_COMPLETED:
Message.Show(copterId.ToString() + "---设置成功");
break;
case MavComm.SEARCH_OUTTIME:
Message.Show("超时无响应");
break;
case MavComm.MISSION_SEND_COMPLETED:
MissionSendCopterId = copterId;
break;
case MavComm.P2P_SEND_FAILED:
Message.Show("点对点通信发送失败");
break;
default:
if (ret > MavComm.ERROR_CODE_START && ret <= MavComm.ERROR_CODE_END)
{
ErrorCode = ret;
Message.Show($"{copterId}--错误码：{ret}");
}
break;
}
}
private void AnalyzeComm3RetrunPacket(short copterId, byte[] buffer)
{
byte type = buffer[0];
byte connectRet;
switch (type)
{
case 0x01:
//connectRet = buffer[1];
//if (connectRet == 0x0) //飞机连接上 不管飞控和通信模块的连接
CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
break;
case 0x02:
connectRet = buffer[1];
if (connectRet == 0x0) //飞机断开
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
break;
case 0x03:
SaveMissionWriteStat(copterId, buffer[1]);
break;
case 0x04:
Message.Show($"飞机{copterId}:功率设置成功，新回传功率：{buffer[1]}");
break;
case 0x0e:
if (copterId == 0)
Message.Show("----------全部更新完成----------");
else
Message.Show($"飞机{copterId}:更新进度{buffer[1]}%");
break;
default:
// Message.Show($"Comm3返回：{type}");
break;
}
}
private void SaveMissionWriteStat(short copterId, byte wirteMissRet)
{
Task.Run(async () =>
{
lock (MissinLocker)
{
// var copter = _copterManager.Copters.FirstOrDefault(c => c.Id == arrs[0]);
if (wirteMissRet == 0x20)
{
if (missionWriteState.ContainsKey(copterId))
missionWriteState[copterId].WriteSucceed = true;
Message.Show($"ID:{copterId}:航点写入成功");
}
if (wirteMissRet > 0x20 && wirteMissRet < 0x30)
{
if (missionWriteState.ContainsKey(copterId))
missionWriteState[copterId].WriteSucceed = false;
Message.Show($"ID:{copterId}:航点写入失败");
}
}
await Task.Delay(10).ConfigureAwait(false);
}
);
}
private void AnalyzeCopterInfosPacket(byte[] buffer)
{
ushort beginNum = BitConverter.ToUInt16(buffer, 0);
ushort endNum = BitConverter.ToUInt16(buffer, 2);
int count = endNum - beginNum + 1;
byte[] copter_packets = buffer.Skip(4).ToArray();
if (copter_packets.Length != count * 4)
{
return;
}
int offset = 0;
for (int i = beginNum; i <= endNum; i++)
{
byte[] onePacket = copter_packets.Skip(offset).Take(4).ToArray();
UInt16 state = BitConverter.ToUInt16(onePacket, 0);
short copterId = (short)i;
switch (state >> 13)
{
//0B000
case 0x0000 >> 13:
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
break;
//0B100
case 0x8000 >> 13:
break;
//0B110
case 0xC000 >> 13:
//0B111
case 0xE000 >> 13:
CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
break;
}
offset += 4;
}
}
private void AnalyzeComm4RetrunPacket(short copterId, byte[] buffer)
{
byte[] packet = buffer.Take(28).ToArray();
byte[] state_packet = buffer.Skip(28).ToArray();
UInt16 state = BitConverter.ToUInt16(state_packet,0);
byte version = state_packet[3];
switch (state >> 13)
{
//0B000
case 0x0000 >> 13:
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
break;
//0B100
case 0x8000 >> 13:
break;
//0B110
case 0xC000 >> 13:
//0B111
case 0xE000 >> 13:
CommunicationReceived?.Invoke(this, new CommunicationReceiveCopterInfoEventArgs(copterId, packet, version));
break;
}
}
}
}
using Plane.Communication;
using Plane.Protocols;
using Plane.Windows.Messages;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace Plane.CommunicationManagement
{
public partial class CommModuleManager
{
private int MissionStartCopterId = 0;
private int MissionSendCopterId = 0;
private int MissionErrorId = -1;
private int ErrorCode = 0;
public event EventHandler<CommunicationReceiveCopterInfoEventArgs> CommunicationReceived;
public event EventHandler<CommunicationCopterDisconnectEventArgs> CommunicationCopterDisconnect;
public event EventHandler<CommunicationConnectEventArgs> CommunicationConnected;
private Dictionary<int, CommWriteMissinState> missionWriteState = new Dictionary<int, CommWriteMissinState>();
private static readonly object MissinLocker = new object();
public Dictionary<int, CommWriteMissinState> MissionWriteState
{
get {return missionWriteState; }
}
public void ClearMissionWriteState()
{
missionWriteState.Clear();
}
private void AnalyzeMissionStartPacket(short copterId, byte[] buffer)
{
short errorId = BitConverter.ToInt16(buffer, 0);
MissionStartCopterId = copterId;
MissionErrorId = errorId;
if(errorId != 0)
Message.Show($"{copterId}:返回 = {errorId}");
}
private void AnalyzeStringPacket(short copterId, byte[] buffer)
{
int count = Array.IndexOf(buffer, (byte)0);
string msg = System.Text.Encoding.Default.GetString(buffer, 0, count).Replace("tcpserver", "flicube");
Message.Show(msg);
}
private void AnalyzeComm2RetrunPacket(short copterId, byte[] buffer)
{
ushort ret = BitConverter.ToUInt16(buffer, 0);
switch (ret)
{
case MavComm.SEARCH_FINDCOPTER:
Message.Show(copterId.ToString() + "---飞机开始相应");
break;
case MavComm.SEARCH_COMPLETED:
Message.Show(copterId.ToString() + "---设置成功");
break;
case MavComm.SEARCH_OUTTIME:
Message.Show("超时无响应");
break;
case MavComm.MISSION_SEND_COMPLETED:
MissionSendCopterId = copterId;
break;
case MavComm.P2P_SEND_FAILED:
Message.Show("点对点通信发送失败");
break;
default:
if (ret > MavComm.ERROR_CODE_START && ret <= MavComm.ERROR_CODE_END)
{
ErrorCode = ret;
Message.Show($"{copterId}--错误码：{ret}");
}
break;
}
}
private void AnalyzeComm3RetrunPacket(short copterId, byte[] buffer)
{
byte type = buffer[0];
byte connectRet;
switch (type)
{
case 0x01:
//connectRet = buffer[1];
//if (connectRet == 0x0) //飞机连接上 不管飞控和通信模块的连接
CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
break;
case 0x02:
connectRet = buffer[1];
if (connectRet == 0x0) //飞机断开
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
break;
case 0x03:
SaveMissionWriteStat(copterId, buffer[1]);
break;
case 0x04:
Message.Show($"飞机{copterId}:功率设置成功，新回传功率：{buffer[1]}");
break;
case 0x0e:
if (copterId == 0)
Message.Show("----------全部更新完成----------");
else
Message.Show($"飞机{copterId}:更新进度{buffer[1]}%");
break;
default:
// Message.Show($"Comm3返回：{type}");
break;
}
}
private void SaveMissionWriteStat(short copterId, byte wirteMissRet)
{
Task.Run(async () =>
{
lock (MissinLocker)
{
// var copter = _copterManager.Copters.FirstOrDefault(c => c.Id == arrs[0]);
if (wirteMissRet == 0x20)
{
if (missionWriteState.ContainsKey(copterId))
missionWriteState[copterId].WriteSucceed = true;
Message.Show($"ID:{copterId}:航点写入成功");
}
if (wirteMissRet > 0x20 && wirteMissRet < 0x30)
{
if (missionWriteState.ContainsKey(copterId))
missionWriteState[copterId].WriteSucceed = false;
Message.Show($"ID:{copterId}:航点写入失败");
}
}
await Task.Delay(10).ConfigureAwait(false);
}
);
}
private void AnalyzeCopterInfosPacket(byte[] buffer)
{
ushort beginNum = BitConverter.ToUInt16(buffer, 0);
ushort endNum = BitConverter.ToUInt16(buffer, 2);
int count = endNum - beginNum + 1;
byte[] copter_packets = buffer.Skip(4).ToArray();
if (copter_packets.Length != count * 4)
{
return;
}
int offset = 0;
for (int i = beginNum; i <= endNum; i++)
{
byte[] onePacket = copter_packets.Skip(offset).Take(4).ToArray();
UInt16 state = BitConverter.ToUInt16(onePacket, 0);
short copterId = (short)i;
switch (state >> 13)
{
//0B000
case 0x0000 >> 13:
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
break;
//0B100
case 0x8000 >> 13:
break;
//0B110
case 0xC000 >> 13:
//0B111
case 0xE000 >> 13:
CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
break;
}
offset += 4;
}
}
private void AnalyzeComm4RetrunPacket(short copterId, byte[] buffer)
{
byte[] packet = buffer.Take(28).ToArray();
byte[] state_packet = buffer.Skip(28).ToArray();
UInt16 state = BitConverter.ToUInt16(state_packet,0);
byte version = state_packet[3];
switch (state >> 13)
{
//0B000
case 0x0000 >> 13:
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
break;
//0B100
case 0x8000 >> 13:
break;
//0B110
case 0xC000 >> 13:
//0B111
case 0xE000 >> 13: //最后正在用的版本
CommunicationReceived?.Invoke(this, new CommunicationReceiveCopterInfoEventArgs(copterId, packet, version));
break;
}
}
}
}

2887
PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs
View File

File diff suppressed because it is too large Load Diff

626
PlaneGcsSdk_Shared/Copters/EHCopter.InternalCopterEventHandlers.cs
View File

@ -1,309 +1,317 @@
using System;
using static Plane.Protocols.MAVLink;
namespace Plane.Copters
{
public partial class PLCopter : CopterImplSharedPart
{
private bool _fetchingFirmwareVersion;
private void _internalCopter_ConnectionBroken(object sender, EventArgs e)
{
IsConnected = _internalCopter.IsConnected;
}
private void _internalCopter_GetLogDataEvent(string log)
{
//飞机消息日志，后面需要改为记录方式
StatusText =Name+":"+log;
var e = new MessageCreatedEventArgs { Message = StatusText };
RaiseTextReceived(e);
}
private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType)
{
switch (StreamType)
{
case MAVLINK_MSG_ID_GPS_RAW_INT: //需要
{
SatCount = _internalCopter.satcount;
GpsFixType = _internalCopter.gpsstatus.ToGpsFixType();
GpsHdop = _internalCopter.gpshdop;
Altitude = _internalCopter.gpsalt;
Retain = BitConverter.GetBytes(_internalCopter.retain);
Voltage = _internalCopter.battery_voltage;
CommModuleMode = (FlightMode)_internalCopter.commModuleMode;
CommModuleVersion = _internalCopter.commModuleVersion;
IsUnlocked = _internalCopter.isUnlocked;
//Yaw = _internalCopter.yaw;
Heading = _internalCopter.heading;
HeartbeatCount++;
if (SatCount > 8)
{
IsGpsAccurate = true;
RecordLat = _internalCopter.lat;
RecordLng = _internalCopter.lng;
}
Latitude = RecordLat;
Longitude = RecordLng;
UpdateFlightDataIfNeeded();
RaiseLocationChangedIfNeeded();
RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);
break;
}
case MAVLINK_MSG_ID_ATTITUDE: //无用
{
Roll = _internalCopter.roll;
Pitch = _internalCopter.pitch;
Yaw = _internalCopter.yaw;
TimebootMs = _internalCopter.timebootms;
RaiseAttitudeChanged();
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: //只用于显示,可以不要
{
Channel1 = _internalCopter.ch1in;
Channel2 = _internalCopter.ch2in;
Channel3 = _internalCopter.ch3in;
Channel4 = _internalCopter.ch4in;
Channel5 = _internalCopter.ch5in;
Channel6 = _internalCopter.ch6in;
Channel7 = _internalCopter.ch7in;
Channel8 = _internalCopter.ch8in;
RaiseDataStreamReceived(PDataStreamType.RC_CHANNELS_RAW);
break;
}
case MAVLINK_MSG_ID_VFR_HUD:
{
Heading = _internalCopter.heading; //有用
Altitude = _internalCopter.alt; //有用
AirSpeed = _internalCopter.airspeed; //没用
GroundSpeed = _internalCopter.groundspeed; //没用
RaiseAltitudeChangedIfNeeded();
break;
}
}
}
private async void _internalCopter_ReceiveHeartBearEvent(object sender, uint iError, ulong iCount)
{
IsUnlocked = _internalCopter.armed;
Mode = (FlightMode)_internalCopter.mode;
// 王海, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。
HasSwitchedToGpsMode = (byte)_internalCopter.apname != 0;
++HeartbeatCount;
IsCheckingConnection = false;
/*
if (FirmwareVersion == null && !_fetchingFirmwareVersion)
{
try
{
_fetchingFirmwareVersion = true;
FirmwareVersion = (int)await GetParamAsync("FIRMWARE_VERSION", 5000);
}
catch (TimeoutException)
{
// 吞掉。
}
finally
{
_fetchingFirmwareVersion = false;
}
}
*/
RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(iCount));
}
private void _internalCopter_ReceiveSensorEvent(object sender)
{
RaiseSensorDataReceived(EventArgs.Empty);
}
private void _internalCopter_ReceiveSysStatusEvent(object sender, byte StreamType, bool GPSBadHealth, bool GyroBadHealth, bool AccelBadHealth, bool CompassBadHealth, bool BarometerBadHealth, bool canTakeOff)
{
Voltage = _internalCopter.battery_voltage / 1000;
if (_internalCopter.battery_remaining == 255) // 剩余电量未知，需要计算。
{
CalcBatteryPer();
}
else
{
BatteryPer = _internalCopter.battery_remaining;
}
const int MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD = 12308;
const int MIN_VERSION_OF_GENERATION2 = 0x3000;
if (FirmwareVersion != null)
{
if (FirmwareVersion.Value >= MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD)
{
// 飞控提供了 canTakeOff。
IsGpsAccurate = canTakeOff;
}
else if (FirmwareVersion.Value >= MIN_VERSION_OF_GENERATION2)
{
// 2.0 飞行器，飞控未提供 canTakeOff。
IsGpsAccurate = SatCount >= 12;
}
else
{
// 1.0 飞行器，飞控不提供 canTakeOff。
IsGpsAccurate = SatCount >= 6;
}
}
else
IsGpsAccurate = SatCount >= 8;
FlightDistance2D = _internalCopter.FlightDistance2D;
RaiseSysStatusReceived(new SystemStatusReceivedEventArgs(GPSBadHealth, GyroBadHealth, AccelBadHealth, CompassBadHealth, BarometerBadHealth));
}
private void _internalCopter_PacketHandled(object sender, PlaneCopter.PacketReceivedEventArgs e)
{
switch (e.Packet[5])
{
case MAVLINK_MSG_ID_MISSION_COUNT:
MissionCount = _internalCopter.WpCount;
break;
default:
break;
}
}
private void _internalCopter_UnhandledPacketReceived(object sender, PlaneCopter.PacketReceivedEventArgs e)
{
switch (e.Packet[5])
{
case MAVLINK_MSG_ID_MISSION_ACK:
AnalyzeMissionAckPacket(e.Packet);
break;
case MAVLINK_MSG_ID_MISSION_COUNT:
AnalyzeMissionCountPacket(e.Packet);
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
AnalyzeMissionItemPacket(e.Packet);
break;
case MAVLINK_MSG_ID_MISSION_REQUEST:
AnalyzeMissionRequestPacket(e.Packet);
break;
case MAVLINK_MSG_ID_SET_PAIR:
AnalyzeSetPairPacket(e.Packet);
break;
default:
break;
}
}
#if PRIVATE
protected virtual void RegisterInternalCopterEventHandlers()
#else
private void RegisterInternalCopterEventHandlers()
#endif
{
_internalCopter.GetLogDataEvent += _internalCopter_GetLogDataEvent;
_internalCopter.ReceiveDataStreamEvent += _internalCopter_ReceiveDataStreamEvent;
_internalCopter.ReceiveHeartBearEvent += _internalCopter_ReceiveHeartBearEvent;
_internalCopter.ReceiveSensorEvent += _internalCopter_ReceiveSensorEvent;
_internalCopter.ReceiveSysStatusEvent += _internalCopter_ReceiveSysStatusEvent;
_internalCopter.ConnectionBroken += _internalCopter_ConnectionBroken;
_internalCopter.PacketHandled += _internalCopter_PacketHandled;
_internalCopter.UnhandledPacketReceived += _internalCopter_UnhandledPacketReceived;
_internalCopter.MessageCreated += _internalCopter_MessageCreated;
}
private void _internalCopter_MessageCreated(object sender, MessageCreatedEventArgs e)
{
var txte = new MessageCreatedEventArgs { Message = Name + ":" +e.Message };
RaiseTextReceived(txte);
}
#region 计算剩余电量
private int bPerTimes;
private int outBatteryPer;
private int[] tPerTimes = new int[20];
private int v_num;
/// <summary>
/// 计算电量并在条件满足时更新 <see cref="BatteryPer"/> 属性。
/// </summary>
private void CalcBatteryPer()
{
float volmax = 0f;
float volmin = 0f;
if (Voltage > 5 && Voltage < 9)
{
volmax = 8.2f;
volmin = 7f;
}
else if (Voltage >= 9
&& Voltage < 13.6)
{
volmax = 11.6f;
volmin = 10.2f;
}
else if (Voltage >= 13.6
&& Voltage < 17.2)
{
volmax = 16.3f;
volmin = 14.2f;
}
else if (Voltage >= 17.2
&& Voltage < 26.2)
{
volmax = 24.8f;
volmin = 21.2f;
}
int batteryPer = (int)Math.Round((Voltage - volmin) * 100 / (volmax - volmin));
if (batteryPer == -1 || volmax == 0 || volmin == 0)
return;
if (bPerTimes < 20)
{
tPerTimes[bPerTimes] = batteryPer;
bPerTimes += 1;
}
else
{
tPerTimes[v_num] = batteryPer;
v_num++;
if (v_num == 20)
v_num = 0;
}
for (int i = 0; i < bPerTimes; i++)
{
outBatteryPer += tPerTimes[i];
}
outBatteryPer = outBatteryPer / bPerTimes;
if (outBatteryPer < BatteryPer && bPerTimes > 18)
{
BatteryPer = (byte)outBatteryPer;
}
}
#endregion 计算剩余电量
}
}
using System;
using System.Collections.Generic;
using static Plane.Protocols.MAVLink;
namespace Plane.Copters
{
public partial class PLCopter : CopterImplSharedPart
{
private bool _fetchingFirmwareVersion;
private void _internalCopter_ConnectionBroken(object sender, EventArgs e)
{
IsConnected = _internalCopter.IsConnected;
}
private void _internalCopter_GetLogDataEvent(string log)
{
//飞机消息日志，后面需要改为记录方式
StatusText =Name+":"+log;
var e = new MessageCreatedEventArgs { Message = StatusText };
RaiseTextReceived(e);
}
private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType)
{
switch (StreamType)
{
case MAVLINK_MSG_ID_GPS_RAW_INT: //需要
{
SatCount = _internalCopter.satcount;
GpsFixType = _internalCopter.gpsstatus.ToGpsFixType();
GpsHdop = _internalCopter.gpshdop;
CopterErrorCode =_internalCopter.errorcode;
CopterPreCheckPass = _internalCopter.precheckok;
CopterErrorString = getcoptererrorstr(_internalCopter.errorcode);
Altitude = _internalCopter.gpsalt;
Retain = BitConverter.GetBytes(_internalCopter.retain);
Voltage = _internalCopter.battery_voltage;
CommModuleMode = (FlightMode)_internalCopter.commModuleMode;
CommModuleVersion = _internalCopter.commModuleVersion;
IsUnlocked = _internalCopter.isUnlocked;
//Yaw = _internalCopter.yaw;
Heading = _internalCopter.heading;
HeartbeatCount++;
if (SatCount > 8)
{
IsGpsAccurate = true;
RecordLat = _internalCopter.lat;
RecordLng = _internalCopter.lng;
}
Latitude = RecordLat;
Longitude = RecordLng;
UpdateFlightDataIfNeeded();
RaiseLocationChangedIfNeeded();
RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);
break;
}
case MAVLINK_MSG_ID_ATTITUDE: //无用
{
Roll = _internalCopter.roll;
Pitch = _internalCopter.pitch;
Yaw = _internalCopter.yaw;
TimebootMs = _internalCopter.timebootms;
RaiseAttitudeChanged();
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: //只用于显示,可以不要
{
Channel1 = _internalCopter.ch1in;
Channel2 = _internalCopter.ch2in;
Channel3 = _internalCopter.ch3in;
Channel4 = _internalCopter.ch4in;
Channel5 = _internalCopter.ch5in;
Channel6 = _internalCopter.ch6in;
Channel7 = _internalCopter.ch7in;
Channel8 = _internalCopter.ch8in;
RaiseDataStreamReceived(PDataStreamType.RC_CHANNELS_RAW);
break;
}
case MAVLINK_MSG_ID_VFR_HUD:
{
Heading = _internalCopter.heading; //有用
Altitude = _internalCopter.alt; //有用
AirSpeed = _internalCopter.airspeed; //没用
GroundSpeed = _internalCopter.groundspeed; //没用
RaiseAltitudeChangedIfNeeded();
break;
}
}
}
private async void _internalCopter_ReceiveHeartBearEvent(object sender, uint iError, ulong iCount)
{
IsUnlocked = _internalCopter.armed;
Mode = (FlightMode)_internalCopter.mode;
// 王海, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。
HasSwitchedToGpsMode = (byte)_internalCopter.apname != 0;
++HeartbeatCount;
IsCheckingConnection = false;
/*
if (FirmwareVersion == null && !_fetchingFirmwareVersion)
{
try
{
_fetchingFirmwareVersion = true;
FirmwareVersion = (int)await GetParamAsync("FIRMWARE_VERSION", 5000);
}
catch (TimeoutException)
{
// 吞掉。
}
finally
{
_fetchingFirmwareVersion = false;
}
}
*/
RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(iCount));
}
private void _internalCopter_ReceiveSensorEvent(object sender)
{
RaiseSensorDataReceived(EventArgs.Empty);
}
private void _internalCopter_ReceiveSysStatusEvent(object sender, byte StreamType, bool GPSBadHealth, bool GyroBadHealth, bool AccelBadHealth, bool CompassBadHealth, bool BarometerBadHealth, bool canTakeOff)
{
Voltage = _internalCopter.battery_voltage / 1000;
if (_internalCopter.battery_remaining == 255) // 剩余电量未知，需要计算。
{
CalcBatteryPer();
}
else
{
BatteryPer = _internalCopter.battery_remaining;
}
const int MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD = 12308;
const int MIN_VERSION_OF_GENERATION2 = 0x3000;
if (FirmwareVersion != null)
{
if (FirmwareVersion.Value >= MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD)
{
// 飞控提供了 canTakeOff。
IsGpsAccurate = canTakeOff;
}
else if (FirmwareVersion.Value >= MIN_VERSION_OF_GENERATION2)
{
// 2.0 飞行器，飞控未提供 canTakeOff。
IsGpsAccurate = SatCount >= 12;
}
else
{
// 1.0 飞行器，飞控不提供 canTakeOff。
IsGpsAccurate = SatCount >= 6;
}
}
else
IsGpsAccurate = SatCount >= 8;
FlightDistance2D = _internalCopter.FlightDistance2D;
RaiseSysStatusReceived(new SystemStatusReceivedEventArgs(GPSBadHealth, GyroBadHealth, AccelBadHealth, CompassBadHealth, BarometerBadHealth));
}
private void _internalCopter_PacketHandled(object sender, PlaneCopter.PacketReceivedEventArgs e)
{
switch (e.Packet[5])
{
case MAVLINK_MSG_ID_MISSION_COUNT:
MissionCount = _internalCopter.WpCount;
break;
default:
break;
}
}
private void _internalCopter_UnhandledPacketReceived(object sender, PlaneCopter.PacketReceivedEventArgs e)
{
switch (e.Packet[5])
{
case MAVLINK_MSG_ID_MISSION_ACK:
AnalyzeMissionAckPacket(e.Packet);
break;
case MAVLINK_MSG_ID_MISSION_COUNT:
AnalyzeMissionCountPacket(e.Packet);
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
AnalyzeMissionItemPacket(e.Packet);
break;
case MAVLINK_MSG_ID_MISSION_REQUEST:
AnalyzeMissionRequestPacket(e.Packet);
break;
case MAVLINK_MSG_ID_SET_PAIR:
AnalyzeSetPairPacket(e.Packet);
break;
default:
break;
}
}
#if PRIVATE
protected virtual void RegisterInternalCopterEventHandlers()
#else
private void RegisterInternalCopterEventHandlers()
#endif
{
_internalCopter.GetLogDataEvent += _internalCopter_GetLogDataEvent;
_internalCopter.ReceiveDataStreamEvent += _internalCopter_ReceiveDataStreamEvent;
_internalCopter.ReceiveHeartBearEvent += _internalCopter_ReceiveHeartBearEvent;
_internalCopter.ReceiveSensorEvent += _internalCopter_ReceiveSensorEvent;
_internalCopter.ReceiveSysStatusEvent += _internalCopter_ReceiveSysStatusEvent;
_internalCopter.ConnectionBroken += _internalCopter_ConnectionBroken;
_internalCopter.PacketHandled += _internalCopter_PacketHandled;
_internalCopter.UnhandledPacketReceived += _internalCopter_UnhandledPacketReceived;
_internalCopter.MessageCreated += _internalCopter_MessageCreated;
}
private void _internalCopter_MessageCreated(object sender, MessageCreatedEventArgs e)
{
var txte = new MessageCreatedEventArgs { Message = Name + ":" +e.Message };
RaiseTextReceived(txte);
}
#region 计算剩余电量
private int bPerTimes;
private int outBatteryPer;
private int[] tPerTimes = new int[20];
private int v_num;
/// <summary>
/// 计算电量并在条件满足时更新 <see cref="BatteryPer"/> 属性。
/// </summary>
private void CalcBatteryPer()
{
float volmax = 0f;
float volmin = 0f;
if (Voltage > 5 && Voltage < 9)
{
volmax = 8.2f;
volmin = 7f;
}
else if (Voltage >= 9
&& Voltage < 13.6)
{
volmax = 11.6f;
volmin = 10.2f;
}
else if (Voltage >= 13.6
&& Voltage < 17.2)
{
volmax = 16.3f;
volmin = 14.2f;
}
else if (Voltage >= 17.2
&& Voltage < 26.2)
{
volmax = 24.8f;
volmin = 21.2f;
}
int batteryPer = (int)Math.Round((Voltage - volmin) * 100 / (volmax - volmin));
if (batteryPer == -1 || volmax == 0 || volmin == 0)
return;
if (bPerTimes < 20)
{
tPerTimes[bPerTimes] = batteryPer;
bPerTimes += 1;
}
else
{
tPerTimes[v_num] = batteryPer;
v_num++;
if (v_num == 20)
v_num = 0;
}
for (int i = 0; i < bPerTimes; i++)
{
outBatteryPer += tPerTimes[i];
}
outBatteryPer = outBatteryPer / bPerTimes;
if (outBatteryPer < BatteryPer && bPerTimes > 18)
{
BatteryPer = (byte)outBatteryPer;
}
}
#endregion 计算剩余电量
}
}

114
PlaneGcsSdk_Shared/Copters/FakeCopter.cs
View File

@ -134,9 +134,10 @@ namespace Plane.Copters
private PLLocation _takeoffcentloc;
private PLLocation _taskcentloc;
private float _mindistance;
private int _rettime;
private float _rettime;
private bool _descending;
int Emergency_Retstatus = 0;
DateTime EmergencyRetTime;
//返航路径第一个航点
PLLocation Emergency_firstloc;
@ -290,45 +291,42 @@ namespace Plane.Copters
IsCheckingConnection = false;
return TaskUtils.CompletedTask;
}
private int minretalt=15; //最低返航高度
private int minretalt_first = 25; //第一次返航高度
//返航位置1
private PLLocation EmergencyRetPLLocation1;
private void BuildPath()
{
float taralt;
//返航高度参数
float retalt = 15;
//计算当前位置和起飞中心点的距离
float dis = (float)this.CalcDistance(_takeoffcentloc.Latitude, _takeoffcentloc.Longitude, 0);
_rettime = (dis - _mindistance) * 1.0f;
if (_descending)
{
if (Altitude <= retalt) taralt = Altitude;
else
{
float maxalt = Altitude - retalt;
//降低一个随机高度，随机高度介于0和maxalt之间，也就是目标高度不要低于retalt, 防止飞机在同一高度飞行
taralt = (float)(Altitude - (new Random().NextDouble() * maxalt));
}
Emergency_firstloc = new PLLocation(this.Latitude,this.Longitude, taralt);
}
else
{
//计算当前位置和起飞点的距离
float dis = (float)this.CalcDistance(TakeOffPoint.Latitude, TakeOffPoint.Longitude,Altitude);
//随机水平飞行距离
float maxdis = (float)new Random().NextDouble() * dis;
//计算方向--角度
float takeoffdir =(float)_takeoffcentloc.CalcDirection2D(TakeOffPoint)* 57.29578f;
//计算起飞点距离
float takeoffdis = (float)TakeOffPoint.CalcDistance2D(_takeoffcentloc.Latitude, _takeoffcentloc.Longitude);
ILocation2D take1 = TakeOffPoint.CalcLatLngSomeMetersAway2D(takeoffdir, takeoffdis);
//第一次返航点
EmergencyRetPLLocation1 = new PLLocation(take1.Latitude, take1.Longitude, minretalt_first);
}
}
}
public Task EmergencyRetAsync(PLLocation takeoffcentloc, PLLocation taskcentloc, float mindistance, int rettime, bool descending)
{
_takeoffcentloc = takeoffcentloc;
_taskcentloc = taskcentloc;
_mindistance = mindistance;
_rettime = rettime;
// _rettime = rettime;
_descending = descending;
Emergency_Retstatus = 0;
EmergencyRetTime = DateTime.Now;
//计算返航路径
BuildPath();
Mode = FlightMode.EMERG_RTL;
@ -531,6 +529,9 @@ namespace Plane.Copters
if (flightDistance != null) FlightDistance = flightDistance.Value;
if (flightDistance2D != null) FlightDistance2D = flightDistance2D.Value;
if (flightTimeSpan != null) FlightTimeSpan = flightTimeSpan.Value;
CopterPreCheckPass = true;
//CopterErrorCode = 2;
//CopterErrorString = getcoptererrorstr(CopterErrorCode);
}
public Task StartPairingAsync()
@ -554,7 +555,7 @@ namespace Plane.Copters
public Task MissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
{
MissionStartTime = DateTime.Now;
TakeOffPoint = new PLLocation(Missionlat, Missionlng, 0);
TakeOffPoint = new PLLocation(this.Latitude, this.Longitude, 0);
return TaskUtils.CompletedTask;
}
@ -799,7 +800,7 @@ namespace Plane.Copters
case FlightMode.LAND:
// 王海, 20160317, 降落时也能体感控制若干通道。
UpdateWithChannels();
// UpdateWithChannels();
// 以最大速度降落，直到高度为 0。
if (Altitude > 0)
{
@ -826,42 +827,59 @@ namespace Plane.Copters
switch(Emergency_Retstatus)
{
case 0: //等待返航
if ((DateTime.Now- MissionStartTime).TotalSeconds > _rettime)
if ((DateTime.Now - EmergencyRetTime).TotalSeconds > _rettime)
{
Emergency_Retstatus = 1;
//平飞或降落随机距离
FlyToCoreAsync(Emergency_firstloc.Latitude, Emergency_firstloc.Longitude, Emergency_firstloc.Altitude);
//FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
Mode = FlightMode.EMERG_RTL;
}
break;
case 1: //返航阶段一:等待平飞或降落随机距离并到达,然后飞到起飞点上空
//直接返航
FlyToCoreAsync(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt);
//先到第一返航点再到起飞点上空
//FlyToCoreAsync(EmergencyRetPLLocation1.Latitude, EmergencyRetPLLocation1.Longitude, EmergencyRetPLLocation1.Altitude);
UpdateWithDestination(Emergency_firstloc.Latitude, Emergency_firstloc.Longitude, Emergency_firstloc.Altitude);
if (ReachedDestination(Emergency_firstloc.Latitude, Emergency_firstloc.Longitude, Emergency_firstloc.Altitude))
{
FlyToCoreAsync(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude);
//FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
Mode = FlightMode.EMERG_RTL;
Emergency_Retstatus = 2;
}
break;
case 2: //返航阶段二:等待到达起飞点上空,然后降落
break;
UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude);
if (ReachedDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude))
case 1: //返航阶段一:等待平飞或降落随机距离并到达,然后飞到起飞点上空
//直接返航
UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt);
if (ReachedDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt))
{
FlyToCoreAsync(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude);
//FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
Mode = FlightMode.LAND;
//FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
Emergency_Retstatus = 3;
}
/* // //先到第一返航点再到起飞点上空
UpdateWithDestination(EmergencyRetPLLocation1.Latitude, EmergencyRetPLLocation1.Longitude, EmergencyRetPLLocation1.Altitude);
if (ReachedDestination(EmergencyRetPLLocation1.Latitude, EmergencyRetPLLocation1.Longitude, EmergencyRetPLLocation1.Altitude))
{
FlyToCoreAsync(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt);
Mode = FlightMode.EMERG_RTL;
//FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
Emergency_Retstatus = 2;
}
*/
break;
case 2: //返航阶段一:等待平飞或降落随机距离并到达,然后飞到起飞点上空
UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt);
if (ReachedDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt))
{
Mode = FlightMode.LAND;
//FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
Emergency_Retstatus = 3;
}
break;
case 3: //降落
break;
}
break;

806
PlaneGcsSdk_Shared/Copters/PlaneCopter.PacketAnalysis.cs
View File

@ -1,402 +1,404 @@
//#define LOG_PACKETS
using Plane.Protocols;
using System;
using System.Diagnostics;
using System.Text;
using System.Threading.Tasks;
using TaskTools.Utilities;
#if LOG_PACKETS
using System.Diagnostics;
#endif
namespace Plane.Copters
{
partial class PlaneCopter
{
private void AnalyzeAttitudePacket(byte[] buffer)
{
var att = buffer.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6);
roll = att.roll * rad2deg;
pitch = att.pitch * rad2deg;
yaw = att.yaw * rad2deg;
timebootms = att.time_boot_ms;
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
}
private void AnalyzeGlobalPositionIntPacket(byte[] buffer)
{
var loc = buffer.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);
useLocation = true;
if (loc.lat == 0 && loc.lon == 0)
{
useLocation = false;
}
else
{
gpsalt = loc.alt / 1000.0f;
lat = loc.lat / 10000000.0;
lng = loc.lon / 10000000.0;
}
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
}
#if LOG_GPS_RAW_INT
private Stopwatch _lastGpsRawIntPacketTime = new Stopwatch();
private TimeSpan _lastGpsRawIntPacketTimeSpan;
#endif
private void AnalyzeGpsRawIntPacket(byte[] buffer)
{
#if LOG_GPS_RAW_INT
if (!_lastGpsRawIntPacketTime.IsRunning)
{
_lastGpsRawIntPacketTime.Start();
}
else
{
var elapsed = _lastGpsRawIntPacketTime.Elapsed;
Debug.WriteLine(elapsed - _lastGpsRawIntPacketTimeSpan, "----------------------------GpsRawInt");
_lastGpsRawIntPacketTimeSpan = elapsed;
}
#endif
var gps = buffer.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6);
long mylat = BitConverter.ToInt32(buffer, 14);
long mylong = BitConverter.ToInt32(buffer, 18);
//if (!useLocation)
//{
lat = gps.lat * 1.0e-7;
lng = gps.lon * 1.0e-7;
//}
gpsalt = gps.alt * 1.0e-7f; // using vfr as includes baro calc
gpsstatus = gps.fix_type; //0-1: no fix, 2: 2D fix, 3: 3D fix
gpshdop = (float)Math.Round(gps.eph / 100.0, 2);
satcount = gps.satellites_visible;
groundspeed = gps.vel * 1.0e-2f;
groundcourse = gps.cog * 1.0e-2f;
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
}
#if LOG_PACKETS
private DateTime _lastHeartbeatTime = DateTime.MinValue;
#endif
private void AnalyzeHeartbeatPacket(byte[] buffer)
{
#if LOG_PACKETS
var now = DateTime.Now;
if (_lastHeartbeatTime != DateTime.MinValue)
{
Debug.WriteLine(now - _lastHeartbeatTime, "心跳间隔");
}
_lastHeartbeatTime = now;
#endif
DataTimeOut = 0;
var hb = buffer.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
mavlinkversion = hb.mavlink_version;
aptype = (MAVLink.MAV_TYPE)hb.type;
apname = (MAVLink.MAV_AUTOPILOT)hb.autopilot;
sysid = buffer[3];
compid = buffer[4];
recvpacketcount = buffer[2];
armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
failsafe = hb.system_status == (byte)MAVLink.MAV_STATE.CRITICAL;
mode = hb.custom_mode;
var handler = ReceiveHeartBearEvent;
if (handler != null)
{
RunOnUIThread(() =>
{
handler(this, iErrorData, iDataCount);
});
}
if (_autoRequestData && mavlinkversion == 3 && SendReq == false)
{
SendReq = true;
Task.Run(GetCopterDataAsync);
}
}
private void AnalyzeMemInfoPacket(byte[] buffer)
{
var mem = buffer.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
freemem = mem.freemem;
brklevel = mem.brkval;
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
}
private void AnalyzeMissionCurrentPacket(byte[] buffer)
{
var wpcur = buffer.ByteArrayToStructure<MAVLink.mavlink_mission_current_t>(6);
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
}
private void AnalyzeNavControllerOutputPacket(byte[] buffer)
{
var nav = buffer.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6);
nav_roll = nav.nav_roll;
nav_pitch = nav.nav_pitch;
nav_bearing = nav.nav_bearing;
target_bearing = nav.target_bearing;
wp_dist = nav.wp_dist;
alt_error = nav.alt_error;
aspd_error = nav.aspd_error / 100.0f;
xtrack_error = nav.xtrack_error;
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
}
private void AnalyzeParamValuePacket(byte[] buffer)
{
var par = buffer.ByteArrayToStructure<MAVLink.mavlink_param_value_t>(6);
param_total = (par.param_count);
string paramID = Encoding.ASCII.GetString(par.param_id);
int pos = paramID.IndexOf('\0');
if (pos != -1)
{
paramID = paramID.Substring(0, pos);
}
try
{
param[paramID] = (par.param_value);
param_types[paramID] = (MAVLink.MAV_PARAM_TYPE)par.param_type;
}
catch (IndexOutOfRangeException ex)
{
Debug.WriteLine(ex);
}
var handler = ReceiveParamEvent;
if (handler != null)
{
RunOnUIThread(() =>
{
handler(this, paramID, par.param_index, par.param_count);
});
}
}
private void AnalyzeRadioPacket(byte[] buffer)
{
var radio = buffer.ByteArrayToStructure<MAVLink.mavlink_radio_t>(6);
//Debug.Print("fix:" + radio.fixedp + " noise:" + radio.noise + " remnoise:" + radio.remnoise + " remrssi:" + radio.remrssi + " rssi:" + radio.rssi + " rxerr:" + radio.rxerrors + " txbuf:" + radio.txbuf);
remrssi = radio.remrssi;
rssi = radio.rssi;
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
}
private void AnalyzeRawImuPacket(byte[] buffer)
{
var imu = buffer.ByteArrayToStructure<MAVLink.mavlink_raw_imu_t>(6);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
mx = imu.xmag;
my = imu.ymag;
mz = imu.zmag;
var handler = ReceiveSensorEvent;
if (handler != null)
{
RunOnUIThread(() =>
{
handler(this);
});
}
}
private void AnalyzeRCChannelsRawPacket(byte[] buffer)
{
var rcin = buffer.ByteArrayToStructure<MAVLink.mavlink_rc_channels_raw_t>(6);
ch1in = rcin.chan1_raw;
ch2in = rcin.chan2_raw;
ch3in = rcin.chan3_raw;
ch4in = rcin.chan4_raw;
ch5in = rcin.chan5_raw;
ch6in = rcin.chan6_raw;
ch7in = rcin.chan7_raw;
ch8in = rcin.chan8_raw;
if (bInitChannel == false)
{
bInitChannel = true;
}
//percent
rxrssi = (float)((rcin.rssi / 255.0) * 100.0);
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
}
private void AnalyzeScaledImu2Packet(byte[] buffer)
{
var imu2 = buffer.ByteArrayToStructure<MAVLink.mavlink_scaled_imu2_t>(6);
gx2 = imu2.xgyro;
gy2 = imu2.ygyro;
gz2 = imu2.zgyro;
ax2 = imu2.xacc;
ay2 = imu2.yacc;
az2 = imu2.zacc;
mx2 = imu2.xmag;
my2 = imu2.ymag;
mz2 = imu2.zmag;
var handler = ReceiveScaleImu2Event;
if (handler != null)
{
RunOnUIThread(() => handler(this));
}
}
private void AnalyzeSensorOffsetsPacket(byte[] buffer)
{
var sensofs = buffer.ByteArrayToStructure<MAVLink.mavlink_sensor_offsets_t>(6);
mag_ofs_x = sensofs.mag_ofs_x;
mag_ofs_y = sensofs.mag_ofs_y;
mag_ofs_z = sensofs.mag_ofs_z;
mag_declination = sensofs.mag_declination;
raw_press = sensofs.raw_press;
raw_temp = sensofs.raw_temp;
gyro_cal_x = sensofs.gyro_cal_x;
gyro_cal_y = sensofs.gyro_cal_y;
gyro_cal_z = sensofs.gyro_cal_z;
accel_cal_x = sensofs.accel_cal_x;
accel_cal_y = sensofs.accel_cal_y;
accel_cal_z = sensofs.accel_cal_z;
var handler = ReceiveSensorEvent;
if (handler != null)
{
RunOnUIThread(() =>
{
handler(this);
});
}
}
private void AnalyzeStatusTextPacket(byte[] buffer)
{
var stext = buffer.ByteArrayToStructure<MAVLink.mavlink_statustext_t>(6);
var length = Array.IndexOf<byte>(stext.text, 0);
var log = Encoding.ASCII.GetString(stext.text, 0, length);
var handler = GetLogDataEvent;
if (handler != null)
{
RunOnUIThread(() => handler(log));
}
}
private void AnalyzeSysStatusPacket(byte[] buffer)
{
var sysstatus = buffer.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
battery_voltage = sysstatus.voltage_battery;
battery_remaining = sysstatus.battery_remaining;
current_battery = sysstatus.current_battery / 100.0f;
var sensors_enabled = new Mavlink_Sensors(sysstatus.onboard_control_sensors_enabled);
var sensors_health = new Mavlink_Sensors(sysstatus.onboard_control_sensors_health);
if (sensors_health.gps != sensors_enabled.gps)
{
bGPSBadHealth = true;
}
else
{
bGPSBadHealth = false;
}
if (sensors_health.gyro != sensors_enabled.gyro)
{
bGyroBadHealth = true;
}
else
{
bGyroBadHealth = false;
}
if (sensors_health.accelerometer != sensors_enabled.accelerometer)
{
bAccel = true;
}
else
{
bAccel = false;
}
if (sensors_health.compass != sensors_enabled.compass)
{
bCompass = true;
}
else
{
bCompass = false;
}
if (sensors_health.barometer != sensors_enabled.barometer)
{
bBarometer = true;
}
else
{
bBarometer = false;
}
FlightDistance2D = sysstatus.errors_count2;
var handler = ReceiveSysStatusEvent;
if (handler != null)
{
RunOnUIThread(() =>
{
handler(this, buffer[5], bGPSBadHealth, bGyroBadHealth, bAccel, bCompass, bBarometer, sensors_health.CanTakeOff);
});
}
}
private void AnalyzeVfrHudPacket(byte[] buffer)
{
var vfr = buffer.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6);
groundspeed = vfr.groundspeed;
airspeed = vfr.airspeed;
altorigin = vfr.alt; // this might include baro
ch3percent = vfr.throttle;
heading = vfr.heading;
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
}
/// <summary>
/// 处理通信模块发过来的数据
/// </summary>
/// <param name="buffer"></param>
public void AnalyzeCommMouldePositionIntPacket(byte[] buffer, byte version)
{
var info = buffer.ByteArrayToStructure<MavComm.comm_copter_info>(0);
lat = info.lat * 1.0e-7;
lng = info.lng * 1.0e-7;
commModuleMode = (uint)info.control_mode;
gpsstatus = (byte)info.gps_status;
gpsalt = ((float)info.alt) / 1000;
satcount = info.gps_num_sats;
isUnlocked = info.lock_status == 1;
battery_voltage = ((float)info.battery_voltage) / 1000;
heading = (short)((info.heading / 100) % 360);
commModuleVersion = version;
retain = info.retain;
RaiseReceiveDataStreamEventOnUIThread(MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT);
}
}
}
//#define LOG_PACKETS
using Plane.Protocols;
using System;
using System.Diagnostics;
using System.Text;
using System.Threading.Tasks;
using TaskTools.Utilities;
#if LOG_PACKETS
using System.Diagnostics;
#endif
namespace Plane.Copters
{
partial class PlaneCopter
{
private void AnalyzeAttitudePacket(byte[] buffer)
{
var att = buffer.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6);
roll = att.roll * rad2deg;
pitch = att.pitch * rad2deg;
yaw = att.yaw * rad2deg;
timebootms = att.time_boot_ms;
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
}
private void AnalyzeGlobalPositionIntPacket(byte[] buffer)
{
var loc = buffer.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);
useLocation = true;
if (loc.lat == 0 && loc.lon == 0)
{
useLocation = false;
}
else
{
gpsalt = loc.alt / 1000.0f;
lat = loc.lat / 10000000.0;
lng = loc.lon / 10000000.0;
}
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
}
#if LOG_GPS_RAW_INT
private Stopwatch _lastGpsRawIntPacketTime = new Stopwatch();
private TimeSpan _lastGpsRawIntPacketTimeSpan;
#endif
private void AnalyzeGpsRawIntPacket(byte[] buffer)
{
#if LOG_GPS_RAW_INT
if (!_lastGpsRawIntPacketTime.IsRunning)
{
_lastGpsRawIntPacketTime.Start();
}
else
{
var elapsed = _lastGpsRawIntPacketTime.Elapsed;
Debug.WriteLine(elapsed - _lastGpsRawIntPacketTimeSpan, "----------------------------GpsRawInt");
_lastGpsRawIntPacketTimeSpan = elapsed;
}
#endif
var gps = buffer.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6);
long mylat = BitConverter.ToInt32(buffer, 14);
long mylong = BitConverter.ToInt32(buffer, 18);
//if (!useLocation)
//{
lat = gps.lat * 1.0e-7;
lng = gps.lon * 1.0e-7;
//}
gpsalt = gps.alt * 1.0e-7f; // using vfr as includes baro calc
gpsstatus = gps.fix_type; //0-1: no fix, 2: 2D fix, 3: 3D fix
gpshdop = (float)Math.Round(gps.eph / 100.0, 2);
satcount = gps.satellites_visible;
groundspeed = gps.vel * 1.0e-2f;
groundcourse = gps.cog * 1.0e-2f;
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
}
#if LOG_PACKETS
private DateTime _lastHeartbeatTime = DateTime.MinValue;
#endif
private void AnalyzeHeartbeatPacket(byte[] buffer)
{
#if LOG_PACKETS
var now = DateTime.Now;
if (_lastHeartbeatTime != DateTime.MinValue)
{
Debug.WriteLine(now - _lastHeartbeatTime, "心跳间隔");
}
_lastHeartbeatTime = now;
#endif
DataTimeOut = 0;
var hb = buffer.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
mavlinkversion = hb.mavlink_version;
aptype = (MAVLink.MAV_TYPE)hb.type;
apname = (MAVLink.MAV_AUTOPILOT)hb.autopilot;
sysid = buffer[3];
compid = buffer[4];
recvpacketcount = buffer[2];
armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
failsafe = hb.system_status == (byte)MAVLink.MAV_STATE.CRITICAL;
mode = hb.custom_mode;
var handler = ReceiveHeartBearEvent;
if (handler != null)
{
RunOnUIThread(() =>
{
handler(this, iErrorData, iDataCount);
});
}
if (_autoRequestData && mavlinkversion == 3 && SendReq == false)
{
SendReq = true;
Task.Run(GetCopterDataAsync);
}
}
private void AnalyzeMemInfoPacket(byte[] buffer)
{
var mem = buffer.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
freemem = mem.freemem;
brklevel = mem.brkval;
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
}
private void AnalyzeMissionCurrentPacket(byte[] buffer)
{
var wpcur = buffer.ByteArrayToStructure<MAVLink.mavlink_mission_current_t>(6);
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
}
private void AnalyzeNavControllerOutputPacket(byte[] buffer)
{
var nav = buffer.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6);
nav_roll = nav.nav_roll;
nav_pitch = nav.nav_pitch;
nav_bearing = nav.nav_bearing;
target_bearing = nav.target_bearing;
wp_dist = nav.wp_dist;
alt_error = nav.alt_error;
aspd_error = nav.aspd_error / 100.0f;
xtrack_error = nav.xtrack_error;
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
}
private void AnalyzeParamValuePacket(byte[] buffer)
{
var par = buffer.ByteArrayToStructure<MAVLink.mavlink_param_value_t>(6);
param_total = (par.param_count);
string paramID = Encoding.ASCII.GetString(par.param_id);
int pos = paramID.IndexOf('\0');
if (pos != -1)
{
paramID = paramID.Substring(0, pos);
}
try
{
param[paramID] = (par.param_value);
param_types[paramID] = (MAVLink.MAV_PARAM_TYPE)par.param_type;
}
catch (IndexOutOfRangeException ex)
{
Debug.WriteLine(ex);
}
var handler = ReceiveParamEvent;
if (handler != null)
{
RunOnUIThread(() =>
{
handler(this, paramID, par.param_index, par.param_count);
});
}
}
private void AnalyzeRadioPacket(byte[] buffer)
{
var radio = buffer.ByteArrayToStructure<MAVLink.mavlink_radio_t>(6);
//Debug.Print("fix:" + radio.fixedp + " noise:" + radio.noise + " remnoise:" + radio.remnoise + " remrssi:" + radio.remrssi + " rssi:" + radio.rssi + " rxerr:" + radio.rxerrors + " txbuf:" + radio.txbuf);
remrssi = radio.remrssi;
rssi = radio.rssi;
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
}
private void AnalyzeRawImuPacket(byte[] buffer)
{
var imu = buffer.ByteArrayToStructure<MAVLink.mavlink_raw_imu_t>(6);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
mx = imu.xmag;
my = imu.ymag;
mz = imu.zmag;
var handler = ReceiveSensorEvent;
if (handler != null)
{
RunOnUIThread(() =>
{
handler(this);
});
}
}
private void AnalyzeRCChannelsRawPacket(byte[] buffer)
{
var rcin = buffer.ByteArrayToStructure<MAVLink.mavlink_rc_channels_raw_t>(6);
ch1in = rcin.chan1_raw;
ch2in = rcin.chan2_raw;
ch3in = rcin.chan3_raw;
ch4in = rcin.chan4_raw;
ch5in = rcin.chan5_raw;
ch6in = rcin.chan6_raw;
ch7in = rcin.chan7_raw;
ch8in = rcin.chan8_raw;
if (bInitChannel == false)
{
bInitChannel = true;
}
//percent
rxrssi = (float)((rcin.rssi / 255.0) * 100.0);
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
}
private void AnalyzeScaledImu2Packet(byte[] buffer)
{
var imu2 = buffer.ByteArrayToStructure<MAVLink.mavlink_scaled_imu2_t>(6);
gx2 = imu2.xgyro;
gy2 = imu2.ygyro;
gz2 = imu2.zgyro;
ax2 = imu2.xacc;
ay2 = imu2.yacc;
az2 = imu2.zacc;
mx2 = imu2.xmag;
my2 = imu2.ymag;
mz2 = imu2.zmag;
var handler = ReceiveScaleImu2Event;
if (handler != null)
{
RunOnUIThread(() => handler(this));
}
}
private void AnalyzeSensorOffsetsPacket(byte[] buffer)
{
var sensofs = buffer.ByteArrayToStructure<MAVLink.mavlink_sensor_offsets_t>(6);
mag_ofs_x = sensofs.mag_ofs_x;
mag_ofs_y = sensofs.mag_ofs_y;
mag_ofs_z = sensofs.mag_ofs_z;
mag_declination = sensofs.mag_declination;
raw_press = sensofs.raw_press;
raw_temp = sensofs.raw_temp;
gyro_cal_x = sensofs.gyro_cal_x;
gyro_cal_y = sensofs.gyro_cal_y;
gyro_cal_z = sensofs.gyro_cal_z;
accel_cal_x = sensofs.accel_cal_x;
accel_cal_y = sensofs.accel_cal_y;
accel_cal_z = sensofs.accel_cal_z;
var handler = ReceiveSensorEvent;
if (handler != null)
{
RunOnUIThread(() =>
{
handler(this);
});
}
}
private void AnalyzeStatusTextPacket(byte[] buffer)
{
var stext = buffer.ByteArrayToStructure<MAVLink.mavlink_statustext_t>(6);
var length = Array.IndexOf<byte>(stext.text, 0);
var log = Encoding.ASCII.GetString(stext.text, 0, length);
var handler = GetLogDataEvent;
if (handler != null)
{
RunOnUIThread(() => handler(log));
}
}
private void AnalyzeSysStatusPacket(byte[] buffer)
{
var sysstatus = buffer.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
battery_voltage = sysstatus.voltage_battery;
battery_remaining = sysstatus.battery_remaining;
current_battery = sysstatus.current_battery / 100.0f;
var sensors_enabled = new Mavlink_Sensors(sysstatus.onboard_control_sensors_enabled);
var sensors_health = new Mavlink_Sensors(sysstatus.onboard_control_sensors_health);
if (sensors_health.gps != sensors_enabled.gps)
{
bGPSBadHealth = true;
}
else
{
bGPSBadHealth = false;
}
if (sensors_health.gyro != sensors_enabled.gyro)
{
bGyroBadHealth = true;
}
else
{
bGyroBadHealth = false;
}
if (sensors_health.accelerometer != sensors_enabled.accelerometer)
{
bAccel = true;
}
else
{
bAccel = false;
}
if (sensors_health.compass != sensors_enabled.compass)
{
bCompass = true;
}
else
{
bCompass = false;
}
if (sensors_health.barometer != sensors_enabled.barometer)
{
bBarometer = true;
}
else
{
bBarometer = false;
}
FlightDistance2D = sysstatus.errors_count2;
var handler = ReceiveSysStatusEvent;
if (handler != null)
{
RunOnUIThread(() =>
{
handler(this, buffer[5], bGPSBadHealth, bGyroBadHealth, bAccel, bCompass, bBarometer, sensors_health.CanTakeOff);
});
}
}
private void AnalyzeVfrHudPacket(byte[] buffer)
{
var vfr = buffer.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6);
groundspeed = vfr.groundspeed;
airspeed = vfr.airspeed;
altorigin = vfr.alt; // this might include baro
ch3percent = vfr.throttle;
heading = vfr.heading;
RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
}
/// <summary>
/// 飞控返回数据 处理通信模块发过来的28个字节数据
/// </summary>
/// <param name="buffer"></param>
public void AnalyzeCommMouldePositionIntPacket(byte[] buffer, byte version)
{
var info = buffer.ByteArrayToStructure<MavComm.comm_copter_info>(0);
lat = info.lat * 1.0e-7;
lng = info.lng * 1.0e-7;
commModuleMode = (uint)info.control_mode;
gpsstatus = (byte)info.gps_status;
errorcode= (byte)info.error_code;
precheckok = info.rpecheck_fault == 0;
gpsalt = ((float)info.alt) / 1000;
satcount = info.gps_num_sats;
isUnlocked = info.lock_status == 1;
battery_voltage = ((float)info.battery_voltage) / 1000;
heading = (short)((info.heading / 100) % 360);
commModuleVersion = version;
retain = info.retain;
RaiseReceiveDataStreamEventOnUIThread(MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT);
}
}
}

3356
PlaneGcsSdk_Shared/Copters/PlaneCopter.cs
View File

File diff suppressed because it is too large Load Diff

364
PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs
View File

@ -1,177 +1,187 @@
using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using System.Text;
namespace Plane.Protocols
{
/*
*
* 通信模块协议
*
*/
public class MavComm
{
/// <summary>
/// 发送数据时的数据包头
/// </summary>
public const ushort COMM_SEND_PACKET_HEADER = 0x9551;
/// <summary>
/// 接受数据时的数据包头
/// </summary>
public const ushort COMM_RECEIVE_PACKET_HEADER = 0x7713;
#region 命令类型 2字节
//-----------------命令类型-----------------
/// <summary>
/// 查询状态
/// </summary>
public const short COMM_QUERY_STATUS = 0x00;
/// <summary>
/// 变更飞机总数及参与者
/// </summary>
public const short COMM_SET_MAV_COUNT = 0x10;
/// <summary>
/// 获取飞机详细信息
/// </summary>
public const short COMM_GET_COPTERS_COUNT = 0x20;
/// <summary>
/// 时间同步，不改变当前模式
/// </summary>
public const short COMM_ASYN_TIME = 0x30;
/// <summary>
/// 进入空闲模式
/// </summary>
public const short COMM_IDLE_MODE = 0x50;
/// <summary>
/// 进入搜索模式，命名编号targetID memcpy(pdata,input,2);
/// </summary>
public const short COMM_SEARCH_MODE = 0x51;
/// <summary>
/// 进入航点下载模式 (写航点)
/// </summary>
public const short COMM_DOWNLOAD_MODE = 0x52;
/// <summary>
/// 下载模式通信
/// </summary>
public const short COMM_DOWNLOAD_COMM = 0x53;
/// <summary>
/// 进入飞行模式（无负载）
/// </summary>
public const short COMM_FLIGHT_MODE = 0x54;
/// <summary>
/// 进入飞行模式（有负载数据）
/// </summary>
public const short COMM_FLIGHT_LOAD_MODE = 0x55;
/// <summary>
/// 发送非针对飞控的内部控制指令
/// </summary>
public const short COMM_TO_MODULE = 0x5F;
/// <summary>
/// 通信模块
/// </summary>
public const short COMM_NOTIFICATION = 0x1234;
/// <summary>
/// 空中升级(更新通信模块飞机端)
/// </summary>
public const short COMM_UPDATE_COPTER_MODULE = 0xFD;
/// <summary>
/// 测试模块
/// </summary>
public const short COMM_TEST_MODULE = 0x3C;
#endregion
public enum CommMode
{
IDLE = 0,
SEARCH = 1,
DOWNLOAD = 3,
FLIGHT = 4
}
public enum MessageType
{
STR = 0,
SCAN2 = 2,
SCAN3 = 3
}
//search 搜索模式相关
public const ushort SEARCH_COMPLETED = 0x0502; //搜索模式 扫描成功
public const ushort FREQUENCY_HOPPING_COMPLETED = 0x0503; //搜索模式 跳频表更新成功
public const ushort SEARCH_FINDCOPTER = 0x0401; //搜索模式 找到指定目标
public const ushort FREQUENCY_HOPPING_ERROR = 0x0100; //搜索模式 跳频表更新错误
public const ushort SEARCH_OUTTIME = 0xFFFF; //搜索模式 没有人应答 超时时间20S
public const ushort MISSION_SEND_COMPLETED = 0x0503; //航点传输到飞控成功，可开始传输下个飞机航点信息,
//需要再后续等待一个成功消息，才算完成
public const ushort P2P_SEND_FAILED = 0x0600; //点对点通信发送失败
public const ushort ERROR_CODE_START = 0x0100;
public const ushort ERROR_CODE_END = 0x03FF;
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
public struct comm_set_mav_count
{
public Int16 mav_count; //飞机总数
};
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)]
public struct comm_update_copter_module
{
public Int16 mav_count; //飞机总数
public Int16 update_code;
};
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)]
public struct comm_asyn_time
{
public Int64 time_stamp;
};
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)]
public struct comm_copter_info
{
public Int32 control_mode;
public UInt32 lat;
public UInt32 lng;
public float retain;
public Int32 alt;
public Int16 gps_status;
public byte lock_status;
public byte gps_num_sats;
public Int16 battery_voltage;
public UInt16 heading;
};
}
}
using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using System.Text;
namespace Plane.Protocols
{
/*
*
* 通信模块协议
*
*/
public class MavComm
{
/// <summary>
/// 发送数据时的数据包头
/// </summary>
public const ushort COMM_SEND_PACKET_HEADER = 0x9551;
/// <summary>
/// 接受数据时的数据包头
/// </summary>
public const ushort COMM_RECEIVE_PACKET_HEADER = 0x7713;
#region 命令类型 2字节
//-----------------命令类型-----------------
/// <summary>
/// 查询状态
/// </summary>
public const short COMM_QUERY_STATUS = 0x00;
/// <summary>
/// 变更飞机总数及参与者
/// </summary>
public const short COMM_SET_MAV_COUNT = 0x10;
/// <summary>
/// 获取飞机详细信息
/// </summary>
public const short COMM_GET_COPTERS_COUNT = 0x20;
/// <summary>
/// 时间同步，不改变当前模式
/// </summary>
public const short COMM_ASYN_TIME = 0x30;
/// <summary>
/// 进入空闲模式
/// </summary>
public const short COMM_IDLE_MODE = 0x50;
/// <summary>
/// 进入搜索模式，命名编号targetID memcpy(pdata,input,2);
/// </summary>
public const short COMM_SEARCH_MODE = 0x51;
/// <summary>
/// 进入航点下载模式 (写航点)
/// </summary>
public const short COMM_DOWNLOAD_MODE = 0x52;
/// <summary>
/// 下载模式通信
/// </summary>
public const short COMM_DOWNLOAD_COMM = 0x53;
/// <summary>
/// 进入飞行模式（无负载）
/// </summary>
public const short COMM_FLIGHT_MODE = 0x54;
/// <summary>
/// 进入飞行模式（有负载数据）
/// </summary>
public const short COMM_FLIGHT_LOAD_MODE = 0x55;
/// <summary>
/// 发送非针对飞控的内部控制指令
/// </summary>
public const short COMM_TO_MODULE = 0x5F;
/// <summary>
/// 通信模块
/// </summary>
public const short COMM_NOTIFICATION = 0x1234;
/// <summary>
/// 空中升级(更新通信模块飞机端)
/// </summary>
public const short COMM_UPDATE_COPTER_MODULE = 0xFD;
/// <summary>
/// 测试模块
/// </summary>
public const short COMM_TEST_MODULE = 0x3C;
#endregion
public enum CommMode
{
IDLE = 0,
SEARCH = 1,
DOWNLOAD = 3,
FLIGHT = 4
}
public enum MessageType
{
STR = 0,
SCAN2 = 2,
SCAN3 = 3
}
//search 搜索模式相关
public const ushort SEARCH_COMPLETED = 0x0502; //搜索模式 扫描成功
public const ushort FREQUENCY_HOPPING_COMPLETED = 0x0503; //搜索模式 跳频表更新成功
public const ushort SEARCH_FINDCOPTER = 0x0401; //搜索模式 找到指定目标
public const ushort FREQUENCY_HOPPING_ERROR = 0x0100; //搜索模式 跳频表更新错误
public const ushort SEARCH_OUTTIME = 0xFFFF; //搜索模式 没有人应答 超时时间20S
public const ushort MISSION_SEND_COMPLETED = 0x0503; //航点传输到飞控成功，可开始传输下个飞机航点信息,
//需要再后续等待一个成功消息，才算完成
public const ushort P2P_SEND_FAILED = 0x0600; //点对点通信发送失败
public const ushort ERROR_CODE_START = 0x0100;
public const ushort ERROR_CODE_END = 0x03FF;
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
public struct comm_set_mav_count
{
public Int16 mav_count; //飞机总数
};
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)]
public struct comm_update_copter_module
{
public Int16 mav_count; //飞机总数
public Int16 update_code;
};
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)]
public struct comm_asyn_time
{
public Int64 time_stamp;
};
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)]
public struct comm_copter_info
{
// public Int32 control_mode; //飞行模式(自动，悬停，定高，返航)
public byte control_mode; //飞行模式(自动，悬停，定高，返航)
public byte rpecheck_fault; //是否有 预解锁错误
public byte reserveddata1; //保留
public byte reserveddata2; //保留
public UInt32 lat; // 经度 in 1E7 degrees
public UInt32 lng; // 维度 in 1E7 degrees
public float retain; // 写参数序列号,返回版本号等不特定返回数据
public Int32 alt; // millimeters above home
public byte gps_status; //锁定类型 (无锁定,3D锁定，RTK浮点，RTK固定)
public byte error_code; //错误号，0表示无错误 --放到低位为了和之前兼容
public byte lock_status; //以及是否解锁
public byte gps_num_sats; // 卫星数量
public Int16 battery_voltage; // 电池电压mV
public UInt16 heading; //方向角度
};
}
}