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;
}

87
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,7 +1307,9 @@ 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 );
@ -1260,18 +1317,18 @@ namespace Plane.CommunicationManagement
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 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长度，连续发可能收不到
}

2
PlaneGcsSdk_Shared/CommunicationManagement/CommModulePacketAnalysis.cs
View File

@ -203,7 +203,7 @@ namespace Plane.CommunicationManagement
//0B110
case 0xC000 >> 13:
//0B111
case 0xE000 >> 13:
case 0xE000 >> 13: //最后正在用的版本
CommunicationReceived?.Invoke(this, new CommunicationReceiveCopterInfoEventArgs(copterId, packet, version));
break;
}

95
PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs
View File

@ -1,5 +1,6 @@
using Plane.Geography;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Drawing;
using System.Threading;
@ -119,6 +120,13 @@ namespace Plane.Copters
private short _Heading;
private byte _CopterErrorCode=0;
private bool _CopterPreCheckPass=true;
private String _CopterPreCheckPassStr;
private String _CopterErrorString;
private ulong _HeartbeatCount;
private bool _IsAbsolutelyConnected;
@ -278,6 +286,43 @@ namespace Plane.Copters
RC_CHANNELS_RAW = 35 // MAVLINK_MSG_ID_RC_CHANNELS_RAW
}
private static readonly Dictionary<int, string> ErrorIdToString = new Dictionary<int, string>
{
{2, "气压计异常"},
{3, "磁罗盘异常"},
{4, "GPS异常"},
{5, "惯性传感器异常"},
{6, "参数异常"},
{7, "遥控异常"},
{8, "飞控电压低"},
{9, "电池电压低"},
{10, "空速传感器失败"},
{11, "日志记录失败"},
{12, "安全开关未开"},
{13, "GPS配置异常"},
{14, "系统异常"},
{15, "任务异常"},
{16, "测距传感器异常"},
{17, "摄像头异常"},
{18, "混控异常"},
{19, "版本异常"},
{20, "FFT异常"},
{21, "陀螺仪无数据"},
{22, "陀螺仪没校准"},
{23, "加速计没数据"},
{24, "加速计没校准"},
{25, "加速计需要重启"},
{26, "加速计不一致"},
{27, "陀螺仪不一致"},
};
public String getcoptererrorstr(byte errorcode)
{
String errorstr = "未知";
ErrorIdToString.TryGetValue(errorcode, out errorstr);
String retstr = "["+errorcode.ToString()+"]" + errorstr;
return retstr;
}
public float AirSpeed
{
get { return _AirSpeed; }
@ -521,6 +566,56 @@ namespace Plane.Copters
protected set { Set(nameof(GpsHdop), ref _GpsHdop, value); }
}
/// <summary>
/// 飞机回传的错误码，可知道飞机当前状态，比如是否加速计不一致，是否磁罗盘不一致等，是黄灯的具体代码。
/// </summary>
public byte CopterErrorCode
{
get { return _CopterErrorCode; }
protected set { Set(nameof(CopterErrorCode), ref _CopterErrorCode, value); }
}
/// <summary>
/// 飞机预检查是否通过。
/// </summary>
public bool CopterPreCheckPass
{
get { return _CopterPreCheckPass; }
protected set {
Set(nameof(CopterPreCheckPass), ref _CopterPreCheckPass, value);
if (_CopterPreCheckPass)
CopterPreCheckPassStr= "通过";
else
CopterPreCheckPassStr= "异常";
}
}
/// <summary>
/// 飞机预检查是否通过。
/// </summary>
public string CopterPreCheckPassStr
{
get
{
return _CopterPreCheckPassStr;
}
protected set { Set(nameof(CopterPreCheckPassStr), ref _CopterPreCheckPassStr, value); }
}
/// <summary>
/// 飞机回传的最后错误码转换的字符串，可知道飞机当前状态，比如是否加速计不一致，是否磁罗盘不一致等，是黄灯的具体代码。
/// </summary>
public String CopterErrorString
{
get { return _CopterErrorString; }
protected set { Set(nameof(CopterErrorString), ref _CopterErrorString, value); }
}
public float GroundSpeed
{
get { return _GroundSpeed; }

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

@ -1,10 +1,14 @@
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)
@ -20,6 +24,7 @@ namespace Plane.Copters
RaiseTextReceived(e);
}
private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType)
{
switch (StreamType)
@ -29,6 +34,9 @@ namespace Plane.Copters
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);

108
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,34 +291,30 @@ 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);
}
}
@ -326,9 +323,10 @@ namespace Plane.Copters
_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(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt);
//先到第一返航点再到起飞点上空
//FlyToCoreAsync(EmergencyRetPLLocation1.Latitude, EmergencyRetPLLocation1.Longitude, EmergencyRetPLLocation1.Altitude);
//FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
Mode = FlightMode.EMERG_RTL;
}
break;
case 1: //返航阶段一:等待平飞或降落随机距离并到达,然后飞到起飞点上空
UpdateWithDestination(Emergency_firstloc.Latitude, Emergency_firstloc.Longitude, Emergency_firstloc.Altitude);
if (ReachedDestination(Emergency_firstloc.Latitude, Emergency_firstloc.Longitude, Emergency_firstloc.Altitude))
//直接返航
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.EMERG_RTL;
Emergency_Retstatus = 2;
}
break;
case 2: //返航阶段二:等待到达起飞点上空,然后降落
UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude);
if (ReachedDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude))
{
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;

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

@ -375,7 +375,7 @@ namespace Plane.Copters
}
/// <summary>
/// 处理通信模块发过来的数据
/// 飞控返回数据 处理通信模块发过来的28个字节数据
/// </summary>
/// <param name="buffer"></param>
public void AnalyzeCommMouldePositionIntPacket(byte[] buffer, byte version)
@ -385,6 +385,8 @@ namespace Plane.Copters
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;

4
PlaneGcsSdk_Shared/Copters/PlaneCopter.cs
View File

@ -55,7 +55,11 @@ namespace Plane.Copters
public bool useLocation { get; set; }
public float gpsalt { get; set; }
public bool precheckok { get; set; }
public byte gpsstatus { get; set; }
public byte errorcode { get; set; }
public float gpshdop { get; set; }
public byte satcount { get; set; }
public float retain { get; set; }

30
PlaneGcsSdk_Shared/Protocols/MavCommTypes.cs
View File

@ -158,18 +158,28 @@ namespace Plane.Protocols
[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 Int32 control_mode; //飞行模式(自动，悬停，定高，返航)
public byte control_mode; //飞行模式(自动，悬停，定高，返航)
public byte rpecheck_fault; //是否有 预解锁错误
public byte reserveddata1; //保留
public byte reserveddata2; //保留
public Int16 gps_status;
public byte lock_status;
public byte gps_num_sats;
public Int16 battery_voltage;
public UInt16 heading;
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; //方向角度
};