Subversion Repositories svnkaklik

Rev

Go to most recent revision | Details | Last modification | View Log

Rev Author Line No. Line
268 kaklik 1
/*
2
    Copyright (C) 2004    John Orlando
3
 
4
   AVRcam: a small real-time image processing engine.
5
 
6
    This program is free software; you can redistribute it and/or
7
    modify it under the terms of the GNU General Public
8
    License as published by the Free Software Foundation; either
9
    version 2 of the License, or (at your option) any later version.
10
 
11
    This program is distributed in the hope that it will be useful,
12
    but WITHOUT ANY WARRANTY; without even the implied warranty of
13
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
14
    General Public License for more details.
15
 
16
    You should have received a copy of the GNU General Public
17
    License along with this program; if not, write to the Free Software
18
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
19
 
20
   For more information on the AVRcam, please contact:
21
 
22
   john@jrobot.net
23
 
24
   or go to www.jrobot.net for more details regarding the system.
25
*/
26
/*********************************************************
27
	Module Name: FrameMgr.c
28
	Module Date: 04/10/2004
29
	Module Auth: John Orlando
30
 
31
	Description: This modules is responsible for performing
32
	both medium and high level processing on image data.
33
	This is performed at both the line level as well as
34
	the frame level.  It controls the main flow of the
35
	system, adhering to all the critical timing 
36
	requirements (such as when serial data can be transferred,
37
	etc).
38
 
39
    Revision History:
40
    Date        Rel Ver.    Notes
41
    4/10/2004      0.1     Module created
42
    6/30/2004      1.0     Initial release for Circuit Cellar
43
                           contest.
44
    11/15/2004     1.2     Updated processLine() function so 
45
                           it will remove objects less than
46
                           a specified length/width (reduces
47
                           shot noise)
48
*********************************************************/
49
 
50
/*	Includes */
51
#include <stdlib.h>
52
#include <string.h>
53
#include <avr/io.h>
54
#include "Executive.h"
55
#include "UIMgr.h"
56
#include "FrameMgr.h"
57
#include "CamInterface.h"
58
#include "UartInterface.h"
59
#include "Utility.h"
60
#include "I2CInterface.h"
61
#include "CamConfig.h"
62
#include "CommonDefs.h"
63
 
64
/* 	Local Structures and Typedefs */
65
enum
66
{
67
	ST_FrameMgr_idle,
68
	ST_FrameMgr_TrackingFrame,
69
	ST_FrameMgr_DumpingFrame
70
};
71
 
72
typedef unsigned char FrameMgr_State_t;
73
 
74
/*  Definitions */
75
/* The most objects that can be tracked at any one time is 8.  
76
This number is determined by the number of bytes that can be
77
sent out during a frame (one byte per line, 144 lines per frame) 
78
with the number of bytes in a tracked object (7) + some wiggle
79
room :-) ... I guess this could be increased to around 20 if
80
we had enough room and cycles to process objects between lines */
81
#define MAX_TRACKED_OBJECTS	8
82
 
83
/* This defines the number of bytes that make up a trackedObject_t
84
structure... */
85
#define SIZE_OF_TRACKED_OBJECT 8
86
 
87
/* This define is used to turn off the timer overflow interrupt
88
that is generated when the PCLK overflows TIMER1 */
89
#define DISABLE_PCLK_TIMER1_OVERFLOW_BITMASK 0xFB
90
 
91
/* This define is used to determine if a run length is too small
92
to be concerned with.  This helps to reduce the number of false
93
positives. */
94
#define MIN_OBJECT_TRACKING_WIDTH 3
95
 
96
/* This define is used to determine if an object has enough
97
height to be considered worth tracking...it is used to reduce
98
shot noise */
99
#define MIN_OBJECT_TRACKING_HEIGHT 3
100
 
101
/* This define is used to indicate how often the filter routine
102
that removes objects less than MIN_OBJECT_TRACKING_HEIGHT should
103
be executed.  It is measured in a number of lines (7 nominally). */
104
#define RUN_OBJECT_FILTER_MASK 0x07
105
 
