AHRS.cpp

00001 /* This file is part of the Razor AHRS Firmware */
00002 #include "AHRS.h"
00003 #include <math.h>
00004 
00005 void IMU::read_sensors()
00006 {
00007     Read_Gyro(); // Read gyroscope (and temperature)
00008     Read_Accel(); // Read accelerometer
00009     Read_Magn(); // Read magnetometer
00010 }
00011 
00012 // Read every sensor and record a time stamp
00013 // Init DCM with unfiltered orientation
00014 // TODO re-init global vars?
00015 void IMU::reset_sensor_fusion()
00016 {
00017     float temp0[3] = { accel[0], accel[1], accel[2] };
00018     float temp1[3];
00019     float temp2[3];
00020     float xAxis[] = {1.0f, 0.0f, 0.0f};
00021 
00022     read_sensors();
00023     timestamp = timer.read_ms();
00024 
00025     // GET PITCH
00026     // Using y-z-plane-component/x-component of gravity vector
00027     pitch = -atan2(accel[0], sqrt((double)(accel[1] * accel[1] + accel[2] * accel[2])));
00028 
00029     // GET ROLL
00030     // Compensate pitch of gravity vector
00031 
00032     Vector_Cross_Product(temp1, temp0, xAxis);
00033     Vector_Cross_Product(temp2, xAxis, temp1);
00034     // Normally using x-z-plane-component/y-component of compensated gravity vector
00035     // roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2]));
00036     // Since we compensated for pitch, x-z-plane-component equals z-component:
00037     roll = atan2(temp2[1], temp2[2]);
00038 
00039     // GET YAW
00040     Compass_Heading();
00041     yaw = MAG_Heading;
00042 
00043     // Init rotation matrix
00044     init_rotation_matrix(DCM_Matrix, yaw, pitch, roll);
00045 }
00046 
00047 // Apply calibration to raw sensor readings
00048 void IMU::compensate_sensor_errors()
00049 {
00050     // Compensate accelerometer error
00051     accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
00052     accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
00053     accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
00054 
00055     // Compensate magnetometer error
00056     magnetom[0] = (magnetom[0] - MAGN_X_OFFSET) * MAGN_X_SCALE;
00057     magnetom[1] = (magnetom[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE;
00058     magnetom[2] = (magnetom[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE;
00059 
00060     // Compensate gyroscope error
00061     gyro[0] -= GYRO_AVERAGE_OFFSET_X;
00062     gyro[1] -= GYRO_AVERAGE_OFFSET_Y;
00063     gyro[2] -= GYRO_AVERAGE_OFFSET_Z;
00064 }
00065 
00066 // Reset calibration session if reset_calibration_session_flag is set
00067 void IMU::check_reset_calibration_session()
00068 {
00069     // Raw sensor values have to be read already, but no error compensation applied
00070 
00071     // Reset this calibration session?
00072     if (!reset_calibration_session_flag) return;
00073 
00074     // Reset acc and mag calibration variables
00075     for (int i = 0; i < 3; i++) {
00076         accel_min[i] = accel_max[i] = accel[i];
00077         magnetom_min[i] = magnetom_max[i] = magnetom[i];
00078     }
00079 
00080     // Reset gyro calibration variables
00081     gyro_num_samples = 0;  // Reset gyro calibration averaging
00082     gyro_average[0] = gyro_average[1] = gyro_average[2] = 0;
00083 
00084     reset_calibration_session_flag = false;
00085 }
00086 
00087 void IMU::turn_output_stream_on()
00088 {
00089     output_stream_on = true;
00090     statusLed = 1;
00091 }
00092 
00093 void IMU::turn_output_stream_off()
00094 {
00095     output_stream_on = false;
00096     statusLed = 0;
00097 }
00098 
00099 // Blocks until another byte is available on serial port
00100 char IMU::readChar()
00101 {
00102     while (pc.rxBufferGetCount() < 1) { } // Block
00103     return pc.getc();
00104 }
00105 
00106 void IMU::readInput()
00107 {
00108     // Read incoming control messages
00109     if (pc.rxBufferGetCount() >= 2) {
00110         if (pc.getc() == '#') { // Start of new control message
00111             int command = pc.getc(); // Commands
00112             if (command == 'f') // request one output _f_rame
00113                 output_single_on = true;
00114             else if (command == 's') { // _s_ynch request
00115                 // Read ID
00116                 char id[2];
00117                 id[0] = readChar();
00118                 id[1] = readChar();
00119 
00120                 // Reply with synch message
00121                 pc.printf("#SYNCH");
00122                 pc.putc(id[0]);
00123                 pc.putc(id[1]);
00124                 pc.printf(NEW_LINE);
00125             } else if (command == 'o') { // Set _o_utput mode
00126                 char output_param = readChar();
00127                 if (output_param == 'n') { // Calibrate _n_ext sensor
00128                     curr_calibration_sensor = (curr_calibration_sensor + 1) % 3;
00129                     reset_calibration_session_flag = true;
00130                 } else if (output_param == 't') // Output angles as _t_ext
00131                     output_mode = OUTPUT__MODE_ANGLES_TEXT;
00132                 else if (output_param == 'b') // Output angles in _b_inary form
00133                     output_mode = OUTPUT__MODE_ANGLES_BINARY;
00134                 else if (output_param == 'c') { // Go to _c_alibration mode
00135                     output_mode = OUTPUT__MODE_CALIBRATE_SENSORS;
00136                     reset_calibration_session_flag = true;
00137                 } else if (output_param == 's') // Output _s_ensor values as text
00138                     output_mode = OUTPUT__MODE_SENSORS_TEXT;
00139                 else if (output_param == '0') { // Disable continuous streaming output
00140                     turn_output_stream_off();
00141                     reset_calibration_session_flag = true;
00142                 } else if (output_param == '1') { // Enable continuous streaming output
00143                     reset_calibration_session_flag = true;
00144                     turn_output_stream_on();
00145                 } else if (output_param == 'e') { // _e_rror output settings
00146                     char error_param = readChar();
00147                     if (error_param == '0') output_errors = false;
00148                     else if (error_param == '1') output_errors = true;
00149                     else if (error_param == 'c') { // get error count
00150                         pc.printf("#AMG-ERR:%d,%d,%d" NEW_LINE,num_accel_errors,num_magn_errors,num_gyro_errors);
00151                     }
00152                 }
00153             }
00154 #if OUTPUT__HAS_RN_BLUETOOTH == true
00155             // Read messages from bluetooth module
00156             // For this to work, the connect/disconnect message prefix of the module has to be set to "#".
00157             else if (command == 'C') // Bluetooth "#CONNECT" message (does the same as "#o1")
00158                 turn_output_stream_on();
00159             else if (command == 'D') // Bluetooth "#DISCONNECT" message (does the same as "#o0")
00160                 turn_output_stream_off();
00161 #endif // OUTPUT__HAS_RN_BLUETOOTH == true
00162         } else { } // Skip character
00163     }
00164 }
00165 
00166 IMU::IMU()
00167     : gyro_num_samples(0)
00168     , yaw(0)
00169     , pitch(0)
00170     , roll(0)
00171     , MAG_Heading(0)
00172     , timestamp(0)
00173     , timestamp_old(0)
00174     , G_Dt(0)
00175     , output_mode(OUTPUT__MODE_ANGLES_BINARY) // Select your startup output mode here!// Select your startup output mode here!
00176     , output_stream_on(false)
00177     , output_single_on(true)
00178     , curr_calibration_sensor(0)
00179     , reset_calibration_session_flag(true)
00180     , num_accel_errors(0)
00181     , num_magn_errors(0)
00182     , num_gyro_errors(0)
00183     , output_errors(true)
00184     , statusLed(LED1)
00185     , pc(USBTX, USBRX)
00186     , Wire(p28, p27)
00187 {
00188 
00189     accel[0] = accel_min[0] = accel_max[0] = magnetom[0] = magnetom_min[0] = magnetom_max[0] = gyro[0] = gyro_average[0] = 0;
00190     accel[1] = accel_min[1] = accel_max[1] = magnetom[1] = magnetom_min[1] = magnetom_max[1] = gyro[1] = gyro_average[1] = 0;
00191     accel[2] = accel_min[2] = accel_max[2] = magnetom[2] = magnetom_min[2] = magnetom_max[2] = gyro[2] = gyro_average[2] = 0;
00192 
00193     Accel_Vector[0] = Gyro_Vector[0] = Omega_Vector[0] = Omega_P[0] = Omega_I[0] = Omega[0] = errorRollPitch[0] = errorYaw[0] = 0;
00194     Accel_Vector[1] = Gyro_Vector[1] = Omega_Vector[1] = Omega_P[1] = Omega_I[1] = Omega[1] = errorRollPitch[1] = errorYaw[1] = 0;
00195     Accel_Vector[2] = Gyro_Vector[2] = Omega_Vector[2] = Omega_P[2] = Omega_I[2] = Omega[2] = errorRollPitch[2] = errorYaw[2] = 0;
00196 
00197     DCM_Matrix[0][0] = 1;
00198     DCM_Matrix[0][1] = 0;
00199     DCM_Matrix[0][2] = 0;
00200     DCM_Matrix[1][0] = 0;
00201     DCM_Matrix[1][1] = 1;
00202     DCM_Matrix[1][2] = 0;
00203     DCM_Matrix[2][0] = 0;
00204     DCM_Matrix[2][1] = 0;
00205     DCM_Matrix[2][2] = 1;
00206 
00207     Update_Matrix[0][0] = 0;
00208     Update_Matrix[0][1] = 1;
00209     Update_Matrix[0][2] = 2;
00210     Update_Matrix[1][0] = 3;
00211     Update_Matrix[1][1] = 4;
00212     Update_Matrix[1][2] = 5;
00213     Update_Matrix[2][0] = 6;
00214     Update_Matrix[2][1] = 7;
00215     Update_Matrix[2][2] = 8;
00216 
00217     Temporary_Matrix[0][0] = 0;
00218     Temporary_Matrix[0][1] = 0;
00219     Temporary_Matrix[0][2] = 0;
00220     Temporary_Matrix[1][0] = 0;
00221     Temporary_Matrix[1][1] = 0;
00222     Temporary_Matrix[1][2] = 0;
00223     Temporary_Matrix[2][0] = 0;
00224     Temporary_Matrix[2][1] = 0;
00225     Temporary_Matrix[2][2] = 0;
00226 
00227     timer.start();
00228     // Init serial output
00229     pc.baud(OUTPUT__BAUD_RATE);
00230 
00231     // Init status LED
00232     statusLed = 0;
00233 
00234     // Init sensors
00235     wait_ms(50);  // Give sensors enough time to start
00236     I2C_Init();
00237     Accel_Init();
00238     Magn_Init();
00239     Gyro_Init();
00240 
00241     // Read sensors, init DCM algorithm
00242     wait_ms(20);  // Give sensors enough time to collect data
00243     reset_sensor_fusion();
00244 
00245     // Init output
00246 #if (OUTPUT__HAS_RN_BLUETOOTH == true) || (OUTPUT__STARTUP_STREAM_ON == false)
00247     turn_output_stream_off();
00248 #else
00249     turn_output_stream_on();
00250 #endif
00251 }
00252 
00253 // Main loop
00254 void IMU::loop()
00255 {
00256     timestamp_old = timestamp;
00257     timestamp = timer.read_ms();
00258     if (timestamp > timestamp_old)
00259         G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
00260     else
00261         G_Dt = 0;
00262 
00263     // Update sensor readings
00264     read_sensors();
00265 
00266     if (output_mode == OUTPUT__MODE_CALIBRATE_SENSORS) { // We're in calibration mode
00267         check_reset_calibration_session();  // Check if this session needs a reset
00268         if (output_stream_on || output_single_on)
00269             output_calibration(curr_calibration_sensor);
00270     } else if (output_mode == OUTPUT__MODE_SENSORS_TEXT) {
00271         // Apply sensor calibration
00272         compensate_sensor_errors();
00273 
00274         if (output_stream_on || output_single_on)
00275             output_sensors();
00276     } else if (output_mode == OUTPUT__MODE_ANGLES_TEXT || output_mode == OUTPUT__MODE_ANGLES_BINARY) {
00277         // Apply sensor calibration
00278         compensate_sensor_errors();
00279 
00280         // Run DCM algorithm
00281         Compass_Heading(); // Calculate magnetic heading
00282         Matrix_update();
00283         Normalize();
00284         Drift_correction();
00285         Euler_angles();
00286 
00287         if (output_stream_on || output_single_on)
00288             output_angles();
00289     }
00290 
00291     output_single_on = false;
00292 
00293 #if DEBUG__PRINT_LOOP_TIME == true
00294     pc.printf("loop time (ms) = %f" NEW_LINE, timer.read_ms() - timestamp);
00295 #endif
00296 }
00297 
00298 
00299 // Main loop
00300 void IMU::loop(float *data)
00301 {
00302     timestamp_old = timestamp;
00303     timestamp = timer.read_ms();
00304     if (timestamp > timestamp_old)
00305         G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
00306     else
00307         G_Dt = 0;
00308 
00309     // Update sensor readings
00310     read_sensors();
00311 
00312     // Apply sensor calibration
00313     //compensate_sensor_errors();
00314 
00315     // Run DCM algorithm
00316     Compass_Heading(); // Calculate magnetic heading
00317     Matrix_update();
00318     Normalize();
00319     Drift_correction();
00320     Euler_angles();
00321     
00322     data[0] = temperature;
00323     data[1] = TO_DEG(yaw);
00324     data[2] = TO_DEG(pitch);
00325     data[3] = TO_DEG(roll);
00326 }