int main(int argc, char** argv) {
PORT_USERDATA userdata;
MMAL_STATUS_T status;
cairo_surface_t *surface,*surface2;
cairo_t *context,*context2;
int i;
int duration = 1;
int valid = 1;
char text[256];
strcpy (text,"Camera 1");
for (i = 1; i < argc && valid; i++)
{
if (strcmp(argv[i], "-s")==0) {
strcpy(text, argv[i + 1]);
i++;
}
if (strcmp(argv[i], "-t")==0) {
sscanf(argv[i + 1], "%u", &duration);
i++;
}
if (strcmp(argv[i], "-d")==0) {
}
}
//fprintf(stderr, "CAPTION : %s\n", text);
duration = 3;
memset(&userdata, 0, sizeof (PORT_USERDATA));
userdata.width = VIDEO_WIDTH;
userdata.height = VIDEO_HEIGHT;
userdata.fps = 0.0;
//fprintf(stderr, "VIDEO_WIDTH : %i\n", userdata.width );
//fprintf(stderr, "VIDEO_HEIGHT: %i\n", userdata.height );
//fprintf(stderr, "VIDEO_FPS : %i\n", VIDEO_FPS);
//fprintf(stderr, "Running...\n");
bcm_host_init();
surface = cairo_image_surface_create(CAIRO_FORMAT_ARGB32, 600, 100);
context = cairo_create(surface);
cairo_rectangle(context, 0.0, 0.0, 600, 100);
cairo_set_source_rgba(context, 0.0, 0.0, 0.0, 1.0);
cairo_fill(context);
userdata.overlay_buffer = cairo_image_surface_get_data(surface);
userdata.overlay = 1;
surface2 = cairo_image_surface_create(CAIRO_FORMAT_ARGB32, 600, 100);
context2 = cairo_create(surface2);
cairo_rectangle(context2, 0.0, 0.0, 600, 100);
cairo_set_source_rgba(context2, 0.0, 0.0, 0.0, 1.0);
cairo_fill(context2);
userdata.overlay_buffer2 = cairo_image_surface_get_data(surface2);
if (1 && setup_camera(&userdata) != 0) {
fprintf(stderr, "Error: setup camera %x\n", status);
return -1;
}
if (1 && setup_encoder(&userdata) != 0) {
fprintf(stderr, "Error: setup encoder %x\n", status);
return -1;
}
if (1 && setup_preview(&userdata) != 0) {
fprintf(stderr, "Error: setup preview %x\n", status);
return -1;
}
while (duration >0) {
//Update Draw to unused buffer that way there is no flickering of the overlay text if the overlay update rate
//and video FPS are not the same
if (userdata.overlay == 1) {
cairo_rectangle(context, 0.0, 0.0, 600, 100);
cairo_set_source_rgba(context, 0.0, 0.0, 0.0, 1.0);
cairo_fill(context);
cairo_move_to(context, 0.0, 0.0);
cairo_set_source_rgba(context, 1.0, 1.0, 1.0, 1.0);
cairo_move_to(context, 0.0, 30.0);
cairo_set_font_size(context, 20.0);
//sprintf(text, "%.2fFPS GPS: %.3f, %.3f Speed %.1fkm/h b0", userdata.fps,lat,lon,speed);
cairo_show_text(context, text);
userdata.overlay = 0;
}
else {
cairo_rectangle(context2, 0.0, 0.0, 600, 100);
cairo_set_source_rgba(context2, 0.0, 0.0, 0.0, 1.0);
cairo_fill(context2);
cairo_move_to(context2, 0.0, 0.0);
cairo_set_source_rgba(context2, 1.0, 1.0, 1.0, 1.0);
cairo_move_to(context2, 0.0, 30.0);
cairo_set_font_size(context2, 20.0);
//sprintf(text, "%.2fFPS GPS: %.3f, %.3f Speed %.1fkm/h b1", userdata.fps,lat,lon,speed);
//sprintf(text, "%.2fFPS GPS: 0.00000, 0.00000 Speed 0km/h b1", userdata.fps);
cairo_show_text(context2, text);
userdata.overlay = 1;
}
duration--;
usleep(30000);
}
return 0;
}
int main(int argc, char *argv[])
{
int line_count = 0, start_write = 0;
unsigned char *line = NULL;
char *output_file = NULL;
int strsize, i, j;
int seconds = 0;
FILE *lck = NULL;
pid_t pid;
time_t now;
/* get current UNIX time and PID */
time(&now);
pid = getpid();
lock_link = malloc(25+7);
if(lock_link == NULL)
exit(1);
snprintf(lock_link, 25+7,"%s.%d", ELOCKLINK, pid);
signal(SIGPIPE,cleanup);
signal(SIGHUP,cleanup);
signal(SIGUSR1,cleanup);
signal(SIGINT,cleanup);
signal(SIGALRM,cleanup);
/*******************/
/* Check Arguments */
/*******************/
if(argc > 1) {
for(i = 1; argv[i] && (i <= argc); i++) {
if((strncmp(argv[i], "-", 1) == 0
&& (strlen(argv[i]) == 1))) {
exit(1);
} else if((strncmp(argv[i], "-", 1) == 0) && (argv[i][1] == '\0'))
continue;
else if(strncmp(argv[i], "/", 1) == 0) {
/* File location */
strsize = strlen(argv[i]);
output_file = malloc(strsize + 1);
if(output_file == NULL)
continue;
strncpy(output_file, argv[i], strsize + 1);
continue;
} else {
/* Recording Time */
for(j = 0; argv[i][j] != '\0'; j++) {
if(isdigit(argv[i][j]) == 0) {
/* Bad Argument Given */
fprintf(stderr, "program: ERROR: bad option! %s\n", argv[i]);
fprintf(stderr, "Usage: program [seconds] [mpegfile]\n");
exit(1);
}
}
seconds = (int) atoi(argv[i]);
if(VERBOSE)
printf("Recording for %d Seconds\n", seconds);
}
}
} else {
fprintf(stderr, "Usage: program [seconds] [mpegfile]\n");
exit(1);
}
/* Lock Video Input */
lck = fopen(lock_link, "w");
if(lck == NULL)
return 1;
fprintf(lck, "%d", pid);
if(link(lock_link, ELOCK) != 0) {
fclose(lck);
unlink(lock_link);
return 1;
} else
fclose(lck);
/* Setup Codec */
setup_encoder();
/* Time the Recording */
alarm(seconds);
/* Setup Environment */
umask(022);
/* Allocate input line */
line = malloc(MAX_INPUT_LINE + 1);
if(line == NULL)
exit(1);
/* Open Video Input */
if ( (fdin = open(VIDEO_DEV, O_RDONLY) ) < 0 ) {
fprintf(stderr, "Failed to open %s: %s\n", VIDEO_DEV, strerror(errno));
exit(1);
}
/* Open Mpeg Output */
if ( (fdout = open(output_file, O_CREAT|O_TRUNC|O_WRONLY) ) < 0 ) {
fprintf(stderr, "Failed to open %s: %s\n", output_file, strerror(errno));
exit(1);
}
chmod(output_file, 0644);
/* Read from Video Input while Writing to MPEG2 File */
while(read(fdin, line, MAX_INPUT_LINE) > 0) {
line_count++;
/* Start Late to avoid bad Encoder Startup */
if(line_count > 10000)
start_write = 1;
/* Write Data to MPEG2 File */
if(start_write == 1)
write(fdout, line, MAX_INPUT_LINE);
if(SHOW_LINE_COUNT)
printf("%d\n", line_count);
}
cleanup(0);
exit(0);
}
/*********************************************************************
Function: int main(void)
PreCondition: None.
Input: None.
Output: None.
Side Effects: None.
Overview: main function of the application. Peripherals are
initialized.
Note: None.
********************************************************************/
int main(void)
{
int cs;
// vars used for detection of incremental motion
unsigned short new_cmd,last_cmd, new_fb,last_fb;
setup_io(); // make all i/o pins go the right dir
STATUS_LED = 0; // led on
setup_uart(); // setup the serial interface to the PC
setup_TMR1(); // set up 1ms timer
IEC0bits.T1IE = 1; // Enable interrupts for timer 1
// needed for delays in following routines
// 1/2 seconds startup delay
timer_test = 5000;
while ( timer_test );
printf("\r\nPowerup..i/o...uart...timer...");
setup_pwm(); // start analog output
set_pwm(0.0);
printf("pwm...");
init_pid();
printf("pid...");
setup_encoder(); // 16 bit quadrature encoder module setup
printf("encoder...");
setup_capture(); // 2 pins with quadrature cmd from PC
printf("capture...");
printf("done\r\n");
// some junk for the serial channel
printf("%s%s\n\r",CPWRT,VERSION);
STATUS_LED = 1; // led off when init finished
// restore config from eeprom
// Read array named "setupEE" from DataEEPROM and place
// the result into array in RAM named, "setup"
restore_setup();
cs = calc_cksum(sizeof(pid)/sizeof(int),(int*)&pid);
if ( cs )
{
// opps, no valid setup detected
// assume we are starting from a new box
printf("No valid setup found in EEPROM\r\n");
init_pid();
}
else
{
printf("Using setup from eeprom.. ? for help\r\n");
print_tuning();
}
printf("using %fms servo loop interval\r\n",pid.ticksperservo * 0.1);
// BLOCK OF TEST ROUTINES FOR HARDWARE DEBUGGING
// test_pwm_interface(); // play with opa549 hardware
// test_pc_interface(); // echo cmded posn to serial port
// test_pid_interface(); // test pid loop operation
new_cmd = last_cmd = new_fb = last_fb = 0;
while (1)
{
if ( do_servo ) // check and see if timer 1 has asked for servo calcs to be run
{
do_servo = 0;
if (SVO_ENABLE)
{
if ( pid.enable == 0 ) // last loop, servo was off
{
set_pwm( 0.0 );
printf("servo-enabled\r\n>");
pid.enable = 1;
// make sure we dont move on enabling
cmd_posn = POSCNT; // make 16bit incr registers match
pid.command = 0L; // make 32 bit counter match
pid.feedback = 0L;
// make the 1ms loop temps match
new_cmd = last_cmd = new_fb = last_fb = 0;
pid.error_i = 0.0; // reset integrator
}
// we can time the servo cycle calcs by scoping the PID_ACTIVE pin
PID_ACTIVE = 1; // seems to take about 140us
new_cmd = cmd_posn; // grab current cmd from pc
new_fb = POSCNT; // grab current posn from encoder
pid.command += (long int)((short)(new_cmd - last_cmd));
pid.feedback += (long int)((short)(new_fb - last_fb ));
last_cmd = new_cmd;
last_fb = new_fb;
calc_pid();
// check for a drive fault ( posn error > allowed )
if (( pid.maxerror > 0.0 ) &&
( fabs(pid.error) > pid.maxerror ))
{
short temp = SVO_ENABLE;
set_pwm( 0.0 );
while (1) // trap here until svo disabled or pwr cycle
{
// reset integrator as it may have wound up
pid.error_i = 0.0;
printf("drive fault... maxerror exceeded\r\n");
STATUS_LED = 0; timer_test = 2500; while ( timer_test );
STATUS_LED = 1; timer_test = 2500; while ( timer_test );
STATUS_LED = 0; timer_test = 2500; while ( timer_test );
STATUS_LED = 1; timer_test = 2500; while ( timer_test );
if (temp != SVO_ENABLE)
break;
}
}
else
{
set_pwm(pid.output); // update motor drive
}
PID_ACTIVE = 0; // i/o pin for timing pid calcs
}
else
{
if ( pid.enable == 1 ) // last loop servo was active
{
set_pwm( 0.0 );
pid.enable = 0;
printf("servo-disabled\r\n>");
// extra delay keeps us faulted for min 1 sec to let mechanicals settle
STATUS_LED = 1; timer_test = 10000; while ( timer_test );
}
}
}
// look for serial cmds
// doing this while svo is enabled will cause bumps in servo loop
// because of serial i/o time ( unless we get smart and move svo loop
// into an isr )
if ( rxrdy )
process_serial_buffer();
if (pid.limit_state) // show we are at drive limit(error)
STATUS_LED = 0;
else
STATUS_LED = 1;
}
// to keep compiler happy....
return 0;
}
// ------------------------------------------------------------------------------------------------------------------------------------------------
//
int main(int argc, char** argv)
{
PORT_USERDATA userdata;
MMAL_STATUS_T status;
memset(&userdata, 0, sizeof (PORT_USERDATA));
userdata.width = VIDEO_WIDTH;
userdata.height = VIDEO_HEIGHT;
userdata.motion = 0;
userdata.grabframe = 1;
fprintf(stderr, "VIDEO_WIDTH : %i\n", userdata.width );
fprintf(stderr, "VIDEO_HEIGHT: %i\n", userdata.height );
fprintf(stderr, "VIDEO_FPS : %i\n", VIDEO_FPS);
bcm_host_init();
if (1 && setup_camera(&userdata) != 0) {
fprintf(stderr, "Error: setup camera %x\n", status);
return -1;
}
if (1 && setup_encoder(&userdata) != 0) {
fprintf(stderr, "Error: setup encoder %x\n", status);
return -1;
}
vcos_semaphore_create(&userdata.complete_semaphore, "mmal_opencv_video", 0);
int count = 0;
int opencv_frames = 0;
struct timespec t1;
struct timespec t2;
clock_gettime(CLOCK_MONOTONIC, &t1);
struct timespec s;
s.tv_sec = 0;
s.tv_nsec = 30000000;
int nTotalFrameCount = VIDEO_FPS * 30;
userdata.motion = nTotalFrameCount;
unsigned dwTickCount_Start = GetTickCount( );
while (1)
{
nanosleep(&s, NULL);
if (userdata.motion == 0)
{
break;
}
opencv_frames++;
if( (CALC_FPS) && (opencv_frames % (VIDEO_FPS*2) == 0) ){
fprintf(stderr, "FPS: Video = %.2f\n", userdata.video_fps);
}
}
unsigned dwTickCount_End = GetTickCount( );
fprintf( stderr, "%d frames were stored in %dms -> %.2f fps\n", nTotalFrameCount, (dwTickCount_End - dwTickCount_Start), nTotalFrameCount / ((dwTickCount_End - dwTickCount_Start) / 1000.0) );
return 0;
}
int main(int argc, char** argv) {
PORT_USERDATA userdata;
MMAL_STATUS_T status;
cairo_surface_t *surface,*surface2;
cairo_t *context,*context2;
memset(&userdata, 0, sizeof (PORT_USERDATA));
userdata.width = VIDEO_WIDTH;
userdata.height = VIDEO_HEIGHT;
userdata.fps = 0.0;
fprintf(stderr, "VIDEO_WIDTH : %i\n", userdata.width );
fprintf(stderr, "VIDEO_HEIGHT: %i\n", userdata.height );
fprintf(stderr, "VIDEO_FPS : %i\n", VIDEO_FPS);
fprintf(stderr, "Running...\n");
bcm_host_init();
surface = cairo_image_surface_create(CAIRO_FORMAT_ARGB32, 600, 100);
context = cairo_create(surface);
cairo_rectangle(context, 0.0, 0.0, 600, 100);
cairo_set_source_rgba(context, 0.0, 0.0, 0.0, 1.0);
cairo_fill(context);
userdata.overlay_buffer = cairo_image_surface_get_data(surface);
userdata.overlay = 1;
surface2 = cairo_image_surface_create(CAIRO_FORMAT_ARGB32, 600, 100);
context2 = cairo_create(surface2);
cairo_rectangle(context2, 0.0, 0.0, 600, 100);
cairo_set_source_rgba(context2, 0.0, 0.0, 0.0, 1.0);
cairo_fill(context2);
userdata.overlay_buffer2 = cairo_image_surface_get_data(surface2);
if (1 && setup_camera(&userdata) != 0) {
fprintf(stderr, "Error: setup camera %x\n", status);
return -1;
}
if (1 && setup_encoder(&userdata) != 0) {
fprintf(stderr, "Error: setup encoder %x\n", status);
return -1;
}
if (1 && setup_preview(&userdata) != 0) {
fprintf(stderr, "Error: setup preview %x\n", status);
return -1;
}
char text[256];
//fake Speed and GPS data
float lat = 47.4912;
float lon = 8.906;
float speed = 20.0;
while (1) {
//Update Draw to unused buffer that way there is no flickering of the overlay text if the overlay update rate
//and video FPS are not the same
if (userdata.overlay == 1) {
cairo_rectangle(context, 0.0, 0.0, 600, 100);
cairo_set_source_rgba(context, 0.0, 0.0, 0.0, 1.0);
cairo_fill(context);
cairo_move_to(context, 0.0, 0.0);
cairo_set_source_rgba(context, 1.0, 1.0, 1.0, 1.0);
cairo_move_to(context, 0.0, 30.0);
cairo_set_font_size(context, 20.0);
sprintf(text, "%.2fFPS GPS: %.3f, %.3f Speed %.1fkm/h b0", userdata.fps,lat,lon,speed);
cairo_show_text(context, text);
userdata.overlay = 0;
}
else {
cairo_rectangle(context2, 0.0, 0.0, 600, 100);
cairo_set_source_rgba(context2, 0.0, 0.0, 0.0, 1.0);
cairo_fill(context2);
cairo_move_to(context2, 0.0, 0.0);
cairo_set_source_rgba(context2, 1.0, 1.0, 1.0, 1.0);
cairo_move_to(context2, 0.0, 30.0);
cairo_set_font_size(context2, 20.0);
sprintf(text, "%.2fFPS GPS: %.3f, %.3f Speed %.1fkm/h b1", userdata.fps,lat,lon,speed);
//sprintf(text, "%.2fFPS GPS: 0.00000, 0.00000 Speed 0km/h b1", userdata.fps);
cairo_show_text(context2, text);
userdata.overlay = 1;
}
lat += 0.01;
lon += 0.01;
speed += 0.1;
usleep(30000);
}
return 0;
}