106
/* This enum describes the possible colors that can
107
be tracked by the system.  This can't be represented as
108
simple color names (red, brown, etc) due to the fact that
109
the user sets which colors will be associated with which
110
bits.  Remember...after the AND operation of the indexed
111
color map values executes, either a single bit indicating
112
the color should be set, or no bits indicating that the
113
color isn't represented in the color map (notTracked). */
114
enum
115
{
116
	notTracked,
117
	color1,		/* bit 1 color */
118
	color2,		/* bit 2 color */
119
	color3,		/* bit 3 color */
120
	color4,		/* bit 4 color */
121
	color5,		/* bit 5 color */
122
	color6,		/* bit 6 color */
123
	color7,		/* bit 7 color */
124
	color8		/* bit 8 color */
125
};
126
 
127
typedef unsigned char trackedColor_t;
128
 
129
/* This structure defines the info that needs to be
130
maintained for each trackedObject in the trackingTable */
131
typedef struct
132
{
133
	trackedColor_t  color;
134
	unsigned char lastLineXStart;
135
	unsigned char lastLineXFinish;
136
	unsigned char x_upperLeft;
137
	unsigned char y_upperLeft;
138
	unsigned char x_lowerRight;
139
	unsigned char y_lowerRight;
140
	unsigned char objectValid;  /* used to be a fill byte...now it is
141
                                     used to determine if the object is valid
142
                                     or not...it gets invalidated if it is
143
                                     determined that it is too small, or
144
                                     that the object is within another object */
145
} trackedObject_t;
146
 
147
/* These defines are used to index into each individual element in the
148
trackedObject_t structure.  This seems to be MUCH more efficient than
149
accessing the elements in GCC. */
150
#define COLOR_OFFSET                0
151
#define LAST_LINE_X_START_OFFSET    1
152
#define LAST_LINE_X_FINISH_OFFSET   2
153
#define X_UPPER_LEFT_OFFSET         3
154
#define Y_UPPER_LEFT_OFFSET         4
155
#define X_LOWER_RIGHT_OFFSET        5
156
#define Y_LOWER_RIGHT_OFFSET        6
157
#define VALID_OBJECT_OFFSET         7
158
 
159
/*  Local Variables */
160
/* The trackedObjectTable is used to hold up to eight tracked objects
161
while they are being acquired. */
162
static trackedObject_t trackedObjectTable[MAX_TRACKED_OBJECTS];
163
static trackedObject_t *pCurrentTrackedObjectTable = trackedObjectTable;
164
static unsigned char lineCount = 0;
165
static FrameMgr_State_t currentState = ST_FrameMgr_idle;
166
static unsigned char numCurrTrackedObjects = 0;
167
static unsigned char numPrevTrackedObjects = 0;
168
static unsigned char trackedLineCount = 0;
169
 
170
/*  Local Functions  */
171
static void FrameMgr_findConnectedness(void);
172
 
173
/*  Extern Functions */
174
/* These functions are located in assembly files, and thus
175
must be externed here so they can be referenced in the source below. */
176
extern void CamIntAsm_waitForNewTrackingFrame(unsigned char *pBuffer, unsigned char *pMemLookup);
177
extern void CamIntAsm_waitForNewDumpFrame(unsigned char *pCurrBuffer, unsigned char *pPrevBuffer);
178
extern void CamIntAsm_acquireTrackingLine(unsigned char *pBuffer, unsigned char *pMemLookup);
179
extern void CamIntAsm_acquireDumpLine(unsigned char *pCurrBuffer, unsigned char *pPrevBuffer);
180
 
181
/***********************************************************
182
	Function Name: FrameMgr_init
183
	Function Description: This function is responsible
184
	for initializing the FrameMgr.  This includes 
185
	setting up the various buffers and data needed to 
186
	process each frame of image data.
187
	Inputs:  none
188
	Outputs: none
189
***********************************************************/	
190
void FrameMgr_init(void)
191
{
192
	memset(trackedObjectTable,0x00,sizeof(trackedObjectTable));
193
}
194
 
195
 
