Menu
▾
▴
[Robotflow-cvs] RobotFlow/Devices/src IMU400CC_200.cc,1.1,1.2 SerialDriver.cc,1.4,1.5
|
From: Dominic L. <ma...@us...> - 2005-04-26 14:26:22
|
Update of /cvsroot/robotflow/RobotFlow/Devices/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv24902 Modified Files: IMU400CC_200.cc SerialDriver.cc Log Message: working device Index: SerialDriver.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Devices/src/SerialDriver.cc,v retrieving revision 1.4 retrieving revision 1.5 diff -C2 -d -r1.4 -r1.5 *** SerialDriver.cc 30 Mar 2005 19:11:02 -0000 1.4 --- SerialDriver.cc 26 Apr 2005 14:25:48 -0000 1.5 *************** *** 121,124 **** --- 121,125 ---- void End_session(int fd) { + tcflush(fd,TCIOFLUSH); close(fd); } Index: IMU400CC_200.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Devices/src/IMU400CC_200.cc,v retrieving revision 1.1 retrieving revision 1.2 diff -C2 -d -r1.1 -r1.2 *** IMU400CC_200.cc 25 Apr 2005 19:51:13 -0000 1.1 --- IMU400CC_200.cc 26 Apr 2005 14:25:48 -0000 1.2 *************** *** 55,58 **** --- 55,63 ---- * @parameter_value /dev/ttyUSB0 * + * @parameter_name BIAS_VALUES + * @parameter_type int + * @parameter_value 0 + * @parameter_description The number of values to calculate the bias on gyro rate before processing. + * * @output_name X_ACCEL * @output_type float *************** *** 110,114 **** #define IMU_RR_PRECISION 200.0 #define IMU_TIME_PRECISION 0.79E-6 ! #define IMU_GRAVITY_ACCEL_VALUE 9.81 class IMU400_DATA { --- 115,119 ---- #define IMU_RR_PRECISION 200.0 #define IMU_TIME_PRECISION 0.79E-6 ! #define IMU_GRAVITY_ACCEL_VALUE 9.80 class IMU400_DATA { *************** *** 190,193 **** --- 195,199 ---- //parameters string m_serial_port; + int m_bias_values; //Internal variables *************** *** 337,341 **** pthread_mutex_lock(&m_recv_list_lock); ! if (m_recv_list.size() < 1000) { m_recv_list.push_back(data); } --- 343,347 ---- pthread_mutex_lock(&m_recv_list_lock); ! if (m_recv_list.size() < max(1000,m_bias_values)) { m_recv_list.push_back(data); } *************** *** 449,456 **** //parameters m_serial_port = object_cast<String>(parameters.get("SERIAL_PORT")); ! ! //open serial port ! m_fd = Port_initialisation(m_serial_port.c_str(),BAUDRATE_38400); ! cerr<<"Open serial port : "<<m_serial_port<<" fd : "<<m_fd<<endl; } --- 455,460 ---- //parameters m_serial_port = object_cast<String>(parameters.get("SERIAL_PORT")); ! m_bias_values = dereference_cast<int>(parameters.get("BIAS_VALUES")); ! } *************** *** 463,506 **** int timeout = 0; ! //verify if serial port opened ! if (m_fd >= 0) { ! ! cerr<<"STARTING POLLED MODE"<<endl; ! message[0] = 'P'; ! write(m_fd, message,1); timeout = 0; do { //PING MESSAGE message[0] = 'R'; ! write(m_fd, message,1); ! //READ DEVICE ANSWER ! read(m_fd,message,1); ! usleep(50000); ! cerr<<"GOT PING ANSWER : "<<message[0]<<endl; ! } while(message[0] != 'H' && timeout++ < 100); if (message[0] != 'H') { throw new GeneralException("No device found",__FILE__,__LINE__); } ! ! message[0] = 'z'; //ZERO ! message[1] = 0xC8; ! write(m_fd, message,2); ! cerr<<"WAITING FOR ZERO"<<endl; ! read(m_fd,message,1); ! cerr<<"GOT ZERO MESSAGE : "<<message[0]<<endl; ! ! sleep(10);//wait 10 seconds do { message[0] = 'c'; //SCALED MODE MESSAGE write(m_fd, message,1); ! read(m_fd,message,1); ! cerr<<"GOT SCALED MODE ANSWER : "<<message[0]<<endl; ! } while (message[0] != 'C'); ! cerr<<"STARTING CONTINUOUS MODE"<<endl; message[0] = 'C'; //CONTINUOUS MODE START write(m_fd, message,1); --- 467,540 ---- int timeout = 0; ! //open serial port ! m_fd = Port_initialisation(m_serial_port.c_str(),BAUDRATE_38400); ! cerr<<"IMU400CC_200 : Open serial port (38400): "<<m_serial_port<<" fd : "<<m_fd<<endl; ! ! cerr<<"IMU400CC_200 : STARTING POLLED MODE"<<endl; ! message[0] = 'P'; ! write(m_fd, message,1); ! ! //TODO MAKE AUTOMATIC BAUD RATE DETECTION WORK... ! //message[0] = 'b'; ! //write(m_fd,message,1); ! //End_session(m_fd); ! //reopen serial port ! //m_fd = Port_initialisation(m_serial_port.c_str(),BAUDRATE_57600); ! //cerr<<"Open serial port (57600): "<<m_serial_port<<" fd : "<<m_fd<<endl; ! ! //verify if serial port opened ! if (m_fd >= 0) { timeout = 0; + do { //PING MESSAGE message[0] = 'R'; ! write(m_fd, message,1); ! ! //READ DEVICE ANSWER (TIMEOUT 1 SEC ON READ) ! if (read(m_fd,message,1) > 0) { ! cerr<<"IMU400CC_200 : GOT PING ANSWER : "<<message[0]<<endl; ! } ! } while(message[0] != 'H' && timeout++ < 10); if (message[0] != 'H') { + End_session(m_fd); + m_fd = -1; throw new GeneralException("No device found",__FILE__,__LINE__); } + + bool zero_sent=false; + timeout = 0; + do { + if (!zero_sent) { + message[0] = 'z'; //ZERO + message[1] = 0xFF; + write(m_fd, message,2); + zero_sent = true; + } + cerr<<"IMU400CC_200 : WAITING FOR ZERO"<<endl; + if (read(m_fd,message,1) > 0) { + cerr<<"IMU400CC_200 : GOT ZERO MESSAGE : "<<message[0]<<endl; + } + } while ( message[0] != 'Z' && timeout++ < 10); ! //wait until unit zeroed ! cerr<<"IMU400CC_200 : WAITING 25SEC UNTIL UNIT IS CALIBRATED..."<<endl; ! sleep(25); ! ! timeout = 0; do { + cerr<<"IMU400CC_200 : SENDING SCALED MODE"<<endl; message[0] = 'c'; //SCALED MODE MESSAGE write(m_fd, message,1); ! ! if (read(m_fd,message,1) > 0) { ! cerr<<"IMU400CC_200 : GOT SCALED MODE ANSWER : "<<message[0]<<endl; ! } ! } while (message[0] != 'C' && timeout++ < 10); ! cerr<<"IMU400CC_200 : STARTING CONTINUOUS MODE"<<endl; message[0] = 'C'; //CONTINUOUS MODE START write(m_fd, message,1); *************** *** 519,556 **** pthread_create(&m_recvThread, NULL, imu400_recv_thread, this); ! ! //bias calib values over 1000 samples ! while(1) { ! pthread_mutex_lock(&m_recv_list_lock); ! if (m_recv_list.size() == 1000) break; ! pthread_mutex_unlock(&m_recv_list_lock); ! } ! ! ! float count = 0; ! ! //calculate bias ! for (list<IMU400_DATA>::iterator iter = m_recv_list.begin(); ! iter != m_recv_list.end(); iter++) { ! m_yaw_bias += iter->m_yaw_rate; ! m_pitch_bias += iter->m_pitch_rate; ! m_roll_bias += iter->m_roll_rate; ! ! count += 1.0; ! } ! ! if (count > 0) { ! m_yaw_bias /= count; ! m_pitch_bias /= count; ! m_roll_bias /= count; } - cerr<<"BIASES : "<<m_yaw_bias<<" "<<m_pitch_bias<<" "<<m_roll_bias<<endl; - - //clear list - m_recv_list.resize(0); - - //very important to - pthread_mutex_unlock(&m_recv_list_lock); } } --- 553,592 ---- pthread_create(&m_recvThread, NULL, imu400_recv_thread, this); ! if (m_bias_values > 0) { ! ! //bias calib values over m_bias_values samples ! while(1) { ! pthread_mutex_lock(&m_recv_list_lock); ! if (m_recv_list.size() >= m_bias_values) break; ! pthread_mutex_unlock(&m_recv_list_lock); ! } ! ! float count = 0; ! ! //calculate bias ! for (list<IMU400_DATA>::iterator iter = m_recv_list.begin(); ! iter != m_recv_list.end(); iter++) { ! ! m_yaw_bias += iter->m_yaw_rate; ! m_pitch_bias += iter->m_pitch_rate; ! m_roll_bias += iter->m_roll_rate; ! ! count += 1.0; ! } ! ! if (count > 0) { ! m_yaw_bias /= count; ! m_pitch_bias /= count; ! m_roll_bias /= count; ! } ! cerr<<"BIASES : "<<m_yaw_bias<<" "<<m_pitch_bias<<" "<<m_roll_bias<<endl; ! ! //clear list ! m_recv_list.resize(0); ! ! //very important to ! pthread_mutex_unlock(&m_recv_list_lock); } } } *************** *** 633,660 **** unsigned char message[IMU_PACKET_SIZE]; cerr << "IMU400CC_200 : destructor" << endl; - cerr << "IMU400CC_200 : waiting for send & receive threads" << endl; - pthread_mutex_unlock(&m_send_list_lock); // unlock all - pthread_mutex_unlock(&m_recv_list_lock); - - m_killed = 1; - pseudosem_post(&m_semaphore); - - pthread_join(m_sendThread,NULL); - cerr << "IMU400CC_200 : Sending_thread killed (joined)" << endl; - - pthread_join(m_recvThread,NULL); - cerr << "IMU400CC_200 : Receiving_thread killed (joined)" << endl; - - pseudosem_destroy(&m_semaphore); - pthread_mutex_destroy(&m_send_list_lock); - pthread_mutex_destroy(&m_recv_list_lock); - ! cerr<<"STARTING POLLED MODE"<<endl; ! message[0] = 'P'; ! write(m_fd, message,1); ! sleep(1); ! ! End_session(m_fd); cerr << "IMU400CC_200 (done!)" << endl; --- 669,699 ---- unsigned char message[IMU_PACKET_SIZE]; cerr << "IMU400CC_200 : destructor" << endl; ! if (m_fd > 0) { ! cerr << "IMU400CC_200 : waiting for send & receive threads" << endl; ! pthread_mutex_unlock(&m_send_list_lock); // unlock all ! pthread_mutex_unlock(&m_recv_list_lock); ! ! m_killed = 1; ! pseudosem_post(&m_semaphore); ! ! pthread_join(m_sendThread,NULL); ! cerr << "IMU400CC_200 : Sending_thread killed (joined)" << endl; ! ! pthread_join(m_recvThread,NULL); ! cerr << "IMU400CC_200 : Receiving_thread killed (joined)" << endl; ! ! pseudosem_destroy(&m_semaphore); ! pthread_mutex_destroy(&m_send_list_lock); ! pthread_mutex_destroy(&m_recv_list_lock); ! ! ! cerr<<"IMU400CC_200 : STARTING POLLED MODE"<<endl; ! message[0] = 'P'; ! write(m_fd, message,1); ! sleep(1); ! ! End_session(m_fd); ! } cerr << "IMU400CC_200 (done!)" << endl; |
