EmaMaker
/
SPQR-Team-2019-REVAMPED
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

camera: only filter if seen

pull/2/head
emamaker 2022-07-05 12:01:40 +02:00
parent d4bc1cf904
commit 8beec8c246
1 changed files with 13 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/sensors/data_source_camera_conicmirror.cpp
View File

@ -78,11 +78,20 @@ void DataSourceCameraConic ::readSensor()
void DataSourceCameraConic ::computeCoordsAngles()
{
CURRENT_DATA_WRITE.bSeen = bangle != 999;
CURRENT_DATA_WRITE.ySeen = yangle != 999;
#ifdef CAMERA_CONIC_FILTER_POINTS
yangle = filt_yangle->calculate(yangle);
ydist = filt_ydist->calculate(ydist);
bangle = filt_bangle->calculate(bangle);
bdist = filt_bdist->calculate(bdist);
if (CURRENT_DATA_WRITE.ySeen)
{
yangle = filt_yangle->calculate(yangle);
ydist = filt_ydist->calculate(ydist);
}
if (CURRENT_DATA_WRITE.bSeen)
{
bangle = filt_bangle->calculate(bangle);
bdist = filt_bdist->calculate(bdist);
}
#endif
// Fix angles using the IMU
@ -100,8 +109,6 @@ void DataSourceCameraConic ::computeCoordsAngles()
CURRENT_DATA_WRITE.bangle_fix = bangle_fix;
CURRENT_DATA_WRITE.ydist = ydist;
CURRENT_DATA_WRITE.bdist = bdist;
CURRENT_DATA_WRITE.bSeen = bangle != 999;
CURRENT_DATA_WRITE.ySeen = yangle != 999;
CURRENT_DATA_WRITE.atkGAngle = goalOrientation ? yangle : bangle;
CURRENT_DATA_WRITE.atkGAngle_fix = goalOrientation ? yangle_fix : bangle_fix;