196
/***********************************************************
197
	Function Name: FrameMgr_dispatchEvent
198
	Function Description: This function is responsible for
199
	taking an incoming event and performing the needed
200
	actions with it as pertains to the FrameMgr.
201
	Inputs:  event - the generated event
202
	Outputs: none
203
***********************************************************/	
204
void FrameMgr_dispatchEvent(unsigned char event)
205
{	
206
	switch(event)
207
	{
208
		case EV_DUMP_FRAME:
209
            /* try re-initializing the camera before we start dumping */
210
 
211
			CamConfig_setCamReg(0x11,0x01);  /* reduce the frame rate for dumping*/
212
			CamConfig_sendFifoCmds();
213
			Utility_delay(1000);		/* allow the new frame rate to settle */
214
			lineCount = 0;
215
			currentState = ST_FrameMgr_DumpingFrame;
216
			//CamIntAsm_waitForNewDumpFrame(currentLineBuffer,previousLineBuffer);
217
            FrameMgr_acquireLine();
218
			break;
219
 
220
		case EV_ENABLE_TRACKING:
221
			currentState = ST_FrameMgr_TrackingFrame;					
222
			FrameMgr_acquireFrame();
223
			break;
224
 
225
		case EV_ACQUIRE_FRAME_COMPLETE:
226
			FrameMgr_processFrame();
227
			break;
228
 
229
		case EV_PROCESS_FRAME_COMPLETE:
230
			FrameMgr_acquireFrame();
231
			break;
232
 
233
		case EV_SERIAL_DATA_RECEIVED:
234
			if (currentState != ST_FrameMgr_idle)
235
			{
236
				/* we need to go back to processing line data, since
237
				serial data reception interrupted us....just trash the
238
				frame and act like the frame has been processed, which
239
				will kick off the system to wait for the next line */
240
				PUBLISH_EVENT(EV_PROCESS_FRAME_COMPLETE);
241
			}
242
			break;
243
 
244
		case EV_DISABLE_TRACKING:
245
			/* tracking needs to be turned off */
246
			currentState = ST_FrameMgr_idle;
247
			break;
248
	}
249
}
250
 
251
/***********************************************************
252
	Function Name: FrameMgr_acquireFrame
253
	Function Description: This function is responsible for
254
	beginning of the acquisition of a new frame of data
255
	from the camera interface. The acquisition of this line 
256
	depends on the current state of the FrameMgr.
257
	Inputs:  none
258
	Outputs: none
259
***********************************************************/	
260
void FrameMgr_acquireFrame(void)
261
{
262
	if (currentState == ST_FrameMgr_TrackingFrame)
263
	{
264
		trackedLineCount = 0;
265
		numPrevTrackedObjects = numCurrTrackedObjects;
266
		numCurrTrackedObjects = 0;
267
 
268
		/* clear out the tracking table, and wait for the new frame
269
		to start */
270
		memset(trackedObjectTable,0x00,sizeof(trackedObjectTable));
271
		//CamIntAsm_waitForNewTrackingFrame(currentLineBuffer,colorMap);
272
        WAIT_FOR_VSYNC_HIGH();
273
        CamIntAsm_acquireTrackingLine(currentLineBuffer,colorMap);
274
	}
275
}
276
 
277
/***********************************************************
278
	Function Name: FrameMgr_acquireLine
279
	Function Description: This function is responsible for
280
	acquiring a line of data from the camera interface.
281
	The acquisition of this line depends on the current
282
	state of the FrameMgr.
283
	Inputs:  none
284
	Outputs: none
285
***********************************************************/	
286
void FrameMgr_acquireLine(void)
287
{
288
	unsigned char tmpLineCount;
289
 
290
	/* clearing out the buffers takes too long...we should
291
	just overwrite the data here without a problem when
292
	we start acquiring...at no point do we check for 
293
	a 0x00 value in the current or previous lineBuffers,
294
	so it was a bit excessive :-)  */
295
 
296
	/* check which state we are in and proceed as needed */
297
	if (currentState == ST_FrameMgr_DumpingFrame)
298
	{
299
		tmpLineCount = lineCount*2;
300
 
301
        /* clearing out the line data in dump mode is ok, and actually
302
        is needed, since it is possible for the first dump line in
303
        a frame to come back with the last line captured of the
304
        last capture session...*/
305
        memset(currentLineBuffer,0x00,LENGTH_OF_LINE_BUFFER);
306
        memset(previousLineBuffer,0x00,LENGTH_OF_LINE_BUFFER);
307
		/* wait for another VSYNC so we know which frame to use 
308
		to start looking for a line to receive */
309
		WAIT_FOR_VSYNC_HIGH();  
310
		WAIT_FOR_VSYNC_LOW();
311
 
312
		/* look at lineCount to determine how many HREFs we should
313
		wait before we start sampling */
314
		while(tmpLineCount != 0)
315
		{
316
			WAIT_FOR_HREF_HIGH(); 
317
			tmpLineCount--;
318
			WAIT_FOR_HREF_LOW(); 
319
		}
320
 
321
		/*  we should now be ready to sample our line...*/
322
		CamIntAsm_acquireDumpLine(currentLineBuffer,previousLineBuffer);
323
	}		
324
	else if (currentState == ST_FrameMgr_TrackingFrame)
325
	{
326
		WAIT_FOR_HREF_LOW();
327
		CamIntAsm_acquireTrackingLine(currentLineBuffer,colorMap);
328
	}
329
}
330
 
