更新使用Vs2022编译
模拟飞机路线计算调整
This commit is contained in:
parent
14c489c016
commit
c0f0f616dc
@ -1,118 +1,125 @@
|
||||
#if !NETFX_CORE
|
||||
|
||||
using System;
|
||||
using System.Net;
|
||||
using System.Net.Sockets;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace Plane.Communication
|
||||
{
|
||||
/// <summary>
|
||||
/// 提供作为 TCP 客户端通信的能力。
|
||||
/// </summary>
|
||||
public class TcpConnection : TcpConnectionBase
|
||||
{
|
||||
private string _remoteHostname;
|
||||
const int tcpReceiveBufferSize = 20 * 1024; //40 * 1024,
|
||||
const int tcpReceiveTimeout = 1200;
|
||||
|
||||
private int _remotePort;
|
||||
|
||||
public TcpConnection(string remoteHostname, int remotePort = 5250)
|
||||
{
|
||||
_remoteHostname = remoteHostname;
|
||||
_remotePort = remotePort;
|
||||
_client = new TcpClient
|
||||
{
|
||||
ReceiveBufferSize = tcpReceiveBufferSize,
|
||||
ReceiveTimeout = tcpReceiveTimeout
|
||||
};
|
||||
}
|
||||
|
||||
public void Open()
|
||||
{
|
||||
if (!IsOpen)
|
||||
{
|
||||
try
|
||||
{
|
||||
_client.Connect(_remoteHostname, _remotePort);
|
||||
}
|
||||
catch (SocketException)
|
||||
{
|
||||
CreateClientAndConnect();
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
CreateClientAndConnect();
|
||||
}
|
||||
_isBroken = false;
|
||||
}
|
||||
|
||||
_stream = _client.GetStream();
|
||||
}
|
||||
|
||||
public async Task<bool> BindIP(string ip)
|
||||
{
|
||||
bool bind = false;
|
||||
try
|
||||
{
|
||||
IPAddress IPLocal = IPAddress.Parse(ip);
|
||||
_client.Client.Bind(new IPEndPoint(IPLocal, 0));
|
||||
bind = true;
|
||||
}
|
||||
catch
|
||||
{
|
||||
bind = false;
|
||||
}
|
||||
await Task.Delay(10).ConfigureAwait(false);
|
||||
return bind;
|
||||
}
|
||||
|
||||
public override async Task OpenAsync()
|
||||
{
|
||||
string logstr;
|
||||
if (!IsOpen)
|
||||
{
|
||||
try
|
||||
{
|
||||
await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
|
||||
}
|
||||
//屏蔽掉异常处理
|
||||
//CreateClientAndConnectAsync会new一个TcpClient并且重新连接
|
||||
//之前设置的TcpClient中Socket Bind会无效，在多网卡工作时会导致断线重连的时间特别长
|
||||
catch (SocketException e)
|
||||
{
|
||||
logstr= e.Message;
|
||||
//await CreateClientAndConnectAsync();
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
//await CreateClientAndConnectAsync();
|
||||
}
|
||||
_isBroken = false;
|
||||
}
|
||||
_stream = _client.GetStream();
|
||||
}
|
||||
|
||||
private void CreateClientAndConnect()
|
||||
{
|
||||
_client = new TcpClient(_remoteHostname, _remotePort)
|
||||
{
|
||||
ReceiveBufferSize = tcpReceiveBufferSize,
|
||||
ReceiveTimeout = tcpReceiveTimeout
|
||||
};
|
||||
}
|
||||
|
||||
private async Task CreateClientAndConnectAsync()
|
||||
{
|
||||
_client = new TcpClient
|
||||
{
|
||||
ReceiveBufferSize = tcpReceiveBufferSize,// 40 * 1024,
|
||||
ReceiveTimeout = tcpReceiveTimeout// 1200
|
||||
};
|
||||
await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
#if !NETFX_CORE
|
||||
|
||||
using System;
|
||||
using System.Net;
|
||||
using System.Net.Sockets;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace Plane.Communication
|
||||
{
|
||||
/// <summary>
|
||||
/// 提供作为 TCP 客户端通信的能力。
|
||||
/// </summary>
|
||||
public class TcpConnection : TcpConnectionBase
|
||||
{
|
||||
private string _remoteHostname;
|
||||
const int tcpReceiveBufferSize = 20 * 1024; //40 * 1024,
|
||||
const int tcpReceiveTimeout = 1200;
|
||||
|
||||
private int _remotePort;
|
||||
|
||||
public TcpConnection(string remoteHostname, int remotePort = 5250)
|
||||
{
|
||||
_remoteHostname = remoteHostname;
|
||||
_remotePort = remotePort;
|
||||
_client = new TcpClient
|
||||
{
|
||||
ReceiveBufferSize = tcpReceiveBufferSize,
|
||||
ReceiveTimeout = tcpReceiveTimeout
|
||||
};
|
||||
}
|
||||
|
||||
public void Open()
|
||||
{
|
||||
if (!IsOpen)
|
||||
{
|
||||
try
|
||||
{
|
||||
_client.Connect(_remoteHostname, _remotePort);
|
||||
}
|
||||
catch (SocketException)
|
||||
{
|
||||
CreateClientAndConnect();
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
CreateClientAndConnect();
|
||||
}
|
||||
_isBroken = false;
|
||||
}
|
||||
|
||||
_stream = _client.GetStream();
|
||||
}
|
||||
|
||||
public async Task<bool> BindIP(string ip)
|
||||
{
|
||||
bool bind = false;
|
||||
try
|
||||
{
|
||||
IPAddress IPLocal = IPAddress.Parse(ip);
|
||||
_client.Client.Bind(new IPEndPoint(IPLocal, 0));
|
||||
bind = true;
|
||||
}
|
||||
catch
|
||||
{
|
||||
bind = false;
|
||||
}
|
||||
await Task.Delay(10).ConfigureAwait(false);
|
||||
return bind;
|
||||
}
|
||||
|
||||
public override async Task OpenAsync()
|
||||
{
|
||||
string logstr;
|
||||
if (!IsOpen)
|
||||
{
|
||||
if (_client == null)
|
||||
CreateClientAndConnect();
|
||||
try
|
||||
{
|
||||
var connectTask = _client.ConnectAsync(_remoteHostname, _remotePort);
|
||||
if (await Task.WhenAny(connectTask, Task.Delay(5000)) == connectTask)
|
||||
{
|
||||
await connectTask.ConfigureAwait(false);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Connection timed out
|
||||
throw new TimeoutException("Connection timed out");
|
||||
}
|
||||
}
|
||||
catch (SocketException e)
|
||||
{
|
||||
logstr = e.Message;
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
}
|
||||
_isBroken = false;
|
||||
}
|
||||
_stream = _client.GetStream();
|
||||
}
|
||||
|
||||
|
||||
private void CreateClientAndConnect()
|
||||
{
|
||||
_client = new TcpClient(_remoteHostname, _remotePort)
|
||||
{
|
||||
ReceiveBufferSize = tcpReceiveBufferSize,
|
||||
ReceiveTimeout = tcpReceiveTimeout
|
||||
};
|
||||
}
|
||||
|
||||
private async Task CreateClientAndConnectAsync()
|
||||
{
|
||||
_client = new TcpClient
|
||||
{
|
||||
ReceiveBufferSize = tcpReceiveBufferSize,// 40 * 1024,
|
||||
ReceiveTimeout = tcpReceiveTimeout// 1200
|
||||
};
|
||||
await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -1,347 +1,355 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
|
||||
namespace Plane.CopterControllers
|
||||
{
|
||||
class GuidController
|
||||
{
|
||||
|
||||
static float fabsf(float vvalue)
|
||||
{
|
||||
return Math.Abs(vvalue);
|
||||
|
||||
}
|
||||
static float sqrt(float vvalue)
|
||||
{
|
||||
return (float)Math.Sqrt(vvalue);
|
||||
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 计算小航点飞行
|
||||
/// </summary>
|
||||
/// <param name="Distance"></param>
|
||||
/// <param name="fc_acc"></param>
|
||||
/// <param name="fc_maxspeed"></param>
|
||||
/// <returns></returns>
|
||||
|
||||
//单算减速，不算加速的最小飞行时间
|
||||
static float getMinfligthtime_v2(float Distance, float fc_acc, float fc_maxspeed)
|
||||
{
|
||||
float fDis = fabsf(Distance); // 总距离
|
||||
float facc = fabsf(fc_acc); // 减速度
|
||||
|
||||
// 物体开始时即以最大速度运动，不考虑加速过程
|
||||
float vel = fc_maxspeed;
|
||||
|
||||
// 计算减速所需的时间和距离
|
||||
// 减速时间 (从最大速度减速到0)
|
||||
float dectime = vel / facc;
|
||||
// 减速阶段覆盖的距离
|
||||
float decdis = (vel * dectime) - (0.5f * facc * dectime * dectime);
|
||||
|
||||
// 判断是否有足够的距离进行减速
|
||||
if (decdis >= fDis)
|
||||
{
|
||||
// 如果减速所需距离已经超过或等于总距离，调整最大速度
|
||||
// 使用公式 v^2 = u^2 + 2as 解出 v
|
||||
vel = (float)Math.Sqrt(2 * facc * fDis);
|
||||
// 重新计算减速时间
|
||||
dectime = vel / facc;
|
||||
}
|
||||
|
||||
// 计算匀速阶段时间
|
||||
float unftime = 0.0f;
|
||||
if (decdis < fDis)
|
||||
{
|
||||
// 如果有剩余距离进行匀速运动
|
||||
unftime = (fDis - decdis) / vel;
|
||||
}
|
||||
|
||||
// 总飞行时间 = 匀速阶段时间 + 减速阶段时间
|
||||
return unftime + dectime;
|
||||
}
|
||||
|
||||
//单算减速，不算加速的最大飞行速度
|
||||
public static float CalculateMaximumVelocity(float distance, float time, float acceleration)
|
||||
{
|
||||
// 二次方程系数
|
||||
float a_coeff = 1;
|
||||
float b_coeff = -2 * time;
|
||||
float c_coeff = (2 * distance) / acceleration;
|
||||
|
||||
// 计算判别式
|
||||
float discriminant = b_coeff * b_coeff - 4 * a_coeff * c_coeff;
|
||||
|
||||
if (discriminant < 0)
|
||||
{
|
||||
return -1; // 无实数解
|
||||
}
|
||||
|
||||
// 计算二次方程的根
|
||||
float t1_root1 = (-b_coeff + (float)Math.Sqrt(discriminant)) / (2 * a_coeff);
|
||||
float t1_root2 = (-b_coeff - (float)Math.Sqrt(discriminant)) / (2 * a_coeff);
|
||||
|
||||
// 选择合理的根作为 t1
|
||||
float t1 = Math.Min(t1_root1, t1_root2);
|
||||
if (t1 < 0 || t1 > time)
|
||||
{
|
||||
return -1; // 无合理解
|
||||
}
|
||||
|
||||
// 计算最大速度 v
|
||||
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)
|
||||
{
|
||||
vdist = 0;
|
||||
vspeed = 0;
|
||||
float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc);
|
||||
if ((desV == -1) || (desV > fc_maxspeed)) //飞不到
|
||||
{
|
||||
vspeed = Distance / flight_time;
|
||||
vdist = vspeed * curr_time; //匀速距离
|
||||
return 0;
|
||||
|
||||
}
|
||||
float dect = desV / fc_acc; // 总加速时间
|
||||
float unit = flight_time - dect; //匀速段时间
|
||||
float decD = (fc_acc * dect * dect) / 2; //总加速距离
|
||||
|
||||
if (dect > flight_time) dect = flight_time;//约束时间
|
||||
if (curr_time > dect) //大于加速段时间--匀速
|
||||
{
|
||||
vspeed = (Distance - decD) / (flight_time - dect);
|
||||
vdist = vspeed * (curr_time - dect);
|
||||
vdist = vdist + decD; //总距离=匀速距离+减速距离
|
||||
if (vdist > Distance) vdist = Distance; //约束距离
|
||||
|
||||
}
|
||||
else //加速段
|
||||
{
|
||||
vspeed = fc_acc * curr_time;
|
||||
vdist = (fc_acc * curr_time * curr_time) / 2; //加速距离
|
||||
}
|
||||
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)
|
||||
{
|
||||
vdist = 0;
|
||||
vspeed = 0;
|
||||
float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc);
|
||||
if ((desV == -1) || (desV > fc_maxspeed)) //飞不到
|
||||
{
|
||||
vspeed = Distance / flight_time;
|
||||
vdist = vspeed * curr_time; //匀速距离
|
||||
return 0;
|
||||
|
||||
}
|
||||
float dect = desV / fc_acc; // 总减速时间
|
||||
float unit = flight_time - dect; //匀速段时间
|
||||
float decD = desV * dect - (fc_acc * dect * dect) / 2; //总减速距离
|
||||
|
||||
if (dect > flight_time) dect = flight_time;//约束时间
|
||||
//匀速时间内
|
||||
if (curr_time < unit)
|
||||
{
|
||||
vspeed = (Distance - decD) / (flight_time - dect);
|
||||
vdist = vspeed * curr_time;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (((flight_time - dect) * unit) == 0)
|
||||
vdist = 0;
|
||||
else
|
||||
vdist = (Distance - decD) / (flight_time - dect) * unit; //匀速距离
|
||||
float currdect = curr_time - unit; //减速时间
|
||||
if (currdect >= 0)
|
||||
{
|
||||
vspeed = desV - fc_acc * currdect;
|
||||
decD = desV * currdect - (fc_acc * currdect * currdect) / 2; //减速距离
|
||||
}
|
||||
vdist = vdist + decD; //总距离=匀速距离+减速距离
|
||||
if (vdist > Distance) vdist = Distance; //约束距离
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
static float getMinfligthtime(float Distance, float fc_acc, float fc_maxspeed)
|
||||
{
|
||||
float fDis = fabsf(Distance);
|
||||
float facc = fabsf(fc_acc);
|
||||
|
||||
float realflytime = 0.0f;
|
||||
//计算一半的距离
|
||||
float hafdis = (fDis / 2);
|
||||
//计算最大速度
|
||||
float vel = (float)sqrt(2 * facc * hafdis);
|
||||
//如果速度超过最大速度
|
||||
if (vel > fc_maxspeed)
|
||||
//使用最大速度
|
||||
vel = fc_maxspeed;
|
||||
//加速时间
|
||||
float acctim = vel / facc;
|
||||
//加速距离
|
||||
float accdis = vel * vel / (2 * facc);
|
||||
//匀速段时间
|
||||
float vtime = (hafdis - accdis) / vel;
|
||||
//到一半的时间
|
||||
float haftime = acctim + vtime;
|
||||
realflytime = haftime * 2;
|
||||
return realflytime;
|
||||
}
|
||||
|
||||
///计算飞行距离和速度 单位--米，秒----
|
||||
//返回 整个距离最大飞行速度
|
||||
///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 plan_flytime = flight_time;
|
||||
//计算距离用绝对值
|
||||
float fDis = fabsf(Distance);
|
||||
if (curr_time < 0)
|
||||
{
|
||||
vdist = 0;
|
||||
vspeed = 0;
|
||||
return 0.0f;
|
||||
}
|
||||
//导航加速度 米/秒*秒 ---不使用导航库
|
||||
float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5;
|
||||
|
||||
//最大目标速度---米/秒
|
||||
float desired_velocity = 0;
|
||||
float dist = 0;
|
||||
//
|
||||
float speed = 0;
|
||||
|
||||
//计算最小飞行时间
|
||||
float minflytime = getMinfligthtime(Distance, fc_acc, fc_maxspeed);
|
||||
if (flight_time < minflytime)
|
||||
plan_flytime = minflytime;// + 0.1f;
|
||||
|
||||
|
||||
//根据总飞行时间和总距离计算飞行速度----起始和结束速度都是0，中间按匀加速和匀减速计算，没考虑加加速度
|
||||
float delta = (0.5f * plan_flytime) * (0.5f * plan_flytime) - fDis / wpacc;
|
||||
if (delta >= 0)
|
||||
{
|
||||
desired_velocity = (0.5f * plan_flytime - sqrt(delta)) / (1 / wpacc);
|
||||
if (desired_velocity > fc_maxspeed)
|
||||
{
|
||||
plan_flytime = minflytime;
|
||||
desired_velocity = fc_maxspeed;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
desired_velocity = (0.5f * plan_flytime) / (1 / wpacc);
|
||||
}
|
||||
if (desired_velocity < 0.2f) desired_velocity = 0.2f; //限制最小速度0.2米/秒
|
||||
if (desired_velocity > fc_maxspeed) desired_velocity = fc_maxspeed;//限制最大速度10米/秒，应该参数化
|
||||
|
||||
//计算实时速度
|
||||
if (curr_time <= (plan_flytime / 2.0))
|
||||
speed = curr_time * wpacc;
|
||||
else
|
||||
//需要减速
|
||||
speed = (plan_flytime - curr_time) * wpacc;
|
||||
//不能大于目标速度
|
||||
if (speed > desired_velocity)
|
||||
speed = desired_velocity;
|
||||
if (speed <= 0)
|
||||
speed = 0;
|
||||
vspeed = speed;
|
||||
//计算实时距离
|
||||
float V_start = 0.0f;
|
||||
float V_end = 0.0f;
|
||||
//加速段
|
||||
float t1 = (desired_velocity - V_start) / wpacc;
|
||||
//减速段
|
||||
float t3 = (desired_velocity - V_end) / wpacc;
|
||||
//平飞段
|
||||
float t2 = plan_flytime - t1 - t3;
|
||||
if (curr_time < t1)
|
||||
{
|
||||
dist = 0.5f * wpacc * curr_time * curr_time + curr_time * V_start;
|
||||
}
|
||||
else if (curr_time < t1 + t2)
|
||||
{
|
||||
dist = -0.5f * wpacc * t1 * t1 + (curr_time) * desired_velocity;
|
||||
}
|
||||
else
|
||||
{
|
||||
float t = curr_time - t1 - t2;
|
||||
dist = -0.5f * wpacc * t1 * t1 + curr_time * desired_velocity - 0.5f * wpacc * t * t;
|
||||
}
|
||||
|
||||
if (fabsf(dist) > fabsf(Distance))
|
||||
vdist = Distance;
|
||||
else
|
||||
{
|
||||
if (Distance < 0)
|
||||
vdist = -dist;
|
||||
else vdist = dist;
|
||||
}
|
||||
|
||||
return desired_velocity;
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
switch (flytype)
|
||||
{
|
||||
case 0: //匀速
|
||||
//case 1: //匀速
|
||||
vspeed = Distance / flight_time;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
|
||||
namespace Plane.CopterControllers
|
||||
{
|
||||
class GuidController
|
||||
{
|
||||
|
||||
float fabsf(float vvalue)
|
||||
{
|
||||
return Math.Abs(vvalue);
|
||||
|
||||
}
|
||||
float sqrt(float vvalue)
|
||||
{
|
||||
return (float)Math.Sqrt(vvalue);
|
||||
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 计算小航点飞行
|
||||
/// </summary>
|
||||
/// <param name="Distance"></param>
|
||||
/// <param name="fc_acc"></param>
|
||||
/// <param name="fc_maxspeed"></param>
|
||||
/// <returns></returns>
|
||||
|
||||
//单算减速，不算加速的最小飞行时间
|
||||
float getMinfligthtime_v2(float Distance, float fc_acc, float fc_maxspeed)
|
||||
{
|
||||
float fDis = fabsf(Distance); // 总距离
|
||||
float facc = fabsf(fc_acc); // 减速度
|
||||
|
||||
// 物体开始时即以最大速度运动，不考虑加速过程
|
||||
float vel = fc_maxspeed;
|
||||
|
||||
// 计算减速所需的时间和距离
|
||||
// 减速时间 (从最大速度减速到0)
|
||||
float dectime = vel / facc;
|
||||
// 减速阶段覆盖的距离
|
||||
float decdis = (vel * dectime) - (0.5f * facc * dectime * dectime);
|
||||
|
||||
// 判断是否有足够的距离进行减速
|
||||
if (decdis >= fDis)
|
||||
{
|
||||
// 如果减速所需距离已经超过或等于总距离，调整最大速度
|
||||
// 使用公式 v^2 = u^2 + 2as 解出 v
|
||||
vel = (float)Math.Sqrt(2 * facc * fDis);
|
||||
// 重新计算减速时间
|
||||
dectime = vel / facc;
|
||||
}
|
||||
|
||||
// 计算匀速阶段时间
|
||||
float unftime = 0.0f;
|
||||
if (decdis < fDis)
|
||||
{
|
||||
// 如果有剩余距离进行匀速运动
|
||||
unftime = (fDis - decdis) / vel;
|
||||
}
|
||||
|
||||
// 总飞行时间 = 匀速阶段时间 + 减速阶段时间
|
||||
return unftime + dectime;
|
||||
}
|
||||
|
||||
//单算减速，不算加速的最大飞行速度
|
||||
public float CalculateMaximumVelocity(float distance, float time, float acceleration)
|
||||
{
|
||||
// 二次方程系数
|
||||
float a_coeff = 1;
|
||||
float b_coeff = -2 * time;
|
||||
float c_coeff = (2 * distance) / acceleration;
|
||||
|
||||
// 计算判别式
|
||||
float discriminant = b_coeff * b_coeff - 4 * a_coeff * c_coeff;
|
||||
|
||||
if (discriminant < 0)
|
||||
{
|
||||
return -1; // 无实数解
|
||||
}
|
||||
|
||||
// 计算二次方程的根
|
||||
float t1_root1 = (-b_coeff + (float)Math.Sqrt(discriminant)) / (2 * a_coeff);
|
||||
float t1_root2 = (-b_coeff - (float)Math.Sqrt(discriminant)) / (2 * a_coeff);
|
||||
|
||||
// 选择合理的根作为 t1
|
||||
float t1 = Math.Min(t1_root1, t1_root2);
|
||||
if (t1 < 0 || t1 > time)
|
||||
{
|
||||
return -1; // 无合理解
|
||||
}
|
||||
|
||||
// 计算最大速度 v
|
||||
return acceleration * t1;
|
||||
}
|
||||
|
||||
//单算加速，不算减速的距离速度计算
|
||||
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;
|
||||
vspeed = 0;
|
||||
float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc);
|
||||
if ((desV == -1) || (desV > fc_maxspeed)) //飞不到
|
||||
{
|
||||
vspeed = Distance / flight_time;
|
||||
vdist = vspeed * curr_time; //匀速距离
|
||||
return 0;
|
||||
|
||||
}
|
||||
float dect = desV / fc_acc; // 总加速时间
|
||||
float unit = flight_time - dect; //匀速段时间
|
||||
float decD = (fc_acc * dect * dect) / 2; //总加速距离
|
||||
|
||||
if (dect > flight_time) dect = flight_time;//约束时间
|
||||
if (curr_time > dect) //大于加速段时间--匀速
|
||||
{
|
||||
vspeed = (Distance - decD) / (flight_time - dect);
|
||||
vdist = vspeed * (curr_time - dect);
|
||||
vdist = vdist + decD; //总距离=匀速距离+减速距离
|
||||
if (vdist > Distance) vdist = Distance; //约束距离
|
||||
|
||||
}
|
||||
else //加速段
|
||||
{
|
||||
vspeed = fc_acc * curr_time;
|
||||
vdist = (fc_acc * curr_time * curr_time) / 2; //加速距离
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
//单算减速，不算加速的距离速度计算
|
||||
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;
|
||||
vspeed = 0;
|
||||
float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc);
|
||||
if ((desV == -1) || (desV > fc_maxspeed)) //飞不到
|
||||
{
|
||||
vspeed = Distance / flight_time;
|
||||
vdist = vspeed * curr_time; //匀速距离
|
||||
return 0;
|
||||
|
||||
}
|
||||
float dect = desV / fc_acc; // 总减速时间
|
||||
float unit = flight_time - dect; //匀速段时间
|
||||
float decD = desV * dect - (fc_acc * dect * dect) / 2; //总减速距离
|
||||
|
||||
if (dect > flight_time) dect = flight_time;//约束时间
|
||||
//匀速时间内
|
||||
if (curr_time < unit)
|
||||
{
|
||||
vspeed = (Distance - decD) / (flight_time - dect);
|
||||
vdist = vspeed * curr_time;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (((flight_time - dect) * unit) == 0)
|
||||
vdist = 0;
|
||||
else
|
||||
vdist = (Distance - decD) / (flight_time - dect) * unit; //匀速距离
|
||||
float currdect = curr_time - unit; //减速时间
|
||||
if (currdect >= 0)
|
||||
{
|
||||
vspeed = desV - fc_acc * currdect;
|
||||
decD = desV * currdect - (fc_acc * currdect * currdect) / 2; //减速距离
|
||||
}
|
||||
vdist = vdist + decD; //总距离=匀速距离+减速距离
|
||||
if (vdist > Distance) vdist = Distance; //约束距离
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
float getMinfligthtime(float Distance, float fc_acc, float fc_maxspeed)
|
||||
{
|
||||
float fDis = fabsf(Distance);
|
||||
float facc = fabsf(fc_acc);
|
||||
|
||||
float realflytime = 0.0f;
|
||||
//计算一半的距离
|
||||
float hafdis = (fDis / 2);
|
||||
//计算最大速度
|
||||
float vel = (float)sqrt(2 * facc * hafdis);
|
||||
//如果速度超过最大速度
|
||||
if (vel > fc_maxspeed)
|
||||
//使用最大速度
|
||||
vel = fc_maxspeed;
|
||||
//加速时间
|
||||
float acctim = vel / facc;
|
||||
//加速距离
|
||||
float accdis = vel * vel / (2 * facc);
|
||||
//匀速段时间
|
||||
float vtime = (hafdis - accdis) / vel;
|
||||
//到一半的时间
|
||||
float haftime = acctim + vtime;
|
||||
realflytime = haftime * 2;
|
||||
return realflytime;
|
||||
}
|
||||
|
||||
///计算飞行距离和速度 单位--米，秒----
|
||||
//返回 整个距离最大飞行速度
|
||||
///flight_time 总飞行时间 Distance 飞行总距离 curr_time 当前飞行时间 fc_acc加速度, fc_maxspeed最大速度 vdist 计算的应该飞的距离 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 desired_velocity=_desired_velocity;
|
||||
float dist = 0;
|
||||
//导航加速度 米/秒*秒 ---不使用导航库
|
||||
float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5;
|
||||
//
|
||||
float speed = 0;
|
||||
//计算实时速度
|
||||
if (curr_time <= (plan_flytime / 2.0))
|
||||
speed = curr_time * wpacc;
|
||||
else
|
||||
//需要减速
|
||||
speed = (plan_flytime - curr_time) * wpacc;
|
||||
//不能大于目标速度
|
||||
if (speed > desired_velocity)
|
||||
speed = desired_velocity;
|
||||
if (speed <= 0)
|
||||
speed = 0;
|
||||
vspeed = speed;
|
||||
//计算实时距离
|
||||
float V_start = 0.0f;
|
||||
float V_end = 0.0f;
|
||||
//加速段
|
||||
float t1 = (desired_velocity - V_start) / wpacc;
|
||||
//减速段
|
||||
float t3 = (desired_velocity - V_end) / wpacc;
|
||||
//平飞段
|
||||
float t2 = plan_flytime - t1 - t3;
|
||||
if (curr_time < t1)
|
||||
{
|
||||
dist = 0.5f * wpacc * curr_time * curr_time + curr_time * V_start;
|
||||
}
|
||||
else if (curr_time < t1 + t2)
|
||||
{
|
||||
dist = -0.5f * wpacc * t1 * t1 + (curr_time) * desired_velocity;
|
||||
}
|
||||
else
|
||||
{
|
||||
float t = curr_time - t1 - t2;
|
||||
dist = -0.5f * wpacc * t1 * t1 + curr_time * desired_velocity - 0.5f * wpacc * t * t;
|
||||
}
|
||||
|
||||
if (fabsf(dist) > fabsf(Distance))
|
||||
vdist = Distance;
|
||||
else
|
||||
{
|
||||
if (Distance < 0)
|
||||
vdist = -dist;
|
||||
else vdist = dist;
|
||||
}
|
||||
|
||||
return desired_velocity;
|
||||
}
|
||||
|
||||
float _vspeed=0;
|
||||
|
||||
float _desired_velocity = 0;
|
||||
float _plan_flytime = 0;
|
||||
|
||||
float initgetspeeddist_V1(float flight_time, float Distance, float fc_acc, float fc_maxspeed)
|
||||
{
|
||||
//计划飞行时间
|
||||
float plan_flytime = flight_time;
|
||||
//计算距离用绝对值
|
||||
float fDis = fabsf(Distance);
|
||||
|
||||
//导航加速度 米/秒*秒 ---不使用导航库
|
||||
float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5;
|
||||
|
||||
//最大目标速度---米/秒
|
||||
float desired_velocity = 0;
|
||||
|
||||
|
||||
//计算最小飞行时间
|
||||
float minflytime = getMinfligthtime(Distance, fc_acc, fc_maxspeed);
|
||||
if (flight_time < minflytime)
|
||||
plan_flytime = minflytime;// + 0.1f;
|
||||
|
||||
|
||||
//根据总飞行时间和总距离计算飞行速度----起始和结束速度都是0，中间按匀加速和匀减速计算，没考虑加加速度
|
||||
float delta = (0.5f * plan_flytime) * (0.5f * plan_flytime) - fDis / wpacc;
|
||||
if (delta >= 0)
|
||||
{
|
||||
desired_velocity = (0.5f * plan_flytime - sqrt(delta)) / (1 / wpacc);
|
||||
if (desired_velocity > fc_maxspeed)
|
||||
{
|
||||
plan_flytime = minflytime;
|
||||
desired_velocity = fc_maxspeed;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
desired_velocity = (0.5f * plan_flytime) / (1 / wpacc);
|
||||
}
|
||||
if (desired_velocity < 0.2f) desired_velocity = 0.2f; //限制最小速度0.2米/秒
|
||||
if (desired_velocity > fc_maxspeed) desired_velocity = fc_maxspeed;//限制最大速度10米/秒，应该参数化
|
||||
|
||||
_desired_velocity = desired_velocity;
|
||||
_plan_flytime = plan_flytime;
|
||||
return desired_velocity;
|
||||
}
|
||||
|
||||
|
||||
public float initgetspeeddist(int flytype, float flight_time, float Distance, float fc_acc, float fc_maxspeed)
|
||||
{
|
||||
flytype = 0;
|
||||
switch (flytype)
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue
Block a user