From 241e56ed8d8d13a615332b321a5da9df1013dad8 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Sun, 19 Nov 2017 01:33:48 +0100 Subject: [PATCH 01/26] Added support of SEN-14001 (*9DoF Razor IMU M0*) --- Arduino/Razor_AHRS/Output.ino | 76 +++++++++++++------------- Arduino/Razor_AHRS/Razor_AHRS.ino | 68 +++++++++++++++++------ Arduino/Razor_AHRS/Sensors.ino | 89 +++++++++++++++++++++++++++++++ README.md | 5 +- 4 files changed, 182 insertions(+), 56 deletions(-) diff --git a/Arduino/Razor_AHRS/Output.ino b/Arduino/Razor_AHRS/Output.ino index 0780082..9d56c8f 100644 --- a/Arduino/Razor_AHRS/Output.ino +++ b/Arduino/Razor_AHRS/Output.ino @@ -9,14 +9,14 @@ void output_angles() ypr[0] = TO_DEG(yaw); ypr[1] = TO_DEG(pitch); ypr[2] = TO_DEG(roll); - Serial.write((byte*) ypr, 12); // No new-line + LOG_PORT.write((byte*) ypr, 12); // No new-line } else if (output_format == OUTPUT__FORMAT_TEXT) { - Serial.print("#YPR="); - Serial.print(TO_DEG(yaw)); Serial.print(","); - Serial.print(TO_DEG(pitch)); Serial.print(","); - Serial.print(TO_DEG(roll)); Serial.println(); + LOG_PORT.print("#YPR="); + LOG_PORT.print(TO_DEG(yaw)); LOG_PORT.print(","); + LOG_PORT.print(TO_DEG(pitch)); LOG_PORT.print(","); + LOG_PORT.print(TO_DEG(roll)); LOG_PORT.println(); } } @@ -25,29 +25,29 @@ void output_calibration(int calibration_sensor) if (calibration_sensor == 0) // Accelerometer { // Output MIN/MAX values - Serial.print("accel x,y,z (min/max) = "); + LOG_PORT.print("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]; - Serial.print(accel_min[i]); - Serial.print("/"); - Serial.print(accel_max[i]); - if (i < 2) Serial.print(" "); - else Serial.println(); + LOG_PORT.print(accel_min[i]); + LOG_PORT.print("/"); + LOG_PORT.print(accel_max[i]); + if (i < 2) LOG_PORT.print(" "); + else LOG_PORT.println(); } } else if (calibration_sensor == 1) // Magnetometer { // Output MIN/MAX values - Serial.print("magn x,y,z (min/max) = "); + LOG_PORT.print("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]; - Serial.print(magnetom_min[i]); - Serial.print("/"); - Serial.print(magnetom_max[i]); - if (i < 2) Serial.print(" "); - else Serial.println(); + LOG_PORT.print(magnetom_min[i]); + LOG_PORT.print("/"); + LOG_PORT.print(magnetom_max[i]); + if (i < 2) LOG_PORT.print(" "); + else LOG_PORT.println(); } } else if (calibration_sensor == 2) // Gyroscope @@ -58,40 +58,40 @@ void output_calibration(int calibration_sensor) gyro_num_samples++; // Output current and averaged gyroscope values - Serial.print("gyro x,y,z (current/average) = "); + LOG_PORT.print("gyro x,y,z (current/average) = "); for (int i = 0; i < 3; i++) { - Serial.print(gyro[i]); - Serial.print("/"); - Serial.print(gyro_average[i] / (float) gyro_num_samples); - if (i < 2) Serial.print(" "); - else Serial.println(); + LOG_PORT.print(gyro[i]); + LOG_PORT.print("/"); + LOG_PORT.print(gyro_average[i] / (float) gyro_num_samples); + if (i < 2) LOG_PORT.print(" "); + else LOG_PORT.println(); } } } void output_sensors_text(char raw_or_calibrated) { - Serial.print("#A-"); Serial.print(raw_or_calibrated); Serial.print('='); - Serial.print(accel[0]); Serial.print(","); - Serial.print(accel[1]); Serial.print(","); - Serial.print(accel[2]); Serial.println(); + LOG_PORT.print("#A-"); LOG_PORT.print(raw_or_calibrated); LOG_PORT.print('='); + LOG_PORT.print(accel[0]); LOG_PORT.print(","); + LOG_PORT.print(accel[1]); LOG_PORT.print(","); + LOG_PORT.print(accel[2]); LOG_PORT.println(); - Serial.print("#M-"); Serial.print(raw_or_calibrated); Serial.print('='); - Serial.print(magnetom[0]); Serial.print(","); - Serial.print(magnetom[1]); Serial.print(","); - Serial.print(magnetom[2]); Serial.println(); + LOG_PORT.print("#M-"); LOG_PORT.print(raw_or_calibrated); LOG_PORT.print('='); + LOG_PORT.print(magnetom[0]); LOG_PORT.print(","); + LOG_PORT.print(magnetom[1]); LOG_PORT.print(","); + LOG_PORT.print(magnetom[2]); LOG_PORT.println(); - Serial.print("#G-"); Serial.print(raw_or_calibrated); Serial.print('='); - Serial.print(gyro[0]); Serial.print(","); - Serial.print(gyro[1]); Serial.print(","); - Serial.print(gyro[2]); Serial.println(); + LOG_PORT.print("#G-"); LOG_PORT.print(raw_or_calibrated); LOG_PORT.print('='); + LOG_PORT.print(gyro[0]); LOG_PORT.print(","); + LOG_PORT.print(gyro[1]); LOG_PORT.print(","); + LOG_PORT.print(gyro[2]); LOG_PORT.println(); } void output_sensors_binary() { - Serial.write((byte*) accel, 12); - Serial.write((byte*) magnetom, 12); - Serial.write((byte*) gyro, 12); + LOG_PORT.write((byte*) accel, 12); + LOG_PORT.write((byte*) magnetom, 12); + LOG_PORT.write((byte*) gyro, 12); } void output_sensors() diff --git a/Arduino/Razor_AHRS/Razor_AHRS.ino b/Arduino/Razor_AHRS/Razor_AHRS.ino index 2fa807a..fc5a3f9 100644 --- a/Arduino/Razor_AHRS/Razor_AHRS.ino +++ b/Arduino/Razor_AHRS/Razor_AHRS.ino @@ -159,6 +159,7 @@ // 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 14001 // SparkFun "9DoF Razor IMU M0" version "SEN-14001" //#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) @@ -168,6 +169,13 @@ /*****************************************************************/ // Set your serial port baud rate used to send out data here! #define OUTPUT__BAUD_RATE 57600 +#if HW__VERSION_CODE == 14001 +// Set your port used to send out data here! +#define LOG_PORT SERIAL_PORT_USBVIRTUAL +#else +// Set your port used to send out data here! +#define LOG_PORT Serial +#endif // HW__VERSION_CODE // Sensor data output interval in milliseconds // This may not work, if faster than 20ms (=50Hz) @@ -211,6 +219,7 @@ boolean output_errors = false; // true or false /*****************************************************************/ // 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! +// For the M0, only the extended magnetometer calibration seems to be really necessary... // 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) @@ -304,7 +313,19 @@ const float magn_ellipsoid_transform[3][3] = {{0.902, -0.00354, 0.000636}, {-0.0 #error YOU HAVE TO SELECT THE HARDWARE YOU ARE USING! See "HARDWARE OPTIONS" in "USER SETUP AREA" at top of Razor_AHRS.ino! #endif +#if HW__VERSION_CODE == 14001 +// MPU-9250 Digital Motion Processing (DMP) Library +#include + +// Danger - don't change unless using a different platform +#define MPU9250_INT_PIN 4 +#define SD_CHIP_SELECT_PIN 38 +#define MPU9250_INT_ACTIVE LOW + +MPU9250_DMP imu; // Create an instance of the MPU9250_DMP class +#else #include +#endif // HW__VERSION_CODE // Sensor calibration scale and offset values #define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f) @@ -322,9 +343,14 @@ const float magn_ellipsoid_transform[3][3] = {{0.902, -0.00354, 0.000636}, {-0.0 #define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET)) + +#if HW__VERSION_CODE == 14001 +#define GYRO_SCALED_RAD(x) (x) // Calculate the scaled gyro readings in radians per second +#else // 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 +#endif // HW__VERSION_CODE // DCM parameters #define Kp_ROLLPITCH 0.02f @@ -386,9 +412,13 @@ int num_magn_errors = 0; int num_gyro_errors = 0; void read_sensors() { +#if HW__VERSION_CODE == 14001 + loop_imu(); +#else Read_Gyro(); // Read gyroscope Read_Accel(); // Read accelerometer Read_Magn(); // Read magnetometer +#endif // HW__VERSION_CODE } // Read every sensor and record a time stamp @@ -483,14 +513,14 @@ void turn_output_stream_off() // Blocks until another byte is available on serial port char readChar() { - while (Serial.available() < 1) { } // Block - return Serial.read(); + while (LOG_PORT.available() < 1) { } // Block + return LOG_PORT.read(); } void setup() { // Init serial output - Serial.begin(OUTPUT__BAUD_RATE); + LOG_PORT.begin(OUTPUT__BAUD_RATE); // Init status LED pinMode (STATUS_LED_PIN, OUTPUT); @@ -498,10 +528,16 @@ void setup() // Init sensors delay(50); // Give sensors enough time to start +#if HW__VERSION_CODE == 14001 + // Set up MPU-9250 interrupt input (active-low) + pinMode(MPU9250_INT_PIN, INPUT_PULLUP); + initIMU(); +#else I2C_Init(); Accel_Init(); Magn_Init(); Gyro_Init(); +#endif // HW__VERSION_CODE // Read sensors, init DCM algorithm delay(20); // Give sensors enough time to collect data @@ -519,11 +555,11 @@ void setup() void loop() { // Read incoming control messages - if (Serial.available() >= 2) + if (LOG_PORT.available() >= 2) { - if (Serial.read() == '#') // Start of new control message + if (LOG_PORT.read() == '#') // Start of new control message { - int command = Serial.read(); // Commands + int command = LOG_PORT.read(); // Commands if (command == 'f') // request one output _f_rame output_single_on = true; else if (command == 's') // _s_ynch request @@ -534,9 +570,9 @@ void loop() id[1] = readChar(); // Reply with synch message - Serial.print("#SYNCH"); - Serial.write(id, 2); - Serial.println(); + LOG_PORT.print("#SYNCH"); + LOG_PORT.write(id, 2); + LOG_PORT.println(); } else if (command == 'o') // Set _o_utput mode { @@ -594,10 +630,10 @@ void loop() 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); + LOG_PORT.print("#AMG-ERR:"); + LOG_PORT.print(num_accel_errors); LOG_PORT.print(","); + LOG_PORT.print(num_magn_errors); LOG_PORT.print(","); + LOG_PORT.println(num_gyro_errors); } } } @@ -653,14 +689,14 @@ void loop() output_single_on = false; #if DEBUG__PRINT_LOOP_TIME == true - Serial.print("loop time (ms) = "); - Serial.println(millis() - timestamp); + LOG_PORT.print("loop time (ms) = "); + LOG_PORT.println(millis() - timestamp); #endif } #if DEBUG__PRINT_LOOP_TIME == true else { - Serial.println("waiting..."); + LOG_PORT.println("waiting..."); } #endif } diff --git a/Arduino/Razor_AHRS/Sensors.ino b/Arduino/Razor_AHRS/Sensors.ino index c3728cf..ca8f5cc 100644 --- a/Arduino/Razor_AHRS/Sensors.ino +++ b/Arduino/Razor_AHRS/Sensors.ino @@ -1,5 +1,93 @@ /* This file is part of the Razor AHRS Firmware */ +#if HW__VERSION_CODE == 14001 +#define DMP_SAMPLE_RATE 100 // Logging/DMP sample rate(4-200 Hz) +#define IMU_COMPASS_SAMPLE_RATE 100 // Compass sample rate (4-100 Hz) +#define IMU_AG_SAMPLE_RATE 100 // Accel/gyro sample rate Must be between 4Hz and 1kHz +#define IMU_GYRO_FSR 2000 // Gyro full-scale range (250, 500, 1000, or 2000) +#define IMU_ACCEL_FSR 16 // Accel full-scale range (2, 4, 8, or 16) +#define IMU_AG_LPF 5 // Accel/Gyro LPF corner frequency (5, 10, 20, 42, 98, or 188 Hz) +#define ENABLE_GYRO_CALIBRATION true + +unsigned short accelFSR = IMU_ACCEL_FSR; +unsigned short gyroFSR = IMU_GYRO_FSR; +unsigned short fifoRate = DMP_SAMPLE_RATE; + +bool initIMU(void) +{ + // imu.begin() should return 0 on success. Will initialize + // I2C bus, and reset MPU-9250 to defaults. + if (imu.begin() != INV_SUCCESS) + return false; + + // Set up MPU-9250 interrupt: + imu.enableInterrupt(); // Enable interrupt output + imu.setIntLevel(1); // Set interrupt to active-low + imu.setIntLatched(1); // Latch interrupt output + + // Configure sensors: + // Set gyro full-scale range: options are 250, 500, 1000, or 2000: + imu.setGyroFSR(gyroFSR); + // Set accel full-scale range: options are 2, 4, 8, or 16 g + imu.setAccelFSR(accelFSR); + // Set gyro/accel LPF: options are5, 10, 20, 42, 98, 188 Hz + imu.setLPF(IMU_AG_LPF); + // Set gyro/accel sample rate: must be between 4-1000Hz + // (note: this value will be overridden by the DMP sample rate) + imu.setSampleRate(IMU_AG_SAMPLE_RATE); + // Set compass sample rate: between 4-100Hz + imu.setCompassSampleRate(IMU_COMPASS_SAMPLE_RATE); + + // Configure digital motion processor. Use the FIFO to get + // data from the DMP. + unsigned short dmpFeatureMask = 0; + if (ENABLE_GYRO_CALIBRATION) + { + // Gyro calibration re-calibrates the gyro after a set amount + // of no motion detected + dmpFeatureMask |= DMP_FEATURE_GYRO_CAL; //added this line + dmpFeatureMask |= DMP_FEATURE_SEND_CAL_GYRO; + } + else + { + // Otherwise add raw gyro readings to the DMP + dmpFeatureMask |= DMP_FEATURE_SEND_RAW_GYRO; + } + // Add accel and quaternion's to the DMP + dmpFeatureMask |= DMP_FEATURE_SEND_RAW_ACCEL; + dmpFeatureMask |= DMP_FEATURE_6X_LP_QUAT; + + // Initialize the DMP, and set the FIFO's update rate: + imu.dmpBegin(dmpFeatureMask, fifoRate); + + return true; // Return success +} + +void loop_imu() +{ + // Then check IMU for new data, and log it + if (!imu.fifoAvailable()) // If no new data is available + return; // return to the top of the loop + + // Read from the digital motion processor's FIFO + if (imu.dmpUpdateFifo() != INV_SUCCESS) + return; // If that fails (uh, oh), return to top + + // If enabled, read from the compass. + if (imu.updateCompass() != INV_SUCCESS) + return; // If compass read fails (uh, oh) return to top + + accel[0] = -250.0f*imu.calcAccel(imu.ax); + accel[1] = 250.0f*imu.calcAccel(imu.ay); + accel[2] = 250.0f*imu.calcAccel(imu.az); + gyro[0] = imu.calcGyro(imu.gx)*PI/180.0f; + gyro[1] = -imu.calcGyro(imu.gy)*PI/180.0f; + gyro[2] = -imu.calcGyro(imu.gz)*PI/180.0f; + magnetom[0] = (10.0f*imu.calcMag(imu.my)); + magnetom[1] = (-10.0f*imu.calcMag(imu.mx)); + magnetom[2] = (10.0f*imu.calcMag(imu.mz)); +} +#else // I2C code to read the sensors // Sensor I2C addresses @@ -208,3 +296,4 @@ void Read_Gyro() if (output_errors) Serial.println("!ERR: reading gyroscope"); } } +#endif // HW__VERSION_CODE diff --git a/README.md b/README.md index dd0f29c..60bd443 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ -Razor AHRS v1.4.2 +Razor AHRS --- -**9 Degree of Measurement Attitude and Heading Reference System** for Sparkfun *9DOF Razor IMU* (SEN-10125 and SEN-10736) and SparkFun *9DOF Sensor Stick* (SEN-10183, SEN-10321 and SEN-10724) +**9 Degree of Measurement Attitude and Heading Reference System** for Sparkfun *9DOF Razor IMU* (SEN-10125, SEN-10736 and SEN-14001) and SparkFun *9DOF Sensor Stick* (SEN-10183, SEN-10321 and SEN-10724) Infos, updates, bug reports, contributions and feedback: https://github.com/ptrbrtz/razor-9dof-ahrs @@ -14,6 +14,7 @@ Tutorial --- You find a [detailed tutorial in the Wiki](https://github.com/ptrbrtz/razor-9dof-ahrs/wiki/Tutorial). +Note: for SEN-14001 (*9DoF Razor IMU M0*), you will need to follow before https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide to install the necessary prerequisites, as well as an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library (an updated version of the default firmware is also available from https://github.com/lebarsfa/9DOF_Razor_IMU). Quick setup --- From 7a9bcb28955dc016410f3087e116ee4f95d3f410 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Sun, 19 Nov 2017 01:43:07 +0100 Subject: [PATCH 02/26] Updated README --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 60bd443..d6c2343 100644 --- a/README.md +++ b/README.md @@ -14,7 +14,7 @@ Tutorial --- You find a [detailed tutorial in the Wiki](https://github.com/ptrbrtz/razor-9dof-ahrs/wiki/Tutorial). -Note: for SEN-14001 (*9DoF Razor IMU M0*), you will need to follow before https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide to install the necessary prerequisites, as well as an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library (an updated version of the default firmware is also available from https://github.com/lebarsfa/9DOF_Razor_IMU). +Note: for SEN-14001 (*9DoF Razor IMU M0*), you will need to make some adaptations and follow before https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide to install the necessary prerequisites, as well as an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library (an updated version of the default firmware is also available from https://github.com/lebarsfa/9DOF_Razor_IMU). Quick setup --- From 762a47ff0808d3574556e510eb2b68872146cc14 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Sun, 19 Nov 2017 16:31:43 +0100 Subject: [PATCH 03/26] Updated README --- Arduino/Razor_AHRS/Razor_AHRS.ino | 11 +++++++++++ README.md | 4 ++-- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/Arduino/Razor_AHRS/Razor_AHRS.ino b/Arduino/Razor_AHRS/Razor_AHRS.ino index fc5a3f9..33adaf6 100644 --- a/Arduino/Razor_AHRS/Razor_AHRS.ino +++ b/Arduino/Razor_AHRS/Razor_AHRS.ino @@ -44,12 +44,23 @@ * * Added static magnetometer soft iron distortion compensation * * v1.4.2 * * (No core firmware changes) +* * v1.5 +* * Added support for "9DoF Razor IMU M0": SEN-14001. * * 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 M0" hardware versions: SEN-14001 + + Arduino IDE : Follow the same instructions as for the default firmware on + https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide + and use an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from + https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library" +*/ + /* "9DOF Razor IMU" hardware versions: SEN-10125 and SEN-10736 diff --git a/README.md b/README.md index d6c2343..b03a440 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -Razor AHRS +Razor AHRS v1.5 --- **9 Degree of Measurement Attitude and Heading Reference System** for Sparkfun *9DOF Razor IMU* (SEN-10125, SEN-10736 and SEN-14001) and SparkFun *9DOF Sensor Stick* (SEN-10183, SEN-10321 and SEN-10724) @@ -14,7 +14,7 @@ Tutorial --- You find a [detailed tutorial in the Wiki](https://github.com/ptrbrtz/razor-9dof-ahrs/wiki/Tutorial). -Note: for SEN-14001 (*9DoF Razor IMU M0*), you will need to make some adaptations and follow before https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide to install the necessary prerequisites, as well as an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library (an updated version of the default firmware is also available from https://github.com/lebarsfa/9DOF_Razor_IMU). +Note: for SEN-14001 (*9DoF Razor IMU M0*), you will need to follow the same instructions as for the default firmware on https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide and use an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library (an updated version of the default firmware is also available on https://github.com/lebarsfa/9DOF_Razor_IMU). Quick setup --- From dcf01e267f5426e9fc24af1c81c09527ec2c3b39 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Mon, 20 Nov 2017 18:59:19 +0100 Subject: [PATCH 04/26] Added comments about the conversions in Sensors.ino --- Arduino/Razor_AHRS/Sensors.ino | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Arduino/Razor_AHRS/Sensors.ino b/Arduino/Razor_AHRS/Sensors.ino index ca8f5cc..138d68a 100644 --- a/Arduino/Razor_AHRS/Sensors.ino +++ b/Arduino/Razor_AHRS/Sensors.ino @@ -77,12 +77,15 @@ void loop_imu() if (imu.updateCompass() != INV_SUCCESS) return; // If compass read fails (uh, oh) return to top + // Conversion from g to similar unit as older versions of Razor... accel[0] = -250.0f*imu.calcAccel(imu.ax); accel[1] = 250.0f*imu.calcAccel(imu.ay); accel[2] = 250.0f*imu.calcAccel(imu.az); + // Conversion from degrees/s to rad/s. gyro[0] = imu.calcGyro(imu.gx)*PI/180.0f; gyro[1] = -imu.calcGyro(imu.gy)*PI/180.0f; gyro[2] = -imu.calcGyro(imu.gz)*PI/180.0f; + // Conversion from uT to mGauss. magnetom[0] = (10.0f*imu.calcMag(imu.my)); magnetom[1] = (-10.0f*imu.calcMag(imu.mx)); magnetom[2] = (10.0f*imu.calcMag(imu.mz)); From 29d7718ac8118d769977582a100cbcff13d5d95d Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Mon, 20 Nov 2017 19:56:17 +0100 Subject: [PATCH 05/26] Added error count in Sensors.ino --- Arduino/Razor_AHRS/Sensors.ino | 30 +++++++++++++++++++++++------- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/Arduino/Razor_AHRS/Sensors.ino b/Arduino/Razor_AHRS/Sensors.ino index 138d68a..8c2461e 100644 --- a/Arduino/Razor_AHRS/Sensors.ino +++ b/Arduino/Razor_AHRS/Sensors.ino @@ -65,17 +65,33 @@ bool initIMU(void) void loop_imu() { - // Then check IMU for new data, and log it - if (!imu.fifoAvailable()) // If no new data is available - return; // return to the top of the loop + // Check IMU for new data, and log it. + if (!imu.fifoAvailable()) + { + num_accel_errors++; + if (output_errors) LOG_PORT.println("!ERR: reading accelerometer"); + num_gyro_errors++; + if (output_errors) LOG_PORT.println("!ERR: reading gyroscope"); + return; + } - // Read from the digital motion processor's FIFO + // Read from the digital motion processor's FIFO. if (imu.dmpUpdateFifo() != INV_SUCCESS) - return; // If that fails (uh, oh), return to top + { + num_accel_errors++; + if (output_errors) LOG_PORT.println("!ERR: reading accelerometer"); + num_gyro_errors++; + if (output_errors) LOG_PORT.println("!ERR: reading gyroscope"); + return; + } - // If enabled, read from the compass. + // Read from the compass. if (imu.updateCompass() != INV_SUCCESS) - return; // If compass read fails (uh, oh) return to top + { + num_magn_errors++; + if (output_errors) LOG_PORT.println("!ERR: reading magnetometer"); + return; + } // Conversion from g to similar unit as older versions of Razor... accel[0] = -250.0f*imu.calcAccel(imu.ax); From cc615fd61c9134d634766f299ae7ca0515d3a924 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Fri, 24 Nov 2017 22:09:30 +0100 Subject: [PATCH 06/26] Minor correction in Sensors.ino --- Arduino/Razor_AHRS/Sensors.ino | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Arduino/Razor_AHRS/Sensors.ino b/Arduino/Razor_AHRS/Sensors.ino index 8c2461e..acc0e86 100644 --- a/Arduino/Razor_AHRS/Sensors.ino +++ b/Arduino/Razor_AHRS/Sensors.ino @@ -180,7 +180,7 @@ void Read_Accel() else { num_accel_errors++; - if (output_errors) Serial.println("!ERR: reading accelerometer"); + if (output_errors) LOG_PORT.println("!ERR: reading accelerometer"); } } @@ -248,7 +248,7 @@ void Read_Magn() else { num_magn_errors++; - if (output_errors) Serial.println("!ERR: reading magnetometer"); + if (output_errors) LOG_PORT.println("!ERR: reading magnetometer"); } } @@ -312,7 +312,7 @@ void Read_Gyro() else { num_gyro_errors++; - if (output_errors) Serial.println("!ERR: reading gyroscope"); + if (output_errors) LOG_PORT.println("!ERR: reading gyroscope"); } } #endif // HW__VERSION_CODE From 9c743d9e0200edcccb1b41b58db91249b57b9945 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Fri, 24 Nov 2017 22:10:06 +0100 Subject: [PATCH 07/26] Added ROS-compatible output mode --- Arduino/Razor_AHRS/Output.ino | 16 ++++++++++++++++ Arduino/Razor_AHRS/Razor_AHRS.ino | 32 +++++++++++++++++++++++++++++-- 2 files changed, 46 insertions(+), 2 deletions(-) diff --git a/Arduino/Razor_AHRS/Output.ino b/Arduino/Razor_AHRS/Output.ino index 9d56c8f..fe1291b 100644 --- a/Arduino/Razor_AHRS/Output.ino +++ b/Arduino/Razor_AHRS/Output.ino @@ -87,6 +87,22 @@ void output_sensors_text(char raw_or_calibrated) LOG_PORT.print(gyro[2]); LOG_PORT.println(); } +void output_both_angles_and_sensors_text() +{ + LOG_PORT.print("#YPRAG="); + LOG_PORT.print(TO_DEG(yaw)); LOG_PORT.print(","); + LOG_PORT.print(TO_DEG(pitch)); LOG_PORT.print(","); + LOG_PORT.print(TO_DEG(roll)); LOG_PORT.print(","); + + LOG_PORT.print(Accel_Vector[0]); LOG_PORT.print(","); + LOG_PORT.print(Accel_Vector[1]); LOG_PORT.print(","); + LOG_PORT.print(Accel_Vector[2]); LOG_PORT.print(","); + + LOG_PORT.print(Gyro_Vector[0]); LOG_PORT.print(","); + LOG_PORT.print(Gyro_Vector[1]); LOG_PORT.print(","); + LOG_PORT.print(Gyro_Vector[2]); LOG_PORT.println(); +} + void output_sensors_binary() { LOG_PORT.write((byte*) accel, 12); diff --git a/Arduino/Razor_AHRS/Razor_AHRS.ino b/Arduino/Razor_AHRS/Razor_AHRS.ino index 33adaf6..72504d3 100644 --- a/Arduino/Razor_AHRS/Razor_AHRS.ino +++ b/Arduino/Razor_AHRS/Razor_AHRS.ino @@ -41,11 +41,13 @@ * * 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 +* * Added static magnetometer soft iron distortion compensation. * * v1.4.2 * * (No core firmware changes) * * v1.5 * * Added support for "9DoF Razor IMU M0": SEN-14001. +* * v1.5.1 +* * Added ROS-compatible output mode. * * TODOs: * * Allow optional use of EEPROM for storing and reading calibration values. @@ -110,7 +112,13 @@ 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]). - + "#ox" - Output angles and linear acceleration and rotational + velocity. Angles are in degrees, acceleration is + in units of 1.0 = 1/256 G (9.8/256 m/s^2). Rotational + velocity is in rad/s^2. (Output frames have form like + "#YPRAG=-142.28,-5.38,33.52,0.1,0.1,1.0,0.01,0.01,0.01", + 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. @@ -199,6 +207,7 @@ #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_AG_SENSORS 5 // Outputs yaw/pitch/roll in degrees + linear accel + rot. vel // 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 @@ -608,6 +617,11 @@ void loop() output_mode = OUTPUT__MODE_CALIBRATE_SENSORS; reset_calibration_session_flag = true; } + else if (output_param == 'x') // Output angles + accel + rot. vel as te_x_t + { + output_mode = OUTPUT__MODE_ANGLES_AG_SENSORS; + output_format = OUTPUT__FORMAT_TEXT; + } else if (output_param == 's') // Output _s_ensor values { char values_param = readChar(); @@ -692,6 +706,20 @@ void loop() if (output_stream_on || output_single_on) output_angles(); } + else if (output_mode == OUTPUT__MODE_ANGLES_AG_SENSORS) // Output angles + accel + rot. vel + { + // 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_both_angles_and_sensors_text(); + } else // Output sensor values { if (output_stream_on || output_single_on) output_sensors(); From b943157d32a2da8fc946083ae044a1f7f9f575d0 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Fri, 24 Nov 2017 22:51:22 +0100 Subject: [PATCH 08/26] Updated README --- Arduino/Razor_AHRS/Razor_AHRS.ino | 2 +- README.md | 6 +++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/Arduino/Razor_AHRS/Razor_AHRS.ino b/Arduino/Razor_AHRS/Razor_AHRS.ino index 72504d3..b8df948 100644 --- a/Arduino/Razor_AHRS/Razor_AHRS.ino +++ b/Arduino/Razor_AHRS/Razor_AHRS.ino @@ -1,5 +1,5 @@ /*************************************************************************************************************** -* Razor AHRS Firmware v1.4.2 +* Razor AHRS Firmware v1.5.1 * 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) diff --git a/README.md b/README.md index b03a440..2246935 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -Razor AHRS v1.5 +Razor AHRS v1.5.1 --- **9 Degree of Measurement Attitude and Heading Reference System** for Sparkfun *9DOF Razor IMU* (SEN-10125, SEN-10736 and SEN-14001) and SparkFun *9DOF Sensor Stick* (SEN-10183, SEN-10321 and SEN-10724) @@ -55,6 +55,10 @@ Building your own app: * If you want to use the DeclinationHelper class you also need: `android.permission.ACCESS_FINE_LOCATION` and `android.permission.ACCESS_COARSE_LOCATION` +### Optional: ROS Interface + +See http://wiki.ros.org/razor_imu_9dof. + Donation --- From 249b2b60d5fb9a9949f326e90f087145d34f23f6 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Fri, 24 Nov 2017 23:52:22 +0100 Subject: [PATCH 09/26] Added ROS-compatible input mode to set calibration parameters --- Arduino/Razor_AHRS/Razor_AHRS.ino | 320 ++++++++++++++---- .../magnetometer_calibration.m | 6 +- .../Magnetometer_calibration.pde | 6 +- README.md | 2 +- 4 files changed, 264 insertions(+), 70 deletions(-) diff --git a/Arduino/Razor_AHRS/Razor_AHRS.ino b/Arduino/Razor_AHRS/Razor_AHRS.ino index b8df948..e0f9eac 100644 --- a/Arduino/Razor_AHRS/Razor_AHRS.ino +++ b/Arduino/Razor_AHRS/Razor_AHRS.ino @@ -1,5 +1,5 @@ /*************************************************************************************************************** -* Razor AHRS Firmware v1.5.1 +* Razor AHRS Firmware v1.5.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) @@ -48,6 +48,8 @@ * * Added support for "9DoF Razor IMU M0": SEN-14001. * * v1.5.1 * * Added ROS-compatible output mode. +* * v1.5.2 +* * Added ROS-compatible input mode to set calibration parameters. * * TODOs: * * Allow optional use of EEPROM for storing and reading calibration values. @@ -101,6 +103,13 @@ /* Serial commands that the firmware understands: + "#c" - SET _c_alibration parameters. The available options are: + [a|m|g|c|t] _a_ccelerometer, _m_agnetometer, _g_yro, magnetometerellipsoid_c_enter, magnetometerellipsoid_t_ransform. + [x|y|z] x,y or z. + [m|M|X|Y|Z] _m_in or _M_ax (accel or magnetometer), X, Y, or Z of transform (magnetometerellipsoid_t_ransform). + + "#p" - PRINT current calibration values. + "#o" - Set OUTPUT mode and parameters. The available options are: // Streaming output @@ -242,67 +251,67 @@ boolean output_errors = false; // true or false // For the M0, only the extended magnetometer calibration seems to be really necessary... // 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) +float ACCEL_X_MIN = -250; +float ACCEL_X_MAX = 250; +float ACCEL_Y_MIN = -250; +float ACCEL_Y_MAX = 250; +float ACCEL_Z_MIN = -250; +float ACCEL_Z_MAX = 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) +float MAGN_X_MIN = -600; +float MAGN_X_MAX = 600; +float MAGN_Y_MIN = -600; +float MAGN_Y_MAX = 600; +float MAGN_Z_MIN = -600; +float MAGN_Z_MAX = 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}}; +// Set to true to use extended magnetometer calibration (compensates hard & soft iron errors) +boolean CALIBRATION__MAGN_USE_EXTENDED = false; +float magn_ellipsoid_center[3] = {0, 0, 0}; +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) +float GYRO_AVERAGE_OFFSET_X = 0.0; +float GYRO_AVERAGE_OFFSET_Y = 0.0; +float GYRO_AVERAGE_OFFSET_Z = 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) +float ACCEL_X_MIN = -277; +float ACCEL_X_MAX = 264; +float ACCEL_Y_MIN = -256; +float ACCEL_Y_MAX = 278; +float ACCEL_Z_MIN = -299; +float ACCEL_Z_MAX = 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) +float MAGN_X_MIN = -511; +float MAGN_X_MAX = 581; +float MAGN_Y_MIN = -516; +float MAGN_Y_MAX = 568; +float MAGN_Z_MIN = -489; +float MAGN_Z_MAX = 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}}; +boolean CALIBRATION__MAGN_USE_EXTENDED = true; +float magn_ellipsoid_center[3] = {91.5, -13.5, -48.1}; +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}}; +//boolean CALIBRATION__MAGN_USE_EXTENDED = true; +//float magn_ellipsoid_center[3] = {72.3360, 23.0954, 53.6261}; +//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) +float GYRO_AVERAGE_OFFSET_X = -42.05; +float GYRO_AVERAGE_OFFSET_Y = 96.20; +float GYRO_AVERAGE_OFFSET_Z = -18.36; */ @@ -347,20 +356,22 @@ MPU9250_DMP imu; // Create an instance of the MPU9250_DMP class #include #endif // HW__VERSION_CODE +#define GRAVITY 256.0f // "1G reference" used for DCM filter and accelerometer calibration + // 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)) +float ACCEL_X_OFFSET = ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f); +float ACCEL_Y_OFFSET = ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f); +float ACCEL_Z_OFFSET = ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f); +float ACCEL_X_SCALE = (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET)); +float ACCEL_Y_SCALE = (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET)); +float 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)) +float MAGN_X_OFFSET = ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f); +float MAGN_Y_OFFSET = ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f); +float MAGN_Z_OFFSET = ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f); +float MAGN_X_SCALE = (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET)); +float MAGN_Y_SCALE = (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET)); +float MAGN_Z_SCALE = (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET)); @@ -380,7 +391,6 @@ MPU9250_DMP imu; // Create an instance of the MPU9250_DMP class // 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 @@ -441,6 +451,26 @@ void read_sensors() { #endif // HW__VERSION_CODE } +// Should be called after every #ca calibration command +void recalculateAccelCalibration() { + ACCEL_X_OFFSET = ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f); + ACCEL_Y_OFFSET = ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f); + ACCEL_Z_OFFSET = ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f); + ACCEL_X_SCALE = (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET)); + ACCEL_Y_SCALE = (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET)); + ACCEL_Z_SCALE = (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET)); +} + +// Should be called after every #cm calibration command +void recalculateMagnCalibration() { + MAGN_X_OFFSET = ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f); + MAGN_Y_OFFSET = ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f); + MAGN_Z_OFFSET = ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f); + MAGN_X_SCALE = (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET)); + MAGN_Y_SCALE = (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET)); + MAGN_Z_SCALE = (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET)); +} + // Read every sensor and record a time stamp // Init DCM with unfiltered orientation // TODO re-init global vars? @@ -481,15 +511,18 @@ void compensate_sensor_errors() { 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 + if (CALIBRATION__MAGN_USE_EXTENDED) + { + 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; + } // Compensate gyroscope error gyro[0] -= GYRO_AVERAGE_OFFSET_X; @@ -662,6 +695,167 @@ void loop() } } } + else if (command == 'p') // Set _p_rint calibration values + { + LOG_PORT.print("ACCEL_X_MIN:");LOG_PORT.println(ACCEL_X_MIN); + LOG_PORT.print("ACCEL_X_MAX:");LOG_PORT.println(ACCEL_X_MAX); + LOG_PORT.print("ACCEL_Y_MIN:");LOG_PORT.println(ACCEL_Y_MIN); + LOG_PORT.print("ACCEL_Y_MAX:");LOG_PORT.println(ACCEL_Y_MAX); + LOG_PORT.print("ACCEL_Z_MIN:");LOG_PORT.println(ACCEL_Z_MIN); + LOG_PORT.print("ACCEL_Z_MAX:");LOG_PORT.println(ACCEL_Z_MAX); + LOG_PORT.println(""); + LOG_PORT.print("MAGN_X_MIN:");LOG_PORT.println(MAGN_X_MIN); + LOG_PORT.print("MAGN_X_MAX:");LOG_PORT.println(MAGN_X_MAX); + LOG_PORT.print("MAGN_Y_MIN:");LOG_PORT.println(MAGN_Y_MIN); + LOG_PORT.print("MAGN_Y_MAX:");LOG_PORT.println(MAGN_Y_MAX); + LOG_PORT.print("MAGN_Z_MIN:");LOG_PORT.println(MAGN_Z_MIN); + LOG_PORT.print("MAGN_Z_MAX:");LOG_PORT.println(MAGN_Z_MAX); + LOG_PORT.println(""); + LOG_PORT.print("MAGN_USE_EXTENDED:"); + if (CALIBRATION__MAGN_USE_EXTENDED) + LOG_PORT.println("true"); + else + LOG_PORT.println("false"); + LOG_PORT.print("magn_ellipsoid_center:[");LOG_PORT.print(magn_ellipsoid_center[0],4);LOG_PORT.print(","); + LOG_PORT.print(magn_ellipsoid_center[1],4);LOG_PORT.print(","); + LOG_PORT.print(magn_ellipsoid_center[2],4);LOG_PORT.println("]"); + LOG_PORT.print("magn_ellipsoid_transform:["); + for(int i = 0; i < 3; i++){ + LOG_PORT.print("["); + for(int j = 0; j < 3; j++){ + LOG_PORT.print(magn_ellipsoid_transform[i][j],7); + if (j < 2) LOG_PORT.print(","); + } + LOG_PORT.print("]"); + if (i < 2) LOG_PORT.print(","); + } + LOG_PORT.println("]"); + LOG_PORT.println(""); + LOG_PORT.print("GYRO_AVERAGE_OFFSET_X:");LOG_PORT.println(GYRO_AVERAGE_OFFSET_X); + LOG_PORT.print("GYRO_AVERAGE_OFFSET_Y:");LOG_PORT.println(GYRO_AVERAGE_OFFSET_Y); + LOG_PORT.print("GYRO_AVERAGE_OFFSET_Z:");LOG_PORT.println(GYRO_AVERAGE_OFFSET_Z); + } + else if (command == 'c') // Set _i_nput mode + { + char input_param = readChar(); + if (input_param == 'a') // Calibrate _a_ccelerometer + { + char axis_param = readChar(); + char type_param = readChar(); + float value_param = LOG_PORT.parseFloat(); + if (axis_param == 'x') // x value + { + if (type_param == 'm') + ACCEL_X_MIN = value_param; + else if (type_param == 'M') + ACCEL_X_MAX = value_param; + } + else if (axis_param == 'y') // y value + { + if (type_param == 'm') + ACCEL_Y_MIN = value_param; + else if (type_param == 'M') + ACCEL_Y_MAX = value_param; + } + else if (axis_param == 'z') // z value + { + if (type_param == 'm') + ACCEL_Z_MIN = value_param; + else if (type_param == 'M') + ACCEL_Z_MAX = value_param; + } + recalculateAccelCalibration(); + } + else if (input_param == 'm') // Calibrate _m_agnetometer (basic) + { + //disable extended magnetometer calibration + CALIBRATION__MAGN_USE_EXTENDED = false; + char axis_param = readChar(); + char type_param = readChar(); + float value_param = LOG_PORT.parseFloat(); + if (axis_param == 'x') // x value + { + if (type_param == 'm') + MAGN_X_MIN = value_param; + else if (type_param == 'M') + MAGN_X_MAX = value_param; + } + else if (axis_param == 'y') // y value + { + if (type_param == 'm') + MAGN_Y_MIN = value_param; + else if (type_param == 'M') + MAGN_Y_MAX = value_param; + } + else if (axis_param == 'z') // z value + { + if (type_param == 'm') + MAGN_Z_MIN = value_param; + else if (type_param == 'M') + MAGN_Z_MAX = value_param; + } + recalculateMagnCalibration(); + } + else if (input_param == 'c') // Calibrate magnetometerellipsoid_c_enter (extended) + { + //enable extended magnetometer calibration + CALIBRATION__MAGN_USE_EXTENDED = true; + char axis_param = readChar(); + float value_param = LOG_PORT.parseFloat(); + if (axis_param == 'x') // x value + magn_ellipsoid_center[0] = value_param; + else if (axis_param == 'y') // y value + magn_ellipsoid_center[1] = value_param; + else if (axis_param == 'z') // z value + magn_ellipsoid_center[2] = value_param; + } + else if (input_param == 't') // Calibrate magnetometerellipsoid_t_ransform (extended) + { + //enable extended magnetometer calibration + CALIBRATION__MAGN_USE_EXTENDED = true; + char axis_param = readChar(); + char type_param = readChar(); + float value_param = LOG_PORT.parseFloat(); + if (axis_param == 'x') // x value + { + if (type_param == 'X') + magn_ellipsoid_transform[0][0] = value_param; + else if (type_param == 'Y') + magn_ellipsoid_transform[0][1] = value_param; + else if (type_param == 'Z') + magn_ellipsoid_transform[0][2] = value_param; + } + else if (axis_param == 'y') // y value + { + if (type_param == 'X') + magn_ellipsoid_transform[1][0] = value_param; + else if (type_param == 'Y') + magn_ellipsoid_transform[1][1] = value_param; + else if (type_param == 'Z') + magn_ellipsoid_transform[1][2] = value_param; + } + else if (axis_param == 'z') // z value + { + if (type_param == 'X') + magn_ellipsoid_transform[2][0] = value_param; + else if (type_param == 'Y') + magn_ellipsoid_transform[2][1] = value_param; + else if (type_param == 'Z') + magn_ellipsoid_transform[2][2] = value_param; + } + } + else if (input_param == 'g') // Calibrate _g_yro + { + char axis_param = readChar(); + float value_param = LOG_PORT.parseFloat(); + if (axis_param == 'x') // x value + GYRO_AVERAGE_OFFSET_X = value_param; + else if (axis_param == 'y') // y value + GYRO_AVERAGE_OFFSET_Y = value_param; + else if (axis_param == 'z') // z value + GYRO_AVERAGE_OFFSET_Z = value_param; + } + } #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 "#". diff --git a/Matlab/magnetometer_calibration/magnetometer_calibration.m b/Matlab/magnetometer_calibration/magnetometer_calibration.m index cd01f83..88746d6 100644 --- a/Matlab/magnetometer_calibration/magnetometer_calibration.m +++ b/Matlab/magnetometer_calibration/magnetometer_calibration.m @@ -35,9 +35,9 @@ % output info fprintf('In the Razor_AHRS.ino, under "SENSOR CALIBRATION" find the section that reads "Magnetometer (extended calibration)"\n'); fprintf('Replace the existing 3 lines with these:\n\n'); -fprintf('#define CALIBRATION__MAGN_USE_EXTENDED true\n'); -fprintf('const float magn_ellipsoid_center[3] = {%.6g, %.6g, %.6g};\n', e_center); -fprintf('const float magn_ellipsoid_transform[3][3] = {{%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}};\n', comp); +fprintf('boolean CALIBRATION__MAGN_USE_EXTENDED = true;\n'); +fprintf('float magn_ellipsoid_center[3] = {%.6g, %.6g, %.6g};\n', e_center); +fprintf('float magn_ellipsoid_transform[3][3] = {{%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}};\n', comp); % draw ellipsoid fit figure; diff --git a/Processing/Magnetometer_calibration/Magnetometer_calibration.pde b/Processing/Magnetometer_calibration/Magnetometer_calibration.pde index 684e0f4..3a60d96 100644 --- a/Processing/Magnetometer_calibration/Magnetometer_calibration.pde +++ b/Processing/Magnetometer_calibration/Magnetometer_calibration.pde @@ -366,9 +366,9 @@ void outputCalibration() { // Output calibration System.out.printf("In the Razor_AHRS.ino, under 'SENSOR CALIBRATION' find the section that reads 'Magnetometer (extended calibration)'\n"); System.out.printf("Replace the existing 3 lines with these:\n\n"); - System.out.printf("#define CALIBRATION__MAGN_USE_EXTENDED true\n"); - System.out.printf("const float magn_ellipsoid_center[3] = {%.6g, %.6g, %.6g};\n", center.get(0), center.get(1), center.get(2)); - System.out.printf("const float magn_ellipsoid_transform[3][3] = {{%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}};\n", + System.out.printf("boolean CALIBRATION__MAGN_USE_EXTENDED = true;\n"); + System.out.printf("float magn_ellipsoid_center[3] = {%.6g, %.6g, %.6g};\n", center.get(0), center.get(1), center.get(2)); + System.out.printf("float magn_ellipsoid_transform[3][3] = {{%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}};\n", comp.get(0), comp.get(1), comp.get(2), comp.get(3), comp.get(4), comp.get(5), comp.get(6), comp.get(7), comp.get(8)); println("\n"); } diff --git a/README.md b/README.md index 2246935..79cca1f 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -Razor AHRS v1.5.1 +Razor AHRS v1.5.2 --- **9 Degree of Measurement Attitude and Heading Reference System** for Sparkfun *9DOF Razor IMU* (SEN-10125, SEN-10736 and SEN-14001) and SparkFun *9DOF Sensor Stick* (SEN-10183, SEN-10321 and SEN-10724) From 55b0efc5114116ce26b13d556524f2608c0edfd9 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Sat, 25 Nov 2017 00:07:52 +0100 Subject: [PATCH 10/26] Updated README --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 79cca1f..4a44790 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,7 @@ Infos, updates, bug reports, contributions and feedback: https://github.com/ptrb Download --- -Clone the [repository on GitHub](https://github.com/ptrbrtz/razor-9dof-ahrs) or [download as .zip](https://github.com/ptrbrtz/razor-9dof-ahrs/archive/Release-v1.4.2.zip). +Clone the [repository on GitHub](https://github.com/lebarsfa/razor-9dof-ahrs) or [download as .zip](https://github.com/lebarsfa/razor-9dof-ahrs/releases). Tutorial --- From 16e1e0f7fc7c0a032cd968098456d66c9937f8b8 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Sat, 25 Nov 2017 00:09:26 +0100 Subject: [PATCH 11/26] Updated README --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 4a44790..d58ebae 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,7 @@ Infos, updates, bug reports, contributions and feedback: https://github.com/ptrb Download --- -Clone the [repository on GitHub](https://github.com/lebarsfa/razor-9dof-ahrs) or [download as .zip](https://github.com/lebarsfa/razor-9dof-ahrs/releases). +Clone the [repository on GitHub](https://github.com/lebarsfa/razor-9dof-ahrs) or [download a specific release](https://github.com/lebarsfa/razor-9dof-ahrs/releases). Tutorial --- From 54e85af4409d431589357cdea54a5ad15f87aded Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Sat, 25 Nov 2017 00:10:37 +0100 Subject: [PATCH 12/26] Updated README --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index d58ebae..7481dc3 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -Razor AHRS v1.5.2 +Razor AHRS --- **9 Degree of Measurement Attitude and Heading Reference System** for Sparkfun *9DOF Razor IMU* (SEN-10125, SEN-10736 and SEN-14001) and SparkFun *9DOF Sensor Stick* (SEN-10183, SEN-10321 and SEN-10724) From 63ff26f70dd1d8fc51c436d3e075168b8e95ba06 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Mon, 27 Nov 2017 23:17:03 +0100 Subject: [PATCH 13/26] Fixed the problem where commands were ignored by the M0 depending on how they were sent --- Arduino/Razor_AHRS/Razor_AHRS.ino | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/Arduino/Razor_AHRS/Razor_AHRS.ino b/Arduino/Razor_AHRS/Razor_AHRS.ino index e0f9eac..b1aa028 100644 --- a/Arduino/Razor_AHRS/Razor_AHRS.ino +++ b/Arduino/Razor_AHRS/Razor_AHRS.ino @@ -1,5 +1,5 @@ /*************************************************************************************************************** -* Razor AHRS Firmware v1.5.2 +* Razor AHRS Firmware * 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) @@ -50,6 +50,8 @@ * * Added ROS-compatible output mode. * * v1.5.2 * * Added ROS-compatible input mode to set calibration parameters. +* * v1.5.3 +* * Fixed the problem where commands were ignored by the M0 depending on how they were sent. * * TODOs: * * Allow optional use of EEPROM for storing and reading calibration values. @@ -608,11 +610,21 @@ void setup() void loop() { // Read incoming control messages + #if HW__VERSION_CODE == 14001 + // Compatibility fix : if bytes are sent 1 by 1 without being read, available() might never return more than 1... + // Therefore, we need to read bytes 1 by 1 and the command byte needs to be a blocking read... + if (LOG_PORT.available() >= 1) + { + if (LOG_PORT.read() == '#') // Start of new control message + { + int command = readChar(); // Commands +#else if (LOG_PORT.available() >= 2) { if (LOG_PORT.read() == '#') // Start of new control message { int command = LOG_PORT.read(); // Commands +#endif // HW__VERSION_CODE if (command == 'f') // request one output _f_rame output_single_on = true; else if (command == 's') // _s_ynch request From 3cf447ce4095aeb9ee29d7680745f0ccc47bcc29 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Mon, 27 Nov 2017 23:51:26 +0100 Subject: [PATCH 14/26] Initialized some unitialized global variables --- Arduino/Razor_AHRS/Razor_AHRS.ino | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/Arduino/Razor_AHRS/Razor_AHRS.ino b/Arduino/Razor_AHRS/Razor_AHRS.ino index b1aa028..d098056 100644 --- a/Arduino/Razor_AHRS/Razor_AHRS.ino +++ b/Arduino/Razor_AHRS/Razor_AHRS.ino @@ -54,7 +54,7 @@ * * Fixed the problem where commands were ignored by the M0 depending on how they were sent. * * TODOs: -* * Allow optional use of EEPROM for storing and reading calibration values. +* * Allow optional use of Flash/EEPROM for storing and reading calibration values. * * Use self-test and temperature-compensation features of the sensors. ***************************************************************************************************************/ @@ -411,7 +411,7 @@ float gyro_average[3]; int gyro_num_samples = 0; // DCM variables -float MAG_Heading; +float MAG_Heading = 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 @@ -425,18 +425,18 @@ 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; +float yaw = 0; +float pitch = 0; +float roll = 0; // DCM timing in the main loop -unsigned long timestamp; -unsigned long timestamp_old; -float G_Dt; // Integration time for DCM algorithm +unsigned long timestamp = 0; +unsigned long timestamp_old = 0; +float G_Dt = 0; // Integration time for DCM algorithm // More output-state variables -boolean output_stream_on; -boolean output_single_on; +boolean output_stream_on = false; +boolean output_single_on = false; int curr_calibration_sensor = 0; boolean reset_calibration_session_flag = true; int num_accel_errors = 0; From af3551238637b010e648068d9157084b4e141f34 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Tue, 5 Dec 2017 16:39:17 +0100 Subject: [PATCH 15/26] Added an option to get yaw/pitch/roll from the M0 DMP and a command to enable/disable the use of magnetometers for yaw computation --- Arduino/Razor_AHRS/Compass.ino | 12 ++-- Arduino/Razor_AHRS/DCM.ino | 58 ++++++++-------- Arduino/Razor_AHRS/Razor_AHRS.ino | 110 +++++++++++++++++++++++------- Arduino/Razor_AHRS/Sensors.ino | 36 ++++++++++ 4 files changed, 157 insertions(+), 59 deletions(-) diff --git a/Arduino/Razor_AHRS/Compass.ino b/Arduino/Razor_AHRS/Compass.ino index 25104ad..7c4680f 100644 --- a/Arduino/Razor_AHRS/Compass.ino +++ b/Arduino/Razor_AHRS/Compass.ino @@ -2,12 +2,12 @@ void Compass_Heading() { - float mag_x; - float mag_y; - float cos_roll; - float sin_roll; - float cos_pitch; - float sin_pitch; + float mag_x = 0; + float mag_y = 0; + float cos_roll = 0; + float sin_roll = 0; + float cos_pitch = 0; + float sin_pitch = 0; cos_roll = cos(roll); sin_roll = sin(roll); diff --git a/Arduino/Razor_AHRS/DCM.ino b/Arduino/Razor_AHRS/DCM.ino index bf8a924..ffe80a9 100644 --- a/Arduino/Razor_AHRS/DCM.ino +++ b/Arduino/Razor_AHRS/DCM.ino @@ -32,14 +32,14 @@ void Normalize(void) /**************************************************/ void Drift_correction(void) { - float mag_heading_x; - float mag_heading_y; - float errorCourse; + float mag_heading_x = 0; + float mag_heading_y = 0; + float errorCourse = 0; //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; + float Accel_magnitude = 0; + float Accel_weight = 0; //*****Roll and Pitch*************** @@ -85,27 +85,30 @@ void Matrix_update(void) 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 + if (DEBUG__NO_DRIFT_CORRECTION) // 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; + } Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c @@ -120,8 +123,7 @@ void Matrix_update(void) void Euler_angles(void) { - pitch = -asin(DCM_Matrix[2][0]); + if ((DCM_Matrix[2][0] < -1)||(DCM_Matrix[2][0] > 1)) num_math_errors++; else pitch = -asin(DCM_Matrix[2][0]); // Attempt to prevent nan problems... roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); } - diff --git a/Arduino/Razor_AHRS/Razor_AHRS.ino b/Arduino/Razor_AHRS/Razor_AHRS.ino index d098056..44dfb75 100644 --- a/Arduino/Razor_AHRS/Razor_AHRS.ino +++ b/Arduino/Razor_AHRS/Razor_AHRS.ino @@ -52,6 +52,10 @@ * * Added ROS-compatible input mode to set calibration parameters. * * v1.5.3 * * Fixed the problem where commands were ignored by the M0 depending on how they were sent. +* * v1.5.4 +* * Attempts to fix random nan problems in orientation computations. +* * Added an option to get yaw/pitch/roll from the M0 DMP. +* * Added a command to enable/disable the use of magnetometers for yaw computation. * * TODOs: * * Allow optional use of Flash/EEPROM for storing and reading calibration values. @@ -110,8 +114,10 @@ [x|y|z] x,y or z. [m|M|X|Y|Z] _m_in or _M_ax (accel or magnetometer), X, Y, or Z of transform (magnetometerellipsoid_t_ransform). + "#p" - PRINT current calibration values. - + + "#o" - Set OUTPUT mode and parameters. The available options are: // Streaming output @@ -153,20 +159,25 @@ // Error message output "#oe0" - Disable ERROR message output. "#oe1" - Enable ERROR message output. - - + "#oec" - Output ERROR COUNT. + "#oem" - Output MATH ERROR COUNT. + + + "#I" - Toggle INERTIAL-only mode for yaw computation. + + "#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 @@ -320,10 +331,18 @@ float GYRO_AVERAGE_OFFSET_Z = -18.36; // DEBUG OPTIONS /*****************************************************************/ // When set to true, gyro drift correction will not be applied -#define DEBUG__NO_DRIFT_CORRECTION false +boolean DEBUG__NO_DRIFT_CORRECTION = false; // Print elapsed time after each I/O loop #define DEBUG__PRINT_LOOP_TIME false +#define DEBUG__PRINT_LOOP_TIME_2 false + +#define DEBUG__ADD_LOOP_DELAY false + +#define DEBUG__LOOP_DELAY 1 + +#define DEBUG__USE_DMP_M0 false + /*****************************************************************/ /****************** END OF USER SETUP AREA! *********************/ @@ -354,6 +373,11 @@ float GYRO_AVERAGE_OFFSET_Z = -18.36; #define MPU9250_INT_ACTIVE LOW MPU9250_DMP imu; // Create an instance of the MPU9250_DMP class + +#if DEBUG__USE_DMP_M0 == true +float initialmagyaw = -10000; +float initialimuyaw = -10000; +#endif // DEBUG__USE_DMP_M0 #else #include #endif // HW__VERSION_CODE @@ -397,17 +421,17 @@ float MAGN_Z_SCALE = (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET)); #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 accel[3] = {0, 0, 0}; // Actually stores the NEGATED acceleration (equals gravity, if board not moving). +float accel_min[3] = {0, 0, 0}; +float accel_max[3] = {0, 0, 0}; -float magnetom[3]; -float magnetom_min[3]; -float magnetom_max[3]; -float magnetom_tmp[3]; +float magnetom[3] = {0, 0, 0}; +float magnetom_min[3] = {0, 0, 0}; +float magnetom_max[3] = {0, 0, 0}; +float magnetom_tmp[3] = {0, 0, 0}; -float gyro[3]; -float gyro_average[3]; +float gyro[3] = {0, 0, 0}; +float gyro_average[3] = {0, 0, 0}; int gyro_num_samples = 0; // DCM variables @@ -442,6 +466,7 @@ boolean reset_calibration_session_flag = true; int num_accel_errors = 0; int num_magn_errors = 0; int num_gyro_errors = 0; +int num_math_errors = 0; void read_sensors() { #if HW__VERSION_CODE == 14001 @@ -477,16 +502,16 @@ void recalculateMagnCalibration() { // 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}; + float temp1[3] = {0, 0, 0}; + float temp2[3] = {0, 0, 0}; + float xAxis[3] = {1, 0, 0}; 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])); + if ((accel[0] == 0)&&(sqrt(accel[1] * accel[1] + accel[2] * accel[2]) == 0)) num_math_errors++; else pitch = -atan2(accel[0], sqrt(accel[1] * accel[1] + accel[2] * accel[2])); // Attempt to prevent nan problems... // GET ROLL // Compensate pitch of gravity vector @@ -495,7 +520,7 @@ void reset_sensor_fusion() { // 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]); + if ((temp2[1] == 0)&&(temp2[2] == 0)) num_math_errors++; else roll = atan2(temp2[1], temp2[2]); // Attempt to prevent nan problems... // GET YAW Compass_Heading(); @@ -586,7 +611,9 @@ void setup() #if HW__VERSION_CODE == 14001 // Set up MPU-9250 interrupt input (active-low) pinMode(MPU9250_INT_PIN, INPUT_PULLUP); + //delay(50); // Give sensors enough time to start initIMU(); + //delay(50); // Give sensors enough time to start #else I2C_Init(); Accel_Init(); @@ -698,13 +725,18 @@ void loop() 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 + else if (error_param == 'c') // get error _c_ount { LOG_PORT.print("#AMG-ERR:"); LOG_PORT.print(num_accel_errors); LOG_PORT.print(","); LOG_PORT.print(num_magn_errors); LOG_PORT.print(","); LOG_PORT.println(num_gyro_errors); } + else if (error_param == 'm') // get _m_ath error count + { + LOG_PORT.print("#MATH-ERR:"); + LOG_PORT.println(num_math_errors); + } } } else if (command == 'p') // Set _p_rint calibration values @@ -747,7 +779,7 @@ void loop() LOG_PORT.print("GYRO_AVERAGE_OFFSET_Y:");LOG_PORT.println(GYRO_AVERAGE_OFFSET_Y); LOG_PORT.print("GYRO_AVERAGE_OFFSET_Z:");LOG_PORT.println(GYRO_AVERAGE_OFFSET_Z); } - else if (command == 'c') // Set _i_nput mode + else if (command == 'c') // Set _i_nput mode { char input_param = readChar(); if (input_param == 'a') // Calibrate _a_ccelerometer @@ -868,6 +900,15 @@ void loop() GYRO_AVERAGE_OFFSET_Z = value_param; } } + else if (command == 'I') // Toggle _i_nertial-only mode for yaw computation + { + DEBUG__NO_DRIFT_CORRECTION = !DEBUG__NO_DRIFT_CORRECTION; +#if DEBUG__USE_DMP_M0 == true + // Update reference for yaw... + initialmagyaw = -MAG_Heading; + initialimuyaw = imu.yaw*PI/180.0f; +#endif // DEBUG__USE_DMP_M0 + } #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 "#". @@ -884,6 +925,10 @@ void loop() // Time to read the sensors again? if((millis() - timestamp) >= OUTPUT__DATA_INTERVAL) { +#if DEBUG__PRINT_LOOP_TIME_2 == true + LOG_PORT.print("loop time (ms) = "); + LOG_PORT.println(millis() - timestamp); +#endif // DEBUG__PRINT_LOOP_TIME_2 timestamp_old = timestamp; timestamp = millis(); if (timestamp > timestamp_old) @@ -902,13 +947,17 @@ void loop() { // Apply sensor calibration compensate_sensor_errors(); - + +#if DEBUG__USE_DMP_M0 == true + Euler_angles_DMP_M0(); +#else // Run DCM algorithm Compass_Heading(); // Calculate magnetic heading Matrix_update(); Normalize(); Drift_correction(); Euler_angles(); +#endif // DEBUG__USE_DMP_M0 if (output_stream_on || output_single_on) output_angles(); } @@ -917,12 +966,16 @@ void loop() // Apply sensor calibration compensate_sensor_errors(); +#if DEBUG__USE_DMP_M0 == true + Euler_angles_DMP_M0(); +#else // Run DCM algorithm Compass_Heading(); // Calculate magnetic heading Matrix_update(); Normalize(); Drift_correction(); Euler_angles(); +#endif // DEBUG__USE_DMP_M0 if (output_stream_on || output_single_on) output_both_angles_and_sensors_text(); } @@ -943,5 +996,12 @@ void loop() { LOG_PORT.println("waiting..."); } +#else +#if DEBUG__ADD_LOOP_DELAY == true + else + { + delay(DEBUG__LOOP_DELAY); + } +#endif // DEBUG__ADD_LOOP_DELAY #endif } diff --git a/Arduino/Razor_AHRS/Sensors.ino b/Arduino/Razor_AHRS/Sensors.ino index acc0e86..c98bba6 100644 --- a/Arduino/Razor_AHRS/Sensors.ino +++ b/Arduino/Razor_AHRS/Sensors.ino @@ -106,6 +106,42 @@ void loop_imu() magnetom[1] = (-10.0f*imu.calcMag(imu.mx)); magnetom[2] = (10.0f*imu.calcMag(imu.mz)); } + +#if DEBUG__USE_DMP_M0 == true +void Euler_angles_DMP_M0(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]; + + imu.computeEulerAngles(); + + // Convert from NWU to NED... + pitch = -imu.pitch*PI/180.0f; + roll = imu.roll*PI/180.0f; + + Compass_Heading(); // Calculate magnetic heading + + float magyaw = -MAG_Heading; + float imuyaw = imu.yaw*PI/180.0f; + float fusionyaw = 0; + float magfusioncoef = 0.0f; + if (initialmagyaw == -10000) initialmagyaw = magyaw; + if (initialimuyaw == -10000) initialimuyaw = imuyaw; + if (!DEBUG__NO_DRIFT_CORRECTION) + fusionyaw = magyaw*180.0f/PI; + else + fusionyaw = atan2((1-magfusioncoef)*sin(imuyaw+initialmagyaw-initialimuyaw)+magfusioncoef*sin(magyaw), + (1-magfusioncoef)*cos(imuyaw+initialmagyaw-initialimuyaw)+magfusioncoef*cos(magyaw))*180.0f/PI; + + // Convert from NWU to NED... + yaw = -fusionyaw*PI/180.0f; +} +#endif // DEBUG__USE_DMP_M0 #else // I2C code to read the sensors From af774878bb267bfe29ff30e0044343911141b16b Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Wed, 6 Dec 2017 22:40:45 +0100 Subject: [PATCH 16/26] Added various options to determine the most stable configuration for the M0 --- Arduino/Razor_AHRS/Razor_AHRS.ino | 49 ++++++++++---- Arduino/Razor_AHRS/Sensors.ino | 102 ++++++++++++++++++++++++++---- README.md | 9 --- 3 files changed, 124 insertions(+), 36 deletions(-) diff --git a/Arduino/Razor_AHRS/Razor_AHRS.ino b/Arduino/Razor_AHRS/Razor_AHRS.ino index 44dfb75..805e539 100644 --- a/Arduino/Razor_AHRS/Razor_AHRS.ino +++ b/Arduino/Razor_AHRS/Razor_AHRS.ino @@ -56,6 +56,8 @@ * * Attempts to fix random nan problems in orientation computations. * * Added an option to get yaw/pitch/roll from the M0 DMP. * * Added a command to enable/disable the use of magnetometers for yaw computation. +* * v1.5.5 +* * Added various options to determine the most stable configuration for the M0. * * TODOs: * * Allow optional use of Flash/EEPROM for storing and reading calibration values. @@ -261,7 +263,7 @@ boolean output_errors = false; // true or false /*****************************************************************/ // 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! -// For the M0, only the extended magnetometer calibration seems to be really necessary... +// For the M0, only the extended magnetometer calibration seems to be really necessary if DEBUG__USE_DMP_M0 is set to true... // Accelerometer // "accel x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX" float ACCEL_X_MIN = -250; @@ -340,8 +342,19 @@ boolean DEBUG__NO_DRIFT_CORRECTION = false; #define DEBUG__ADD_LOOP_DELAY false #define DEBUG__LOOP_DELAY 1 - +// Set to true to enable auto-calibration features of the M0 (does not apply to magnetometers) #define DEBUG__USE_DMP_M0 false +// Set to true to disable the use of the DCM algorithm +#define DEBUG__USE_ONLY_DMP_M0 false + +#define DEBUG__ENABLE_FIFO_M0 false + +#define DEBUG__ENABLE_INTERRUPT_M0 false + +#define DEBUG__USE_DEFAULT_GYRO_FSR_M0 false + +#define DEBUG__USE_DEFAULT_ACCEL_FSR_M0 false + /*****************************************************************/ @@ -374,10 +387,20 @@ boolean DEBUG__NO_DRIFT_CORRECTION = false; MPU9250_DMP imu; // Create an instance of the MPU9250_DMP class +#if DEBUG__USE_ONLY_DMP_M0 == true +#undef DEBUG__USE_DMP_M0 +#define DEBUG__USE_DMP_M0 true +#endif // DEBUG__USE_ONLY_DMP_M0 + #if DEBUG__USE_DMP_M0 == true +#undef DEBUG__ENABLE_FIFO_M0 +#define DEBUG__ENABLE_FIFO_M0 true +#endif // DEBUG__USE_DMP_M0 + +#if DEBUG__USE_ONLY_DMP_M0 == true float initialmagyaw = -10000; float initialimuyaw = -10000; -#endif // DEBUG__USE_DMP_M0 +#endif // DEBUG__USE_ONLY_DMP_M0 #else #include #endif // HW__VERSION_CODE @@ -609,11 +632,11 @@ void setup() // Init sensors delay(50); // Give sensors enough time to start #if HW__VERSION_CODE == 14001 +#if DEBUG__ENABLE_INTERRUPT_M0 == true // Set up MPU-9250 interrupt input (active-low) pinMode(MPU9250_INT_PIN, INPUT_PULLUP); - //delay(50); // Give sensors enough time to start +#endif // DEBUG__ENABLE_INTERRUPT_M0 initIMU(); - //delay(50); // Give sensors enough time to start #else I2C_Init(); Accel_Init(); @@ -903,11 +926,11 @@ void loop() else if (command == 'I') // Toggle _i_nertial-only mode for yaw computation { DEBUG__NO_DRIFT_CORRECTION = !DEBUG__NO_DRIFT_CORRECTION; -#if DEBUG__USE_DMP_M0 == true +#if DEBUG__USE_ONLY_DMP_M0 == true // Update reference for yaw... initialmagyaw = -MAG_Heading; initialimuyaw = imu.yaw*PI/180.0f; -#endif // DEBUG__USE_DMP_M0 +#endif // DEBUG__USE_ONLY_DMP_M0 } #if OUTPUT__HAS_RN_BLUETOOTH == true // Read messages from bluetooth module @@ -948,8 +971,8 @@ void loop() // Apply sensor calibration compensate_sensor_errors(); -#if DEBUG__USE_DMP_M0 == true - Euler_angles_DMP_M0(); +#if DEBUG__USE_ONLY_DMP_M0 == true + Euler_angles_only_DMP_M0(); #else // Run DCM algorithm Compass_Heading(); // Calculate magnetic heading @@ -957,7 +980,7 @@ void loop() Normalize(); Drift_correction(); Euler_angles(); -#endif // DEBUG__USE_DMP_M0 +#endif // DEBUG__USE_ONLY_DMP_M0 if (output_stream_on || output_single_on) output_angles(); } @@ -966,8 +989,8 @@ void loop() // Apply sensor calibration compensate_sensor_errors(); -#if DEBUG__USE_DMP_M0 == true - Euler_angles_DMP_M0(); +#if DEBUG__USE_ONLY_DMP_M0 == true + Euler_angles_only_DMP_M0(); #else // Run DCM algorithm Compass_Heading(); // Calculate magnetic heading @@ -975,7 +998,7 @@ void loop() Normalize(); Drift_correction(); Euler_angles(); -#endif // DEBUG__USE_DMP_M0 +#endif // DEBUG__USE_ONLY_DMP_M0 if (output_stream_on || output_single_on) output_both_angles_and_sensors_text(); } diff --git a/Arduino/Razor_AHRS/Sensors.ino b/Arduino/Razor_AHRS/Sensors.ino index c98bba6..cd24b86 100644 --- a/Arduino/Razor_AHRS/Sensors.ino +++ b/Arduino/Razor_AHRS/Sensors.ino @@ -1,11 +1,32 @@ /* This file is part of the Razor AHRS Firmware */ #if HW__VERSION_CODE == 14001 +/* +* Known Bug - +* DMP when enabled will sample sensor data at 200Hz and output to FIFO at the rate +* specified in the dmp_set_fifo_rate API. The DMP will then sent an interrupt once +* a sample has been put into the FIFO. Therefore if the dmp_set_fifo_rate is at 25Hz +* there will be a 25Hz interrupt from the MPU device. +* +* There is a known issue in which if you do not enable DMP_FEATURE_TAP +* then the interrupts will be at 200Hz even if fifo rate +* is set at a different rate. To avoid this issue include the DMP_FEATURE_TAP +* +* DMP sensor fusion works only with gyro at +-2000dps and accel +-2G +*/ #define DMP_SAMPLE_RATE 100 // Logging/DMP sample rate(4-200 Hz) #define IMU_COMPASS_SAMPLE_RATE 100 // Compass sample rate (4-100 Hz) #define IMU_AG_SAMPLE_RATE 100 // Accel/gyro sample rate Must be between 4Hz and 1kHz +#if DEBUG__USE_DEFAULT_GYRO_FSR_M0 == true #define IMU_GYRO_FSR 2000 // Gyro full-scale range (250, 500, 1000, or 2000) +#else +#define IMU_GYRO_FSR 500 // Gyro full-scale range (250, 500, 1000, or 2000) +#endif // DEBUG__USE_DEFAULT_GYRO_FSR_M0 +#if DEBUG__USE_DEFAULT_ACCEL_FSR_M0 == true +#define IMU_ACCEL_FSR 2 // Accel full-scale range (2, 4, 8, or 16) +#else #define IMU_ACCEL_FSR 16 // Accel full-scale range (2, 4, 8, or 16) +#endif // DEBUG__USE_DEFAULT_ACCEL_FSR_M0 #define IMU_AG_LPF 5 // Accel/Gyro LPF corner frequency (5, 10, 20, 42, 98, or 188 Hz) #define ENABLE_GYRO_CALIBRATION true @@ -20,10 +41,24 @@ bool initIMU(void) if (imu.begin() != INV_SUCCESS) return false; - // Set up MPU-9250 interrupt: - imu.enableInterrupt(); // Enable interrupt output - imu.setIntLevel(1); // Set interrupt to active-low - imu.setIntLatched(1); // Latch interrupt output +#if DEBUG__ENABLE_INTERRUPT_M0 == true + // Use enableInterrupt() to configure the MPU-9250's + // interrupt output as a "data ready" indicator. + imu.enableInterrupt(); + + // The interrupt level can either be active-high or low. + // Configure as active-low, since we'll be using the pin's + // internal pull-up resistor. + // Options are INT_ACTIVE_LOW or INT_ACTIVE_HIGH + imu.setIntLevel(INT_ACTIVE_LOW); + + // The interrupt can be set to latch until data has + // been read, or to work as a 50us pulse. + // Use latching method -- we'll read from the sensor + // as soon as we see the pin go LOW. + // Options are INT_LATCHED or INT_50US_PULSE + imu.setIntLatched(INT_LATCHED); +#endif // DEBUG__ENABLE_INTERRUPT_M0 // Configure sensors: // Set gyro full-scale range: options are 250, 500, 1000, or 2000: @@ -38,9 +73,11 @@ bool initIMU(void) // Set compass sample rate: between 4-100Hz imu.setCompassSampleRate(IMU_COMPASS_SAMPLE_RATE); +#if DEBUG__USE_DMP_M0 == true // Configure digital motion processor. Use the FIFO to get // data from the DMP. unsigned short dmpFeatureMask = 0; + //dmp_set_interrupt_mode(DMP_INT_CONTINUOUS); if (ENABLE_GYRO_CALIBRATION) { // Gyro calibration re-calibrates the gyro after a set amount @@ -56,9 +93,23 @@ bool initIMU(void) // Add accel and quaternion's to the DMP dmpFeatureMask |= DMP_FEATURE_SEND_RAW_ACCEL; dmpFeatureMask |= DMP_FEATURE_6X_LP_QUAT; + // Because of known issue without DMP_FEATURE_TAP... + dmpFeatureMask |= DMP_FEATURE_TAP; // Initialize the DMP, and set the FIFO's update rate: imu.dmpBegin(dmpFeatureMask, fifoRate); +#else +#if DEBUG__ENABLE_FIFO_M0 == true + // Use configureFifo to set which sensors should be stored + // in the buffer. + // Parameter to this function can be: INV_XYZ_GYRO, + // INV_XYZ_ACCEL, INV_X_GYRO, INV_Y_GYRO, or INV_Z_GYRO + imu.configureFifo(INV_XYZ_GYRO|INV_XYZ_ACCEL); + + // Compass needs to be enabled again here due to a side effect of mpu_configure_fifo()... + imu.setSensors(INV_XYZ_GYRO|INV_XYZ_ACCEL|INV_XYZ_COMPASS); +#endif // DEBUG__ENABLE_FIFO_M0 +#endif // DEBUG__USE_DMP_M0 return true; // Return success } @@ -66,7 +117,15 @@ bool initIMU(void) void loop_imu() { // Check IMU for new data, and log it. +#if DEBUG__USE_DMP_M0 == true + if (!imu.fifoAvailable()) +#else +#if DEBUG__ENABLE_FIFO_M0 == true if (!imu.fifoAvailable()) +#else + if (!imu.dataReady()) +#endif // DEBUG__ENABLE_FIFO_M0 +#endif // DEBUG__USE_DMP_M0 { num_accel_errors++; if (output_errors) LOG_PORT.println("!ERR: reading accelerometer"); @@ -75,14 +134,29 @@ void loop_imu() return; } - // Read from the digital motion processor's FIFO. - if (imu.dmpUpdateFifo() != INV_SUCCESS) + // Get the latest data... +#if DEBUG__USE_DMP_M0 == true + while (imu.fifoAvailable()) { - num_accel_errors++; - if (output_errors) LOG_PORT.println("!ERR: reading accelerometer"); - num_gyro_errors++; - if (output_errors) LOG_PORT.println("!ERR: reading gyroscope"); - return; + // Read from the digital motion processor's FIFO. + if (imu.dmpUpdateFifo() != INV_SUCCESS) +#else +#if DEBUG__ENABLE_FIFO_M0 == true + while (imu.fifoAvailable()) + { + if (imu.updateFifo() != INV_SUCCESS) +#else + { + if (imu.update(UPDATE_ACCEL|UPDATE_GYRO) != INV_SUCCESS) +#endif // DEBUG__ENABLE_FIFO_M0 +#endif // DEBUG__USE_DMP_M0 + { + num_accel_errors++; + if (output_errors) LOG_PORT.println("!ERR: reading accelerometer"); + num_gyro_errors++; + if (output_errors) LOG_PORT.println("!ERR: reading gyroscope"); + return; + } } // Read from the compass. @@ -107,8 +181,8 @@ void loop_imu() magnetom[2] = (10.0f*imu.calcMag(imu.mz)); } -#if DEBUG__USE_DMP_M0 == true -void Euler_angles_DMP_M0(void) +#if DEBUG__USE_ONLY_DMP_M0 == true +void Euler_angles_only_DMP_M0(void) { Gyro_Vector[0] = GYRO_SCALED_RAD(gyro[0]); //gyro x roll Gyro_Vector[1] = GYRO_SCALED_RAD(gyro[1]); //gyro y pitch @@ -141,7 +215,7 @@ void Euler_angles_DMP_M0(void) // Convert from NWU to NED... yaw = -fusionyaw*PI/180.0f; } -#endif // DEBUG__USE_DMP_M0 +#endif // DEBUG__USE_ONLY_DMP_M0 #else // I2C code to read the sensors diff --git a/README.md b/README.md index 7481dc3..1b48df5 100644 --- a/README.md +++ b/README.md @@ -58,12 +58,3 @@ Building your own app: ### Optional: ROS Interface See http://wiki.ros.org/razor_imu_9dof. - -Donation ---- - -If the code and the tutorial helped you and you'd like to buy me a beer to say thanks, I'll be happy and say cheers! - -PayPal: - -[![](https://www.paypalobjects.com/en_US/i/btn/btn_donateCC_LG.gif)](https://www.paypal.com/cgi-bin/webscr?cmd=_s-xclick&hosted_button_id=KNQHN837SKX8Q) From fcb68f5e4a55cc0309674adf2045451cf4774757 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Thu, 7 Dec 2017 16:34:51 +0100 Subject: [PATCH 17/26] Removed some unnecessary math error checking, set back gyro full-scale range to the maximum and increased startup delay for the M0 --- Arduino/Razor_AHRS/Razor_AHRS.ino | 12 ++++++++++-- Arduino/Razor_AHRS/Sensors.ino | 2 +- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/Arduino/Razor_AHRS/Razor_AHRS.ino b/Arduino/Razor_AHRS/Razor_AHRS.ino index 805e539..3d9988a 100644 --- a/Arduino/Razor_AHRS/Razor_AHRS.ino +++ b/Arduino/Razor_AHRS/Razor_AHRS.ino @@ -58,6 +58,10 @@ * * Added a command to enable/disable the use of magnetometers for yaw computation. * * v1.5.5 * * Added various options to determine the most stable configuration for the M0. +* * v1.5.6 +* * Removed some unnecessary math error checking. +* * Set back gyro full-scale range to the maximum for the M0. +* * Increased startup delay to try to get a correct initial orientation for the M0. * * TODOs: * * Allow optional use of Flash/EEPROM for storing and reading calibration values. @@ -534,7 +538,7 @@ void reset_sensor_fusion() { // GET PITCH // Using y-z-plane-component/x-component of gravity vector - if ((accel[0] == 0)&&(sqrt(accel[1] * accel[1] + accel[2] * accel[2]) == 0)) num_math_errors++; else pitch = -atan2(accel[0], sqrt(accel[1] * accel[1] + accel[2] * accel[2])); // Attempt to prevent nan problems... + pitch = -atan2(accel[0], sqrt(accel[1] * accel[1] + accel[2] * accel[2])); // GET ROLL // Compensate pitch of gravity vector @@ -543,7 +547,7 @@ void reset_sensor_fusion() { // 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: - if ((temp2[1] == 0)&&(temp2[2] == 0)) num_math_errors++; else roll = atan2(temp2[1], temp2[2]); // Attempt to prevent nan problems... + roll = atan2(temp2[1], temp2[2]); // GET YAW Compass_Heading(); @@ -645,7 +649,11 @@ void setup() #endif // HW__VERSION_CODE // Read sensors, init DCM algorithm +#if HW__VERSION_CODE == 14001 + delay(400); // Give sensors enough time to collect data +#else delay(20); // Give sensors enough time to collect data +#endif // HW__VERSION_CODE reset_sensor_fusion(); // Init output diff --git a/Arduino/Razor_AHRS/Sensors.ino b/Arduino/Razor_AHRS/Sensors.ino index cd24b86..c59e2f9 100644 --- a/Arduino/Razor_AHRS/Sensors.ino +++ b/Arduino/Razor_AHRS/Sensors.ino @@ -20,7 +20,7 @@ #if DEBUG__USE_DEFAULT_GYRO_FSR_M0 == true #define IMU_GYRO_FSR 2000 // Gyro full-scale range (250, 500, 1000, or 2000) #else -#define IMU_GYRO_FSR 500 // Gyro full-scale range (250, 500, 1000, or 2000) +#define IMU_GYRO_FSR 2000 // Gyro full-scale range (250, 500, 1000, or 2000) #endif // DEBUG__USE_DEFAULT_GYRO_FSR_M0 #if DEBUG__USE_DEFAULT_ACCEL_FSR_M0 == true #define IMU_ACCEL_FSR 2 // Accel full-scale range (2, 4, 8, or 16) From a1157440f2850441ed86e0b285e4792d5714d4af Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Thu, 7 Dec 2017 17:12:14 +0100 Subject: [PATCH 18/26] Calibration data are now also used to compute the initial orientation --- Arduino/Razor_AHRS/Razor_AHRS.ino | 57 ++++++++++++++++--------------- 1 file changed, 30 insertions(+), 27 deletions(-) diff --git a/Arduino/Razor_AHRS/Razor_AHRS.ino b/Arduino/Razor_AHRS/Razor_AHRS.ino index 3d9988a..c65b8e8 100644 --- a/Arduino/Razor_AHRS/Razor_AHRS.ino +++ b/Arduino/Razor_AHRS/Razor_AHRS.ino @@ -62,6 +62,8 @@ * * Removed some unnecessary math error checking. * * Set back gyro full-scale range to the maximum for the M0. * * Increased startup delay to try to get a correct initial orientation for the M0. +* * v1.5.7 +* * Calibration data are now also used to compute the initial orientation. * * TODOs: * * Allow optional use of Flash/EEPROM for storing and reading calibration values. @@ -525,6 +527,33 @@ void recalculateMagnCalibration() { MAGN_Z_SCALE = (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET)); } +// 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) + { + 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; + } + + // Compensate gyroscope error + gyro[0] -= GYRO_AVERAGE_OFFSET_X; + gyro[1] -= GYRO_AVERAGE_OFFSET_Y; + gyro[2] -= GYRO_AVERAGE_OFFSET_Z; +} + // Read every sensor and record a time stamp // Init DCM with unfiltered orientation // TODO re-init global vars? @@ -534,6 +563,7 @@ void reset_sensor_fusion() { float xAxis[3] = {1, 0, 0}; read_sensors(); + compensate_sensor_errors(); timestamp = millis(); // GET PITCH @@ -557,33 +587,6 @@ void reset_sensor_fusion() { 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) - { - 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; - } - - // 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() { From 2520ab9f32ace425d25f837280bd814172e57939 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Wed, 20 Dec 2017 16:38:21 +0100 Subject: [PATCH 19/26] Updated README and comments --- Arduino/Razor_AHRS/Razor_AHRS.ino | 2 +- README.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Arduino/Razor_AHRS/Razor_AHRS.ino b/Arduino/Razor_AHRS/Razor_AHRS.ino index c65b8e8..13b113c 100644 --- a/Arduino/Razor_AHRS/Razor_AHRS.ino +++ b/Arduino/Razor_AHRS/Razor_AHRS.ino @@ -140,7 +140,7 @@ "#ox" - Output angles and linear acceleration and rotational velocity. Angles are in degrees, acceleration is in units of 1.0 = 1/256 G (9.8/256 m/s^2). Rotational - velocity is in rad/s^2. (Output frames have form like + velocity is in rad/s. (Output frames have form like "#YPRAG=-142.28,-5.38,33.52,0.1,0.1,1.0,0.01,0.01,0.01", followed by carriage return and line feed [\r\n]). diff --git a/README.md b/README.md index 1b48df5..cdba59d 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,7 @@ Clone the [repository on GitHub](https://github.com/lebarsfa/razor-9dof-ahrs) or Tutorial --- -You find a [detailed tutorial in the Wiki](https://github.com/ptrbrtz/razor-9dof-ahrs/wiki/Tutorial). +You find a [detailed tutorial in the Wiki](https://github.com/ptrbrtz/razor-9dof-ahrs/wiki/Tutorial). Note: for SEN-14001 (*9DoF Razor IMU M0*), you will need to follow the same instructions as for the default firmware on https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide and use an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library (an updated version of the default firmware is also available on https://github.com/lebarsfa/9DOF_Razor_IMU). Quick setup From 8020c610a8f3e5e44622c77f5feba1a986042c8e Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Wed, 27 Dec 2017 00:10:59 +0100 Subject: [PATCH 20/26] Added Windows compatibility to C++ sample --- C++/Example.cpp | 14 +- C++/RazorAHRS.cpp | 43 +- C++/RazorAHRS.h | 9 +- C++/RazorAHRS.pro | 27 ++ C++/RazorAHRS.sln | 36 ++ C++/RazorAHRS.vcxproj | 119 ++++++ C++/RazorAHRS.vcxproj.filters | 54 +++ C++/RazorAHRS_Linux.vcxproj | 130 ++++++ C++/RazorAHRS_Linux.vcxproj.filters | 30 ++ C++/unix_adapt/bits/fcntl2.h | 223 ++++++++++ C++/unix_adapt/bits/termios.h | 219 ++++++++++ C++/unix_adapt/sys/time.h | 148 +++++++ C++/unix_adapt/termios.h | 610 ++++++++++++++++++++++++++++ C++/unix_adapt/unistd.h | 102 +++++ README.md | 8 +- 15 files changed, 1745 insertions(+), 27 deletions(-) create mode 100644 C++/RazorAHRS.pro create mode 100644 C++/RazorAHRS.sln create mode 100644 C++/RazorAHRS.vcxproj create mode 100644 C++/RazorAHRS.vcxproj.filters create mode 100644 C++/RazorAHRS_Linux.vcxproj create mode 100644 C++/RazorAHRS_Linux.vcxproj.filters create mode 100644 C++/unix_adapt/bits/fcntl2.h create mode 100644 C++/unix_adapt/bits/termios.h create mode 100644 C++/unix_adapt/sys/time.h create mode 100644 C++/unix_adapt/termios.h create mode 100644 C++/unix_adapt/unistd.h diff --git a/C++/Example.cpp b/C++/Example.cpp index 7da7caf..3557d49 100644 --- a/C++/Example.cpp +++ b/C++/Example.cpp @@ -1,5 +1,5 @@ /****************************************************************************************** -* Test Program: Mac OSX / Unix / Linux C++ Interface for Razor AHRS v1.4.2 +* Test Program: Mac OSX / Unix / Linux / Windows C++ Interface for Razor AHRS * 9 Degree of Measurement Attitude and Heading Reference System * for Sparkfun "9DOF Razor IMU" and "9DOF Sensor Stick" * @@ -20,12 +20,15 @@ using namespace std; - // Set your serial port here! +#ifdef _WIN32 +const string serial_port_name = "COM5"; +#else //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 +const string serial_port_name = "/dev/ttyACM0"; // a good guess for the M0 on Linux +#endif // _WIN32 // Razor error callback handler // Will be called from (and in) Razor background thread! @@ -107,4 +110,3 @@ int main() getchar(); // wait for RETURN key return 0; } - diff --git a/C++/RazorAHRS.cpp b/C++/RazorAHRS.cpp index 42b8d2e..7c2fe5b 100644 --- a/C++/RazorAHRS.cpp +++ b/C++/RazorAHRS.cpp @@ -1,5 +1,5 @@ /****************************************************************************************** -* Mac OSX / Unix / Linux C++ Interface for Razor AHRS v1.4.2 +* Mac OSX / Unix / Linux / Windows C++ Interface for Razor AHRS * 9 Degree of Measurement Attitude and Heading Reference System * for Sparkfun "9DOF Razor IMU" and "9DOF Sensor Stick" * @@ -8,13 +8,26 @@ * Copyright (C) 2011-2012 Quality & Usability Lab, Deutsche Telekom Laboratories, TU Berlin * Written by Peter Bartz (peter-bartz@gmx.de) * -* Infos, updates, bug reports,contributions and feedback: +* Infos, updates, bug reports, contributions and feedback: * https://github.com/ptrbrtz/razor-9dof-ahrs ******************************************************************************************/ #include "RazorAHRS.h" #include +#ifdef _WIN32 +#include // for open(), ... + +// Since open(), close(), read(), write() functions already exist on Windows but with a +// limited compatibility with Linux, we need to replace them with other versions... +#define open open_linux +#define close close_linux +#define read read_linux +#define write write_linux +#endif // _WIN32 + +static pthread_t null_pthread_id = {0}; + RazorAHRS::RazorAHRS(const std::string &port, DataCallbackFunc data_func, ErrorCallbackFunc error_func, Mode mode, int connect_timeout_ms, speed_t speed) : _mode(mode) @@ -22,7 +35,7 @@ RazorAHRS::RazorAHRS(const std::string &port, DataCallbackFunc data_func, ErrorC , _connect_timeout_ms(connect_timeout_ms) , data(data_func) , error(error_func) - , _thread_id(0) + , _thread_id(null_pthread_id) , _stop_thread(false) { // check data type sizes @@ -89,8 +102,8 @@ RazorAHRS::RazorAHRS(const std::string &port, DataCallbackFunc data_func, ErrorC RazorAHRS::~RazorAHRS() { // if thread was started, stop thread - if (_thread_id) _stop_io_thread(); - close(_serial_port); + if (!pthread_equal(_thread_id, null_pthread_id)) _stop_io_thread(); + if (_serial_port != -1) close(_serial_port); } bool @@ -116,8 +129,8 @@ RazorAHRS::_read_token(const std::string &token, char c) bool RazorAHRS::_init_razor() { - char in; - int result; + char in = 0; + int result = 0; struct timeval t0, t1, t2; const std::string synch_token = "#SYNCH"; const std::string new_line = "\r\n"; @@ -139,7 +152,7 @@ RazorAHRS::_init_razor() while (true) { // try to read one byte from the port - result = read(_serial_port, &in, 1); + result = (int)read(_serial_port, &in, 1); // one byte read if (result > 0) @@ -194,7 +207,7 @@ RazorAHRS::_init_razor() while (true) { // try to read one byte from the port - result = read(_serial_port, &in, 1); + result = (int)read(_serial_port, &in, 1); // one byte read if (result > 0) @@ -228,14 +241,14 @@ RazorAHRS::_open_serial_port(const char *port) } // something didn't work - close(_serial_port); + if (_serial_port != -1) close(_serial_port); return false; } bool RazorAHRS::_set_blocking_io() { - int flags; + int flags = 0; // clear O_NDELAY to make I/O blocking again // in fact this is semi-blocking, since we set VTIME on the port @@ -251,7 +264,7 @@ RazorAHRS::_set_blocking_io() bool RazorAHRS::_set_nonblocking_io() { - int flags; + int flags = 0; // set O_NDELAY to make I/O non-blocking if (((flags = fcntl(_serial_port, F_GETFL, 0)) != -1) && @@ -272,8 +285,8 @@ RazorAHRS::_is_io_blocking() void* RazorAHRS::_thread(void *arg) { - char c; - int result; + char c = 0; + int result = 0; try { @@ -291,7 +304,7 @@ RazorAHRS::_thread(void *arg) while (!_stop_thread) { - if ((result = read(_serial_port, &c, 1)) > 0) // blocks only for VTIME before returning + if ((result = (int)read(_serial_port, &c, 1)) > 0) // blocks only for VTIME before returning { // read binary stream // (type-punning: aliasing with char* is ok) diff --git a/C++/RazorAHRS.h b/C++/RazorAHRS.h index 9cbd897..ca8c3ad 100644 --- a/C++/RazorAHRS.h +++ b/C++/RazorAHRS.h @@ -1,10 +1,10 @@ /****************************************************************************************** -* Mac OSX / Unix / Linux C++ Interface for Razor AHRS v1.4.2 +* Mac OSX / Unix / Linux / Windows C++ Interface for Razor AHRS * 9 Degree of Measurement Attitude and Heading Reference System * for Sparkfun "9DOF Razor IMU" and "9DOF Sensor Stick" * * Released under GNU GPL (General Public License) v3.0 -* Copyright (C) 2013 Peter Bartz Bartz [http://ptrbrtz.net] +* Copyright (C) 2013 Peter Bartz [http://ptrbrtz.net] * Copyright (C) 2011-2012 Quality & Usability Lab, Deutsche Telekom Laboratories, TU Berlin * Written by Peter Bartz (peter-bartz@gmx.de) * @@ -17,7 +17,11 @@ #include #include +#ifdef _MSC_VER +#include +#else #include +#endif // _MSC_VER #include #include #include // for write(), close(), ... @@ -25,6 +29,7 @@ #include // for open(), ... #include #include +#include #ifndef _REENTRANT #error You need to compile with _REENTRANT defined since this uses threads! diff --git a/C++/RazorAHRS.pro b/C++/RazorAHRS.pro new file mode 100644 index 0000000..2805cfc --- /dev/null +++ b/C++/RazorAHRS.pro @@ -0,0 +1,27 @@ +TEMPLATE = app +CONFIG += console c++11 +CONFIG -= app_bundle +CONFIG -= qt + +DEFINES -= UNICODE + +win32:INCLUDEPATH += unix_adapt +win32:DEFINES += DISABLE_TIMEZONE_STRUCT_REDEFINITION + +DEFINES += _REENTRANT +LIBS += -lpthread + +# Uncomment if using Visual Studio compiler for Qt, install also Pthreads-win32 2.9.1 +# from http://www.ensta-bretagne.fr/lebars/Share/pthreads-win32-msvc.zip (only in 32 bit). +#win32:INCLUDEPATH += "C:\Program Files (x86)\pthreads-w32-2.9.1-msvc" +#win32:DEFINES += HAVE_STRUCT_TIMESPEC +#win32:LIBS -= -lpthread +#win32:LIBS += -L"C:\Program Files (x86)\pthreads-w32-2.9.1-msvc" +#win32:LIBS += -lpthreadVC2 + +SOURCES += \ + Example.cpp \ + RazorAHRS.cpp + +HEADERS += \ + RazorAHRS.h diff --git a/C++/RazorAHRS.sln b/C++/RazorAHRS.sln new file mode 100644 index 0000000..612e85b --- /dev/null +++ b/C++/RazorAHRS.sln @@ -0,0 +1,36 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio 15 +VisualStudioVersion = 15.0.26430.6 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "RazorAHRS", "RazorAHRS.vcxproj", "{53C89697-1513-4B96-99FE-09494B336B96}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "RazorAHRS_Linux", "RazorAHRS_Linux.vcxproj", "{4AC80B0F-8909-4D91-96C8-ACCD4A78B2C4}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {53C89697-1513-4B96-99FE-09494B336B96}.Debug|x64.ActiveCfg = Debug|Win32 + {53C89697-1513-4B96-99FE-09494B336B96}.Debug|x86.ActiveCfg = Debug|Win32 + {53C89697-1513-4B96-99FE-09494B336B96}.Debug|x86.Build.0 = Debug|Win32 + {53C89697-1513-4B96-99FE-09494B336B96}.Release|x64.ActiveCfg = Release|Win32 + {53C89697-1513-4B96-99FE-09494B336B96}.Release|x86.ActiveCfg = Release|Win32 + {53C89697-1513-4B96-99FE-09494B336B96}.Release|x86.Build.0 = Release|Win32 + {4AC80B0F-8909-4D91-96C8-ACCD4A78B2C4}.Debug|x64.ActiveCfg = Debug|x64 + {4AC80B0F-8909-4D91-96C8-ACCD4A78B2C4}.Debug|x64.Build.0 = Debug|x64 + {4AC80B0F-8909-4D91-96C8-ACCD4A78B2C4}.Debug|x86.ActiveCfg = Debug|x86 + {4AC80B0F-8909-4D91-96C8-ACCD4A78B2C4}.Debug|x86.Build.0 = Debug|x86 + {4AC80B0F-8909-4D91-96C8-ACCD4A78B2C4}.Release|x64.ActiveCfg = Release|x64 + {4AC80B0F-8909-4D91-96C8-ACCD4A78B2C4}.Release|x64.Build.0 = Release|x64 + {4AC80B0F-8909-4D91-96C8-ACCD4A78B2C4}.Release|x86.ActiveCfg = Release|x86 + {4AC80B0F-8909-4D91-96C8-ACCD4A78B2C4}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/C++/RazorAHRS.vcxproj b/C++/RazorAHRS.vcxproj new file mode 100644 index 0000000..f73c5f7 --- /dev/null +++ b/C++/RazorAHRS.vcxproj @@ -0,0 +1,119 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + + + + + + + + + + + + + + {53C89697-1513-4B96-99FE-09494B336B96} + RazorAHRS + Win32Proj + 10.0.15063.0 + + + + Application + v141 + NotSet + true + + + Application + v141 + NotSet + + + + + + + + + + + + + <_ProjectFileVersion>11.0.61030.0 + + + $(ProjectDir)$(Configuration)\ + $(ProjectDir)$(Configuration)\ + true + false + C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\Team Tools\Static Analysis Tools\Rule Sets\NativeRecommendedRules.ruleset + + + $(ProjectDir)$(Configuration)\ + $(ProjectDir)$(Configuration)\ + false + false + NativeRecommendedRules.ruleset + + + + Disabled + WIN32;HAVE_STRUCT_TIMESPEC;_REENTRANT;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + EnableFastChecks + MultiThreadedDebugDLL + + Level4 + EditAndContinue + false + .;..;unix_adapt;$(ProgramFiles)\pthreads-w32-2.9.1-msvc; + + + pthreadVC2.lib;%(AdditionalDependencies) + true + Console + false + + MachineX86 + $(ProgramFiles)\pthreads-w32-2.9.1-msvc + + + + + + WIN32;HAVE_STRUCT_TIMESPEC;_REENTRANT;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + MultiThreaded + + Level4 + ProgramDatabase + false + .;..;unix_adapt;$(ProgramFiles)\pthreads-w32-2.9.1-msvc; + + + pthreadVC2.lib;%(AdditionalDependencies) + true + Console + true + true + false + + MachineX86 + $(ProgramFiles)\pthreads-w32-2.9.1-msvc + + + + + + \ No newline at end of file diff --git a/C++/RazorAHRS.vcxproj.filters b/C++/RazorAHRS.vcxproj.filters new file mode 100644 index 0000000..8c7e300 --- /dev/null +++ b/C++/RazorAHRS.vcxproj.filters @@ -0,0 +1,54 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hpp;hxx;hm;inl;inc;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav + + + {1065271c-70ea-4715-a4a9-4fe59dc81026} + + + {aade8419-768f-4238-9ea0-c1ed0b406f71} + + + {5a57374b-444c-4a81-9434-c047ee01f2a7} + + + + + Header Files + + + unix_adapt + + + unix_adapt + + + unix_adapt\bits + + + unix_adapt\bits + + + unix_adapt\sys + + + + + Source Files + + + Source Files + + + \ No newline at end of file diff --git a/C++/RazorAHRS_Linux.vcxproj b/C++/RazorAHRS_Linux.vcxproj new file mode 100644 index 0000000..cfc8a2b --- /dev/null +++ b/C++/RazorAHRS_Linux.vcxproj @@ -0,0 +1,130 @@ + + + + + Debug + x86 + + + Release + x86 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + + {4AC80B0F-8909-4D91-96C8-ACCD4A78B2C4} + Linux + RazorAHRS_Linux + 14.0 + Linux + 1.0 + Generic + {D51BCBC9-82E9-4017-911E-C93873C4EA2B} + + + + true + + + false + + + true + + + false + + + + + + + + $(IncludePath);$(ISenseIncludePath) + + + $(IncludePath);$(ISenseIncludePath) + + + $(IncludePath);$(ISenseIncludePath) + + + $(IncludePath);$(ISenseIncludePath) + + + + _REENTRANT;NDEBUG;%(PreprocessorDefinitions) + $(LINUX_SDK_DIR)\include;$(LINUX_SDK_DIR)\include\x86_64-linux-gnu;$(LINUX_SDK_DIR)\local\include;/home/user/projects/$(ProjectName)/RazorAHRS;/home/user/projects/$(ProjectName);.;..;$(StlIncludeDirectories);%(AdditionalIncludeDirectories) + no-unknown-pragmas;switch;no-deprecated-declarations;empty-body;conversion;return-type;parentheses;no-pointer-sign;no-format;uninitialized;unreachable-code;unused-function;unused-value;unused-variable;%(CAdditionalWarning) + no-unknown-pragmas;switch;no-deprecated-declarations;empty-body;conversion;return-type;parentheses;no-format;uninitialized;unreachable-code;unused-function;unused-value;unused-variable;%(CppAdditionalWarning) + + + pthread;rt;m + + + + + + + + + $(LINUX_SDK_DIR)\include;$(LINUX_SDK_DIR)\include\x86_64-linux-gnu;$(LINUX_SDK_DIR)\local\include;/home/user/projects/$(ProjectName)/RazorAHRS;/home/user/projects/$(ProjectName);.;..;$(StlIncludeDirectories);%(AdditionalIncludeDirectories) + _REENTRANT;_DEBUG;%(PreprocessorDefinitions) + no-unknown-pragmas;switch;no-deprecated-declarations;empty-body;conversion;return-type;parentheses;no-pointer-sign;no-format;uninitialized;unreachable-code;unused-function;unused-value;unused-variable;%(CAdditionalWarning) + no-unknown-pragmas;switch;no-deprecated-declarations;empty-body;conversion;return-type;parentheses;no-format;uninitialized;unreachable-code;unused-function;unused-value;unused-variable;%(CppAdditionalWarning) + + + pthread;rt;m + + + + + + + + + _REENTRANT;NDEBUG;%(PreprocessorDefinitions) + $(LINUX_SDK_DIR)\include;$(LINUX_SDK_DIR)\include\x86_64-linux-gnu;$(LINUX_SDK_DIR)\local\include;/home/user/projects/$(ProjectName)/RazorAHRS;/home/user/projects/$(ProjectName);.;..;$(StlIncludeDirectories);%(AdditionalIncludeDirectories) + no-unknown-pragmas;switch;no-deprecated-declarations;empty-body;conversion;return-type;parentheses;no-pointer-sign;no-format;uninitialized;unreachable-code;unused-function;unused-value;unused-variable;%(CAdditionalWarning) + no-unknown-pragmas;switch;no-deprecated-declarations;empty-body;conversion;return-type;parentheses;no-format;uninitialized;unreachable-code;unused-function;unused-value;unused-variable;%(CppAdditionalWarning) + + + pthread;rt;m + + + + + + + + + $(LINUX_SDK_DIR)\include;$(LINUX_SDK_DIR)\include\x86_64-linux-gnu;$(LINUX_SDK_DIR)\local\include;/home/user/projects/$(ProjectName)/RazorAHRS;/home/user/projects/$(ProjectName);.;..;$(StlIncludeDirectories);%(AdditionalIncludeDirectories) + _REENTRANT;_DEBUG;%(PreprocessorDefinitions) + no-unknown-pragmas;switch;no-deprecated-declarations;empty-body;conversion;return-type;parentheses;no-pointer-sign;no-format;uninitialized;unreachable-code;unused-function;unused-value;unused-variable;%(CAdditionalWarning) + no-unknown-pragmas;switch;no-deprecated-declarations;empty-body;conversion;return-type;parentheses;no-format;uninitialized;unreachable-code;unused-function;unused-value;unused-variable;%(CppAdditionalWarning) + + + pthread;rt;m + + + + + + + + + \ No newline at end of file diff --git a/C++/RazorAHRS_Linux.vcxproj.filters b/C++/RazorAHRS_Linux.vcxproj.filters new file mode 100644 index 0000000..3bb3d7d --- /dev/null +++ b/C++/RazorAHRS_Linux.vcxproj.filters @@ -0,0 +1,30 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hpp;hxx;hm;inl;inc;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav + + + + + Header Files + + + + + Source Files + + + Source Files + + + \ No newline at end of file diff --git a/C++/unix_adapt/bits/fcntl2.h b/C++/unix_adapt/bits/fcntl2.h new file mode 100644 index 0000000..2bf9a7c --- /dev/null +++ b/C++/unix_adapt/bits/fcntl2.h @@ -0,0 +1,223 @@ +#ifndef FCNCTL2_H +#define FCNCTL2_H + +// Prevent Winsock.h from being included by the Windows.h header. +// This must be done if we plan to include Winsock2.h in other files. +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN +#endif // WIN32_LEAN_AND_MEAN +#include + +#include + +#include +#include +#include + +#ifdef _MSC_VER +// Disable some Visual Studio warnings. +#pragma warning(disable : 4996) +#endif // _MSC_VER + +/* open/fcntl. */ +#ifndef O_NOCTTY +# define O_NOCTTY 0400 /* Not fcntl. */ +#endif +//#ifndef O_TRUNC +//# define O_TRUNC 01000 /* Not fcntl. */ +//#endif +//#ifndef O_APPEND +//# define O_APPEND 02000 +//#endif +#ifndef O_NONBLOCK +# define O_NONBLOCK 04000 +#endif +#ifndef O_NDELAY +# define O_NDELAY O_NONBLOCK +#endif + +#ifndef __O_DIRECTORY +# define __O_DIRECTORY 0200000 +#endif +#ifndef __O_NOFOLLOW +# define __O_NOFOLLOW 0400000 +#endif +#ifndef __O_CLOEXEC +# define __O_CLOEXEC 02000000 +#endif +#ifndef __O_DIRECT +# define __O_DIRECT 040000 +#endif +#ifndef __O_NOATIME +# define __O_NOATIME 01000000 +#endif +#ifndef __O_PATH +# define __O_PATH 010000000 +#endif +#ifndef __O_TMPFILE +# define __O_TMPFILE (020000000 | __O_DIRECTORY) +#endif + +# define O_DIRECTORY __O_DIRECTORY /* Must be a directory. */ +# define O_NOFOLLOW __O_NOFOLLOW /* Do not follow links. */ +# define O_CLOEXEC __O_CLOEXEC /* Set close_on_exec. */ + +# define O_DIRECT __O_DIRECT /* Direct disk access. */ +# define O_NOATIME __O_NOATIME /* Do not set atime. */ +# define O_PATH __O_PATH /* Resolve pathname but do not open file. */ +# define O_TMPFILE __O_TMPFILE /* Atomically create nameless file. */ + +/* Values for the second argument to `fcntl'. */ +#define F_DUPFD 0 /* Duplicate file descriptor. */ +#define F_GETFD 1 /* Get file descriptor flags. */ +#define F_SETFD 2 /* Set file descriptor flags. */ +#define F_GETFL 3 /* Get file status flags. */ +#define F_SETFL 4 /* Set file status flags. */ + +#define MAX_PATH_LENGTH_OPEN_LINUX (128-8) + +// Very limited implementation... +__inline int fcntl(int __fd, int __cmd, ...) +{ + HANDLE hDev = (HANDLE)__fd; + COMMTIMEOUTS timeouts; + va_list argptr; + va_start(argptr, __cmd); + + if (__cmd == F_SETFL) + { + int flags = va_arg(argptr, int); + + memset(&timeouts, 0, sizeof(COMMTIMEOUTS)); + if (!GetCommTimeouts(hDev, &timeouts)) + { + errno = EIO; + return -1; + } + if (flags & O_NDELAY) + { + timeouts.ReadIntervalTimeout = MAXDWORD; + timeouts.ReadTotalTimeoutConstant = 0; + timeouts.ReadTotalTimeoutMultiplier = 0; + timeouts.WriteTotalTimeoutConstant = 1; // ??? + timeouts.WriteTotalTimeoutMultiplier = 0; + if (!SetCommTimeouts(hDev, &timeouts)) + { + errno = EIO; + return -1; + } + } + else + { + timeouts.ReadIntervalTimeout = 0; + timeouts.ReadTotalTimeoutConstant = 0; + timeouts.ReadTotalTimeoutMultiplier = 0; + timeouts.WriteTotalTimeoutConstant = 0; + timeouts.WriteTotalTimeoutMultiplier = 0; + if (!SetCommTimeouts(hDev, &timeouts)) + { + errno = EIO; + return -1; + } + } + } + else if (__cmd == F_GETFL) + { + int flags = 0; + + memset(&timeouts, 0, sizeof(COMMTIMEOUTS)); + if (!GetCommTimeouts(hDev, &timeouts)) + { + errno = EIO; + return -1; + } + if ((timeouts.ReadIntervalTimeout == MAXDWORD)&&(timeouts.ReadTotalTimeoutConstant == 0)&&(timeouts.ReadTotalTimeoutMultiplier == 0)) + { + // Polling read. + flags |= O_NDELAY; + } + else + { + // Blocking read. + flags &= ~O_NDELAY; + } + return flags; + } + + return 0; +} + +__inline int open_linux(const char *__path, int __oflag, ...) +{ + // The optional arguments are ignored... + + int fd = -1; + HANDLE hDev = INVALID_HANDLE_VALUE; + char szDeviceTemp[2*(MAX_PATH_LENGTH_OPEN_LINUX+8)]; + TCHAR tstr[2*(MAX_PATH_LENGTH_OPEN_LINUX+8)]; + DWORD dwDesiredAccess = 0; + DWORD dwCreationDisposition = 0; + DWORD dwShareMode = 0; + //SECURITY_ATTRIBUTES SecurityAttributes; + DWORD dwFlagsAndAttributes = 0; + + // To be able to use COM10 and greater we need to add "\\.\" (that becomes "\\\\.\\" + // in C because the '\' is a special character). + memset(szDeviceTemp, 0, sizeof(szDeviceTemp)); +//#ifdef WINCE + strcpy(szDeviceTemp, __path); +//#else +// sprintf(szDeviceTemp, "\\\\.\\%s", __path); +//#endif // WINCE + +#ifdef UNICODE + mbstowcs(tstr, szDeviceTemp, sizeof(szDeviceTemp)/2); +#else + memcpy(tstr, szDeviceTemp, sizeof(szDeviceTemp)/2); +#endif // UNICODE + tstr[sizeof(tstr)-1] = 0; + + if (__oflag & O_RDONLY) dwDesiredAccess |= GENERIC_READ; + if (__oflag & O_WRONLY) dwDesiredAccess |= GENERIC_WRITE; + if (__oflag & O_RDWR) dwDesiredAccess |= GENERIC_READ|GENERIC_WRITE; + + if (__oflag & O_APPEND) dwCreationDisposition |= OPEN_EXISTING; + if (__oflag & O_CREAT) + { + if (__oflag & O_EXCL) dwCreationDisposition |= CREATE_NEW; + else dwCreationDisposition |= CREATE_ALWAYS; + } + if (__oflag & O_TRUNC) dwCreationDisposition |= TRUNCATE_EXISTING; + + if ((__oflag & O_NONBLOCK)||(__oflag&O_NDELAY)) dwFlagsAndAttributes |= 0; // ? + + if (__oflag & O_DIRECT) dwFlagsAndAttributes |= FILE_FLAG_NO_BUFFERING|FILE_FLAG_WRITE_THROUGH; + + if (__oflag & O_TMPFILE) dwFlagsAndAttributes |= FILE_ATTRIBUTE_TEMPORARY; + + hDev = CreateFile(tstr, dwDesiredAccess, dwShareMode, NULL, dwCreationDisposition, dwFlagsAndAttributes, NULL); + + if (hDev == INVALID_HANDLE_VALUE) + { + errno = EIO; + return -1; + } + + //#ifndef DISABLE_FORCE_CLEAR_DTR + //if (!EscapeCommFunction(hDev, CLRDTR)) + //{ + // + //} + //#endif // DISABLE_FORCE_CLEAR_DTR + + fd = (intptr_t)hDev; + + return fd; +} + +#ifdef _MSC_VER +// Restore the Visual Studio warnings previously disabled. +#pragma warning(default : 4996) +#endif // _MSC_VER + +#endif // FCNCTL2_H diff --git a/C++/unix_adapt/bits/termios.h b/C++/unix_adapt/bits/termios.h new file mode 100644 index 0000000..bbf093d --- /dev/null +++ b/C++/unix_adapt/bits/termios.h @@ -0,0 +1,219 @@ +/* termios type and macro definitions. Linux version. + Copyright (C) 1993-2016 Free Software Foundation, Inc. + This file is part of the GNU C Library. + + The GNU C Library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + The GNU C Library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the GNU C Library; if not, see + . */ + +#ifndef _TERMIOS_H +# error "Never include directly; use instead." +#endif + +typedef unsigned char cc_t; +typedef unsigned int speed_t; +typedef unsigned int tcflag_t; + +#define NCCS 32 +struct termios + { + tcflag_t c_iflag; /* input mode flags */ + tcflag_t c_oflag; /* output mode flags */ + tcflag_t c_cflag; /* control mode flags */ + tcflag_t c_lflag; /* local mode flags */ + cc_t c_line; /* line discipline */ + cc_t c_cc[NCCS]; /* control characters */ + speed_t c_ispeed; /* input speed */ + speed_t c_ospeed; /* output speed */ +#define _HAVE_STRUCT_TERMIOS_C_ISPEED 1 +#define _HAVE_STRUCT_TERMIOS_C_OSPEED 1 + }; + +/* c_cc characters */ +#define VINTR 0 +#define VQUIT 1 +#define VERASE 2 +#define VKILL 3 +#define VEOF 4 +#define VTIME 5 +#define VMIN 6 +#define VSWTC 7 +#define VSTART 8 +#define VSTOP 9 +#define VSUSP 10 +#define VEOL 11 +#define VREPRINT 12 +#define VDISCARD 13 +#define VWERASE 14 +#define VLNEXT 15 +#define VEOL2 16 + +/* c_iflag bits */ +#define IGNBRK 0000001 +#define BRKINT 0000002 +#define IGNPAR 0000004 +#define PARMRK 0000010 +#define INPCK 0000020 +#define ISTRIP 0000040 +#define INLCR 0000100 +#define IGNCR 0000200 +#define ICRNL 0000400 +#define IUCLC 0001000 +#define IXON 0002000 +#define IXANY 0004000 +#define IXOFF 0010000 +#define IMAXBEL 0020000 +#define IUTF8 0040000 + +/* c_oflag bits */ +#define OPOST 0000001 +#define OLCUC 0000002 +#define ONLCR 0000004 +#define OCRNL 0000010 +#define ONOCR 0000020 +#define ONLRET 0000040 +#define OFILL 0000100 +#define OFDEL 0000200 +#if defined __USE_MISC || defined __USE_XOPEN +# define NLDLY 0000400 +# define NL0 0000000 +# define NL1 0000400 +# define CRDLY 0003000 +# define CR0 0000000 +# define CR1 0001000 +# define CR2 0002000 +# define CR3 0003000 +# define TABDLY 0014000 +# define TAB0 0000000 +# define TAB1 0004000 +# define TAB2 0010000 +# define TAB3 0014000 +# define BSDLY 0020000 +# define BS0 0000000 +# define BS1 0020000 +# define FFDLY 0100000 +# define FF0 0000000 +# define FF1 0100000 +#endif + +#define VTDLY 0040000 +#define VT0 0000000 +#define VT1 0040000 + +#ifdef __USE_MISC +# define XTABS 0014000 +#endif + +/* c_cflag bit meaning */ +#ifdef __USE_MISC +# define CBAUD 0010017 +#endif +#define B0 0000000 /* hang up */ +#define B50 0000001 +#define B75 0000002 +#define B110 0000003 +#define B134 0000004 +#define B150 0000005 +#define B200 0000006 +#define B300 0000007 +#define B600 0000010 +#define B1200 0000011 +#define B1800 0000012 +#define B2400 0000013 +#define B4800 0000014 +#define B9600 0000015 +#define B19200 0000016 +#define B38400 0000017 +#ifdef __USE_MISC +# define EXTA B19200 +# define EXTB B38400 +#endif +#define CSIZE 0000060 +#define CS5 0000000 +#define CS6 0000020 +#define CS7 0000040 +#define CS8 0000060 +#define CSTOPB 0000100 +#define CREAD 0000200 +#define PARENB 0000400 +#define PARODD 0001000 +#define HUPCL 0002000 +#define CLOCAL 0004000 +#ifdef __USE_MISC +# define CBAUDEX 0010000 +#endif +#define B57600 0010001 +#define B115200 0010002 +#define B230400 0010003 +#define B460800 0010004 +#define B500000 0010005 +#define B576000 0010006 +#define B921600 0010007 +#define B1000000 0010010 +#define B1152000 0010011 +#define B1500000 0010012 +#define B2000000 0010013 +#define B2500000 0010014 +#define B3000000 0010015 +#define B3500000 0010016 +#define B4000000 0010017 +#define __MAX_BAUD B4000000 +#ifdef __USE_MISC +# define CIBAUD 002003600000 /* input baud rate (not used) */ +# define CMSPAR 010000000000 /* mark or space (stick) parity */ +# define CRTSCTS 020000000000 /* flow control */ +#endif + +/* c_lflag bits */ +#define ISIG 0000001 +#define ICANON 0000002 +#if defined __USE_MISC || defined __USE_XOPEN +# define XCASE 0000004 +#endif +#define ECHO 0000010 +#define ECHOE 0000020 +#define ECHOK 0000040 +#define ECHONL 0000100 +#define NOFLSH 0000200 +#define TOSTOP 0000400 +#ifdef __USE_MISC +# define ECHOCTL 0001000 +# define ECHOPRT 0002000 +# define ECHOKE 0004000 +# define FLUSHO 0010000 +# define PENDIN 0040000 +#endif +#define IEXTEN 0100000 +#ifdef __USE_MISC +# define EXTPROC 0200000 +#endif + +/* tcflow() and TCXONC use these */ +#define TCOOFF 0 +#define TCOON 1 +#define TCIOFF 2 +#define TCION 3 + +/* tcflush() and TCFLSH use these */ +#define TCIFLUSH 0 +#define TCOFLUSH 1 +#define TCIOFLUSH 2 + +/* tcsetattr uses these */ +#define TCSANOW 0 +#define TCSADRAIN 1 +#define TCSAFLUSH 2 + + +#define _IOT_termios /* Hurd ioctl type field. */ \ + _IOT (_IOTS (cflag_t), 4, _IOTS (cc_t), NCCS, _IOTS (speed_t), 2) diff --git a/C++/unix_adapt/sys/time.h b/C++/unix_adapt/sys/time.h new file mode 100644 index 0000000..bae6532 --- /dev/null +++ b/C++/unix_adapt/sys/time.h @@ -0,0 +1,148 @@ +#ifndef _SYS_TIME_H_ +#define _SYS_TIME_H_ +#include + +#ifdef _WIN32 + +#ifndef ENABLE_SYS_TIME_H_WIN32 +#define ENABLE_SYS_TIME_H_WIN32 +#endif // ENABLE_SYS_TIME_H_WIN32 + +// To get struct timeval. +#include +// To get struct _timeb. +//#include + +//#define DELTA_EPOCH_IN_MICROSECS 11644473600000000ui64 +//#define DELTA_EPOCH_IN_MICROSECS 11644473600000000ULL +#define DELTA_EPOCH_IN_MICROSECS_LOW 1216757760 +#define DELTA_EPOCH_IN_MICROSECS_HIGH 2711190 + +#ifdef DISABLE_TIMEZONE_STRUCT_REDEFINITION +struct timezone2 +{ + int tz_minuteswest; // Minutes W of Greenwich. + int tz_dsttime; // Type of DST correction. +}; +#else +#ifdef _MSC_VER +// Disable Visual Studio warnings about timezone declaration. +#pragma warning(disable : 6244) +#endif // _MSC_VER + +// Local declaration of timezone hides previous declaration in time.h. +struct timezone +{ + int tz_minuteswest; // Minutes W of Greenwich. + int tz_dsttime; // Type of DST correction. +}; + +#ifdef _MSC_VER +// Restore the Visual Studio warnings previously disabled. +#pragma warning(default : 6244) +#endif // _MSC_VER +#endif // DISABLE_TIMEZONE_STRUCT_REDEFINITION + +/* +Obtain the current time, expressed as seconds and microseconds since the Epoch, +and store it in the timeval structure pointed to by tv (the accuracy should be +around 15 ms). + +struct timeval* tv : (INOUT) Valid pointer to the structure that will receive +the current time. +struct timezone* tz : (INOUT) Usually set to NULL. + +Return : EXIT_SUCCESS or EXIT_FAILURE if there is an error. +*/ +#ifdef DISABLE_TIMEZONE_STRUCT_REDEFINITION +__inline int gettimeofday(struct timeval* tv, struct timezone2* tz) +#else +__inline int gettimeofday(struct timeval* tv, struct timezone* tz) +#endif // DISABLE_TIMEZONE_STRUCT_REDEFINITION +{ + FILETIME ft; // Will contain a 64-bit value representing the number of 100-nanosecond + // intervals since January 1, 1601 (UTC). + ULONGLONG tmpres = 0; + ULARGE_INTEGER li; + ULARGE_INTEGER epoch; +#ifdef USE__TZSET + static int tzflag; +#else + TIME_ZONE_INFORMATION tz_winapi; + int rez = 0; +#endif // USE__TZSET + + if (tv) + { + GetSystemTimeAsFileTime(&ft); + li.LowPart = ft.dwLowDateTime; + li.HighPart = ft.dwHighDateTime; + + // Converting file time to UNIX Epoch. + tmpres = li.QuadPart/(ULONGLONG)10; // Convert into microseconds. + //tmpres -= DELTA_EPOCH_IN_MICROSECS; + epoch.LowPart = DELTA_EPOCH_IN_MICROSECS_LOW; + epoch.HighPart = DELTA_EPOCH_IN_MICROSECS_HIGH; + tmpres -= epoch.QuadPart; + + tv->tv_sec = (long)(tmpres/(ULONGLONG)1000000); + tv->tv_usec = (long)(tmpres%(ULONGLONG)1000000); + } + +#ifdef USE__TZSET + if (tz) + { + if (!tzflag) + { + _tzset(); + tzflag++; + } + tz->tz_minuteswest = _timezone/60; + tz->tz_dsttime = _daylight; + } +#else + if (tz) + { + // _tzset(), do not work properly, so we use GetTimeZoneInformation. + rez = GetTimeZoneInformation(&tz_winapi); + tz->tz_dsttime = (rez == 2)?TRUE:FALSE; + tz->tz_minuteswest = tz_winapi.Bias + ((rez == 2)?tz_winapi.DaylightBias:0); + } +#endif // USE__TZSET + + return 0; +} + +//__inline int gettimeofday(struct timeval* tp, void* tz) +//{ +// struct _timeb timebuffer; +// +// UNREFERENCED_PARAMETER(tz); +// +// _ftime(&timebuffer); +// tp->tv_sec = (long)timebuffer.time; +// tp->tv_usec = timebuffer.millitm*1000; +// return 0; +//} + +//// From olsrd... +//__inline void gettimeofday(struct timeval *TVal, void *TZone __attribute__ ((unused))) +//{ +// SYSTEMTIME SysTime; +// FILETIME FileTime; +// unsigned __int64 Ticks; +// +// GetSystemTime(&SysTime); +// SystemTimeToFileTime(&SysTime, &FileTime); +// +// Ticks = ((__int64) FileTime.dwHighDateTime << 32) | (__int64) FileTime.dwLowDateTime; +// +// Ticks -= 116444736000000000LL; +// +// TVal->tv_sec = (unsigned int)(Ticks / 10000000); +// TVal->tv_usec = (unsigned int)(Ticks % 10000000) / 10; +//} + +#endif // _WIN32 + +#endif // _SYS_TIME_H_ diff --git a/C++/unix_adapt/termios.h b/C++/unix_adapt/termios.h new file mode 100644 index 0000000..62a1c39 --- /dev/null +++ b/C++/unix_adapt/termios.h @@ -0,0 +1,610 @@ +#ifndef _TERMIOS_H +#define _TERMIOS_H + +/* +This file intended to serve as a drop-in replacement for termios.h on Windows. +Please add/remove functionality as neeeded. +*/ + +// Not sure what is that... +#ifndef __USE_MISC +#define __USE_MISC +#endif // __USE_MISC + +/* Get the system-dependent definitions of `struct termios', `tcflag_t', + `cc_t', `speed_t', and all the macros specifying the flag bits. */ +#include + +#ifdef __USE_MISC +/* Compare a character C to a value VAL from the `c_cc' array in a + `struct termios'. If VAL is _POSIX_VDISABLE, no character can match it. */ +# define CCEQ(val, c) ((c) == (val) && (val) != _POSIX_VDISABLE) +#endif + +// Prevent Winsock.h from being included by the Windows.h header. +// This must be done if we plan to include Winsock2.h in other files. +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN +#endif // WIN32_LEAN_AND_MEAN +#include + +#include + +//#include "NtPathFromHandle.h" + +inline UINT _linuxbaudrate2windows(speed_t BaudRate) +{ + switch (BaudRate) + { + case B110: + return CBR_110; + case B300: + return CBR_300; + case B600: + return CBR_600; + case B1200: + return CBR_1200; + case B4800: + return CBR_4800; + case B2400: + return CBR_2400; + case B9600: + return CBR_9600; + case B19200: + return CBR_19200; + case B38400: + return CBR_38400; + case B57600: + return CBR_57600; + case B115200: + return CBR_115200; + default: + return 0; + } +} + +inline speed_t _windowsbaudrate2linux(UINT BaudRate) +{ + switch (BaudRate) + { + case CBR_110: + return B110; + case CBR_300: + return B300; + case CBR_600: + return B600; + case CBR_1200: + return B1200; + case CBR_4800: + return B4800; + case CBR_2400: + return B2400; + case CBR_9600: + return B9600; + case CBR_19200: + return B19200; + case CBR_38400: + return B38400; + case CBR_57600: + return B57600; + case CBR_115200: + return B115200; + default: + return 0; + } +} + +__inline speed_t cfgetospeed(const struct termios *__termios_p) +{ + return __termios_p->c_ospeed; +} + +__inline speed_t cfgetispeed (const struct termios *__termios_p) +{ + return __termios_p->c_ispeed; +} + +__inline int cfsetospeed (struct termios *__termios_p, speed_t __speed) +{ + __termios_p->c_ospeed = __speed; + return 0; +} + +__inline int cfsetispeed (struct termios *__termios_p, speed_t __speed) +{ + __termios_p->c_ispeed = __speed; + return 0; +} + +#ifdef __USE_MISC +__inline int cfsetspeed (struct termios *__termios_p, speed_t __speed) +{ + __termios_p->c_ispeed = __speed; + __termios_p->c_ospeed = __speed; + return 0; +} +#endif + +/* Put the state of FD into *TERMIOS_P. */ +__inline int tcgetattr (int __fd, struct termios *__termios_p) +{ + HANDLE hDev = (HANDLE)__fd; + COMMTIMEOUTS timeouts; + DCB dcb; + speed_t speed = 0; + +//#pragma region TIMEOUTS + memset(&timeouts, 0, sizeof(COMMTIMEOUTS)); + + if (!GetCommTimeouts(hDev, &timeouts)) + { + errno = EIO; + return -1; + } + + if ((timeouts.ReadIntervalTimeout == MAXDWORD)&&(timeouts.ReadTotalTimeoutConstant == 0)&&(timeouts.ReadTotalTimeoutMultiplier == 0)) + { + // Polling read. + __termios_p->c_cc[VMIN] = 0; // Minimum number of characters to read. + __termios_p->c_cc[VTIME] = 0; // Time to wait for every character read in tenths of seconds. + } + else if ((timeouts.ReadIntervalTimeout == 0)&&(timeouts.ReadTotalTimeoutConstant == 0)&&(timeouts.ReadTotalTimeoutMultiplier == 0)) + { + // Blocking read. + __termios_p->c_cc[VMIN] = 1; // Minimum number of characters to read. + __termios_p->c_cc[VTIME] = 0; // Time to wait for every character read in tenths of seconds. + } + else if ((timeouts.ReadIntervalTimeout == MAXDWORD)&&(timeouts.ReadTotalTimeoutMultiplier == MAXDWORD)) + { + // Read with timeout. + if (timeouts.ReadTotalTimeoutConstant/100 > 255) + { + errno = EINVAL; + return -1; + } + __termios_p->c_cc[VMIN] = 0; // Minimum number of characters to read. + __termios_p->c_cc[VTIME] = (cc_t)(timeouts.ReadTotalTimeoutConstant/100); // Time to wait for every character read in tenths of seconds. + } + else if ((timeouts.ReadTotalTimeoutConstant == 0)&&(timeouts.ReadTotalTimeoutMultiplier == 0)) + { + // Should be approximately equivalent... + + // Read with interbyte timeout. + if (timeouts.ReadIntervalTimeout/100 > 255) + { + errno = EINVAL; + return -1; + } + __termios_p->c_cc[VMIN] = 255; // Minimum number of characters to read. + __termios_p->c_cc[VTIME] = (cc_t)(timeouts.ReadIntervalTimeout/100); // Time to wait for every character read in tenths of seconds. + } + else + { + // Not sure what to do... + + // Read with timeout. + if (timeouts.ReadTotalTimeoutConstant/100 > 255) + { + errno = EINVAL; + return -1; + } + __termios_p->c_cc[VMIN] = 0; // Minimum number of characters to read. + __termios_p->c_cc[VTIME] = (cc_t)(timeouts.ReadTotalTimeoutConstant/100); // Time to wait for every character read in tenths of seconds. + } +//#pragma endregion + +//#pragma region DCB + memset(&dcb, 0, sizeof(DCB)); + + if (!GetCommState(hDev, &dcb)) + { + errno = EIO; + return -1; + } + + speed = _windowsbaudrate2linux(dcb.BaudRate); + __termios_p->c_ispeed = speed; + __termios_p->c_ospeed = speed; + + switch (dcb.Parity) + { + case NOPARITY : + __termios_p->c_cflag &= ~CMSPAR; + __termios_p->c_cflag &= ~PARENB; + __termios_p->c_cflag &= ~PARODD; + break; + case MARKPARITY : + __termios_p->c_cflag |= CMSPAR; + __termios_p->c_cflag &= ~PARENB; + __termios_p->c_cflag |= PARODD; + break; + case SPACEPARITY : + __termios_p->c_cflag |= CMSPAR; + __termios_p->c_cflag &= ~PARENB; + __termios_p->c_cflag &= ~PARODD; + break; + case ODDPARITY : + __termios_p->c_cflag &= ~CMSPAR; + __termios_p->c_cflag |= PARENB; + __termios_p->c_cflag |= PARODD; + break; + case EVENPARITY : + __termios_p->c_cflag &= ~CMSPAR; + __termios_p->c_cflag |= PARENB; + __termios_p->c_cflag &= ~PARODD; + break; + default : + errno = EINVAL; + return -1; + } + + if (dcb.fParity) + { + __termios_p->c_iflag |= INPCK; + } + else + { + __termios_p->c_iflag &= ~INPCK; + } + + if (!dcb.fErrorChar) + { + __termios_p->c_iflag |= IGNPAR; // A character with a framing or parity error will be discarded. + // This is only valid (at least for parity errors) if parity checking is enabled. + } + else + { + __termios_p->c_iflag &= ~IGNPAR; + } + + if (dcb.ErrorChar == 0) + { + __termios_p->c_iflag &= ~PARMRK; // Never mark a framing or parity error with prefix bytes. + } + else + { + __termios_p->c_iflag |= PARMRK; + } + + __termios_p->c_cflag &= ~CSIZE; // Erase the previous flag for the number of data bits. + + switch (dcb.ByteSize) + { + case 8: + __termios_p->c_cflag |= CS8; + break; + case 7: + __termios_p->c_cflag |= CS7; + break; + case 6: + __termios_p->c_cflag |= CS6; + break; + case 5: + __termios_p->c_cflag |= CS5; + break; + default : + errno = EINVAL; + return -1; + } + + switch (dcb.StopBits) + { + case ONESTOPBIT: + __termios_p->c_cflag &= ~CSTOPB; + break; + case TWOSTOPBITS: + __termios_p->c_cflag |= CSTOPB; + break; + default : + errno = EINVAL; + return -1; + } + + // Binary mode. + if (dcb.fBinary) + { + // The c_cflag member contains two options that should always be enabled, + // CLOCAL and CREAD. These will ensure that your program does not become + // the 'owner' of the port subject to sporatic job control and hangup signals, and + // also that the serial interface driver will read incoming data bytes. + __termios_p->c_cflag |= (CLOCAL | CREAD); + + // Raw input, no echo, no signals. + __termios_p->c_lflag &= ~(ICANON | IEXTEN | ECHO | ECHOE | ECHOK | ECHONL | ISIG); + + // Raw input. + __termios_p->c_iflag &= ~(IGNBRK | BRKINT | INLCR | IGNCR | ICRNL); + + // Raw output. + __termios_p->c_oflag &= ~(OPOST | ONLCR | ONOCR | ONLRET | OCRNL); + } + else + { + // What should be done...? + + // The c_cflag member contains two options that should always be enabled, + // CLOCAL and CREAD. These will ensure that your program does not become + // the 'owner' of the port subject to sporatic job control and hangup signals, and + // also that the serial interface driver will read incoming data bytes. + __termios_p->c_cflag |= (CLOCAL | CREAD); + + __termios_p->c_lflag |= ICANON; + } + + // Software flow control. + if (!(dcb.fOutX||dcb.fInX)) + { + __termios_p->c_iflag &= ~(IXON | IXOFF | IXANY); + } + else + { + __termios_p->c_iflag |= (IXON | IXOFF); + } + + __termios_p->c_cc[VSTART] = dcb.XonChar; + __termios_p->c_cc[VSTOP] = dcb.XoffChar; + __termios_p->c_cc[VEOF] = dcb.EofChar; + + // Hardware flow control. + if (!dcb.fOutxCtsFlow) + { +#ifdef CRTSCTS + __termios_p->c_cflag &= ~CRTSCTS; +#endif // CRTSCTS +#ifdef CNEW_RTSCTS + __termios_p->c_cflag &= ~CNEW_RTSCTS; +#endif // CNEW_RTSCTS + } + else + { +#ifdef CRTSCTS + __termios_p->c_cflag |= CRTSCTS; +#endif // CRTSCTS +#ifdef CNEW_RTSCTS + __termios_p->c_cflag |= CNEW_RTSCTS; +#endif // CNEW_RTSCTS + } + + // The most likely scenario where this flag is useful is if the communication channel is + // configured for parity and seven bits per character. In this case, the eigth bit on every + // received character is a parity bit, not part of the data payload. The user program does + // not need to know the value of the parity bit. + __termios_p->c_iflag &= ~ISTRIP; +//#pragma endregion + + return 0; +} + +/* Set the state of FD to *TERMIOS_P. + Values for OPTIONAL_ACTIONS (TCSA*) are in . */ +__inline int tcsetattr (int __fd, int __optional_actions, const struct termios *__termios_p) +{ + HANDLE hDev = (HANDLE)__fd; + COMMTIMEOUTS timeouts; + DCB dcb; + + UNREFERENCED_PARAMETER(__optional_actions); + +//#pragma region TIMEOUTS + memset(&timeouts, 0, sizeof(COMMTIMEOUTS)); + + if ((__termios_p->c_cc[VMIN] == 0)&&(__termios_p->c_cc[VTIME]) == 0) + { + // Polling read. + timeouts.ReadIntervalTimeout = MAXDWORD; timeouts.ReadTotalTimeoutConstant = 0; timeouts.ReadTotalTimeoutMultiplier = 0; + timeouts.WriteTotalTimeoutConstant = 1; // ??? + } + else if ((__termios_p->c_cc[VMIN] == 1)&&(__termios_p->c_cc[VTIME]) == 0) + { + // Blocking read. + timeouts.ReadIntervalTimeout = 0; timeouts.ReadTotalTimeoutConstant = 0; timeouts.ReadTotalTimeoutMultiplier = 0; + timeouts.WriteTotalTimeoutConstant = 0; // ??? + } + else if ((__termios_p->c_cc[VMIN] == 0)&&(__termios_p->c_cc[VTIME]) > 0) + { + // Read with timeout. + if (__termios_p->c_cc[VTIME]*100 > MAXDWORD) + { + errno = EINVAL; + return -1; + } + timeouts.ReadIntervalTimeout = MAXDWORD; timeouts.ReadTotalTimeoutConstant = __termios_p->c_cc[VTIME]*100; timeouts.ReadTotalTimeoutMultiplier = MAXDWORD; + timeouts.WriteTotalTimeoutConstant = __termios_p->c_cc[VTIME]*100; // ??? + } + else if ((__termios_p->c_cc[VMIN] == 255)&&(__termios_p->c_cc[VTIME]) > 0) + { + // Should be approximately equivalent... + + // Read with interbyte timeout. + if (__termios_p->c_cc[VTIME]*100 > MAXDWORD) + { + errno = EINVAL; + return -1; + } + timeouts.ReadIntervalTimeout = __termios_p->c_cc[VTIME]*100; timeouts.ReadTotalTimeoutConstant = 0; timeouts.ReadTotalTimeoutMultiplier = 0; + timeouts.WriteTotalTimeoutConstant = 0; // ??? + } + else + { + // Not sure what to do... + + // Read with interbyte timeout. + if (__termios_p->c_cc[VTIME]*100 > MAXDWORD) + { + errno = EINVAL; + return -1; + } + timeouts.ReadIntervalTimeout = __termios_p->c_cc[VTIME]*100; timeouts.ReadTotalTimeoutConstant = 0; timeouts.ReadTotalTimeoutMultiplier = 0; + timeouts.WriteTotalTimeoutConstant = 0; // ??? + } + + if (!SetCommTimeouts(hDev, &timeouts)) + { + errno = EIO; + return -1; + } +//#pragma endregion + +//#pragma region DCB + memset(&dcb, 0, sizeof(DCB)); + + // Not possible to have different c_ispeed and c_ospeed on Windows... + dcb.BaudRate = _linuxbaudrate2windows(__termios_p->c_ispeed); + if (dcb.BaudRate == 0) + { + errno = EINVAL; + return -1; + } + + switch (__termios_p->c_cflag & (CMSPAR | PARENB | PARODD)) + { + case 0: + dcb.Parity = NOPARITY; + break; + case (CMSPAR | PARODD): + dcb.Parity = MARKPARITY; + break; + case (CMSPAR): + dcb.Parity = SPACEPARITY; + break; + case (PARENB | PARODD): + dcb.Parity = ODDPARITY; + break; + case (PARENB): + dcb.Parity = EVENPARITY; + break; + default: + errno = EINVAL; + return -1; + } + + dcb.fParity = (__termios_p->c_iflag & INPCK)? TRUE: FALSE; + + if (__termios_p->c_iflag & IGNPAR) + { + dcb.fErrorChar = FALSE; // Indicates whether bytes received with parity errors are + // replaced with the character specified by the ErrorChar member. + } + else + { + dcb.fErrorChar = TRUE; + } + + if (__termios_p->c_iflag & PARMRK) + { + dcb.ErrorChar = 37; // ? + } + else + { + dcb.ErrorChar = 0; + } + + switch (__termios_p->c_cflag & CSIZE) + { + case CS8: + dcb.ByteSize = 8; + break; + case CS7: + dcb.ByteSize = 7; + break; + case CS6: + dcb.ByteSize = 6; + break; + case CS5: + dcb.ByteSize = 5; + break; + default: + errno = EINVAL; + return -1; + } + + dcb.StopBits = (__termios_p->c_cflag & CSTOPB)? TWOSTOPBITS: ONESTOPBIT; + + // Binary mode. + if (__termios_p->c_lflag & ~ICANON) + { + dcb.fBinary = TRUE; + } + else + { + dcb.fBinary = FALSE; + } + + // Software flow control. + if (!(__termios_p->c_iflag & IXON)||(__termios_p->c_iflag & IXOFF)) + { + dcb.fOutX = FALSE; + dcb.fInX = FALSE; + } + else + { + dcb.fOutX = TRUE; + dcb.fInX = TRUE; + } + + dcb.XonChar = __termios_p->c_cc[VSTART]; + dcb.XoffChar = __termios_p->c_cc[VSTOP]; + dcb.EofChar = __termios_p->c_cc[VEOF]; + +#ifdef CRTSCTS + if (__termios_p->c_cflag & CRTSCTS) + { + dcb.fOutxCtsFlow = TRUE; + dcb.fRtsControl = RTS_CONTROL_HANDSHAKE; + dcb.fOutxDsrFlow = TRUE; + //dcb.fDsrSensitivity = FALSE; + } + else + { + dcb.fOutxCtsFlow = FALSE; + dcb.fRtsControl = RTS_CONTROL_DISABLE; + dcb.fOutxDsrFlow = FALSE; + //dcb.fDsrSensitivity = FALSE; + } +#endif // CRTSCTS +#ifdef CNEW_RTSCTS + if (__termios_p->c_cflag & CNEW_RTSCTS) + { + dcb.fOutxCtsFlow = TRUE; + dcb.fRtsControl = RTS_CONTROL_HANDSHAKE; + dcb.fOutxDsrFlow = TRUE; + //dcb.fDsrSensitivity = FALSE; + } + else + { + dcb.fOutxCtsFlow = FALSE; + dcb.fRtsControl = RTS_CONTROL_DISABLE; + dcb.fOutxDsrFlow = FALSE; + //dcb.fDsrSensitivity = FALSE; + } +#endif // CNEW_RTSCTS + + // Miscellaneous default options. + //dcb.fDtrControl = DTR_CONTROL_DISABLE; + //dcb.fAbortOnError = FALSE; + + // The SetCommState() function reconfigures the communications resource, but it does not affect + // the internal output and input buffers of the specified driver. The buffers are not flushed, + // and pending read and write operations are not terminated prematurely. + if (!SetCommState(hDev, &dcb)) + { + errno = EIO; + return -1; + } +//#pragma endregion + + return 0; +} + +#ifdef __USE_MISC +__inline void cfmakeraw(struct termios *__termios_p) +{ + __termios_p->c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON); + __termios_p->c_oflag &= ~OPOST; + __termios_p->c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); + __termios_p->c_cflag &= ~(CSIZE | PARENB); + __termios_p->c_cflag |= CS8; +} +#endif + +#endif // _TERMIOS_H diff --git a/C++/unix_adapt/unistd.h b/C++/unix_adapt/unistd.h new file mode 100644 index 0000000..27dff51 --- /dev/null +++ b/C++/unix_adapt/unistd.h @@ -0,0 +1,102 @@ +//#ifdef __MINGW32__ +// +//# include_next +// +//#else + +#ifndef UNISTD_H +#define UNISTD_H + +/* +This file intended to serve as a drop-in replacement for unistd.h on Windows. +Please add/remove functionality as neeeded. + +See http://pubs.opengroup.org/onlinepubs/7908799/xsh/unistd.h.html. +*/ + +// Prevent Winsock.h from being included by the Windows.h header. +// This must be done if we plan to include Winsock2.h in other files. +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN +#endif // WIN32_LEAN_AND_MEAN +#include + +#include +#include +#include +//#include // getopt from: http://www.pwilson.net/sample.html. +// To get Sleep(). +#include + +#include + +#include + +//#define srandom srand +//#define random rand + +//const W_OK = 2; +//const R_OK = 4; + +//#define access _access +//#define ftruncate _chsize + +#ifndef _SSIZE_T_ +#ifndef ssize_t +#define ssize_t long +#endif // ssize_t +#endif // _SSIZE_T_ + +__inline int usleep(unsigned int usec) +{ + Sleep(usec/1000); + return 0; +} + +__inline ssize_t read_linux (int __fd, void* __buf, size_t __nbytes) +{ + HANDLE hDev = (HANDLE)__fd; + DWORD readBytes = 0; + + if (!ReadFile(hDev, __buf, __nbytes, &readBytes, NULL)) + { + // To be improved by checking GetLastError()... + errno = EIO; + return -1; + } + + return (ssize_t)readBytes; +} + +__inline ssize_t write_linux(int __fd, const void* __buf, size_t __nbytes) +{ + HANDLE hDev = (HANDLE)__fd; + DWORD writtenBytes = 0; + + if (!WriteFile(hDev, __buf, __nbytes, &writtenBytes, NULL)) + { + // To be improved by checking GetLastError()... + errno = EIO; + return -1; + } + + return (ssize_t)writtenBytes; +} + +__inline int close_linux(int __fd) +{ + HANDLE hDev = (HANDLE)__fd; + + if (!CloseHandle(hDev)) + { + // To be improved by checking GetLastError()... + errno = EIO; + return -1; + } + + return 0; +} + +#endif // UNISTD_H + +//#endif // __MINGW32__ diff --git a/README.md b/README.md index cdba59d..a248fa6 100644 --- a/README.md +++ b/README.md @@ -14,7 +14,7 @@ Tutorial --- You find a [detailed tutorial in the Wiki](https://github.com/ptrbrtz/razor-9dof-ahrs/wiki/Tutorial). -Note: for SEN-14001 (*9DoF Razor IMU M0*), you will need to follow the same instructions as for the default firmware on https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide and use an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library (an updated version of the default firmware is also available on https://github.com/lebarsfa/9DOF_Razor_IMU). +Note: For SEN-14001 (*9DoF Razor IMU M0*), you will need to follow the same instructions as for the default firmware on https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide and use an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library (an updated version of the default firmware is also available on https://github.com/lebarsfa/9DOF_Razor_IMU). Quick setup --- @@ -25,9 +25,9 @@ Select your hardware in `Arduino/Razor_AHRS/Razor_AHRS.ino` under `"USER SETUP A Upload the firmware using *Arduino*. Run `Processing/Razor_AHRS_test/Razor_AHRS_test.pde` using *Processing*. -### Optional: Mac OS X / Unix / Linux C++ Interface +### Optional: Mac OS X / Unix / Linux / Windows C++ Interface -Compile test program: +Use the provided Qt project (check Projects\Run Settings\Run in terminal to force your application to run inside a separate terminal) or compile test program from the command line (add `-Iunix_adapt -DDISABLE_TIMEZONE_STRUCT_REDEFINITION` for MinGW/MSYS): g++ Example.cpp RazorAHRS.cpp -Wall -D_REENTRANT -lpthread -o example @@ -35,7 +35,7 @@ Run it: ./example -Sorry, no Windows support. But you could try to compile using cygwin. +Note: To use the provided Visual Studio 2017 project, you will need to install Pthreads-win32 2.9.1 from http://www.ensta-bretagne.fr/lebars/Share/pthreads-win32-msvc.zip (only in 32 bit). ### Optional: Android Interface From 866865668ab83502d71805fd53cd99ec40d77ea3 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Wed, 27 Dec 2017 01:00:20 +0100 Subject: [PATCH 21/26] Added Visual Studio 64 bit compatibility to C++ sample --- C++/RazorAHRS.pro | 17 +++++--- C++/RazorAHRS.sln | 12 +++--- C++/RazorAHRS.vcxproj | 83 ++++++++++++++++++++++++++++++++++++ C++/unix_adapt/bits/fcntl2.h | 4 +- C++/unix_adapt/termios.h | 18 ++++---- C++/unix_adapt/unistd.h | 30 ++++++------- README.md | 2 +- 7 files changed, 125 insertions(+), 41 deletions(-) diff --git a/C++/RazorAHRS.pro b/C++/RazorAHRS.pro index 2805cfc..239ee59 100644 --- a/C++/RazorAHRS.pro +++ b/C++/RazorAHRS.pro @@ -11,13 +11,16 @@ win32:DEFINES += DISABLE_TIMEZONE_STRUCT_REDEFINITION DEFINES += _REENTRANT LIBS += -lpthread -# Uncomment if using Visual Studio compiler for Qt, install also Pthreads-win32 2.9.1 -# from http://www.ensta-bretagne.fr/lebars/Share/pthreads-win32-msvc.zip (only in 32 bit). -#win32:INCLUDEPATH += "C:\Program Files (x86)\pthreads-w32-2.9.1-msvc" -#win32:DEFINES += HAVE_STRUCT_TIMESPEC -#win32:LIBS -= -lpthread -#win32:LIBS += -L"C:\Program Files (x86)\pthreads-w32-2.9.1-msvc" -#win32:LIBS += -lpthreadVC2 +# If using Visual Studio compiler for Qt, you will need to install also Pthreads-win32 +# from http://www.ensta-bretagne.fr/lebars/Share/pthreads-win32-msvc.zip. +win32-msvc:contains(QMAKE_HOST.arch, x86):INCLUDEPATH += "C:\Program Files (x86)\pthreads-w32-2.9.1-msvc" +win32-msvc:contains(QMAKE_HOST.arch, x86_64):INCLUDEPATH += "C:\Program Files\pthreads-w32-2.8.0-msvc" +win32-msvc:DEFINES += HAVE_STRUCT_TIMESPEC +win32-msvc:LIBS -= -lpthread +win32-msvc:contains(QMAKE_HOST.arch, x86):LIBS += -L"C:\Program Files (x86)\pthreads-w32-2.9.1-msvc" +win32-msvc:contains(QMAKE_HOST.arch, x86_64):LIBS += -L"C:\Program Files\pthreads-w32-2.8.0-msvc" +win32-msvc:contains(QMAKE_HOST.arch, x86):LIBS += -lpthreadVC2 +win32-msvc:contains(QMAKE_HOST.arch, x86_64):LIBS += -lpthreadVC2_x64 SOURCES += \ Example.cpp \ diff --git a/C++/RazorAHRS.sln b/C++/RazorAHRS.sln index 612e85b..ec2007e 100644 --- a/C++/RazorAHRS.sln +++ b/C++/RazorAHRS.sln @@ -1,7 +1,7 @@  Microsoft Visual Studio Solution File, Format Version 12.00 # Visual Studio 15 -VisualStudioVersion = 15.0.26430.6 +VisualStudioVersion = 15.0.26430.16 MinimumVisualStudioVersion = 10.0.40219.1 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "RazorAHRS", "RazorAHRS.vcxproj", "{53C89697-1513-4B96-99FE-09494B336B96}" EndProject @@ -15,20 +15,18 @@ Global Release|x86 = Release|x86 EndGlobalSection GlobalSection(ProjectConfigurationPlatforms) = postSolution - {53C89697-1513-4B96-99FE-09494B336B96}.Debug|x64.ActiveCfg = Debug|Win32 + {53C89697-1513-4B96-99FE-09494B336B96}.Debug|x64.ActiveCfg = Debug|x64 + {53C89697-1513-4B96-99FE-09494B336B96}.Debug|x64.Build.0 = Debug|x64 {53C89697-1513-4B96-99FE-09494B336B96}.Debug|x86.ActiveCfg = Debug|Win32 {53C89697-1513-4B96-99FE-09494B336B96}.Debug|x86.Build.0 = Debug|Win32 - {53C89697-1513-4B96-99FE-09494B336B96}.Release|x64.ActiveCfg = Release|Win32 + {53C89697-1513-4B96-99FE-09494B336B96}.Release|x64.ActiveCfg = Release|x64 + {53C89697-1513-4B96-99FE-09494B336B96}.Release|x64.Build.0 = Release|x64 {53C89697-1513-4B96-99FE-09494B336B96}.Release|x86.ActiveCfg = Release|Win32 {53C89697-1513-4B96-99FE-09494B336B96}.Release|x86.Build.0 = Release|Win32 {4AC80B0F-8909-4D91-96C8-ACCD4A78B2C4}.Debug|x64.ActiveCfg = Debug|x64 - {4AC80B0F-8909-4D91-96C8-ACCD4A78B2C4}.Debug|x64.Build.0 = Debug|x64 {4AC80B0F-8909-4D91-96C8-ACCD4A78B2C4}.Debug|x86.ActiveCfg = Debug|x86 - {4AC80B0F-8909-4D91-96C8-ACCD4A78B2C4}.Debug|x86.Build.0 = Debug|x86 {4AC80B0F-8909-4D91-96C8-ACCD4A78B2C4}.Release|x64.ActiveCfg = Release|x64 - {4AC80B0F-8909-4D91-96C8-ACCD4A78B2C4}.Release|x64.Build.0 = Release|x64 {4AC80B0F-8909-4D91-96C8-ACCD4A78B2C4}.Release|x86.ActiveCfg = Release|x86 - {4AC80B0F-8909-4D91-96C8-ACCD4A78B2C4}.Release|x86.Build.0 = Release|x86 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/C++/RazorAHRS.vcxproj b/C++/RazorAHRS.vcxproj index f73c5f7..293343e 100644 --- a/C++/RazorAHRS.vcxproj +++ b/C++/RazorAHRS.vcxproj @@ -5,10 +5,18 @@ Debug Win32 + + Debug + x64 + Release Win32 + + Release + x64 + @@ -35,20 +43,37 @@ NotSet true + + Application + v141 + NotSet + true + Application v141 NotSet + + Application + v141 + NotSet + + + + + + + <_ProjectFileVersion>11.0.61030.0 @@ -60,6 +85,11 @@ false C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\Team Tools\Static Analysis Tools\Rule Sets\NativeRecommendedRules.ruleset + + true + false + C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\Team Tools\Static Analysis Tools\Rule Sets\NativeRecommendedRules.ruleset + $(ProjectDir)$(Configuration)\ $(ProjectDir)$(Configuration)\ @@ -67,6 +97,11 @@ false NativeRecommendedRules.ruleset + + false + false + NativeRecommendedRules.ruleset + Disabled @@ -91,6 +126,31 @@ + + + Disabled + WIN32;HAVE_STRUCT_TIMESPEC;_REENTRANT;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + EnableFastChecks + MultiThreadedDebugDLL + + + Level4 + ProgramDatabase + false + .;..;unix_adapt;$(ProgramW6432)\pthreads-w32-2.8.0-msvc; + + + pthreadVC2_x64.lib;%(AdditionalDependencies) + true + Console + false + + + $(ProgramW6432)\pthreads-w32-2.8.0-msvc + + + + WIN32;HAVE_STRUCT_TIMESPEC;_REENTRANT;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) @@ -113,6 +173,29 @@ $(ProgramFiles)\pthreads-w32-2.9.1-msvc + + + WIN32;HAVE_STRUCT_TIMESPEC;_REENTRANT;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + MultiThreaded + + + Level4 + ProgramDatabase + false + .;..;unix_adapt;$(ProgramW6432)\pthreads-w32-2.8.0-msvc; + + + pthreadVC2_x64.lib;%(AdditionalDependencies) + true + Console + true + true + false + + + $(ProgramW6432)\pthreads-w32-2.8.0-msvc + + diff --git a/C++/unix_adapt/bits/fcntl2.h b/C++/unix_adapt/bits/fcntl2.h index 2bf9a7c..7737d97 100644 --- a/C++/unix_adapt/bits/fcntl2.h +++ b/C++/unix_adapt/bits/fcntl2.h @@ -79,7 +79,7 @@ // Very limited implementation... __inline int fcntl(int __fd, int __cmd, ...) { - HANDLE hDev = (HANDLE)__fd; + HANDLE hDev = (HANDLE)(intptr_t)__fd; COMMTIMEOUTS timeouts; va_list argptr; va_start(argptr, __cmd); @@ -210,7 +210,7 @@ __inline int open_linux(const char *__path, int __oflag, ...) //} //#endif // DISABLE_FORCE_CLEAR_DTR - fd = (intptr_t)hDev; + fd = (int)(intptr_t)hDev; return fd; } diff --git a/C++/unix_adapt/termios.h b/C++/unix_adapt/termios.h index 62a1c39..84aa914 100644 --- a/C++/unix_adapt/termios.h +++ b/C++/unix_adapt/termios.h @@ -94,30 +94,30 @@ inline speed_t _windowsbaudrate2linux(UINT BaudRate) } } -__inline speed_t cfgetospeed(const struct termios *__termios_p) +__inline speed_t cfgetospeed(const struct termios *__termios_p) { return __termios_p->c_ospeed; } -__inline speed_t cfgetispeed (const struct termios *__termios_p) +__inline speed_t cfgetispeed(const struct termios *__termios_p) { return __termios_p->c_ispeed; } -__inline int cfsetospeed (struct termios *__termios_p, speed_t __speed) +__inline int cfsetospeed(struct termios *__termios_p, speed_t __speed) { __termios_p->c_ospeed = __speed; return 0; } -__inline int cfsetispeed (struct termios *__termios_p, speed_t __speed) +__inline int cfsetispeed(struct termios *__termios_p, speed_t __speed) { __termios_p->c_ispeed = __speed; return 0; } #ifdef __USE_MISC -__inline int cfsetspeed (struct termios *__termios_p, speed_t __speed) +__inline int cfsetspeed(struct termios *__termios_p, speed_t __speed) { __termios_p->c_ispeed = __speed; __termios_p->c_ospeed = __speed; @@ -126,9 +126,9 @@ __inline int cfsetspeed (struct termios *__termios_p, speed_t __speed) #endif /* Put the state of FD into *TERMIOS_P. */ -__inline int tcgetattr (int __fd, struct termios *__termios_p) +__inline int tcgetattr(int __fd, struct termios *__termios_p) { - HANDLE hDev = (HANDLE)__fd; + HANDLE hDev = (HANDLE)(intptr_t)__fd; COMMTIMEOUTS timeouts; DCB dcb; speed_t speed = 0; @@ -377,9 +377,9 @@ __inline int tcgetattr (int __fd, struct termios *__termios_p) /* Set the state of FD to *TERMIOS_P. Values for OPTIONAL_ACTIONS (TCSA*) are in . */ -__inline int tcsetattr (int __fd, int __optional_actions, const struct termios *__termios_p) +__inline int tcsetattr(int __fd, int __optional_actions, const struct termios *__termios_p) { - HANDLE hDev = (HANDLE)__fd; + HANDLE hDev = (HANDLE)(intptr_t)__fd; COMMTIMEOUTS timeouts; DCB dcb; diff --git a/C++/unix_adapt/unistd.h b/C++/unix_adapt/unistd.h index 27dff51..2fbf542 100644 --- a/C++/unix_adapt/unistd.h +++ b/C++/unix_adapt/unistd.h @@ -14,13 +14,6 @@ Please add/remove functionality as neeeded. See http://pubs.opengroup.org/onlinepubs/7908799/xsh/unistd.h.html. */ -// Prevent Winsock.h from being included by the Windows.h header. -// This must be done if we plan to include Winsock2.h in other files. -#ifndef WIN32_LEAN_AND_MEAN -#define WIN32_LEAN_AND_MEAN -#endif // WIN32_LEAN_AND_MEAN -#include - #include #include #include @@ -28,6 +21,13 @@ See http://pubs.opengroup.org/onlinepubs/7908799/xsh/unistd.h.html. // To get Sleep(). #include +// Prevent Winsock.h from being included by the Windows.h header. +// This must be done if we plan to include Winsock2.h in other files. +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN +#endif // WIN32_LEAN_AND_MEAN +#include + #include #include @@ -53,39 +53,39 @@ __inline int usleep(unsigned int usec) return 0; } -__inline ssize_t read_linux (int __fd, void* __buf, size_t __nbytes) +__inline ssize_t read_linux(int __fd, void* __buf, size_t __nbytes) { - HANDLE hDev = (HANDLE)__fd; + HANDLE hDev = (HANDLE)(intptr_t)__fd; DWORD readBytes = 0; - if (!ReadFile(hDev, __buf, __nbytes, &readBytes, NULL)) + if (!ReadFile(hDev, __buf, (DWORD)__nbytes, &readBytes, NULL)) { // To be improved by checking GetLastError()... errno = EIO; return -1; } - + return (ssize_t)readBytes; } __inline ssize_t write_linux(int __fd, const void* __buf, size_t __nbytes) { - HANDLE hDev = (HANDLE)__fd; + HANDLE hDev = (HANDLE)(intptr_t)__fd; DWORD writtenBytes = 0; - if (!WriteFile(hDev, __buf, __nbytes, &writtenBytes, NULL)) + if (!WriteFile(hDev, __buf, (DWORD)__nbytes, &writtenBytes, NULL)) { // To be improved by checking GetLastError()... errno = EIO; return -1; } - + return (ssize_t)writtenBytes; } __inline int close_linux(int __fd) { - HANDLE hDev = (HANDLE)__fd; + HANDLE hDev = (HANDLE)(intptr_t)__fd; if (!CloseHandle(hDev)) { diff --git a/README.md b/README.md index a248fa6..0e32c2e 100644 --- a/README.md +++ b/README.md @@ -35,7 +35,7 @@ Run it: ./example -Note: To use the provided Visual Studio 2017 project, you will need to install Pthreads-win32 2.9.1 from http://www.ensta-bretagne.fr/lebars/Share/pthreads-win32-msvc.zip (only in 32 bit). +Note: To use the provided Visual Studio 2017 project, you will need to install Pthreads-win32 from http://www.ensta-bretagne.fr/lebars/Share/pthreads-win32-msvc.zip. ### Optional: Android Interface From 98505623b5772c24febd49a8f856b4e5fae202d9 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Wed, 27 Dec 2017 19:06:37 +0100 Subject: [PATCH 22/26] Updated Windows compatibility of C++ sample --- C++/RazorAHRS.cpp | 18 ++++----- C++/RazorAHRS.h | 22 +++++------ C++/RazorAHRS.pro | 2 +- C++/RazorAHRS.vcxproj | 8 ++-- C++/unix_adapt/bits/fcntl2.h | 74 ++++++++++++++++++++++++++++++++++++ C++/unix_adapt/termios.h | 50 +++++++++++++++++++++++- README.md | 3 +- 7 files changed, 149 insertions(+), 28 deletions(-) diff --git a/C++/RazorAHRS.cpp b/C++/RazorAHRS.cpp index 7c2fe5b..52832e3 100644 --- a/C++/RazorAHRS.cpp +++ b/C++/RazorAHRS.cpp @@ -167,7 +167,7 @@ RazorAHRS::_init_razor() else { if (errno != EAGAIN && errno != EINTR) - throw std::runtime_error("Can not read from serial port (1)."); + throw std::runtime_error("Cannot read from serial port (1)."); } // check timeout @@ -181,7 +181,7 @@ RazorAHRS::_init_razor() } if (elapsed_ms(t0, t2) > _connect_timeout_ms) // timeout -> tracker not present - throw std::runtime_error("Can not init: tracker does not answer."); + throw std::runtime_error("Cannot init: tracker does not answer."); } @@ -196,7 +196,7 @@ 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 throw std::runtime_error("Can not init: unknown 'mode' parameter."); + else throw std::runtime_error("Cannot init: unknown 'mode' parameter."); write(_serial_port, config.data(), config.length()); @@ -219,7 +219,7 @@ RazorAHRS::_init_razor() else { if (errno != EAGAIN && errno != EINTR) - throw std::runtime_error("Can not read from serial port (2)."); + throw std::runtime_error("Cannot read from serial port (2)."); } } @@ -313,10 +313,10 @@ RazorAHRS::_thread(void *arg) if (_mode == YAW_PITCH_ROLL) { // 3 floats if (_input_pos == 12) // we received a full frame { - // convert endianess if necessary + // convert endianness if necessary if (_big_endian()) { - _swap_endianess(_input_buf.ypr, 3); + _swap_endianness(_input_buf.ypr, 3); } // invoke callback @@ -327,10 +327,10 @@ RazorAHRS::_thread(void *arg) } else { // raw or calibrated sensor data (9 floats) if (_input_pos == 36) // we received a full frame { - // convert endianess if necessary + // convert endianness if necessary if (_big_endian()) { - _swap_endianess(_input_buf.amg, 9); + _swap_endianness(_input_buf.amg, 9); } // invoke callback @@ -345,7 +345,7 @@ RazorAHRS::_thread(void *arg) { if (errno != EAGAIN && errno != EINTR) { - error("Can not read from serial port (3)."); + error("Cannot read from serial port (3)."); return arg; } } diff --git a/C++/RazorAHRS.h b/C++/RazorAHRS.h index ca8c3ad..8d6d405 100644 --- a/C++/RazorAHRS.h +++ b/C++/RazorAHRS.h @@ -89,7 +89,7 @@ class RazorAHRS /* threading stuff */ pthread_t _thread_id; void* _thread(void*); // thread main function - volatile bool _stop_thread; // thred stop flag + volatile bool _stop_thread; // thread stop flag // start the tracking thread void _start_io_thread() @@ -124,14 +124,14 @@ class RazorAHRS return (*(reinterpret_cast (&num))) != 1; } - // swap endianess of int - void _swap_endianess(int &i) + // swap endianness of int + void _swap_endianness(int &i) { i = (i >> 24) | ((i << 8) & 0x00FF0000) | ((i >> 8) & 0x0000FF00) | (i << 24); } - // swap endianess of float - void _swap_endianess(float &f) + // swap endianness of float + void _swap_endianness(float &f) { float swapped; char *f_as_char = reinterpret_cast (&f); @@ -146,18 +146,18 @@ class RazorAHRS f = swapped; } - // swap endianess of int array - void _swap_endianess(int arr[], int arr_length) + // swap endianness of int array + void _swap_endianness(int arr[], int arr_length) { for (int i = 0; i < arr_length; i++) - _swap_endianess(arr[i]); + _swap_endianness(arr[i]); } - // swap endianess of float array - void _swap_endianess(float arr[], int arr_length) + // swap endianness of float array + void _swap_endianness(float arr[], int arr_length) { for (int i = 0; i < arr_length; i++) - _swap_endianess(arr[i]); + _swap_endianness(arr[i]); } }; diff --git a/C++/RazorAHRS.pro b/C++/RazorAHRS.pro index 239ee59..f0fe913 100644 --- a/C++/RazorAHRS.pro +++ b/C++/RazorAHRS.pro @@ -6,7 +6,7 @@ CONFIG -= qt DEFINES -= UNICODE win32:INCLUDEPATH += unix_adapt -win32:DEFINES += DISABLE_TIMEZONE_STRUCT_REDEFINITION +win32:DEFINES += DISABLE_TIMEZONE_STRUCT_REDEFINITION ENABLE_O_NDELAY_WORKAROUND DEFINES += _REENTRANT LIBS += -lpthread diff --git a/C++/RazorAHRS.vcxproj b/C++/RazorAHRS.vcxproj index 293343e..043add8 100644 --- a/C++/RazorAHRS.vcxproj +++ b/C++/RazorAHRS.vcxproj @@ -105,7 +105,7 @@ Disabled - WIN32;HAVE_STRUCT_TIMESPEC;_REENTRANT;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + HAVE_STRUCT_TIMESPEC;ENABLE_O_NDELAY_WORKAROUND;_REENTRANT;WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) true EnableFastChecks MultiThreadedDebugDLL @@ -129,7 +129,7 @@ Disabled - WIN32;HAVE_STRUCT_TIMESPEC;_REENTRANT;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + HAVE_STRUCT_TIMESPEC;ENABLE_O_NDELAY_WORKAROUND;_REENTRANT;WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) EnableFastChecks MultiThreadedDebugDLL @@ -153,7 +153,7 @@ - WIN32;HAVE_STRUCT_TIMESPEC;_REENTRANT;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + HAVE_STRUCT_TIMESPEC;ENABLE_O_NDELAY_WORKAROUND;_REENTRANT;WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) MultiThreaded Level4 @@ -175,7 +175,7 @@ - WIN32;HAVE_STRUCT_TIMESPEC;_REENTRANT;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + HAVE_STRUCT_TIMESPEC;ENABLE_O_NDELAY_WORKAROUND;_REENTRANT;WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) MultiThreaded diff --git a/C++/unix_adapt/bits/fcntl2.h b/C++/unix_adapt/bits/fcntl2.h index 7737d97..b238659 100644 --- a/C++/unix_adapt/bits/fcntl2.h +++ b/C++/unix_adapt/bits/fcntl2.h @@ -81,6 +81,11 @@ __inline int fcntl(int __fd, int __cmd, ...) { HANDLE hDev = (HANDLE)(intptr_t)__fd; COMMTIMEOUTS timeouts; + // Disable if XON/XOFF are necessary, enable if fcntl() is used together with O_NDELAY and timeouts... +#ifdef ENABLE_O_NDELAY_WORKAROUND + DCB dcb; + DWORD ReadIntervalTimeout = 0, ReadTotalTimeoutConstant = 0, ReadTotalTimeoutMultiplier = 0, WriteTotalTimeoutConstant = 0; +#endif // ENABLE_O_NDELAY_WORKAROUND va_list argptr; va_start(argptr, __cmd); @@ -96,6 +101,41 @@ __inline int fcntl(int __fd, int __cmd, ...) } if (flags & O_NDELAY) { + // Disable if XON/XOFF are necessary, enable if fcntl() is used together with O_NDELAY and timeouts... +#ifdef ENABLE_O_NDELAY_WORKAROUND + // Need to save the original timeouts settings for next calls of fcntl()... + + memset(&dcb, 0, sizeof(DCB)); + if (!GetCommState(hDev, &dcb)) + { + errno = EIO; + return -1; + } + + // Warning : XoffLim and XonLim are just used as placeholders to save the original timeouts settings, + // they are not normally related to timeouts... + + ReadIntervalTimeout = timeouts.ReadIntervalTimeout; + ReadTotalTimeoutConstant = timeouts.ReadTotalTimeoutConstant; + ReadTotalTimeoutMultiplier = timeouts.ReadTotalTimeoutMultiplier; + WriteTotalTimeoutConstant = timeouts.WriteTotalTimeoutConstant; + if (ReadIntervalTimeout == MAXDWORD) ReadIntervalTimeout = 255*100; + else if (ReadIntervalTimeout/100 > 254) ReadIntervalTimeout = 254*100; + if (ReadTotalTimeoutConstant == MAXDWORD) ReadTotalTimeoutConstant = 255*100; + else if (ReadTotalTimeoutConstant/100 > 254) ReadTotalTimeoutConstant = 254*100; + if (ReadTotalTimeoutMultiplier == MAXDWORD) ReadTotalTimeoutMultiplier = 255*100; + else if (ReadTotalTimeoutMultiplier/100 > 254) ReadTotalTimeoutMultiplier = 254*100; + if (WriteTotalTimeoutConstant == MAXDWORD) WriteTotalTimeoutConstant = 255*100; + else if (WriteTotalTimeoutConstant/100 > 254) WriteTotalTimeoutConstant = 254*100; + dcb.XoffLim = (WORD)(((ReadIntervalTimeout/100)<<8)|(ReadTotalTimeoutConstant/100)); + dcb.XonLim = (WORD)(((ReadTotalTimeoutMultiplier/100)<<8)|(WriteTotalTimeoutConstant/100)); + if (!SetCommState(hDev, &dcb)) + { + errno = EIO; + return -1; + } +#endif // ENABLE_O_NDELAY_WORKAROUND + // Change timeout settings. timeouts.ReadIntervalTimeout = MAXDWORD; timeouts.ReadTotalTimeoutConstant = 0; timeouts.ReadTotalTimeoutMultiplier = 0; @@ -109,11 +149,45 @@ __inline int fcntl(int __fd, int __cmd, ...) } else { + // Disable if XON/XOFF are necessary, enable if fcntl() is used together with O_NDELAY and timeouts... +#ifdef ENABLE_O_NDELAY_WORKAROUND + // Should restore original timeout values... + + memset(&dcb, 0, sizeof(DCB)); + if (!GetCommState(hDev, &dcb)) + { + errno = EIO; + return -1; + } + + // Warning : XoffLim and XonLim are just used as placeholders to save the original timeouts settings, + // they are not normally related to timeouts... + + if ((dcb.XoffLim>>8) == 255) timeouts.ReadIntervalTimeout = MAXDWORD; + else timeouts.ReadIntervalTimeout = (dcb.XoffLim>>8)*100; + if ((dcb.XoffLim&0x00FF) == 255) timeouts.ReadTotalTimeoutConstant = MAXDWORD; + else timeouts.ReadTotalTimeoutConstant = (dcb.XoffLim&0x00FF)*100; + if ((dcb.XonLim>>8) == 255) timeouts.ReadTotalTimeoutMultiplier = MAXDWORD; + else timeouts.ReadTotalTimeoutMultiplier = (dcb.XonLim>>8)*100; + if ((dcb.XonLim&0x00FF) == 255) timeouts.WriteTotalTimeoutConstant = MAXDWORD; + else timeouts.WriteTotalTimeoutConstant = (dcb.XonLim&0x00FF)*100; + timeouts.WriteTotalTimeoutMultiplier = 0; + // Set to full blocking in case the restored values were non-blocking. + if ((timeouts.ReadIntervalTimeout == MAXDWORD)&&(timeouts.ReadTotalTimeoutConstant == 0)&&(timeouts.ReadTotalTimeoutMultiplier == 0)) + { + timeouts.ReadIntervalTimeout = 0; + timeouts.ReadTotalTimeoutConstant = 0; + timeouts.ReadTotalTimeoutMultiplier = 0; + timeouts.WriteTotalTimeoutConstant = 0; + timeouts.WriteTotalTimeoutMultiplier = 0; + } +#else timeouts.ReadIntervalTimeout = 0; timeouts.ReadTotalTimeoutConstant = 0; timeouts.ReadTotalTimeoutMultiplier = 0; timeouts.WriteTotalTimeoutConstant = 0; timeouts.WriteTotalTimeoutMultiplier = 0; +#endif // ENABLE_O_NDELAY_WORKAROUND if (!SetCommTimeouts(hDev, &timeouts)) { errno = EIO; diff --git a/C++/unix_adapt/termios.h b/C++/unix_adapt/termios.h index 84aa914..45bfc45 100644 --- a/C++/unix_adapt/termios.h +++ b/C++/unix_adapt/termios.h @@ -135,7 +135,6 @@ __inline int tcgetattr(int __fd, struct termios *__termios_p) //#pragma region TIMEOUTS memset(&timeouts, 0, sizeof(COMMTIMEOUTS)); - if (!GetCommTimeouts(hDev, &timeouts)) { errno = EIO; @@ -195,7 +194,6 @@ __inline int tcgetattr(int __fd, struct termios *__termios_p) //#pragma region DCB memset(&dcb, 0, sizeof(DCB)); - if (!GetCommState(hDev, &dcb)) { errno = EIO; @@ -381,12 +379,20 @@ __inline int tcsetattr(int __fd, int __optional_actions, const struct termios *_ { HANDLE hDev = (HANDLE)(intptr_t)__fd; COMMTIMEOUTS timeouts; +#ifdef ENABLE_O_NDELAY_WORKAROUND + DWORD ReadIntervalTimeout = 0, ReadTotalTimeoutConstant = 0, ReadTotalTimeoutMultiplier = 0, WriteTotalTimeoutConstant = 0; +#endif // ENABLE_O_NDELAY_WORKAROUND DCB dcb; UNREFERENCED_PARAMETER(__optional_actions); //#pragma region TIMEOUTS memset(&timeouts, 0, sizeof(COMMTIMEOUTS)); + if (!GetCommTimeouts(hDev, &timeouts)) + { + errno = EIO; + return -1; + } if ((__termios_p->c_cc[VMIN] == 0)&&(__termios_p->c_cc[VTIME]) == 0) { @@ -438,6 +444,41 @@ __inline int tcsetattr(int __fd, int __optional_actions, const struct termios *_ timeouts.WriteTotalTimeoutConstant = 0; // ??? } + // Disable if XON/XOFF are necessary, enable if fcntl() is used together with O_NDELAY and timeouts... +#ifdef ENABLE_O_NDELAY_WORKAROUND + // Need to save the original timeouts settings for next calls of fcntl()... + + memset(&dcb, 0, sizeof(DCB)); + if (!GetCommState(hDev, &dcb)) + { + errno = EIO; + return -1; + } + + // Warning : XoffLim and XonLim are just used as placeholders to save the original timeouts settings, + // they are not normally related to timeouts... + + ReadIntervalTimeout = timeouts.ReadIntervalTimeout; + ReadTotalTimeoutConstant = timeouts.ReadTotalTimeoutConstant; + ReadTotalTimeoutMultiplier = timeouts.ReadTotalTimeoutMultiplier; + WriteTotalTimeoutConstant = timeouts.WriteTotalTimeoutConstant; + if (ReadIntervalTimeout == MAXDWORD) ReadIntervalTimeout = 255*100; + else if (ReadIntervalTimeout/100 > 254) ReadIntervalTimeout = 254*100; + if (ReadTotalTimeoutConstant == MAXDWORD) ReadTotalTimeoutConstant = 255*100; + else if (ReadTotalTimeoutConstant/100 > 254) ReadTotalTimeoutConstant = 254*100; + if (ReadTotalTimeoutMultiplier == MAXDWORD) ReadTotalTimeoutMultiplier = 255*100; + else if (ReadTotalTimeoutMultiplier/100 > 254) ReadTotalTimeoutMultiplier = 254*100; + if (WriteTotalTimeoutConstant == MAXDWORD) WriteTotalTimeoutConstant = 255*100; + else if (WriteTotalTimeoutConstant/100 > 254) WriteTotalTimeoutConstant = 254*100; + dcb.XoffLim = (WORD)(((ReadIntervalTimeout/100)<<8)|(ReadTotalTimeoutConstant/100)); + dcb.XonLim = (WORD)(((ReadTotalTimeoutMultiplier/100)<<8)|(WriteTotalTimeoutConstant/100)); + if (!SetCommState(hDev, &dcb)) + { + errno = EIO; + return -1; + } +#endif // ENABLE_O_NDELAY_WORKAROUND + if (!SetCommTimeouts(hDev, &timeouts)) { errno = EIO; @@ -447,6 +488,11 @@ __inline int tcsetattr(int __fd, int __optional_actions, const struct termios *_ //#pragma region DCB memset(&dcb, 0, sizeof(DCB)); + if (!GetCommState(hDev, &dcb)) + { + errno = EIO; + return -1; + } // Not possible to have different c_ispeed and c_ospeed on Windows... dcb.BaudRate = _linuxbaudrate2windows(__termios_p->c_ispeed); diff --git a/README.md b/README.md index 0e32c2e..c55a472 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,7 @@ Tutorial --- You find a [detailed tutorial in the Wiki](https://github.com/ptrbrtz/razor-9dof-ahrs/wiki/Tutorial). + Note: For SEN-14001 (*9DoF Razor IMU M0*), you will need to follow the same instructions as for the default firmware on https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide and use an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library (an updated version of the default firmware is also available on https://github.com/lebarsfa/9DOF_Razor_IMU). Quick setup @@ -27,7 +28,7 @@ Run `Processing/Razor_AHRS_test/Razor_AHRS_test.pde` using *Processing*. ### Optional: Mac OS X / Unix / Linux / Windows C++ Interface -Use the provided Qt project (check Projects\Run Settings\Run in terminal to force your application to run inside a separate terminal) or compile test program from the command line (add `-Iunix_adapt -DDISABLE_TIMEZONE_STRUCT_REDEFINITION` for MinGW/MSYS): +Use the provided Qt project (check Projects\Run Settings\Run in terminal to force your application to run inside a separate terminal) or compile test program from the command line (add `-Iunix_adapt -DDISABLE_TIMEZONE_STRUCT_REDEFINITION -DENABLE_O_NDELAY_WORKAROUND` for MinGW/MSYS): g++ Example.cpp RazorAHRS.cpp -Wall -D_REENTRANT -lpthread -o example From 5c179de5e7b109045992f8b8af7373633b1fcb9a Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Wed, 27 Dec 2017 19:10:25 +0100 Subject: [PATCH 23/26] Updated README --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index c55a472..aad35f5 100644 --- a/README.md +++ b/README.md @@ -28,7 +28,7 @@ Run `Processing/Razor_AHRS_test/Razor_AHRS_test.pde` using *Processing*. ### Optional: Mac OS X / Unix / Linux / Windows C++ Interface -Use the provided Qt project (check Projects\Run Settings\Run in terminal to force your application to run inside a separate terminal) or compile test program from the command line (add `-Iunix_adapt -DDISABLE_TIMEZONE_STRUCT_REDEFINITION -DENABLE_O_NDELAY_WORKAROUND` for MinGW/MSYS): +Use the provided Qt project (check `Projects` → `Run Settings` → `Run in terminal` to force your application to run inside a separate terminal) or compile test program from the command line (add `-Iunix_adapt -DDISABLE_TIMEZONE_STRUCT_REDEFINITION -DENABLE_O_NDELAY_WORKAROUND` for MinGW/MSYS): g++ Example.cpp RazorAHRS.cpp -Wall -D_REENTRANT -lpthread -o example From 71f259394ef9db48ef9f8fb1c9a3e9841035e9c3 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Sat, 10 Oct 2020 22:23:02 +0200 Subject: [PATCH 24/26] Added comment about LOG_PORT SERIAL_PORT_HARDWARE --- Arduino/Razor_AHRS/Razor_AHRS.ino | 1 + 1 file changed, 1 insertion(+) diff --git a/Arduino/Razor_AHRS/Razor_AHRS.ino b/Arduino/Razor_AHRS/Razor_AHRS.ino index 13b113c..5f22555 100644 --- a/Arduino/Razor_AHRS/Razor_AHRS.ino +++ b/Arduino/Razor_AHRS/Razor_AHRS.ino @@ -221,6 +221,7 @@ #if HW__VERSION_CODE == 14001 // Set your port used to send out data here! #define LOG_PORT SERIAL_PORT_USBVIRTUAL +//#define LOG_PORT SERIAL_PORT_HARDWARE #else // Set your port used to send out data here! #define LOG_PORT Serial From 6f20e11fabfe8c932f1b9a18ba679b99a9ce9565 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Sun, 20 Dec 2020 23:44:31 +0100 Subject: [PATCH 25/26] Added support for "OpenLog Artemis": DEV-16832 --- Arduino/Razor_AHRS/Razor_AHRS.ino | 96 ++++++++++---- Arduino/Razor_AHRS/Sensors.ino | 200 +++++++++++++++++++++++++++++- README.md | 12 +- 3 files changed, 277 insertions(+), 31 deletions(-) diff --git a/Arduino/Razor_AHRS/Razor_AHRS.ino b/Arduino/Razor_AHRS/Razor_AHRS.ino index 5f22555..6af33cc 100644 --- a/Arduino/Razor_AHRS/Razor_AHRS.ino +++ b/Arduino/Razor_AHRS/Razor_AHRS.ino @@ -1,14 +1,14 @@ /*************************************************************************************************************** * Razor AHRS Firmware * 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) +* for Sparkfun "OpenLog Artemis" (DEV-16832), "9DoF Razor IMU M0" (SEN-14001), +* "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: +* Original repository: * https://github.com/ptrbrtz/razor-9dof-ahrs * * @@ -44,7 +44,7 @@ * * Added static magnetometer soft iron distortion compensation. * * v1.4.2 * * (No core firmware changes) -* * v1.5 +* * v1.5.0 * * Added support for "9DoF Razor IMU M0": SEN-14001. * * v1.5.1 * * Added ROS-compatible output mode. @@ -64,19 +64,29 @@ * * Increased startup delay to try to get a correct initial orientation for the M0. * * v1.5.7 * * Calibration data are now also used to compute the initial orientation. +* * v1.6.0 +* * Added support for "OpenLog Artemis": DEV-16832. See OLA_IMU_Basics.ino sample from https://github.com/sparkfun/OpenLog_Artemis. +* * Added a command to toggle the status LED. * * TODOs: * * Allow optional use of Flash/EEPROM for storing and reading calibration values. * * Use self-test and temperature-compensation features of the sensors. ***************************************************************************************************************/ +/* + "OpenLog Artemis" hardware versions: DEV-16832 + + Arduino IDE : Follow the same instructions as for the default firmware on + https://github.com/sparkfun/OpenLog_Artemis/blob/master/Firmware/Test%20Sketches/OLA_IMU_Basics/OLA_IMU_Basics.ino +*/ + /* "9DoF Razor IMU M0" hardware versions: SEN-14001 Arduino IDE : Follow the same instructions as for the default firmware on https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide and use an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from - https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library" + https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library */ /* @@ -102,8 +112,8 @@ */ /* - Axis definition (differs from definition printed on the board!): - X axis pointing forward (towards the short edge with the connector holes) + Axis definition (differs from definition printed on the board, see also https://github.com/Razor-AHRS/razor-9dof-ahrs/issues/57#issuecomment-378585065!): + X axis pointing forward (towards where "sparkfun" is written for DEV-16832 and SEN-14001, the short edge with the connector holes for SEN-10125 and SEN-10736) Y axis pointing to the right and Z axis pointing down. @@ -174,6 +184,9 @@ "#I" - Toggle INERTIAL-only mode for yaw computation. + "#L" - Toggle status LED. + + "#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. @@ -192,7 +205,7 @@ 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. + The status LED will be on if streaming output is enabled and off otherwise, unless "#L" is sent. Byte order of binary output is little-endian: least significant byte comes first. */ @@ -209,6 +222,7 @@ //#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 14001 // SparkFun "9DoF Razor IMU M0" version "SEN-14001" +//#define HW__VERSION_CODE 16832 // SparkFun "OpenLog Artemis" version "DEV-16832" //#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) @@ -218,12 +232,16 @@ /*****************************************************************/ // Set your serial port baud rate used to send out data here! #define OUTPUT__BAUD_RATE 57600 -#if HW__VERSION_CODE == 14001 // Set your port used to send out data here! +#if HW__VERSION_CODE == 16832 +#define LOG_PORT Serial +//#define LOG_PORT Serial1 +//Uart SerialLog(1, 13, 12); // Declares a Uart object called Serial1 using instance 1 of Apollo3 UART peripherals with RX on pin 13 and TX on pin 12 (note, you specify *pins* not Apollo3 pads. This uses the variant's pin map to determine the Apollo3 pad) +//#define LOG_PORT SerialLog +#elif HW__VERSION_CODE == 14001 #define LOG_PORT SERIAL_PORT_USBVIRTUAL //#define LOG_PORT SERIAL_PORT_HARDWARE #else -// Set your port used to send out data here! #define LOG_PORT Serial #endif // HW__VERSION_CODE @@ -268,7 +286,7 @@ boolean output_errors = false; // true or false // SENSOR CALIBRATION /*****************************************************************/ -// How to calibrate? Read the tutorial at http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs +// How to calibrate? Read the tutorial at https://github.com/ptrbrtz/razor-9dof-ahrs/wiki/Tutorial // Put MIN/MAX and OFFSET readings for your board here! // For the M0, only the extended magnetometer calibration seems to be really necessary if DEBUG__USE_DMP_M0 is set to true... // Accelerometer @@ -383,7 +401,34 @@ boolean DEBUG__NO_DRIFT_CORRECTION = false; #error YOU HAVE TO SELECT THE HARDWARE YOU ARE USING! See "HARDWARE OPTIONS" in "USER SETUP AREA" at top of Razor_AHRS.ino! #endif -#if HW__VERSION_CODE == 14001 +#if HW__VERSION_CODE == 16832 +// OLA Specifics: +const byte PIN_IMU_POWER = 27; // The Red SparkFun version of the OLA (V10) uses pin 27 +//const byte PIN_IMU_POWER = 22; // The Black SparkX version of the OLA (X04) uses pin 22 +const byte PIN_IMU_INT = 37; +const byte PIN_IMU_CHIP_SELECT = 44; +// For low power... +const byte PIN_QWIIC_POWER = 18; +const byte PIN_MICROSD_POWER = 15; //x04 + +#include "ICM_20948.h" // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU + +#define USE_SPI // Uncomment this to use SPI + +#define SPI_PORT SPI // Your desired SPI port. Used only when "USE_SPI" is defined +#define CS_PIN PIN_IMU_CHIP_SELECT // Which pin you connect CS to. Used only when "USE_SPI" is defined. OLA uses pin 44. + +#define WIRE_PORT Wire // Your desired Wire port. Used when "USE_SPI" is not defined +#define AD0_VAL 1 // The value of the last bit of the I2C address. + // On the SparkFun 9DoF IMU breakout the default is 1, and when + // the ADR jumper is closed the value becomes 0 + +#ifdef USE_SPI + ICM_20948_SPI myICM; // If using SPI create an ICM_20948_SPI object +#else + ICM_20948_I2C myICM; // Otherwise create an ICM_20948_I2C object +#endif +#elif HW__VERSION_CODE == 14001 // MPU-9250 Digital Motion Processing (DMP) Library #include @@ -431,7 +476,7 @@ float MAGN_Z_SCALE = (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET)); -#if HW__VERSION_CODE == 14001 +#if (HW__VERSION_CODE == 16832) || (HW__VERSION_CODE == 14001) #define GYRO_SCALED_RAD(x) (x) // Calculate the scaled gyro readings in radians per second #else // Gain for gyroscope (ITG-3200) @@ -445,8 +490,13 @@ float MAGN_Z_SCALE = (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET)); #define Kp_YAW 1.2f #define Ki_YAW 0.00002f -// Stuff -#define STATUS_LED_PIN 13 // Pin number of status LED +// Pin number of status LED +#if (HW__VERSION_CODE == 16832) +#define STATUS_LED_PIN 19 +#else +#define STATUS_LED_PIN 13 +#endif // HW__VERSION_CODE + #define TO_RAD(x) (x * 0.01745329252) // *pi/180 #define TO_DEG(x) (x * 57.2957795131) // *180/pi @@ -499,7 +549,7 @@ int num_gyro_errors = 0; int num_math_errors = 0; void read_sensors() { -#if HW__VERSION_CODE == 14001 +#if (HW__VERSION_CODE == 16832) || (HW__VERSION_CODE == 14001) loop_imu(); #else Read_Gyro(); // Read gyroscope @@ -639,12 +689,8 @@ void setup() // Init sensors delay(50); // Give sensors enough time to start -#if HW__VERSION_CODE == 14001 -#if DEBUG__ENABLE_INTERRUPT_M0 == true - // Set up MPU-9250 interrupt input (active-low) - pinMode(MPU9250_INT_PIN, INPUT_PULLUP); -#endif // DEBUG__ENABLE_INTERRUPT_M0 - initIMU(); +#if (HW__VERSION_CODE == 16832) || (HW__VERSION_CODE == 14001) + beginIMU(); #else I2C_Init(); Accel_Init(); @@ -653,7 +699,7 @@ void setup() #endif // HW__VERSION_CODE // Read sensors, init DCM algorithm -#if HW__VERSION_CODE == 14001 +#if (HW__VERSION_CODE == 16832) || (HW__VERSION_CODE == 14001) delay(400); // Give sensors enough time to collect data #else delay(20); // Give sensors enough time to collect data @@ -944,6 +990,10 @@ void loop() initialimuyaw = imu.yaw*PI/180.0f; #endif // DEBUG__USE_ONLY_DMP_M0 } + else if (command == 'L') // Toggle status _L_ed + { + if (digitalRead(STATUS_LED_PIN) == HIGH) digitalWrite(STATUS_LED_PIN, LOW); else digitalWrite(STATUS_LED_PIN, HIGH); + } #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 "#". diff --git a/Arduino/Razor_AHRS/Sensors.ino b/Arduino/Razor_AHRS/Sensors.ino index c59e2f9..7b9c316 100644 --- a/Arduino/Razor_AHRS/Sensors.ino +++ b/Arduino/Razor_AHRS/Sensors.ino @@ -1,6 +1,197 @@ /* This file is part of the Razor AHRS Firmware */ -#if HW__VERSION_CODE == 14001 +#if HW__VERSION_CODE == 16832 +bool imuAccDLPF = true; // IMU accelerometer Digital Low Pass Filter. +bool imuGyroDLPF = true; // IMU gyro Digital Low Pass Filter. +// IMU accelerometer full scale (ICM_20948_ACCEL_CONFIG_FS_SEL_e) +// 0: +/- 2g +// 1: +/- 4g +// 2: +/- 8g +// 3: +/- 16g +int imuAccFSS = gpm16; +// IMU accelerometer DLPF bandwidth (ICM_20948_ACCEL_CONFIG_DLPCFG_e) +// 0: 246.0 (3dB) 265.0 (Nyquist) (Hz) +// 1: 246.0 (3dB) 265.0 (Nyquist) (Hz) +// 2: 111.4 (3dB) 136.0 (Nyquist) (Hz) +// 3: 50.4 (3dB) 68.8 (Nyquist) (Hz) +// 4: 23.9 (3dB) 34.4 (Nyquist) (Hz) +// 5: 11.5 (3dB) 17.0 (Nyquist) (Hz) +// 6: 5.7 (3dB) 8.3 (Nyquist) (Hz) +// 7: 473 (3dB) 499 (Nyquist) (Hz) +int imuAccDLPFBW = 6;//acc_d50bw4_n68bw8; +// IMU gyro full scale (ICM_20948_GYRO_CONFIG_1_FS_SEL_e) +// 0: +/- 250dps +// 1: +/- 500dps +// 2: +/- 1000dps +// 3: +/- 2000dps +int imuGyroFSS = dps2000; +// IMU gyro DLPF bandwidth (ICM_20948_GYRO_CONFIG_1_DLPCFG_e) +// 0: 196.6 (3dB) 229.8 (Nyquist) (Hz) +// 1: 151.8 (3dB) 187.6 (Nyquist) (Hz) +// 2: 119.5 (3dB) 154.3 (Nyquist) (Hz) +// 3: 51.2 (3dB) 73.3 (Nyquist) (Hz) +// 4: 23.9 (3dB) 35.9 (Nyquist) (Hz) +// 5: 11.6 (3dB) 17.8 (Nyquist) (Hz) +// 6: 5.7 (3dB) 8.9 (Nyquist) (Hz) +// 7: 361.4 (3dB) 376.5 (Nyquist) (Hz) +int imuGyroDLPFBW = 6;//gyr_d51bw2_n73bw3; + +void qwiicPowerOn() +{ + pinMode(PIN_QWIIC_POWER, OUTPUT); + digitalWrite(PIN_QWIIC_POWER, LOW); +} + +void qwiicPowerOff() +{ + pinMode(PIN_QWIIC_POWER, OUTPUT); + digitalWrite(PIN_QWIIC_POWER, HIGH); +} + +void microSDPowerOn() +{ + pinMode(PIN_MICROSD_POWER, OUTPUT); + digitalWrite(PIN_MICROSD_POWER, LOW); +} + +void microSDPowerOff() +{ + pinMode(PIN_MICROSD_POWER, OUTPUT); + digitalWrite(PIN_MICROSD_POWER, HIGH); +} + +void imuPowerOn() +{ + pinMode(PIN_IMU_POWER, OUTPUT); + digitalWrite(PIN_IMU_POWER, HIGH); +} + +void imuPowerOff() +{ + pinMode(PIN_IMU_POWER, OUTPUT); + digitalWrite(PIN_IMU_POWER, LOW); +} + +bool enableCIPOpullUp() +{ + //Add CIPO pull-up + ap3_err_t retval = AP3_OK; + am_hal_gpio_pincfg_t cipoPinCfg = AP3_GPIO_DEFAULT_PINCFG; + cipoPinCfg.uFuncSel = AM_HAL_PIN_6_M0MISO; + cipoPinCfg.eDriveStrength = AM_HAL_GPIO_PIN_DRIVESTRENGTH_12MA; + cipoPinCfg.eGPOutcfg = AM_HAL_GPIO_PIN_OUTCFG_PUSHPULL; + cipoPinCfg.uIOMnum = AP3_SPI_IOM; + cipoPinCfg.ePullup = AM_HAL_GPIO_PIN_PULLUP_1_5K; + padMode(MISO, cipoPinCfg, &retval); + return (retval == AP3_OK); +} + +bool beginIMU(void) +{ + // For low power... + power_adc_disable(); // Power down ADC. It it started by default before setup(). + //Disable all pads + //for (int x = 0; x < 50; x++) + // am_hal_gpio_pinconfig(x, g_AM_HAL_GPIO_DISABLE); + pinMode(PIN_QWIIC_POWER, OUTPUT); + pinMode(PIN_MICROSD_POWER, OUTPUT); + qwiicPowerOff(); + microSDPowerOff(); + +#ifdef USE_SPI + SPI_PORT.begin(); +#else + WIRE_PORT.begin(); + WIRE_PORT.setClock(400000); +#endif + + enableCIPOpullUp(); // Enable CIPO pull-up on the OLA + + pinMode(PIN_IMU_CHIP_SELECT, OUTPUT); + digitalWrite(PIN_IMU_CHIP_SELECT, HIGH); //Be sure IMU is deselected + + //Reset ICM by power cycling it + imuPowerOff(); + + delay(10); + + imuPowerOn(); // Enable power for the OLA IMU + + delay(100); // Wait for the IMU to power up + + bool initialized = false; + while (!initialized) { +#ifdef USE_SPI + myICM.begin(CS_PIN, SPI_PORT); +#else + myICM.begin(WIRE_PORT, AD0_VAL); +#endif + if (myICM.status != ICM_20948_Stat_Ok) { + if (output_errors) LOG_PORT.println("!ERR: initializing ICM"); + delay(500); + } + else { + initialized = true; + } + } + + delay(25); // See https://github.com/sparkfun/OpenLog_Artemis/issues/18... + + if (myICM.enableDLPF(ICM_20948_Internal_Acc, imuAccDLPF) != ICM_20948_Stat_Ok) + { + if (output_errors) LOG_PORT.println("!ERR: configuring DLPF"); + } + if (myICM.enableDLPF(ICM_20948_Internal_Gyr, imuGyroDLPF) != ICM_20948_Stat_Ok) + { + if (output_errors) LOG_PORT.println("!ERR: configuring DLPF"); + } + ICM_20948_dlpcfg_t dlpcfg; + dlpcfg.a = imuAccDLPFBW; + dlpcfg.g = imuGyroDLPFBW; + if (myICM.setDLPFcfg((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), dlpcfg) != ICM_20948_Stat_Ok) + { + if (output_errors) LOG_PORT.println("!ERR: configuring DLPFBW"); + } + ICM_20948_fss_t FSS; + FSS.a = imuAccFSS; + FSS.g = imuGyroFSS; + if (myICM.setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), FSS) != ICM_20948_Stat_Ok) + { + if (output_errors) LOG_PORT.println("!ERR: configuring FSS"); + } + + return true; +} + +void loop_imu() +{ + if (!myICM.dataReady()) + { + num_accel_errors++; + if (output_errors) LOG_PORT.println("!ERR: reading accelerometer"); + num_gyro_errors++; + if (output_errors) LOG_PORT.println("!ERR: reading gyroscope"); + num_magn_errors++; + if (output_errors) LOG_PORT.println("!ERR: reading magnetometer"); + return; + } + + myICM.getAGMT(); // The values are only updated when you call 'getAGMT' + + // Conversion from g to similar unit as older versions of Razor... + accel[0] = 250.0f*myICM.accY()/1000.0; + accel[1] = 250.0f*myICM.accX()/1000.0; + accel[2] = 250.0f*myICM.accZ()/1000.0; + // Conversion from degrees/s to rad/s. + gyro[0] = -myICM.gyrY()*PI/180.0f; + gyro[1] = -myICM.gyrX()*PI/180.0f; + gyro[2] = -myICM.gyrZ()*PI/180.0f; + // Conversion from uT to mGauss. + magnetom[0] = (10.0f*myICM.magY()); + magnetom[1] = (-10.0f*myICM.magX()); + magnetom[2] = (10.0f*myICM.magZ()); +} +#elif HW__VERSION_CODE == 14001 /* * Known Bug - * DMP when enabled will sample sensor data at 200Hz and output to FIFO at the rate @@ -34,8 +225,13 @@ unsigned short accelFSR = IMU_ACCEL_FSR; unsigned short gyroFSR = IMU_GYRO_FSR; unsigned short fifoRate = DMP_SAMPLE_RATE; -bool initIMU(void) +bool beginIMU(void) { +#if DEBUG__ENABLE_INTERRUPT_M0 == true + // Set up MPU-9250 interrupt input (active-low) + pinMode(MPU9250_INT_PIN, INPUT_PULLUP); +#endif // DEBUG__ENABLE_INTERRUPT_M0 + // imu.begin() should return 0 on success. Will initialize // I2C bus, and reset MPU-9250 to defaults. if (imu.begin() != INV_SUCCESS) diff --git a/README.md b/README.md index aad35f5..3742fe8 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,9 @@ Razor AHRS --- -**9 Degree of Measurement Attitude and Heading Reference System** for Sparkfun *9DOF Razor IMU* (SEN-10125, SEN-10736 and SEN-14001) and SparkFun *9DOF Sensor Stick* (SEN-10183, SEN-10321 and SEN-10724) +**9 Degree of Measurement Attitude and Heading Reference System** for Sparkfun *OpenLog Artemis* (DEV-16832), *9DoF Razor IMU M0* (SEN-14001), *9DOF Razor IMU* (SEN-10125 and SEN-10736) and *9DOF Sensor Stick* (SEN-10183, SEN-10321 and SEN-10724) -Infos, updates, bug reports, contributions and feedback: https://github.com/ptrbrtz/razor-9dof-ahrs +Original repository : https://github.com/ptrbrtz/razor-9dof-ahrs Download --- @@ -13,9 +13,9 @@ Clone the [repository on GitHub](https://github.com/lebarsfa/razor-9dof-ahrs) or Tutorial --- -You find a [detailed tutorial in the Wiki](https://github.com/ptrbrtz/razor-9dof-ahrs/wiki/Tutorial). +You find a [detailed tutorial in the original Wiki](https://github.com/ptrbrtz/razor-9dof-ahrs/wiki/Tutorial). -Note: For SEN-14001 (*9DoF Razor IMU M0*), you will need to follow the same instructions as for the default firmware on https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide and use an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library (an updated version of the default firmware is also available on https://github.com/lebarsfa/9DOF_Razor_IMU). +Note: For DEV-16832 (*OpenLog Artemis*), you will need to follow the same instructions as for the `OLA_IMU_Basics.ino` sample from https://github.com/sparkfun/OpenLog_Artemis (i.e. get the drivers from https://learn.sparkfun.com/tutorials/how-to-install-ch340-drivers, install SparkFun Apollo3 boards in Arduino IDE as in https://learn.sparkfun.com/tutorials/installing-board-definitions-in-the-arduino-ide and ensure you select `SparkFun Apollo3` → `SparkFun RedBoard Artemis ATP` as the board and install SparkFun ICM 20948 IMU Arduino library as in https://learn.sparkfun.com/tutorials/installing-an-arduino-library). For SEN-14001 (*9DoF Razor IMU M0*), you will need to follow the same instructions as for the default firmware on https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide and use an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library (an updated version of the default firmware is also available on https://github.com/lebarsfa/9DOF_Razor_IMU). Quick setup --- @@ -28,7 +28,7 @@ Run `Processing/Razor_AHRS_test/Razor_AHRS_test.pde` using *Processing*. ### Optional: Mac OS X / Unix / Linux / Windows C++ Interface -Use the provided Qt project (check `Projects` → `Run Settings` → `Run in terminal` to force your application to run inside a separate terminal) or compile test program from the command line (add `-Iunix_adapt -DDISABLE_TIMEZONE_STRUCT_REDEFINITION -DENABLE_O_NDELAY_WORKAROUND` for MinGW/MSYS): +Use the provided *Qt Creator* project (check `Projects` → `Run Settings` → `Run in terminal` to force your application to run inside a separate terminal) or compile test program from the command line (add `-Iunix_adapt -DDISABLE_TIMEZONE_STRUCT_REDEFINITION -DENABLE_O_NDELAY_WORKAROUND` for MinGW/MSYS): g++ Example.cpp RazorAHRS.cpp -Wall -D_REENTRANT -lpthread -o example @@ -36,7 +36,7 @@ Run it: ./example -Note: To use the provided Visual Studio 2017 project, you will need to install Pthreads-win32 from http://www.ensta-bretagne.fr/lebars/Share/pthreads-win32-msvc.zip. +Note: To use the provided *Visual Studio 2017* project, you will need to install Pthreads-win32 from http://www.ensta-bretagne.fr/lebars/Share/pthreads-win32-msvc.zip. ### Optional: Android Interface From 49e9e3d08135a480e7563eebe905c8d7e710a9e8 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Mon, 28 Dec 2020 16:14:50 +0100 Subject: [PATCH 26/26] Support for "OpenLog Artemis": SPX-15846 (retired version, not tested) --- Arduino/Razor_AHRS/Razor_AHRS.ino | 36 ++++++++++++++++++------------- Arduino/Razor_AHRS/Sensors.ino | 4 +--- README.md | 4 ++-- 3 files changed, 24 insertions(+), 20 deletions(-) diff --git a/Arduino/Razor_AHRS/Razor_AHRS.ino b/Arduino/Razor_AHRS/Razor_AHRS.ino index 6af33cc..5d0c04d 100644 --- a/Arduino/Razor_AHRS/Razor_AHRS.ino +++ b/Arduino/Razor_AHRS/Razor_AHRS.ino @@ -1,7 +1,7 @@ /*************************************************************************************************************** * Razor AHRS Firmware * 9 Degree of Measurement Attitude and Heading Reference System -* for Sparkfun "OpenLog Artemis" (DEV-16832), "9DoF Razor IMU M0" (SEN-14001), +* for Sparkfun "OpenLog Artemis" (SPX-15846 and DEV-16832), "9DoF Razor IMU M0" (SEN-14001), * "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 @@ -67,6 +67,8 @@ * * v1.6.0 * * Added support for "OpenLog Artemis": DEV-16832. See OLA_IMU_Basics.ino sample from https://github.com/sparkfun/OpenLog_Artemis. * * Added a command to toggle the status LED. +* * v1.6.1 +* * Attempt to add support for "OpenLog Artemis": SPX-15846 (retired version, not tested). * * TODOs: * * Allow optional use of Flash/EEPROM for storing and reading calibration values. @@ -74,10 +76,11 @@ ***************************************************************************************************************/ /* - "OpenLog Artemis" hardware versions: DEV-16832 + "OpenLog Artemis" hardware versions: SPX-15846 and DEV-16832 - Arduino IDE : Follow the same instructions as for the default firmware on - https://github.com/sparkfun/OpenLog_Artemis/blob/master/Firmware/Test%20Sketches/OLA_IMU_Basics/OLA_IMU_Basics.ino + Arduino IDE : Follow the same instructions as for the OLA_IMU_Basics.ino + sample from https://github.com/sparkfun/OpenLog_Artemis. Select board + "SparkFun RedBoard Artemis ATP" */ /* @@ -86,7 +89,8 @@ Arduino IDE : Follow the same instructions as for the default firmware on https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide and use an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from - https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library + https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library. Select + board "SparkFun 9DoF Razor IMU M0" */ /* @@ -113,7 +117,7 @@ /* Axis definition (differs from definition printed on the board, see also https://github.com/Razor-AHRS/razor-9dof-ahrs/issues/57#issuecomment-378585065!): - X axis pointing forward (towards where "sparkfun" is written for DEV-16832 and SEN-14001, the short edge with the connector holes for SEN-10125 and SEN-10736) + X axis pointing forward (towards where "sparkfun" is written for SPX-15846, DEV-16832 and SEN-14001, the short edge with the connector holes for SEN-10125 and SEN-10736) Y axis pointing to the right and Z axis pointing down. @@ -222,6 +226,7 @@ //#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 14001 // SparkFun "9DoF Razor IMU M0" version "SEN-14001" +//#define HW__VERSION_CODE 15846 // SparkFun "OpenLog Artemis" version "SPX-15846" //#define HW__VERSION_CODE 16832 // SparkFun "OpenLog Artemis" version "DEV-16832" //#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) @@ -233,9 +238,8 @@ // Set your serial port baud rate used to send out data here! #define OUTPUT__BAUD_RATE 57600 // Set your port used to send out data here! -#if HW__VERSION_CODE == 16832 +#if (HW__VERSION_CODE == 16832) || (HW__VERSION_CODE == 15846) #define LOG_PORT Serial -//#define LOG_PORT Serial1 //Uart SerialLog(1, 13, 12); // Declares a Uart object called Serial1 using instance 1 of Apollo3 UART peripherals with RX on pin 13 and TX on pin 12 (note, you specify *pins* not Apollo3 pads. This uses the variant's pin map to determine the Apollo3 pad) //#define LOG_PORT SerialLog #elif HW__VERSION_CODE == 14001 @@ -401,10 +405,12 @@ boolean DEBUG__NO_DRIFT_CORRECTION = false; #error YOU HAVE TO SELECT THE HARDWARE YOU ARE USING! See "HARDWARE OPTIONS" in "USER SETUP AREA" at top of Razor_AHRS.ino! #endif +#if (HW__VERSION_CODE == 16832) || (HW__VERSION_CODE == 15846) #if HW__VERSION_CODE == 16832 -// OLA Specifics: const byte PIN_IMU_POWER = 27; // The Red SparkFun version of the OLA (V10) uses pin 27 -//const byte PIN_IMU_POWER = 22; // The Black SparkX version of the OLA (X04) uses pin 22 +#elif HW__VERSION_CODE == 15846 +const byte PIN_IMU_POWER = 22; // The Black SparkX version of the OLA (X04) uses pin 22 +#endif // HW__VERSION_CODE const byte PIN_IMU_INT = 37; const byte PIN_IMU_CHIP_SELECT = 44; // For low power... @@ -476,7 +482,7 @@ float MAGN_Z_SCALE = (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET)); -#if (HW__VERSION_CODE == 16832) || (HW__VERSION_CODE == 14001) +#if (HW__VERSION_CODE == 16832) || (HW__VERSION_CODE == 15846) || (HW__VERSION_CODE == 14001) #define GYRO_SCALED_RAD(x) (x) // Calculate the scaled gyro readings in radians per second #else // Gain for gyroscope (ITG-3200) @@ -491,7 +497,7 @@ float MAGN_Z_SCALE = (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET)); #define Ki_YAW 0.00002f // Pin number of status LED -#if (HW__VERSION_CODE == 16832) +#if (HW__VERSION_CODE == 16832) || (HW__VERSION_CODE == 15846) #define STATUS_LED_PIN 19 #else #define STATUS_LED_PIN 13 @@ -549,7 +555,7 @@ int num_gyro_errors = 0; int num_math_errors = 0; void read_sensors() { -#if (HW__VERSION_CODE == 16832) || (HW__VERSION_CODE == 14001) +#if (HW__VERSION_CODE == 16832) || (HW__VERSION_CODE == 15846) || (HW__VERSION_CODE == 14001) loop_imu(); #else Read_Gyro(); // Read gyroscope @@ -689,7 +695,7 @@ void setup() // Init sensors delay(50); // Give sensors enough time to start -#if (HW__VERSION_CODE == 16832) || (HW__VERSION_CODE == 14001) +#if (HW__VERSION_CODE == 16832) || (HW__VERSION_CODE == 15846) || (HW__VERSION_CODE == 14001) beginIMU(); #else I2C_Init(); @@ -699,7 +705,7 @@ void setup() #endif // HW__VERSION_CODE // Read sensors, init DCM algorithm -#if (HW__VERSION_CODE == 16832) || (HW__VERSION_CODE == 14001) +#if (HW__VERSION_CODE == 16832) || (HW__VERSION_CODE == 15846) || (HW__VERSION_CODE == 14001) delay(400); // Give sensors enough time to collect data #else delay(20); // Give sensors enough time to collect data diff --git a/Arduino/Razor_AHRS/Sensors.ino b/Arduino/Razor_AHRS/Sensors.ino index 7b9c316..1304b29 100644 --- a/Arduino/Razor_AHRS/Sensors.ino +++ b/Arduino/Razor_AHRS/Sensors.ino @@ -1,6 +1,6 @@ /* This file is part of the Razor AHRS Firmware */ -#if HW__VERSION_CODE == 16832 +#if (HW__VERSION_CODE == 16832) || (HW__VERSION_CODE == 15846) bool imuAccDLPF = true; // IMU accelerometer Digital Low Pass Filter. bool imuGyroDLPF = true; // IMU gyro Digital Low Pass Filter. // IMU accelerometer full scale (ICM_20948_ACCEL_CONFIG_FS_SEL_e) @@ -93,8 +93,6 @@ bool beginIMU(void) //Disable all pads //for (int x = 0; x < 50; x++) // am_hal_gpio_pinconfig(x, g_AM_HAL_GPIO_DISABLE); - pinMode(PIN_QWIIC_POWER, OUTPUT); - pinMode(PIN_MICROSD_POWER, OUTPUT); qwiicPowerOff(); microSDPowerOff(); diff --git a/README.md b/README.md index 3742fe8..ff6722d 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ Razor AHRS --- -**9 Degree of Measurement Attitude and Heading Reference System** for Sparkfun *OpenLog Artemis* (DEV-16832), *9DoF Razor IMU M0* (SEN-14001), *9DOF Razor IMU* (SEN-10125 and SEN-10736) and *9DOF Sensor Stick* (SEN-10183, SEN-10321 and SEN-10724) +**9 Degree of Measurement Attitude and Heading Reference System** for Sparkfun *OpenLog Artemis* (SPX-15846 and DEV-16832), *9DoF Razor IMU M0* (SEN-14001), *9DOF Razor IMU* (SEN-10125 and SEN-10736) and *9DOF Sensor Stick* (SEN-10183, SEN-10321 and SEN-10724) Original repository : https://github.com/ptrbrtz/razor-9dof-ahrs @@ -15,7 +15,7 @@ Tutorial You find a [detailed tutorial in the original Wiki](https://github.com/ptrbrtz/razor-9dof-ahrs/wiki/Tutorial). -Note: For DEV-16832 (*OpenLog Artemis*), you will need to follow the same instructions as for the `OLA_IMU_Basics.ino` sample from https://github.com/sparkfun/OpenLog_Artemis (i.e. get the drivers from https://learn.sparkfun.com/tutorials/how-to-install-ch340-drivers, install SparkFun Apollo3 boards in Arduino IDE as in https://learn.sparkfun.com/tutorials/installing-board-definitions-in-the-arduino-ide and ensure you select `SparkFun Apollo3` → `SparkFun RedBoard Artemis ATP` as the board and install SparkFun ICM 20948 IMU Arduino library as in https://learn.sparkfun.com/tutorials/installing-an-arduino-library). For SEN-14001 (*9DoF Razor IMU M0*), you will need to follow the same instructions as for the default firmware on https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide and use an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library (an updated version of the default firmware is also available on https://github.com/lebarsfa/9DOF_Razor_IMU). +Note: For SPX-15846 and DEV-16832 (*OpenLog Artemis*), you will need to follow the same instructions as for the `OLA_IMU_Basics.ino` sample from https://github.com/sparkfun/OpenLog_Artemis (i.e. get the drivers from https://learn.sparkfun.com/tutorials/how-to-install-ch340-drivers, install SparkFun Apollo3 boards in Arduino IDE as in https://learn.sparkfun.com/tutorials/installing-board-definitions-in-the-arduino-ide (add https://raw.githubusercontent.com/sparkfun/Arduino_Boards/master/IDE_Board_Manager/package_sparkfun_index.json to `File` → `Preferences` → `Additional Board Manager URLs`) and ensure you select `SparkFun Apollo3` → `SparkFun RedBoard Artemis ATP` as the board and install SparkFun ICM 20948 IMU Arduino library as in https://learn.sparkfun.com/tutorials/installing-an-arduino-library). For SEN-14001 (*9DoF Razor IMU M0*), you will need to follow the same instructions as for the default firmware on https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide and use an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library (an updated version of the default firmware is also available on https://github.com/lebarsfa/9DOF_Razor_IMU). Quick setup ---