parent
3bfe9ffb6f
commit
ee96b4191c
@ -0,0 +1,75 @@
|
|||||||
|
#include "Arith_PosAnalyse.h"
|
||||||
|
|
||||||
|
ImagePosRecord::ImagePosRecord()
|
||||||
|
{
|
||||||
|
reset();
|
||||||
|
}
|
||||||
|
|
||||||
|
ImagePosRecord::~ImagePosRecord()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
ImagePosRecord::STATUS ImagePosRecord::update(POINT32F pos)
|
||||||
|
{
|
||||||
|
if (p_Last.x == 0 && p_Last.y == 0)
|
||||||
|
{
|
||||||
|
stPositonMove[ubEnd].x = 0;
|
||||||
|
stPositonMove[ubEnd].y = 0;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
stPositonMove[ubEnd].x = pos.x - p_Last.x;
|
||||||
|
stPositonMove[ubEnd].y = pos.y - p_Last.y;
|
||||||
|
}
|
||||||
|
|
||||||
|
p_Last = pos;
|
||||||
|
ubEnd = (ubEnd + 1) % TRACK_POS_REC_MAX;
|
||||||
|
nTotalCnt++;
|
||||||
|
|
||||||
|
return Analyse();
|
||||||
|
}
|
||||||
|
|
||||||
|
void ImagePosRecord::reset()
|
||||||
|
{
|
||||||
|
ubEnd = 0;
|
||||||
|
nTotalCnt = 0;
|
||||||
|
p_Last.x = 0;
|
||||||
|
p_Last.y = 0;
|
||||||
|
memset(stPositonMove, 0, sizeof(POINT32F) * TRACK_POS_REC_MAX);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
ImagePosRecord::STATUS ImagePosRecord::Analyse()
|
||||||
|
{
|
||||||
|
//
|
||||||
|
int VecFluCnt = 0; //速度波动
|
||||||
|
int aFluCnt = 0; //加速度波动
|
||||||
|
|
||||||
|
if (nTotalCnt > 10)
|
||||||
|
{
|
||||||
|
for (size_t i = 0; i < MIN(TRACK_POS_REC_MAX, nTotalCnt); i++)
|
||||||
|
{
|
||||||
|
int indCurr = (ubEnd - i + TRACK_POS_REC_MAX) % TRACK_POS_REC_MAX;
|
||||||
|
int indLast = (ubEnd - i - 1 + TRACK_POS_REC_MAX) % TRACK_POS_REC_MAX;
|
||||||
|
auto movedisCurr = sqrt(stPositonMove[indCurr].x * stPositonMove[indCurr].x + stPositonMove[indCurr].y * stPositonMove[indCurr].y);
|
||||||
|
auto movedisLast = sqrt(stPositonMove[indLast].x * stPositonMove[indLast].x + stPositonMove[indLast].y * stPositonMove[indLast].y);
|
||||||
|
if (movedisCurr > 6)
|
||||||
|
{
|
||||||
|
VecFluCnt++;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (ABS(movedisCurr) > ABS(movedisLast) * 2 && ABS(movedisCurr) > 5)
|
||||||
|
{
|
||||||
|
aFluCnt++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (VecFluCnt > 2 || aFluCnt > 0)
|
||||||
|
{
|
||||||
|
return STATUS::NOTSTABLE;
|
||||||
|
}
|
||||||
|
|
||||||
|
return STATUS::STATBLE;
|
||||||
|
}
|
||||||
@ -0,0 +1,36 @@
|
|||||||
|
// 目标像方位置记录，并估计伺服控制状态，不稳定时不使用惯性预测
|
||||||
|
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "Arith_Common.hpp"
|
||||||
|
|
||||||
|
|
||||||
|
class ImagePosRecord
|
||||||
|
{
|
||||||
|
enum STATUS
|
||||||
|
{
|
||||||
|
STATBLE,
|
||||||
|
NOTSTABLE
|
||||||
|
};
|
||||||
|
public:
|
||||||
|
ImagePosRecord();
|
||||||
|
~ImagePosRecord();
|
||||||
|
|
||||||
|
public:
|
||||||
|
STATUS update(POINT32F pos); // 更新坐标
|
||||||
|
|
||||||
|
void reset();
|
||||||
|
|
||||||
|
STATUS Analyse();
|
||||||
|
|
||||||
|
private:
|
||||||
|
UBYTE8 ubEnd;
|
||||||
|
SINT32 nTotalCnt;
|
||||||
|
POINT32F stPositonMove[TRACK_POS_REC_MAX]; //帧间运动
|
||||||
|
|
||||||
|
POINT32F p_Last; //起始点
|
||||||
|
|
||||||
|
//
|
||||||
|
BBOOL bStable; //稳态
|
||||||
|
};
|
||||||
Loading…
Reference in new issue