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: case (short)MavComm.MessageType.SCAN3:
AnalyzeComm3RetrunPacket(copterNum, dealData); AnalyzeComm3RetrunPacket(copterNum, dealData);
break; break;
case 4: case 4: //版本13通讯模块收到飞机返回的数据
AnalyzeComm4RetrunPacket(copterNum, dealData); AnalyzeComm4RetrunPacket(copterNum, dealData);
break; break;
} }

91
PlaneGcsSdk_Shared/CommunicationManagement/CommModuleGenerateMavLink.cs
View File

@ -851,14 +851,10 @@ namespace Plane.CommunicationManagement
param7 = param7 | 1; param7 = param7 | 1;
if (UseTransModule) if (UseTransModule)
{ {
byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.RETURN_TO_LAUNCH,
(float)takeoffcentloc.Latitude * 1000, (float)takeoffcentloc.Latitude * 10000000,
(float)takeoffcentloc.Longitude * 1000,//起飞高度不用传=0 (float)takeoffcentloc.Longitude * 10000000,//起飞高度不用传=0
(float)taskcentloc.Latitude * 1000, mindistance*100, 0, 0,0,0);
(float)taskcentloc.Longitude * 1000,
taskcentloc.Altitude,
mindistance, param7
);
short copterId = 0; short copterId = 0;
byte[] batchPacket = null; byte[] batchPacket = null;
await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false); 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>
/// 校准指南针 /// 校准指南针
/// </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> /// <summary>
@ -1252,26 +1307,28 @@ namespace Plane.CommunicationManagement
//gps.data[0] = rtcm_tmpser++; //gps.data[0] = rtcm_tmpser++;
gps.len = (byte)datalen; gps.len = (byte)datalen;
gps.target_component = rtcm_tmpser++; 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); byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len);
await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); 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)飞机可以检测出来重复接收的 //重发一次，有序列号(target_component)飞机可以检测出来重复接收的
//需要新固件支持 //需要新固件支持
// await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); // 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)飞机可以检测出来重复接收的--需要新固件支持 //重发一次，有序列号(target_component)飞机可以检测出来重复接收的--需要新固件支持
await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false); //await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
await Task.Delay(50).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到 // await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
*/
} }

426
PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs
View File

@ -1,213 +1,213 @@
using Plane.Communication; using Plane.Communication;
using Plane.Protocols; using Plane.Protocols;
using Plane.Windows.Messages; using Plane.Windows.Messages;
using System; using System;
using System.Collections.Generic; using System.Collections.Generic;
using System.Diagnostics; using System.Diagnostics;
using System.Linq; using System.Linq;
using System.Text; using System.Text;
using System.Threading.Tasks; using System.Threading.Tasks;
namespace Plane.CommunicationManagement namespace Plane.CommunicationManagement
{ {
public partial class CommModuleManager public partial class CommModuleManager
{ {
private int MissionStartCopterId = 0; private int MissionStartCopterId = 0;
private int MissionSendCopterId = 0; private int MissionSendCopterId = 0;
private int MissionErrorId = -1; private int MissionErrorId = -1;
private int ErrorCode = 0; private int ErrorCode = 0;
public event EventHandler<CommunicationReceiveCopterInfoEventArgs> CommunicationReceived; public event EventHandler<CommunicationReceiveCopterInfoEventArgs> CommunicationReceived;
public event EventHandler<CommunicationCopterDisconnectEventArgs> CommunicationCopterDisconnect; public event EventHandler<CommunicationCopterDisconnectEventArgs> CommunicationCopterDisconnect;
public event EventHandler<CommunicationConnectEventArgs> CommunicationConnected; public event EventHandler<CommunicationConnectEventArgs> CommunicationConnected;
private Dictionary<int, CommWriteMissinState> missionWriteState = new Dictionary<int, CommWriteMissinState>(); private Dictionary<int, CommWriteMissinState> missionWriteState = new Dictionary<int, CommWriteMissinState>();
private static readonly object MissinLocker = new object(); private static readonly object MissinLocker = new object();
public Dictionary<int, CommWriteMissinState> MissionWriteState public Dictionary<int, CommWriteMissinState> MissionWriteState
{ {
get {return missionWriteState; } get {return missionWriteState; }
} }
public void ClearMissionWriteState() public void ClearMissionWriteState()
{ {
missionWriteState.Clear(); missionWriteState.Clear();
} }
private void AnalyzeMissionStartPacket(short copterId, byte[] buffer) private void AnalyzeMissionStartPacket(short copterId, byte[] buffer)
{ {
short errorId = BitConverter.ToInt16(buffer, 0); short errorId = BitConverter.ToInt16(buffer, 0);
MissionStartCopterId = copterId; MissionStartCopterId = copterId;
MissionErrorId = errorId; MissionErrorId = errorId;
if(errorId != 0) if(errorId != 0)
Message.Show($"{copterId}:返回 = {errorId}"); Message.Show($"{copterId}:返回 = {errorId}");
} }
private void AnalyzeStringPacket(short copterId, byte[] buffer) private void AnalyzeStringPacket(short copterId, byte[] buffer)
{ {
int count = Array.IndexOf(buffer, (byte)0); int count = Array.IndexOf(buffer, (byte)0);
string msg = System.Text.Encoding.Default.GetString(buffer, 0, count).Replace("tcpserver", "flicube"); string msg = System.Text.Encoding.Default.GetString(buffer, 0, count).Replace("tcpserver", "flicube");
Message.Show(msg); Message.Show(msg);
} }
private void AnalyzeComm2RetrunPacket(short copterId, byte[] buffer) private void AnalyzeComm2RetrunPacket(short copterId, byte[] buffer)
{ {
ushort ret = BitConverter.ToUInt16(buffer, 0); ushort ret = BitConverter.ToUInt16(buffer, 0);
switch (ret) switch (ret)
{ {
case MavComm.SEARCH_FINDCOPTER: case MavComm.SEARCH_FINDCOPTER:
Message.Show(copterId.ToString() + "---飞机开始相应"); Message.Show(copterId.ToString() + "---飞机开始相应");
break; break;
case MavComm.SEARCH_COMPLETED: case MavComm.SEARCH_COMPLETED:
Message.Show(copterId.ToString() + "---设置成功"); Message.Show(copterId.ToString() + "---设置成功");
break; break;
case MavComm.SEARCH_OUTTIME: case MavComm.SEARCH_OUTTIME:
Message.Show("超时无响应"); Message.Show("超时无响应");
break; break;
case MavComm.MISSION_SEND_COMPLETED: case MavComm.MISSION_SEND_COMPLETED:
MissionSendCopterId = copterId; MissionSendCopterId = copterId;
break; break;
case MavComm.P2P_SEND_FAILED: case MavComm.P2P_SEND_FAILED:
Message.Show("点对点通信发送失败"); Message.Show("点对点通信发送失败");
break; break;
default: default:
if (ret > MavComm.ERROR_CODE_START && ret <= MavComm.ERROR_CODE_END) if (ret > MavComm.ERROR_CODE_START && ret <= MavComm.ERROR_CODE_END)
{ {
ErrorCode = ret; ErrorCode = ret;
Message.Show($"{copterId}--错误码：{ret}"); Message.Show($"{copterId}--错误码：{ret}");
} }
break; break;
} }
} }
private void AnalyzeComm3RetrunPacket(short copterId, byte[] buffer) private void AnalyzeComm3RetrunPacket(short copterId, byte[] buffer)
{ {
byte type = buffer[0]; byte type = buffer[0];
byte connectRet; byte connectRet;
switch (type) switch (type)
{ {
case 0x01: case 0x01:
//connectRet = buffer[1]; //connectRet = buffer[1];
//if (connectRet == 0x0) //飞机连接上 不管飞控和通信模块的连接 //if (connectRet == 0x0) //飞机连接上 不管飞控和通信模块的连接
CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId)); CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
break; break;
case 0x02: case 0x02:
connectRet = buffer[1]; connectRet = buffer[1];
if (connectRet == 0x0) //飞机断开 if (connectRet == 0x0) //飞机断开
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId)); CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
break; break;
case 0x03: case 0x03:
SaveMissionWriteStat(copterId, buffer[1]); SaveMissionWriteStat(copterId, buffer[1]);
break; break;
case 0x04: case 0x04:
Message.Show($"飞机{copterId}:功率设置成功，新回传功率：{buffer[1]}"); Message.Show($"飞机{copterId}:功率设置成功，新回传功率：{buffer[1]}");
break; break;
case 0x0e: case 0x0e:
if (copterId == 0) if (copterId == 0)
Message.Show("----------全部更新完成----------"); Message.Show("----------全部更新完成----------");
else else
Message.Show($"飞机{copterId}:更新进度{buffer[1]}%"); Message.Show($"飞机{copterId}:更新进度{buffer[1]}%");
break; break;
default: default:
// Message.Show($"Comm3返回：{type}"); // Message.Show($"Comm3返回：{type}");
break; break;
} }
} }
private void SaveMissionWriteStat(short copterId, byte wirteMissRet) private void SaveMissionWriteStat(short copterId, byte wirteMissRet)
{ {
Task.Run(async () => Task.Run(async () =>
{ {
lock (MissinLocker) lock (MissinLocker)
{ {
// var copter = _copterManager.Copters.FirstOrDefault(c => c.Id == arrs[0]); // var copter = _copterManager.Copters.FirstOrDefault(c => c.Id == arrs[0]);
if (wirteMissRet == 0x20) if (wirteMissRet == 0x20)
{ {
if (missionWriteState.ContainsKey(copterId)) if (missionWriteState.ContainsKey(copterId))
missionWriteState[copterId].WriteSucceed = true; missionWriteState[copterId].WriteSucceed = true;
Message.Show($"ID:{copterId}:航点写入成功"); Message.Show($"ID:{copterId}:航点写入成功");
} }
if (wirteMissRet > 0x20 && wirteMissRet < 0x30) if (wirteMissRet > 0x20 && wirteMissRet < 0x30)
{ {
if (missionWriteState.ContainsKey(copterId)) if (missionWriteState.ContainsKey(copterId))
missionWriteState[copterId].WriteSucceed = false; missionWriteState[copterId].WriteSucceed = false;
Message.Show($"ID:{copterId}:航点写入失败"); Message.Show($"ID:{copterId}:航点写入失败");
} }
} }
await Task.Delay(10).ConfigureAwait(false); await Task.Delay(10).ConfigureAwait(false);
} }
); );
} }
private void AnalyzeCopterInfosPacket(byte[] buffer) private void AnalyzeCopterInfosPacket(byte[] buffer)
{ {
ushort beginNum = BitConverter.ToUInt16(buffer, 0); ushort beginNum = BitConverter.ToUInt16(buffer, 0);
ushort endNum = BitConverter.ToUInt16(buffer, 2); ushort endNum = BitConverter.ToUInt16(buffer, 2);
int count = endNum - beginNum + 1; int count = endNum - beginNum + 1;
byte[] copter_packets = buffer.Skip(4).ToArray(); byte[] copter_packets = buffer.Skip(4).ToArray();
if (copter_packets.Length != count * 4) if (copter_packets.Length != count * 4)
{ {
return; return;
} }
int offset = 0; int offset = 0;
for (int i = beginNum; i <= endNum; i++) for (int i = beginNum; i <= endNum; i++)
{ {
byte[] onePacket = copter_packets.Skip(offset).Take(4).ToArray(); byte[] onePacket = copter_packets.Skip(offset).Take(4).ToArray();
UInt16 state = BitConverter.ToUInt16(onePacket, 0); UInt16 state = BitConverter.ToUInt16(onePacket, 0);
short copterId = (short)i; short copterId = (short)i;
switch (state >> 13) switch (state >> 13)
{ {
//0B000 //0B000
case 0x0000 >> 13: case 0x0000 >> 13:
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId)); CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
break; break;
//0B100 //0B100
case 0x8000 >> 13: case 0x8000 >> 13:
break; break;
//0B110 //0B110
case 0xC000 >> 13: case 0xC000 >> 13:
//0B111 //0B111
case 0xE000 >> 13: case 0xE000 >> 13:
CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId)); CommunicationConnected?.Invoke(this, new CommunicationConnectEventArgs(copterId));
break; break;
} }
offset += 4; offset += 4;
} }
} }
private void AnalyzeComm4RetrunPacket(short copterId, byte[] buffer) private void AnalyzeComm4RetrunPacket(short copterId, byte[] buffer)
{ {
byte[] packet = buffer.Take(28).ToArray(); byte[] packet = buffer.Take(28).ToArray();
byte[] state_packet = buffer.Skip(28).ToArray(); byte[] state_packet = buffer.Skip(28).ToArray();
UInt16 state = BitConverter.ToUInt16(state_packet,0); UInt16 state = BitConverter.ToUInt16(state_packet,0);
byte version = state_packet[3]; byte version = state_packet[3];
switch (state >> 13) switch (state >> 13)
{ {
//0B000 //0B000
case 0x0000 >> 13: case 0x0000 >> 13:
CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId)); CommunicationCopterDisconnect?.Invoke(this, new CommunicationCopterDisconnectEventArgs(copterId));
break; break;
//0B100 //0B100
case 0x8000 >> 13: case 0x8000 >> 13:
break; break;
//0B110 //0B110
case 0xC000 >> 13: case 0xC000 >> 13:
//0B111 //0B111
case 0xE000 >> 13: case 0xE000 >> 13: //最后正在用的版本
CommunicationReceived?.Invoke(this, new CommunicationReceiveCopterInfoEventArgs(copterId, packet, version)); CommunicationReceived?.Invoke(this, new CommunicationReceiveCopterInfoEventArgs(copterId, packet, version));
break; 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 System;
using static Plane.Protocols.MAVLink; using System.Collections.Generic;
using static Plane.Protocols.MAVLink;
namespace Plane.Copters
{ namespace Plane.Copters
public partial class PLCopter : CopterImplSharedPart {
{
private bool _fetchingFirmwareVersion;
public partial class PLCopter : CopterImplSharedPart
private void _internalCopter_ConnectionBroken(object sender, EventArgs e) {
{
IsConnected = _internalCopter.IsConnected; private bool _fetchingFirmwareVersion;
}
private void _internalCopter_ConnectionBroken(object sender, EventArgs e)
private void _internalCopter_GetLogDataEvent(string log) {
{ IsConnected = _internalCopter.IsConnected;
//飞机消息日志，后面需要改为记录方式 }
StatusText =Name+":"+log;
var e = new MessageCreatedEventArgs { Message = StatusText }; private void _internalCopter_GetLogDataEvent(string log)
RaiseTextReceived(e); {
} //飞机消息日志，后面需要改为记录方式
StatusText =Name+":"+log;
private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType) var e = new MessageCreatedEventArgs { Message = StatusText };
{ RaiseTextReceived(e);
switch (StreamType) }
{
case MAVLINK_MSG_ID_GPS_RAW_INT: //需要
{ private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType)
SatCount = _internalCopter.satcount; {
GpsFixType = _internalCopter.gpsstatus.ToGpsFixType(); switch (StreamType)
GpsHdop = _internalCopter.gpshdop; {
Altitude = _internalCopter.gpsalt; case MAVLINK_MSG_ID_GPS_RAW_INT: //需要
Retain = BitConverter.GetBytes(_internalCopter.retain); {
SatCount = _internalCopter.satcount;
Voltage = _internalCopter.battery_voltage; GpsFixType = _internalCopter.gpsstatus.ToGpsFixType();
CommModuleMode = (FlightMode)_internalCopter.commModuleMode; GpsHdop = _internalCopter.gpshdop;
CommModuleVersion = _internalCopter.commModuleVersion; CopterErrorCode =_internalCopter.errorcode;
IsUnlocked = _internalCopter.isUnlocked; CopterPreCheckPass = _internalCopter.precheckok;
//Yaw = _internalCopter.yaw; CopterErrorString = getcoptererrorstr(_internalCopter.errorcode);
Heading = _internalCopter.heading; Altitude = _internalCopter.gpsalt;
HeartbeatCount++; Retain = BitConverter.GetBytes(_internalCopter.retain);
if (SatCount > 8) Voltage = _internalCopter.battery_voltage;
{ CommModuleMode = (FlightMode)_internalCopter.commModuleMode;
IsGpsAccurate = true; CommModuleVersion = _internalCopter.commModuleVersion;
RecordLat = _internalCopter.lat; IsUnlocked = _internalCopter.isUnlocked;
RecordLng = _internalCopter.lng; //Yaw = _internalCopter.yaw;
} Heading = _internalCopter.heading;
HeartbeatCount++;
Latitude = RecordLat;
Longitude = RecordLng; if (SatCount > 8)
UpdateFlightDataIfNeeded(); {
RaiseLocationChangedIfNeeded(); IsGpsAccurate = true;
RecordLat = _internalCopter.lat;
RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT); RecordLng = _internalCopter.lng;
}
break;
} Latitude = RecordLat;
case MAVLINK_MSG_ID_ATTITUDE: //无用 Longitude = RecordLng;
{ UpdateFlightDataIfNeeded();
Roll = _internalCopter.roll; RaiseLocationChangedIfNeeded();
Pitch = _internalCopter.pitch;
Yaw = _internalCopter.yaw; RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);
TimebootMs = _internalCopter.timebootms;
RaiseAttitudeChanged(); break;
}
break; case MAVLINK_MSG_ID_ATTITUDE: //无用
} {
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: //只用于显示,可以不要 Roll = _internalCopter.roll;
{ Pitch = _internalCopter.pitch;
Channel1 = _internalCopter.ch1in; Yaw = _internalCopter.yaw;
Channel2 = _internalCopter.ch2in; TimebootMs = _internalCopter.timebootms;
Channel3 = _internalCopter.ch3in; RaiseAttitudeChanged();
Channel4 = _internalCopter.ch4in;
Channel5 = _internalCopter.ch5in; break;
Channel6 = _internalCopter.ch6in; }
Channel7 = _internalCopter.ch7in; case MAVLINK_MSG_ID_RC_CHANNELS_RAW: //只用于显示,可以不要
Channel8 = _internalCopter.ch8in; {
Channel1 = _internalCopter.ch1in;
RaiseDataStreamReceived(PDataStreamType.RC_CHANNELS_RAW); Channel2 = _internalCopter.ch2in;
Channel3 = _internalCopter.ch3in;
break; Channel4 = _internalCopter.ch4in;
} Channel5 = _internalCopter.ch5in;
case MAVLINK_MSG_ID_VFR_HUD: Channel6 = _internalCopter.ch6in;
{ Channel7 = _internalCopter.ch7in;
Heading = _internalCopter.heading; //有用 Channel8 = _internalCopter.ch8in;
Altitude = _internalCopter.alt; //有用
AirSpeed = _internalCopter.airspeed; //没用 RaiseDataStreamReceived(PDataStreamType.RC_CHANNELS_RAW);
GroundSpeed = _internalCopter.groundspeed; //没用
break;
RaiseAltitudeChangedIfNeeded(); }
case MAVLINK_MSG_ID_VFR_HUD:
break; {
} Heading = _internalCopter.heading; //有用
} Altitude = _internalCopter.alt; //有用
} AirSpeed = _internalCopter.airspeed; //没用
GroundSpeed = _internalCopter.groundspeed; //没用
private async void _internalCopter_ReceiveHeartBearEvent(object sender, uint iError, ulong iCount)
{ RaiseAltitudeChangedIfNeeded();
IsUnlocked = _internalCopter.armed;
Mode = (FlightMode)_internalCopter.mode; break;
// 王海, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。 }
HasSwitchedToGpsMode = (byte)_internalCopter.apname != 0; }
++HeartbeatCount; }
IsCheckingConnection = false; private async void _internalCopter_ReceiveHeartBearEvent(object sender, uint iError, ulong iCount)
/* {
if (FirmwareVersion == null && !_fetchingFirmwareVersion) IsUnlocked = _internalCopter.armed;
{ Mode = (FlightMode)_internalCopter.mode;
try // 王海, 20160311, 将闲置的 apname 用作已经切过 GPS 模式的标志。
{ HasSwitchedToGpsMode = (byte)_internalCopter.apname != 0;
_fetchingFirmwareVersion = true; ++HeartbeatCount;
FirmwareVersion = (int)await GetParamAsync("FIRMWARE_VERSION", 5000);
} IsCheckingConnection = false;
catch (TimeoutException) /*
{ if (FirmwareVersion == null && !_fetchingFirmwareVersion)
// 吞掉。 {
} try
finally {
{ _fetchingFirmwareVersion = true;
_fetchingFirmwareVersion = false; FirmwareVersion = (int)await GetParamAsync("FIRMWARE_VERSION", 5000);
} }
} catch (TimeoutException)
*/ {
RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(iCount)); // 吞掉。
} }
finally
private void _internalCopter_ReceiveSensorEvent(object sender) {
{ _fetchingFirmwareVersion = false;
RaiseSensorDataReceived(EventArgs.Empty); }
} }
*/
private void _internalCopter_ReceiveSysStatusEvent(object sender, byte StreamType, bool GPSBadHealth, bool GyroBadHealth, bool AccelBadHealth, bool CompassBadHealth, bool BarometerBadHealth, bool canTakeOff) RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(iCount));
{ }
Voltage = _internalCopter.battery_voltage / 1000;
private void _internalCopter_ReceiveSensorEvent(object sender)
if (_internalCopter.battery_remaining == 255) // 剩余电量未知，需要计算。 {
{ RaiseSensorDataReceived(EventArgs.Empty);
CalcBatteryPer(); }
}
else private void _internalCopter_ReceiveSysStatusEvent(object sender, byte StreamType, bool GPSBadHealth, bool GyroBadHealth, bool AccelBadHealth, bool CompassBadHealth, bool BarometerBadHealth, bool canTakeOff)
{ {
BatteryPer = _internalCopter.battery_remaining; Voltage = _internalCopter.battery_voltage / 1000;
}
if (_internalCopter.battery_remaining == 255) // 剩余电量未知，需要计算。
const int MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD = 12308; {
const int MIN_VERSION_OF_GENERATION2 = 0x3000; CalcBatteryPer();
if (FirmwareVersion != null) }
{ else
if (FirmwareVersion.Value >= MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD) {
{ BatteryPer = _internalCopter.battery_remaining;
// 飞控提供了 canTakeOff。 }
IsGpsAccurate = canTakeOff;
} const int MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD = 12308;
else if (FirmwareVersion.Value >= MIN_VERSION_OF_GENERATION2) const int MIN_VERSION_OF_GENERATION2 = 0x3000;
{ if (FirmwareVersion != null)
// 2.0 飞行器，飞控未提供 canTakeOff。 {
IsGpsAccurate = SatCount >= 12; if (FirmwareVersion.Value >= MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD)
} {
else // 飞控提供了 canTakeOff。
{ IsGpsAccurate = canTakeOff;
// 1.0 飞行器，飞控不提供 canTakeOff。 }
IsGpsAccurate = SatCount >= 6; else if (FirmwareVersion.Value >= MIN_VERSION_OF_GENERATION2)
} {
} // 2.0 飞行器，飞控未提供 canTakeOff。
else IsGpsAccurate = SatCount >= 12;
IsGpsAccurate = SatCount >= 8; }
else
FlightDistance2D = _internalCopter.FlightDistance2D; {
// 1.0 飞行器，飞控不提供 canTakeOff。
RaiseSysStatusReceived(new SystemStatusReceivedEventArgs(GPSBadHealth, GyroBadHealth, AccelBadHealth, CompassBadHealth, BarometerBadHealth)); IsGpsAccurate = SatCount >= 6;
} }
}
private void _internalCopter_PacketHandled(object sender, PlaneCopter.PacketReceivedEventArgs e) else
{ IsGpsAccurate = SatCount >= 8;
switch (e.Packet[5])
{ FlightDistance2D = _internalCopter.FlightDistance2D;
case MAVLINK_MSG_ID_MISSION_COUNT:
MissionCount = _internalCopter.WpCount; RaiseSysStatusReceived(new SystemStatusReceivedEventArgs(GPSBadHealth, GyroBadHealth, AccelBadHealth, CompassBadHealth, BarometerBadHealth));
break; }
default: private void _internalCopter_PacketHandled(object sender, PlaneCopter.PacketReceivedEventArgs e)
break; {
} switch (e.Packet[5])
} {
case MAVLINK_MSG_ID_MISSION_COUNT:
private void _internalCopter_UnhandledPacketReceived(object sender, PlaneCopter.PacketReceivedEventArgs e) MissionCount = _internalCopter.WpCount;
{ break;
switch (e.Packet[5])
{ default:
case MAVLINK_MSG_ID_MISSION_ACK: break;
AnalyzeMissionAckPacket(e.Packet); }
break; }
case MAVLINK_MSG_ID_MISSION_COUNT: private void _internalCopter_UnhandledPacketReceived(object sender, PlaneCopter.PacketReceivedEventArgs e)
AnalyzeMissionCountPacket(e.Packet); {
break; switch (e.Packet[5])
{
case MAVLINK_MSG_ID_MISSION_ITEM: case MAVLINK_MSG_ID_MISSION_ACK:
AnalyzeMissionItemPacket(e.Packet); AnalyzeMissionAckPacket(e.Packet);
break; break;
case MAVLINK_MSG_ID_MISSION_REQUEST: case MAVLINK_MSG_ID_MISSION_COUNT:
AnalyzeMissionRequestPacket(e.Packet); AnalyzeMissionCountPacket(e.Packet);
break; break;
case MAVLINK_MSG_ID_SET_PAIR: case MAVLINK_MSG_ID_MISSION_ITEM:
AnalyzeSetPairPacket(e.Packet); AnalyzeMissionItemPacket(e.Packet);
break; break;
default: case MAVLINK_MSG_ID_MISSION_REQUEST:
break; AnalyzeMissionRequestPacket(e.Packet);
} break;
}
case MAVLINK_MSG_ID_SET_PAIR:
#if PRIVATE AnalyzeSetPairPacket(e.Packet);
protected virtual void RegisterInternalCopterEventHandlers() break;
#else
default:
private void RegisterInternalCopterEventHandlers() break;
#endif }
{ }
_internalCopter.GetLogDataEvent += _internalCopter_GetLogDataEvent;
_internalCopter.ReceiveDataStreamEvent += _internalCopter_ReceiveDataStreamEvent; #if PRIVATE
_internalCopter.ReceiveHeartBearEvent += _internalCopter_ReceiveHeartBearEvent; protected virtual void RegisterInternalCopterEventHandlers()
_internalCopter.ReceiveSensorEvent += _internalCopter_ReceiveSensorEvent; #else
_internalCopter.ReceiveSysStatusEvent += _internalCopter_ReceiveSysStatusEvent;
_internalCopter.ConnectionBroken += _internalCopter_ConnectionBroken; private void RegisterInternalCopterEventHandlers()
_internalCopter.PacketHandled += _internalCopter_PacketHandled; #endif
_internalCopter.UnhandledPacketReceived += _internalCopter_UnhandledPacketReceived; {
_internalCopter.MessageCreated += _internalCopter_MessageCreated; _internalCopter.GetLogDataEvent += _internalCopter_GetLogDataEvent;
} _internalCopter.ReceiveDataStreamEvent += _internalCopter_ReceiveDataStreamEvent;
_internalCopter.ReceiveHeartBearEvent += _internalCopter_ReceiveHeartBearEvent;
private void _internalCopter_MessageCreated(object sender, MessageCreatedEventArgs e) _internalCopter.ReceiveSensorEvent += _internalCopter_ReceiveSensorEvent;
{ _internalCopter.ReceiveSysStatusEvent += _internalCopter_ReceiveSysStatusEvent;
var txte = new MessageCreatedEventArgs { Message = Name + ":" +e.Message }; _internalCopter.ConnectionBroken += _internalCopter_ConnectionBroken;
RaiseTextReceived(txte); _internalCopter.PacketHandled += _internalCopter_PacketHandled;
} _internalCopter.UnhandledPacketReceived += _internalCopter_UnhandledPacketReceived;
_internalCopter.MessageCreated += _internalCopter_MessageCreated;
#region 计算剩余电量 }
private int bPerTimes; private void _internalCopter_MessageCreated(object sender, MessageCreatedEventArgs e)
private int outBatteryPer; {
private int[] tPerTimes = new int[20]; var txte = new MessageCreatedEventArgs { Message = Name + ":" +e.Message };
private int v_num; RaiseTextReceived(txte);
}
/// <summary>
/// 计算电量并在条件满足时更新 <see cref="BatteryPer"/> 属性。 #region 计算剩余电量
/// </summary>
private void CalcBatteryPer() private int bPerTimes;
{ private int outBatteryPer;
float volmax = 0f; private int[] tPerTimes = new int[20];
float volmin = 0f; private int v_num;
if (Voltage > 5 && Voltage < 9)
{ /// <summary>
volmax = 8.2f; /// 计算电量并在条件满足时更新 <see cref="BatteryPer"/> 属性。
volmin = 7f; /// </summary>
} private void CalcBatteryPer()
else if (Voltage >= 9 {
&& Voltage < 13.6) float volmax = 0f;
{ float volmin = 0f;
volmax = 11.6f; if (Voltage > 5 && Voltage < 9)
volmin = 10.2f; {
} volmax = 8.2f;
else if (Voltage >= 13.6 volmin = 7f;
&& Voltage < 17.2) }
{ else if (Voltage >= 9
volmax = 16.3f; && Voltage < 13.6)
volmin = 14.2f; {
} volmax = 11.6f;
else if (Voltage >= 17.2 volmin = 10.2f;
&& Voltage < 26.2) }
{ else if (Voltage >= 13.6
volmax = 24.8f; && Voltage < 17.2)
volmin = 21.2f; {
} volmax = 16.3f;
volmin = 14.2f;
int batteryPer = (int)Math.Round((Voltage - volmin) * 100 / (volmax - volmin)); }
else if (Voltage >= 17.2
if (batteryPer == -1 || volmax == 0 || volmin == 0) && Voltage < 26.2)
return; {
volmax = 24.8f;
if (bPerTimes < 20) volmin = 21.2f;
{ }
tPerTimes[bPerTimes] = batteryPer;
bPerTimes += 1; int batteryPer = (int)Math.Round((Voltage - volmin) * 100 / (volmax - volmin));
}
else if (batteryPer == -1 || volmax == 0 || volmin == 0)
{ return;
tPerTimes[v_num] = batteryPer;
v_num++; if (bPerTimes < 20)
if (v_num == 20) {
v_num = 0; tPerTimes[bPerTimes] = batteryPer;
} bPerTimes += 1;
for (int i = 0; i < bPerTimes; i++) }
{ else
outBatteryPer += tPerTimes[i]; {
} tPerTimes[v_num] = batteryPer;
v_num++;
outBatteryPer = outBatteryPer / bPerTimes; if (v_num == 20)
if (outBatteryPer < BatteryPer && bPerTimes > 18) v_num = 0;
{ }
BatteryPer = (byte)outBatteryPer; for (int i = 0; i < bPerTimes; i++)
} {
} outBatteryPer += tPerTimes[i];
}
#endregion 计算剩余电量
} 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 _takeoffcentloc;
private PLLocation _taskcentloc; private PLLocation _taskcentloc;
private float _mindistance; private float _mindistance;
private int _rettime; private float _rettime;
private bool _descending; private bool _descending;
int Emergency_Retstatus = 0; int Emergency_Retstatus = 0;
DateTime EmergencyRetTime;
//返航路径第一个航点 //返航路径第一个航点
PLLocation Emergency_firstloc; PLLocation Emergency_firstloc;
@ -290,45 +291,42 @@ namespace Plane.Copters
IsCheckingConnection = false; IsCheckingConnection = false;
return TaskUtils.CompletedTask; return TaskUtils.CompletedTask;
} }
private int minretalt=15; //最低返航高度
private int minretalt_first = 25; //第一次返航高度
//返航位置1
private PLLocation EmergencyRetPLLocation1;
private void BuildPath() private void BuildPath()
{ {
float taralt; //计算当前位置和起飞中心点的距离
//返航高度参数 float dis = (float)this.CalcDistance(_takeoffcentloc.Latitude, _takeoffcentloc.Longitude, 0);
float retalt = 15; _rettime = (dis - _mindistance) * 1.0f;
if (_descending) //计算方向--角度
{ float takeoffdir =(float)_takeoffcentloc.CalcDirection2D(TakeOffPoint)* 57.29578f;
if (Altitude <= retalt) taralt = Altitude; //计算起飞点距离
else float takeoffdis = (float)TakeOffPoint.CalcDistance2D(_takeoffcentloc.Latitude, _takeoffcentloc.Longitude);
{
float maxalt = Altitude - retalt;
//降低一个随机高度，随机高度介于0和maxalt之间，也就是目标高度不要低于retalt, 防止飞机在同一高度飞行 ILocation2D take1 = TakeOffPoint.CalcLatLngSomeMetersAway2D(takeoffdir, takeoffdis);
taralt = (float)(Altitude - (new Random().NextDouble() * maxalt)); //第一次返航点
} EmergencyRetPLLocation1 = new PLLocation(take1.Latitude, take1.Longitude, minretalt_first);
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;
}
} }
public Task EmergencyRetAsync(PLLocation takeoffcentloc, PLLocation taskcentloc, float mindistance, int rettime, bool descending) public Task EmergencyRetAsync(PLLocation takeoffcentloc, PLLocation taskcentloc, float mindistance, int rettime, bool descending)
{ {
_takeoffcentloc = takeoffcentloc; _takeoffcentloc = takeoffcentloc;
_taskcentloc = taskcentloc; _taskcentloc = taskcentloc;
_mindistance = mindistance; _mindistance = mindistance;
_rettime = rettime; // _rettime = rettime;
_descending = descending; _descending = descending;
Emergency_Retstatus = 0; Emergency_Retstatus = 0;
EmergencyRetTime = DateTime.Now;
//计算返航路径 //计算返航路径
BuildPath(); BuildPath();
Mode = FlightMode.EMERG_RTL; Mode = FlightMode.EMERG_RTL;
@ -531,6 +529,9 @@ namespace Plane.Copters
if (flightDistance != null) FlightDistance = flightDistance.Value; if (flightDistance != null) FlightDistance = flightDistance.Value;
if (flightDistance2D != null) FlightDistance2D = flightDistance2D.Value; if (flightDistance2D != null) FlightDistance2D = flightDistance2D.Value;
if (flightTimeSpan != null) FlightTimeSpan = flightTimeSpan.Value; if (flightTimeSpan != null) FlightTimeSpan = flightTimeSpan.Value;
CopterPreCheckPass = true;
//CopterErrorCode = 2;
//CopterErrorString = getcoptererrorstr(CopterErrorCode);
} }
public Task StartPairingAsync() 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) public Task MissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
{ {
MissionStartTime = DateTime.Now; MissionStartTime = DateTime.Now;
TakeOffPoint = new PLLocation(Missionlat, Missionlng, 0); TakeOffPoint = new PLLocation(this.Latitude, this.Longitude, 0);
return TaskUtils.CompletedTask; return TaskUtils.CompletedTask;
} }
@ -799,7 +800,7 @@ namespace Plane.Copters
case FlightMode.LAND: case FlightMode.LAND:
// 王海, 20160317, 降落时也能体感控制若干通道。 // 王海, 20160317, 降落时也能体感控制若干通道。
UpdateWithChannels(); // UpdateWithChannels();
// 以最大速度降落，直到高度为 0。 // 以最大速度降落，直到高度为 0。
if (Altitude > 0) if (Altitude > 0)
{ {
@ -826,42 +827,59 @@ namespace Plane.Copters
switch(Emergency_Retstatus) switch(Emergency_Retstatus)
{ {
case 0: //等待返航 case 0: //等待返航
if ((DateTime.Now- MissionStartTime).TotalSeconds > _rettime) if ((DateTime.Now - EmergencyRetTime).TotalSeconds > _rettime)
{ {
Emergency_Retstatus = 1; Emergency_Retstatus = 1;
//平飞或降落随机距离 //平飞或降落随机距离
FlyToCoreAsync(Emergency_firstloc.Latitude, Emergency_firstloc.Longitude, Emergency_firstloc.Altitude); //直接返航
//FlyToCoreAsync里面把模式改成了GUIDED，这里改回来 FlyToCoreAsync(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt);
Mode = FlightMode.EMERG_RTL; //先到第一返航点再到起飞点上空
} //FlyToCoreAsync(EmergencyRetPLLocation1.Latitude, EmergencyRetPLLocation1.Longitude, EmergencyRetPLLocation1.Altitude);
break;
case 1: //返航阶段一:等待平飞或降落随机距离并到达,然后飞到起飞点上空
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，这里改回来 //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
Mode = FlightMode.EMERG_RTL; Mode = FlightMode.EMERG_RTL;
Emergency_Retstatus = 2;
} }
break;
break;
case 2: //返航阶段二:等待到达起飞点上空,然后降落
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; Mode = FlightMode.LAND;
//FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
Emergency_Retstatus = 3; 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; break;
case 3: //降落 case 3: //降落
break; break;
} }
break; break;

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

@ -1,402 +1,404 @@
//#define LOG_PACKETS //#define LOG_PACKETS
using Plane.Protocols; using Plane.Protocols;
using System; using System;
using System.Diagnostics; using System.Diagnostics;
using System.Text; using System.Text;
using System.Threading.Tasks; using System.Threading.Tasks;
using TaskTools.Utilities; using TaskTools.Utilities;
#if LOG_PACKETS #if LOG_PACKETS
using System.Diagnostics; using System.Diagnostics;
#endif #endif
namespace Plane.Copters namespace Plane.Copters
{ {
partial class PlaneCopter partial class PlaneCopter
{ {
private void AnalyzeAttitudePacket(byte[] buffer) private void AnalyzeAttitudePacket(byte[] buffer)
{ {
var att = buffer.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6); var att = buffer.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6);
roll = att.roll * rad2deg; roll = att.roll * rad2deg;
pitch = att.pitch * rad2deg; pitch = att.pitch * rad2deg;
yaw = att.yaw * rad2deg; yaw = att.yaw * rad2deg;
timebootms = att.time_boot_ms; timebootms = att.time_boot_ms;
RaiseReceiveDataStreamEventOnUIThread(buffer[5]); RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
} }
private void AnalyzeGlobalPositionIntPacket(byte[] buffer) private void AnalyzeGlobalPositionIntPacket(byte[] buffer)
{ {
var loc = buffer.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6); var loc = buffer.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);
useLocation = true; useLocation = true;
if (loc.lat == 0 && loc.lon == 0) if (loc.lat == 0 && loc.lon == 0)
{ {
useLocation = false; useLocation = false;
} }
else else
{ {
gpsalt = loc.alt / 1000.0f; gpsalt = loc.alt / 1000.0f;
lat = loc.lat / 10000000.0; lat = loc.lat / 10000000.0;
lng = loc.lon / 10000000.0; lng = loc.lon / 10000000.0;
} }
RaiseReceiveDataStreamEventOnUIThread(buffer[5]); RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
} }
#if LOG_GPS_RAW_INT #if LOG_GPS_RAW_INT
private Stopwatch _lastGpsRawIntPacketTime = new Stopwatch(); private Stopwatch _lastGpsRawIntPacketTime = new Stopwatch();
private TimeSpan _lastGpsRawIntPacketTimeSpan; private TimeSpan _lastGpsRawIntPacketTimeSpan;
#endif #endif
private void AnalyzeGpsRawIntPacket(byte[] buffer) private void AnalyzeGpsRawIntPacket(byte[] buffer)
{ {
#if LOG_GPS_RAW_INT #if LOG_GPS_RAW_INT
if (!_lastGpsRawIntPacketTime.IsRunning) if (!_lastGpsRawIntPacketTime.IsRunning)
{ {
_lastGpsRawIntPacketTime.Start(); _lastGpsRawIntPacketTime.Start();
} }
else else
{ {
var elapsed = _lastGpsRawIntPacketTime.Elapsed; var elapsed = _lastGpsRawIntPacketTime.Elapsed;
Debug.WriteLine(elapsed - _lastGpsRawIntPacketTimeSpan, "----------------------------GpsRawInt"); Debug.WriteLine(elapsed - _lastGpsRawIntPacketTimeSpan, "----------------------------GpsRawInt");
_lastGpsRawIntPacketTimeSpan = elapsed; _lastGpsRawIntPacketTimeSpan = elapsed;
} }
#endif #endif
var gps = buffer.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6); var gps = buffer.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6);
long mylat = BitConverter.ToInt32(buffer, 14); long mylat = BitConverter.ToInt32(buffer, 14);
long mylong = BitConverter.ToInt32(buffer, 18); long mylong = BitConverter.ToInt32(buffer, 18);
//if (!useLocation) //if (!useLocation)
//{ //{
lat = gps.lat * 1.0e-7; lat = gps.lat * 1.0e-7;
lng = gps.lon * 1.0e-7; lng = gps.lon * 1.0e-7;
//} //}
gpsalt = gps.alt * 1.0e-7f; // using vfr as includes baro calc 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 gpsstatus = gps.fix_type; //0-1: no fix, 2: 2D fix, 3: 3D fix
gpshdop = (float)Math.Round(gps.eph / 100.0, 2); gpshdop = (float)Math.Round(gps.eph / 100.0, 2);
satcount = gps.satellites_visible; satcount = gps.satellites_visible;
groundspeed = gps.vel * 1.0e-2f; groundspeed = gps.vel * 1.0e-2f;
groundcourse = gps.cog * 1.0e-2f; groundcourse = gps.cog * 1.0e-2f;
RaiseReceiveDataStreamEventOnUIThread(buffer[5]); RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
} }
#if LOG_PACKETS #if LOG_PACKETS
private DateTime _lastHeartbeatTime = DateTime.MinValue; private DateTime _lastHeartbeatTime = DateTime.MinValue;
#endif #endif
private void AnalyzeHeartbeatPacket(byte[] buffer) private void AnalyzeHeartbeatPacket(byte[] buffer)
{ {
#if LOG_PACKETS #if LOG_PACKETS
var now = DateTime.Now; var now = DateTime.Now;
if (_lastHeartbeatTime != DateTime.MinValue) if (_lastHeartbeatTime != DateTime.MinValue)
{ {
Debug.WriteLine(now - _lastHeartbeatTime, "心跳间隔"); Debug.WriteLine(now - _lastHeartbeatTime, "心跳间隔");
} }
_lastHeartbeatTime = now; _lastHeartbeatTime = now;
#endif #endif
DataTimeOut = 0; DataTimeOut = 0;
var hb = buffer.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6); var hb = buffer.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
mavlinkversion = hb.mavlink_version; mavlinkversion = hb.mavlink_version;
aptype = (MAVLink.MAV_TYPE)hb.type; aptype = (MAVLink.MAV_TYPE)hb.type;
apname = (MAVLink.MAV_AUTOPILOT)hb.autopilot; apname = (MAVLink.MAV_AUTOPILOT)hb.autopilot;
sysid = buffer[3]; sysid = buffer[3];
compid = buffer[4]; compid = buffer[4];
recvpacketcount = buffer[2]; recvpacketcount = buffer[2];
armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED; 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; failsafe = hb.system_status == (byte)MAVLink.MAV_STATE.CRITICAL;
mode = hb.custom_mode; mode = hb.custom_mode;
var handler = ReceiveHeartBearEvent; var handler = ReceiveHeartBearEvent;
if (handler != null) if (handler != null)
{ {
RunOnUIThread(() => RunOnUIThread(() =>
{ {
handler(this, iErrorData, iDataCount); handler(this, iErrorData, iDataCount);
}); });
} }
if (_autoRequestData && mavlinkversion == 3 && SendReq == false) if (_autoRequestData && mavlinkversion == 3 && SendReq == false)
{ {
SendReq = true; SendReq = true;
Task.Run(GetCopterDataAsync); Task.Run(GetCopterDataAsync);
} }
} }
private void AnalyzeMemInfoPacket(byte[] buffer) private void AnalyzeMemInfoPacket(byte[] buffer)
{ {
var mem = buffer.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6); var mem = buffer.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
freemem = mem.freemem; freemem = mem.freemem;
brklevel = mem.brkval; brklevel = mem.brkval;
RaiseReceiveDataStreamEventOnUIThread(buffer[5]); RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
} }
private void AnalyzeMissionCurrentPacket(byte[] buffer) private void AnalyzeMissionCurrentPacket(byte[] buffer)
{ {
var wpcur = buffer.ByteArrayToStructure<MAVLink.mavlink_mission_current_t>(6); var wpcur = buffer.ByteArrayToStructure<MAVLink.mavlink_mission_current_t>(6);
RaiseReceiveDataStreamEventOnUIThread(buffer[5]); RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
} }
private void AnalyzeNavControllerOutputPacket(byte[] buffer) private void AnalyzeNavControllerOutputPacket(byte[] buffer)
{ {
var nav = buffer.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6); var nav = buffer.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6);
nav_roll = nav.nav_roll; nav_roll = nav.nav_roll;
nav_pitch = nav.nav_pitch; nav_pitch = nav.nav_pitch;
nav_bearing = nav.nav_bearing; nav_bearing = nav.nav_bearing;
target_bearing = nav.target_bearing; target_bearing = nav.target_bearing;
wp_dist = nav.wp_dist; wp_dist = nav.wp_dist;
alt_error = nav.alt_error; alt_error = nav.alt_error;
aspd_error = nav.aspd_error / 100.0f; aspd_error = nav.aspd_error / 100.0f;
xtrack_error = nav.xtrack_error; xtrack_error = nav.xtrack_error;
RaiseReceiveDataStreamEventOnUIThread(buffer[5]); RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
} }
private void AnalyzeParamValuePacket(byte[] buffer) private void AnalyzeParamValuePacket(byte[] buffer)
{ {
var par = buffer.ByteArrayToStructure<MAVLink.mavlink_param_value_t>(6); var par = buffer.ByteArrayToStructure<MAVLink.mavlink_param_value_t>(6);
param_total = (par.param_count); param_total = (par.param_count);
string paramID = Encoding.ASCII.GetString(par.param_id); string paramID = Encoding.ASCII.GetString(par.param_id);
int pos = paramID.IndexOf('\0'); int pos = paramID.IndexOf('\0');
if (pos != -1) if (pos != -1)
{ {
paramID = paramID.Substring(0, pos); paramID = paramID.Substring(0, pos);
} }
try try
{ {
param[paramID] = (par.param_value); param[paramID] = (par.param_value);
param_types[paramID] = (MAVLink.MAV_PARAM_TYPE)par.param_type; param_types[paramID] = (MAVLink.MAV_PARAM_TYPE)par.param_type;
} }
catch (IndexOutOfRangeException ex) catch (IndexOutOfRangeException ex)
{ {
Debug.WriteLine(ex); Debug.WriteLine(ex);
} }
var handler = ReceiveParamEvent; var handler = ReceiveParamEvent;
if (handler != null) if (handler != null)
{ {
RunOnUIThread(() => RunOnUIThread(() =>
{ {
handler(this, paramID, par.param_index, par.param_count); handler(this, paramID, par.param_index, par.param_count);
}); });
} }
} }
private void AnalyzeRadioPacket(byte[] buffer) private void AnalyzeRadioPacket(byte[] buffer)
{ {
var radio = buffer.ByteArrayToStructure<MAVLink.mavlink_radio_t>(6); 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); //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; remrssi = radio.remrssi;
rssi = radio.rssi; rssi = radio.rssi;
RaiseReceiveDataStreamEventOnUIThread(buffer[5]); RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
} }
private void AnalyzeRawImuPacket(byte[] buffer) private void AnalyzeRawImuPacket(byte[] buffer)
{ {
var imu = buffer.ByteArrayToStructure<MAVLink.mavlink_raw_imu_t>(6); var imu = buffer.ByteArrayToStructure<MAVLink.mavlink_raw_imu_t>(6);
gx = imu.xgyro; gx = imu.xgyro;
gy = imu.ygyro; gy = imu.ygyro;
gz = imu.zgyro; gz = imu.zgyro;
ax = imu.xacc; ax = imu.xacc;
ay = imu.yacc; ay = imu.yacc;
az = imu.zacc; az = imu.zacc;
mx = imu.xmag; mx = imu.xmag;
my = imu.ymag; my = imu.ymag;
mz = imu.zmag; mz = imu.zmag;
var handler = ReceiveSensorEvent; var handler = ReceiveSensorEvent;
if (handler != null) if (handler != null)
{ {
RunOnUIThread(() => RunOnUIThread(() =>
{ {
handler(this); handler(this);
}); });
} }
} }
private void AnalyzeRCChannelsRawPacket(byte[] buffer) private void AnalyzeRCChannelsRawPacket(byte[] buffer)
{ {
var rcin = buffer.ByteArrayToStructure<MAVLink.mavlink_rc_channels_raw_t>(6); var rcin = buffer.ByteArrayToStructure<MAVLink.mavlink_rc_channels_raw_t>(6);
ch1in = rcin.chan1_raw; ch1in = rcin.chan1_raw;
ch2in = rcin.chan2_raw; ch2in = rcin.chan2_raw;
ch3in = rcin.chan3_raw; ch3in = rcin.chan3_raw;
ch4in = rcin.chan4_raw; ch4in = rcin.chan4_raw;
ch5in = rcin.chan5_raw; ch5in = rcin.chan5_raw;
ch6in = rcin.chan6_raw; ch6in = rcin.chan6_raw;
ch7in = rcin.chan7_raw; ch7in = rcin.chan7_raw;
ch8in = rcin.chan8_raw; ch8in = rcin.chan8_raw;
if (bInitChannel == false) if (bInitChannel == false)
{ {
bInitChannel = true; bInitChannel = true;
} }
//percent //percent
rxrssi = (float)((rcin.rssi / 255.0) * 100.0); rxrssi = (float)((rcin.rssi / 255.0) * 100.0);
RaiseReceiveDataStreamEventOnUIThread(buffer[5]); RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
} }
private void AnalyzeScaledImu2Packet(byte[] buffer) private void AnalyzeScaledImu2Packet(byte[] buffer)
{ {
var imu2 = buffer.ByteArrayToStructure<MAVLink.mavlink_scaled_imu2_t>(6); var imu2 = buffer.ByteArrayToStructure<MAVLink.mavlink_scaled_imu2_t>(6);
gx2 = imu2.xgyro; gx2 = imu2.xgyro;
gy2 = imu2.ygyro; gy2 = imu2.ygyro;
gz2 = imu2.zgyro; gz2 = imu2.zgyro;
ax2 = imu2.xacc; ax2 = imu2.xacc;
ay2 = imu2.yacc; ay2 = imu2.yacc;
az2 = imu2.zacc; az2 = imu2.zacc;
mx2 = imu2.xmag; mx2 = imu2.xmag;
my2 = imu2.ymag; my2 = imu2.ymag;
mz2 = imu2.zmag; mz2 = imu2.zmag;
var handler = ReceiveScaleImu2Event; var handler = ReceiveScaleImu2Event;
if (handler != null) if (handler != null)
{ {
RunOnUIThread(() => handler(this)); RunOnUIThread(() => handler(this));
} }
} }
private void AnalyzeSensorOffsetsPacket(byte[] buffer) private void AnalyzeSensorOffsetsPacket(byte[] buffer)
{ {
var sensofs = buffer.ByteArrayToStructure<MAVLink.mavlink_sensor_offsets_t>(6); var sensofs = buffer.ByteArrayToStructure<MAVLink.mavlink_sensor_offsets_t>(6);
mag_ofs_x = sensofs.mag_ofs_x; mag_ofs_x = sensofs.mag_ofs_x;
mag_ofs_y = sensofs.mag_ofs_y; mag_ofs_y = sensofs.mag_ofs_y;
mag_ofs_z = sensofs.mag_ofs_z; mag_ofs_z = sensofs.mag_ofs_z;
mag_declination = sensofs.mag_declination; mag_declination = sensofs.mag_declination;
raw_press = sensofs.raw_press; raw_press = sensofs.raw_press;
raw_temp = sensofs.raw_temp; raw_temp = sensofs.raw_temp;
gyro_cal_x = sensofs.gyro_cal_x; gyro_cal_x = sensofs.gyro_cal_x;
gyro_cal_y = sensofs.gyro_cal_y; gyro_cal_y = sensofs.gyro_cal_y;
gyro_cal_z = sensofs.gyro_cal_z; gyro_cal_z = sensofs.gyro_cal_z;
accel_cal_x = sensofs.accel_cal_x; accel_cal_x = sensofs.accel_cal_x;
accel_cal_y = sensofs.accel_cal_y; accel_cal_y = sensofs.accel_cal_y;
accel_cal_z = sensofs.accel_cal_z; accel_cal_z = sensofs.accel_cal_z;
var handler = ReceiveSensorEvent; var handler = ReceiveSensorEvent;
if (handler != null) if (handler != null)
{ {
RunOnUIThread(() => RunOnUIThread(() =>
{ {
handler(this); handler(this);
}); });
} }
} }
private void AnalyzeStatusTextPacket(byte[] buffer) private void AnalyzeStatusTextPacket(byte[] buffer)
{ {
var stext = buffer.ByteArrayToStructure<MAVLink.mavlink_statustext_t>(6); var stext = buffer.ByteArrayToStructure<MAVLink.mavlink_statustext_t>(6);
var length = Array.IndexOf<byte>(stext.text, 0); var length = Array.IndexOf<byte>(stext.text, 0);
var log = Encoding.ASCII.GetString(stext.text, 0, length); var log = Encoding.ASCII.GetString(stext.text, 0, length);
var handler = GetLogDataEvent; var handler = GetLogDataEvent;
if (handler != null) if (handler != null)
{ {
RunOnUIThread(() => handler(log)); RunOnUIThread(() => handler(log));
} }
} }
private void AnalyzeSysStatusPacket(byte[] buffer) private void AnalyzeSysStatusPacket(byte[] buffer)
{ {
var sysstatus = buffer.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6); var sysstatus = buffer.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
battery_voltage = sysstatus.voltage_battery; battery_voltage = sysstatus.voltage_battery;
battery_remaining = sysstatus.battery_remaining; battery_remaining = sysstatus.battery_remaining;
current_battery = sysstatus.current_battery / 100.0f; current_battery = sysstatus.current_battery / 100.0f;
var sensors_enabled = new Mavlink_Sensors(sysstatus.onboard_control_sensors_enabled); var sensors_enabled = new Mavlink_Sensors(sysstatus.onboard_control_sensors_enabled);
var sensors_health = new Mavlink_Sensors(sysstatus.onboard_control_sensors_health); var sensors_health = new Mavlink_Sensors(sysstatus.onboard_control_sensors_health);
if (sensors_health.gps != sensors_enabled.gps) if (sensors_health.gps != sensors_enabled.gps)
{ {
bGPSBadHealth = true; bGPSBadHealth = true;
} }
else else
{ {
bGPSBadHealth = false; bGPSBadHealth = false;
} }
if (sensors_health.gyro != sensors_enabled.gyro) if (sensors_health.gyro != sensors_enabled.gyro)
{ {
bGyroBadHealth = true; bGyroBadHealth = true;
} }
else else
{ {
bGyroBadHealth = false; bGyroBadHealth = false;
} }
if (sensors_health.accelerometer != sensors_enabled.accelerometer) if (sensors_health.accelerometer != sensors_enabled.accelerometer)
{ {
bAccel = true; bAccel = true;
} }
else else
{ {
bAccel = false; bAccel = false;
} }
if (sensors_health.compass != sensors_enabled.compass) if (sensors_health.compass != sensors_enabled.compass)
{ {
bCompass = true; bCompass = true;
} }
else else
{ {
bCompass = false; bCompass = false;
} }
if (sensors_health.barometer != sensors_enabled.barometer) if (sensors_health.barometer != sensors_enabled.barometer)
{ {
bBarometer = true; bBarometer = true;
} }
else else
{ {
bBarometer = false; bBarometer = false;
} }
FlightDistance2D = sysstatus.errors_count2; FlightDistance2D = sysstatus.errors_count2;
var handler = ReceiveSysStatusEvent; var handler = ReceiveSysStatusEvent;
if (handler != null) if (handler != null)
{ {
RunOnUIThread(() => RunOnUIThread(() =>
{ {
handler(this, buffer[5], bGPSBadHealth, bGyroBadHealth, bAccel, bCompass, bBarometer, sensors_health.CanTakeOff); handler(this, buffer[5], bGPSBadHealth, bGyroBadHealth, bAccel, bCompass, bBarometer, sensors_health.CanTakeOff);
}); });
} }
} }
private void AnalyzeVfrHudPacket(byte[] buffer) private void AnalyzeVfrHudPacket(byte[] buffer)
{ {
var vfr = buffer.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6); var vfr = buffer.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6);
groundspeed = vfr.groundspeed; groundspeed = vfr.groundspeed;
airspeed = vfr.airspeed; airspeed = vfr.airspeed;
altorigin = vfr.alt; // this might include baro altorigin = vfr.alt; // this might include baro
ch3percent = vfr.throttle; ch3percent = vfr.throttle;
heading = vfr.heading; heading = vfr.heading;
RaiseReceiveDataStreamEventOnUIThread(buffer[5]); RaiseReceiveDataStreamEventOnUIThread(buffer[5]);
} }
/// <summary> /// <summary>
/// 处理通信模块发过来的数据 /// 飞控返回数据 处理通信模块发过来的28个字节数据
/// </summary> /// </summary>
/// <param name="buffer"></param> /// <param name="buffer"></param>
public void AnalyzeCommMouldePositionIntPacket(byte[] buffer, byte version) public void AnalyzeCommMouldePositionIntPacket(byte[] buffer, byte version)
{ {
var info = buffer.ByteArrayToStructure<MavComm.comm_copter_info>(0); var info = buffer.ByteArrayToStructure<MavComm.comm_copter_info>(0);
lat = info.lat * 1.0e-7; lat = info.lat * 1.0e-7;
lng = info.lng * 1.0e-7; lng = info.lng * 1.0e-7;
commModuleMode = (uint)info.control_mode; commModuleMode = (uint)info.control_mode;
gpsstatus = (byte)info.gps_status; gpsstatus = (byte)info.gps_status;
gpsalt = ((float)info.alt) / 1000; errorcode= (byte)info.error_code;
satcount = info.gps_num_sats; precheckok = info.rpecheck_fault == 0;
isUnlocked = info.lock_status == 1; gpsalt = ((float)info.alt) / 1000;
satcount = info.gps_num_sats;
battery_voltage = ((float)info.battery_voltage) / 1000; isUnlocked = info.lock_status == 1;
heading = (short)((info.heading / 100) % 360); battery_voltage = ((float)info.battery_voltage) / 1000;
commModuleVersion = version; heading = (short)((info.heading / 100) % 360);
retain = info.retain; commModuleVersion = version;
RaiseReceiveDataStreamEventOnUIThread(MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT);
} 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;
using System.Collections.Generic; using System.Collections.Generic;
using System.Runtime.InteropServices; using System.Runtime.InteropServices;
using System.Text; using System.Text;
namespace Plane.Protocols namespace Plane.Protocols
{ {
/* /*
* *
* 通信模块协议 * 通信模块协议
* *
*/ */
public class MavComm public class MavComm
{ {
/// <summary> /// <summary>
/// 发送数据时的数据包头 /// 发送数据时的数据包头
/// </summary> /// </summary>
public const ushort COMM_SEND_PACKET_HEADER = 0x9551; public const ushort COMM_SEND_PACKET_HEADER = 0x9551;
/// <summary> /// <summary>
/// 接受数据时的数据包头 /// 接受数据时的数据包头
/// </summary> /// </summary>
public const ushort COMM_RECEIVE_PACKET_HEADER = 0x7713; public const ushort COMM_RECEIVE_PACKET_HEADER = 0x7713;
#region 命令类型 2字节 #region 命令类型 2字节
//-----------------命令类型----------------- //-----------------命令类型-----------------
/// <summary> /// <summary>
/// 查询状态 /// 查询状态
/// </summary> /// </summary>
public const short COMM_QUERY_STATUS = 0x00; public const short COMM_QUERY_STATUS = 0x00;
/// <summary> /// <summary>
/// 变更飞机总数及参与者 /// 变更飞机总数及参与者
/// </summary> /// </summary>
public const short COMM_SET_MAV_COUNT = 0x10; public const short COMM_SET_MAV_COUNT = 0x10;
/// <summary> /// <summary>
/// 获取飞机详细信息 /// 获取飞机详细信息
/// </summary> /// </summary>
public const short COMM_GET_COPTERS_COUNT = 0x20; public const short COMM_GET_COPTERS_COUNT = 0x20;
/// <summary> /// <summary>
/// 时间同步，不改变当前模式 /// 时间同步，不改变当前模式
/// </summary> /// </summary>
public const short COMM_ASYN_TIME = 0x30; public const short COMM_ASYN_TIME = 0x30;
/// <summary> /// <summary>
/// 进入空闲模式 /// 进入空闲模式
/// </summary> /// </summary>
public const short COMM_IDLE_MODE = 0x50; public const short COMM_IDLE_MODE = 0x50;
/// <summary> /// <summary>
/// 进入搜索模式，命名编号targetID memcpy(pdata,input,2); /// 进入搜索模式，命名编号targetID memcpy(pdata,input,2);
/// </summary> /// </summary>
public const short COMM_SEARCH_MODE = 0x51; public const short COMM_SEARCH_MODE = 0x51;
/// <summary> /// <summary>
/// 进入航点下载模式 (写航点) /// 进入航点下载模式 (写航点)
/// </summary> /// </summary>
public const short COMM_DOWNLOAD_MODE = 0x52; public const short COMM_DOWNLOAD_MODE = 0x52;
/// <summary> /// <summary>
/// 下载模式通信 /// 下载模式通信
/// </summary> /// </summary>
public const short COMM_DOWNLOAD_COMM = 0x53; public const short COMM_DOWNLOAD_COMM = 0x53;
/// <summary> /// <summary>
/// 进入飞行模式（无负载） /// 进入飞行模式（无负载）
/// </summary> /// </summary>
public const short COMM_FLIGHT_MODE = 0x54; public const short COMM_FLIGHT_MODE = 0x54;
/// <summary> /// <summary>
/// 进入飞行模式（有负载数据） /// 进入飞行模式（有负载数据）
/// </summary> /// </summary>
public const short COMM_FLIGHT_LOAD_MODE = 0x55; public const short COMM_FLIGHT_LOAD_MODE = 0x55;
/// <summary> /// <summary>
/// 发送非针对飞控的内部控制指令 /// 发送非针对飞控的内部控制指令
/// </summary> /// </summary>
public const short COMM_TO_MODULE = 0x5F; public const short COMM_TO_MODULE = 0x5F;
/// <summary> /// <summary>
/// 通信模块 /// 通信模块
/// </summary> /// </summary>
public const short COMM_NOTIFICATION = 0x1234; public const short COMM_NOTIFICATION = 0x1234;
/// <summary> /// <summary>
/// 空中升级(更新通信模块飞机端) /// 空中升级(更新通信模块飞机端)
/// </summary> /// </summary>
public const short COMM_UPDATE_COPTER_MODULE = 0xFD; public const short COMM_UPDATE_COPTER_MODULE = 0xFD;
/// <summary> /// <summary>
/// 测试模块 /// 测试模块
/// </summary> /// </summary>
public const short COMM_TEST_MODULE = 0x3C; public const short COMM_TEST_MODULE = 0x3C;
#endregion #endregion
public enum CommMode public enum CommMode
{ {
IDLE = 0, IDLE = 0,
SEARCH = 1, SEARCH = 1,
DOWNLOAD = 3, DOWNLOAD = 3,
FLIGHT = 4 FLIGHT = 4
} }
public enum MessageType public enum MessageType
{ {
STR = 0, STR = 0,
SCAN2 = 2, SCAN2 = 2,
SCAN3 = 3 SCAN3 = 3
} }
//search 搜索模式相关 //search 搜索模式相关
public const ushort SEARCH_COMPLETED = 0x0502; //搜索模式 扫描成功 public const ushort SEARCH_COMPLETED = 0x0502; //搜索模式 扫描成功
public const ushort FREQUENCY_HOPPING_COMPLETED = 0x0503; //搜索模式 跳频表更新成功 public const ushort FREQUENCY_HOPPING_COMPLETED = 0x0503; //搜索模式 跳频表更新成功
public const ushort SEARCH_FINDCOPTER = 0x0401; //搜索模式 找到指定目标 public const ushort SEARCH_FINDCOPTER = 0x0401; //搜索模式 找到指定目标
public const ushort FREQUENCY_HOPPING_ERROR = 0x0100; //搜索模式 跳频表更新错误 public const ushort FREQUENCY_HOPPING_ERROR = 0x0100; //搜索模式 跳频表更新错误
public const ushort SEARCH_OUTTIME = 0xFFFF; //搜索模式 没有人应答 超时时间20S public const ushort SEARCH_OUTTIME = 0xFFFF; //搜索模式 没有人应答 超时时间20S
public const ushort MISSION_SEND_COMPLETED = 0x0503; //航点传输到飞控成功，可开始传输下个飞机航点信息, public const ushort MISSION_SEND_COMPLETED = 0x0503; //航点传输到飞控成功，可开始传输下个飞机航点信息,
//需要再后续等待一个成功消息，才算完成 //需要再后续等待一个成功消息，才算完成
public const ushort P2P_SEND_FAILED = 0x0600; //点对点通信发送失败 public const ushort P2P_SEND_FAILED = 0x0600; //点对点通信发送失败
public const ushort ERROR_CODE_START = 0x0100; public const ushort ERROR_CODE_START = 0x0100;
public const ushort ERROR_CODE_END = 0x03FF; public const ushort ERROR_CODE_END = 0x03FF;
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)] [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
public struct comm_set_mav_count public struct comm_set_mav_count
{ {
public Int16 mav_count; //飞机总数 public Int16 mav_count; //飞机总数
}; };
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)] [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)]
public struct comm_update_copter_module public struct comm_update_copter_module
{ {
public Int16 mav_count; //飞机总数 public Int16 mav_count; //飞机总数
public Int16 update_code; public Int16 update_code;
}; };
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)] [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)]
public struct comm_asyn_time public struct comm_asyn_time
{ {
public Int64 time_stamp; public Int64 time_stamp;
}; };
[StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)] [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)]
public struct comm_copter_info public struct comm_copter_info
{ {
public Int32 control_mode; // public Int32 control_mode; //飞行模式(自动，悬停，定高，返航)
public UInt32 lat; public byte control_mode; //飞行模式(自动，悬停，定高，返航)
public UInt32 lng; public byte rpecheck_fault; //是否有 预解锁错误
public float retain; public byte reserveddata1; //保留
public Int32 alt; public byte reserveddata2; //保留
public Int16 gps_status;
public UInt32 lat; // 经度 in 1E7 degrees
public byte lock_status; public UInt32 lng; // 维度 in 1E7 degrees
public byte gps_num_sats; public float retain; // 写参数序列号,返回版本号等不特定返回数据
public Int16 battery_voltage; public Int32 alt; // millimeters above home
public UInt16 heading;
}; 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; //方向角度
};
}
}