331
/***********************************************************
332
	Function Name: FrameMgr_processLine
333
	Function Description: This function is responsible for
334
	parsing the received image line and performing either
335
	connected region mapping (if in the Tracking state) or
336
	sending out the raw sampled data (if in the Dumping
337
	state).
338
	Inputs:  none
339
	Outputs: none
340
***********************************************************/	
341
void FrameMgr_processLine(void)
342
{
343
	unsigned char i;
344
	volatile unsigned char dataToSend;
345
	unsigned char *pTrackedObjectData = (unsigned char *)pCurrentTrackedObjectTable;
346
#ifdef DEBUG_TRACKED_LINE    
347
	unsigned char *pSendData;
348
    unsigned char asciiBuffer[5];
349
    unsigned char pixelCount = 0;
350
#endif    
351
 
352
	if (currentState == ST_FrameMgr_DumpingFrame)
353
	{
354
		/* we want to sit in a tight loop and send the acquired data
355
		sitting in current and previous line buffers out the serial
356
		port...it is sent out the serial port immediately instead
357
		of going into the UIMgr tx fifo because we can't do anything
358
		until its sent out anyway...may as well just get it out now	*/
359
 
360
		/* currentLineBuffer is getting "g" previousLineBuffer is getting "b-r" */
361
		UartInt_txByte(0x0B);			/* send the header byte */
362
		UartInt_txByte(lineCount);		/* send the line count */
363
		for (i=0; i<NUM_PIXELS_IN_A_DUMP_LINE; i+=2)
364
		{
365
			/* when a dump line is sampled, the upper byte can potentially
366
			have garbage in it...we don't have time to mask it off as we're
367
			sampling, so it is done here before we send it out...we also
368
			combine the samples together so we really are sending up a
369
			sample for line N as well as line N+1 */
370
			dataToSend = currentLineBuffer[i];
371
			dataToSend &= 0x0F;
372
			dataToSend <<= 4;
373
			dataToSend |= (previousLineBuffer[i] & 0x0F);
374
 
375
			/* dataToSend should be packed now */
376
			UartInt_txByte(dataToSend);
377
 
378
			/* flip the colors around since we are doing all G on Y and BR on UV */
379
			dataToSend = previousLineBuffer[i+1];
380
			dataToSend &= 0x0F;
381
			dataToSend <<= 4;
382
			dataToSend |= (currentLineBuffer[i+1] & 0x0F);
383
 
384
			/* dataToSend should be packed now */
385
			UartInt_txByte(dataToSend);
386
		}
387
		UartInt_txByte(0x0F);  /* send line end */
388
		/* once all the data is sent, increment out line count by 2 since
389
		we really get 2 lines worth of pixels on each pass */
390
		/* Update...increment only by 1, but only send 72 double-lines */
391
		lineCount++;
392
 
393
		/* check to see if we have retrieved all of the needed lines */
394
		if (lineCount >= 72)  /* half 144, since we send two lines at a time */
395
		{
396
			/* we're done, so send the dump complete?...nope, just change
397
			states and we should be fine */
398
			lineCount = 0;
399
			currentState = ST_FrameMgr_idle;
400
 
401
			/* disable the PCLK counting overflow interrupt */
402
			TIMSK &= DISABLE_PCLK_TIMER1_OVERFLOW_BITMASK;
403
 
404
			CamConfig_setCamReg(0x11,0x00);  /* reset the frame rate to normal*/
405
			CamConfig_sendFifoCmds();
406
		}
407
		else
408
		{
409
			/* we have more lines to acquire in this frame, so keep on truckin...*/
410
			PUBLISH_FAST_EVENT(FEV_PROCESS_LINE_COMPLETE);
411
		}
412
	}
413
	else if (currentState == ST_FrameMgr_TrackingFrame)
414
	{
415
#ifdef DEBUG_TRACKED_LINE	
416
		/* send the received line over serial...this should only send
417
		until a pixelCount == 176 */
418
		pSendData = currentLineBuffer;
419
		itoa(trackedLineCount,asciiBuffer,10);
420
		UIMgr_txBuffer(asciiBuffer,3);
421
		UIMgr_txBuffer(" ",1);
422
		while(pixelCount < ACTUAL_NUM_PIXELS_IN_A_LINE)  
423
		{
424
			memset(asciiBuffer,0x00,5);
425
			itoa(*pSendData++,asciiBuffer,10);	/* color is first byte */
426
			UIMgr_txBuffer(asciiBuffer,3); /* 3 ascii bytes for data */
427
			UIMgr_txBuffer(" ",1);
428
 
429
			pixelCount += *pSendData;	/* run-length is second byte */
430
			memset(asciiBuffer,0x00,5);
431
			itoa(*pSendData++,asciiBuffer,10);
432
			UIMgr_txBuffer(asciiBuffer,3);
433
			UIMgr_txBuffer(" ",1);
434
		}
435
		UIMgr_txBuffer("\n\r",2);
436
 
437
		trackedLineCount++;
438
		if (trackedLineCount == 144)
439
		{
440
			UIMgr_txBuffer("  FC  \n\r",8);
441
			trackedLineCount = 0;
442
			PUBLISH_EVENT(EV_PROCESS_FRAME_COMPLETE);
443
		}
444
		else
445
		{
446
			PUBLISH_EVENT(EV_PROCESS_LINE_COMPLETE);
447
		}	
448
#else
449
        /* determine if any of the RLE blocks overlap */
450
		FrameMgr_findConnectedness();
451
 
452
        /* we also want to remove any objects that are less than
453
        a minimum height...we already removed portions of the 
454
        run-length that are less than MIN_PIXEL_WIDTH in the
455
        findConnectedness() routine...doing it here instead of 
456
        a function to speed things up...this may end up slowing down the
457
        frame rate slightly, and can be removed if this isn't needed */
458
 
459
        /* run this routine once every 8 lines */       
460
        if ( (trackedLineCount & RUN_OBJECT_FILTER_MASK) == RUN_OBJECT_FILTER_MASK)
461
        {
462
            for (i=0; i<MAX_TRACKED_OBJECTS; i++)
463
            {
464
                if ( *(pTrackedObjectData + VALID_OBJECT_OFFSET) == TRUE)
465
                {
466
                    /* check to see if the object is already in
467
                    our past...i.e., its last */
468
                    if ( (*(pTrackedObjectData + Y_LOWER_RIGHT_OFFSET) - 
469
                          *(pTrackedObjectData + Y_UPPER_LEFT_OFFSET)) < MIN_OBJECT_TRACKING_HEIGHT)
470
                    {
471
                        /* the object is less than the minimum height...see if it is adjacent
472
                        to the current line we just processed...if so, leave it here...otherwise,
473
                        it needs to be invalidated since its too small */
474
                        if ( trackedLineCount - *(pTrackedObjectData + Y_LOWER_RIGHT_OFFSET) > 2)
475
                        {
476
                            /* invalidate the object */
477
                            *(pTrackedObjectData + VALID_OBJECT_OFFSET) = FALSE;
478
                            numCurrTrackedObjects--;
479
                        }
480
                    }
481
                }
482
                pTrackedObjectData += SIZE_OF_TRACKED_OBJECT;
483
            }
484
        }     
485
 
486
		trackedLineCount++;
487
		if (trackedLineCount == ACTUAL_NUM_LINES_IN_A_FRAME)
488
		{
489
			/* an entire frame of tracking data has been acquired, so
490
			publish an event letting the system know this fact */
491
			PUBLISH_EVENT(EV_ACQUIRE_FRAME_COMPLETE);
492
			/* disable the PCLK counting overflow interrupt */
493
			TIMSK &= DISABLE_PCLK_TIMER1_OVERFLOW_BITMASK;
494
			trackedLineCount = 0;
495
		}
496
		else
497
		{
498
			PUBLISH_FAST_EVENT(FEV_PROCESS_LINE_COMPLETE);
499
		}
500
#endif		
501
	}
502
	else
503
	{
504
		/* ...and here? */
505
	}
506
}
507
 
