#include "main.h"
bool lineCheck(U16 verLimit, U16 lineIntensity, U16 lineLowerIntensity, U16 lineEndIntensity, U16 lineScanThickness, U16 & verPosition, U16 & leftWidth, U16 & rightWidth)
{
U16 st = 0, y, yy, x = sensor.horPixel() / 2, wl, wr, h, Y;
Pixel pixt, pixb;
bool first = false, ret = false, ldone, rdone, l, r;
// Grab an image
camera.grab(image);
// Find line position from the bottom to the top
for(y = sensor.verPixel() - 1; y > verLimit; y--)
{
pixt = image.pixel(x, y);
if(!first)
{
if(pixt.word > lineIntensity)
{
first = true;
st = y; // Vertical line position
}
}
else
if(pixt.word < lineLowerIntensity)
break;
}
// At this point y is the last position of the line
if(st > verLimit && first)
{
ret = true;
y = st + (y - st) / 2; // Center of the line
wl = wr = 0;
h = sensor.horPixel() / 2;
l = r = true;
for(x = 0; x < (otus->_lineScanRange / 2); x++)
{
// Right check
rdone = false;
// Adjust for line distorsion
Y = y + x * otus->_lineScanRightAdj;
if(r)
for(yy = y - lineScanThickness; yy < y + lineScanThickness; yy++)
{
pixb = image.pixel(h + x, yy);
if(pixb.word > lineEndIntensity)
{
wr++;
rdone = true;
break;
}
}
if(!rdone) r = false; // If not found any pixel disable this loop
// Left check
ldone = false;
// Adjust for line distorsion
Y = y + x * otus->_lineScanLeftAdj;
if(l)
for(yy = y - lineScanThickness; yy < y + lineScanThickness; yy++)
{
pixt = image.pixel(h - x, yy);
if(pixt.word > lineEndIntensity)
{
wl++;
ldone = true;
break;
}
}
if(!ldone) l = false; // If not found any pixel disable this loop
if(!ldone && !rdone)
break;
}
verPosition = y;
leftWidth = wl;
rightWidth = wr;
}
else
{
verPosition = 0;
leftWidth = 0;
rightWidth = 0;
}
return ret;
}
void lineScanProcess()
{
TimeMark(TL_LINE_SCAN);
if(lineCheck(otus->_verLimit, otus->_lineIntensity, otus->_lineLowerIntensity, otus->_lineEndIntensity, otus->_lineScanThickness, telemetry->_verPosition, telemetry->_leftWidth, telemetry->_rightWidth))
{
telemetry->_lineScanBench = TimeMeasure(TL_LINE_SCAN);
if(otus->_lineScanDebug) printf("At = %3u, Left = %3u, Right = %3u, ET: %3u ms\n", telemetry->_verPosition, telemetry->_leftWidth, telemetry->_rightWidth, telemetry->_lineScanBench);
otus->_lineScanValid = true;
}
else
otus->_lineScanValid = false;
}
void lineScanInit()
{
otus->_lineScanLeftAdj = (F32) otus->_lineScanLeftDrift / (F32) otus->_lineScanRange;
otus->_lineScanRightAdj = (F32) otus->_lineScanRightDrift / (F32) otus->_lineScanRange;
otus->_lineScanLeftAdj *= -1.0f;
otus->_lineScanRightAdj *= -1.0f;
}
bool saveImage(U8 fileExtension, int quality)
{
bool r = false;
int year, month, day;
U8 hh, mm, ss;
char fn[64];
RtcGetDate(&year, &month, &day);
RtcGetTime(&hh, &mm, &ss);
// Create dir
sprintf(fn, "DCIM/%04u%02u%02u", year, month, day);
file.mkdir(fn);
// Create file name path
sprintf(fn, "DCIM/%04u%02u%02u/%02u%02u%02u.", year, month, day, hh, mm, ss);
switch(fileExtension)
{
case FE_RAW: _strcat(fn, "RAW");
r = imgFile.save(fn, &image, 0);
break;
case FE_JPG: _strcat(fn, "JPG");
#if(IMAGE_DEPTH == 16)
imgVar.create(sensor.horPixel(), sensor.verPixel(), 1, otImage::IMG_U8);
image.shrink(imgVar, 0, 4);
r = imgFile.save(fn, &imgVar, 0, quality);
imgVar.close();
#else
r = imgFile.save(fn, &image, 0, quality);
#endif
break;
case FE_AVI: _strcat(fn, "AVI");
break;
}
//printf("File: %s = %d\n", fn, r);
return r;
}
bool timeLapse(U32 nx, U32 it, U8 ff, int qy)
{
bool r;
ElapsedTimeStart(TIMER0);
for(U32 n = 0; n < nx; n++)
{
while(ElapsedTimeEnd(TIMER0) < it)
camera.grab(image);
ElapsedTimeStart(TIMER0);
r = saveImage(ff, qy);
if(!r) break;
}
return r;
}