Skip to content

T
Thick-Common
Commit 87cb1e4e authored by 潘栩锋's avatar 潘栩锋 🚴
Browse files
Options

修复 timegrid每个数据是精确1.28ms，但中间会有数据丢失，导致每个数据包虽然是200个，间隔确不是200*1.28ms。 

TimeGridAdvHelper 确保输出数列肯定是1.28ms，数据不够插值补齐
parent 5c7a9769
Hide whitespace changes
Inline Side-by-side
Showing with 207 additions and 60 deletions
Project.FLY.FlyADBase/FlyADBase/FlyAD7.cs
View file @ 87cb1e4e
......@@ -71,7 +71,9 @@ namespace FlyADBase
bool isReadyGetPos2 = false;
bool isReadyGetIO = false;
static FlyAD7() {
propertyName_save = FlyAD7JsonDb.GetMemberNames();
}
/// <summary>
///
/// </summary>
......@@ -405,7 +407,7 @@ namespace FlyADBase
FLYAD7_OBJ_INTERFACE.FLYIO_OBJ_INTERFACE.GET_IO);
NotifyPropertyChanged(nameof(Pos1LCShift));//写入到AD盒
NotifyPropertyChanged(nameof(Pos2Comp));//写入到AD盒
SyncPos2Clear();
SyncClear();
......@@ -778,6 +780,11 @@ namespace FlyADBase
}
else if (version == 2)
{
if (pack.inChange == 0)
{
//异常包，， 这个是AD盒的bug, systick 是错的！！！！！ 不能让它通过
return;
}
IStatus = pack.istatus;
OStatus = pack.ostatus;
Position = pack.pos1;
......@@ -1593,10 +1600,6 @@ namespace FlyADBase
}.ToBytes());
}
/// <summary>
/// 纵向值补偿系数，补偿时，先乘除后加减 不应该有！！！
/// </summary>
public float Pos2Comp { get; set; } = 1;
/// <summary>
/// 纵向同步事件，0-1事件
......@@ -1830,13 +1833,8 @@ namespace FlyADBase
#endregion
#endregion
private static string[] propertyName_save = new string[] {
nameof(LocalEP),
nameof(HasCRC),
nameof(GridSmooth),
nameof(IsCalSpeed),
nameof(ADLag)
};
private static string[] propertyName_save;
string defaultPath = "flyad.json";
private string jsonDbPath;
public bool Load()
......@@ -1851,6 +1849,15 @@ namespace FlyADBase
this.PropertyChanged -= FlyAD7_PropertyChanged2;
bool ret = FlyAD7JsonDb.Load(jsonDbPath, this);
this.PropertyChanged += FlyAD7_PropertyChanged2;
MotorType = Backup_MotorType;
PosOfGrid = Backup_PosOfGrid;
Ratio01 = Backup_Ratio01;
Ratio02 = Backup_Ratio02;
PosOffset = Backup_PosOffset;
JogVelocity = Backup_JogVelocity;
return ret;
}
......@@ -1919,6 +1926,23 @@ namespace FlyADBase
public int GridSmooth = 0;
public bool IsCalSpeed = true;
public int ADLag = 0;
public int PosMin = 0;
public int PosMax = 8900;
public bool HasPosMaxMin = false;
//备份参数
public MOTORTYPE Backup_MotorType = MOTORTYPE.SERVO;
public UInt16 Backup_PosOfGrid = 10;
public UInt16 Backup_Ratio01 = 4;
public UInt16 Backup_Ratio02 = 1;
public Int16 Backup_PosOffset = 0;
public UInt32 Backup_JogVelocity = 5000;
public static string[] GetMemberNames() {
var type = typeof(FlyAD7JsonDb);
return type.GetMembers().Select(mi => mi.Name).ToArray();
}
}
......
Project.FLY.FlyADBase/FlyADBase/FlyADClientAdv.cs
View file @ 87cb1e4e
......@@ -29,6 +29,32 @@ namespace FlyADBase
/// 机架总长
/// </summary>
public int PosLen { get; set; } = 8900;
/// <summary>
/// 使用独立的脉冲最大最小值
/// </summary>
public bool HasPosMaxMin { get; set; }
/// <summary>
/// 最小脉冲，默认是0
/// </summary>
public int PosMin { get; set; } = 0;
/// <summary>
/// 最大脉冲，默认于PosLen 一样
/// </summary>
public int PosMax { get; set; } = 8900;
#region 备份参数
public MOTORTYPE Backup_MotorType { get; set; } = MOTORTYPE.SERVO;
public UInt16 Backup_PosOfGrid { get; set; } = 10;
public UInt16 Backup_Ratio01 { get; set; } = 4;
public UInt16 Backup_Ratio02 { get; set; } = 1;
public Int16 Backup_PosOffset { get; set; } = 0;
public UInt32 Backup_JogVelocity { get; set; } = 300;
#endregion
public int GridLen => PosLen / PosOfGrid;
/// <summary>
/// Speed1 = Velocity * Speed1Scale
......@@ -209,7 +235,10 @@ namespace FlyADBase
/// </summary>
public void RuntoMin()
{
Runto(0);
if (HasPosMaxMin)
Runto(PosMin);
else
Runto(0);
}
/// <summary>
......@@ -217,7 +246,10 @@ namespace FlyADBase
/// </summary>
public void RuntoMax()
{
Runto(PosLen);
if (HasPosMaxMin)
Runto(PosMax);
else
Runto(PosLen);
}
/// <summary>
......@@ -411,15 +443,14 @@ namespace FlyADBase
void advPushTimeGrid(DateTime end_dt, int[] datas)
{
double ts_ms = 1.28;
long ticks = (long)((ts_ms * datas.Length) * TimeSpan.TicksPerMillisecond);
#region 高级 timegrid
//TODO
mTimeGridAdvHelper.AddAD(end_dt - TimeSpan.FromMilliseconds(ADLag), datas);
if (isTimeToPushTimeGridAdv) {
isTimeToPushTimeGridAdv = false;
if (IsTimeToPushTimeGridAdv) {
IsTimeToPushTimeGridAdv = false;
if (TimeGridAdv2Event != null)
{
......@@ -479,14 +510,19 @@ namespace FlyADBase
#endregion
}
bool isTimeToPushTimeGridAdv = false;
/// <summary>
/// 动作指令完成，准备推送 timegridadv 事件
/// </summary>
public bool IsTimeToPushTimeGridAdv = false;
void advPushStatus()
{
mTimeGridAdvHelper.AddDriveStatus(Now, DriveOrder, DriveStatus, Marker);
if (DriveStatus != DRIVE_MAN_STATUS.RUNNING)
{
//通知 mTimeGridAdvHelper 下次触发 timegridadv
isTimeToPushTimeGridAdv = true;
IsTimeToPushTimeGridAdv = true;
}
}
......
Project.FLY.FlyADBase/FlyADBase/IFlyAD.cs
View file @ 87cb1e4e
......@@ -285,10 +285,6 @@ namespace FlyADBase
/// </summary>
void SetPos2Offset(int offset);
/// <summary>
/// 纵向值补偿系数，补偿时，先乘除后加减
/// </summary>
float Pos2Comp { get; set; }
/// <summary>
/// 纵向同步事件，0-1事件
/// </summary>
......
Project.FLY.FlyADBase/FlyADBase/IFlyADClientAdv.cs
View file @ 87cb1e4e
......@@ -100,11 +100,34 @@ namespace FlyADBase
/// </summary>
double Speed1Scale { get; }
#region 备份参数
MOTORTYPE Backup_MotorType { get; }
UInt16 Backup_PosOfGrid { get; }
UInt16 Backup_Ratio01 { get; }
UInt16 Backup_Ratio02 { get; }
Int16 Backup_PosOffset { get; }
UInt32 Backup_JogVelocity { get; }
#endregion
/// <summary>
/// 机架总长
/// </summary>
int PosLen { get; set; }
/// <summary>
/// 使用独立的脉冲最大最小值
/// </summary>
bool HasPosMaxMin { get; set; }
/// <summary>
/// 最小脉冲，默认是0
/// </summary>
int PosMin { get; set; }
/// <summary>
/// 最大脉冲，默认于PosLen 一样
/// </summary>
int PosMax { get; set; }
/// <summary>
/// Runto(0)， 不同于 Backward
/// </summary>
......
Project.FLY.FlyADBase/FlyADBase/TimeGridAdvHelper.cs
View file @ 87cb1e4e
......@@ -17,7 +17,7 @@ namespace FlyADBase
/// bool ret = mTimeGridAdvHelper.GetLastRunningTime(out DateTime beginTime, out DateTime endTime, out int marker, out DRIVE_MAN_ORDER order);
///
/// 获取 指定时间范围的 timegrid 数据
/// var adList = mTimeGridAdvHelper.GetTimeGrid(beginTime, endTime, out DateTime reponse_endTime);
/// var adList = mTimeGridAdvHelper.GetAD(beginTime, endTime, out DateTime reponse_endTime);
///
/// 获取 指定时间范围的 pos 数据
/// var posList = mTimeGridAdvHelper.GetPos(reponse_endTime,adList.Count());
......@@ -40,6 +40,8 @@ namespace FlyADBase
#region 一分钟数据缓存池, 目标每个AD数据（1.28ms） 都有对应的其它数据
/// <summary>
/// AD数据池时间间隔为1.28ms, 肯定最少有一个数据
/// 每个数据包（200个数据）理论间隔是200*1.28ms， 但实际上不是。
/// 数据输出时，输出的每个数据必须是1.28ms, 需要通过数据包的systick线性填充数据
/// </summary>
public List<DateTimeUnit3> ADPool = new List<DateTimeUnit3>();
......@@ -64,7 +66,7 @@ namespace FlyADBase
public List<DateTimeUnit4> DriveStatusPool = new List<DateTimeUnit4>();
#endregion
DateTime NewestTime;
public DateTime NewestTime;
/// <summary>
///
......@@ -87,21 +89,23 @@ namespace FlyADBase
return;
//------------------------------------------------------------------------------------------
//AD数据添加
DateTime dt = DateTime.MinValue;
//DateTime dt = DateTime.MinValue;
ADPool.Add(new DateTimeUnit3() { dt = end_dt, datas = adArray});
//long ticks = (long)((ts_ms * adArray.Count()) * TimeSpan.TicksPerMillisecond);
NewestTime = dt;
NewestTime = end_dt;
LimitPool();
}
void LimitPool()
{
LimitSimplePool(ADPool);
//LimitSimplePool_PosPool();
LimitSimplePool(PosPool,1.5);
LimitSimplePool(Pos2Pool, 1.5);
LimitSimplePool(DriveStatusPool, 1.5);
LimitSimplePool(IStatusPool, 1.5);
}
/// <summary>
///
/// </summary>
......@@ -111,6 +115,8 @@ namespace FlyADBase
void LimitSimplePool<T>(List<T> pool, double cap_min = 1) where T : IDateTimeUnit {
if (NewestTime == DateTime.MinValue)
return;
//限制的时间
var limitTime = NewestTime - TimeSpan.FromMinutes(cap_min);
//删除Pos 数据池
int removeCnt = 0;
......@@ -186,12 +192,33 @@ namespace FlyADBase
if (pool.Last().dt > time)
{
//异常，把之前的数据删除
pool.Clear();
//TODO, AD盒有bug， IO 推送， 的systick 比 pos 的systick 要慢，
//正常的操作，全删除
//pool.Clear();
//数据插入就算了
for (int i = 1; i < pool.Count(); i++) {
int index = pool.Count() - 1 - i;
if (time > pool[index].dt)
{
//找到了
pool.Insert(index, new DateTimeUnit() { dt = time, data = data });
}
else if (time == pool[index].dt) {
//什么都不干
return;
}
}
//找完了，不插了
}
else if (pool.Last().dt == time)
{
//删除之前那个
pool.RemoveAt(pool.Count() - 1);
//pool.RemoveAt(pool.Count() - 1);
return;
}
}
pool.Add(new DateTimeUnit() { dt = time, data = data });
......@@ -480,17 +507,65 @@ namespace FlyADBase
}
public static List<int> GetAD(List<DateTimeUnit3> adPool)
{
double t_ms = adPool.First().datas.Count() * ad_ts_ms;
DateTime begin = adPool.First().dt - TimeSpan.FromTicks((long)(t_ms * TimeSpan.TicksPerMillisecond));
return GetAD(adPool, begin);
}
static List<int> GetAD(List<DateTimeUnit3> adPool, DateTime begin)
{
var ts = TimeSpan.FromTicks((long)(ad_ts_ms * TimeSpan.TicksPerMillisecond));
List<int> reponse = new List<int>();
DateTime lastTime = adPool.Last().dt;
//每个ad包的每个数据不是准确的1.28ms，肯定比1.28ms大，每次需要准确计算
for (int i = 0; i < adPool.Count(); i++)
{
reponse.AddRange(adPool[i].datas);
int idx = adPool.Count() - 1 - i;
double curr_ad_ts_ms = ad_ts_ms;
//计算这个包每个数据的时间间隔
DateTime end_dt = adPool[idx].dt;
if (idx > 0)
{
var begin_dt = adPool[idx - 1].dt;
double total_ms = (end_dt - begin_dt).Ticks / TimeSpan.TicksPerMillisecond;
curr_ad_ts_ms = total_ms / adPool[idx].datas.Count();
}
double act_ms = 0;
double ideal_ms = 0;
for (int j = 0; j < adPool[idx].datas.Count(); j++)
{
int idx2 = adPool[idx].datas.Count() - 1 - j;
int ad = adPool[idx].datas[idx2];
act_ms += curr_ad_ts_ms;
while (ideal_ms < act_ms)
{
reponse.Add(ad);
ideal_ms += ad_ts_ms;
lastTime -= ts;
}
if (lastTime < begin)
goto _end;//完成
}
}
_end:
//反转
reponse.Reverse();
return reponse;
}
/// <summary>
///
/// adPool 每个包内，数据间隔不为1.28ms，肯定比1.28ms大；
/// 输出的数据列，必须是 间隔为1.28ms
/// </summary>
/// <param name="adPool"></param>
/// <param name="ad_ts_ms"></param>
......@@ -501,44 +576,29 @@ namespace FlyADBase
static List<int> GetAD(List<DateTimeUnit3> adPool, double ad_ts_ms, DateTime begin, DateTime end, out DateTime reponse_endTime)
{
reponse_endTime = DateTime.MinValue;
List<int> reponse = new List<int>();
long ticks = (long)(ad_ts_ms * TimeSpan.TicksPerMillisecond);
var ts = TimeSpan.FromTicks(ticks);
var reponse2 = GetAD2(adPool, begin, end);
if (reponse2.Count() == 0)
return reponse;
return new List<int>();
DateTime lastTime = reponse2.Last().dt;
reponse_endTime = lastTime;
for (int i = 0; i < reponse2.Count(); i++)
{
int idx = reponse2.Count() - 1 - i;
for (int j = 0; j < reponse2[idx].datas.Count(); j++)
{
int idx2 = reponse2[idx].datas.Count() - 1 - j;
int ad = reponse2[idx].datas[idx2];
reponse.Add(ad);
lastTime -= ts;
if (lastTime < begin)
goto _end;//完成
}
}
_end:
//反转
reponse.Reverse();
return reponse;
return GetAD(reponse2, begin);
}
/// <summary>
/// <summary>
/// 获取 时间在 begin~end 的 AD数据包
/// </summary>
/// <param name="adPool"></param>
/// <param name="begin"></param>
/// <param name="end"></param>
/// <returns></returns>
static List<DateTimeUnit3> GetAD2(List<DateTimeUnit3> adPool, DateTime begin, DateTime end)
{
//获取 从 idxOfRunning 到 idxOfStop 时间段的 timegrid
//ad 包的总数量不可能多，从头找也没所谓
List<DateTimeUnit3> reponse = new List<DateTimeUnit3>();
for (int i = 0; i < adPool.Count(); i++)
{
......@@ -611,8 +671,16 @@ namespace FlyADBase
int index = searchIndex(posPool, dt, searchIdx);
searchIdx = index;
if (index < 0)
index = 0;
if (index == posPool.Count() - 1)
{
//发生在未来，返回最后一个脉冲，不进行线性预测
return posPool[index].data;
}
else if (index < 0) {
//发生在过去，返回第一个脉冲，不进行线性预测
return posPool[0].data;
}
return GetPosLinear(posPool, dt, index);
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment