static void main_loop()
{
fd_set rfds, wfds;
struct timeval tv;
while (1) {
FD_ZERO(&rfds);
FD_ZERO(&wfds);
#if UIM_XIM_USE_DELAY
tv.tv_sec = 1;
#else
tv.tv_sec = 2;
#endif
tv.tv_usec = 0;
std::map<int, fd_watch_struct>::iterator it;
int fd_max = 0;
for (it = fd_watch_stat.begin(); it != fd_watch_stat.end(); ++it) {
int fd = it->first;
if (it->second.mask & READ_OK)
FD_SET(fd, &rfds);
if (it->second.mask & WRITE_OK)
FD_SET(fd, &wfds);
if (fd_max < fd)
fd_max = fd;
}
if ((select(fd_max + 1, &rfds, &wfds, NULL, &tv)) == 0) {
check_pending_xevent();
#if UIM_XIM_USE_DELAY
timer_check();
#endif
continue;
}
it = fd_watch_stat.begin();
while (it != fd_watch_stat.end()) {
int fd = it->first;
if (FD_ISSET(fd, &rfds))
it->second.fn(fd, READ_OK);
if (FD_ISSET(fd, &wfds))
it->second.fn(fd, WRITE_OK);
// fd_watch_stat may be modified by above functions at
// this point. Since the behavior with incrementing
// invalidated iterator is compiler dependent, use safer
// way.
it = fd_watch_stat.find(fd);
if (it == fd_watch_stat.end()) // shouldn't happen
break;
++it;
}
#if UIM_XIM_USE_DELAY
timer_check();
#endif
}
}
static void ser_opto_keypad_cfg(int val)
{
int start_time;
GPIOB_ENABLE &=~ 0x80;
outl(inl(0x7000c104) | 0xc000000, 0x7000c104);
outl(val, 0x7000c120);
outl(inl(0x7000c100) | 0x80000000, 0x7000c100);
GPIOB_OUTPUT_VAL &=~ 0x10;
GPIOB_OUTPUT_EN |= 0x10;
start_time = USEC_TIMER;
do {
if ((inl(0x7000c104) & 0x80000000) == 0) {
break;
}
} while (timer_check(start_time, 1500) != 0);
outl(inl(0x7000c100) & ~0x80000000, 0x7000c100);
GPIOB_ENABLE |= 0x80;
GPIOB_OUTPUT_VAL |= 0x10;
GPIOB_OUTPUT_EN &=~0x10;
outl(inl(0x7000c104) | 0xc000000, 0x7000c104);
outl(inl(0x7000c100) | 0x60000000, 0x7000c100);
}
void unix_wait()
{
while (1) {
ticks next = timer_check();
select_timer_block(next);
}
}
static void
loopZ (void)
{
fd_set rfds, wfds;
struct timeval before, tv;
gettimeofday(&tv, 0);
for (;;) {
register int n, i;
if (timer_list) {
gettimeofday(&before, 0);
timer_check(&tv, &before);
if (timer_list)
tv = timer_list->tv;
}
rfds = hxd_rfds;
wfds = hxd_wfds;
n = select(high_fd + 1, &rfds, &wfds, 0, timer_list ? &tv : 0);
if (n < 0 && errno != EINTR) {
hxd_log("loopZ: select: %s", strerror(errno));
exit(1);
}
gettimeofday(&tv, 0);
if (hxd_cfg.operation.nospam)
loopZ_timeval = tv;
if (timer_list) {
timer_check(&before, &tv);
}
if (n <= 0)
continue;
for (i = 0; i < high_fd + 1; i++) {
if (FD_ISSET(i, &rfds) && FD_ISSET(i, &hxd_rfds)) {
if (hxd_files[i].ready_read)
hxd_files[i].ready_read(i);
if (!--n)
break;
}
if (FD_ISSET(i, &wfds) && FD_ISSET(i, &hxd_wfds)) {
if (hxd_files[i].ready_write)
hxd_files[i].ready_write(i);
if (!--n)
break;
}
}
}
}
static void timer_callback(registers_t* regs) {
tick++;
if (tick % 1000 == 0){
//kprintf("Tick = %d \n", tick);
//timer_check();
}
timer_check();
}
double timer_check_slang(int *t_handle)
{
chrono_t *t;
t = (chrono_t *) table_get_data(chrono_table, *t_handle);
return timer_check(t);
}
/* wait for LCD with timeout */
static void lcd_wait_write(void)
{
if ((inl(lcd_base) & lcd_busy_mask) != 0) {
int start = timer_get_current();
do {
if ((inl(lcd_base) & lcd_busy_mask) == 0) break;
} while (timer_check(start, 1000) == 0);
}
}
/* wait for LCD with timeout */
static void
lcd_wait_write(void)
{
if ( (inl(IPOD_LCD_BASE) & 0x1) != 0 ) {
int start = timer_get_current();
do {
if ( (inl(IPOD_LCD_BASE) & (unsigned int)0x8000) == 0 ) break;
} while ( timer_check(start, 1000) == 0 );
}
}
static void
usb_hub_port_setup(void *data)
{
struct usbdevice_s *usbdev = data;
struct usbhub_s *hub = usbdev->hub;
u32 port = usbdev->port;
for (;;) {
// Detect if device present (and possibly start reset)
int ret = hub->op->detect(hub, port);
if (ret > 0)
// Device connected.
break;
if (ret < 0 || timer_check(hub->detectend))
// No device found.
goto done;
msleep(5);
}
// XXX - wait USB_TIME_ATTDB time?
// Reset port and determine device speed
mutex_lock(&hub->cntl->resetlock);
int ret = hub->op->reset(hub, port);
if (ret < 0)
// Reset failed
goto resetfail;
usbdev->speed = ret;
// Set address of port
ret = usb_set_address(usbdev);
if (ret) {
hub->op->disconnect(hub, port);
goto resetfail;
}
mutex_unlock(&hub->cntl->resetlock);
// Configure the device
int count = configure_usb_device(usbdev);
usb_free_pipe(usbdev, usbdev->defpipe);
if (!count)
hub->op->disconnect(hub, port);
hub->devcount += count;
done:
hub->threads--;
free(usbdev);
return;
resetfail:
mutex_unlock(&hub->cntl->resetlock);
goto done;
}
// Reset a drive
static void
ata_reset(struct atadrive_s *adrive_gf)
{
struct ata_channel_s *chan_gf = GET_GLOBALFLAT(adrive_gf->chan_gf);
u8 slave = GET_GLOBALFLAT(adrive_gf->slave);
u16 iobase1 = GET_GLOBALFLAT(chan_gf->iobase1);
u16 iobase2 = GET_GLOBALFLAT(chan_gf->iobase2);
dprintf(6, "ata_reset drive=%p\n", &adrive_gf->drive);
// Pulse SRST
outb(ATA_CB_DC_HD15 | ATA_CB_DC_NIEN | ATA_CB_DC_SRST, iobase2+ATA_CB_DC);
udelay(5);
outb(ATA_CB_DC_HD15 | ATA_CB_DC_NIEN, iobase2+ATA_CB_DC);
msleep(2);
// wait for device to become not busy.
int status = await_not_bsy(iobase1);
if (status < 0)
goto done;
if (slave) {
// Change device.
u32 end = timer_calc(IDE_TIMEOUT);
for (;;) {
outb(ATA_CB_DH_DEV1, iobase1 + ATA_CB_DH);
status = ndelay_await_not_bsy(iobase1);
if (status < 0)
goto done;
if (inb(iobase1 + ATA_CB_DH) == ATA_CB_DH_DEV1)
break;
// Change drive request failed to take effect - retry.
if (timer_check(end)) {
warn_timeout();
goto done;
}
}
} else {
// QEMU doesn't reset dh on reset, so set it explicitly.
outb(ATA_CB_DH_DEV0, iobase1 + ATA_CB_DH);
}
// On a user-reset request, wait for RDY if it is an ATA device.
u8 type=GET_GLOBALFLAT(adrive_gf->drive.type);
if (type == DTYPE_ATA)
status = await_rdy(iobase1);
done:
// Enable interrupts
outb(ATA_CB_DC_HD15, iobase2+ATA_CB_DC);
dprintf(6, "ata_reset exit status=%x\n", status);
}
void evquick_loop(void)
{
int pollret, i;
for(;;) {
if (giveup)
break;
if (ctx->changed) {
rebuild_poll();
continue;
}
if (ctx->pfd == NULL) {
sleep(3600);
ctx->changed = 1;
continue;
}
pollret = poll(ctx->pfd, (unsigned)ctx->n_events, 3600 * 1000);
if (pollret <= 0)
continue;
if ((ctx->pfd[0].revents & POLLIN) == POLLIN) {
char discard;
if(read(ctx->time_machine[0], &discard, 1) < 0)
printf("libevquick: problem with read!\n");
timer_check();
continue;
}
if (ctx->n_events < 2)
continue;
for (i = ctx->last_served +1; i < ctx->n_events; i++) {
if (ctx->pfd[i].revents != 0) {
serve_event(i);
goto end_loop;
}
}
for (i = 1; i <= ctx->last_served; i++) {
if (ctx->pfd[i].revents != 0) {
serve_event(i);
goto end_loop;
}
}
end_loop:
continue;
} /* main loop */
}
// Wait for the specified ide state
static inline int
await_ide(u8 mask, u8 flags, u16 base, u16 timeout)
{
u32 end = timer_calc(timeout);
for (;;) {
u8 status = inb(base+ATA_CB_STAT);
if ((status & mask) == flags)
return status;
if (timer_check(end)) {
warn_timeout();
return -1;
}
yield();
}
}
// Check if a SCSI device is ready to receive commands
int
scsi_is_ready(struct disk_op_s *op)
{
dprintf(6, "scsi_is_ready (drive=%p)\n", op->drive_gf);
/* Retry TEST UNIT READY for 5 seconds unless MEDIUM NOT PRESENT is
* reported by the device. If the device reports "IN PROGRESS",
* 30 seconds is added. */
int in_progress = 0;
u32 end = timer_calc(5000);
for (;;) {
if (timer_check(end)) {
dprintf(1, "test unit ready failed\n");
return -1;
}
int ret = cdb_test_unit_ready(op);
if (!ret)
// Success
break;
struct cdbres_request_sense sense;
ret = cdb_get_sense(op, &sense);
if (ret)
// Error - retry.
continue;
// Sense succeeded.
if (sense.asc == 0x3a) { /* MEDIUM NOT PRESENT */
dprintf(1, "Device reports MEDIUM NOT PRESENT\n");
return -1;
}
if (sense.asc == 0x04 && sense.ascq == 0x01 && !in_progress) {
/* IN PROGRESS OF BECOMING READY */
dprintf(1, "Waiting for device to detect medium... ");
/* Allow 30 seconds more */
end = timer_calc(30000);
in_progress = 1;
}
}
return 0;
}
int opto_keypad_read(void)
{
int loop_cnt, had_io = 0;
for (loop_cnt = 5; loop_cnt != 0;)
{
int key_pressed = 0;
int start_time;
unsigned int key_pad_val;
ser_opto_keypad_cfg(0x8000023a);
start_time = USEC_TIMER;
do {
if (inl(0x7000c104) & 0x4000000) {
had_io = 1;
break;
}
if (had_io != 0) {
break;
}
} while (timer_check(start_time, 1500) != 0);
key_pad_val = inl(0x7000c140);
if ((key_pad_val & ~0x7fff0000) != 0x8000023a) {
loop_cnt--;
} else {
key_pad_val = (key_pad_val << 11) >> 27;
key_pressed = 1;
}
outl(inl(0x7000c100) | 0x60000000, 0x7000c100);
outl(inl(0x7000c104) | 0xc000000, 0x7000c104);
if (key_pressed != 0) {
return key_pad_val ^ 0x1f;
}
}
return 0;
}
//Documentation in header file
uint8_t dhcp_requestIP(uint16_t timeout_ms)
{
//Get a timer
int8_t timer = timer_get();
timer_set(timer,timeout_ms);
//Send dhcp discover packet
dhcp_sendDiscover();
do
{
//Wait for a packet to arrive
//If no packets arrive in time, return 0
while(enc28j60Read(EPKTCNT) == 0)
if(timer_check(timer) == TIMER_EXPIRED)
{
//Release the timer
timer_release(timer);
return 0;
}
//Receive packet
ns.plength = enc28j60PacketReceive(ns.pbuf, ns.pbuf_size);
//Check if packet is DHCP!
if(ns.plength > 0)
if(getPacketType() == DHCP)
dhcp_handlePacket();
}
while(ns.ipAcquired == 0);
//Release the timer
timer_release(timer);
//Return success
return ns.ipAcquired;
}
/*
* Old, quicksort function.
*/
void quicksort_population(population *pop)
{
int k; /* Loop variable. */
int first=0, last=pop->size-1; /* Indices into population. */
entity **array_of_ptrs=pop->entity_iarray;
boolean done=FALSE; /* Whether shuffle sort is complete. */
plog(LOG_VERBOSE, "Sorting population with %d members.", pop->size);
#ifdef GA_QSORT_TIME
timer_start();
#endif
#if GA_QSORT_DEBUG>2
printf("Unsorted:\n");
for (i=0; i<pop->size; i++)
printf("%6d: %f\n", i, pop->entity_iarray[i]->fitness);
#endif
/*
* Do the actual quicksort.
* But don't bother if the array is really small.
*/
if (last > 7) qksort_population(array_of_ptrs, 0, last);
#if GA_QSORT_DEBUG>2
printf("Almost sorted:\n");
for (i=0; i<pop->size; i++)
printf("%6d: %f\n", i, pop->entity_iarray[i]->fitness);
#endif
/*
* A bi-directional bubble sort (actually called shuffle sort, apparently)
* to complete the sort.
* NB/ Could optimise more by moving this into the qksort_population()
* function, thus avoiding many unnecessary comparisons.
*/
/*
for (k = 0 ; k < pop->size ; k++)
printf("-- rank %d id %d fitness %f.\n", k, ga_get_entity_id_from_rank(pop, k), array_of_ptrs[k]->fitness);
*/
/* for (i=0;i<3;i++) */
while (done == FALSE)
{
for (k = first ; k < last ; k++)
{
if ( array_of_ptrs[k]->fitness < array_of_ptrs[k+1]->fitness )
{
swap_e(array_of_ptrs[k], array_of_ptrs[k+1]);
}
}
last--; /* The last one *MUST* be correct now. */
done = TRUE;
for (k = last ; k > first ; k--)
{
if ( array_of_ptrs[k]->fitness > array_of_ptrs[k-1]->fitness )
{
swap_e(array_of_ptrs[k], array_of_ptrs[k-1]);
done = FALSE;
}
}
first++; /* The first one *MUST* be correct now. */
}
#if GA_QSORT_DEBUG>1
/* Check that the population is correctly sorted. */
printf("rank 0 id %d fitness %f.\n", ga_get_entity_id_from_rank(pop, 0), array_of_ptrs[0]->fitness);
for (k = 1 ; k < pop->size ; k++)
{
printf("rank %d id %d fitness %f.\n", k, ga_get_entity_id_from_rank(pop, k), array_of_ptrs[k]->fitness);
if ( array_of_ptrs[k-1]->fitness < array_of_ptrs[k]->fitness )
{
plog(LOG_WARNING, "Population is incorrectly ordered.");
}
}
#endif
#ifdef GA_QSORT_TIME
timer_check();
#endif
return;
}
/*
* New, shuffle sort function.
* Fairly efficient when much of the population is already in order.
*/
void sort_population(population *pop)
{
int k; /* Loop variable. */
int first=0, last=pop->size-1; /* Indices into population. */
entity **array_of_ptrs=pop->entity_iarray;
boolean done=TRUE; /* Whether shuffle sort is complete. */
plog(LOG_VERBOSE, "Sorting population with %d members.", pop->size);
#ifdef GA_QSORT_TIME
timer_start();
#endif
if (pop->rank == ga_rank_fitness)
{
/*
* This optimised code for the typical fitness ranking method.
* It avoids a function call per comparision that is required in the
* general case.
*/
/*
* A bi-directional bubble sort (actually called shuffle sort, apparently)
* algorithm. We stop when the first pop->stable_size entities are
* definitely sorted.
* There's an extra bubble-up at the start.
*/
/*
for (k = 0 ; k < pop->size ; k++)
printf("-- rank %d id %d fitness %f.\n", k, ga_get_entity_id_from_rank(pop, k), array_of_ptrs[k]->fitness);
*/
for (k = last ; k > first ; k--)
{
if ( array_of_ptrs[k]->fitness > array_of_ptrs[k-1]->fitness )
{
swap_e(array_of_ptrs[k], array_of_ptrs[k-1]);
done = FALSE;
}
}
first++; /* The first one *MUST* be correct now. */
while (done == FALSE && first <= pop->stable_size && first < last)
{
for (k = last ; k > first ; k--)
{
if ( array_of_ptrs[k]->fitness > array_of_ptrs[k-1]->fitness )
{
swap_e(array_of_ptrs[k], array_of_ptrs[k-1]);
}
}
first++; /* The first one *MUST* be correct now. */
done = TRUE;
for (k = first ; k < last ; k++)
{
if ( array_of_ptrs[k]->fitness < array_of_ptrs[k+1]->fitness )
{
swap_e(array_of_ptrs[k], array_of_ptrs[k+1]);
done = FALSE;
}
}
last--; /* The last one *MUST* be correct now. */
}
}
else
{
/*
* A bi-directional bubble sort (actually called shuffle sort, apparently)
* algorithm. We stop when the first pop->stable_size entities are
* definitely sorted.
* There's an extra bubble-up at the start.
*/
/*
for (k = 0 ; k < pop->size ; k++)
printf("-- rank %d id %d fitness %f.\n", k, ga_get_entity_id_from_rank(pop, k), array_of_ptrs[k]->fitness);
*/
for (k = last ; k > first ; k--)
{
if ( pop->rank(pop, array_of_ptrs[k], pop, array_of_ptrs[k-1]) > 0 )
{
swap_e(array_of_ptrs[k], array_of_ptrs[k-1]);
done = FALSE;
}
}
first++; /* The first one *MUST* be correct now. */
while (done == FALSE && first <= pop->stable_size && first < last)
{
for (k = last ; k > first ; k--)
{
if ( pop->rank(pop, array_of_ptrs[k], pop, array_of_ptrs[k-1]) > 0 )
{
swap_e(array_of_ptrs[k], array_of_ptrs[k-1]);
}
}
first++; /* The first one *MUST* be correct now. */
done = TRUE;
for (k = first ; k < last ; k++)
{
if ( pop->rank(pop, array_of_ptrs[k], pop, array_of_ptrs[k+1]) < 0 )
{
swap_e(array_of_ptrs[k], array_of_ptrs[k+1]);
done = FALSE;
}
}
last--; /* The last one *MUST* be correct now. */
}
}
#if GA_QSORT_DEBUG>1
/* Check that the population is correctly sorted. */
printf("rank 0 id %d fitness %f.\n", ga_get_entity_id_from_rank(pop, 0), array_of_ptrs[0]->fitness);
for (k = 1 ; k < pop->stable_size ; k++)
{
printf("rank %d id %d fitness %f.\n", k, ga_get_entity_id_from_rank(pop, k), array_of_ptrs[k]->fitness);
if ( pop->rank(pop, array_of_ptrs[k-1]->fitness, pop, array_of_ptrs[k]->fitness) < 0 )
{
plog(LOG_WARNING, "Population is incorrectly ordered.");
}
}
#endif
#ifdef GA_QSORT_TIME
timer_check();
#endif
return;
}
/**
* @brief Main function.
*/
int main(void)
{
__enable_irq();
snapshot_buffer = BuildCameraImageBuffer(snapshot_buffer_mem);
/* load settings and parameters */
global_data_reset_param_defaults();
global_data_reset();
/* init led */
LEDInit(LED_ACT);
LEDInit(LED_COM);
LEDInit(LED_ERR);
LEDOff(LED_ACT);
LEDOff(LED_COM);
LEDOff(LED_ERR);
/* enable FPU on Cortex-M4F core */
SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */
/* init timers */
timer_init();
/* init usb */
USBD_Init( &USB_OTG_dev,
USB_OTG_FS_CORE_ID,
&USR_desc,
&USBD_CDC_cb,
&USR_cb);
/* init mavlink */
communication_init();
/* initialize camera: */
img_stream_param.size.x = FLOW_IMAGE_SIZE;
img_stream_param.size.y = FLOW_IMAGE_SIZE;
img_stream_param.binning = 4;
{
camera_image_buffer buffers[5] = {
BuildCameraImageBuffer(image_buffer_8bit_1),
BuildCameraImageBuffer(image_buffer_8bit_2),
BuildCameraImageBuffer(image_buffer_8bit_3),
BuildCameraImageBuffer(image_buffer_8bit_4),
BuildCameraImageBuffer(image_buffer_8bit_5)
};
camera_init(&cam_ctx, mt9v034_get_sensor_interface(), dcmi_get_transport_interface(),
mt9v034_get_clks_per_row(64, 4) * 1, mt9v034_get_clks_per_row(64, 4) * 64, 2.0,
&img_stream_param, buffers, 5);
}
/* gyro config */
gyro_config();
/* usart config*/
usart_init();
/* i2c config*/
i2c_init();
/* sonar config*/
float sonar_distance_filtered = 0.0f; // distance in meter
float sonar_distance_raw = 0.0f; // distance in meter
bool distance_valid = false;
sonar_config();
/* reset/start timers */
timer_register(sonar_update_fn, SONAR_POLL_MS);
timer_register(system_state_send_fn, SYSTEM_STATE_MS);
timer_register(system_receive_fn, SYSTEM_STATE_MS / 2);
timer_register(send_params_fn, PARAMS_MS);
timer_register(send_video_fn, global_data.param[PARAM_VIDEO_RATE]);
timer_register(take_snapshot_fn, 500);
//timer_register(switch_params_fn, 2000);
/* variables */
uint32_t counter = 0;
result_accumulator_ctx mavlink_accumulator;
result_accumulator_init(&mavlink_accumulator);
uint32_t fps_timing_start = get_boot_time_us();
uint16_t fps_counter = 0;
uint16_t fps_skipped_counter = 0;
uint32_t last_frame_index = 0;
/* main loop */
while (1)
{
/* check timers */
timer_check();
if (snap_capture_done) {
snap_capture_done = false;
camera_snapshot_acknowledge(&cam_ctx);
snap_ready = true;
if (snap_capture_success) {
/* send the snapshot! */
LEDToggle(LED_COM);
mavlink_send_image(&snapshot_buffer);
}
}
/* calculate focal_length in pixel */
const float focal_length_px = (global_data.param[PARAM_FOCAL_LENGTH_MM]) / (4.0f * 0.006f); //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled
/* new gyroscope data */
float x_rate_sensor, y_rate_sensor, z_rate_sensor;
int16_t gyro_temp;
gyro_read(&x_rate_sensor, &y_rate_sensor, &z_rate_sensor,&gyro_temp);
/* gyroscope coordinate transformation to flow sensor coordinates */
float x_rate = y_rate_sensor; // change x and y rates
float y_rate = - x_rate_sensor;
float z_rate = z_rate_sensor; // z is correct
/* get sonar data */
distance_valid = sonar_read(&sonar_distance_filtered, &sonar_distance_raw);
/* reset to zero for invalid distances */
if (!distance_valid) {
sonar_distance_filtered = 0.0f;
sonar_distance_raw = 0.0f;
}
bool use_klt = global_data.param[PARAM_ALGORITHM_CHOICE] != 0;
uint32_t start_computations = 0;
/* get recent images */
camera_image_buffer *frames[2];
camera_img_stream_get_buffers(&cam_ctx, frames, 2, true);
start_computations = get_boot_time_us();
int frame_delta = ((int32_t)frames[0]->frame_number - (int32_t)last_frame_index);
last_frame_index = frames[0]->frame_number;
fps_skipped_counter += frame_delta - 1;
flow_klt_image *klt_images[2] = {NULL, NULL};
{
/* make sure that the new images get the correct treatment */
/* this algorithm will still work if both images are new */
int i;
bool used_klt_image[2] = {false, false};
for (i = 0; i < 2; ++i) {
if (frames[i]->frame_number != frames[i]->meta) {
// the image is new. apply pre-processing:
/* filter the new image */
if (global_data.param[PARAM_ALGORITHM_IMAGE_FILTER]) {
filter_image(frames[i]->buffer, frames[i]->param.p.size.x);
}
/* update meta data to mark it as an up-to date image: */
frames[i]->meta = frames[i]->frame_number;
} else {
// the image has the preprocessing already applied.
if (use_klt) {
int j;
/* find the klt image that matches: */
for (j = 0; j < 2; ++j) {
if (flow_klt_images[j].meta == frames[i]->frame_number) {
used_klt_image[j] = true;
klt_images[i] = &flow_klt_images[j];
}
}
}
}
}
if (use_klt) {
/* only for KLT: */
/* preprocess the images if they are not yet preprocessed */
for (i = 0; i < 2; ++i) {
if (klt_images[i] == NULL) {
// need processing. find unused KLT image:
int j;
for (j = 0; j < 2; ++j) {
if (!used_klt_image[j]) {
used_klt_image[j] = true;
klt_images[i] = &flow_klt_images[j];
break;
}
}
klt_preprocess_image(frames[i]->buffer, klt_images[i]);
}
}
}
}
float frame_dt = (frames[0]->timestamp - frames[1]->timestamp) * 0.000001f;
/* compute gyro rate in pixels and change to image coordinates */
float x_rate_px = - y_rate * (focal_length_px * frame_dt);
float y_rate_px = x_rate * (focal_length_px * frame_dt);
float z_rate_fr = - z_rate * frame_dt;
/* compute optical flow in pixels */
flow_raw_result flow_rslt[32];
uint16_t flow_rslt_count = 0;
if (!use_klt) {
flow_rslt_count = compute_flow(frames[1]->buffer, frames[0]->buffer, x_rate_px, y_rate_px, z_rate_fr, flow_rslt, 32);
} else {
flow_rslt_count = compute_klt(klt_images[1], klt_images[0], x_rate_px, y_rate_px, z_rate_fr, flow_rslt, 32);
}
/* calculate flow value from the raw results */
float pixel_flow_x;
float pixel_flow_y;
float outlier_threshold = global_data.param[PARAM_ALGORITHM_OUTLIER_THR_RATIO];
float min_outlier_threshold = 0;
if(global_data.param[PARAM_ALGORITHM_CHOICE] == 0)
{
min_outlier_threshold = global_data.param[PARAM_ALGORITHM_OUTLIER_THR_BLOCK];
}else
{
min_outlier_threshold = global_data.param[PARAM_ALGORITHM_OUTLIER_THR_KLT];
}
uint8_t qual = flow_extract_result(flow_rslt, flow_rslt_count, &pixel_flow_x, &pixel_flow_y,
outlier_threshold, min_outlier_threshold);
/* create flow image if needed (previous_image is not needed anymore)
* -> can be used for debugging purpose
*/
previous_image = frames[1];
if (global_data.param[PARAM_USB_SEND_VIDEO])
{
uint16_t frame_size = global_data.param[PARAM_IMAGE_WIDTH];
uint8_t *prev_img = previous_image->buffer;
for (int i = 0; i < flow_rslt_count; i++) {
if (flow_rslt[i].quality > 0) {
prev_img[flow_rslt[i].at_y * frame_size + flow_rslt[i].at_x] = 255;
int ofs = (int)floor(flow_rslt[i].at_y + flow_rslt[i].y * 2 + 0.5f) * frame_size + (int)floor(flow_rslt[i].at_x + flow_rslt[i].x * 2 + 0.5f);
if (ofs >= 0 && ofs < frame_size * frame_size) {
prev_img[ofs] = 200;
}
}
}
}
/* return the image buffers */
camera_img_stream_return_buffers(&cam_ctx, frames, 2);
/* decide which distance to use */
float ground_distance = 0.0f;
if(global_data.param[PARAM_SONAR_FILTERED])
{
ground_distance = sonar_distance_filtered;
}
else
{
ground_distance = sonar_distance_raw;
}
/* update I2C transmit buffer */
update_TX_buffer(frame_dt,
x_rate, y_rate, z_rate, gyro_temp,
qual, pixel_flow_x, pixel_flow_y, 1.0f / focal_length_px,
distance_valid, ground_distance, get_time_delta_us(get_sonar_measure_time()));
/* accumulate the results */
result_accumulator_feed(&mavlink_accumulator, frame_dt,
x_rate, y_rate, z_rate, gyro_temp,
qual, pixel_flow_x, pixel_flow_y, 1.0f / focal_length_px,
distance_valid, ground_distance, get_time_delta_us(get_sonar_measure_time()));
uint32_t computaiton_time_us = get_time_delta_us(start_computations);
counter++;
fps_counter++;
/* serial mavlink + usb mavlink output throttled */
if (counter % (uint32_t)global_data.param[PARAM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor
{
float fps = 0;
float fps_skip = 0;
if (fps_counter + fps_skipped_counter > 100) {
uint32_t dt = get_time_delta_us(fps_timing_start);
fps_timing_start += dt;
fps = (float)fps_counter / ((float)dt * 1e-6f);
fps_skip = (float)fps_skipped_counter / ((float)dt * 1e-6f);
fps_counter = 0;
fps_skipped_counter = 0;
mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "TIMING", get_boot_time_us(), computaiton_time_us, fps, fps_skip);
}
mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "EXPOSURE", get_boot_time_us(),
frames[0]->param.exposure, frames[0]->param.analog_gain, cam_ctx.last_brightness);
/* calculate the output values */
result_accumulator_output_flow output_flow;
result_accumulator_output_flow_rad output_flow_rad;
int min_valid_ratio = global_data.param[PARAM_ALGORITHM_MIN_VALID_RATIO];
result_accumulator_calculate_output_flow(&mavlink_accumulator, min_valid_ratio, &output_flow);
result_accumulator_calculate_output_flow_rad(&mavlink_accumulator, min_valid_ratio, &output_flow_rad);
// send flow
mavlink_msg_optical_flow_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
output_flow.flow_x, output_flow.flow_y,
output_flow.flow_comp_m_x, output_flow.flow_comp_m_y,
output_flow.quality, output_flow.ground_distance);
mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_0, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
output_flow_rad.integration_time,
output_flow_rad.integrated_x, output_flow_rad.integrated_y,
output_flow_rad.integrated_xgyro, output_flow_rad.integrated_ygyro, output_flow_rad.integrated_zgyro,
output_flow_rad.temperature, output_flow_rad.quality,
output_flow_rad.time_delta_distance_us,output_flow_rad.ground_distance);
if (global_data.param[PARAM_USB_SEND_FLOW] && (output_flow.quality > 0 || global_data.param[PARAM_USB_SEND_QUAL_0]))
{
mavlink_msg_optical_flow_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
output_flow.flow_x, output_flow.flow_y,
output_flow.flow_comp_m_x, output_flow.flow_comp_m_y,
output_flow.quality, output_flow.ground_distance);
mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_2, get_boot_time_us(), global_data.param[PARAM_SENSOR_ID],
output_flow_rad.integration_time,
output_flow_rad.integrated_x, output_flow_rad.integrated_y,
output_flow_rad.integrated_xgyro, output_flow_rad.integrated_ygyro, output_flow_rad.integrated_zgyro,
output_flow_rad.temperature, output_flow_rad.quality,
output_flow_rad.time_delta_distance_us,output_flow_rad.ground_distance);
}
if(global_data.param[PARAM_USB_SEND_GYRO])
{
mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "GYRO", get_boot_time_us(), x_rate, y_rate, z_rate);
}
result_accumulator_reset(&mavlink_accumulator);
}
/* forward flow from other sensors */
if (counter % 2)
{
communication_receive_forward();
}
}
}
bool mqtt_process(MQTT* mqtt) {
if(mqtt->length < sizeof(MQTT_VHeader)) {
return false;
}
MQTT_VHeader* vheader= (MQTT_VHeader*)mqtt->body;
ssize_t len = mqtt->length - (sizeof(MQTT_VHeader) + vheader->topic_length);
char buf[256] = {0,};
if(len < 0 || len > sizeof(buf)) {
return false;
}
memcpy(buf, (char*)vheader->topic + vheader->topic_length, len);
json_object* jso = json_tokener_parse(buf);
if(jso) {
Sensor* sensor = NULL;
char name[64] = {0, };
json_object_object_foreach(jso, key, child_object) {
if(!strcmp(key, "name_of_sensor")) {
strcpy(name, json_object_to_json_string(child_object));
printf("name %s\n", name);
remove_blank(name);
sensor = sensor_database_get(name);
}
#ifdef TIMER_LOG
if(!strcmp(key, "id_of_sensor")) {
char id[64] = {0, };
strcpy(id, json_object_to_json_string(child_object));
remove_blank(id);
extern void timer_check(char* id);
timer_check(id);
}
#endif
}
if(sensor) {
json_object_object_foreach(jso, key1, child_object1) {
char name[64] = {0, };
strcpy(name, key1);
remove_blank(name);
Data* data = sensor_get_data(sensor, name);
if(data) {
char value[64] = {0, };
strcpy(value, json_object_to_json_string(child_object1));
remove_blank(value);
char* ptr;
int64_t val = strtol(value, &ptr, 10);
#ifdef DEBUG_MSG
printf("\t%s: %ld\n", name, val);
#endif
data_push_value(data, val);
}
}
}
#ifdef DEBUG_MSG
printf("\n");
#endif
json_object_put(jso); //free
}
/* Usage: a.out [port]. If no port is specified, a default port is used.
*/
int main(int argc, char *argv[]){
int bind_port = argc > 1 ? atoi(argv[1]) : 0;
int i;
/* Read from standard input.
*/
struct file_info *input = file_info_add(FI_FILE, 0, stdin_handler, POLLIN);
/* Create a TCP socket.
*/
int skt;
// YOUR CODE HERE
skt = socket(AF_INET, SOCK_STREAM, 0);
report_error(skt, "socket", errno);
/* Make the socket non-blocking.
*/
// YOUR CODE HERE
int res = fcntl(skt, F_SETFL, O_NONBLOCK);
report_error(res, "fcntl", errno);
int optval = 1;
res = setsockopt(skt, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
report_error(res, "setsocketopt", errno);
/* Initialize addr in as far as possible.
*/
// YOUR CODE HERE
struct sockaddr_in sock_addr;
sock_addr.sin_family = AF_INET;
sock_addr.sin_port = bind_port;
/* Bind the socket.
*/
// YOUR CODE HERE
res = bind(skt, (struct sockaddr *) &sock_addr, sizeof(struct sockaddr));
report_error(res, "bind", errno);
/* Keep track of the socket.
*/
file_info_add(FI_SERVER, skt, server_handler, POLLIN);
/* Get my address.
*/
struct sockaddr_in addr;
socklen_t len = sizeof(addr);
if (getsockname(skt, (struct sockaddr *) &addr, &len) < 0) {
perror("getsocknane");
}
/* Get my IP addresses.
*/
struct ifaddrs *addr_list, *ifa;
if (getifaddrs(&addr_list) < 0) {
perror("getifaddrs");
return 1;
}
for (ifa = addr_list; ifa != 0; ifa = ifa->ifa_next) {
if (ifa->ifa_addr != 0 && ifa->ifa_addr->sa_family == AF_INET &&
!(ifa->ifa_flags & IFF_LOOPBACK)) {
struct sockaddr_in *si = (struct sockaddr_in *) ifa->ifa_addr;
printf("%s: %s:%d\n", ifa->ifa_name, inet_ntoa(si->sin_addr), ntohs(addr.sin_port));
my_addr = *si;
my_addr.sin_port = addr.sin_port;
}
}
freeifaddrs(addr_list);
/* Pretend standard input is a peer...
*/
input->addr = my_addr;
input->status = FI_KNOWN;
/* Listen on the socket.
*/
if (listen(skt, 5) < 0) {
perror("listen");
return 1;
}
// printf("server socket = %d\n", skt);
printf("> "); fflush(stdout);
for (;;) {
/* Handle expired timers first.
*/
int timeout = timer_check();
/* Prepare poll.
*/
struct pollfd *fds = calloc(nfiles, sizeof(*fds));
struct file_info *fi, **fi_index = calloc(nfiles, sizeof(*fi_index));
int i;
for (i = 0, fi = file_info; fi != 0; fi = fi->next) {
if (fi->type != FI_FREE && fi->fd >= 0) {
fds[i].fd = fi->fd;
fds[i].events = fi->events;
if (fi->amount_to_send > 0) {
fds[i].events |= POLLOUT;
}
fi_index[i++] = fi;
}
}
int n = i; // n may be less than nfiles
if (poll(fds, n, timeout) < 0) {
perror("poll");
return 1;
}
/* See if there's activity on any of the files/sockets.
*/
for (i = 0; i < n; i++) {
if (fds[i].revents != 0 && fi_index[i]->type != FI_FREE) {
(*fi_index[i]->handler)(fi_index[i], fds[i].revents);
}
}
free(fds);
free(fi_index);
}
return 0;
}
