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

更新使用Vs2022编译

Browse Source 
模拟飞机路线计算调整
This commit is contained in:
pxzleo 2023-12-25 18:54:28 +08:00
parent 14c489c016
commit c0f0f616dc
4 changed files with 2333 additions and 2302 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

243
PlaneGcsSdk_Shared/Communication/TcpConnection.cs
View File

@ -1,118 +1,125 @@
#if !NETFX_CORE #if !NETFX_CORE
using System; using System;
using System.Net; using System.Net;
using System.Net.Sockets; using System.Net.Sockets;
using System.Threading.Tasks; using System.Threading.Tasks;
namespace Plane.Communication namespace Plane.Communication
{ {
/// <summary> /// <summary>
/// 提供作为 TCP 客户端通信的能力。 /// 提供作为 TCP 客户端通信的能力。
/// </summary> /// </summary>
public class TcpConnection : TcpConnectionBase public class TcpConnection : TcpConnectionBase
{ {
private string _remoteHostname; private string _remoteHostname;
const int tcpReceiveBufferSize = 20 * 1024; //40 * 1024, const int tcpReceiveBufferSize = 20 * 1024; //40 * 1024,
const int tcpReceiveTimeout = 1200; const int tcpReceiveTimeout = 1200;
private int _remotePort; private int _remotePort;
public TcpConnection(string remoteHostname, int remotePort = 5250) public TcpConnection(string remoteHostname, int remotePort = 5250)
{ {
_remoteHostname = remoteHostname; _remoteHostname = remoteHostname;
_remotePort = remotePort; _remotePort = remotePort;
_client = new TcpClient _client = new TcpClient
{ {
ReceiveBufferSize = tcpReceiveBufferSize, ReceiveBufferSize = tcpReceiveBufferSize,
ReceiveTimeout = tcpReceiveTimeout ReceiveTimeout = tcpReceiveTimeout
}; };
} }
public void Open() public void Open()
{ {
if (!IsOpen) if (!IsOpen)
{ {
try try
{ {
_client.Connect(_remoteHostname, _remotePort); _client.Connect(_remoteHostname, _remotePort);
} }
catch (SocketException) catch (SocketException)
{ {
CreateClientAndConnect(); CreateClientAndConnect();
} }
catch (ObjectDisposedException) catch (ObjectDisposedException)
{ {
CreateClientAndConnect(); CreateClientAndConnect();
} }
_isBroken = false; _isBroken = false;
} }
_stream = _client.GetStream(); _stream = _client.GetStream();
} }
public async Task<bool> BindIP(string ip) public async Task<bool> BindIP(string ip)
{ {
bool bind = false; bool bind = false;
try try
{ {
IPAddress IPLocal = IPAddress.Parse(ip); IPAddress IPLocal = IPAddress.Parse(ip);
_client.Client.Bind(new IPEndPoint(IPLocal, 0)); _client.Client.Bind(new IPEndPoint(IPLocal, 0));
bind = true; bind = true;
} }
catch catch
{ {
bind = false; bind = false;
} }
await Task.Delay(10).ConfigureAwait(false); await Task.Delay(10).ConfigureAwait(false);
return bind; return bind;
} }
public override async Task OpenAsync() public override async Task OpenAsync()
{ {
string logstr; string logstr;
if (!IsOpen) if (!IsOpen)
{ {
try if (_client == null)
{ CreateClientAndConnect();
await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false); try
} {
//屏蔽掉异常处理 var connectTask = _client.ConnectAsync(_remoteHostname, _remotePort);
//CreateClientAndConnectAsync会new一个TcpClient并且重新连接 if (await Task.WhenAny(connectTask, Task.Delay(5000)) == connectTask)
//之前设置的TcpClient中Socket Bind会无效，在多网卡工作时会导致断线重连的时间特别长 {
catch (SocketException e) await connectTask.ConfigureAwait(false);
{ }
logstr= e.Message; else
//await CreateClientAndConnectAsync(); {
} // Connection timed out
catch (ObjectDisposedException) throw new TimeoutException("Connection timed out");
{ }
//await CreateClientAndConnectAsync(); }
} catch (SocketException e)
_isBroken = false; {
} logstr = e.Message;
_stream = _client.GetStream(); }
} catch (ObjectDisposedException)
{
private void CreateClientAndConnect() }
{ _isBroken = false;
_client = new TcpClient(_remoteHostname, _remotePort) }
{ _stream = _client.GetStream();
ReceiveBufferSize = tcpReceiveBufferSize, }
ReceiveTimeout = tcpReceiveTimeout
};
} private void CreateClientAndConnect()
{
private async Task CreateClientAndConnectAsync() _client = new TcpClient(_remoteHostname, _remotePort)
{ {
_client = new TcpClient ReceiveBufferSize = tcpReceiveBufferSize,
{ ReceiveTimeout = tcpReceiveTimeout
ReceiveBufferSize = tcpReceiveBufferSize,// 40 * 1024, };
ReceiveTimeout = tcpReceiveTimeout// 1200 }
};
await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false); private async Task CreateClientAndConnectAsync()
} {
} _client = new TcpClient
} {
ReceiveBufferSize = tcpReceiveBufferSize,// 40 * 1024,
#endif ReceiveTimeout = tcpReceiveTimeout// 1200
};
await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
}
}
}
#endif