508
/***********************************************************
509
	Function Name: FrameMgr_processFrame
510
	Function Description: This function is responsible for
511
	parsing the completed frame and performing all actions
512
	needed at this level.
513
	Inputs:  none
514
	Outputs: none
515
***********************************************************/	
516
void FrameMgr_processFrame(void)
517
{
518
	unsigned char i,k,color;
519
#if DEBUG_FRAME_DATA    
520
	unsigned char asciiBuffer[5];
521
    unsigned char j;
522
#endif    
523
	unsigned char *pTableData = (unsigned char *)pCurrentTrackedObjectTable;
524
	unsigned char tmpUpperLeftX,tmpUpperLeftY,tmpLowerRightX,tmpLowerRightY;
525
 
526
#if DEBUG_FRAME_DATA	
527
	/* we want to send all of the currently tracked table out
528
	the serial port for debugging */
529
	for (i=0; i<numCurrTrackedObjects; i++)
530
	{
531
		UIMgr_txBuffer("----------\r\n",12);
532
		for (j=0; j<SIZE_OF_TRACKED_OBJECT; j++)
533
		{
534
			memset(asciiBuffer,0x00,5);
535
			itoa(*pTableData++,asciiBuffer,10);
536
			UIMgr_txBuffer(asciiBuffer,3); /* 3 ascii bytes for data
537
														+ 1 space */
538
			UIMgr_txBuffer("\r\n",2);
539
		}
540
	}
541
 
542
	/* finally, send a new line */
543
	UIMgr_txBuffer("\r\n",2);
544
 
545
	memset(asciiBuffer,0x00,5);
546
	itoa(numCurrTrackedObjects,asciiBuffer,10);
547
	UIMgr_txBuffer(asciiBuffer,3);
548
	UIMgr_txBuffer(" PFC\r\n",5);
549
 
550
#else	
551
	/* we only send tracking packets if there are tracked objects */	        
552
 
553
	if (numCurrTrackedObjects > 0)
554
	{		
555
		UIMgr_writeTxFifo(0x0A);					/* header byte for a tracking packet */
556
        /* reset the pointer */
557
        pTableData = (unsigned char *)pCurrentTrackedObjectTable;
558
 
559
		UIMgr_writeTxFifo(numCurrTrackedObjects);	/* num of objects tracked */
560
		for (i=0; i<MAX_TRACKED_OBJECTS; i++)
561
		{
562
            /* we only want to process objects that have their objectValid flag
563
            set to TRUE */
564
            if ( *(pTableData + VALID_OBJECT_OFFSET) == TRUE)
565
            {
566
                /* the object is valid...convert the color from bit position to value...remember, 
567
                each bit in the "color" byte corresponds to a color */
568
                k=0;
569
                color = *(pTableData + COLOR_OFFSET);
570
                if (color == 128) k=0;
571
                else if (color == 64) k=1;
572
                else if (color == 32) k=2;
573
                else if (color == 16) k=3;
574
                else if (color == 8)  k=4;
575
                else if (color == 4)  k=5;
576
                else if (color == 2)  k=6;
577
                else if (color == 1)  k=7;
578
 
579
                tmpUpperLeftX = *(pTableData + X_UPPER_LEFT_OFFSET);	    /* get the upper left X */
580
                tmpUpperLeftY = *(pTableData + Y_UPPER_LEFT_OFFSET);		/* get the upper left Y */		
581
                tmpLowerRightX = *(pTableData + X_LOWER_RIGHT_OFFSET);		/* get the lower right X */
582
                tmpLowerRightY = *(pTableData + Y_LOWER_RIGHT_OFFSET);		/* get the lower right Y */	                
583
 
584
                UIMgr_writeTxFifo(k);				  	/* send the color first */
585
                UIMgr_writeTxFifo(tmpUpperLeftX);
586
                UIMgr_writeTxFifo(tmpUpperLeftY);
587
                UIMgr_writeTxFifo(tmpLowerRightX);
588
                UIMgr_writeTxFifo(tmpLowerRightY);			
589
            }
590
 
591
            /* move our pointer up to the beginning of the next object */
592
            pTableData += SIZE_OF_TRACKED_OBJECT;
593
        }
594
 
595
		/* all done...send the end of tracking packets char */
596
		UIMgr_writeTxFifo(0xFF);
597
	}	
598
#endif	
599
 
600
    /* the tracked object table will be cleared out right before we start
601
    to wait for VSYNC to indicate a new frame...so it doesn't need to be
602
    done now */
603
 
604
	/* schedule the next action to acquire a new frame */	
605
	PUBLISH_EVENT(EV_PROCESS_FRAME_COMPLETE);
606
}
607
 
