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);
}
