// This function is called each time the DSP receives a new picture
void userProcessColorImageFunc_laser(bgr *ptrImage) {
if (ptrImage != NULL) {
// add your vision processing code here
// Send image to Color LCD if LCD ready for new data
if (updateLCD) {
updateLCD = 0;
BCACHE_inv((void *)(ADDR_VIDEO_DATA_BASE+0x1A900),IMAGE_ROWS*IMAGE_COLUMNS*3,EDMA3_CACHE_WAIT);
// Flush or write back source
BCACHE_wb ((void *)ptrImage,IMAGE_ROWS*IMAGE_COLUMNS*3,EDMA3_CACHE_WAIT);
//Need to clean the cache here
EDMA3_1_Regs->PARAMENTRY[33].OPT = 0x0011E00C;
EDMA3_1_Regs->PARAMENTRY[33].SRC = (unsigned int)Image_data;
EDMA3_1_Regs->PARAMENTRY[33].A_B_CNT = 0x004004A4; //Maybe to ACNT = 1188 and BCNT = 64
EDMA3_1_Regs->PARAMENTRY[33].DST = (ADDR_VIDEO_DATA_BASE+LCD_IMAGE_OFFSET);
EDMA3_1_Regs->PARAMENTRY[33].SRC_DST_BIDX = 0x04A404A4;
EDMA3_1_Regs->PARAMENTRY[33].LINK_BCNTRLD = 0x0000FFFF; // Null link
EDMA3_1_Regs->PARAMENTRY[33].SRC_DST_CIDX = 0x0;
EDMA3_1_Regs->PARAMENTRY[33].CCNT = 0x1; //Last command triggers transmission
}
// If Linux is ready for another full 176X144 RGB image start the EDMA transfer of the image to external memory
if (GET_IMAGE_TO_LINUX) {
// Invalidate Destination
BCACHE_inv((void *)Linux_Image,IMAGE_ROWS*IMAGE_COLUMNS*3,EDMA3_CACHE_WAIT);
// Flush or write back source
BCACHE_wb ((void *)ptrImage,IMAGE_ROWS*IMAGE_COLUMNS*3,EDMA3_CACHE_WAIT);
EDMA3_1_Regs->PARAMENTRY[32].OPT = 0x0011F00C;
EDMA3_1_Regs->PARAMENTRY[32].SRC = (unsigned int)Image_data;
EDMA3_1_Regs->PARAMENTRY[32].A_B_CNT = 0x004004A4; //Maybe to ACNT = 1188 and BCNT = 64
EDMA3_1_Regs->PARAMENTRY[32].DST = (ADDR_VIDEO_DATA_BASE+LINUX_IMAGE_OFFSET);
EDMA3_1_Regs->PARAMENTRY[32].SRC_DST_BIDX = 0x04A404A4;
EDMA3_1_Regs->PARAMENTRY[32].LINK_BCNTRLD = 0x0000FFFF; // Null link
EDMA3_1_Regs->PARAMENTRY[32].SRC_DST_CIDX = 0x0;
EDMA3_1_Regs->PARAMENTRY[32].CCNT = 0x1; //Last command triggers transmission
}
} // Ends if statement to see if image pointer is null
}
void dprintf (const char * format, ...)
{
int n;
va_list args;
va_start (args, format);
n = vsprintf (printf_buffer,format, args);
va_end (args);
if (n<=0) return;
// writeback cache:
BCACHE_wb (printf_buffer, n, 1);
// notify GPP:
NOTIFY_notify (ID_GPP, 0, 6, (Uint32)printf_buffer);
// wait for GPP acknowledge
SEM_pendBinary (dprint_sema, SYS_FOREVER);
}
/**
* \brief EDMA3 Cache Flush
*
* This function flushes (cleans) the Cache
*
* \param mem_start_ptr [IN] Starting adress of memory. Please note that
* this should be 32 bytes alinged.
* \param num_bytes [IN] length of buffer
* \return nil return value
*/
void Edma3_CacheFlush(unsigned int mem_start_ptr,
unsigned int num_bytes)
{
/* Verify whether the start address is 128-bytes aligned or not */
if((mem_start_ptr & (0x7FU)) != 0)
{
#ifdef EDMA3_DRV_DEBUG
EDMA3_DRV_PRINTF("\r\n Cache : Memory is not 128 bytes alinged\r\n");
#endif
}
BCACHE_wb ((void *)mem_start_ptr,
num_bytes,
EDMA3_CACHE_WAIT);
}
// This function is called each time the DSP receives a new picture
void userProcessColorImageFunc_laser(bgr *ptrImage) {
int i;
if (toggle)
{
// seeing blue golf ball
specs_h = 131;
specs_hrad = 26;
if((specs_h-specs_hrad)<0) // wrap 0->360
{
specs_h2=specs_h+256;
}
else // wrap 360->0
{
specs_h2=specs_h-256;
}
specs_s = 91;
specs_srad = 36;
specs_v = 121;
specs_vrad = 64;
}
else
{
// seeing green
specs_h = 91;
specs_hrad = 25;
if((specs_h-specs_hrad)<0) // wrap 0->360
{
specs_h2=specs_h+256;
}
else // wrap 360->0
{
specs_h2=specs_h-256;
}
specs_s = 157;
specs_srad = 50;
specs_v = 170;
specs_vrad = 25;
}
if (ptrImage != NULL) {
// Initialize all arrays for equivalency
for (i=0; i < MAX_NUM_EQUIVALENCIES; i++) {
equivalency_objects[i] = 0; // initialze link array
object_stats[i].num_pixels_in_object = 0;
object_stats[i].sum_r = 0;
object_stats[i].sum_c = 0;
}
num_unique_objects = 0;
object_detected = 0;
// First Pass thru image. Convert RGB to HSV. This code is taking into account that the robot's camera only returns
// a value between 16 and 240 for pixel intensity. It also adds a gain of 2 to the blue intensity.
for (r=0;r<IMAGE_ROWS;r++) {
for(c=0;c<IMAGE_COLUMNS;c++) {
red = ((ptrImage[r*IMAGE_COLUMNS+c].red - 16)*255)/224;
green = ((ptrImage[r*IMAGE_COLUMNS+c].green - 16)*255)/224;
blue = ptrImage[r*IMAGE_COLUMNS+c].blue*2;
if (blue > 240) {
blue = 240;
}
blue = ((blue - 16)*255)/224;
min = my_min( red, green, blue );
value = my_max( red, green, blue );
delta = value - min;
if( value != 0 ) {
sat = (delta*255) / value; // s
if (delta != 0) {
if( red == value )
hue = 60*( green - blue ) / delta; // between yellow & magenta
else if( green == value )
hue = 120 + 60*( blue - red ) / delta; // between cyan & yellow
else
hue = 240 + 60*( red - green ) / delta; // between magenta & cyan
if( hue < 0 )
hue += 360;
} else {
hue = 0;
sat = 0;
}
} else {
// r = g = b = 0 // s = 0, v is undefined
sat = 0;
hue = 0;
}
hue = (hue*255)/360;
if ( (abs(sat-specs_s)<=specs_srad)
&& (abs(value-specs_v)<=specs_vrad)
&& ( (abs(hue-specs_h)<=specs_hrad)
|| (abs(hue-specs_h2)<=specs_hrad) // catch the hue wraparround
)
) {
object_detected = 1; // Set a flag that at least one pixel found above threshold
// -------- Connectivity calculations ------------
// Labels pixels 1 to MAX_NUM_EQUIVALENCIES depending on top and left neighbors
// The labels represent object number...
if (r == 0) top = 0; else top = Thres_Image[(r-1)*IMAGE_COLUMNS+c]; // previous row, same column
if (c == 0) left = 0; else left = Thres_Image[r*IMAGE_COLUMNS+(c-1)]; // same row, previous column
neighbor_type = 0;
if (left != 0) neighbor_type += 1;
if (top != 0) neighbor_type += 2;
current_object = 0;
switch (neighbor_type) {
case 0: // Both neighbors zero, New object needed
if (num_unique_objects < (MAX_NUM_EQUIVALENCIES-1) ) {
num_unique_objects++;
equivalency_objects[num_unique_objects] = num_unique_objects;
} else {
too_many_objects++;
}
current_object = num_unique_objects;
break;
case 1: // Top is zero, left is not zero
current_object = left;
break;
case 2: // Left is zero, top is not zero
current_object = top;
break;
case 3: // Top and left are not zero... must note equivalency
if (top == left) current_object = left;
else {
if (Check_Equivalency(top,left) == 0) {
current_object = Set_Equivalency(top,left);
}
else {
current_object = left;
}
}
break;
default: // Should NEVER enter here
current_object = 0; // Object 0 stores errors
break;
}
Thres_Image[r*IMAGE_COLUMNS+c] = current_object;
object_stats[current_object].num_pixels_in_object +=1;
object_stats[current_object].sum_r += r;
object_stats[current_object].sum_c += c;
// ---------- Done with connectivity calculations (first pass) ----------
} else {
Thres_Image[r*IMAGE_COLUMNS+c] = 0;
}
}
}
// initialize final object stats
for (i=1; i<= MAX_NUM_OBJECTS; i++) {
final_object_stats[i].sum_r = 0;
final_object_stats[i].sum_c = 0;
final_object_stats[i].num_pixels_in_object = 0;
final_object_stats[i].center_r = 0.0;
final_object_stats[i].center_c = 0.0;
final_object_stats[i].C02_sum = 0.0;
final_object_stats[i].C11_sum = 0.0;
final_object_stats[i].C20_sum = 0.0;
final_object_stats[i].theta = 0.0;
}
if (object_detected == 0) {
num_unique_objects = 0;
}
else {
num_unique_objects = Fix_Equivalency(num_unique_objects);// num_unique_objects contains the number of initial equivalencies found
}
if (num_unique_objects > MAX_NUM_OBJECTS) num_unique_objects = MAX_NUM_OBJECTS;
// Third pass: correct image for nice display and calculate object moments
// This is commented out because for the 176X144 image this adds a large amount of proccessing time when a large blob is found
// for (r=0; r < IMAGE_ROWS; r++) { // Loop over rows
// for (c=0; c < IMAGE_COLUMNS; c++) { // Loop over columns
// if (Thres_Image[r*IMAGE_COLUMNS+c] > 0) {
// // Fix pixel equivalency
// Thres_Image[r*IMAGE_COLUMNS+c] = equivalency_objects[Thres_Image[r*IMAGE_COLUMNS+c]];
//
// // Calculate second moments here
// if ((Thres_Image[r*IMAGE_COLUMNS+c] > 0) && (Thres_Image[r*IMAGE_COLUMNS+c] <= MAX_NUM_OBJECTS)) {
// final_object_stats[Thres_Image[r*IMAGE_COLUMNS+c]].C02_sum += (c - final_object_stats[Thres_Image[r*IMAGE_COLUMNS+c]].center_c)*(c - final_object_stats[Thres_Image[r*IMAGE_COLUMNS+c]].center_c);
// final_object_stats[Thres_Image[r*IMAGE_COLUMNS+c]].C11_sum += (r - final_object_stats[Thres_Image[r*IMAGE_COLUMNS+c]].center_r)*(c - final_object_stats[Thres_Image[r*IMAGE_COLUMNS+c]].center_c);
// final_object_stats[Thres_Image[r*IMAGE_COLUMNS+c]].C20_sum += (r - final_object_stats[Thres_Image[r*IMAGE_COLUMNS+c]].center_r)*(r - final_object_stats[Thres_Image[r*IMAGE_COLUMNS+c]].center_r);
// }
//
// }
// }
// }
// Find largest object and calculate object orientation
largest_num_pixels = 0;
largest_object = 1;
for (k = 1; k <= num_unique_objects ; k++) {
if (final_object_stats[k].num_pixels_in_object > largest_num_pixels) {
largest_num_pixels = final_object_stats[k].num_pixels_in_object;
largest_object = k;
}
// find theta of found blob
// commented out because moments need to be calculated above to use this code.
// if (final_object_stats[k].num_pixels_in_object > NUMPIXELS_TO_CALC_ANGLE) {
// // Calculate the object orientation angle if there are NUMPIXELS_TO_CALC_ANGLE in the object
// Cdifference = final_object_stats[k].C20_sum - final_object_stats[k].C02_sum;
// if (Cdifference != 0.0F) { // can't divide by zero
// final_object_stats[k].theta = atansp(final_object_stats[k].C11_sum/Cdifference)/2.0F;
// } else {
// final_object_stats[k].theta = 0.0;
// }
// if (final_object_stats[k].C20_sum > final_object_stats[k].C02_sum) {
// if (final_object_stats[k].theta < 0) final_object_stats[k].theta += PI/2.0F;
// else final_object_stats[k].theta += -PI/2.0F;
// }
// } else {
// final_object_stats[k].theta = 0;
// }
} // Ends loop through objects
// Find the middle
if (new_vision_data == 0)
{
if (toggle)
{
blue_rbar = (int) (final_object_stats[largest_object].center_r);
blue_cbar = (int) (final_object_stats[largest_object].center_c);
blueNumPixels = (int) final_object_stats[largest_object].num_pixels_in_object;
toggle = 0;
}
else
{
green_rbar = (int) (final_object_stats[largest_object].center_r);
green_cbar = (int) (final_object_stats[largest_object].center_c);
greenNumPixels = (int) final_object_stats[largest_object].num_pixels_in_object;
toggle = 1;
}
new_vision_data = 1;
}
// pass data to RobotControl()
if (new_coordata == 0) {
if (final_object_stats[largest_object].num_pixels_in_object > 1) {
noimagefound = 0;
new_num_found_objects = num_unique_objects;
object_x = cbar - IMAGE_COLUMNS/2;
object_y = rbar - IMAGE_ROWS/2;
numpels = final_object_stats[largest_object].num_pixels_in_object;
//new_object_theta = final_object_stats[largest_object].theta;
//new_C20 = final_object_stats[largest_object].C20_sum;
//new_C02 = final_object_stats[largest_object].C02_sum;
new_coordata = 1;
} else {
noimagefound = 1;
new_num_found_objects = num_unique_objects;
object_x = 0.0;
object_y = 0.0;
numpels = 0;
//new_object_theta = 0.0;
//new_C20 = 0.0;
//new_C02 = 0.0;
new_coordata = 1;
}
}
//create green x for largest centroid position
if (final_object_stats[largest_object].num_pixels_in_object > 6) {
ptrImage[rbar*IMAGE_COLUMNS+cbar].red = 0;
ptrImage[rbar*IMAGE_COLUMNS+cbar].blue = 0;
ptrImage[rbar*IMAGE_COLUMNS+cbar].green = 255;
if (rbar > 0) {
ptrImage[(rbar-1)*IMAGE_COLUMNS+cbar].red = 0;
ptrImage[(rbar-1)*IMAGE_COLUMNS+cbar].blue = 0;
ptrImage[(rbar-1)*IMAGE_COLUMNS+cbar].green = 255;
}
if (rbar < (IMAGE_ROWS-1)) {
ptrImage[(rbar+1)*IMAGE_COLUMNS+cbar].red = 0;
ptrImage[(rbar+1)*IMAGE_COLUMNS+cbar].blue = 0;
ptrImage[(rbar+1)*IMAGE_COLUMNS+cbar].green = 255;
}
if (cbar > 0) {
ptrImage[rbar*IMAGE_COLUMNS+(cbar-1)].red = 0;
ptrImage[rbar*IMAGE_COLUMNS+(cbar-1)].blue = 0;
ptrImage[rbar*IMAGE_COLUMNS+(cbar-1)].green = 255;
}
if (cbar < (IMAGE_COLUMNS-1)) {
ptrImage[rbar*IMAGE_COLUMNS+(cbar+1)].red = 0;
ptrImage[rbar*IMAGE_COLUMNS+(cbar+1)].blue = 0;
ptrImage[rbar*IMAGE_COLUMNS+(cbar+1)].green = 255;
}
}
if ( 12 == switchstate) {
UpdateLCDwithLADAR(ptrImage,1);
}
// Send image to Color LCD if LCD ready for new data
if (updateLCD) {
updateLCD = 0;
BCACHE_inv((void *)(ADDR_VIDEO_DATA_BASE+0x1A900),IMAGE_ROWS*IMAGE_COLUMNS*3,EDMA3_CACHE_WAIT);
// Flush or write back source
BCACHE_wb ((void *)ptrImage,IMAGE_ROWS*IMAGE_COLUMNS*3,EDMA3_CACHE_WAIT);
//Need to clean the cache here
EDMA3_1_Regs->PARAMENTRY[33].OPT = 0x0011E00C;
EDMA3_1_Regs->PARAMENTRY[33].SRC = (unsigned int)Image_data;
EDMA3_1_Regs->PARAMENTRY[33].A_B_CNT = 0x004004A4; //Maybe to ACNT = 1188 and BCNT = 64
EDMA3_1_Regs->PARAMENTRY[33].DST = (ADDR_VIDEO_DATA_BASE+LCD_IMAGE_OFFSET);
EDMA3_1_Regs->PARAMENTRY[33].SRC_DST_BIDX = 0x04A404A4;
EDMA3_1_Regs->PARAMENTRY[33].LINK_BCNTRLD = 0x0000FFFF; // Null link
EDMA3_1_Regs->PARAMENTRY[33].SRC_DST_CIDX = 0x0;
EDMA3_1_Regs->PARAMENTRY[33].CCNT = 0x1; //Last command triggers transmission
}
// If Linux is ready for another full 176X144 RGB image start the EDMA transfer of the image to external memory
if (GET_IMAGE_TO_LINUX) {
// Invalidate Destination
BCACHE_inv((void *)Linux_Image,IMAGE_ROWS*IMAGE_COLUMNS*3,EDMA3_CACHE_WAIT);
// Flush or write back source
BCACHE_wb ((void *)ptrImage,IMAGE_ROWS*IMAGE_COLUMNS*3,EDMA3_CACHE_WAIT);
EDMA3_1_Regs->PARAMENTRY[32].OPT = 0x0011F00C;
EDMA3_1_Regs->PARAMENTRY[32].SRC = (unsigned int)Image_data;
EDMA3_1_Regs->PARAMENTRY[32].A_B_CNT = 0x004004A4; //Maybe to ACNT = 1188 and BCNT = 64
EDMA3_1_Regs->PARAMENTRY[32].DST = (ADDR_VIDEO_DATA_BASE+LINUX_IMAGE_OFFSET);
EDMA3_1_Regs->PARAMENTRY[32].SRC_DST_BIDX = 0x04A404A4;
EDMA3_1_Regs->PARAMENTRY[32].LINK_BCNTRLD = 0x0000FFFF; // Null link
EDMA3_1_Regs->PARAMENTRY[32].SRC_DST_CIDX = 0x0;
EDMA3_1_Regs->PARAMENTRY[32].CCNT = 0x1; //Last command triggers transmission
}
} // Ends if statement to see if image pointer is null
}
void ComWithLinux(void) {
int i = 0;
TSK_sleep(100);
while(1) {
BCACHE_inv((void *)ptrshrdmem,sizeof(sharedmemstruct),EDMA3_CACHE_WAIT);
if (GET_DATA_FROM_LINUX) {
if (newnavdata == 0) {
newvref = ptrshrdmem->Floats_to_DSP[0];
newturn = ptrshrdmem->Floats_to_DSP[1];
newnavdata = 1;
}
CLR_DATA_FROM_LINUX;
}
if (GET_LVDATA_FROM_LINUX) {
if (ptrshrdmem->DSPRec_size > 256) ptrshrdmem->DSPRec_size = 256;
for (i=0;i<ptrshrdmem->DSPRec_size;i++) {
fromLinuxstring[i] = ptrshrdmem->DSPRec_buf[i];
}
fromLinuxstring[i] = '\0';
if (new_LV_data == 0) {
sscanf(fromLinuxstring,"%f%f",&LVvalue1,&LVvalue2);
new_LV_data = 1;
}
CLR_LVDATA_FROM_LINUX;
}
if ((tskcount%6)==0) {
if (GET_LVDATA_TO_LINUX) {
// Default
ptrshrdmem->DSPSend_size = sprintf(toLinuxstring,"1.0 1.0 1.0 1.0");
// you would do something like this
//ptrshrdmem->DSPSend_size = sprintf(toLinuxstring,"%.1f %.1f %.1f %.1f",var1,var2,var3,var4);
for (i=0;i<ptrshrdmem->DSPSend_size;i++) {
ptrshrdmem->DSPSend_buf[i] = toLinuxstring[i];
}
// Flush or write back source
BCACHE_wb((void *)ptrshrdmem,sizeof(sharedmemstruct),EDMA3_CACHE_WAIT);
CLR_LVDATA_TO_LINUX;
}
}
if (GET_DATAFORFILE_TO_LINUX) {
// First make sure all scratch elements are zero
for (i=0;i<500;i++) {
ptrshrdmem->scratch[i] = 0;
}
// Write LADARdataX to scratch
for (i=0;i<228;i++) {
ptrshrdmem->scratch[i] = LADARdataX[i];
}
// Write LADARdataY to scratch
for (i=0;i<228;i++) {
ptrshrdmem->scratch[228+i] = LADARdataY[i];
}
// Flush or write back source
BCACHE_wb((void *)ptrshrdmem,sizeof(sharedmemstruct),EDMA3_CACHE_WAIT);
CLR_DATAFORFILE_TO_LINUX;
}
tskcount++;
TSK_sleep(40);
}
}
