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

19
PlaneGcsSdk_Shared/Communication/TcpConnection.cs
View File

@ -73,27 +73,34 @@ namespace Plane.Communication
string logstr; string logstr;
if (!IsOpen) if (!IsOpen)
{ {
if (_client == null)
CreateClientAndConnect();
try 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) catch (SocketException e)
{ {
logstr = e.Message; logstr = e.Message;
//await CreateClientAndConnectAsync();
} }
catch (ObjectDisposedException) catch (ObjectDisposedException)
{ {
//await CreateClientAndConnectAsync();
} }
_isBroken = false; _isBroken = false;
} }
_stream = _client.GetStream(); _stream = _client.GetStream();
} }
private void CreateClientAndConnect() private void CreateClientAndConnect()
{ {
_client = new TcpClient(_remoteHostname, _remotePort) _client = new TcpClient(_remoteHostname, _remotePort)

7
PlaneGcsSdk_Shared/CommunicationManagement/CommModule.cs
View File

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

158
PlaneGcsSdk_Shared/CopterControllers/GuidController.cs
View File

@ -7,12 +7,12 @@ 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);
@ -28,7 +28,7 @@ namespace Plane.CopterControllers
/// <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); // 减速度
@ -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; 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; vdist = 0;
vspeed = 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; vdist = 0;
vspeed = 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 fDis = fabsf(Distance);
float facc = fabsf(fc_acc); float facc = fabsf(fc_acc);
@ -203,51 +203,15 @@ namespace Plane.CopterControllers
//返回 整个距离最大飞行速度 //返回 整个距离最大飞行速度
///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)
{
vdist = 0;
vspeed = 0;
return 0.0f;
}
//导航加速度 米/秒*秒 ---不使用导航库 //导航加速度 米/秒*秒 ---不使用导航库
float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5; float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5;
//最大目标速度---米/秒
float desired_velocity = 0;
float dist = 0;
// //
float speed = 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)) if (curr_time <= (plan_flytime / 2.0))
speed = curr_time * wpacc; speed = curr_time * wpacc;
@ -295,13 +259,83 @@ namespace Plane.CopterControllers
return desired_velocity; 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) switch (flytype)
{ {
case 0: //匀速 case 0: //匀速
//case 1: //匀速 //case 1: //匀速
vspeed = Distance / flight_time; vspeed = _vspeed;
vdist = vspeed * curr_time; vdist = vspeed * curr_time;
return 0; return 0;
case 1: //同时计算加减速 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; GpsHdop = 1;
IsGpsAccurate = true; IsGpsAccurate = true;
HasSwitchedToGpsMode = true; HasSwitchedToGpsMode = true;
GC_xy = new GuidController();
GC_z = new GuidController();
/* /*
// 持续假装收到飞行器发来的心跳。 // 持续假装收到飞行器发来的心跳。
@ -260,6 +262,9 @@ namespace Plane.Copters
public string Id { get; set; } public string Id { get; set; }
private int ReturnToLaunchAltitude { get { return _takeOffTargetAltitude; } } private int ReturnToLaunchAltitude { get { return _takeOffTargetAltitude; } }
private GuidController GC_xy;
private GuidController GC_z;
public Task ConnectAsync() public Task ConnectAsync()
{ {
@ -304,6 +309,14 @@ namespace Plane.Copters
_flytype = flytype; _flytype = flytype;
// _xy_speed = _distance_xy / _flytime; // _xy_speed = _distance_xy / _flytime;
// _z_speed = _distance_z / _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; Mode = FlightMode.GUIDED;
@ -895,12 +908,12 @@ namespace Plane.Copters
float vspeed = 0; 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) 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 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}"); Console.WriteLine($"{this.Id}：time:{currflytime / 1000} to {currdis_z}");