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