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

21
PlaneGcsSdk_Shared/Communication/TcpConnection.cs
View File

@ -73,27 +73,34 @@ namespace Plane.Communication
string logstr;
if (!IsOpen)
{
if (_client == null)
CreateClientAndConnect();
try
{
await _client.ConnectAsync(_remoteHostname, _remotePort).ConfigureAwait(false);
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");
}
}
//屏蔽掉异常处理
//CreateClientAndConnectAsync会new一个TcpClient并且重新连接
//之前设置的TcpClient中Socket Bind会无效，在多网卡工作时会导致断线重连的时间特别长
catch (SocketException e)
{
logstr= e.Message;
//await CreateClientAndConnectAsync();
logstr = e.Message;
}
catch (ObjectDisposedException)
{
//await CreateClientAndConnectAsync();
}
_isBroken = false;
}
_stream = _client.GetStream();
}
private void CreateClientAndConnect()
{
_client = new TcpClient(_remoteHostname, _remotePort)

7
PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs
View File

@ -170,6 +170,7 @@ namespace Plane.CommunicationManagement
{
try
{
if (!Connection.IsOnline)
await Connection.OpenAsync().ConfigureAwait(false);
}
catch
@ -179,6 +180,8 @@ namespace Plane.CommunicationManagement
if (Connection.IsOnline)
{
Message.Connect(true);
Message.Show($"通讯基站连接成功!");
times = 0;
SendQuery();
await StartReadingPacketsAsync();
}
@ -239,7 +242,7 @@ namespace Plane.CommunicationManagement
}
*/
int times = 1;
int times = 0;
private void Reconnect()
{
// Message.Show($"正在重新连接...");
@ -247,7 +250,7 @@ namespace Plane.CommunicationManagement
{
CloseConnection();
await Task.Delay(250).ConfigureAwait(false);
//Message.Show($"正在重连:次数{times++}");
Message.Show($"正在重连:次数{++times}");
await Task.Delay(250).ConfigureAwait(false);
await ConnectAsync();

158
PlaneGcsSdk_Shared/CopterControllers/GuidController.cs
View File

@ -7,12 +7,12 @@ namespace Plane.CopterControllers
class GuidController
{
static float fabsf(float vvalue)
float fabsf(float vvalue)
{
return Math.Abs(vvalue);
}
static float sqrt(float vvalue)
float sqrt(float vvalue)
{
return (float)Math.Sqrt(vvalue);
@ -28,7 +28,7 @@ namespace Plane.CopterControllers
/// <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 facc = fabsf(fc_acc); // 减速度
@ -65,7 +65,7 @@ namespace Plane.CopterControllers
}
//单算减速，不算加速的最大飞行速度
public static float CalculateMaximumVelocity(float distance, float time, float acceleration)
public float CalculateMaximumVelocity(float distance, float time, float acceleration)
{
// 二次方程系数
float a_coeff = 1;
@ -96,7 +96,7 @@ namespace Plane.CopterControllers
}
//单算加速，不算减速的距离速度计算
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;
vspeed = 0;
@ -130,7 +130,7 @@ namespace Plane.CopterControllers
}
//单算减速，不算加速的距离速度计算
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;
vspeed = 0;
@ -173,7 +173,7 @@ namespace Plane.CopterControllers
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 facc = fabsf(fc_acc);
@ -203,51 +203,15 @@ namespace Plane.CopterControllers
//返回 整个距离最大飞行速度
///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 = flight_time;
//计算距离用绝对值
float fDis = fabsf(Distance);
if (curr_time < 0)
{
vdist = 0;
vspeed = 0;
return 0.0f;
}
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 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;
@ -295,13 +259,83 @@ namespace Plane.CopterControllers
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)
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 = Distance / flight_time;
vspeed = _vspeed;
vdist = vspeed * curr_time;
return 0;
case 1: //同时计算加减速
@ -317,31 +351,5 @@ namespace Plane.CopterControllers
}
}
}
}

19
PlaneGcsSdk_Shared/Copters/FakeCopter.cs
View File

@ -225,6 +225,8 @@ namespace Plane.Copters
GpsHdop = 1;
IsGpsAccurate = true;
HasSwitchedToGpsMode = true;
GC_xy = new GuidController();
GC_z = new GuidController();
/*
// 持续假装收到飞行器发来的心跳。
@ -260,6 +262,9 @@ namespace Plane.Copters
public string Id { get; set; }
private int ReturnToLaunchAltitude { get { return _takeOffTargetAltitude; } }
private GuidController GC_xy;
private GuidController GC_z;
public Task ConnectAsync()
{
@ -304,6 +309,14 @@ namespace Plane.Copters
_flytype = flytype;
// _xy_speed = _distance_xy / _flytime;
// _z_speed = _distance_z / _flytime;
GC_xy.initgetspeeddist(_flytype, _flytime / 1000, _distance_xy, acc_xy, maxspeed_xy);
if (_targetAlt>Altitude )
GC_z.initgetspeeddist(_flytype, _flytime / 1000, _distance_z, acc_z, maxspeed_up);
else
GC_z.initgetspeeddist(_flytype, _flytime / 1000, _distance_z, acc_z, maxspeed_down);
Mode = FlightMode.GUIDED;
@ -895,12 +908,12 @@ namespace Plane.Copters
float vspeed = 0;
GuidController.getspeeddist(_flytype, _flytime / 1000, _distance_xy, currflytime / 1000, acc_xy,maxspeed_xy , out currdis_xy, out vspeed);
GC_xy.getspeeddist(_flytype, _flytime / 1000, _distance_xy, currflytime / 1000, acc_xy,maxspeed_xy , out currdis_xy, out vspeed);
if (alt> Altitude)
GuidController.getspeeddist(_flytype, _flytime / 1000, _distance_z, currflytime / 1000, acc_z,maxspeed_up, out currdis_z, out vspeed);
GC_z.getspeeddist(_flytype, _flytime / 1000, _distance_z, currflytime / 1000, acc_z,maxspeed_up, out currdis_z, out vspeed);
else
GuidController.getspeeddist(_flytype, _flytime / 1000, _distance_z, currflytime / 1000, acc_z,maxspeed_down, out currdis_z, out vspeed);
GC_z.getspeeddist(_flytype, _flytime / 1000, _distance_z, currflytime / 1000, acc_z,maxspeed_down, out currdis_z, out vspeed);
Console.WriteLine($"{this.Id}：time:{currflytime / 1000} to {currdis_z}");