diff --git a/PlaneGcsSdk.Contract_Shared/Copters/ICopterActionsSharedByCopterManager.cs b/PlaneGcsSdk.Contract_Shared/Copters/ICopterActionsSharedByCopterManager.cs
index 90f82b5..0e073fb 100644
--- a/PlaneGcsSdk.Contract_Shared/Copters/ICopterActionsSharedByCopterManager.cs
+++ b/PlaneGcsSdk.Contract_Shared/Copters/ICopterActionsSharedByCopterManager.cs
@@ -32,7 +32,8 @@ namespace Plane.Copters
/// 相对于解锁点的高度。
/// 表示此命令异步发送操作的 实例。
/// flytime =飞行时间 秒
- Task FlyToAsync(double lat, double lng, float alt,float flytime=0);
+ /// flytype 0;匀速 1;加减速 2;单加速 3;单减速
+ Task FlyToAsync(double lat, double lng, float alt,float flytime=0,int flytype = 0);
///
/// 切换到 并悬停。此操作需要使用 GPS 定位，卫星数不足导致定位不准时非常危险。
diff --git a/PlaneGcsSdk.Contract_Shared/Copters/ICopterAttribute.cs b/PlaneGcsSdk.Contract_Shared/Copters/ICopterAttribute.cs
index e66a729..6507432 100644
--- a/PlaneGcsSdk.Contract_Shared/Copters/ICopterAttribute.cs
+++ b/PlaneGcsSdk.Contract_Shared/Copters/ICopterAttribute.cs
@@ -16,5 +16,11 @@ namespace Plane.Copters
float RetainInt{ get; }
//模拟飞行更新间隔
int sim_update_int { get; set; }
+ float maxspeed_xy { get; set; }
+ float maxspeed_up { get; set; }
+ float maxspeed_down { get; set; }
+ float acc_z { get; set; }
+ float acc_xy { get; set; }
+
}
}
diff --git a/PlaneGcsSdk_Shared/CopterControllers/GuidController.cs b/PlaneGcsSdk_Shared/CopterControllers/GuidController.cs
new file mode 100644
index 0000000..bc7de4f
--- /dev/null
+++ b/PlaneGcsSdk_Shared/CopterControllers/GuidController.cs
@@ -0,0 +1,347 @@
+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);
+
+ }
+
+
+ ///
+ /// 计算小航点飞行
+ ///
+ ///
+ ///
+ ///
+ ///
+
+ //单算减速，不算加速的最小飞行时间
+ 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;
+ }
+ }
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ }
+ }
diff --git a/PlaneGcsSdk_Shared/CopterManagement/SingleCopterManager.cs b/PlaneGcsSdk_Shared/CopterManagement/SingleCopterManager.cs
index a90a3a2..365954c 100644
--- a/PlaneGcsSdk_Shared/CopterManagement/SingleCopterManager.cs
+++ b/PlaneGcsSdk_Shared/CopterManagement/SingleCopterManager.cs
@@ -104,7 +104,7 @@ namespace Plane.CopterManagement
return Copter.FlyToAsync(lat, lng);
}
- public Task FlyToAsync(double lat, double lng, float alt, float flytime = 0)
+ public Task FlyToAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
{
return Copter.FlyToAsync(lat, lng, alt);
}
diff --git a/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs b/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs
index bb6e956..5b0aca5 100644
--- a/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs
+++ b/PlaneGcsSdk_Shared/Copters/CopterImplSharedPart.cs
@@ -180,6 +180,12 @@ namespace Plane.Copters
private bool _DisplayID = true;
+ private float _maxspeed_xy=3;
+ private float _maxspeed_down=3;
+ private float _maxspeed_up = 3;
+ private float _acc_z=1f;
+ private float _acc_xy=2.5f;
+
#endregion Backing Fields
public CopterImplSharedPart(SynchronizationContext uiSyncContext) : base(uiSyncContext)
@@ -683,6 +689,48 @@ namespace Plane.Copters
}
+ public float maxspeed_xy {
+ get { return _maxspeed_xy; }
+ set
+ {
+ Set(nameof(maxspeed_xy), ref _maxspeed_xy, value);
+ }
+ }
+
+ public float maxspeed_up
+ {
+ get { return _maxspeed_up; }
+ set
+ {
+ Set(nameof(maxspeed_up), ref _maxspeed_up, value);
+ }
+ }
+ public float maxspeed_down
+ {
+ get { return _maxspeed_down; }
+ set
+ {
+ Set(nameof(maxspeed_down), ref _maxspeed_down, value);
+ }
+ }
+ public float acc_z
+ {
+ get { return _acc_z; }
+ set
+ {
+ Set(nameof(acc_z), ref _acc_z, value);
+ }
+ }
+ public float acc_xy
+ {
+ get { return _acc_xy; }
+ set
+ {
+ Set(nameof(acc_xy), ref _acc_xy, value);
+ }
+ }
+
+
public byte[] Retain
{
@@ -865,11 +913,11 @@ namespace Plane.Copters
return FlyToAsync(lat, lng, Altitude);
}
- public Task FlyToAsync(double lat, double lng, float alt, float flytime = 0)
+ public Task FlyToAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
{
_shouldFollow = false;
//State = CopterState.CommandMode;
- return FlyToCoreAsync(lat, lng, alt, flytime);
+ return FlyToCoreAsync(lat, lng, alt, flytime, flytype);
}
public void Follow(IVisibleStatus target, bool keepYawDifference = false, bool keepFacingTarget = true, bool keep3DRelativeLocations = false)
@@ -1125,7 +1173,7 @@ namespace Plane.Copters
/// 经度。
/// 高度。
/// 表示此异步发送操作的 实例。
- protected abstract Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0);
+ protected abstract Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0);
protected void RaiseAltitudeChanged() => AltitudeChanged?.Invoke(this, EventArgs.Empty);
diff --git a/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs b/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs
index e54d73d..9c55c68 100644
--- a/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs
+++ b/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs
@@ -144,7 +144,7 @@ namespace Plane.Copters
return Task.FromResult(true);
}
- protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0)
+ protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
{
return TaskUtils.CompletedTask;
}
diff --git a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs
index 256c545..240748c 100644
--- a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs
+++ b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs
@@ -7,6 +7,8 @@ using System.Threading;
using System.Threading.Tasks;
using static Plane.Copters.Constants;
using System.Windows.Media;
+using Plane.CopterControllers;
+
namespace Plane.Copters
{
///
@@ -89,8 +91,9 @@ namespace Plane.Copters
private float _distance_xy;
private float _distance_z;
- private float _distance_xy_delta;
- private float _distance_z_delta;
+ private float _xy_speed;
+ private float _z_speed;
+ private int _flytype;
private float currflytime;
// 根据飞行时间计算飞行距离。
private float currdis_xy;
@@ -274,7 +277,7 @@ namespace Plane.Copters
return TaskUtils.CompletedTask;
}
- protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0)
+ protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
{
if (!IsEmergencyHoverActive)
{
@@ -298,10 +301,9 @@ namespace Plane.Copters
_distance_z = alt - Altitude;
Console.WriteLine($"{this.Id}d:{_distance_xy},lat{this.Latitude},lng{this.Longitude },tlat:{lat}tlng:{lng}");
- _distance_xy_delta = _distance_xy / _flytime;
- _distance_z_delta = _distance_z / _flytime;
-
-
+ _flytype = flytype;
+ // _xy_speed = _distance_xy / _flytime;
+ // _z_speed = _distance_z / _flytime;
Mode = FlightMode.GUIDED;
@@ -674,6 +676,7 @@ namespace Plane.Copters
break;
case FlightMode.AUTO:
+ /*
// 暂时只管起飞。以最大速度上升，直到达到目标高度。
if (Altitude < _takeOffTargetAltitude)
{
@@ -683,6 +686,7 @@ namespace Plane.Copters
{
Mode = FlightMode.LOITER;
}
+ */
break;
case FlightMode.GUIDED:
@@ -888,8 +892,18 @@ namespace Plane.Copters
//if (currflytime > 13000)
// return;
// 根据飞行时间计算飞行距离
- currdis_xy = _distance_xy_delta * currflytime;
- currdis_z = _distance_z_delta * currflytime;
+
+ float vspeed = 0;
+
+ GuidController.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);
+ else
+ GuidController.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}");
+
// 距离已经很近，直接移动到目标点。
if ((Math.Abs(_distance_xy-currdis_xy) < 0.1)&& (Math.Abs(_distance_z - currdis_z) < 0.1))
{
diff --git a/PlaneGcsSdk_Shared/Copters/PLCopter.cs b/PlaneGcsSdk_Shared/Copters/PLCopter.cs
index 2203513..2bbcf84 100644
--- a/PlaneGcsSdk_Shared/Copters/PLCopter.cs
+++ b/PlaneGcsSdk_Shared/Copters/PLCopter.cs
@@ -418,7 +418,7 @@ namespace Plane.Copters
return !anotherSetModeActionCalled && Mode == mode;
}
- protected override async Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0)
+ protected override async Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
{
if (!IsEmergencyHoverActive)
{
diff --git a/PlaneGcsSdk_Shared/PlaneGcsSdk_Shared.projitems b/PlaneGcsSdk_Shared/PlaneGcsSdk_Shared.projitems
index ceddaef..b0f95b8 100644
--- a/PlaneGcsSdk_Shared/PlaneGcsSdk_Shared.projitems
+++ b/PlaneGcsSdk_Shared/PlaneGcsSdk_Shared.projitems
@@ -34,6 +34,7 @@
+