608
/***********************************************************
609
	Function Name: FrameMgr_findConnectedness
610
	Function Description: This function is responsible for
611
	finding the connectedness between two particular run-
612
	length encoded lines of pixel data.  It updates the
613
	trackingTable as needed.
614
	Inputs:  none
615
	Outputs: none
616
***********************************************************/	
617
static void FrameMgr_findConnectedness(void)
618
{
619
	trackedColor_t currColor;
620
	unsigned char *pCurrLineColorInfo = currentLineBuffer;
621
	unsigned char *pTrackedObjectData;
622
	register unsigned char currPixelRunStart=0;
623
	register unsigned char currPixelRunFinish=0; 
624
	register unsigned char lastLineXStart=0;
625
	register unsigned char lastLineXFinish=0;  
626
	register unsigned char runLength=1;
627
	unsigned char i;
628
	bool_t colorConnected;	
629
 
630
	do
631
	{
632
		/* grab both the current color and the number of pixels
633
		in the run...remember, pixels start at 1, not 0! */
634
		colorConnected = FALSE;
635
		currColor = *pCurrLineColorInfo++;
636
		currPixelRunStart += runLength;
637
		runLength = *pCurrLineColorInfo++;
638
		currPixelRunFinish += runLength;
639
 
640
        /* make sure that the run-length is at least as wide as
641
        the minimum horizontal tracking width, and we care about the color */ 
642
 
643
		if ( (currColor != notTracked) && (runLength > MIN_OBJECT_TRACKING_WIDTH) )
644
		{			
645
            /* this run contains a color we care about, so 
646
			either it will begin a new tracked object, or it
647
			is connected to a currently tracked object...
648
			compare it with each object in the tracking
649
			table...we can't just look at the numTrackedObjects because
650
            it is entirely possible that the first couple of objects could
651
            be invalid...
652
 
653
            NOTE: Instead of accessing each element in the trackedObjectTable
654
            through the 'i' index, and then accessing the fields in each structure,
655
            a pointer to each entry is established each time through the loop, followed
656
            by accessing the elements through specified offsets.  GCC seems to be
657
            able to optimize this code much better than simply accessing the elements
658
            of each structure in the array the more normal way...*/
659
 
660
            pTrackedObjectData = (unsigned char *)pCurrentTrackedObjectTable;
661
			for (i=0; i<MAX_TRACKED_OBJECTS; i++)
662
			{
663
				if ( (currColor == *(pTrackedObjectData + COLOR_OFFSET)) && 
664
                     (*(pTrackedObjectData + VALID_OBJECT_OFFSET) == TRUE) &&
665
                     (*(pTrackedObjectData + Y_LOWER_RIGHT_OFFSET) == trackedLineCount - 1) &&
666
                    /* add a check to limit the vertical size of object to 18 pixels...this will help segment the
667
                       image for the purpose of tracking a line for the Chibots contest */
668
                     ( (*(pTrackedObjectData + Y_LOWER_RIGHT_OFFSET) - (*(pTrackedObjectData + Y_UPPER_LEFT_OFFSET) ) ) < 18) )                       
669
				{
670
					/* found a color match and the object is valid...check to see if there is
671
					connectedness */
672
					lastLineXStart = *(pTrackedObjectData + LAST_LINE_X_START_OFFSET);
673
					lastLineXFinish = *(pTrackedObjectData + LAST_LINE_X_FINISH_OFFSET);
674
 
675
					/* Check for the 5 following types of line connectedness:
676
					---------------------
677
					|                   |
678
					---------------------
679
					         -------------------------
680
							 |                       |
681
							 -------------------------  */
682
					if ( (	(currPixelRunStart >= lastLineXStart) &&
683
							(currPixelRunStart <= lastLineXFinish) )  ||
684
 
685
					/*               ---------------------
686
					                 |                   |
687
									 ---------------------
688
						-------------------
689
						|                 |
690
						-------------------  
691
						                   OR
692
						     ------------------------------
693
							 |                            |
694
							 ------------------------------
695
							              ---------
696
										  |       |
697
										  ---------  */
698
						 (	(currPixelRunFinish >= lastLineXStart) && 
699
							(currPixelRunFinish <= lastLineXFinish) ) ||
700
 
701
 
702
					/*     -------------------------------
703
					       |                             |
704
						   -------------------------------
705
						   -------------------------------
706
						   |                             |
707
						   -------------------------------
708
						                  OR
709
								     -------------
710
									 |           |
711
									 -------------
712
							-------------------------------
713
							|                             |
714
							-------------------------------   */
715
						 (  (currPixelRunStart <= lastLineXStart) &&
716
							(currPixelRunFinish >= lastLineXFinish) ) )
717
					{
718
						/* THERE IS CONNECTEDNESS...update the lastLineXStart and lastLineXFinish
719
						data pointed to by pTrackedObjectData */
720
						*(pTrackedObjectData + LAST_LINE_X_START_OFFSET) = currPixelRunStart;
721
						*(pTrackedObjectData + LAST_LINE_X_FINISH_OFFSET) = currPixelRunFinish;
722
 
723
						/* check if the bounding box needs to be updated */
724
						if (*(pTrackedObjectData + X_UPPER_LEFT_OFFSET) > currPixelRunStart)
725
						{
726
							/* need to update the bounding box for the upper left point to 
727
							enclose this new left-most point...we never have to update the
728
							upper left Y point, since each scan line we process moves from
729
							top to bottom */
730
							*(pTrackedObjectData + X_UPPER_LEFT_OFFSET) = currPixelRunStart;
731
						}
732
 
733
						if ( *(pTrackedObjectData + X_LOWER_RIGHT_OFFSET) < currPixelRunFinish)
734
						{
735
							/* need to update the bounding box for the lower right X point to
736
							enclose this new right-most point */
737
							*(pTrackedObjectData + X_LOWER_RIGHT_OFFSET) = currPixelRunFinish;
738
						}
739
 
740
						/* the lower right 'y' point always gets updated when connectedness is found */
741
						*(pTrackedObjectData + Y_LOWER_RIGHT_OFFSET) = trackedLineCount;
742
 
743
						/* set a flag indicating that that color run is part of another
744
						object and thus doesn't need to be added as a new entry into the
745
						tracking table */
746
						colorConnected = TRUE;
747
						break;
748
					}
749
				}
750
 
751
                /* go to the next object */
752
                pTrackedObjectData += SIZE_OF_TRACKED_OBJECT;
753
			}
754
 
755
			if (colorConnected == FALSE)
756
			{
757
				/* a new entry needs to be made to the tracking table, since we have
758
				a run-length with a color, and it isn't connected to anything...but we
759
				can only do this if there is space left in the trackedObject table */
760
				if (numCurrTrackedObjects < MAX_TRACKED_OBJECTS)
761
				{                
762
                    /* space is available...add the object...but first we need to find an
763
                    invalid object in the object tracking table */
764
                    pTrackedObjectData = (unsigned char *)pCurrentTrackedObjectTable;
765
                    for (i=0; i<MAX_TRACKED_OBJECTS; i++)
766
                    {
767
                        if ( *(pTrackedObjectData + VALID_OBJECT_OFFSET) == FALSE)  break;
768
 
769
                        /* if we haven't broken above, then the object must have been valid...
770
                        go ahead and move the pointer to the next object to check it */
771
                        pTrackedObjectData += SIZE_OF_TRACKED_OBJECT;
772
                    }
773
 
774
 
775
					/* now that we have a pointer to the tracked object to be updated, update all
776
					the fields */
777
					*(pTrackedObjectData + COLOR_OFFSET)                = currColor;			/* color */
778
					*(pTrackedObjectData + LAST_LINE_X_START_OFFSET)    = currPixelRunStart; 	/* lastLineXStart */
779
					*(pTrackedObjectData + LAST_LINE_X_FINISH_OFFSET)   = currPixelRunFinish;	/* lastLineXFinish */
780
					*(pTrackedObjectData + X_UPPER_LEFT_OFFSET)         = currPixelRunStart;	/* x_upperLeft */
781
					*(pTrackedObjectData + Y_UPPER_LEFT_OFFSET)         = trackedLineCount;	/* y_upperLeft */
782
					*(pTrackedObjectData + X_LOWER_RIGHT_OFFSET)        = currPixelRunFinish;	/* x_lowerRight */
783
					*(pTrackedObjectData + Y_LOWER_RIGHT_OFFSET)        = trackedLineCount;	/* y_lowerRight */
784
                    *(pTrackedObjectData + VALID_OBJECT_OFFSET)         = TRUE;                /* objectValid flag */
785
 
786
					numCurrTrackedObjects++;
787
				}
788
			}
789
 
790
            /* move the pointer to the beginning of the next tracked object */
791
            pTrackedObjectData += SIZE_OF_TRACKED_OBJECT;
792
		}
793
	} while(currPixelRunFinish < ACTUAL_NUM_PIXELS_IN_A_LINE);
794
}
795