AHRS with RTOS
Dependencies: AHRS mbed-rtos mbed
Fork of RazorAHRS by
Revision 3:c2d895d76e75, committed 2012-11-08
- Comitter:
- tylerjw
- Date:
- Thu Nov 08 18:57:58 2012 +0000
- Parent:
- 2:5aa75c3d8cc3
- Child:
- 4:12eb2834abbd
- Commit message:
- RtosTimer AHRS demo.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AHRS.lib Thu Nov 08 18:57:58 2012 +0000 @@ -0,0 +1,1 @@ +AHRS#014ee3239c80
--- a/Compass.cpp Wed Dec 28 18:09:14 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,25 +0,0 @@ -/* This file is part of the Razor AHRS Firmware */ -#include "Razor_AHRS.h" -#include <math.h> - -void IMU::Compass_Heading() -{ - float mag_x; - float mag_y; - float cos_roll; - float sin_roll; - float cos_pitch; - float sin_pitch; - - cos_roll = cos(roll); - sin_roll = sin(roll); - cos_pitch = cos(pitch); - sin_pitch = sin(pitch); - - // Tilt compensated magnetic field X - mag_x = magnetom[0]*cos_pitch + magnetom[1]*sin_roll*sin_pitch + magnetom[2]*cos_roll*sin_pitch; - // Tilt compensated magnetic field Y - mag_y = magnetom[1]*cos_roll - magnetom[2]*sin_roll; - // Magnetic Heading - MAG_Heading = atan2(-mag_y, mag_x); -}
--- a/DCM.cpp Wed Dec 28 18:09:14 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,129 +0,0 @@ -/* This file is part of the Razor AHRS Firmware */ -#include "Razor_AHRS.h" -#include <math.h> - -// DCM algorithm - -/**************************************************/ -void IMU::Normalize(void) -{ - float error=0; - float temporary[3][3]; - float renorm=0; - - error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19 - - Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19 - Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19 - - Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19 - Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19 - - Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20 - - renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21 - Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); - - renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21 - Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); - - renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21 - Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm); -} - -/**************************************************/ -void IMU::Drift_correction(void) -{ - float mag_heading_x; - float mag_heading_y; - float errorCourse; - //Compensation the Roll, Pitch and Yaw drift. - static float Scaled_Omega_P[3]; - static float Scaled_Omega_I[3]; - float Accel_magnitude; - float Accel_weight; - - - //*****Roll and Pitch*************** - - // Calculate the magnitude of the accelerometer vector - Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]); - Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity. - // Dynamic weighting of accelerometer info (reliability filter) - // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) - Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); // - - Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference - Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight); - - Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight); - Vector_Add(Omega_I,Omega_I,Scaled_Omega_I); - - //*****YAW*************** - // We make the gyro YAW drift correction based on compass magnetic heading - - mag_heading_x = cos(MAG_Heading); - mag_heading_y = sin(MAG_Heading); - errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error - Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. - - Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW. - Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. - - Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator - Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I -} - -void IMU::Matrix_update(void) -{ - Gyro_Vector[0]=GYRO_SCALED_RAD(gyro[0]); //gyro x roll - Gyro_Vector[1]=GYRO_SCALED_RAD(gyro[1]); //gyro y pitch - Gyro_Vector[2]=GYRO_SCALED_RAD(gyro[2]); //gyro z yaw - - Accel_Vector[0]=accel[0]; - Accel_Vector[1]=accel[1]; - Accel_Vector[2]=accel[2]; - - Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term - Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term - -#if DEBUG__NO_DRIFT_CORRECTION == true // Do not use drift correction - Update_Matrix[0][0]=0; - Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z - Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y - Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z - Update_Matrix[1][1]=0; - Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0]; - Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1]; - Update_Matrix[2][1]=G_Dt*Gyro_Vector[0]; - Update_Matrix[2][2]=0; -#else // Use drift correction - Update_Matrix[0][0]=0; - Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z - Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y - Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z - Update_Matrix[1][1]=0; - Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x - Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y - Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x - Update_Matrix[2][2]=0; -#endif - - Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c - - for(int x=0; x<3; x++) //Matrix Addition (update) - { - for(int y=0; y<3; y++) - { - DCM_Matrix[x][y]+=Temporary_Matrix[x][y]; - } - } -} - -void IMU::Euler_angles(void) -{ - pitch = -asin(DCM_Matrix[2][0]); - roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); - yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); -} -
--- a/MODSERIAL.lib Wed Dec 28 18:09:14 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/AjK/code/MODSERIAL/#af2af4c61c2f
--- a/Math.cpp Wed Dec 28 18:09:14 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,93 +0,0 @@ -/* This file is part of the Razor AHRS Firmware */ -#include <math.h> - -// Computes the dot product of two vectors -float Vector_Dot_Product(float vector1[3], float vector2[3]) -{ - float op=0; - - for(int c=0; c<3; c++) - { - op+=vector1[c]*vector2[c]; - } - - return op; -} - -// Computes the cross product of two vectors -void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]) -{ - vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]); - vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]); - vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]); -} - -// Multiply the vector by a scalar. -void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2) -{ - for(int c=0; c<3; c++) - { - vectorOut[c]=vectorIn[c]*scale2; - } -} - -// Adds two vectors -void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]) -{ - for(int c=0; c<3; c++) - { - vectorOut[c]=vectorIn1[c]+vectorIn2[c]; - } -} - -//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). -void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]) -{ - float op[3]; - for(int x=0; x<3; x++) - { - for(int y=0; y<3; y++) - { - for(int w=0; w<3; w++) - { - op[w]=a[x][w]*b[w][y]; - } - mat[x][y]=0; - mat[x][y]=op[0]+op[1]+op[2]; - - float test=mat[x][y]; - } - } -} - -// Init rotation matrix using euler angles -void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll) -{ - float c1 = cos(roll); - float s1 = sin(roll); - float c2 = cos(pitch); - float s2 = sin(pitch); - float c3 = cos(yaw); - float s3 = sin(yaw); - - // Euler angles, right-handed, intrinsic, XYZ convention - // (which means: rotate around body axes Z, Y', X'') - m[0][0] = c2 * c3; - m[0][1] = c3 * s1 * s2 - c1 * s3; - m[0][2] = s1 * s3 + c1 * c3 * s2; - - m[1][0] = c2 * s3; - m[1][1] = c1 * c3 + s1 * s2 * s3; - m[1][2] = c1 * s2 * s3 - c3 * s1; - - m[2][0] = -s2; - m[2][1] = c2 * s1; - m[2][2] = c1 * c2; -} - -float constrain(float in, float min, float max) -{ - in = in > max ? max : in; - in = in < min ? min : in; - return in; -} \ No newline at end of file
--- a/Output.cpp Wed Dec 28 18:09:14 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,73 +0,0 @@ -/* This file is part of the Razor AHRS Firmware */ -#include "Razor_AHRS.h" - -void WriteBytes(Serial& s, char* b, int count) -{ - for ( int i = 0; i < count; ++i ) - s.putc(b[i]); -} - -// Output angles: yaw, pitch, roll -void IMU::output_angles() -{ - if (output_mode == OUTPUT__MODE_ANGLES_BINARY) - { - float ypr[3]; - ypr[0] = TO_DEG(yaw); - ypr[1] = TO_DEG(pitch); - ypr[2] = TO_DEG(roll); - WriteBytes(pc, (char*)ypr, 12); // No new-line - } - else if (output_mode == OUTPUT__MODE_ANGLES_TEXT) - { - pc.printf("#YPR=%f,%f,%f" NEW_LINE,TO_DEG(yaw),TO_DEG(pitch),TO_DEG(roll)); - } -} - -void IMU::output_calibration(int calibration_sensor) -{ - if (calibration_sensor == 0) // Accelerometer - { - // Output MIN/MAX values - pc.printf("accel x,y,z (min/max) = "); - for (int i = 0; i < 3; i++) { - if (accel[i] < accel_min[i]) accel_min[i] = accel[i]; - if (accel[i] > accel_max[i]) accel_max[i] = accel[i]; - pc.printf("%+5i/%+5i%s", accel_min[i], accel_max[i], (i < 2 ? " " : " ")); - } - pc.printf("%c" ,13,10); - } - else if (calibration_sensor == 1) // Magnetometer - { - // Output MIN/MAX values - pc.printf("magn x,y,z (min/max) = "); - for (int i = 0; i < 3; i++) { - if (magnetom[i] < magnetom_min[i]) magnetom_min[i] = magnetom[i]; - if (magnetom[i] > magnetom_max[i]) magnetom_max[i] = magnetom[i]; - pc.printf("%+4i/%+4i%s", magnetom_min[i], magnetom_max[i], (i < 2 ? " " : " ")); - } - pc.printf("%c" ,13,10); - } - else if (calibration_sensor == 2) // Gyroscope - { - // Average gyro values - for (int i = 0; i < 3; i++) - gyro_average[i] += gyro[i]; - gyro_num_samples++; - - // Output current and averaged gyroscope values - pc.printf("gyro x,y,z (current/average) = "); - for (int i = 0; i < 3; i++) { - pc.printf("%+5i/%+5i%s",gyro[i], (int16_t)(gyro_average[i] / gyro_num_samples), (i < 2 ? " " : " ")); - } - pc.printf("%c" ,13,10); - } -} - -void IMU::output_sensors() -{ - pc.printf("#ACC=%+5i %+5i %+5i ", accel[0], accel[1], accel[2]); - pc.printf("#MAG=%+4i %+4i %+4i ", magnetom[0], magnetom[1], magnetom[2]); - pc.printf("#GYR=%+5i %+5i %+5i", gyro[0], gyro[1], gyro[2]); - pc.printf("%c" ,13,10); -}
--- a/Razor_AHRS.cpp Wed Dec 28 18:09:14 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,301 +0,0 @@ -/* This file is part of the Razor AHRS Firmware */ -#include "Razor_AHRS.h" -#include <math.h> - -void IMU::read_sensors() { - Read_Gyro(); // Read gyroscope - Read_Accel(); // Read accelerometer - Read_Magn(); // Read magnetometer -} - -// Read every sensor and record a time stamp -// Init DCM with unfiltered orientation -// TODO re-init global vars? -void IMU::reset_sensor_fusion() { - float temp0[3] = { accel[0], accel[1], accel[2] }; - float temp1[3]; - float temp2[3]; - float xAxis[] = {1.0f, 0.0f, 0.0f}; - - read_sensors(); - timestamp = timer.read_ms(); - - // GET PITCH - // Using y-z-plane-component/x-component of gravity vector - pitch = -atan2(accel[0], sqrt((double)(accel[1] * accel[1] + accel[2] * accel[2]))); - - // 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 - // roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2])); - // Since we compensated for pitch, x-z-plane-component equals z-component: - roll = atan2(temp2[1], temp2[2]); - - // GET YAW - Compass_Heading(); - yaw = MAG_Heading; - - // Init rotation matrix - init_rotation_matrix(DCM_Matrix, yaw, pitch, roll); -} - -// Apply calibration to raw sensor readings -void IMU::compensate_sensor_errors() { - // Compensate accelerometer error - accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE; - accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE; - accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE; - - // Compensate magnetometer error - magnetom[0] = (magnetom[0] - MAGN_X_OFFSET) * MAGN_X_SCALE; - magnetom[1] = (magnetom[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE; - magnetom[2] = (magnetom[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE; - - // Compensate gyroscope error - gyro[0] -= GYRO_AVERAGE_OFFSET_X; - gyro[1] -= GYRO_AVERAGE_OFFSET_Y; - gyro[2] -= GYRO_AVERAGE_OFFSET_Z; -} - -// Reset calibration session if reset_calibration_session_flag is set -void IMU::check_reset_calibration_session() { - // Raw sensor values have to be read already, but no error compensation applied - - // Reset this calibration session? - if (!reset_calibration_session_flag) return; - - // Reset acc and mag calibration variables - for (int i = 0; i < 3; i++) { - accel_min[i] = accel_max[i] = accel[i]; - magnetom_min[i] = magnetom_max[i] = magnetom[i]; - } - - // Reset gyro calibration variables - gyro_num_samples = 0; // Reset gyro calibration averaging - gyro_average[0] = gyro_average[1] = gyro_average[2] = 0; - - reset_calibration_session_flag = false; -} - -void IMU::turn_output_stream_on() { - output_stream_on = true; - statusLed = 1; -} - -void IMU::turn_output_stream_off() { - output_stream_on = false; - statusLed = 0; -} - -// Blocks until another byte is available on serial port -char IMU::readChar() { - while (pc.rxBufferGetCount() < 1) { } // Block - return pc.getc(); -} - -void IMU::readInput() { - // Read incoming control messages - if (pc.rxBufferGetCount() >= 2) { - if (pc.getc() == '#') { // Start of new control message - int command = pc.getc(); // Commands - if (command == 'f') // request one output _f_rame - output_single_on = true; - else if (command == 's') { // _s_ynch request - // Read ID - char id[2]; - id[0] = readChar(); - id[1] = readChar(); - - // Reply with synch message - pc.printf("#SYNCH"); - pc.putc(id[0]); - pc.putc(id[1]); - pc.printf(NEW_LINE); - } else if (command == 'o') { // Set _o_utput mode - char output_param = readChar(); - if (output_param == 'n') { // Calibrate _n_ext sensor - curr_calibration_sensor = (curr_calibration_sensor + 1) % 3; - reset_calibration_session_flag = true; - } else if (output_param == 't') // Output angles as _t_ext - output_mode = OUTPUT__MODE_ANGLES_TEXT; - else if (output_param == 'b') // Output angles in _b_inary form - output_mode = OUTPUT__MODE_ANGLES_BINARY; - else if (output_param == 'c') { // Go to _c_alibration mode - output_mode = OUTPUT__MODE_CALIBRATE_SENSORS; - reset_calibration_session_flag = true; - } else if (output_param == 's') // Output _s_ensor values as text - output_mode = OUTPUT__MODE_SENSORS_TEXT; - else if (output_param == '0') { // Disable continuous streaming output - turn_output_stream_off(); - reset_calibration_session_flag = true; - } else if (output_param == '1') { // Enable continuous streaming output - reset_calibration_session_flag = true; - turn_output_stream_on(); - } else if (output_param == 'e') { // _e_rror output settings - char error_param = readChar(); - if (error_param == '0') output_errors = false; - else if (error_param == '1') output_errors = true; - else if (error_param == 'c') { // get error count - pc.printf("#AMG-ERR:%d,%d,%d" NEW_LINE,num_accel_errors,num_magn_errors,num_gyro_errors); - } - } - } -#if OUTPUT__HAS_RN_BLUETOOTH == true - // Read messages from bluetooth module - // For this to work, the connect/disconnect message prefix of the module has to be set to "#". - else if (command == 'C') // Bluetooth "#CONNECT" message (does the same as "#o1") - turn_output_stream_on(); - else if (command == 'D') // Bluetooth "#DISCONNECT" message (does the same as "#o0") - turn_output_stream_off(); -#endif // OUTPUT__HAS_RN_BLUETOOTH == true - } else { } // Skip character - } -} - -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; - - 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; - - 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() { - 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(); - - 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_sensors(); - } else if (output_mode == OUTPUT__MODE_ANGLES_TEXT || output_mode == OUTPUT__MODE_ANGLES_BINARY) { - // Apply sensor calibration - compensate_sensor_errors(); - - // 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 - 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; -}
--- a/Razor_AHRS.h Wed Dec 28 18:09:14 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,381 +0,0 @@ -/*************************************************************************************************************** -* Razor AHRS Firmware v1.4.0 -* 9 Degree of Measurement Attitude and Heading Reference System -* for Sparkfun "9DOF Razor IMU" (SEN-10125 and SEN-10736) -* and "9DOF Sensor Stick" (SEN-10183, 10321 and SEN-10724) -* -* Released under GNU GPL (General Public License) v3.0 -* Copyright (C) 2011 Quality & Usability Lab, Deutsche Telekom Laboratories, TU Berlin -* -* Infos, updates, bug reports and feedback: -* http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs -* -* -* History: -* * Original code (http://code.google.com/p/sf9domahrs/) by Doug Weibel and Jose Julio, -* based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose Julio and Doug Weibel. Thank you! -* -* * Updated code (http://groups.google.com/group/sf_9dof_ahrs_update) by David Malik (david.zsolt.malik@gmail.com) -* for new Sparkfun 9DOF Razor hardware (SEN-10125). -* -* * Updated and extended by Peter Bartz (peter-bartz@gmx.de): -* * v1.3.0 -* * Cleaned up, streamlined and restructured most of the code to make it more comprehensible. -* * Added sensor calibration (improves precision and responsiveness a lot!). -* * Added binary yaw/pitch/roll output. -* * Added basic serial command interface to set output modes/calibrate sensors/synch stream/etc. -* * Added support to synch automatically when using Rovering Networks Bluetooth modules (and compatible). -* * Wrote new easier to use test program (using Processing). -* * Added support for new version of "9DOF Razor IMU": SEN-10736. -* --> The output of this code is not compatible with the older versions! -* --> A Processing sketch to test the tracker is available. -* * v1.3.1 -* * Initializing rotation matrix based on start-up sensor readings -> orientation OK right away. -* * Adjusted gyro low-pass filter and output rate settings. -* * v1.3.2 -* * Adapted code to work with new Arduino 1.0 (and older versions still). -* * v1.3.3 -* * 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. -* * Use self-test and temperature-compensation features of the sensors. -* * Add binary output of unfused sensor data for all 9 axes. -***************************************************************************************************************/ - -/* - "9DOF Razor IMU" hardware versions: SEN-10125 and SEN-10736 - - ATMega328@3.3V, 8MHz - - ADXL345 : Accelerometer - HMC5843 : Magnetometer on SEN-10125 - HMC5883L : Magnetometer on SEN-10736 - ITG-3200 : Gyro - - Arduino IDE : Select board "Arduino Pro or Pro Mini (3.3v, 8Mhz) w/ATmega328" -*/ - -/* - "9DOF Sensor Stick" hardware versions: SEN-10183, SEN-10321 and SEN-10724 - - ADXL345 : Accelerometer - HMC5843 : Magnetometer on SEN-10183 and SEN-10321 - HMC5883L : Magnetometer on SEN-10724 - ITG-3200 : Gyro -*/ - -/* - Axis definition (differs from definition printed on the board!): - X axis pointing forward (towards the short edge with the connector holes) - Y axis pointing to the right - and Z axis pointing down. - - Positive yaw : clockwise - Positive roll : right wing down - Positive pitch : nose up - - Transformation order: first yaw then pitch then roll. -*/ - -/* - Commands that the firmware understands: - - "#o<param>" - Set output parameter. The available options are: - "#o0" - Disable continuous streaming output. - "#o1" - Enable continuous streaming output. - "#ob" - Output angles in binary format (yaw/pitch/roll as binary float, so one output frame - is 3x4 = 12 bytes long). - "#ot" - Output angles in text format (Output frames have form like "#YPR=-142.28,-5.38,33.52", - followed by carriage return and line feed [\r\n]). - "#os" - Output (calibrated) sensor data of all 9 axes in text format. One frame consist of - three lines - one for each sensor. - "#oc" - Go to calibration output mode. - "#on" - When in calibration mode, go on to calibrate next sensor. - "#oe0" - Disable error message output. - "#oe1" - Enable error message output. - - "#f" - Request one output frame - useful when continuous output is disabled and updates are - required in larger intervals only. - "#s<xy>" - Request synch token - useful to find out where the frame boundaries are in a continuous - binary stream or to see if tracker is present and answering. The tracker will send - "#SYNCH<xy>\r\n" in response (so it's possible to read using a readLine() function). - x and y are two mandatory but arbitrary bytes that can be used to find out which request - the answer belongs to. - - ("#C" and "#D" - Reserved for communication with optional Bluetooth module.) - - Newline characters are not required. So you could send "#ob#o1#s", which - would set binary output mode, enable continuous streaming output and request - a synch token all at once. - - The status LED will be on if streaming output is enabled and off otherwise. - - Byte order of binary output is little-endian: least significant byte comes first. -*/ -#include <mbed.h> -#include <MODSERIAL.h> - - -/*****************************************************************/ -/*********** USER SETUP AREA! Set your options here! *************/ -/*****************************************************************/ - -// HARDWARE OPTIONS -/*****************************************************************/ -// Select your hardware here by uncommenting one line! -//#define HW__VERSION_CODE 10125 // SparkFun "9DOF Razor IMU" version "SEN-10125" (HMC5843 magnetometer) -//#define HW__VERSION_CODE 10736 // SparkFun "9DOF Razor IMU" version "SEN-10736" (HMC5883L magnetometer) -//#define HW__VERSION_CODE 10183 // SparkFun "9DOF Sensor Stick" version "SEN-10183" (HMC5843 magnetometer) -//#define HW__VERSION_CODE 10321 // SparkFun "9DOF Sensor Stick" version "SEN-10321" (HMC5843 magnetometer) -//#define HW__VERSION_CODE 10724 // SparkFun "9DOF Sensor Stick" version "SEN-10724" (HMC5883L magnetometer) - - -// OUTPUT OPTIONS -/*****************************************************************/ -// Set your serial port baud rate used to send out data here! -#define OUTPUT__BAUD_RATE 57600 - -// Sensor data output interval in milliseconds -// This may not work, if faster than 20ms (=50Hz) -// Code is tuned for 20ms, so better leave it like that -#define OUTPUT__DATA_INTERVAL 20 // in milliseconds - -// Output mode -#define OUTPUT__MODE_CALIBRATE_SENSORS 0 // Outputs sensor min/max values as text for manual calibration -#define OUTPUT__MODE_ANGLES_TEXT 1 // Outputs yaw/pitch/roll in degrees as text -#define OUTPUT__MODE_ANGLES_BINARY 2 // Outputs yaw/pitch/roll in degrees as binary float -#define OUTPUT__MODE_SENSORS_TEXT 3 // Outputs (calibrated) sensor values for all 9 axes as text - -// Select if serial continuous streaming output is enabled per default on startup. -#define OUTPUT__STARTUP_STREAM_ON false // true or false - -// Bluetooth -// You can set this to true, if you have a Rovering Networks Bluetooth Module attached. -// The connect/disconnect message prefix of the module has to be set to "#". -// (Refer to manual, it can be set like this: SO,#) -// When using this, streaming output will only be enabled as long as we're connected. That way -// receiver and sender are synchronzed easily just by connecting/disconnecting. -// It is not necessary to set this! It just makes life easier when writing code for -// the receiving side. The Processing test sketch also works without setting this. -// NOTE: When using this, OUTPUT__STARTUP_STREAM_ON has no effect! -#define OUTPUT__HAS_RN_BLUETOOTH false // true or false - - -// SENSOR CALIBRATION -/*****************************************************************/ -// How to calibrate? Read the tutorial at http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs -// Put MIN/MAX and OFFSET readings for your board here! -// Accelerometer -// "accel x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX" -#define ACCEL_X_MIN ((float) -250) -#define ACCEL_X_MAX ((float) 250) -#define ACCEL_Y_MIN ((float) -250) -#define ACCEL_Y_MAX ((float) 250) -#define ACCEL_Z_MIN ((float) -250) -#define ACCEL_Z_MAX ((float) 250) - -// Magnetometer -// "magn x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX" -#define MAGN_X_MIN ((float) -600) -#define MAGN_X_MAX ((float) 600) -#define MAGN_Y_MIN ((float) -600) -#define MAGN_Y_MAX ((float) 600) -#define MAGN_Z_MIN ((float) -600) -#define MAGN_Z_MAX ((float) 600) - -// Gyroscope -// "gyro x,y,z (current/average) = .../OFFSET_X .../OFFSET_Y .../OFFSET_Z -#define GYRO_AVERAGE_OFFSET_X ((float) 0.0) -#define GYRO_AVERAGE_OFFSET_Y ((float) 0.0) -#define GYRO_AVERAGE_OFFSET_Z ((float) 0.0) - -/* -// Calibration example: -// "accel x,y,z (min/max) = -278.00/270.00 -254.00/284.00 -294.00/235.00" -#define ACCEL_X_MIN ((float) -278) -#define ACCEL_X_MAX ((float) 270) -#define ACCEL_Y_MIN ((float) -254) -#define ACCEL_Y_MAX ((float) 284) -#define ACCEL_Z_MIN ((float) -294) -#define ACCEL_Z_MAX ((float) 235) - -// "magn x,y,z (min/max) = -511.00/581.00 -516.00/568.00 -489.00/486.00" -#define MAGN_X_MIN ((float) -511) -#define MAGN_X_MAX ((float) 581) -#define MAGN_Y_MIN ((float) -516) -#define MAGN_Y_MAX ((float) 568) -#define MAGN_Z_MIN ((float) -489) -#define MAGN_Z_MAX ((float) 486) - -//"gyro x,y,z (current/average) = -32.00/-34.82 102.00/100.41 -16.00/-16.38" -#define GYRO_AVERAGE_OFFSET_X ((float) -34.82) -#define GYRO_AVERAGE_OFFSET_Y ((float) 100.41) -#define GYRO_AVERAGE_OFFSET_Z ((float) -16.38) -*/ - - -// DEBUG OPTIONS -/*****************************************************************/ -// When set to true, gyro drift correction will not be applied -#define DEBUG__NO_DRIFT_CORRECTION false -// Print elapsed time after each I/O loop -#define DEBUG__PRINT_LOOP_TIME false - - -/*****************************************************************/ -/****************** END OF USER SETUP AREA! *********************/ -/*****************************************************************/ - - -// Check if hardware version code is defined -#ifndef HW__VERSION_CODE - // Generate compile error - #error YOU HAVE TO SELECT THE HARDWARE YOU ARE USING! See "HARDWARE OPTIONS" in "USER SETUP AREA" at top of Razor_AHRS.pde! -#endif - -//#include <Wire.h> - -// Sensor calibration scale and offset values -#define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f) -#define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f) -#define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f) -#define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET)) -#define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET)) -#define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET)) - -#define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f) -#define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f) -#define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f) -#define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET)) -#define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET)) -#define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET)) - - -// Gain for gyroscope (ITG-3200) -#define GYRO_GAIN 0.06957 // Same gain on all axes -#define GYRO_SCALED_RAD(x) (x * TO_RAD(GYRO_GAIN)) // Calculate the scaled gyro readings in radians per second - -// DCM parameters -#define Kp_ROLLPITCH 0.02f -#define Ki_ROLLPITCH 0.00002f -#define Kp_YAW 1.2f -#define Ki_YAW 0.00002f - -// Stuff -#define GRAVITY 256.0f // "1G reference" used for DCM filter and accelerometer calibration -#define TO_RAD(x) (x * 0.01745329252) // *pi/180 -#define TO_DEG(x) (x * 57.2957795131) // *180/pi -#define NEW_LINE "\r\n" - -class IMU { -public: - // Sensor variables - int16_t accel[3]; // Actually stores the NEGATED acceleration (equals gravity, if board not moving). - int16_t accel_min[3]; - int16_t accel_max[3]; - - int16_t magnetom[3]; - int16_t magnetom_min[3]; - int16_t magnetom_max[3]; - - int16_t gyro[3]; - int16_t gyro_average[3]; - int gyro_num_samples; - - // Euler angles - float yaw; - float pitch; - float roll; - - // DCM variables - float MAG_Heading; - float Accel_Vector[3]; // Store the acceleration in a vector - float Gyro_Vector[3]; // Store the gyros turn rate in a vector - float Omega_Vector[3]; // Corrected Gyro_Vector data - float Omega_P[3];//= {0, 0, 0}; // Omega Proportional correction - float Omega_I[3];//= {0, 0, 0}; // Omega Integrator - float Omega[3];//= {0, 0, 0}; - float errorRollPitch[3];// = {0, 0, 0}; - float errorYaw[3];// = {0, 0, 0}; - float DCM_Matrix[3][3];// = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; - float Update_Matrix[3][3];// = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}}; - float Temporary_Matrix[3][3];// = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - - // DCM timing in the main loop - long timestamp; - long timestamp_old; - float G_Dt; // Integration time for DCM algorithm - - // More output-state variables - int output_mode; - bool output_stream_on; - bool output_single_on; - int curr_calibration_sensor; - bool reset_calibration_session_flag; - int num_accel_errors; - int num_magn_errors; - int num_gyro_errors; - - // If set true, an error message will be output if we fail to read sensor data. - // Message format: "!ERR: reading <sensor>", followed by "\r\n". - bool output_errors; - - DigitalOut statusLed; - MODSERIAL pc; - I2C Wire; - Timer timer; - -public: - // Compass.cpp - void Compass_Heading(); - - // DCM.cpp - void Normalize(); - void Drift_correction(); - void Matrix_update(); - void Euler_angles(); - - // Output.cpp - void output_angles(); - void output_calibration(int calibration_sensor); - void output_sensors(); - - - // Razor_AHRS.cpp - void read_sensors(); - void reset_sensor_fusion(); - void compensate_sensor_errors(); - void check_reset_calibration_session(); - void turn_output_stream_on(); - void turn_output_stream_off(); - char readChar(); - void readInput(); - - IMU(); - void loop(); - - - // Sensors.cpp - void I2C_Init(); - void Accel_Init(); - void Read_Accel(); - void Magn_Init(); - void Read_Magn(); - void Gyro_Init(); - void Read_Gyro(); - -}; - -float Vector_Dot_Product(float vector1[3], float vector2[3]); -void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]); -void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2); -void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]); -void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]); -void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll); -float constrain(float in, float min, float max); -
--- a/Sensors.cpp Wed Dec 28 18:09:14 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,152 +0,0 @@ -/* This file is part of the Razor AHRS Firmware */ -#include "Razor_AHRS.h" - -// I2C code to read the sensors - -// Sensor I2C addresses -#define ACCEL_READ 0xA7 -#define ACCEL_WRITE 0xA6 -#define MAGN_WRITE 0x3C -#define MAGN_READ 0x3D -#define GYRO_WRITE 0xD0 -#define GYRO_READ 0xD1 - -void IMU::I2C_Init() { - Wire.frequency(100000); -} - -void IMU::Accel_Init() { - char tx[2]; - - tx[0] = 0x2D; // Power register - tx[1] = 0x08; // Power register - Wire.write(ACCEL_WRITE, tx, 2); - wait_ms(5); - - tx[0] = 0x31; // Data format register - tx[1] = 0x08; // Set to full resolution - Wire.write(ACCEL_WRITE, tx, 2); - wait_ms(5); - - // Because our main loop runs at 50Hz we adjust the output data rate to 50Hz (25Hz bandwidth) - tx[0] = 0x2C; // Rate - tx[1] = 0x09; // Set to 50Hz, normal operation - Wire.write(ACCEL_WRITE, tx, 2); - wait_ms(5); -} - -// Reads x, y and z accelerometer registers -void IMU::Read_Accel() { - char buff[6]; - char tx = 0x32; // Send address to read from - - Wire.write(ACCEL_WRITE, &tx, 1); - - if (Wire.read(ACCEL_READ, buff, 6) == 0) { // All bytes received? - // No multiply by -1 for coordinate system transformation here, because of double negation: - // We want the gravity vector, which is negated acceleration vector. - accel[0] = (int)buff[3] << 8 | (int)buff[2]; // X axis (internal sensor y axis) - accel[1] = (int)buff[1] << 8 | (int)buff[0]; // Y axis (internal sensor x axis) - accel[2] = (int)buff[5] << 8 | (int)buff[4]; // Z axis (internal sensor z axis) - } else { - num_accel_errors++; - if (output_errors) pc.printf("!ERR: reading accelerometer" NEW_LINE); - } -} - -void IMU::Magn_Init() { - char tx[2]; - tx[0] = 0x02; // Mode - tx[1] = 0x00; // Set continuous mode (default 10Hz) - Wire.write(MAGN_WRITE, tx, 2); - wait_ms(5); - - tx[0] = 0x00; // CONFIG_A - tx[1] = 0x18; // Set 50Hz - Wire.write(MAGN_WRITE, tx, 2); - wait_ms(5); -} - -void IMU::Read_Magn() { - char buff[6]; - char tx = 0x03; // Send address to read from - - Wire.write(MAGN_WRITE, &tx, 1); - - if (Wire.read(MAGN_READ, buff, 6) == 0) { // All bytes received? -// 9DOF Razor IMU SEN-10125 using HMC5843 magnetometer -#if HW__VERSION_CODE == 10125 - // MSB byte first, then LSB; X, Y, Z - magnetom[0] = -1 * (((int) buff[2]) << 8) | buff[3]; // X axis (internal sensor -y axis) - magnetom[1] = -1 * (((int) buff[0]) << 8) | buff[1]; // Y axis (internal sensor -x axis) - magnetom[2] = -1 * (((int) buff[4]) << 8) | buff[5]; // Z axis (internal sensor -z axis) -// 9DOF Razor IMU SEN-10736 using HMC5883L magnetometer -#elif HW__VERSION_CODE == 10736 - // MSB byte first, then LSB; Y and Z reversed: X, Z, Y - magnetom[0] = -1 * (((int) buff[4]) << 8) | buff[5]; // X axis (internal sensor -y axis) - magnetom[1] = -1 * (((int) buff[0]) << 8) | buff[1]; // Y axis (internal sensor -x axis) - magnetom[2] = -1 * (((int) buff[2]) << 8) | buff[3]; // Z axis (internal sensor -z axis) -// 9DOF Sensor Stick SEN-10183 and SEN-10321 using HMC5843 magnetometer -#elif (HW__VERSION_CODE == 10183) || (HW__VERSION_CODE == 10321) - // MSB byte first, then LSB; X, Y, Z - magnetom[0] = (((int) buff[0]) << 8) | buff[1]; // X axis (internal sensor x axis) - magnetom[1] = -1 * (((int) buff[2]) << 8) | buff[3]; // Y axis (internal sensor -y axis) - magnetom[2] = -1 * (((int) buff[4]) << 8) | buff[5]; // Z axis (internal sensor -z axis) -// 9DOF Sensor Stick SEN-10724 using HMC5883L magnetometer -#elif HW__VERSION_CODE == 10724 - // MSB byte first, then LSB; Y and Z reversed: X, Z, Y - magnetom[0] = 1 * ((int)buff[0] << 8 | (int)buff[1]); // X axis (internal sensor x axis) - magnetom[1] = -1 * ((int)buff[4] << 8 | (int)buff[5]); // Y axis (internal sensor -y axis) - magnetom[2] = -1 * ((int)buff[2] << 8 | (int)buff[3]); // Z axis (internal sensor -z axis) -#endif - } else { - num_magn_errors++; - if (output_errors) pc.printf("!ERR: reading magnetometer" NEW_LINE); - } -} - -void IMU::Gyro_Init() { - char tx[2]; - - // Power up reset defaults - tx[0] = 0x3E; // Power management - tx[1] = 0x80; // ? - Wire.write(GYRO_WRITE, tx, 2); - wait_ms(5); - - // Select full-scale range of the gyro sensors - // Set LP filter bandwidth to 42Hz - tx[0] = 0x16; // - tx[1] = 0x1B; // DLPF_CFG = 3, FS_SEL = 3 - Wire.write(GYRO_WRITE, tx, 2); - wait_ms(5); - - // Set sample rato to 50Hz - tx[0] = 0x15; // - tx[1] = 0x0A; // SMPLRT_DIV = 10 (50Hz) - Wire.write(GYRO_WRITE, tx, 2); - wait_ms(5); - - // Set clock to PLL with z gyro reference - tx[0] = 0x3E; - tx[1] = 0x00; - Wire.write(GYRO_WRITE, tx, 2); - wait_ms(5); -} - -// Reads x, y and z gyroscope registers -void IMU::Read_Gyro() { - char buff[6]; - char tx = 0x1D; // Sends address to read from - - Wire.write(GYRO_WRITE, &tx, 1); - - if (Wire.read(GYRO_READ, buff, 6) == 0) { // All bytes received? - gyro[0] = -1 * ((int)buff[2] << 8 | (int)buff[3]); // X axis (internal sensor -y axis) - gyro[1] = -1 * ((int)buff[0] << 8 | (int)buff[1]); // Y axis (internal sensor -x axis) - gyro[2] = -1 * ((int)buff[4] << 8 | (int)buff[5]); // Z axis (internal sensor -z axis) - } else { - num_gyro_errors++; - if (output_errors) pc.printf("!ERR: reading gyroscope" NEW_LINE); - } -}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Nov 08 18:57:58 2012 +0000 @@ -0,0 +1,23 @@ +#include "mbed.h" +#include "AHRS.h" +#include "rtos.h" + +IMU imu; +Serial pc(USBTX, USBRX); + +void imu_thread(void const *arg) +{ + float data[4]; + imu.loop(data); + pc.printf("#YPR=%f,%f,%f\t%f\r\n", data[1],data[2],data[3],data[0]); +} + +int main() +{ + pc.baud(9600); + + RtosTimer ahrs_timer(imu_thread, osTimerPeriodic, NULL); + ahrs_timer.start(20); + + while (1); +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Thu Nov 08 18:57:58 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#9654a71f5a90
--- a/mbed.bld Wed Dec 28 18:09:14 2011 +0000 +++ b/mbed.bld Thu Nov 08 18:57:58 2012 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/078e4b97a13e +http://mbed.org/users/mbed_official/code/mbed/builds/e2ed12d17f06 \ No newline at end of file