int main(int argc, char *argv[]){ HANDLE acq_image_thread, save_thread, acq_imu_thread; DWORD acq_image_thread_id, save_thread_id, acq_imu_thread_id; char line_buffer[128] ; FILE *imuFile; int frame_index = 0 ; if(argc == 1){ printf("Usage : capture_sequence <nb_frames> <capture folder> \n"); exit(0); } if(argc > 1) nb_frames = atoi(argv[1]); if(argc > 2){ sprintf(path_base, "%s", argv[2]); }else{ sprintf(path_base, "./"); } if(init_frame_buffer(&my_frame_buffer,nb_frames, IMAGE_BYTES) < 0){ printf("Cannot allocate %d bytes \n", IMAGE_BYTES*nb_frames); exit(-1); } max_imu_data = (nb_frames > 0)?(((nb_frames/30)+1024)*250):((3600)*250); nb_imu_data = 0 ; //buffer for enough imu data at 250Hz imu_buffer = malloc(max_imu_data*IMU_LINE_BYTES); if(imu_buffer == NULL){ printf("Cannot allocate buffer for IMU data \n"); return 0 ; } sprintf(line_buffer, "%s/IMU.log", path_base); imuFile = fopen(line_buffer, "w"); if (imuFile == NULL) { perror("cannot open IMU file for write \n"); return ; } export_imu_calib(imuFile, 4.0, 500.0); // AG and 500dps int opt; int i = 0, j = 0 ; int init = 0 ; signal(SIGINT, killHandler); signal(SIGKILL, killHandler); signal(SIGTERM, killHandler); signal(SIGTSTP, killHandler); thread_alive = 1 ; init_time(); if(nb_frames > 0){ acq_image_thread_id = pthread_create(&acq_image_thread, NULL, &acq_image_thread_func, NULL); save_thread_id = pthread_create(&save_thread, NULL, &save_thread_func, NULL); } acq_imu_thread_id = pthread_create(&acq_imu_thread, NULL, &acq_imu_thread_func, NULL); if(nb_frames > 0){ pthread_join(acq_image_thread, NULL ); thread_alive = 0 ; pthread_join(save_thread, NULL ); } pthread_join(acq_imu_thread, NULL ); printf("Saving IMU data to disk %u \n", nb_imu_data); for(i = 0; i < nb_imu_data ; i ++){ unsigned long timestamp ; short raw_imu [6]; memcpy(×tamp, &(imu_buffer[i*IMU_LINE_BYTES]), sizeof(unsigned long)); memcpy(raw_imu, &(imu_buffer[(i*IMU_LINE_BYTES)+sizeof(unsigned long)]), 6*sizeof(short)); export_imu_raw(imuFile, raw_imu, timestamp); usleep(1); } printf("save done \n"); fclose(imuFile); free(imu_buffer); free_frame_buffer(&my_frame_buffer); return 0; }
static void nic_rx(snic *nic) { unsigned int frame; unsigned int rx_page; unsigned int rx_offset; unsigned int len; unsigned int next_pkt; unsigned int numpages; int iobase=nic->iobase; nicframe_header header; frame_buffer *newframe; struct ack_frame *fr; while(1) { outportg(NIC_DMA_DISABLE | NIC_PAGE1, iobase); rx_page=inportb(iobase + CURRENT); outportg(NIC_DMA_DISABLE | NIC_PAGE0, iobase); frame=inportb(iobase + BOUNDARY)+1; if(frame>=nic->pstop) frame=nic->pstart+TXPAGES; if(frame != nic->current_page) { printk("NE2000: ERROR: mismatched read page pointers!\n"); printk("NE2000: NIC-Boundary:%x dev-current_page:%x\n", frame, nic->current_page); } if(frame==rx_page) break; /* all frames read */ rx_offset=frame << 8; nic_get_header(nic,frame,&header); len=header.count - sizeof(nicframe_header); next_pkt=frame + 1 + ((len+4)>>8); numpages=nic->pstop-(nic->pstart+TXPAGES); if ((header.next!=next_pkt) && (header.next!=next_pkt + 1) && (header.next!=next_pkt - numpages) && (header.next != next_pkt +1 - numpages)){ printk("NE2000: ERROR: Index mismatch. header.next:%X next_pkt:%X frame:%X\n", header.next,next_pkt,frame); nic->current_page=frame; outportg(nic->current_page-1, iobase + BOUNDARY); continue; } if(len<60 || len>1518) printk("NE2000: invalid frame size:%d\n",len); else if((header.status & 0x0f) == RSR_PRX) { // We have a good frame, so let's recieve it! newframe=alloc_frame_buffer(len); if (newframe == NULL) // No memory { printk("NE2000 : No memory for new frame received\n"); nic_block_input(nic, (unsigned char *)dummypkt.buf1, len, rx_offset+sizeof(nicframe_header)); // Discarded } else { nic_block_input(nic,(unsigned char *)newframe->buf1,newframe->len, rx_offset+sizeof(nicframe_header)); // Check for duplicate frame newframe->next = NULL; fr = (struct ack_frame *)newframe->buf1; if (fr->ethfrhdr_seqno != (ethseqno_list[fr->pkt_src_saddr_host])) // Other than previous one. { ethseqno_list[fr->pkt_src_saddr_host] = fr->ethfrhdr_seqno; fr->ethfrhdr_seqno = 0; // Required to verify the checksum // Insert this frame into received frames q _lock(&rxq_lock); if (rx_q_tail == NULL) rx_q_head = rx_q_tail = newframe; else { rx_q_tail->next = newframe; rx_q_tail = newframe; } event_wakeup(&nic_recv_event); unlock(&rxq_lock); } else free_frame_buffer(newframe); // Duplicate } } else { printk("NE2000: ERROR: bad frame. header-> status:%X next:%X len:%x.\n", header.status,header.next,header.count); } next_pkt=header.next; if(next_pkt >= nic->pstop) { printk("NE2000: ERROR: next frame beyond local buffer! next:%x.\n", next_pkt); next_pkt=nic->pstart+TXPAGES; } nic->current_page=next_pkt; outportg(next_pkt-1, iobase + BOUNDARY); } outportg(ISR_PRX | ISR_RXE, iobase + INTERRUPTSTATUS); }