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.h
- Revision:
- 2:5aa75c3d8cc3
- Parent:
- 1:e27c4c0b71d8
diff -r e27c4c0b71d8 -r 5aa75c3d8cc3 Razor_AHRS.h --- a/Razor_AHRS.h Wed Dec 28 17:13:14 2011 +0000 +++ b/Razor_AHRS.h Wed Dec 28 18:09:14 2011 +0000 @@ -38,6 +38,8 @@ * * Improved synching. * * v1.4.0 * * Added support for SparkFun "9DOF Sensor Stick" (versions SEN-10183, SEN-10321 and SEN-10724). +* * Ported (v1.4.0) to MBED.org by Luke Petre (lpetre@gmail.com) +* http://mbed.org/users/lpetre/programs/RazorAHRS/ * * TODOs: * * Allow optional use of EEPROM for storing and reading calibration values. @@ -328,50 +330,7 @@ I2C Wire; Timer timer; -public: - 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; - - 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; - } - +public: // Compass.cpp void Compass_Heading(); @@ -395,7 +354,9 @@ void turn_output_stream_on(); void turn_output_stream_off(); char readChar(); - void setup(); + void readInput(); + + IMU(); void loop();