int gfx_draw_op(uarg_t arg, char *ptr) { int err=0; int l; int c = 8; /* 4 x uint16_t */ uint16_t *p = (uint16_t *)(char *)0x5e00; map_for_video(); l = ugetw(ptr); if (l < 6 || l > 512){ err = EINVAL; goto ret; } if (arg != GFXIOC_READ) c = l; if (uget(ptr + 2, (char *)0x5e00, c)){ err = EFAULT; goto ret; } switch(arg) { case GFXIOC_DRAW: /* TODO if (draw_validate(ptr, l, 256, 192)) - or 128! return EINVAL */ video_cmd( ( char *)0x5e00 ); break; case GFXIOC_WRITE: case GFXIOC_READ: if (l < 8){ err= EINVAL; break; } l -= 8; if (p[0] > 191 || p[1] > 31 || p[2] > 191 || p[3] > 31 || p[0] + p[2] > 192 || p[1] + p[3] > 32 || (p[2] * p[3]) > l) { err = -EFAULT; break; } if (arg == GFXIOC_READ) { video_read( (char *)0x5e00 ); if (uput( (char *)0x5e00 + 8, ptr+10, l-2)) err = EFAULT; break; } video_write( (char *)0x5e00 ); } ret: map_for_kernel(); return err; }
static void V4l_GetFrame(Capture * capBox, unsigned char ** pInput) { if (capBox->mmap) { if (xioctl(capBox->fd, VIDIOCSYNC, &capBox->grab_buf[capBox->curframe]) == -1) WARN("Syncing ioctl failed: %d\n", errno); *pInput = capBox->pmap + capBox->gb_buffers.offsets[capBox->curframe]; } else { int retval; while ((retval = video_read(capBox->fd, capBox->grab_data, capBox->imagesize)) == -1) if (errno != EAGAIN) break; if (retval == -1) WARN("Error occurred while reading from device: %s\n", strerror(errno)); *pInput = (unsigned char*) capBox->grab_data; } }
void application_mainloop(application_t *app) { DEBUG("application_mainloop()"); assert(app != 0); void *data; size_t length; float att[3], alt, spd, trk, brg, dst; char wpt[32]; float accsum[3]; float difftime; while(1) { // Process video if(!video_read(app->video, &data, &length)) { ERROR("Cannot read from video device"); break; } graphics_image_set_bitmap(app->image, data, length); graphics_draw(app->graphics, app->image, app->window_width / 2, app->window_height / 2, (float)app->window_width / (float)app->window_height < (float)app->video_width / (float)app->video_height ? (float)app->window_height / (float)app->video_height : (float)app->window_width / (float)app->video_width, 0); imu_get_attitude(app->imu, att); gps_get_pos(app->gps, NULL, NULL, &alt); imu_get_acceleration(app->imu, accsum, &difftime); gps_inertial_update(app->gps, accsum[0], accsum[1], accsum[2], difftime); // Draw landmarks void *iterator; float hangle, vangle, dist; drawable_t *label = gps_get_projection_label(app->gps, &hangle, &vangle, &dist, att, &iterator); while(iterator) { if((hangle > app->video_hfov / -2.0) && (hangle < app->video_hfov / 2.0) && (vangle > app->video_vfov / -2.0) && (vangle < app->video_vfov / 2.0) && (dist < app->visible_distance)) { INFO("Projecting landmark hangle = %f, vangle = %f, distance = %f", hangle, vangle, dist / 1000.0); uint32_t x = (float)app->window_width / 2 + (float)app->window_width * hangle / app->video_hfov; uint32_t y = (float)app->window_height / 2 + (float)app->window_height * vangle / app->video_vfov; graphics_draw(app->graphics, label, x, y, 1, 0); } label = gps_get_projection_label(app->gps, &hangle, &vangle, &dist, att, &iterator); } // Draw HUD overlay gps_get_track(app->gps, &spd, &trk); gps_get_route(app->gps, wpt, &dst, &brg); graphics_hud_draw(app->hud, att, spd, alt, trk, brg, dst, wpt); // Render to screen if(!graphics_flush(app->graphics, NULL)) { ERROR("Cannot draw"); break; } } }