![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Port of http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs to an mbed, tested with a 9DOF Sensor Stick, SEN-10724
Diff: Razor_AHRS.cpp
- Revision:
- 2:5aa75c3d8cc3
- Parent:
- 0:9a72d42c0da3
--- a/Razor_AHRS.cpp Wed Dec 28 17:13:14 2011 +0000 +++ b/Razor_AHRS.cpp Wed Dec 28 18:09:14 2011 +0000 @@ -26,7 +26,7 @@ // GET ROLL // Compensate pitch of gravity vector - + Vector_Cross_Product(temp1, temp0, xAxis); Vector_Cross_Product(temp2, xAxis, temp1); // Normally using x-z-plane-component/y-component of compensated gravity vector @@ -96,36 +96,7 @@ return pc.getc(); } -void IMU::setup() { - timer.start(); - // Init serial output - pc.baud(OUTPUT__BAUD_RATE); - - // Init status LED - statusLed = 0; - - // Init sensors - wait_ms(50); // Give sensors enough time to start - I2C_Init(); - Accel_Init(); - Magn_Init(); - Gyro_Init(); - - // Read sensors, init DCM algorithm - wait_ms(20); // Give sensors enough time to collect data - reset_sensor_fusion(); - - // Init output -#if (OUTPUT__HAS_RN_BLUETOOTH == true) || (OUTPUT__STARTUP_STREAM_ON == false) - turn_output_stream_off(); -#else - turn_output_stream_on(); -#endif -} - -// Main loop -void IMU::loop() { - +void IMU::readInput() { // Read incoming control messages if (pc.rxBufferGetCount() >= 2) { if (pc.getc() == '#') { // Start of new control message @@ -182,55 +153,149 @@ #endif // OUTPUT__HAS_RN_BLUETOOTH == true } else { } // Skip character } +} - // Time to read the sensors again? - int now = timer.read_ms(); - if ((now - timestamp) >= OUTPUT__DATA_INTERVAL) { - timestamp_old = timestamp; - timestamp = now; - if (timestamp > timestamp_old) - G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time) - else - G_Dt = 0; +IMU::IMU() + : gyro_num_samples(0) + , yaw(0) + , pitch(0) + , roll(0) + , MAG_Heading(0) + , timestamp(0) + , timestamp_old(0) + , G_Dt(0) + , output_mode(-1) // Select your startup output mode here!// Select your startup output mode here! + , output_stream_on(false) + , output_single_on(true) + , curr_calibration_sensor(0) + , reset_calibration_session_flag(true) + , num_accel_errors(0) + , num_magn_errors(0) + , num_gyro_errors(0) + , output_errors(true) + , statusLed(LED1) + , pc(USBTX, USBRX) + , Wire(p28, p27) { + + accel[0] = accel_min[0] = accel_max[0] = magnetom[0] = magnetom_min[0] = magnetom_max[0] = gyro[0] = gyro_average[0] = 0; + accel[1] = accel_min[1] = accel_max[1] = magnetom[1] = magnetom_min[1] = magnetom_max[1] = gyro[1] = gyro_average[1] = 0; + accel[2] = accel_min[2] = accel_max[2] = magnetom[2] = magnetom_min[2] = magnetom_max[2] = gyro[2] = gyro_average[2] = 0; + + Accel_Vector[0] = Gyro_Vector[0] = Omega_Vector[0] = Omega_P[0] = Omega_I[0] = Omega[0] = errorRollPitch[0] = errorYaw[0] = 0; + Accel_Vector[1] = Gyro_Vector[1] = Omega_Vector[1] = Omega_P[1] = Omega_I[1] = Omega[1] = errorRollPitch[1] = errorYaw[1] = 0; + Accel_Vector[2] = Gyro_Vector[2] = Omega_Vector[2] = Omega_P[2] = Omega_I[2] = Omega[2] = errorRollPitch[2] = errorYaw[2] = 0; - // Update sensor readings - read_sensors(); + DCM_Matrix[0][0] = 1; + DCM_Matrix[0][1] = 0; + DCM_Matrix[0][2] = 0; + DCM_Matrix[1][0] = 0; + DCM_Matrix[1][1] = 1; + DCM_Matrix[1][2] = 0; + DCM_Matrix[2][0] = 0; + DCM_Matrix[2][1] = 0; + DCM_Matrix[2][2] = 1; + + Update_Matrix[0][0] = 0; + Update_Matrix[0][1] = 1; + Update_Matrix[0][2] = 2; + Update_Matrix[1][0] = 3; + Update_Matrix[1][1] = 4; + Update_Matrix[1][2] = 5; + Update_Matrix[2][0] = 6; + Update_Matrix[2][1] = 7; + Update_Matrix[2][2] = 8; + + Temporary_Matrix[0][0] = 0; + Temporary_Matrix[0][1] = 0; + Temporary_Matrix[0][2] = 0; + Temporary_Matrix[1][0] = 0; + Temporary_Matrix[1][1] = 0; + Temporary_Matrix[1][2] = 0; + Temporary_Matrix[2][0] = 0; + Temporary_Matrix[2][1] = 0; + Temporary_Matrix[2][2] = 0; - if (output_mode == OUTPUT__MODE_CALIBRATE_SENSORS) { // We're in calibration mode - check_reset_calibration_session(); // Check if this session needs a reset - if (output_stream_on || output_single_on) - output_calibration(curr_calibration_sensor); - } else if (output_mode == OUTPUT__MODE_SENSORS_TEXT) { - // Apply sensor calibration - compensate_sensor_errors(); + timer.start(); + // Init serial output + pc.baud(OUTPUT__BAUD_RATE); + + // Init status LED + statusLed = 0; + + // Init sensors + wait_ms(50); // Give sensors enough time to start + I2C_Init(); + Accel_Init(); + Magn_Init(); + Gyro_Init(); + + // Read sensors, init DCM algorithm + wait_ms(20); // Give sensors enough time to collect data + reset_sensor_fusion(); + + // Init output +#if (OUTPUT__HAS_RN_BLUETOOTH == true) || (OUTPUT__STARTUP_STREAM_ON == false) + turn_output_stream_off(); +#else + turn_output_stream_on(); +#endif +} - if (output_stream_on || output_single_on) - output_sensors(); - } else if (output_mode == OUTPUT__MODE_ANGLES_TEXT || output_mode == OUTPUT__MODE_ANGLES_BINARY) { - // Apply sensor calibration - compensate_sensor_errors(); +// Main loop +void IMU::loop() { + timestamp_old = timestamp; + timestamp = timer.read_ms(); + if (timestamp > timestamp_old) + G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time) + else + G_Dt = 0; + + // Update sensor readings + read_sensors(); - // Run DCM algorithm - Compass_Heading(); // Calculate magnetic heading - Matrix_update(); - Normalize(); - Drift_correction(); - Euler_angles(); + if (output_mode == OUTPUT__MODE_CALIBRATE_SENSORS) { // We're in calibration mode + check_reset_calibration_session(); // Check if this session needs a reset + if (output_stream_on || output_single_on) + output_calibration(curr_calibration_sensor); + } else if (output_mode == OUTPUT__MODE_SENSORS_TEXT) { + // Apply sensor calibration + compensate_sensor_errors(); - if (output_stream_on || output_single_on) - output_angles(); - } + if (output_stream_on || output_single_on) + output_sensors(); + } else if (output_mode == OUTPUT__MODE_ANGLES_TEXT || output_mode == OUTPUT__MODE_ANGLES_BINARY) { + // Apply sensor calibration + compensate_sensor_errors(); - output_single_on = false; + // Run DCM algorithm + Compass_Heading(); // Calculate magnetic heading + Matrix_update(); + Normalize(); + Drift_correction(); + Euler_angles(); + + if (output_stream_on || output_single_on) + output_angles(); + } + + output_single_on = false; #if DEBUG__PRINT_LOOP_TIME == true - Serial.print("loop time (ms) = "); - Serial.println(millis() - timestamp); -#endif - } -#if DEBUG__PRINT_LOOP_TIME == true - else { - Serial.println("waiting..."); - } + pc.printf("loop time (ms) = %f" NEW_LINE, timer.read_ms() - timestamp); #endif } + +int main() { + IMU imu; + + Ticker looper; + looper.attach(&imu, &IMU::loop, (float)OUTPUT__DATA_INTERVAL / 1000.0f); // 50Hz update + + while (1) + { + imu.readInput(); + wait_ms(5); + } + + return 0; +}