1833
PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs
View File

File diff suppressed because it is too large Load Diff

702
PlaneGcsSdk_Shared/CopterControllers/GuidController.cs
View File

@ -1,347 +1,355 @@
using System; using System;
using System.Collections.Generic; using System.Collections.Generic;
using System.Text; using System.Text;
namespace Plane.CopterControllers namespace Plane.CopterControllers
{ {
class GuidController class GuidController
{ {
static float fabsf(float vvalue) float fabsf(float vvalue)
{ {
return Math.Abs(vvalue); return Math.Abs(vvalue);
} }
static float sqrt(float vvalue) float sqrt(float vvalue)
{ {
return (float)Math.Sqrt(vvalue); return (float)Math.Sqrt(vvalue);
} }
/// <summary> /// <summary>
/// 计算小航点飞行 /// 计算小航点飞行
/// </summary> /// </summary>
/// <param name="Distance"></param> /// <param name="Distance"></param>
/// <param name="fc_acc"></param> /// <param name="fc_acc"></param>
/// <param name="fc_maxspeed"></param> /// <param name="fc_maxspeed"></param>
/// <returns></returns> /// <returns></returns>
//单算减速，不算加速的最小飞行时间 //单算减速，不算加速的最小飞行时间
static float getMinfligthtime_v2(float Distance, float fc_acc, float fc_maxspeed) float getMinfligthtime_v2(float Distance, float fc_acc, float fc_maxspeed)
{ {
float fDis = fabsf(Distance); // 总距离 float fDis = fabsf(Distance); // 总距离
float facc = fabsf(fc_acc); // 减速度 float facc = fabsf(fc_acc); // 减速度
// 物体开始时即以最大速度运动，不考虑加速过程 // 物体开始时即以最大速度运动，不考虑加速过程
float vel = fc_maxspeed; float vel = fc_maxspeed;
// 计算减速所需的时间和距离 // 计算减速所需的时间和距离
// 减速时间 (从最大速度减速到0) // 减速时间 (从最大速度减速到0)
float dectime = vel / facc; float dectime = vel / facc;
// 减速阶段覆盖的距离 // 减速阶段覆盖的距离
float decdis = (vel * dectime) - (0.5f * facc * dectime * dectime); float decdis = (vel * dectime) - (0.5f * facc * dectime * dectime);
// 判断是否有足够的距离进行减速 // 判断是否有足够的距离进行减速
if (decdis >= fDis) if (decdis >= fDis)
{ {
// 如果减速所需距离已经超过或等于总距离，调整最大速度 // 如果减速所需距离已经超过或等于总距离，调整最大速度
// 使用公式 v^2 = u^2 + 2as 解出 v // 使用公式 v^2 = u^2 + 2as 解出 v
vel = (float)Math.Sqrt(2 * facc * fDis); vel = (float)Math.Sqrt(2 * facc * fDis);
// 重新计算减速时间 // 重新计算减速时间
dectime = vel / facc; dectime = vel / facc;
} }
// 计算匀速阶段时间 // 计算匀速阶段时间
float unftime = 0.0f; float unftime = 0.0f;
if (decdis < fDis) if (decdis < fDis)
{ {
// 如果有剩余距离进行匀速运动 // 如果有剩余距离进行匀速运动
unftime = (fDis - decdis) / vel; unftime = (fDis - decdis) / vel;
} }
// 总飞行时间 = 匀速阶段时间 + 减速阶段时间 // 总飞行时间 = 匀速阶段时间 + 减速阶段时间
return unftime + dectime; return unftime + dectime;
} }
//单算减速，不算加速的最大飞行速度 //单算减速，不算加速的最大飞行速度
public static float CalculateMaximumVelocity(float distance, float time, float acceleration) public float CalculateMaximumVelocity(float distance, float time, float acceleration)
{ {
// 二次方程系数 // 二次方程系数
float a_coeff = 1; float a_coeff = 1;
float b_coeff = -2 * time; float b_coeff = -2 * time;
float c_coeff = (2 * distance) / acceleration; float c_coeff = (2 * distance) / acceleration;
// 计算判别式 // 计算判别式
float discriminant = b_coeff * b_coeff - 4 * a_coeff * c_coeff; float discriminant = b_coeff * b_coeff - 4 * a_coeff * c_coeff;
if (discriminant < 0) if (discriminant < 0)
{ {
return -1; // 无实数解 return -1; // 无实数解
} }
// 计算二次方程的根 // 计算二次方程的根
float t1_root1 = (-b_coeff + (float)Math.Sqrt(discriminant)) / (2 * a_coeff); float t1_root1 = (-b_coeff + (float)Math.Sqrt(discriminant)) / (2 * a_coeff);
float t1_root2 = (-b_coeff - (float)Math.Sqrt(discriminant)) / (2 * a_coeff); float t1_root2 = (-b_coeff - (float)Math.Sqrt(discriminant)) / (2 * a_coeff);
// 选择合理的根作为 t1 // 选择合理的根作为 t1
float t1 = Math.Min(t1_root1, t1_root2); float t1 = Math.Min(t1_root1, t1_root2);
if (t1 < 0 || t1 > time) if (t1 < 0 || t1 > time)
{ {
return -1; // 无合理解 return -1; // 无合理解
} }
// 计算最大速度 v // 计算最大速度 v
return acceleration * t1; return acceleration * t1;
} }
//单算加速，不算减速的距离速度计算 //单算加速，不算减速的距离速度计算
static float getspeeddist_V2(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed) float getspeeddist_V2(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
{ {
vdist = 0; vdist = 0;
vspeed = 0; vspeed = 0;
float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc); float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc);
if ((desV == -1) || (desV > fc_maxspeed)) //飞不到 if ((desV == -1) || (desV > fc_maxspeed)) //飞不到
{ {
vspeed = Distance / flight_time; vspeed = Distance / flight_time;
vdist = vspeed * curr_time; //匀速距离 vdist = vspeed * curr_time; //匀速距离
return 0; return 0;
} }
float dect = desV / fc_acc; // 总加速时间 float dect = desV / fc_acc; // 总加速时间
float unit = flight_time - dect; //匀速段时间 float unit = flight_time - dect; //匀速段时间
float decD = (fc_acc * dect * dect) / 2; //总加速距离 float decD = (fc_acc * dect * dect) / 2; //总加速距离
if (dect > flight_time) dect = flight_time;//约束时间 if (dect > flight_time) dect = flight_time;//约束时间
if (curr_time > dect) //大于加速段时间--匀速 if (curr_time > dect) //大于加速段时间--匀速
{ {
vspeed = (Distance - decD) / (flight_time - dect); vspeed = (Distance - decD) / (flight_time - dect);
vdist = vspeed * (curr_time - dect); vdist = vspeed * (curr_time - dect);
vdist = vdist + decD; //总距离=匀速距离+减速距离 vdist = vdist + decD; //总距离=匀速距离+减速距离
if (vdist > Distance) vdist = Distance; //约束距离 if (vdist > Distance) vdist = Distance; //约束距离
} }
else //加速段 else //加速段
{ {
vspeed = fc_acc * curr_time; vspeed = fc_acc * curr_time;
vdist = (fc_acc * curr_time * curr_time) / 2; //加速距离 vdist = (fc_acc * curr_time * curr_time) / 2; //加速距离
} }
return 0; return 0;
} }
//单算减速，不算加速的距离速度计算 //单算减速，不算加速的距离速度计算
static float getspeeddist_V3(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed) float getspeeddist_V3(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
{ {
vdist = 0; vdist = 0;
vspeed = 0; vspeed = 0;
float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc); float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc);
if ((desV == -1) || (desV > fc_maxspeed)) //飞不到 if ((desV == -1) || (desV > fc_maxspeed)) //飞不到
{ {
vspeed = Distance / flight_time; vspeed = Distance / flight_time;
vdist = vspeed * curr_time; //匀速距离 vdist = vspeed * curr_time; //匀速距离
return 0; return 0;
} }
float dect = desV / fc_acc; // 总减速时间 float dect = desV / fc_acc; // 总减速时间
float unit = flight_time - dect; //匀速段时间 float unit = flight_time - dect; //匀速段时间
float decD = desV * dect - (fc_acc * dect * dect) / 2; //总减速距离 float decD = desV * dect - (fc_acc * dect * dect) / 2; //总减速距离
if (dect > flight_time) dect = flight_time;//约束时间 if (dect > flight_time) dect = flight_time;//约束时间
//匀速时间内 //匀速时间内
if (curr_time < unit) if (curr_time < unit)
{ {
vspeed = (Distance - decD) / (flight_time - dect); vspeed = (Distance - decD) / (flight_time - dect);
vdist = vspeed * curr_time; vdist = vspeed * curr_time;
} }
else else
{ {
if (((flight_time - dect) * unit) == 0) if (((flight_time - dect) * unit) == 0)
vdist = 0; vdist = 0;
else else
vdist = (Distance - decD) / (flight_time - dect) * unit; //匀速距离 vdist = (Distance - decD) / (flight_time - dect) * unit; //匀速距离
float currdect = curr_time - unit; //减速时间 float currdect = curr_time - unit; //减速时间
if (currdect >= 0) if (currdect >= 0)
{ {
vspeed = desV - fc_acc * currdect; vspeed = desV - fc_acc * currdect;
decD = desV * currdect - (fc_acc * currdect * currdect) / 2; //减速距离 decD = desV * currdect - (fc_acc * currdect * currdect) / 2; //减速距离
} }
vdist = vdist + decD; //总距离=匀速距离+减速距离 vdist = vdist + decD; //总距离=匀速距离+减速距离
if (vdist > Distance) vdist = Distance; //约束距离 if (vdist > Distance) vdist = Distance; //约束距离
} }
return 0; return 0;
} }
static float getMinfligthtime(float Distance, float fc_acc, float fc_maxspeed) float getMinfligthtime(float Distance, float fc_acc, float fc_maxspeed)
{ {
float fDis = fabsf(Distance); float fDis = fabsf(Distance);
float facc = fabsf(fc_acc); float facc = fabsf(fc_acc);
float realflytime = 0.0f; float realflytime = 0.0f;
//计算一半的距离 //计算一半的距离
float hafdis = (fDis / 2); float hafdis = (fDis / 2);
//计算最大速度 //计算最大速度
float vel = (float)sqrt(2 * facc * hafdis); float vel = (float)sqrt(2 * facc * hafdis);
//如果速度超过最大速度 //如果速度超过最大速度
if (vel > fc_maxspeed) if (vel > fc_maxspeed)
//使用最大速度 //使用最大速度
vel = fc_maxspeed; vel = fc_maxspeed;
//加速时间 //加速时间
float acctim = vel / facc; float acctim = vel / facc;
//加速距离 //加速距离
float accdis = vel * vel / (2 * facc); float accdis = vel * vel / (2 * facc);
//匀速段时间 //匀速段时间
float vtime = (hafdis - accdis) / vel; float vtime = (hafdis - accdis) / vel;
//到一半的时间 //到一半的时间
float haftime = acctim + vtime; float haftime = acctim + vtime;
realflytime = haftime * 2; realflytime = haftime * 2;
return realflytime; return realflytime;
} }
///计算飞行距离和速度 单位--米，秒---- ///计算飞行距离和速度 单位--米，秒----
//返回 整个距离最大飞行速度 //返回 整个距离最大飞行速度
///flight_time 总飞行时间 Distance 飞行总距离 curr_time 当前飞行时间 fc_acc加速度, fc_maxspeed最大速度 vdist 计算的应该飞的距离 vspeed 计算的当前时间应该速度 ///flight_time 总飞行时间 Distance 飞行总距离 curr_time 当前飞行时间 fc_acc加速度, fc_maxspeed最大速度 vdist 计算的应该飞的距离 vspeed 计算的当前时间应该速度
//========这是飞控正在使用的算法======== //========这是飞控正在使用的算法========
static float getspeeddist_V1(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed) float getspeeddist_V1(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
{ {
//计划飞行时间 float plan_flytime=_plan_flytime;
float plan_flytime = flight_time; float desired_velocity=_desired_velocity;
//计算距离用绝对值 float dist = 0;
float fDis = fabsf(Distance); //导航加速度 米/秒*秒 ---不使用导航库
if (curr_time < 0) float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5;
{ //
vdist = 0; float speed = 0;
vspeed = 0; //计算实时速度
return 0.0f; if (curr_time <= (plan_flytime / 2.0))
} speed = curr_time * wpacc;
//导航加速度 米/秒*秒 ---不使用导航库 else
float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5; //需要减速
speed = (plan_flytime - curr_time) * wpacc;
//最大目标速度---米/秒 //不能大于目标速度
float desired_velocity = 0; if (speed > desired_velocity)
float dist = 0; speed = desired_velocity;
// if (speed <= 0)
float speed = 0; speed = 0;
vspeed = speed;
//计算最小飞行时间 //计算实时距离
float minflytime = getMinfligthtime(Distance, fc_acc, fc_maxspeed); float V_start = 0.0f;
if (flight_time < minflytime) float V_end = 0.0f;
plan_flytime = minflytime;// + 0.1f; //加速段
float t1 = (desired_velocity - V_start) / wpacc;
//减速段
//根据总飞行时间和总距离计算飞行速度----起始和结束速度都是0，中间按匀加速和匀减速计算，没考虑加加速度 float t3 = (desired_velocity - V_end) / wpacc;
float delta = (0.5f * plan_flytime) * (0.5f * plan_flytime) - fDis / wpacc; //平飞段
if (delta >= 0) float t2 = plan_flytime - t1 - t3;
{ if (curr_time < t1)
desired_velocity = (0.5f * plan_flytime - sqrt(delta)) / (1 / wpacc); {
if (desired_velocity > fc_maxspeed) dist = 0.5f * wpacc * curr_time * curr_time + curr_time * V_start;
{ }
plan_flytime = minflytime; else if (curr_time < t1 + t2)
desired_velocity = fc_maxspeed; {
} dist = -0.5f * wpacc * t1 * t1 + (curr_time) * desired_velocity;
} }
else else
{ {
desired_velocity = (0.5f * plan_flytime) / (1 / wpacc); float t = curr_time - t1 - t2;
} dist = -0.5f * wpacc * t1 * t1 + curr_time * desired_velocity - 0.5f * wpacc * t * t;
if (desired_velocity < 0.2f) desired_velocity = 0.2f; //限制最小速度0.2米/秒 }
if (desired_velocity > fc_maxspeed) desired_velocity = fc_maxspeed;//限制最大速度10米/秒，应该参数化
if (fabsf(dist) > fabsf(Distance))
//计算实时速度 vdist = Distance;
if (curr_time <= (plan_flytime / 2.0)) else
speed = curr_time * wpacc; {
else if (Distance < 0)
//需要减速 vdist = -dist;
speed = (plan_flytime - curr_time) * wpacc; else vdist = dist;
//不能大于目标速度 }
if (speed > desired_velocity)
speed = desired_velocity; return desired_velocity;
if (speed <= 0) }
speed = 0;
vspeed = speed; float _vspeed=0;
//计算实时距离
float V_start = 0.0f; float _desired_velocity = 0;
float V_end = 0.0f; float _plan_flytime = 0;
//加速段
float t1 = (desired_velocity - V_start) / wpacc; float initgetspeeddist_V1(float flight_time, float Distance, float fc_acc, float fc_maxspeed)
//减速段 {
float t3 = (desired_velocity - V_end) / wpacc; //计划飞行时间
//平飞段 float plan_flytime = flight_time;
float t2 = plan_flytime - t1 - t3; //计算距离用绝对值
if (curr_time < t1) float fDis = fabsf(Distance);
{
dist = 0.5f * wpacc * curr_time * curr_time + curr_time * V_start; //导航加速度 米/秒*秒 ---不使用导航库
} float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5;
else if (curr_time < t1 + t2)
{ //最大目标速度---米/秒
dist = -0.5f * wpacc * t1 * t1 + (curr_time) * desired_velocity; float desired_velocity = 0;
}
else
{ //计算最小飞行时间
float t = curr_time - t1 - t2; float minflytime = getMinfligthtime(Distance, fc_acc, fc_maxspeed);
dist = -0.5f * wpacc * t1 * t1 + curr_time * desired_velocity - 0.5f * wpacc * t * t; if (flight_time < minflytime)
} plan_flytime = minflytime;// + 0.1f;
if (fabsf(dist) > fabsf(Distance))
vdist = Distance; //根据总飞行时间和总距离计算飞行速度----起始和结束速度都是0，中间按匀加速和匀减速计算，没考虑加加速度
else float delta = (0.5f * plan_flytime) * (0.5f * plan_flytime) - fDis / wpacc;
{ if (delta >= 0)
if (Distance < 0) {
vdist = -dist; desired_velocity = (0.5f * plan_flytime - sqrt(delta)) / (1 / wpacc);
else vdist = dist; if (desired_velocity > fc_maxspeed)
} {
plan_flytime = minflytime;
return desired_velocity; desired_velocity = fc_maxspeed;
} }
}
public static float getspeeddist(int flytype, float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed) else
{ {
switch (flytype) desired_velocity = (0.5f * plan_flytime) / (1 / wpacc);
{ }
case 0: //匀速 if (desired_velocity < 0.2f) desired_velocity = 0.2f; //限制最小速度0.2米/秒
//case 1: //匀速 if (desired_velocity > fc_maxspeed) desired_velocity = fc_maxspeed;//限制最大速度10米/秒，应该参数化
vspeed = Distance / flight_time;
vdist = vspeed * curr_time; _desired_velocity = desired_velocity;
return 0; _plan_flytime = plan_flytime;
case 1: //同时计算加减速 return desired_velocity;
return getspeeddist_V1(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed); }
case 2: //单加速
return getspeeddist_V2(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
case 3: //单减速 public float initgetspeeddist(int flytype, float flight_time, float Distance, float fc_acc, float fc_maxspeed)
return getspeeddist_V3(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed); {
default: flytype = 0;
vspeed = 0; switch (flytype)
vdist = 0; {
return 0; case 0: //匀速
} _vspeed = Distance / flight_time;
} return 0;
case 1: //同时计算加减速
return initgetspeeddist_V1(flight_time, Distance, fc_acc, fc_maxspeed);
case 2: //单加速
return 0;
case 3: //单减速
return 0;
default:
return 0;
}
}
public float getspeeddist(int flytype, float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
{
flytype = 0;
switch (flytype)
{
case 0: //匀速
//case 1: //匀速
vspeed = _vspeed;
vdist = vspeed * curr_time;
return 0;
case 1: //同时计算加减速
return getspeeddist_V1(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
case 2: //单加速
return getspeeddist_V2(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
case 3: //单减速
} return getspeeddist_V3(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
} default:
vspeed = 0;
vdist = 0;
return 0;
}
}
}
}

1857
PlaneGcsSdk_Shared/Copters/FakeCopter.cs
View File

File diff suppressed because it is too large Load Diff