/*
* receive an message from mail box
* mbox: mail box
* msg: message buffer
* timeout: receive's max wait time
* <0 -- wait forever; 0 -- no wait; >0 -- wait for timeout ticks
* retrun: RERROR on failed, RTIMEOUT on timeout, RAGAIN on busy, ROK on success.
*/
int mbox_pend(mbox_t *mbox, uint32_t *msg, int timeout)
{
uint32_t f, len;
int ret = RERROR;
struct dtask *myself;
if(mbox && (mbox->flag & IPC_FLAG_VALID))
{
ret = ROK;
SYS_FSAVE(f);
len = queue_buffer_get(&mbox->buf, msg);
SYS_FRESTORE(f);
if(len == 0)
{
if(timeout == 0)
ret = RAGAIN;
else
{
SYS_FSAVE(f);
myself = current;
current->wakeup_cause = RERROR;
if(timeout > 0)
{
assert(current->t_delay.param[0] == TASK_T(current));
assert(!(current->flags & TASK_FLAGS_DELAYING));
ptimer_start(&sysdelay_queue, ¤t->t_delay, timeout);
current->flags |= TASK_FLAGS_DELAYING;
}
#ifdef INCLUDE_JOURNAL
journal_ipc_pend((ipc_t *)mbox, timeout);
#endif // INCLUDE_JOURNAL
task_block(&(mbox->taskq));
SYS_FRESTORE(f);
/* at this point, wakeup cause is modified */
switch ((ret = myself->wakeup_cause))
{
case ROK:
SYS_FSAVE(f);
len = queue_buffer_get(&mbox->buf, msg);
SYS_FRESTORE(f);
if(len <= 0)
ret = RERROR; /* double check for length */
break;
case RTIMEOUT:
case RSIGNAL:
default:
break;
}
}
}
}
return ret;
}
/*-----------------------------------------------------------------------*/
int task_delay(uint32_t tick)
{
int ret = RERROR;
uint32_t f;
if(tick > 0){
SYS_FSAVE(f);
assert(current->t_delay.onexpired_func == task_timeout);
assert(current->t_delay.param[0] == TASK_T(current));
ret = ptimer_start(&sysdelay_queue, ¤t->t_delay, tick);
if(ret == ROK){
current->flags |= TASK_FLAGS_DELAYING;
task_block(NULL);
}
SYS_FRESTORE(f);
}
return ret;
}
void ptimer_consume_time(ptimer_table_t *table, u32 time)
{
ptimer_t *timer;
u32 i;
if(table == NULL) return;
for(i=0; i<time; i++)
{
while(!DLLIST_EMPTY(&table->table[table->curslot]))
{
timer = (ptimer_t *)DLLIST_HEAD(&table->table[table->curslot]);
assert(ptimer_is_running(timer));
/* remove all timers in current slot */
dllist_remove(NULL, (dllist_node_t *)timer);
/* handle remainder */
if(timer->remainder)
{
ptimer_start_remainder(table, timer, timer->remainder);
continue;
}
/* mark timer as not running */
timer->flags &= ~PTIMER_FLAG_RUNNING;
/* call onexpired_func */
// ZLOG_DEBUG("timer expired: 0x%p at slot %u\n", timer, table->curslot);
if(timer->onexpired_func)
{
timer->onexpired_func(timer, timer->param[0], timer->param[1]);
}
/* if periodic timer */
if((timer->flags & PTIMER_FLAG_PERIODIC) && !(timer->flags & PTIMER_FLAG_RUNNING))
ptimer_start(table, timer, timer->duration);
}
table->curslot = (table->curslot + 1) & (table->allslots - 1);
}
}
void pal_startTimers(void) {
// Initialize the hardware to drive the
// signal processing layers.
ctimer_set_callback(pal_captureTimerFn);
comparator_enablePin(LEFT_PORT, LEFT_PIN);
comparator_enablePin(VREF_PORT, VREF_PIN);
comparator_init();
comparator_setup(
comparator_getChannel(LEFT_PORT, LEFT_PIN),
comparator_getChannel(VREF_PORT, VREF_PIN)
);
comparator_on();
ctimer_init();
ptimer_init();
ptimer_start(TIMER_TICKS, pal_periodicTimerFn);
// Wait 100ms for everything to stabalize.
//util_delayMs(100);
}
void RSEDU_image_processing(void * buffer)
{
//process control
static int counter = 0;
//communication
static float vis_data[4];
int status;
static int vis_fifo;
//image variables
int row, col;
float yuv[3];
float feature_pps[3][4];
static unsigned char matchLookup[128][128][128];
//camera variables
float camerapos[3]; //@TODO static?
float camerayaw; //@TODO static?
//Image and Matching Thresholds
static int filtersize = 5; //odd numbered! dummy for potential gaussian filter mask, etc.
int pxls_ftr_min = 5; //minimum required nr of detected pixels per landmark
//Landmarks, Pose Estimation Matrices
int lndmrk_nr = 5, lndmrk_best = 0;
static lndmrk_t lndmrks[5];
//matrices for reconstructing camera pose by intrMatrx_inv*landmark-pixellocation*ldnmrk_pinv
static double ldnmrk_pinv[4][4] =
{
{ -3.093396787626414, 2.082093991671625 , 0, 0.766805472932779},
{2.320047590719810 , 3.438429506246282 , 0 , 0.174895895300416},
{ -1.737061273051754 , -2.676977989292088 , 0 , 0.299821534800714},
{2.510410469958359 , -2.843545508625819 , 0 , -0.241522903033909}
};
static double intrMatrx_inv[3][3] = {{ 0.006551831768882 , 0, -0.550082487527771},
{0, 0.006546559888686, -0.399495805347318},
{0, 0, 1.000000000000000}
};
//wait on first call
if(counter == 1)
{
usleep(20000);
}
//Init Communication, Streaming
int fifo;
if(FEAT_IMSAVE == 2)
{
u8 * raw = buffer;
};
pixel2_t *image = buffer; /* Picture is a 160x120 pixels, stored in YUV422 interlaced format - TO BE CHECKED */
/*
* PROGRAM
*/
//ptiming - declare and start
//------------
long long start;
static FILE *ptfile;
ptimer_start(FEAT_TIME, counter, &(start));
//------------
//process control
counter++;
if(counter == 1)
{
//ptiming - init file
//------------
ptimer_init(FEAT_TIME, __func__, &(ptfile), NULL);
//------------
printf("rsedu_vis(): Init fifo-communication...\n");
//open fifo to dump visual position estimates to control code
if(access("/tmp/vis_fifo", F_OK) != -1)
{
printf("rsedu_vis(): SUCCESS POSVIS FIFO exists! \n");
vis_fifo = open("/tmp/vis_fifo", O_WRONLY);
if(vis_fifo)
{
vis_data[0] = -99.0;
write(vis_fifo, (float*)(&vis_data), sizeof(vis_data));
close(vis_fifo);
printf("rsedu_vis(): SUCCESS opening POSVIS-fifo!\n");
}
else
{
printf("rsedu_vis(): ERROR opening POSVIS-fifo!\n");
}
}
else
{
printf("rsedu_vis(): ERROR opening POSVIS-fifo!\n");
}
}
long nx =0, ny=0; /* image size in x, y direction */
pixel2_t fileimage[80*120];
//helper variables
long i, j; /* loop variables */
float x_avg = 0;
int hits = 0;
int y;
nx=80;
ny=120;
//read image data
for (j=0; j<ny; j++)
{
for (i=0; i<nx; i++)
{
y = (int)image[nx*j+i].y1; //noneg yuv! transform to 0 centerd uav by (-16,-128,-128)
if (y > 240){
x_avg += i;
hits++;
}
}
}
if (hits > 0){
x_avg =(int) (100*(x_avg/hits) / 80.0) - 50;
//XXX printf("x average is %f\n", x_avg);
}
/*
* dump pose into FIFO to make available to controls code
*/
//compile data
vis_data[1] = (float)x_avg;
/*vis_data[1] = 0.0;
vis_data[2] = 0.0;
vis_data[3] = 0.0;*/
vis_fifo = open("/tmp/vis_fifo", O_WRONLY);
if(vis_fifo)
{
write(vis_fifo, (float*)(&vis_data), sizeof(vis_data));
close(vis_fifo);
}
usleep(4000);
//ptiming - store
//----------
ptimer_stopstore(FEAT_TIME, counter, start, ptfile);
//----------
}
static void tc_write_seq(const UINT32 start_lsn, const UINT32 io_num, const UINT32 sector_size)
{
UINT32 i, j, wr_buf_addr, rd_buf_addr, data;
UINT32 lba, num_sectors = sector_size;
UINT32 io_cnt = io_num;
UINT32 const start_lba = start_lsn;
/* UINT32 volatile g_barrier = 0; while (g_barrier == 0); */
led(0);
// STEP 1 - write
for (UINT32 loop = 0; loop < 5; loop++)
{
wr_buf_addr = WR_BUF_ADDR;
data = 0;
lba = start_lba;
uart_print_32(loop); uart_print("");
for (i = 0; i < io_cnt; i++)
{
wr_buf_addr = WR_BUF_PTR(g_ftl_write_buf_id) + ((lba % SECTORS_PER_PAGE) * BYTES_PER_SECTOR);
for (j = 0; j < num_sectors; j++)
{
mem_set_dram(wr_buf_addr, data, BYTES_PER_SECTOR);
wr_buf_addr += BYTES_PER_SECTOR;
if (wr_buf_addr >= WR_BUF_ADDR + WR_BUF_BYTES)
{
wr_buf_addr = WR_BUF_ADDR;
}
data++;
}
if( i == 0x0000081C)
i = i;
ptimer_start();
ftl_write(lba, num_sectors);
ptimer_stop_and_uart_print();
lba += num_sectors;
if (lba >= (UINT32)NUM_LSECTORS)
{
uart_print("adjust lba because of out of lba");
lba = 0;
}
}
// STEP 2 - read and verify
rd_buf_addr = RD_BUF_ADDR;
data = 0;
lba = start_lba;
num_sectors = MIN(num_sectors, NUM_RD_BUFFERS * SECTORS_PER_PAGE);
for (i = 0; i < io_cnt; i++)
{
rd_buf_addr = RD_BUF_PTR(g_ftl_read_buf_id) + ((lba % SECTORS_PER_PAGE) * BYTES_PER_SECTOR);
/* ptimer_start(); */
if( i == 0x0000081C)
i = i;
ftl_read(lba, num_sectors);
flash_finish();
/* ptimer_stop_and_uart_print(); */
for (j = 0; j < num_sectors; j++)
{
UINT32 sample = read_dram_32(rd_buf_addr);
if (sample != data)
{
uart_printf("ftl test fail...io#: %d, %d", lba, num_sectors);
uart_printf("sample data %d should be %d", sample, data);
led_blink();
}
rd_buf_addr += BYTES_PER_SECTOR;
if (rd_buf_addr >= RD_BUF_ADDR + RD_BUF_BYTES)
{
rd_buf_addr = RD_BUF_ADDR;
}
data++;
}
lba += num_sectors;
if (lba >= IO_LIMIT + num_sectors)
{
lba = 0;
}
}
}
ftl_flush();
}
void RSEDU_image_processing(void * buffer)
{
//process control
static int counter = 0;
//communication
static float vis_data[4];
int status;
static int vis_fifo;
//image variables
int i, j;
int row, col;
float yuv[3];
int nx = 80, ny = 120;
float feature_pps[3][4];
static unsigned char matchLookup[128][128][128];
//camera variables
float camerapos[3]; //@TODO static?
float camerayaw; //@TODO static?
//Image and Matching Thresholds
static int filtersize = 5; //odd numbered! dummy for potential gaussian filter mask, etc.
int pxls_ftr_min = 5; //minimum required nr of detected pixels per landmark
//Landmarks, Pose Estimation Matrices
int lndmrk_nr = 5, lndmrk_best = 0;
static lndmrk_t lndmrks[5];
//matrices for reconstructing camera pose by intrMatrx_inv*landmark-pixellocation*ldnmrk_pinv
static double ldnmrk_pinv[4][4] =
{
{ -3.093396787626414, 2.082093991671625 , 0, 0.766805472932779},
{2.320047590719810 , 3.438429506246282 , 0 , 0.174895895300416},
{ -1.737061273051754 , -2.676977989292088 , 0 , 0.299821534800714},
{2.510410469958359 , -2.843545508625819 , 0 , -0.241522903033909}
};
static double intrMatrx_inv[3][3] = {{ 0.006551831768882 , 0, -0.550082487527771},
{0, 0.006546559888686, -0.399495805347318},
{0, 0, 1.000000000000000}
};
//wait on first call
if(counter == 1)
{
usleep(20000);
}
//Init Communication, Streaming
int fifo;
if(FEAT_IMSAVE == 2)
{
u8 * raw = buffer;
};
pixel2_t *image = buffer; /* Picture is a 160x120 pixels, stored in YUV422 interlaced format - TO BE CHECKED */
/*
* PROGRAM
*/
//ptiming - declare and start
//------------
long long start;
static FILE *ptfile;
ptimer_start(FEAT_TIME, counter, &(start));
//------------
//process control
counter++;
if(counter == 1)
{
//ptiming - init file
//------------
ptimer_init(FEAT_TIME, __func__, &(ptfile), NULL);
//------------
printf("rsedu_vis(): Init fifo-communication...\n");
//open fifo to dump visual position estimates to control code
if(access("/tmp/vis_fifo", F_OK) != -1)
{
printf("rsedu_vis(): SUCCESS POSVIS FIFO exists! \n");
vis_fifo = open("/tmp/vis_fifo", O_WRONLY);
if(vis_fifo)
{
vis_data[0] = -99.0;
write(vis_fifo, (float*)(&vis_data), sizeof(vis_data));
close(vis_fifo);
printf("rsedu_vis(): SUCCESS opening POSVIS-fifo!\n");
}
else
{
printf("rsedu_vis(): ERROR opening POSVIS-fifo!\n");
}
}
else
{
printf("rsedu_vis(): ERROR opening POSVIS-fifo!\n");
}
//load lookuptable for matching process
if(FEAT_NOLOOK == 0)
{
FILE* data;
if((data = fopen("/data/edu/params/lookuptable.dat", "rb")) == NULL)
{
printf("rsedu_vis(): ERROR opening lookupfile \n");
}
fread(matchLookup, sizeof(matchLookup), 1, data);
fclose(data);
}
}
//landmark detection
//------------
//1 cycle through image, convert each pixel to hsv, threshold by s and v to find colored balls, match h value to color-closest in "database" (use precomputed lookup), save weighted average pixellocation with database-lndmrkID
//2 reconstruct pose of camera (3D-position with yaw)
if((FEAT_POSVIS_RUN) && ((counter % 15) == 0) && (NULL != image)) //@pseudo4Hz
{
//reset landmark matching data
for(i = 0; i < lndmrk_nr; i++)
{
lndmrks[i].n = 0;
lndmrks[i].weights = 0;
lndmrks[i].px = 0;
lndmrks[i].py = 0;
}
int margin = (int)(filtersize - 1) / 4;
int weight = 1; // weight used to emphasize pixellocation that is close to existing estimate for sufficiently robust, identified landmark
//cycle through image
for(j = 0; j < ny - 2 * margin; j++)
{
for(i = 0; i < nx - 2 * margin; i++)
{
row = margin + j;
col = margin + i;
//Get YUV-values for pixel
yuv[0] = (float)image[nx * row + col].y1;
yuv[1] = (float)image[nx * row + col].u;
yuv[2] = (float)image[nx * row + col].v;
//find best matching landmark for current pixel
if(FEAT_NOLOOK)
{
printf("rsedu_vis(): ERROR Unfortunately, color conversion, etc. too slow onboard. Please set FEAT_NOLOOK = 0. \n");
}
else
//use pre-computed lookup table to find match
{
//printf("here uselu \n");
lndmrk_best = (int)matchLookup[(int)(yuv[0] / 2)][(int)(yuv[1] / 2)][(int)(yuv[2] / 2)];
}
//Store matched landmarks
//----
if(lndmrk_best > 0)
{
//increase weight if close to an existing, highly weighted cluster
if((lndmrks[lndmrk_best].weights > 15) && (abs((col + 1) - lndmrks[lndmrk_best].px) < 20) && (abs((row + 1) - lndmrks[lndmrk_best].py) < 20))
{
weight = 8;
}
else
{
weight = 1;
}
lndmrks[lndmrk_best].px = lndmrks[lndmrk_best].px * lndmrks[lndmrk_best].weights / (lndmrks[lndmrk_best].weights + weight) + (((double)(col + 1)) * 2) * weight / (lndmrks[lndmrk_best].weights + weight); //@TODO is it row?
lndmrks[lndmrk_best].py = lndmrks[lndmrk_best].py * lndmrks[lndmrk_best].weights / (lndmrks[lndmrk_best].weights + weight) + ((double)(row + 1)) * weight / (lndmrks[lndmrk_best].weights + weight); //@TODO is it row?
//save that one more pixel to this landmark
lndmrks[lndmrk_best].n += 1;
lndmrks[lndmrk_best].weights += weight;
//if (lndmrk_best==0) {matchResult[col][row]=255;} else {matchResult[col][row]=0;}
}
}//end inner image-for
}//end outer image-for
/*
printf("image_proc(): Result of matching: \n");
for (i=0;i<lndmrk_nr;i++)
{
printf("image_proc(): Lndmrk %i: nr pxls matched: %i, pixelcoordinates: (%f,%f) \n",i,lndmrks[i].n,lndmrks[i].px,lndmrks[i].py);
}
*/
//reconstruct pose
//-------------
// choose lndmrks with some minimum number of pixels, add them to the feature_pixelposition-matrix (ui,vi,1). TODO: remove hardcoded number of landmarks
int features_valid = 0;
int k;
for(k = 1; k < lndmrk_nr; k++) //start from 1 as yellow (landmark 0) not used!
{
if(lndmrks[k].n >= pxls_ftr_min)
{
features_valid += 1;
feature_pps[0][features_valid - 1] = lndmrks[k].px;
feature_pps[1][features_valid - 1] = lndmrks[k].py;
feature_pps[2][features_valid - 1] = 1.0;
}
}
if(features_valid == 4)
{
reconstructCameraPose(camerapos, &camerayaw, feature_pps, ldnmrk_pinv, intrMatrx_inv);
printf("rsedu_vis(): SUCCESS reconstructed camera pose: (%f, %f, %f,%f)\n", camerapos[0], camerapos[1], camerapos[2], camerayaw * 180 / 3.1415);
/*
* dump pose into FIFO to make available to controls code
*/
//compile data
vis_data[0] = camerapos[0];
vis_data[1] = camerapos[1];
vis_data[2] = camerapos[2];
vis_data[3] = camerayaw;
vis_fifo = open("/tmp/vis_fifo", O_WRONLY);
if(vis_fifo)
{
write(vis_fifo, (float*)(&vis_data), sizeof(vis_data));
close(vis_fifo);
}
}
else
{
printf("rsedu_vis(): WARNING not enough distinct markers (colored balls) found! \n") ;
}
}
//-----------
//STREAMING INSTRUCTIONS
//-----------
/* Enabling image streaming, copies the picture into a named FIFO. Picture can then be sent to a remote Ubuntu computer using standard commands:
Run this one-liner in a shell on the RollingSpider (open terminal, log onto drone via telnet 192.168.1.1) :
(remember to connect via the Bluetooth link, since pluging the USB cable deactivates the camera !!!)
while [ 1 ]; do cat /tmp/picture | nc 192.168.1.2 1234; done
Run these two commands in two different shells on the remote Ubuntu computer:
mkfifo /tmp/rollingspiderpicture ; while [ 1 ]; do nc -l 1234 > /tmp/rollingspiderpicture; done
mplayer -demuxer rawvideo -rawvideo w=160:h=120:format=yuy2 -loop 0 /tmp/rollingspiderpicture
*/
//stream image
//-----------
if((FEAT_IMSAVE == 2) && ((counter % 60) == 0) && (NULL != image)) //@pseudo1Hz
{
printf("image_proc(): Write image to fifo...\n");
mkfifo("/tmp/picture", 0777);
fifo = open("/tmp/picture", O_WRONLY);
if(fifo)
{
//char word = "asd";
//write(fifo,word,320*120);
write(fifo, buffer, 320 * 120);
close(fifo);
usleep(5000);
}
}
//save image
//-----------
if((FEAT_IMSAVE == 1) && ((counter % 6) == 0) && (NULL != image)) //@10Hz
{
FILE* data;
char filename[15];
//sprintf(filename,"/data/edu/imgs/img%i.bin",counter);
sprintf(filename, "/tmp/edu/imgs/img%i.bin", counter);
//sprintf(filename,"/tmp/imgs/img.bin");
//printf("image_proc(): img name: %s \n",filename);
mkdir("/tmp/edu", S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
mkdir("/tmp/edu/imgs", S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
if((data = fopen(filename, "wb")) == NULL)
{
printf("rsedu_vis(): ERROR opening img file\n");
}
fwrite(image, sizeof(pixel2_t) * 80 * 120, 1, data);
fclose(data);
usleep(5000);
}
usleep(4000);
//ptiming - store
//----------
ptimer_stopstore(FEAT_TIME, counter, start, ptfile);
//----------
}
