diff --git a/Arduino/Razor_AHRS/Output.ino b/Arduino/Razor_AHRS/Output.ino index 0780082..49de43e 100644 --- a/Arduino/Razor_AHRS/Output.ino +++ b/Arduino/Razor_AHRS/Output.ino @@ -96,14 +96,14 @@ void output_sensors_binary() void output_sensors() { - if (output_mode == OUTPUT__MODE_SENSORS_RAW) + if ((output_mode == OUTPUT__MODE_SENSORS_RAW)||(output_mode == OUTPUT__MODE_ANGLES_SENSORS_RAW)) { if (output_format == OUTPUT__FORMAT_BINARY) output_sensors_binary(); else if (output_format == OUTPUT__FORMAT_TEXT) output_sensors_text('R'); } - else if (output_mode == OUTPUT__MODE_SENSORS_CALIB) + else if ((output_mode == OUTPUT__MODE_SENSORS_CALIB)||(output_mode == OUTPUT__MODE_ANGLES_SENSORS_CALIB)) { // Apply sensor calibration compensate_sensor_errors(); diff --git a/Arduino/Razor_AHRS/Razor_AHRS.ino b/Arduino/Razor_AHRS/Razor_AHRS.ino index 2fa807a..b853816 100644 --- a/Arduino/Razor_AHRS/Razor_AHRS.ino +++ b/Arduino/Razor_AHRS/Razor_AHRS.ino @@ -1,152 +1,161 @@ /*************************************************************************************************************** -* Razor AHRS Firmware v1.4.2 -* 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) 2013 Peter Bartz [http://ptrbrtz.net] -* Copyright (C) 2011-2012 Quality & Usability Lab, Deutsche Telekom Laboratories, TU Berlin -* -* Infos, updates, bug reports, contributions and feedback: -* https://github.com/ptrbrtz/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). -* * v1.4.1 -* * Added output modes to read raw and/or calibrated sensor data in text or binary format. -* * Added static magnetometer soft iron distortion compensation -* * v1.4.2 -* * (No core firmware changes) -* -* TODOs: -* * Allow optional use of EEPROM for storing and reading calibration values. -* * Use self-test and temperature-compensation features of the sensors. -***************************************************************************************************************/ + * Razor AHRS Firmware v1.4.2 + * 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) 2013 Peter Bartz [http://ptrbrtz.net] + * Copyright (C) 2011-2012 Quality & Usability Lab, Deutsche Telekom Laboratories, TU Berlin + * + * Infos, updates, bug reports, contributions and feedback: + * https://github.com/ptrbrtz/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). + * * v1.4.1 + * * Added output modes to read raw and/or calibrated sensor data in text or binary format. + * * Added static magnetometer soft iron distortion compensation + * * v1.4.2 + * * (No core firmware changes) + * + * TODOs: + * * Allow optional use of EEPROM for storing and reading calibration values. + * * Use self-test and temperature-compensation features of the sensors. + ***************************************************************************************************************/ /* "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" -*/ + + 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 -*/ + + 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. -*/ + 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. + */ /* Serial commands that the firmware understands: - - "#o" - Set OUTPUT mode and parameters. The available options are: - - // Streaming output - "#o0" - DISABLE continuous streaming output. Also see #f below. - "#o1" - ENABLE continuous streaming output. - - // Angles 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]). - - // Sensor calibration - "#oc" - Go to CALIBRATION output mode. - "#on" - When in calibration mode, go on to calibrate NEXT sensor. - - // Sensor data output - "#osct" - Output CALIBRATED SENSOR data of all 9 axes in TEXT format. - One frame consist of three lines - one for each sensor: acc, mag, gyr. - "#osrt" - Output RAW SENSOR data of all 9 axes in TEXT format. - One frame consist of three lines - one for each sensor: acc, mag, gyr. - "#osbt" - Output BOTH raw and calibrated SENSOR data of all 9 axes in TEXT format. - One frame consist of six lines - like #osrt and #osct combined (first RAW, then CALIBRATED). - NOTE: This is a lot of number-to-text conversion work for the little 8MHz chip on the Razor boards. - In fact it's too much and an output frame rate of 50Hz can not be maintained. #osbb. - "#oscb" - Output CALIBRATED SENSOR data of all 9 axes in BINARY format. - One frame consist of three 3x3 float values = 36 bytes. Order is: acc x/y/z, mag x/y/z, gyr x/y/z. - "#osrb" - Output RAW SENSOR data of all 9 axes in BINARY format. - One frame consist of three 3x3 float values = 36 bytes. Order is: acc x/y/z, mag x/y/z, gyr x/y/z. - "#osbb" - Output BOTH raw and calibrated SENSOR data of all 9 axes in BINARY format. - One frame consist of 2x36 = 72 bytes - like #osrb and #oscb combined (first RAW, then CALIBRATED). - - // Error message output - "#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. Though #f only requests one reply, replies are still - bound to the internal 20ms (50Hz) time raster. So worst case delay that #f can add is 19.99ms. - - - "#s" - 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\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. -*/ + + "#o" - Set OUTPUT mode and parameters. The available options are: + + // Streaming output + "#o0" - DISABLE continuous streaming output. Also see #f below. + "#o1" - ENABLE continuous streaming output. + + // Angles 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]). + + // Angles output and sensor data + "#omrb" - Output angles and raw sensor data in BINARY format (yaw/pitch/roll/acc x/y/z/mag x/y/z/gyr x/y/z as binary float, so one output frame + is 12x4 = 48 bytes long). + "#omcb" - Output angles and calibrated sensor data in BINARY format (yaw/pitch/roll/acc x/y/z/mag x/y/z/gyr x/y/z as binary float, so one output frame + is 12x4 = 48 bytes long). + "#omrt" - Output angles and raw sensor data in TEXT format + "#omct" - Output angles and calibrated sensor data in TEXT format + + + // Sensor calibration + "#oc" - Go to CALIBRATION output mode. + "#on" - When in calibration mode, go on to calibrate NEXT sensor. + + // Sensor data output + "#osct" - Output CALIBRATED SENSOR data of all 9 axes in TEXT format. + One frame consist of three lines - one for each sensor: acc, mag, gyr. + "#osrt" - Output RAW SENSOR data of all 9 axes in TEXT format. + One frame consist of three lines - one for each sensor: acc, mag, gyr. + "#osbt" - Output BOTH raw and calibrated SENSOR data of all 9 axes in TEXT format. + One frame consist of six lines - like #osrt and #osct combined (first RAW, then CALIBRATED). + NOTE: This is a lot of number-to-text conversion work for the little 8MHz chip on the Razor boards. + In fact it's too much and an output frame rate of 50Hz can not be maintained. #osbb. + "#oscb" - Output CALIBRATED SENSOR data of all 9 axes in BINARY format. + One frame consist of three 3x3 float values = 36 bytes. Order is: acc x/y/z, mag x/y/z, gyr x/y/z. + "#osrb" - Output RAW SENSOR data of all 9 axes in BINARY format. + One frame consist of three 3x3 float values = 36 bytes. Order is: acc x/y/z, mag x/y/z, gyr x/y/z. + "#osbb" - Output BOTH raw and calibrated SENSOR data of all 9 axes in BINARY format. + One frame consist of 2x36 = 72 bytes - like #osrb and #oscb combined (first RAW, then CALIBRATED). + + // Error message output + "#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. Though #f only requests one reply, replies are still + bound to the internal 20ms (50Hz) time raster. So worst case delay that #f can add is 19.99ms. + + + "#s" - 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\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. + */ @@ -157,7 +166,7 @@ // 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 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) @@ -180,6 +189,8 @@ #define OUTPUT__MODE_SENSORS_CALIB 2 // Outputs calibrated sensor values for all 9 axes #define OUTPUT__MODE_SENSORS_RAW 3 // Outputs raw (uncalibrated) sensor values for all 9 axes #define OUTPUT__MODE_SENSORS_BOTH 4 // Outputs calibrated AND raw sensor values for all 9 axes +#define OUTPUT__MODE_ANGLES_SENSORS_RAW 5 +#define OUTPUT__MODE_ANGLES_SENSORS_CALIB 6 // Output format definitions (do not change) #define OUTPUT__FORMAT_TEXT 0 // Outputs data as text #define OUTPUT__FORMAT_BINARY 1 // Outputs data as binary float @@ -243,38 +254,38 @@ boolean output_errors = false; // true or false /* // Calibration example: - -// "accel x,y,z (min/max) = -277.00/264.00 -256.00/278.00 -299.00/235.00" -#define ACCEL_X_MIN ((float) -277) -#define ACCEL_X_MAX ((float) 264) -#define ACCEL_Y_MIN ((float) -256) -#define ACCEL_Y_MAX ((float) 278) -#define ACCEL_Z_MIN ((float) -299) -#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) - -// Extended magn -#define CALIBRATION__MAGN_USE_EXTENDED true -const float magn_ellipsoid_center[3] = {91.5, -13.5, -48.1}; -const float magn_ellipsoid_transform[3][3] = {{0.902, -0.00354, 0.000636}, {-0.00354, 0.9, -0.00599}, {0.000636, -0.00599, 1}}; - -// Extended magn (with Sennheiser HD 485 headphones) -//#define CALIBRATION__MAGN_USE_EXTENDED true -//const float magn_ellipsoid_center[3] = {72.3360, 23.0954, 53.6261}; -//const float magn_ellipsoid_transform[3][3] = {{0.879685, 0.000540833, -0.0106054}, {0.000540833, 0.891086, -0.0130338}, {-0.0106054, -0.0130338, 0.997494}}; - -//"gyro x,y,z (current/average) = -40.00/-42.05 98.00/96.20 -18.00/-18.36" -#define GYRO_AVERAGE_OFFSET_X ((float) -42.05) -#define GYRO_AVERAGE_OFFSET_Y ((float) 96.20) -#define GYRO_AVERAGE_OFFSET_Z ((float) -18.36) -*/ + + // "accel x,y,z (min/max) = -277.00/264.00 -256.00/278.00 -299.00/235.00" + #define ACCEL_X_MIN ((float) -277) + #define ACCEL_X_MAX ((float) 264) + #define ACCEL_Y_MIN ((float) -256) + #define ACCEL_Y_MAX ((float) 278) + #define ACCEL_Z_MIN ((float) -299) + #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) + + // Extended magn + #define CALIBRATION__MAGN_USE_EXTENDED true + const float magn_ellipsoid_center[3] = {91.5, -13.5, -48.1}; + const float magn_ellipsoid_transform[3][3] = {{0.902, -0.00354, 0.000636}, {-0.00354, 0.9, -0.00599}, {0.000636, -0.00599, 1}}; + + // Extended magn (with Sennheiser HD 485 headphones) + //#define CALIBRATION__MAGN_USE_EXTENDED true + //const float magn_ellipsoid_center[3] = {72.3360, 23.0954, 53.6261}; + //const float magn_ellipsoid_transform[3][3] = {{0.879685, 0.000540833, -0.0106054}, {0.000540833, 0.891086, -0.0130338}, {-0.0106054, -0.0130338, 0.997494}}; + + //"gyro x,y,z (current/average) = -40.00/-42.05 98.00/96.20 -18.00/-18.36" + #define GYRO_AVERAGE_OFFSET_X ((float) -42.05) + #define GYRO_AVERAGE_OFFSET_Y ((float) 96.20) + #define GYRO_AVERAGE_OFFSET_Z ((float) -18.36) + */ // DEBUG OPTIONS @@ -300,8 +311,8 @@ const float magn_ellipsoid_transform[3][3] = {{0.902, -0.00354, 0.000636}, {-0.0 // 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.ino! +// 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.ino! #endif #include @@ -354,17 +365,46 @@ int gyro_num_samples = 0; // DCM variables float MAG_Heading; -float Accel_Vector[3]= {0, 0, 0}; // Store the acceleration in a vector -float Gyro_Vector[3]= {0, 0, 0}; // Store the gyros turn rate in a vector -float Omega_Vector[3]= {0, 0, 0}; // 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}}; +float Accel_Vector[3]= { + 0, 0, 0}; // Store the acceleration in a vector +float Gyro_Vector[3]= { + 0, 0, 0}; // Store the gyros turn rate in a vector +float Omega_Vector[3]= { + 0, 0, 0}; // 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 } +}; // Euler angles float yaw; @@ -397,15 +437,16 @@ void read_sensors() { void reset_sensor_fusion() { float temp1[3]; float temp2[3]; - float xAxis[] = {1.0f, 0.0f, 0.0f}; + float xAxis[] = { + 1.0f, 0.0f, 0.0f }; read_sensors(); timestamp = millis(); - + // GET PITCH // Using y-z-plane-component/x-component of gravity vector pitch = -atan2(accel[0], sqrt(accel[1] * accel[1] + accel[2] * accel[2])); - + // GET ROLL // Compensate pitch of gravity vector Vector_Cross_Product(temp1, accel, xAxis); @@ -414,37 +455,37 @@ void reset_sensor_fusion() { // 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 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 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 + // Compensate magnetometer error #if CALIBRATION__MAGN_USE_EXTENDED == true - for (int i = 0; i < 3; i++) - magnetom_tmp[i] = magnetom[i] - magn_ellipsoid_center[i]; - Matrix_Vector_Multiply(magn_ellipsoid_transform, magnetom_tmp, magnetom); + for (int i = 0; i < 3; i++) + magnetom_tmp[i] = magnetom[i] - magn_ellipsoid_center[i]; + Matrix_Vector_Multiply(magn_ellipsoid_transform, magnetom_tmp, magnetom); #else - 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; + 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; #endif - // Compensate gyroscope error - gyro[0] -= GYRO_AVERAGE_OFFSET_X; - gyro[1] -= GYRO_AVERAGE_OFFSET_Y; - gyro[2] -= GYRO_AVERAGE_OFFSET_Z; + // 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 @@ -454,7 +495,7 @@ void check_reset_calibration_session() // 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]; @@ -464,7 +505,7 @@ void check_reset_calibration_session() // Reset gyro calibration variables gyro_num_samples = 0; // Reset gyro calibration averaging gyro_average[0] = gyro_average[1] = gyro_average[2] = 0.0f; - + reset_calibration_session_flag = false; } @@ -483,7 +524,8 @@ void turn_output_stream_off() // Blocks until another byte is available on serial port char readChar() { - while (Serial.available() < 1) { } // Block + while (Serial.available() < 1) { + } // Block return Serial.read(); } @@ -491,7 +533,7 @@ void setup() { // Init serial output Serial.begin(OUTPUT__BAUD_RATE); - + // Init status LED pinMode (STATUS_LED_PIN, OUTPUT); digitalWrite(STATUS_LED_PIN, LOW); @@ -502,7 +544,7 @@ void setup() Accel_Init(); Magn_Init(); Gyro_Init(); - + // Read sensors, init DCM algorithm delay(20); // Give sensors enough time to collect data reset_sensor_fusion(); @@ -532,7 +574,7 @@ void loop() byte id[2]; id[0] = readChar(); id[1] = readChar(); - + // Reply with synch message Serial.print("#SYNCH"); Serial.write(id, 2); @@ -561,6 +603,21 @@ void loop() output_mode = OUTPUT__MODE_CALIBRATE_SENSORS; reset_calibration_session_flag = true; } + else if (output_param == 'm') // Output angles and sensor values. + { + char values_param = readChar(); + char format_param = readChar(); + + if (format_param == 't') // Output values as _t_text + output_format = OUTPUT__FORMAT_TEXT; + else if (format_param == 'b') // Output values in _b_inary format + output_format = OUTPUT__FORMAT_BINARY; + + if (values_param == 'r') // Output _r_aw sensor values + output_mode = OUTPUT__MODE_ANGLES_SENSORS_RAW; + else if (values_param == 'c') // Output _c_alibrated sensor values + output_mode = OUTPUT__MODE_ANGLES_SENSORS_CALIB; + } else if (output_param == 's') // Output _s_ensor values { char values_param = readChar(); @@ -595,8 +652,10 @@ void loop() else if (error_param == 'c') // get error count { Serial.print("#AMG-ERR:"); - Serial.print(num_accel_errors); Serial.print(","); - Serial.print(num_magn_errors); Serial.print(","); + Serial.print(num_accel_errors); + Serial.print(","); + Serial.print(num_magn_errors); + Serial.print(","); Serial.println(num_gyro_errors); } } @@ -611,7 +670,8 @@ void loop() #endif // OUTPUT__HAS_RN_BLUETOOTH == true } else - { } // Skip character + { + } // Skip character } // Time to read the sensors again? @@ -635,23 +695,54 @@ void loop() { // 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(); } + else if (output_mode == OUTPUT__MODE_ANGLES_SENSORS_CALIB) + { + // 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_sensors(); + } + } + else if (output_mode == OUTPUT__MODE_ANGLES_SENSORS_RAW) + { + // 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_sensors(); + } + } else // Output sensor values { if (output_stream_on || output_single_on) output_sensors(); } - + output_single_on = false; - + #if DEBUG__PRINT_LOOP_TIME == true Serial.print("loop time (ms) = "); Serial.println(millis() - timestamp); @@ -664,3 +755,4 @@ void loop() } #endif } + diff --git a/C++/Example.cpp b/C++/Example.cpp index 7da7caf..045cecb 100644 --- a/C++/Example.cpp +++ b/C++/Example.cpp @@ -23,8 +23,8 @@ using namespace std; // Set your serial port here! //const string serial_port_name = "/dev/tty.FireFly-6162-SPP"; -const string serial_port_name = "/dev/tty.usbserial-A700eEhN"; -//const string serial_port_name = "/dev/ttyUSB0"; // a good guess on linux +//const string serial_port_name = "/dev/tty.usbserial-A700eEhN"; +const string serial_port_name = "/dev/ttyUSB0"; // a good guess on linux // Razor error callback handler @@ -43,8 +43,8 @@ void on_error(const string &msg) // holds 3 float values: yaw, pitch and roll. void on_data(const float data[]) { - cout << " " << fixed << setprecision(1) - << "Yaw = " << setw(6) << data[0] << " Pitch = " << setw(6) << data[1] << " Roll = " << setw(6) << data[2] << endl; + //cout << " " << fixed << setprecision(1) + //<< "Yaw = " << setw(6) << data[0] << " Pitch = " << setw(6) << data[1] << " Roll = " << setw(6) << data[2] << endl; // NOTE: make a copy of the yaw/pitch/roll data if you want to save it or send it to another // thread. Do not save or pass the pointer itself, it will not be valid after this function @@ -58,6 +58,14 @@ void on_data(const float data[]) // << " MAG = " << setw(7) << data[3] << ", " << setw(7) << data[4] << ", " << setw(7) << data[5] // << " GYR = " << setw(7) << data[6] << ", " << setw(7) << data[7] << ", " << setw(7) << data[8] << endl; + // If you created the Razor object using RazorAHRS::YAW_PITCH_ROLL_ACC_MAG_GYR_CALIBRATED or RazorAHRS::YAW_PITCH_ROLL_ACC_MAG_GYR_RAW + // instead of RazorAHRS::YAW_PITCH_ROLL, 'data' would contain 12 values that could be printed like this: + + cout << " " << fixed << setprecision(1) + << "Yaw = " << setw(6) << data[0] << " Pitch = " << setw(6) << data[1] << " Roll = " << setw(6) << data[2] + << " ACC = " << setw(6) << data[3] << ", " << setw(6) << data[4] << ", " << setw(6) << data[5] + << " MAG = " << setw(7) << data[6] << ", " << setw(7) << data[7] << ", " << setw(7) << data[8] + << " GYR = " << setw(7) << data[9] << ", " << setw(7) << data[10] << ", " << setw(7) << data[11] << endl; } RazorAHRS *razor; @@ -76,7 +84,7 @@ int main() // We want to receive yaw/pitch/roll data. If we wanted the unprocessed raw or calibrated sensor // data, we would pass RazorAHRS::ACC_MAG_GYR_RAW or RazorAHRS::ACC_MAG_GYR_CALIBRATED // instead of RazorAHRS::YAW_PITCH_ROLL. - razor = new RazorAHRS(serial_port_name, on_data, on_error, RazorAHRS::YAW_PITCH_ROLL); + razor = new RazorAHRS(serial_port_name, on_data, on_error, RazorAHRS::YAW_PITCH_ROLL_ACC_MAG_GYR_CALIBRATED); // NOTE: If these callback functions were members of a class and not global // functions, you would have to bind them before passing. Like this: diff --git a/C++/RazorAHRS.cpp b/C++/RazorAHRS.cpp index 42b8d2e..26e655c 100644 --- a/C++/RazorAHRS.cpp +++ b/C++/RazorAHRS.cpp @@ -183,6 +183,8 @@ RazorAHRS::_init_razor() if (_mode == YAW_PITCH_ROLL) config = "#ob" + config; else if (_mode == ACC_MAG_GYR_RAW) config = "#osrb" + config; else if (_mode == ACC_MAG_GYR_CALIBRATED) config = "#oscb" + config; + else if (_mode == YAW_PITCH_ROLL_ACC_MAG_GYR_RAW) config = "#omrb" + config; + else if (_mode == YAW_PITCH_ROLL_ACC_MAG_GYR_CALIBRATED) config = "#omcb" + config; else throw std::runtime_error("Can not init: unknown 'mode' parameter."); write(_serial_port, config.data(), config.length()); @@ -309,6 +311,20 @@ RazorAHRS::_thread(void *arg) // invoke callback data(_input_buf.ypr); + _input_pos = 0; + } + } else if ((_mode == YAW_PITCH_ROLL_ACC_MAG_GYR_CALIBRATED)||(_mode == YAW_PITCH_ROLL_ACC_MAG_GYR_RAW)){ //both YPR and Sensor data (12 floats) + if (_input_pos == 48) // we received a full frame + { + // convert endianess if necessary + if (_big_endian()) + { + _swap_endianess(_input_buf.ypramg, 12); + } + + // invoke callback + data(_input_buf.amg); + _input_pos = 0; } } else { // raw or calibrated sensor data (9 floats) diff --git a/C++/RazorAHRS.h b/C++/RazorAHRS.h index 9cbd897..faf3ad7 100644 --- a/C++/RazorAHRS.h +++ b/C++/RazorAHRS.h @@ -37,7 +37,9 @@ class RazorAHRS enum Mode { YAW_PITCH_ROLL, ACC_MAG_GYR_RAW, - ACC_MAG_GYR_CALIBRATED + ACC_MAG_GYR_CALIBRATED, + YAW_PITCH_ROLL_ACC_MAG_GYR_RAW, + YAW_PITCH_ROLL_ACC_MAG_GYR_CALIBRATED }; typedef std::tr1::function DataCallbackFunc; @@ -70,6 +72,7 @@ class RazorAHRS { float ypr[3]; // yaw, pitch, roll float amg[9]; // 3 axes of accelerometer, magnetometer and gyroscope + float ypramg[12]; } _input_buf; size_t _input_pos;