支持紧急返航
支持返回飞机错误
This commit is contained in:
parent
9c2238479f
commit
f0a4484cfc
@ -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;
|
||||
}
|
||||
|
||||
@ -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长度，连续发可能收不到
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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; }
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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(TakeOffPoint.Latitude, TakeOffPoint.Longitude, minretalt);
|
||||
//先到第一返航点再到起飞点上空
|
||||
//FlyToCoreAsync(EmergencyRetPLLocation1.Latitude, EmergencyRetPLLocation1.Longitude, EmergencyRetPLLocation1.Altitude);
|
||||
|
||||
//FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
|
||||
Mode = FlightMode.EMERG_RTL;
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
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;
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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; }
|
||||
|
||||
@ -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; //方向角度
|
||||
|
||||
};
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user