AHRS
Dependencies: Eigen
Diff: AHRS.cpp
- Revision:
- 7:bfde7bd5fe31
- Parent:
- 6:5824bd96b6cf
- Child:
- 8:51062bb877f0
diff -r 5824bd96b6cf -r bfde7bd5fe31 AHRS.cpp --- a/AHRS.cpp Fri Jun 14 07:05:20 2019 +0000 +++ b/AHRS.cpp Wed Jun 26 14:19:01 2019 +0000 @@ -20,15 +20,18 @@ LinearCharacteristics raw_gz2gz(-32767,32768,-500*PI/180.0, 500*PI/180.0); LinearCharacteristics int2magx( -32767,32768,100.0,-100.0); // x-axis reversed LinearCharacteristics int2magy( -32767,32768,100.0,-100.0); // y-axis reversed*/ - LinearCharacteristics raw_ax2ax( 1.0,0.0); // use gain and offset here - LinearCharacteristics raw_ay2ay(-1.0,0.0); // y-axis reversed - LinearCharacteristics raw_az2az( 1.0,0.0); - LinearCharacteristics raw_gx2gx( 1.0,0.0); - LinearCharacteristics raw_gy2gy(-1.0,0.0); // y-axis reversed (lefthanded system) - LinearCharacteristics raw_gz2gz( 1.0,0.0); - LinearCharacteristics int2magx( -1.0,0.0); // x-axis reversed - LinearCharacteristics int2magy( -1.0,0.0); // y-axis reversed - LinearCharacteristics int2magz( 1.0,0.0); + raw_ax2ax.setup( 1.0,-.31); // use gain and offset here + raw_ay2ay.setup(-1.0,-0.31); // y-axis reversed // was -1,0.0 + raw_az2az.setup( 1.0,0.0); + raw_gx2gx.setup( 1.0,0.0); + raw_gy2gy.setup(-1.0,0.0); // y-axis reversed (lefthanded system) + raw_gz2gz.setup( 1.0,0.0); + int2magx.setup( 1.0,0.0); // x-axis reversed + int2magy.setup( 1.0,0.0); // y-axis reversed + int2magz.setup( 1.0,0.0); + local_time = 0.0; + // + //matrix measurement(6,1,0.0); while (!imu.begin()) { wait(1); @@ -41,11 +44,11 @@ void AHRS::update(void) { - while(1){ + while(1){ thread.signal_wait(signal); imu.readAccel(); //imu.readMag_calibrated(); - imu.readMag(); + //imu.readMag(); imu.readGyro(); matrix measurement(6,1,0.0); //Perform filter update @@ -57,15 +60,27 @@ measurement.put_entry(3,0,raw_ax2ax(imu.accX)); measurement.put_entry(4,0,raw_ay2ay(imu.accY)); RPY_filter.loop(&measurement); + //measurement.put_entry(0,0,10*sin(local_time)); my_logger.data_vector[8]=10*sin(local_time); measurement.put_entry(1,0,-5*cos(local_time)); my_logger.data_vector[9]=-5*cos(local_time); measurement.put_entry(3,0,sin(2*local_time)); measurement.put_entry(4,0,cos(3*local_time)); + //if(local_time<=2.0) RPY_filter.loop(&measurement); if(local_time==0.0) printf("\r\n"); if(local_time<=0.2){ printf("%1.2f xk: ",local_time); for(int k=0;k<6;k++) printf("%1.5f ",RPY_filter.get_est_state(k)); printf("\r\n");} + //local_time += 0.01; my_logger.data_vector[0] = raw_gx2gx(imu.gyroX); my_logger.data_vector[1] = raw_gy2gy(imu.gyroY); my_logger.data_vector[2] = raw_gz2gz(imu.gyroZ); my_logger.data_vector[3] = raw_ax2ax(imu.accX); my_logger.data_vector[4] = raw_ay2ay(imu.accY); my_logger.data_vector[5] = raw_az2az(imu.accZ); - my_logger.data_vector[6] = RPY_filter.get_est_state(1); - my_logger.data_vector[7] = RPY_filter.get_est_state(2); + my_logger.data_vector[6] = RPY_filter.get_est_state(0); + my_logger.data_vector[7] = RPY_filter.get_est_state(1); + my_logger.data_vector[8] = RPY_filter.get_est_state(4); + my_logger.data_vector[9] = RPY_filter.get_est_state(5); + + //printf("R %1.5f P %1.5f gx: %1.5f gy: %1.5f\r\n",my_logger.data_vector[6],my_logger.data_vector[7],my_logger.data_vector[0],my_logger.data_vector[1]); + //printf("%1.6f %1.6f %1.6f %1.6f %1.6f %1.6f %1.6f %1.6f\r\n",my_logger.data_vector[6],my_logger.data_vector[7],raw_gx2gx(imu.gyroX),raw_gy2gy(imu.gyroY),raw_gz2gz(imu.gyroZ),raw_ax2ax(imu.accX),raw_ay2ay(imu.accY),raw_az2az(imu.accZ)); + //printf("ax: %1.4f ay: %1.4f az: %1.4f\r\n",raw_ax2ax(imu.accX),raw_ay2ay(imu.accY),raw_az2az(imu.accZ)); + //printf("gx: %1.4f gy: %1.4f gz: %1.4f\r\n",raw_gx2gx(imu.gyroX),raw_gy2gy(imu.gyroY),raw_gz2gz(imu.gyroZ)); + //printf("ax: %1.4f ay: %1.4f az: %1.4f\r\n",raw_ax2ax(imu.accX),raw_ay2ay(imu.accY),raw_az2az(imu.accZ)); } // while(1) (the thread) + } void AHRS::sendSignal() {