/*************************************************************************************************************** * 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" */ /* "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. */ /* 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]). // 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. */ /*****************************************************************/ /*********** 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 definitions (do not change) #define OUTPUT__MODE_CALIBRATE_SENSORS 0 // Outputs sensor min/max values as text for manual calibration #define OUTPUT__MODE_ANGLES 1 // Outputs yaw/pitch/roll in degrees #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 // Select your startup output mode and format here! int output_mode = OUTPUT__MODE_ANGLES; int output_format = OUTPUT__FORMAT_TEXT; // Select if serial continuous streaming output is enabled per default on startup. #define OUTPUT__STARTUP_STREAM_ON true // true or false // If set true, an error message will be output if we fail to read sensor data. // Message format: "!ERR: reading ", followed by "\r\n". boolean output_errors = 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 (standard calibration mode) // "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) // Magnetometer (extended calibration mode) // Uncommend to use extended magnetometer calibration (compensates hard & soft iron errors) //#define CALIBRATION__MAGN_USE_EXTENDED true //const float magn_ellipsoid_center[3] = {0, 0, 0}; //const float magn_ellipsoid_transform[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // 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) = -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 /*****************************************************************/ // 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.ino! #endif #include // 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 STATUS_LED_PIN 13 // Pin number of status LED #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 // Sensor variables float accel[3]; // Actually stores the NEGATED acceleration (equals gravity, if board not moving). float accel_min[3]; float accel_max[3]; float magnetom[3]; float magnetom_min[3]; float magnetom_max[3]; float magnetom_tmp[3]; float gyro[3]; float gyro_average[3]; 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 } }; // Euler angles float yaw; float pitch; float roll; // DCM timing in the main loop unsigned long timestamp; unsigned long timestamp_old; float G_Dt; // Integration time for DCM algorithm // More output-state variables boolean output_stream_on; boolean output_single_on; int curr_calibration_sensor = 0; boolean reset_calibration_session_flag = true; int num_accel_errors = 0; int num_magn_errors = 0; int num_gyro_errors = 0; void 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 reset_sensor_fusion() { float temp1[3]; float temp2[3]; 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); 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 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 #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); #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; #endif // 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 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.0f; reset_calibration_session_flag = false; } void turn_output_stream_on() { output_stream_on = true; digitalWrite(STATUS_LED_PIN, HIGH); } void turn_output_stream_off() { output_stream_on = false; digitalWrite(STATUS_LED_PIN, LOW); } // Blocks until another byte is available on serial port char readChar() { while (Serial.available() < 1) { } // Block return Serial.read(); } void setup() { // Init serial output Serial.begin(OUTPUT__BAUD_RATE); // Init status LED pinMode (STATUS_LED_PIN, OUTPUT); digitalWrite(STATUS_LED_PIN, LOW); // Init sensors delay(50); // Give sensors enough time to start I2C_Init(); Accel_Init(); Magn_Init(); Gyro_Init(); // Read sensors, init DCM algorithm delay(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 loop() { // Read incoming control messages if (Serial.available() >= 2) { if (Serial.read() == '#') // Start of new control message { int command = Serial.read(); // Commands if (command == 'f') // request one output _f_rame output_single_on = true; else if (command == 's') // _s_ynch request { // Read ID byte id[2]; id[0] = readChar(); id[1] = readChar(); // Reply with synch message Serial.print("#SYNCH"); Serial.write(id, 2); Serial.println(); } 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; output_format = OUTPUT__FORMAT_TEXT; } else if (output_param == 'b') // Output angles in _b_inary format { output_mode = OUTPUT__MODE_ANGLES; output_format = OUTPUT__FORMAT_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 == '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(); char format_param = readChar(); if (values_param == 'r') // Output _r_aw sensor values output_mode = OUTPUT__MODE_SENSORS_RAW; else if (values_param == 'c') // Output _c_alibrated sensor values output_mode = OUTPUT__MODE_SENSORS_CALIB; else if (values_param == 'b') // Output _b_oth sensor values (raw and calibrated) output_mode = OUTPUT__MODE_SENSORS_BOTH; 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; } 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 { Serial.print("#AMG-ERR:"); Serial.print(num_accel_errors); Serial.print(","); Serial.print(num_magn_errors); Serial.print(","); Serial.println(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 } // Time to read the sensors again? if((millis() - timestamp) >= OUTPUT__DATA_INTERVAL) { timestamp_old = timestamp; timestamp = millis(); 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_ANGLES) // Output angles { // 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); #endif } #if DEBUG__PRINT_LOOP_TIME == true else { Serial.println("waiting..."); } #endif }