Razor_AHRS.cpp

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