地面站硬件超时逻辑修改为2秒收不到再提示
起飞前改到loiter模式
This commit is contained in:
parent
14d1022775
commit
fc9b2595d6
@ -196,7 +196,7 @@ namespace Plane.CommunicationManagement
|
||||
{
|
||||
long lastPacketCountNum = 0;
|
||||
int faulttimes = 0; //等待没有数据次数
|
||||
const int revtimeout = 1200; //等待超时ms
|
||||
const int revtimeout = 2000; //等待超时ms old=1200
|
||||
while (CommOK)
|
||||
{
|
||||
if (Connection != null)
|
||||
@ -204,7 +204,7 @@ namespace Plane.CommunicationManagement
|
||||
//发送心跳包-等待回复消息
|
||||
await WriteCommPacketAsync(0, MavComm.COMM_QUERY_STATUS);
|
||||
// await SendQueryStatusPacketsAsync();
|
||||
await Task.Delay(revtimeout).ConfigureAwait(false); //1200
|
||||
await Task.Delay(revtimeout).ConfigureAwait(false);
|
||||
//等待是否接受到返回的数据包，如果没有收到认为连结断开，重新联接
|
||||
if (packetCountNum > lastPacketCountNum)
|
||||
{
|
||||
@ -217,8 +217,8 @@ namespace Plane.CommunicationManagement
|
||||
{
|
||||
faulttimes++;
|
||||
Message.Show("设备回复超时("+ revtimeout + "ms)" + faulttimes+"次");
|
||||
//超过3次没数据包返回就重连
|
||||
if (faulttimes > 3)
|
||||
//2次没数据包返回就重连
|
||||
if (faulttimes > 1)
|
||||
{
|
||||
Message.Show("长时间未收到设备回复数据包");
|
||||
break;
|
||||
|
||||
@ -533,7 +533,7 @@ namespace Plane.CommunicationManagement
|
||||
|
||||
byte[] packet1 = SetChannels(1500, 1500, 1100, 1500);
|
||||
|
||||
byte[] packet2 = SetModeAsync(FlightMode.ALT_HOLD);
|
||||
byte[] packet2 = SetModeAsync(FlightMode.ALT_HOLD); //ALT_HOLD LOITER
|
||||
|
||||
byte[] packet3 = DoARMAsync(true);
|
||||
|
||||
|
||||
@ -21,17 +21,17 @@ namespace Plane.Copters
|
||||
/// <summary>
|
||||
/// 更新虚拟飞行器状态的时间间隔，单位为毫秒。
|
||||
/// </summary>
|
||||
private int UPDATE_INTERVAL = 50; //默认100， i7电脑 150可以跑1000架 100可以跑500架 50可以200-300架
|
||||
static private int UPDATE_INTERVAL =50; //默认100， i7电脑 150可以跑1000架 100可以跑500架 50可以200-300架
|
||||
|
||||
/// <summary>
|
||||
/// 在一个更新间隔中的最大移动距离。
|
||||
/// </summary>
|
||||
private float MAX_MOVE_IN_INTERVAL = MAX_VEL * 50 / 1000;
|
||||
private float MAX_MOVE_IN_INTERVAL = MAX_VEL * UPDATE_INTERVAL / 1000;
|
||||
|
||||
/// <summary>
|
||||
/// 高速模式下，在一个更新间隔中的最大移动距离。
|
||||
/// </summary>
|
||||
private float MAX_MOVE_IN_INTERVAL_FAST = MAX_VEL * 50 / 1000 * 4; //MAX_MOVE_IN_INTERVAL*4
|
||||
private float MAX_MOVE_IN_INTERVAL_FAST = MAX_VEL * UPDATE_INTERVAL / 1000 * 4; //MAX_MOVE_IN_INTERVAL*4
|
||||
|
||||
|
||||
|
||||
@ -55,12 +55,12 @@ namespace Plane.Copters
|
||||
/// <summary>
|
||||
/// 高速模式下，按比例缩放过的在一个更新间隔中的最大移动距离。
|
||||
/// </summary>
|
||||
private float _scaledFastMaxMoveInInterval = MAX_VEL * 50 / 1000 * 4; //MAX_MOVE_IN_INTERVAL_FAST
|
||||
private float _scaledFastMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL / 1000 * 4; //MAX_MOVE_IN_INTERVAL_FAST
|
||||
|
||||
/// <summary>
|
||||
/// 按比例缩放过的在一个更新间隔中的最大移动距离。
|
||||
/// </summary>
|
||||
private float _scaledMaxMoveInInterval = MAX_VEL * 50 / 1000; //MAX_MOVE_IN_INTERVAL
|
||||
private float _scaledMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL / 1000; //MAX_MOVE_IN_INTERVAL
|
||||
|
||||
/// <summary>
|
||||
/// 速度缩放比例。
|
||||
@ -708,45 +708,20 @@ namespace Plane.Copters
|
||||
{
|
||||
// 与目标点之间的距离。
|
||||
var distance = this.CalcDistance(lat, lng, alt);
|
||||
|
||||
// 距离已经很近，直接移动到目标点。
|
||||
if (distance < _scaledMaxMoveInInterval)
|
||||
{
|
||||
MoveToPointImmediately(lat, lng, alt);
|
||||
return;
|
||||
}
|
||||
|
||||
// 在空间中的移动距离。
|
||||
var move = _scaledMaxMoveInInterval;
|
||||
|
||||
// 竖直方向的移动距离。
|
||||
var altDelta = (float)(move * (alt - Altitude) / distance);
|
||||
// 更新高度。
|
||||
Altitude += altDelta;
|
||||
|
||||
|
||||
// 目标点相对于当前位置的方向。
|
||||
var direction = this.CalcDirection2D(lat, lng);
|
||||
/*
|
||||
// 更新姿态。
|
||||
|
||||
if (Mode == FlightMode.RTL)
|
||||
{
|
||||
// 王海, 20160126, 目前飞行器只在返航时会旋转机头，面对飞行方向。
|
||||
Yaw = (float)direction.RadToDeg();
|
||||
Heading = (short)Yaw;
|
||||
Roll = 0;
|
||||
Pitch = MAX_TILT_IN_FLIGHT;
|
||||
}
|
||||
else
|
||||
{
|
||||
//不用更新姿态-xu
|
||||
//var directionDelta = direction - Heading.DegToRad();
|
||||
// Roll = MAX_TILT_IN_FLIGHT * (float)Math.Sin(directionDelta);
|
||||
// Pitch = MAX_TILT_IN_FLIGHT * (float)Math.Cos(directionDelta);
|
||||
}
|
||||
*/
|
||||
|
||||
// 水平面上的移动距离。
|
||||
var moveInHorizontalPlane = Math.Sqrt(move * move - altDelta * altDelta);
|
||||
if (double.IsNaN(moveInHorizontalPlane))
|
||||
|
||||
@ -349,7 +349,7 @@ namespace Plane.Copters
|
||||
ch4: 1500
|
||||
).ConfigureAwait(false);
|
||||
|
||||
if (await SetModeAsync(FlightMode.ALT_HOLD).ConfigureAwait(false))
|
||||
if (await SetModeAsync(FlightMode.LOITER).ConfigureAwait(false))
|
||||
{
|
||||
await _internalCopter.DoARMAsync(true).ConfigureAwait(false);
|
||||
}
|
||||
|
||||
@ -18,8 +18,8 @@ namespace Plane.Protocols
|
||||
public const int MAVLINK_BIG_ENDIAN = 0;
|
||||
|
||||
//协议头 飞控4.0以后 254 - 240 Mavlink头 FE 改为 F0
|
||||
public const byte MAVLINK_STX = 254;
|
||||
//public const byte MAVLINK_STX = 240;
|
||||
//public const byte MAVLINK_STX = 254;
|
||||
public const byte MAVLINK_STX = 240;
|
||||
|
||||
public const byte MAVLINK_ENDIAN = MAVLINK_LITTLE_ENDIAN;
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user