diff --git a/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/Alarms.cpp b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/Alarms.cpp new file mode 100644 index 0000000..a335731 --- /dev/null +++ b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/Alarms.cpp @@ -0,0 +1,80 @@ +/* + * Copyright (c) 2020 Gregory Tomasch. All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to + * deal with the Software without restriction, including without limitation the + * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or + * sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimers. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimers in the + * documentation and/or other materials provided with the distribution. + * 3. The names of Gregory Tomasch and his successors + * may not be used to endorse or promote products derived from this Software + * without specific prior written permission. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * WITH THE SOFTWARE. + */ + +#include "Arduino.h" +#include "Alarms.h" + +Alarms::Alarms() +{ +} + +/** +* @fn: blink_blueLED(uint8_t num, uint8_t ontime,uint8_t repeat); +* +* @brief: Flashes red LED indicator; blocking function, do not use in main loop +* +* @params: Blinks per ccle, on time, number of cycles +* @returns: void +*/ +void Alarms::blink_blueLED(uint8_t num, uint8_t ontime,uint8_t repeat) +{ + uint8_t i, r; + for(r=0; r + +// Intermediate data handling variables +float sensor_point[3]; +int16_t gyroADC[2][3]; +int16_t accADC[2][3]; +int16_t magADC[2][3]; +int32_t baroADC[2]; + +// Timing variables +uint32_t Begin; +uint32_t Acq_time; +uint32_t Start_time = 0; +float TimeStamp = 0.0f; +uint32_t currentTime = 0; +uint32_t previousTime = 0; +uint8_t serial_input = 0; +uint32_t last_refresh = 0; +uint32_t delt_t = 0; +uint32_t cycleTime = 0; // Main loop time (us) + +// Interrupt/state flags +volatile uint8_t data_ready[2] = {1, 1}; +volatile uint16_t calibratingG[2] = {0, 0}; + +// Calibration-related variables +full_adv_cal_t gyrocal[2]; +full_adv_cal_t ellipsoid_magcal[2]; +full_adv_cal_t accelcal[2]; +full_adv_cal_t final_magcal[2]; +uint8_t GyroCal_buff[sizeof(full_adv_cal_t)]; +uint8_t EllipMagCal_buff[sizeof(full_adv_cal_t)]; +uint8_t AccelCal_buff[sizeof(full_adv_cal_t)]; +uint8_t FineMagCal_buff[sizeof(full_adv_cal_t)]; +float mag_calData[2][3]; +float dps_per_count = DPS_PER_COUNT; +float g_per_count = G_PER_COUNT; +float UT_per_Count = MMC5983MA_UT_PER_COUNT; +float Mv_Cal = 0.0f; +float Mh_Cal = 0.0f; +float M_Cal = 0.0f; +float Del_Cal = 0.0f; +uint8_t cube_face = 0; +uint8_t face_rotation = 0; + +// USFSMAX-related variables +CoProcessorConfig_t Cfg[2]; +uint8_t algostatus[2]; +uint8_t eventStatus[2]; +int16_t QT_Timestamp[2]; // USFSMAX Quaternion timestamps +uint8_t cfg_buff[sizeof(CoProcessorConfig_t)]; +uint8_t EulerQuatFlag = OUTPUT_EULER_ANGLES; +uint8_t ScaledSensorDataFlag = SCALED_SENSOR_DATA; +uint8_t cal_status[2] = {0, 0}; +uint8_t gyroCalActive[2] = {0, 0}; +uint8_t Quat_flag[2] = {0, 0}; // USFSMAX data ready flags +uint8_t Gyro_flag[2] = {0, 0}; +uint8_t Acc_flag[2] = {0, 0}; +uint8_t Mag_flag[2] = {0, 0}; +uint8_t Baro_flag[2] = {0, 0}; +float Rsq = 0.0f; + +// IMU-related variables +int16_t accLIN[2][3]; // USFSMAX linear acceleration +int16_t grav[2][3]; +float acc_LIN[2][3]; // 9DOF linear acceleration +float Mx[2], My[2]; // Tilt-corrected horizontal magnetic components +float gyroData[2][3] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; +float accData[2][3] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; +float magData[2][3] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; +float qt[2][4] = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f}; // USFSMAX quaternions +float QT[2][4] = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f}; // 9DOF quaternions +float angle[2][2] = {0.0f, 0.0f, 0.0f, 0.0f}; // USFSMAX P,R Euler angles +float ANGLE[2][2] = {0.0f, 0.0f, 0.0f, 0.0f}; // 9DOF P,R Euler angles +float heading[2] = {0.0f, 0.0f}; // USFSMAX Heading Euler angle +float HEADING[2] = {0.0f, 0.0f}; // 9DOF Heading Euler angle +float GyroMeasError = GYRO_MEAS_ERR*0.01745329252f; // Madgewick fliter (RPS) +float GyroMeasDrift = GYRO_MEAS_ERR*0.01745329252f; // Madgewick filter (RPS/s) +float beta = sqrt(3.0f/4.0f)*GyroMeasError; // Madgewick fliter compute beta +float zeta = sqrt(3.0f/4.0f)*GyroMeasDrift; // Madgewick fliter Compute zeta (Small or zero) +float twoKp = 2.0f*KP_DEF; // Mahony filter 2X proportional gain (Kp) +float twoKi = 2.0f*KI_DEF; // Mahony filter 2X integral gain (Ki) +float integralFBx = 0.0f; // Mahony filter integral error terms +float integralFBy = 0.0f; +float integralFBz = 0.0f; + +#endif // Globals_h diff --git a/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/I2Cdev.cpp b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/I2Cdev.cpp new file mode 100644 index 0000000..5ce04ce --- /dev/null +++ b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/I2Cdev.cpp @@ -0,0 +1,158 @@ +/* + * Copyright (c) 2020 Gregory Tomasch. All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to + * deal with the Software without restriction, including without limitation the + * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or + * sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimers. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimers in the + * documentation and/or other materials provided with the distribution. + * 3. The names of Gregory Tomasch and his successors + * may not be used to endorse or promote products derived from this Software + * without specific prior written permission. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * WITH THE SOFTWARE. + */ + +#include "Arduino.h" +#include "I2Cdev.h" + +I2Cdev::I2Cdev(TwoWire* i2c_bus) +{ + _I2C_Bus = i2c_bus; +} + + +/** +* @fn: readByte(uint8_t address, uint8_t subAddress) +* +* @brief: Read one byte from an I2C device +* +* @params: I2C slave device address, Register subAddress +* @returns: unsigned short read +*/ +uint8_t I2Cdev::readByte(uint8_t address, uint8_t subAddress) +{ + uint8_t data; // `data` will store the register data + + _I2C_Bus->beginTransmission(address); // Initialize the Tx buffer + _I2C_Bus->write(subAddress); // Put slave register address in Tx buffer + _I2C_Bus->endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive + _I2C_Bus->requestFrom(address, (size_t) 1); // Read one byte from slave register address + data = _I2C_Bus->read(); // Fill Rx buffer with result + return data; // Return data read from slave register +} + +/** +* @fn: readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) +* +* @brief: Read multiple bytes from an I2C device +* +* @params: I2C slave device address, Register subAddress, number of btes to be read, aray to store the read data +* @returns: void +*/ +void I2Cdev::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) +{ + uint8_t i = 0; + + _I2C_Bus->beginTransmission(address); // Initialize the Tx buffer + _I2C_Bus->write(subAddress); // Put slave register address in Tx buffer + _I2C_Bus->endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive + _I2C_Bus->requestFrom(address, (size_t) count); // Read bytes from slave register address + while (_I2C_Bus->available()) + { + dest[i++] = _I2C_Bus->read(); + } // Put read results in the Rx buffer +} + +/** +* @fn: writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) +* +* @brief: Write one byte to an I2C device +* +* @params: I2C slave device address, Register subAddress, data to be written +* @returns: void +*/ +void I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) +{ + writeBytes(devAddr, regAddr, 1, &data); +} + +/** +* @fn: writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) +* +* @brief: Write multiple bytes to an to an I2C device +* +* @params: I2C slave device address, Register subAddress, data to be written +* @params: number of bytes to be written, data array to be written +* @returns: void +*/ +void I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) +{ + uint8_t status = 0; + + _I2C_Bus->beginTransmission(devAddr); + _I2C_Bus->write((uint8_t) regAddr); + for (uint8_t i=0; i < length; i++) + { + _I2C_Bus->write((uint8_t)data[i]); + } + status = _I2C_Bus->endTransmission(); +} + +/** +* @fn:I2Cscan() +* @brief: Scan the I2C bus for active I2C slave devices +* +* @params: void +* @returns: void +*/ +void I2Cdev::I2Cscan() +{ + // Scan for i2c devices + byte error, address; + int nDevices; + + Serial.println(F("Scanning...")); + + nDevices = 0; + for(address = 1; address < 127; address++ ) + { + // The i2c_scanner uses the return value of the Wire.endTransmisstion to see if a device did acknowledge to the address. + _I2C_Bus->beginTransmission(address); + error = _I2C_Bus->endTransmission(); + + if (error == 0) + { + Serial.print(F("I2C device found at address 0x")); + if (address<16) + Serial.print(F("0")); + Serial.print(address,HEX); + Serial.println(F(" !")); + nDevices++; + } + else if (error==4) + { + Serial.print(F("Unknow error at address 0x")); + if (address<16) + Serial.print(F("0")); + Serial.println(address,HEX); + } + } + if (nDevices == 0) + Serial.println(F("No I2C devices found\n")); + else + Serial.println(F("I2C scan complete\n")); +} diff --git a/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/I2Cdev.h b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/I2Cdev.h new file mode 100644 index 0000000..0a44d1f --- /dev/null +++ b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/I2Cdev.h @@ -0,0 +1,48 @@ +/* + * Copyright (c) 2020 Gregory Tomasch. All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to + * deal with the Software without restriction, including without limitation the + * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or + * sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimers. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimers in the + * documentation and/or other materials provided with the distribution. + * 3. The names of Gregory Tomasch and his successors + * may not be used to endorse or promote products derived from this Software + * without specific prior written permission. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * WITH THE SOFTWARE. + */ + +#ifndef _I2CDEV_H_ +#define _I2CDEV_H_ + +#include "def.h" +#include + +class I2Cdev +{ + public: + I2Cdev(TwoWire*); + uint8_t readByte(uint8_t address, uint8_t subAddress); + void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest); + void writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data); + void writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data); + void I2Cscan(); + private: + TwoWire* _I2C_Bus; +}; + +#endif //_I2CDEV_H_ diff --git a/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/IMU.cpp b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/IMU.cpp new file mode 100644 index 0000000..02ff965 --- /dev/null +++ b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/IMU.cpp @@ -0,0 +1,331 @@ +/* + * Copyright (c) 2020 Gregory Tomasch. All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to + * deal with the Software without restriction, including without limitation the + * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or + * sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimers. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimers in the + * documentation and/or other materials provided with the distribution. + * 3. The names of Gregory Tomasch and his successors + * may not be used to endorse or promote products derived from this Software + * without specific prior written permission. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * WITH THE SOFTWARE. + */ + +#include "Arduino.h" +#include "IMU.h" + +IMU::IMU(USFSMAX* usfsmax, uint8_t sensornum) +{ + _usfsmax = usfsmax; + _sensornum = sensornum; +} + +/** +* @fn: computeIMU() +* +* @brief: Calculates state estimate using USFSMAX-generated data +* +* @params: +* @returns: +*/ +void IMU::computeIMU() +{ + float yaw[2]; + static float buff_roll[2] = {0.0f, 0.0f}, buff_pitch[2] = {0.0f, 0.0f}, buff_heading[2] = {0.0f, 0.0f}; + + Begin = micros(); + if(!EulerQuatFlag) + { + _usfsmax->getQUAT_Lin(); + Acq_time += micros() - Begin; + + // MAXUSFS Quaternion is ENU + buff_heading[_sensornum] = atan2f(2.0f*(qt[_sensornum][1]*qt[_sensornum][2] - qt[_sensornum][0]*qt[_sensornum][3]), qt[_sensornum][0]*qt[_sensornum][0] - + qt[_sensornum][1]*qt[_sensornum][1] + qt[_sensornum][2]*qt[_sensornum][2] - qt[_sensornum][3]*qt[_sensornum][3]); + buff_pitch[_sensornum] = asinf(2.0f*(qt[_sensornum][2]*qt[_sensornum][3] + qt[_sensornum][0]*qt[_sensornum][1])); + buff_roll[_sensornum] = atan2f(2.0f*(qt[_sensornum][0]*qt[_sensornum][2] - qt[_sensornum][1]*qt[_sensornum][3]), qt[_sensornum][0]*qt[_sensornum][0] - + qt[_sensornum][1]*qt[_sensornum][1] - qt[_sensornum][2]*qt[_sensornum][2] + qt[_sensornum][3]*qt[_sensornum][3]); + buff_heading[_sensornum] *= 57.2957795f; + buff_pitch[_sensornum] *= 57.2957795f; + buff_roll[_sensornum] *= 57.2957795f; + angle[_sensornum][1] = buff_roll[_sensornum]; + angle[_sensornum][0] = buff_pitch[_sensornum]; + yaw[_sensornum] = buff_heading[_sensornum]; + heading[_sensornum] = yaw[_sensornum]; // Mag declination added in USFSMAX + if(heading[_sensornum] < 0.0f) heading[_sensornum] += 360.0f; // Convert heading to 0 - 360deg range + } else + { + _usfsmax->getEULER(); + Acq_time += micros() - Begin; + } + TimeStamp = ((float)micros() - (float)Start_time)/1000000.0f; +} + +/** +* @fn: compute_Alternate_IMU() +* +* @brief: Calculates orientation estimate using open-source 9DOF quaternion filters +* +* @params: +* @returns: +*/ + +void IMU::compute_Alternate_IMU() +{ + static float ax[2], ay[2], az[2], gx[2], gy[2], gz[2], mx[2], my[2], mz[2]; + static float deltat[2]; + uint32_t tnow[2]; + static float rps_per_dps = RPS_PER_DPS; + static uint32_t tprev[2] = {Start_time, Start_time}; + static uint32_t intdeltat[2] = {0, 0}; + + // Scale quaternion filter inputs to physical units + ax[_sensornum] = accData[_sensornum][0]; // In g's + ay[_sensornum] = accData[_sensornum][1]; // In g's + az[_sensornum] = accData[_sensornum][2]; // In g's + gx[_sensornum] = gyroData[_sensornum][0]*rps_per_dps; // In rad/s + gy[_sensornum] = gyroData[_sensornum][1]*rps_per_dps; // In rad/s + gz[_sensornum] = -gyroData[_sensornum][2]*rps_per_dps; // In rad/s + mx[_sensornum] = magData[_sensornum][1]*10.0f; // In milligauss + my[_sensornum] = magData[_sensornum][0]*10.0f; // In milligauss + mz[_sensornum] = -magData[_sensornum][2]*10.0f; // In milligauss + + // Calculate deltat for the quaternion update + tnow[_sensornum] = micros(); + intdeltat[_sensornum] = tnow[_sensornum] - tprev[_sensornum]; + deltat[_sensornum] = (float)intdeltat[_sensornum]/1000000.0f; // Result is in seconds + tprev[_sensornum] = tnow[_sensornum]; + #ifdef MADGWICK_9DOF + IMU::MadgwickQuaternionUpdate(ax[_sensornum], ay[_sensornum], az[_sensornum], gx[_sensornum], gy[_sensornum], gz[_sensornum], + mx[_sensornum], my[_sensornum], mz[_sensornum],deltat[_sensornum], QT[_sensornum]); // Alternative quaternion filter + for(uint8_t i=0; i<10; i++) + { + IMU::MadgwickQuaternionUpdate(ax[_sensornum], ay[_sensornum], az[_sensornum], 0.0f, 0.0f, 0.0f, + mx[_sensornum], my[_sensornum], mz[_sensornum],deltat[_sensornum], QT[_sensornum]); // Alternative iteration; angular velocity zeroed-out + } + #endif + #ifdef MAHONY_9DOF + IMU::MahonyQuaternionUpdate(ax[_sensornum], ay[_sensornum], az[_sensornum], gx[_sensornum], gy[_sensornum], gz[_sensornum], + mx[_sensornum], my[_sensornum], mz[_sensornum],deltat[_sensornum], QT[_sensornum]); // Alternative quaternion filter + for(uint8_t i=0; i<10; i++) + { + IMU::MahonyQuaternionUpdate(ax[_sensornum], ay[_sensornum], az[_sensornum], 0.0f, 0.0f, 0.0f, + mx[_sensornum], my[_sensornum], mz[_sensornum],deltat[_sensornum], QT[_sensornum]); // Alternative iteration; angular velocity zeroed-out + } + #endif + HEADING[_sensornum] = -atan2f(QT[_sensornum][0]*QT[_sensornum][0] - QT[_sensornum][1]*QT[_sensornum][1] + QT[_sensornum][2]*QT[_sensornum][2] - + QT[_sensornum][3]*QT[_sensornum][3], 2.0f*(QT[_sensornum][1]*QT[_sensornum][2] - QT[_sensornum][0]*QT[_sensornum][3])); + ANGLE[_sensornum][0] = asinf(2.0f*(QT[_sensornum][2]*QT[_sensornum][3] + QT[_sensornum][0]*QT[_sensornum][1])); + ANGLE[_sensornum][1] = atan2f(2.0f*(QT[_sensornum][0]*QT[_sensornum][2] - QT[_sensornum][1]*QT[_sensornum][3]), QT[_sensornum][0]*QT[_sensornum][0] - + QT[_sensornum][1]*QT[_sensornum][1] - QT[_sensornum][2]*QT[_sensornum][2] + QT[_sensornum][3]*QT[_sensornum][3]); + ANGLE[_sensornum][0] *= 57.2957795f; + ANGLE[_sensornum][1] *= 57.2957795f; + HEADING[_sensornum] *= 57.2957795f; + HEADING[_sensornum] += MAG_DECLINIATION; + if(HEADING[_sensornum] < 0.0f) {HEADING[_sensornum] += 360.0f;} // Ensure yaw stays between 0 and 360 +} + +__attribute__((optimize("O3"))) void IMU::MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, + float mx, float my, float mz, float deltat, float* quat) +{ + float q1 = quat[0], q2 = quat[1], q3 = quat[2], q4 = quat[3]; // Short name local variable for readability + float norm; + float hx, hy, bx, bz; + float vx, vy, vz, wx, wy, wz; + float ex, ey, ez; + float pa, pb, pc; + + // Auxiliary variables to avoid repeated arithmetic + float q1q1 = q1*q1; + float q1q2 = q1*q2; + float q1q3 = q1*q3; + float q1q4 = q1*q4; + float q2q2 = q2*q2; + float q2q3 = q2*q3; + float q2q4 = q2*q4; + float q3q3 = q3*q3; + float q3q4 = q3*q4; + float q4q4 = q4*q4; + + // Normalize accelerometer measurement + norm = sqrtf(ax*ax + ay*ay + az*az); + if(norm == 0.0f) return; // Handle NaN + norm = 1.0f/norm; // Use reciprocal for division + ax *= norm; + ay *= norm; + az *= norm; + + // Normalise magnetometer measurement + norm = sqrtf(mx*mx + my*my + mz*mz); + if(norm == 0.0f) return; // Handle NaN + norm = 1.0f/norm; // Use reciprocal for division + mx *= norm; + my *= norm; + mz *= norm; + + // Reference direction of Earth's magnetic field + hx = 2.0f*mx*(0.5f - q3q3 - q4q4) + 2.0f*my*(q2q3 - q1q4) + 2.0f*mz*(q2q4 + q1q3); + hy = 2.0f*mx*(q2q3 + q1q4) + 2.0f*my*(0.5f - q2q2 - q4q4) + 2.0f*mz*(q3q4 - q1q2); + bx = sqrtf((hx*hx) + (hy*hy)); + bz = 2.0f*mx*(q2q4 - q1q3) + 2.0f*my*(q3q4 + q1q2) + 2.0f*mz*(0.5f - q2q2 - q3q3); + + // Estimated direction of gravity and magnetic field + vx = 2.0f*(q2q4 - q1q3); + vy = 2.0f*(q1q2 + q3q4); + vz = q1q1 - q2q2 - q3q3 + q4q4; + wx = 2.0f*bx*(0.5f - q3q3 - q4q4) + 2.0f*bz*(q2q4 - q1q3); + wy = 2.0f*bx*(q2q3 - q1q4) + 2.0f*bz*(q1q2 + q3q4); + wz = 2.0f*bx*(q1q3 + q2q4) + 2.0f*bz*(0.5f - q2q2 - q3q3); + + // Error is cross product between estimated direction and measured direction of gravity + ex = (ay*vz - az*vy) + (my*wz - mz*wy); + ey = (az*vx - ax*vz) + (mz*wx - mx*wz); + ez = (ax*vy - ay*vx) + (mx*wy - my*wx); + if (twoKi > 0.0f) + { + integralFBx += ex; // Accumulate integral error + integralFBy += ey; + integralFBz += ez; + } else + { + integralFBx = 0.0f; // Prevent integral wind up + integralFBy = 0.0f; + integralFBz = 0.0f; + } + + // Apply feedback terms + gx = gx + twoKp*ex + twoKi*integralFBx; + gy = gy + twoKp*ey + twoKi*integralFBy; + gz = gz + twoKp*ez + twoKi*integralFBz; + + // Integrate rate of change of quaternion + pa = q2; + pb = q3; + pc = q4; + q1 = q1 + (-q2*gx - q3*gy - q4*gz)*(0.5f*deltat); + q2 = pa + (q1*gx + pb*gz - pc*gy)*(0.5f*deltat); + q3 = pb + (q1*gy - pa*gz + pc*gx)*(0.5f*deltat); + q4 = pc + (q1*gz + pa*gy - pb*gx)*(0.5f*deltat); + + // Normalize quaternion + norm = sqrtf(q1*q1 + q2*q2 + q3*q3 + q4*q4); + norm = 1.0f/norm; + quat[0] = q1*norm; + quat[1] = q2*norm; + quat[2] = q3*norm; + quat[3] = q4*norm; +} + +__attribute__((optimize("O3"))) void IMU::MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, + float mx, float my, float mz, float deltat, float* quat) +{ + float q1 = quat[0], q2 = quat[1], q3 = quat[2], q4 = quat[3]; // Short name local variable for readability + float norm; + float hx, hy, _2bx, _2bz; + float s1, s2, s3, s4; + float qDot1, qDot2, qDot3, qDot4; + + // Auxiliary variables to avoid repeated arithmetic + float _2q1mx; + float _2q1my; + float _2q1mz; + float _2q2mx; + float _4bx; + float _4bz; + float _2q1 = 2.0f * q1; + float _2q2 = 2.0f * q2; + float _2q3 = 2.0f * q3; + float _2q4 = 2.0f * q4; + float _2q1q3 = 2.0f * q1 * q3; + float _2q3q4 = 2.0f * q3 * q4; + float q1q1 = q1 * q1; + float q1q2 = q1 * q2; + float q1q3 = q1 * q3; + float q1q4 = q1 * q4; + float q2q2 = q2 * q2; + float q2q3 = q2 * q3; + float q2q4 = q2 * q4; + float q3q3 = q3 * q3; + float q3q4 = q3 * q4; + float q4q4 = q4 * q4; + + // Normalize accelerometer measurement + norm = sqrtf(ax * ax + ay * ay + az * az); + if (norm == 0.0f) return; // Handle NaN + norm = 1.0f/norm; + ax *= norm; + ay *= norm; + az *= norm; + + // Normalize magnetometer measurement + norm = sqrtf(mx * mx + my * my + mz * mz); + if (norm == 0.0f) return; // Handle NaN + norm = 1.0f/norm; + mx *= norm; + my *= norm; + mz *= norm; + + // Reference direction of Earth's magnetic field + _2q1mx = 2.0f * q1 * mx; + _2q1my = 2.0f * q1 * my; + _2q1mz = 2.0f * q1 * mz; + _2q2mx = 2.0f * q2 * mx; + hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; + hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; + _2bx = sqrtf(hx * hx + hy * hy); + _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; + _4bx = 2.0f * _2bx; + _4bz = 2.0f * _2bz; + + // Gradient decent algorithm corrective step + s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + + _2bz * (0.5f - q2q2 - q3q3) - mz); + norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // Normalize step magnitude + norm = 1.0f/norm; + s1 *= norm; + s2 *= norm; + s3 *= norm; + s4 *= norm; + + // Compute rate of change of quaternion + qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1; + qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2; + qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3; + qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4; + + // Integrate to yield quaternion + q1 += qDot1 * deltat; + q2 += qDot2 * deltat; + q3 += qDot3 * deltat; + q4 += qDot4 * deltat; + norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // Normalize quaternion + norm = 1.0f/norm; + quat[0] = q1 * norm; + quat[1] = q2 * norm; + quat[2] = q3 * norm; + quat[3] = q4 * norm; +} diff --git a/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/IMU.h b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/IMU.h new file mode 100644 index 0000000..319a890 --- /dev/null +++ b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/IMU.h @@ -0,0 +1,85 @@ +/* + * Copyright (c) 2020 Gregory Tomasch. All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to + * deal with the Software without restriction, including without limitation the + * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or + * sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimers. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimers in the + * documentation and/or other materials provided with the distribution. + * 3. The names of Gregory Tomasch and his successors + * may not be used to endorse or promote products derived from this Software + * without specific prior written permission. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * WITH THE SOFTWARE. + */ + +#ifndef IMU_h +#define IMU_h + +#include "def.h" +#include "USFSMAX.h" + +#define KP_DEF 0.5f // Mahony filter proportional gain +#define KI_DEF 0.0f // Mahony filter integral gain +#define GYRO_MEAS_ERR 1.0f // Madgewick gyro measurement error. Start at 1 deg/s; original: 2.5deg/s) +#define GYRO_MEAS_DRIFT 0.0f // Madgewick gyro drift. Sstart at 0.0 deg/s/s +#define YAW_GYRO_SF 0.960f // Scales the gyro-based estimate to manage over/undershoot +#define HEADING_KALMAN_R 15.0f // Kalman Sensor variance +#define HEADING_KALMAN_Q 0.0030f // Kalman Process variance; initial baseline = 0.003 + +extern uint8_t EulerQuatFlag; +extern uint32_t Start_time; +extern float TimeStamp; +extern uint32_t Begin; +extern uint32_t Acq_time; +extern float qt[2][4]; +extern float heading[2]; +extern float angle[2][2]; +extern float accData[2][3]; +extern float gyroData[2][3]; +extern float magData[2][3]; +extern int16_t accLIN[2][3]; +extern float QT[2][4]; // 6DOF quaternions +extern float ANGLE[2][2]; // 6DOF P,R Euler angles +extern float HEADING[2]; // 6DOF Heading Euler angle +extern float acc_LIN[2][3]; // 6DOF linear acceleration +extern float Mx[2]; +extern float My[2]; +extern float g_per_count; +extern float beta; +extern float zeta; +extern float twoKp; +extern float twoKi; +extern float integralFBx; +extern float integralFBy; +extern float integralFBz; + +class IMU +{ + public: + IMU(USFSMAX*, uint8_t); + void computeIMU(); + void compute_Alternate_IMU(); + private: + USFSMAX* _usfsmax; + uint8_t _sensornum; + void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, + float mx, float my, float mz, float deltat, float* quat); + void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, + float mx, float my, float mz, float deltat, float* quat); +}; + +#endif // IMU_h diff --git a/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0.ino b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0.ino new file mode 100644 index 0000000..492b5f6 --- /dev/null +++ b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0.ino @@ -0,0 +1,496 @@ +/* + * Copyright (c) 2020 Gregory Tomasch. All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to + * deal with the Software without restriction, including without limitation the + * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or + * sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimers. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimers in the + * documentation and/or other materials provided with the distribution. + * 3. The names of Gregory Tomasch and his successors + * may not be used to endorse or promote products derived from this Software + * without specific prior written permission. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * WITH THE SOFTWARE. + */ + +#include +#include + +#include "Alarms.h" +#include "I2Cdev.h" +#include "USFSMAX.h" +#include "Sensor_cal.h" +#include "IMU.h" +#include "Globals.h" +#include "Types.h" +#include "def.h" + +// Instantiate class objects +I2Cdev i2c_0(&SENSOR_0_WIRE_INSTANCE); +USFSMAX USFSMAX_0(&i2c_0, 0); +IMU imu_0(&USFSMAX_0, 0); +Sensor_cal sensor_cal(&i2c_0, &USFSMAX_0, 0); + +// Declare global scope utility functions +void ProcEventStatus(I2Cdev* i2c_BUS, uint8_t sensorNUM); +void FetchUSFSMAX_Data(USFSMAX* usfsmax, IMU* IMu, uint8_t sensorNUM); +void DRDY_handler_0(); +void SerialInterface_handler(); + +void setup() +{ + // Open serial port + Serial.begin(115200); + delay(4000); + + // Set up DRDY interrupt pin + pinMode(INT_PIN, INPUT); + + // Assign Indicator LED + LEDPIN_PINMODE; + Alarms::blueLEDoff(); + + // Initialize USFSMAX_0 I2C bus + SENSOR_0_WIRE_INSTANCE.begin(); + delay(100); + SENSOR_0_WIRE_INSTANCE.setClock(100000); // Set I2C clock speed to 100kHz cor configuration + delay(2000); + + // Do I2C bus scan if serial debug is active + #ifdef SERIAL_DEBUG // Should see MAX32660 slave bus address (default is 0x57) + i2c_0.I2Cscan(); + #endif + + // Initialize USFSMAX_0 + #ifdef SERIAL_DEBUG + Serial.print(F("Initializing USFSMAX_0...")); + Serial.println(F("")); + #endif + USFSMAX_0.init_USFSMAX(); // Configure USFSMAX and sensors + SENSOR_0_WIRE_INSTANCE.setClock(I2C_CLOCK); // Set the I2C clock to high speed for run-mode data collection + delay(100); + + // Attach interrupts + attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(INT_PIN), DRDY_handler_0, RISING); // Attach DRDY interrupt + + #ifdef SERIAL_DEBUG + Serial.println(F("USFXMAX_0 successfully initialized!")); + Serial.println(F("")); + sensor_cal.sendOneToProceed(); // Halt the serial monitor to let the user read the results + #endif + + // Calculate geomagnetic calibration parameters for your location (set in "config.h") + Mv_Cal = M_V; // Vertical geomagnetic field component + Mh_Cal = M_H; // Horizontal geomagnetic field component + M_Cal = sqrt(Mv_Cal*Mv_Cal + Mh_Cal*Mh_Cal); // Geomagnetic field strength + Del_Cal = atan(Mv_Cal/Mh_Cal); // Geomagnetic inclination or "Dip" angle + + #if !defined(SERIAL_DEBUG) && !defined(MOTION_CAL_GUI_ENABLED) // Print header for spreadsheet data collection + Serial.print(F("Time")); Serial.print(F(",")); Serial.print(F("Heading (deg)")); Serial.print(F(",")); + Serial.print(F("Pitch (deg)")); Serial.print(F(",")); Serial.print(F("Roll (deg)")); Serial.print(F(",")); + Serial.print(F("Cal Status")); Serial.println(F("")); + #endif + + #ifdef SERIAL_DEBUG + Serial.println(F("Do the gyro bias offset calibration. Make sure the USFS is sitting still...")); + Serial.println(F("")); + sensor_cal.sendOneToProceed(); // Wait for user input to proceed + #endif + + calibratingG[0] = 1; + Start_time = micros(); // Set sketch start time +} + +void loop() +{ + // Calculate loop cycle time + currentTime = micros(); + cycleTime = currentTime - previousTime; + previousTime = currentTime; + + // Manage gyro cal + if(calibratingG[0]) + { + calibratingG[0] = 0; + sensor_cal.GyroCal(); + } + if(data_ready[0] == 1) + { + data_ready[0] = 0; + ProcEventStatus(&i2c_0, 0); // I2C instance 0, Sensor instance 0 (and implicitly USFSMAX instance 0) + FetchUSFSMAX_Data(&USFSMAX_0, &imu_0, 0); // USFSMAX instance 0, IMU calculation instance 0 and Sensor instance 0 + } + + // Update serial output + delt_t = millis() - last_refresh; + if (delt_t > UPDATE_PERIOD) // Update the serial monitor every "UPDATE_PERIOD" ms + { + last_refresh = millis(); + #ifdef SERIAL_DEBUG + SerialInterface_handler(); + USFSMAX_0.GetMxMy(); // Get Horizontal magnetic components + if(ENABLE_DHI_CORRECTOR) + { + cal_status[0] = i2c_0.readByte(MAX32660_SLV_ADDR, CALIBRATION_STATUS); // Poll calibration status byte + USFSMAX_0.getDHI_Rsq(); // Get DHI R-square + Serial.print(F("Dynamic Hard Iron Correction Valid = ")); + Serial.println(cal_status[0] & 0x80); // DHI correction status + Serial.print(F("Dynamic Hard Iron Fit R-square = ")); + Serial.println(Rsq, 4); + if(USE_2D_DHI_CORRECTOR) + { + Serial.println(F("Using the 2D Corrector")); + } else + { + Serial.println(F("Using the 3D Corrector")); + } + Serial.println(F("")); + } else + { + Serial.print(F("Dynamic Hard Iron Correction Disabled!")); + Serial.println(F("")); Serial.println(F("")); + } + + // USFSMAX_0 sensor and raw quaternion outout + Serial.print(F("ax = ")); Serial.print((int)(1000.0f*accData[0][0])); Serial.print(F(" ay = ")); Serial.print((int)(1000.0f*accData[0][1])); + Serial.print(F(" az = ")); Serial.print((int)(1000.0f*accData[0][2])); Serial.println(F(" mg")); + Serial.print(F("gx = ")); Serial.print(gyroData[0][0], 1); Serial.print(F(" gy = ")); Serial.print(gyroData[0][1], 1); + Serial.print(F(" gz = ")); Serial.print(gyroData[0][2], 1); Serial.println(F(" deg/s")); + Serial.print(F("mx = ")); Serial.print(magData[0][0], 1); Serial.print(F(" my = ")); Serial.print(magData[0][1], 1); + Serial.print(F(" mz = ")); Serial.print(magData[0][2], 1); Serial.println(F(" uT")); + Serial.print(F("Tomasch Xh, Yh: ")); + Serial.print(Mx[0], 2); Serial.print(F(", ")); Serial.print(My[0], 2); Serial.println(F(" uT")); + Serial.print(F("Baro pressure = ")); Serial.print(((float)baroADC[0])/4096.0f); Serial.println(F(" hPa")); + Serial.println(F("")); + Serial.print(F("USFSMAX Quat: ")); Serial.print(F("q0 = ")); Serial.print(qt[0][0], 4); + Serial.print(F(" qx = ")); Serial.print(qt[0][1], 4); Serial.print(F(" qy = ")); Serial.print(qt[0][2], 4); + Serial.print(F(" qz = ")); Serial.print(qt[0][3], 4); Serial.println(F("")); + + // Euler angles + Serial.print(F("USFSMAX Yaw, Pitch, Roll: ")); + Serial.print(heading[0], 2); Serial.print(F(", ")); Serial.print(angle[0][0], 2); Serial.print(F(", ")); Serial.println(angle[0][1], 2); + + // Critical time deltas + //Serial.println(F("")); Serial.print(F("Loop CT:")); Serial.print(cycleTime); Serial.println(F(" us")); + Serial.print(F("Sensor Acq Time:")); Serial.print(Acq_time); Serial.println(F(" us")); Serial.println(F("")); + #endif + + // Spreadsheet output when "SERIAL_DEBUG" and "MOTION_CAL_GUI_ENABLED" are not defined in config.h + #if !defined(SERIAL_DEBUG) && !defined(MOTION_CAL_GUI_ENABLED) + Serial.print(TimeStamp, 2); Serial.print(F(",")); Serial.print(heading[0], 2); Serial.print(F(",")); + Serial.print(angle[0][0], 2); Serial.print(F(",")); Serial.print(angle[0][1], 2); Serial.print(F(",")); + Serial.print(cal_status[0]); Serial.println(F("")); + #endif + + // Output formatted MotionCal GUI magnetometer data message when "MOTION_CAL_GUI_ENABLED" is defined and "SERIAL_DEBUG" is not defined in config.h + // https://www.pjrc.com/store/prop_shield.html + #if defined(MOTION_CAL_GUI_ENABLED) && !defined(SERIAL_DEBUG) + Serial.print(F("Raw:")); + Serial.print(0); // MotionCal GUI doesn't act upon accel/gyro input; send null data + Serial.print(','); + Serial.print(0); + Serial.print(','); + Serial.print(0); + Serial.print(','); + Serial.print(0); + Serial.print(','); + Serial.print(0); + Serial.print(','); + Serial.print(0); + Serial.print(','); + Serial.print((int16_t)(magData[0][0]*10.0f)); // The MotionCal GUI is expecting 0.1uT/LSB + Serial.print(','); + Serial.print((int16_t)(magData[0][1]*10.0f)); + Serial.print(','); + Serial.print((int16_t)(magData[0][2]*10.0f)); + Serial.println(); + #endif + + // Toggle LED if not calibrating gyroscopes + if(gyroCalActive[0] == 1) + { + Alarms::blueLEDoff(); + if((i2c_0.readByte(MAX32660_SLV_ADDR, CALIBRATION_STATUS) & 0x01) == 0) + { + gyroCalActive[0] = 0; + } + } else + { + Alarms::toggle_blueLED(); + } + } +} + +void ProcEventStatus(I2Cdev* i2c_BUS, uint8_t sensorNUM) +{ + uint8_t temp[1]; + + // Read algorithm status and event status + i2c_BUS->readBytes(MAX32660_SLV_ADDR, COMBO_DRDY_STAT, 1, temp); + eventStatus[sensorNUM] = temp[0]; + + // Decode the event status to determine what data is ready and set the appropriate DRDY fags + if(eventStatus[sensorNUM] & 0x01) Gyro_flag[sensorNUM] = 1; + if(eventStatus[sensorNUM] & 0x02) Acc_flag[sensorNUM] = 1; + if(eventStatus[sensorNUM] & 0x04) Mag_flag[sensorNUM] = 1; + if(eventStatus[sensorNUM] & 0x08) Baro_flag[sensorNUM] = 1; + if(eventStatus[sensorNUM] & 0x10) Quat_flag[sensorNUM] = 1; +} + +void FetchUSFSMAX_Data(USFSMAX* usfsmax, IMU* IMu, uint8_t sensorNUM) +{ + uint8_t call_sensors = eventStatus[sensorNUM] & 0x0F; + + Acq_time = 0; + Begin = micros(); + + // Optimize the I2C read function with respect to whatever sensor data is ready + switch(call_sensors) + { + case 0x01: + usfsmax->GyroAccel_getADC(); + break; + case 0x02: + usfsmax->GyroAccel_getADC(); + break; + case 0x03: + usfsmax->GyroAccel_getADC(); + break; + case 0x07: + usfsmax->GyroAccelMagBaro_getADC(); + break; + case 0x0B: + usfsmax->GyroAccelMagBaro_getADC(); + break; + case 0x0F: + usfsmax->GyroAccelMagBaro_getADC(); + break; + case 0x0C: + usfsmax->MagBaro_getADC(); + break; + case 0x04: + usfsmax->MAG_getADC(); + break; + case 0x08: + usfsmax->BARO_getADC(); + break; + default: + break; + }; + Acq_time += micros() - Begin; + + if(Mag_flag[sensorNUM]) + { + if(ScaledSensorDataFlag) // Calibration data is applied in the coprocessor; just scale + { + for(uint8_t i=0; i<3; i++) + { + magData[sensorNUM][i] = ((float)magADC[sensorNUM][i])*UT_per_Count; + } + } else // Calibration data applied locally + { + sensor_cal.apply_adv_calibration(ellipsoid_magcal[sensorNUM], magADC[sensorNUM], UT_per_Count, mag_calData[sensorNUM]); + sensor_cal.apply_adv_calibration(final_magcal[sensorNUM], mag_calData[sensorNUM], 1.0f, sensor_point); + MAG_ORIENTATION(sensor_point[0], sensor_point[1], sensor_point[2]); + } + Mag_flag[sensorNUM] = 0; + } + if(Acc_flag[sensorNUM]) + { + if(ScaledSensorDataFlag) // Calibration data is applied in the coprocessor; just scale + { + for(uint8_t i=0; i<3; i++) + { + accData[sensorNUM][i] = ((float)accADC[sensorNUM][i])*g_per_count; + } + } else // Calibration data applied locally + { + sensor_cal.apply_adv_calibration(accelcal[sensorNUM], accADC[sensorNUM], g_per_count, sensor_point); + ACC_ORIENTATION(sensor_point[0], sensor_point[1], sensor_point[2]); + } + Acc_flag[sensorNUM] = 0; + } + if(Gyro_flag[sensorNUM] == 1) + { + if(ScaledSensorDataFlag) // Calibration data is applied in the coprocessor; just scale + { + for(uint8_t i=0; i<3; i++) + { + gyroData[sensorNUM][i] = ((float)gyroADC[sensorNUM][i])*dps_per_count; + } + } else // Calibration data applied locally + { + sensor_cal.apply_adv_calibration(gyrocal[sensorNUM], gyroADC[sensorNUM], dps_per_count, sensor_point); + GYRO_ORIENTATION(sensor_point[0], sensor_point[1], sensor_point[2]); + } + + // Call alternative (Madgwick or Mahony) IMU fusion filter + IMu->compute_Alternate_IMU(); + Gyro_flag[sensorNUM] = 0; + } + if(Quat_flag[sensorNUM] == 1) + { + IMu->computeIMU(); + Quat_flag[sensorNUM] = 0; + } +} + +// Host DRDY interrupt handler +void DRDY_handler_0() +{ + data_ready[0] = 1; +} + +// Serial interface handler +void SerialInterface_handler() +{ + serial_input = 0; + if(Serial.available()) serial_input = Serial.read(); + if(serial_input == 49) {calibratingG[0] = 1;} // Type "1" to initiate USFSMAX_0 Gyro Cal + if(serial_input == 50) // Type "2" to list current sensor calibration data + { + SENSOR_0_WIRE_INSTANCE.setClock(100000); // Set I2C clock to 100kHz to read the calibration data from the MAX32660 + delay(100); + USFSMAX_0.Retreive_full_gyrocal(); + delay(100); + USFSMAX_0.Retreive_full_accelcal(); + delay(100); + USFSMAX_0.Retreive_ellip_magcal(); + delay(100); + USFSMAX_0.Retreive_final_magcal(); + delay(100); + SENSOR_0_WIRE_INSTANCE.setClock(I2C_CLOCK); // Resume high-speed I2C operation + delay(100); + + // Print the calibration results + Serial.println(F("Gyroscope Sensor Offsets (dps)")); + Serial.println(gyrocal[0].V[0], 4); + Serial.println(gyrocal[0].V[1], 4); + Serial.println(gyrocal[0].V[2], 4); Serial.println(F("")); + Serial.println(F("Gyroscope Calibration Tensor")); + Serial.print(gyrocal[0].invW[0][0], 4); Serial.print(F(",")); + Serial.print(gyrocal[0].invW[0][1], 4); Serial.print(F(",")); + Serial.println(gyrocal[0].invW[0][2], 4); + Serial.print(gyrocal[0].invW[1][0], 4); Serial.print(F(",")); + Serial.print(gyrocal[0].invW[1][1], 4); Serial.print(F(",")); + Serial.println(gyrocal[0].invW[1][2], 4); + Serial.print(gyrocal[0].invW[2][0], 4); Serial.print(F(",")); + Serial.print(gyrocal[0].invW[2][1], 4); Serial.print(F(",")); + Serial.println(gyrocal[0].invW[2][2], 4); + Serial.println(F("")); Serial.println(F("")); + Serial.println(F("Accelerometer Sensor Offsets (g)")); + Serial.println(accelcal[0].V[0], 4); + Serial.println(accelcal[0].V[1], 4); + Serial.println(accelcal[0].V[2], 4); Serial.println(F("")); + Serial.println(F("Accelerometer Calibration Tensor")); + Serial.print(accelcal[0].invW[0][0], 4); Serial.print(F(",")); + Serial.print(accelcal[0].invW[0][1], 4); Serial.print(F(",")); + Serial.println(accelcal[0].invW[0][2], 4); + Serial.print(accelcal[0].invW[1][0], 4); Serial.print(F(",")); + Serial.print(accelcal[0].invW[1][1], 4); Serial.print(F(",")); + Serial.println(accelcal[0].invW[1][2], 4); + Serial.print(accelcal[0].invW[2][0], 4); Serial.print(F(",")); + Serial.print(accelcal[0].invW[2][1], 4); Serial.print(F(",")); + Serial.println(accelcal[0].invW[2][2], 4); + Serial.println(F("")); Serial.println(F("")); + Serial.println(F("Magnetometer Sensor Offsets (uT)")); + Serial.println(ellipsoid_magcal[0].V[0], 4); + Serial.println(ellipsoid_magcal[0].V[1], 4); + Serial.println(ellipsoid_magcal[0].V[2], 4); + Serial.println(F("")); + Serial.println(F("Magnetometer Soft Iron Correction Tensor")); + Serial.print(ellipsoid_magcal[0].invW[0][0], 4); Serial.print(F(",")); + Serial.print(ellipsoid_magcal[0].invW[0][1], 4); Serial.print(F(",")); + Serial.println(ellipsoid_magcal[0].invW[0][2], 4); + Serial.print(ellipsoid_magcal[0].invW[1][0], 4); Serial.print(F(",")); + Serial.print(ellipsoid_magcal[0].invW[1][1], 4); Serial.print(F(",")); + Serial.println(ellipsoid_magcal[0].invW[1][2], 4); + Serial.print(ellipsoid_magcal[0].invW[2][0], 4); Serial.print(F(",")); + Serial.print(ellipsoid_magcal[0].invW[2][1], 4); Serial.print(F(",")); + Serial.println(ellipsoid_magcal[0].invW[2][2], 4); + Serial.println(F("")); Serial.println(F("")); + Serial.println(F("Magnetometer Residual Hard Iron Offsets (uT)")); + Serial.println(final_magcal[0].V[0], 4); + Serial.println(final_magcal[0].V[1], 4); + Serial.println(final_magcal[0].V[2], 4); + Serial.println(F("")); + Serial.println(F("Magnetometer Fine Calibration/Alignment Tensor")); + Serial.print(final_magcal[0].invW[0][0], 4); Serial.print(F(",")); + Serial.print(final_magcal[0].invW[0][1], 4); Serial.print(F(",")); + Serial.println(final_magcal[0].invW[0][2], 4); + Serial.print(final_magcal[0].invW[1][0], 4); Serial.print(F(",")); + Serial.print(final_magcal[0].invW[1][1], 4); Serial.print(F(",")); + Serial.println(final_magcal[0].invW[1][2], 4); + Serial.print(final_magcal[0].invW[2][0], 4); Serial.print(F(",")); + Serial.print(final_magcal[0].invW[2][1], 4); Serial.print(F(",")); + Serial.println(final_magcal[0].invW[2][2], 4); + Serial.println(F("")); Serial.println(F("")); + sensor_cal.sendOneToProceed(); // Halt the serial monitor to let the user read the calibration data + } + if(serial_input == 51) {USFSMAX_0.Reset_DHI();} // Type "3" to reset the DHI corrector + if(serial_input == 52) // Type "4" to list copro config + { + SENSOR_0_WIRE_INSTANCE.setClock(100000); // Set I2C clock to 100kHz to read the calibration data from the MAX32660 + delay(100); + USFSMAX_0.Retreive_cfg(); // Get the current USFSMAX config + delay(100); + SENSOR_0_WIRE_INSTANCE.setClock(I2C_CLOCK); // Resume high-speed I2C operation + delay(100); + Serial.print(F("Accel scale = ")); Serial.println(Cfg[1].Ascale); + Serial.print(F("Accel ODR = ")); Serial.println(Cfg[1].AODR); + Serial.print(F("Accel LPF = ")); Serial.println(Cfg[1].Alpf); + Serial.print(F("Accel HPF = ")); Serial.println(Cfg[1].Ahpf); + Serial.print(F("Gyro scale = ")); Serial.println(Cfg[1].Gscale); + Serial.print(F("Gyro ODR = ")); Serial.println(Cfg[1].GODR); + Serial.print(F("Gyro LPF = ")); Serial.println(Cfg[1].Glpf); + Serial.print(F("Gyro HPF = ")); Serial.println(Cfg[1].Ghpf); + Serial.print(F("Quat div = ")); Serial.println(Cfg[1].quat_div); + Serial.print(F("Mag scale = ")); Serial.println(Cfg[1].Mscale); + Serial.print(F("Mag ODR = ")); Serial.println(Cfg[1].MODR); + Serial.print(F("Mag LPF = ")); Serial.println(Cfg[1].Mlpf); + Serial.print(F("Mag HPF = ")); Serial.println(Cfg[1].Mhpf); + Serial.print(F("Baro scale = ")); Serial.println(Cfg[1].Pscale); + Serial.print(F("Baro ODR = ")); Serial.println(Cfg[1].PODR); + Serial.print(F("Baro LPF = ")); Serial.println(Cfg[1].Plpf); + Serial.print(F("Baro HPF = ")); Serial.println(Cfg[1].Phpf); + Serial.print(F("AUX_1 scale = ")); Serial.println(Cfg[1].AUX1scale); + Serial.print(F("AUX_1 ODR = ")); Serial.println(Cfg[1].AUX1ODR); + Serial.print(F("AUX_1 LPF = ")); Serial.println(Cfg[1].AUX1lpf); + Serial.print(F("AUX_1 HPF = ")); Serial.println(Cfg[1].AUX1hpf); + Serial.print(F("AUX_2 scale = ")); Serial.println(Cfg[1].AUX2scale); + Serial.print(F("AUX_2 ODR = ")); Serial.println(Cfg[1].AUX2ODR); + Serial.print(F("AUX_2 LPF = ")); Serial.println(Cfg[1].AUX2lpf); + Serial.print(F("AUX_2 HPF = ")); Serial.println(Cfg[1].AUX2hpf); + Serial.print(F("AUX_3 scale = ")); Serial.println(Cfg[1].AUX3scale); + Serial.print(F("AUX_3 ODR = ")); Serial.println(Cfg[1].AUX3ODR); + Serial.print(F("AUX_3 LPF = ")); Serial.println(Cfg[1].AUX3lpf); + Serial.print(F("AUX_3 HPF = ")); Serial.println(Cfg[1].AUX3hpf); + Serial.print(F("Vert FS = ")); Serial.println(Cfg[1].m_v, 5); + Serial.print(F("Horiz FS = ")); Serial.println(Cfg[1].m_h, 5); + Serial.print(F("Declination = ")); Serial.println(Cfg[1].m_dec, 5); + Serial.print(F("Cal points = ")); Serial.println(Cfg[1].cal_points); + Serial.println(F("")); Serial.println(F("")); + sensor_cal.sendOneToProceed(); // Halt the serial monitor to let the user read the calibration data + } + serial_input = 0; + + // Hotkey messaging + Serial.println(F("'1' Gyro Cal")); + Serial.println(F("'2' List Cal Data")); + Serial.println(F("'3' Reset DHI Corrector")); + Serial.println(F("'4' List USFSMAX Config")); + Serial.println(F("")); +} diff --git a/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/Readme.md b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/Readme.md new file mode 100644 index 0000000..a12ce53 --- /dev/null +++ b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/Readme.md @@ -0,0 +1,27 @@ +# USFSMAX MMC Module Uno R3 Test Sketch + +This interconnect information is for operating the [USFSMAX motion coprocessor board](https://cdn.tindiemedia.com/images/resize/S4Os6lUdoJOFuyZHntYHBDiDCUk=/p/full-fit-in/1782x1336/i/44691/products/2020-02-03T20%3A51%3A19.878Z-USFSMAX.top.jpg) with the Tlera [ESP32 development board](https://www.tindie.com/products/onehorse/smallest-esp32-development-board/). The default configuration of the sketch is set up to use the "Wire" instance of the TWI I2C library and pin "27" as the data ready (DRDY) interrupt. The necessary connections between the two boards are: + +## Prototype Breadboard +|USFSMAX Pin|Arduino GPIO Pin| +|:---------:|:--------------:| +| 3V3 | 3V3 | +| GND | GND | +| SCL | 12 | +| SDA | 11 | +| INT | 6 | + +![alt text](https://user-images.githubusercontent.com/5760946/102422599-feba7980-3fbb-11eb-8b88-d18c2cdde258.JPG) + +## Building/Uploading/Running the Test Sketch + +The Sketch is built and uploaded using the [ESP32 core for the Arduino IDE](https://github.com/espressif/arduino-esp32): +* Install the ESP32 core as directed in the repository +* Download the ESP32 USFSMAX Simple Host Utility sketch from this repository and open it with the Arduino IDE +* Go into the "Tools->Board:" menu entry of the IDE and select the "Onehorse ESP32 Dev Module" entry +* Plug the ESP32 development board into an available USB port on your PC +* Build/Upload the sketch as you would with any Arduino board +* Once the upload is complete, power cycle the ESP32/USFSMAX by unplugging and replugging the USB cable +* Open the Arduino serial monitor at 115200 baud. You should see some startup messages and then the calibration data scroll across the serial monitor +* Send a "1" over the serial monitor; you will be prompted to go a gyro cal. Make sure the USFSMAX module is sitting still +* Send a "1" over the serial monitor; you should see AHRS and sensor continuously scrolling across the serial monitor diff --git a/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/Sensor_cal.cpp b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/Sensor_cal.cpp new file mode 100644 index 0000000..5539db2 --- /dev/null +++ b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/Sensor_cal.cpp @@ -0,0 +1,81 @@ +/* + * Copyright (c) 2020 Gregory Tomasch. All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to + * deal with the Software without restriction, including without limitation the + * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or + * sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimers. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimers in the + * documentation and/or other materials provided with the distribution. + * 3. The names of Gregory Tomasch and his successors + * may not be used to endorse or promote products derived from this Software + * without specific prior written permission. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * WITH THE SOFTWARE. + */ + +#include "Arduino.h" +#include "Sensor_cal.h" + +Sensor_cal::Sensor_cal(I2Cdev* i2c, USFSMAX* usfsmax, uint8_t sensornum) +{ + _i2c = i2c; + _usfsmax = usfsmax; + _sensornum = sensornum; +} + +void Sensor_cal::GyroCal() +{ + Alarms::blueLEDoff(); + _i2c->writeByte(MAX32660_SLV_ADDR, CALIBRATION_REQUEST, 0x01); // 0x01 - assert bit 0, start gyro cal + gyroCalActive[_sensornum] = 1; +} + +void Sensor_cal::sendOneToProceed() +{ + uint8_t input = 0; + + Serial.println(F("Send '1' to continue...")); + while(1) + { + input = Serial.read(); + if(input == 49) break; + delay(10); + } +} + +void Sensor_cal::apply_adv_calibration(full_adv_cal_t calibration, int16_t *raw, float sf, float *out) +{ + float x, y, z; + + x = ((float)raw[0] * sf) - calibration.V[0]; + y = ((float)raw[1] * sf) - calibration.V[1]; + z = ((float)raw[2] * sf) - calibration.V[2]; + out[0] = (x * calibration.invW[0][0] + y * calibration.invW[0][1] + z * calibration.invW[0][2]); + out[1] = (x * calibration.invW[1][0] + y * calibration.invW[1][1] + z * calibration.invW[1][2]); + out[2] = (x * calibration.invW[2][0] + y * calibration.invW[2][1] + z * calibration.invW[2][2]); +} + +void Sensor_cal::apply_adv_calibration(full_adv_cal_t calibration, float *raw, float sf, float *out) +{ + float x, y, z; + + x = (raw[0] * sf) - calibration.V[0]; + y = (raw[1] * sf) - calibration.V[1]; + z = (raw[2] * sf) - calibration.V[2]; + out[0] = (x * calibration.invW[0][0] + y * calibration.invW[0][1] + z * calibration.invW[0][2]); + out[1] = (x * calibration.invW[1][0] + y * calibration.invW[1][1] + z * calibration.invW[1][2]); + out[2] = (x * calibration.invW[2][0] + y * calibration.invW[2][1] + z * calibration.invW[2][2]); +} diff --git a/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/Sensor_cal.h b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/Sensor_cal.h new file mode 100644 index 0000000..307d027 --- /dev/null +++ b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/Sensor_cal.h @@ -0,0 +1,64 @@ +/* + * Copyright (c) 2020 Gregory Tomasch. All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to + * deal with the Software without restriction, including without limitation the + * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or + * sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimers. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimers in the + * documentation and/or other materials provided with the distribution. + * 3. The names of Gregory Tomasch and his successors + * may not be used to endorse or promote products derived from this Software + * without specific prior written permission. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * WITH THE SOFTWARE. + */ + +#ifndef Sensor_cal_h +#define Sensor_cal_h + +#include "Alarms.h" +#include "Types.h" +#include "def.h" +#include "config.h" +#include "USFSMAX.h" + +extern uint8_t Acc_flag[2], Mag_flag[2]; +extern full_adv_cal_t gyrocal[2]; +extern full_adv_cal_t accelcal[2]; +extern full_adv_cal_t ellipsoid_magcal[2]; +extern full_adv_cal_t final_magcal[2]; +extern float sensor_point[3]; +extern float gyroData[2][3]; +extern float accData[2][3]; +extern float magData[2][3]; +extern uint8_t gyroCalActive[2]; +extern volatile uint8_t data_ready[2]; + +class Sensor_cal +{ + public: + Sensor_cal(I2Cdev*, USFSMAX*, uint8_t); + void GyroCal(); + void apply_adv_calibration(full_adv_cal_t calibration, int16_t *raw, float sf, float *out); + void apply_adv_calibration(full_adv_cal_t calibration, float *raw, float sf, float *out); + void sendOneToProceed(); + private: + I2Cdev* _i2c; + USFSMAX* _usfsmax; + uint8_t _sensornum; +}; + +#endif // Sensor_cal_h diff --git a/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/Types.h b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/Types.h new file mode 100644 index 0000000..c407e59 --- /dev/null +++ b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/Types.h @@ -0,0 +1,96 @@ +/* + * Copyright (c) 2020 Gregory Tomasch. All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to + * deal with the Software without restriction, including without limitation the + * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or + * sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimers. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimers in the + * documentation and/or other materials provided with the distribution. + * 3. The names of Gregory Tomasch and his successors + * may not be used to endorse or promote products derived from this Software + * without specific prior written permission. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * WITH THE SOFTWARE. + */ + +#ifndef types_h +#define types_h + +/*************************************************************************************************/ +/************* ***************/ +/************* Enumerators and Structure Variables ***************/ +/************* ***************/ +/*************************************************************************************************/ + +enum axes +{ + EAST = 0, + NORTH, + UP +}; + +enum attitudes +{ + PITCH = 0, + ROLL, + YAW +}; + +typedef struct +{ + uint16_t cal_points; + uint8_t Ascale; + uint8_t AODR; + uint8_t Alpf; + uint8_t Ahpf; + uint8_t Gscale; + uint8_t GODR; + uint8_t Glpf; + uint8_t Ghpf; + uint8_t Mscale; + uint8_t MODR; + uint8_t Mlpf; + uint8_t Mhpf; + uint8_t Pscale; + uint8_t PODR; + uint8_t Plpf; + uint8_t Phpf; + uint8_t AUX1scale; + uint8_t AUX1ODR; + uint8_t AUX1lpf; + uint8_t AUX1hpf; + uint8_t AUX2scale; + uint8_t AUX2ODR; + uint8_t AUX2lpf; + uint8_t AUX2hpf; + uint8_t AUX3scale; + uint8_t AUX3ODR; + uint8_t AUX3lpf; + uint8_t AUX3hpf; + float m_v; + float m_h; + float m_dec; + uint8_t quat_div; +} CoProcessorConfig_t; + +typedef struct +{ + float V[3]; // Offset vector components in physical units + float invW[3][3]; // Inverse calibration matrix + uint8_t cal_good; // Byte to verify valid cal is in EEPROM +} full_adv_cal_t; + +#endif // types_h diff --git a/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/USFSMAX.cpp b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/USFSMAX.cpp new file mode 100644 index 0000000..ec9edbc --- /dev/null +++ b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/USFSMAX.cpp @@ -0,0 +1,501 @@ +/* + * Copyright (c) 2020 Gregory Tomasch. All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to + * deal with the Software without restriction, including without limitation the + * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or + * sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimers. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimers in the + * documentation and/or other materials provided with the distribution. + * 3. The names of Gregory Tomasch and his successors + * may not be used to endorse or promote products derived from this Software + * without specific prior written permission. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * WITH THE SOFTWARE. + */ + +#include "Arduino.h" +#include "USFSMAX.h" + +USFSMAX::USFSMAX(I2Cdev* i2c, uint8_t sensornum) +{ + _i2c = i2c; + _sensornum = sensornum; +} + +void USFSMAX::init_USFSMAX() +{ + uint8_t STAT; + uint8_t ConfigByte; + + STAT = _i2c->readByte(MAX32660_SLV_ADDR, FIRMWARE_ID); // Read the coprocessor's firmware ID + #ifdef SERIAL_DEBUG + Serial.print(F("USFSMAX_")); + Serial.print(_sensornum); + Serial.println(F(":")); + Serial.print(F("Firmware ID: 0x")); + Serial.println(STAT, HEX); + Serial.println(F("")); + Serial.print(F("Configuring the coprocessor...")); + Serial.println(F("")); + #endif + + STAT = _i2c->readByte(MAX32660_SLV_ADDR, FUSION_STATUS); // Read the coprocessor's current fusion status + delay(100); + + #ifdef SERIAL_DEBUG + Serial.println(F("")); + Serial.print(F("Fusion status: ")); Serial.println(STAT); + #endif + if(STAT == 0) + { + _i2c->writeByte(MAX32660_SLV_ADDR, FUSION_START_STOP, 0x00); // Stop sensor fusion + delay(100); + + // Upload configuration structure variable + USFSMAX::Upload_cfg(Cfg[_sensornum]); + + // Re-start sensor fusion + ConfigByte = ((0x01 | EulerQuatFlag << 1) | ScaledSensorDataFlag << 2); // Set bit0 to re-start fusion; adjust bit1, bit2 for desired output options + _i2c->writeByte(MAX32660_SLV_ADDR, FUSION_START_STOP, ConfigByte); + delay(100); + + // Poll the FUSION_STATUS register to see if fusion has resumed + while(1) + { + delay(10); + STAT = _i2c->readByte(MAX32660_SLV_ADDR, FUSION_STATUS); + if((STAT & FUSION_RUNNING_MASK)) {break;} + } + #ifdef SERIAL_DEBUG + Serial.println(F("")); + Serial.println(F("USFSMAX sensor fusion running!")); + Serial.println(F("")); + #endif + } + + // Check for sensor errors + STAT = _i2c->readByte(MAX32660_SLV_ADDR, SENS_ERR_STAT); + #ifdef SERIAL_DEBUG + Serial.println(F("")); + Serial.print(F("USFSMAX Sensor Status: ")); Serial.print(STAT); Serial.print(F(" (Should be 0)")); + Serial.println(F("")); Serial.println(F("")); + #endif + if(STAT !=0) + { + #ifdef SERIAL_DEBUG + Serial.print(F("Sensor error!")); + Serial.println(F("")); + #endif + while(1) {;} + } + if(ENABLE_DHI_CORRECTOR) + { + if(USE_2D_DHI_CORRECTOR) + { + _i2c->writeByte(MAX32660_SLV_ADDR, CALIBRATION_REQUEST, 0x50); // Enable DHI corrector, 2D (0x10|0x50) + } else + { + _i2c->writeByte(MAX32660_SLV_ADDR, CALIBRATION_REQUEST, 0x10); // Enable DHI corrector, 3D (0x10) + } + } + Alarms::blink_blueLED(12,100,1); + delay(100); + #ifdef SERIAL_DEBUG + Serial.print(F("Coprocessor configured! Reading sensor calibrations...")); + Serial.println(F("")); + #endif + + USFSMAX::Retreive_full_gyrocal(); + delay(100); + Alarms::blink_blueLED(2,10,1); + USFSMAX::Retreive_full_accelcal(); + delay(100); + Alarms::blink_blueLED(2,10,1); + USFSMAX::Retreive_ellip_magcal(); + delay(100); + Alarms::blink_blueLED(2,10,1); + USFSMAX::Retreive_final_magcal(); + delay(500); + Alarms::blink_blueLED(2,100,4); + #ifdef SERIAL_DEBUG + Serial.println(F(""));Serial.println(F("")); + Serial.println(F("Gyroscope Sensor Offsets (g)")); + Serial.println(gyrocal[_sensornum].V[0], 4); + Serial.println(gyrocal[_sensornum].V[1], 4); + Serial.println(gyrocal[_sensornum].V[2], 4); + Serial.println(F("")); + Serial.println(F("Gyroscope Calibration Tensor")); + Serial.print(gyrocal[_sensornum].invW[0][0], 4); Serial.print(F(",")); + Serial.print(gyrocal[_sensornum].invW[0][1], 4); Serial.print(F(",")); + Serial.println(gyrocal[_sensornum].invW[0][2], 4); + Serial.print(gyrocal[_sensornum].invW[1][0], 4); Serial.print(F(",")); + Serial.print(gyrocal[_sensornum].invW[1][1], 4); Serial.print(F(",")); + Serial.println(gyrocal[_sensornum].invW[1][2], 4); + Serial.print(gyrocal[_sensornum].invW[2][0], 4); Serial.print(F(",")); + Serial.print(gyrocal[_sensornum].invW[2][1], 4); Serial.print(F(",")); + Serial.println(gyrocal[_sensornum].invW[2][2], 4); + Serial.println(F("")); Serial.println(F("")); + Serial.println(F("Accelerometer Sensor Offsets (g)")); + Serial.println(accelcal[_sensornum].V[0], 4); + Serial.println(accelcal[_sensornum].V[1], 4); + Serial.println(accelcal[_sensornum].V[2], 4); + Serial.println(F("")); + Serial.println(F("Accelerometer Calibration Tensor")); + Serial.print(accelcal[_sensornum].invW[0][0], 4); Serial.print(F(",")); + Serial.print(accelcal[_sensornum].invW[0][1], 4); Serial.print(F(",")); + Serial.println(accelcal[_sensornum].invW[0][2], 4); + Serial.print(accelcal[_sensornum].invW[1][0], 4); Serial.print(F(",")); + Serial.print(accelcal[_sensornum].invW[1][1], 4); Serial.print(F(",")); + Serial.println(accelcal[_sensornum].invW[1][2], 4); + Serial.print(accelcal[_sensornum].invW[2][0], 4); Serial.print(F(",")); + Serial.print(accelcal[_sensornum].invW[2][1], 4); Serial.print(F(",")); + Serial.println(accelcal[_sensornum].invW[2][2], 4); + Serial.println(F("")); Serial.println(F("")); + Serial.println(F("Magnetometer Sensor Offsets (uT)")); + Serial.println(ellipsoid_magcal[_sensornum].V[0], 4); + Serial.println(ellipsoid_magcal[_sensornum].V[1], 4); + Serial.println(ellipsoid_magcal[_sensornum].V[2], 4); + Serial.println(F("")); + Serial.println(F("Magnetometer Soft Iron Correction Tensor")); + Serial.print(ellipsoid_magcal[_sensornum].invW[0][0], 4); Serial.print(F(",")); + Serial.print(ellipsoid_magcal[_sensornum].invW[0][1], 4); Serial.print(F(",")); + Serial.println(ellipsoid_magcal[_sensornum].invW[0][2], 4); + Serial.print(ellipsoid_magcal[_sensornum].invW[1][0], 4); Serial.print(F(",")); + Serial.print(ellipsoid_magcal[_sensornum].invW[1][1], 4); Serial.print(F(",")); + Serial.println(ellipsoid_magcal[_sensornum].invW[1][2], 4); + Serial.print(ellipsoid_magcal[_sensornum].invW[2][0], 4); Serial.print(F(",")); + Serial.print(ellipsoid_magcal[_sensornum].invW[2][1], 4); Serial.print(F(",")); + Serial.println(ellipsoid_magcal[_sensornum].invW[2][2], 4); + Serial.println(F("")); Serial.println(F("")); + Serial.println(F("Magnetometer Residual Hard Iron Offsets (uT)")); + Serial.println(final_magcal[_sensornum].V[0], 4); + Serial.println(final_magcal[_sensornum].V[1], 4); + Serial.println(final_magcal[_sensornum].V[2], 4); + Serial.println(F("")); + Serial.println(F("Magnetometer Fine Calibration/Alignment Tensor")); + Serial.print(final_magcal[_sensornum].invW[0][0], 4); Serial.print(F(",")); + Serial.print(final_magcal[_sensornum].invW[0][1], 4); Serial.print(F(",")); + Serial.println(final_magcal[_sensornum].invW[0][2], 4); + Serial.print(final_magcal[_sensornum].invW[1][0], 4); Serial.print(F(",")); + Serial.print(final_magcal[_sensornum].invW[1][1], 4); Serial.print(F(",")); + Serial.println(final_magcal[_sensornum].invW[1][2], 4); + Serial.print(final_magcal[_sensornum].invW[2][0], 4); Serial.print(F(",")); + Serial.print(final_magcal[_sensornum].invW[2][1], 4); Serial.print(F(",")); + Serial.println(final_magcal[_sensornum].invW[2][2], 4); + Serial.println(F("")); Serial.println(F("")); + #endif +} + +void USFSMAX::GoToSleep() +{ + _i2c->writeByte(MAX32660_SLV_ADDR, GO_TO_SLEEP, 0x01); // Put the USFSMAX to sleep... +} + +void USFSMAX::Upload_cfg(CoProcessorConfig_t Config) +{ + uint8_t STAT; + uint8_t CmdByte; + + CmdByte = 0x08; // Clears bit0 to stop fusion an sets bit3 to specify configuration uplaod + _i2c->writeByte(MAX32660_SLV_ADDR, FUSION_START_STOP, CmdByte); + delay(1000); + + // Assign configuration values + Config.cal_points = CAL_POINTS; + Config.Ascale = ACC_SCALE; + Config.AODR = ACC_ODR; + Config.Alpf = LSM6DSM_ACC_DLPF_CFG; + Config.Ahpf = LSM6DSM_ACC_DHPF_CFG; + Config.Gscale = GYRO_SCALE; + Config.GODR = GYRO_ODR; + Config.Glpf = LSM6DSM_GYRO_DLPF_CFG; + Config.Ghpf = LSM6DSM_GYRO_DHPF_CFG; + Config.Mscale = MAG_SCALE; + Config.MODR = MAG_ODR; + Config.Mlpf = MMC5983MA_MAG_LPF; + Config.Mhpf = MMC5983MA_MAG_HPF; + Config.Pscale = BARO_SCALE; + Config.PODR = BARO_ODR; + Config.Plpf = LPS22HB_BARO_LPF; + Config.Phpf = LPS22HB_BARO_HPF; + Config.AUX1scale = AUX1_SCALE; + Config.AUX1ODR = AUX1_ODR; + Config.AUX1lpf = AUX1_LPF; + Config.AUX1hpf = AUX1_HPF; + Config.AUX2scale = AUX2_SCALE; + Config.AUX2ODR = AUX2_ODR; + Config.AUX2lpf = AUX2_LPF; + Config.AUX2hpf = AUX2_HPF; + Config.AUX3scale = AUX3_SCALE; + Config.AUX3ODR = AUX3_ODR; + Config.AUX3lpf = AUX3_LPF; + Config.AUX3hpf = AUX3_HPF; + Config.m_v = M_V; + Config.m_h = M_H; + Config.m_dec = MAG_DECLINIATION; + Config.quat_div = QUAT_DIV; + + // Assign config structure to byte array upload + memcpy(cfg_buff, &Config, sizeof(CoProcessorConfig_t)); + + // Upload configuration bytes + _i2c->writeBytes(MAX32660_SLV_ADDR, COPRO_CFG_DATA0, 30, &cfg_buff[0]); + delay(100); + _i2c->writeBytes(MAX32660_SLV_ADDR, COPRO_CFG_DATA1, (sizeof(CoProcessorConfig_t) - 30), &cfg_buff[30]); + delay(100); +} + +void USFSMAX::GyroAccelMagBaro_getADC() +{ + uint8_t bytes[21]; + + _i2c->readBytes(MAX32660_SLV_ADDR, G_X_L, 21, bytes); + gyroADC[_sensornum][0] = ((int16_t)bytes[1] << 8) | bytes[0]; + gyroADC[_sensornum][1] = ((int16_t)bytes[3] << 8) | bytes[2]; + gyroADC[_sensornum][2] = ((int16_t)bytes[5] << 8) | bytes[4]; + accADC[_sensornum][0] = ((int16_t)bytes[7] << 8) | bytes[6]; + accADC[_sensornum][1] = ((int16_t)bytes[9] << 8) | bytes[8]; + accADC[_sensornum][2] = ((int16_t)bytes[11] << 8) | bytes[10]; + magADC[_sensornum][0] = ((int16_t)bytes[13] << 8) | bytes[12]; + magADC[_sensornum][1] = ((int16_t)bytes[15] << 8) | bytes[14]; + magADC[_sensornum][2] = ((int16_t)bytes[17] << 8) | bytes[16]; + baroADC[_sensornum] = (int32_t)bytes[20] << 16 | (int32_t)bytes[19] << 8 | bytes[18]; +} + +void USFSMAX::GyroAccel_getADC() +{ + uint8_t bytes[12]; + + _i2c->readBytes(MAX32660_SLV_ADDR, G_X_L, 12, bytes); + gyroADC[_sensornum][0] = ((int16_t)bytes[1] << 8) | bytes[0]; + gyroADC[_sensornum][1] = ((int16_t)bytes[3] << 8) | bytes[2]; + gyroADC[_sensornum][2] = ((int16_t)bytes[5] << 8) | bytes[4]; + accADC[_sensornum][0] = ((int16_t)bytes[7] << 8) | bytes[6]; + accADC[_sensornum][1] = ((int16_t)bytes[9] << 8) | bytes[8]; + accADC[_sensornum][2] = ((int16_t)bytes[11] << 8) | bytes[10]; +} + +void USFSMAX::MagBaro_getADC() +{ + uint8_t bytes[9]; + + _i2c->readBytes(MAX32660_SLV_ADDR, M_X_L, 9, bytes); + magADC[_sensornum][0] = ((int16_t)bytes[1] << 8) | bytes[0]; + magADC[_sensornum][1] = ((int16_t)bytes[3] << 8) | bytes[2]; + magADC[_sensornum][2] = ((int16_t)bytes[5] << 8) | bytes[4]; + baroADC[_sensornum] = (int32_t)bytes[8] << 16 | (int32_t)bytes[7] << 8 | bytes[6]; +} + +void USFSMAX::Gyro_getADC() +{ + uint8_t bytes[6]; + + _i2c->readBytes(MAX32660_SLV_ADDR, G_X_L, 6, bytes); + gyroADC[_sensornum][0] = ((int16_t)bytes[1] << 8) | bytes[0]; + gyroADC[_sensornum][1] = ((int16_t)bytes[3] << 8) | bytes[2]; + gyroADC[_sensornum][2] = ((int16_t)bytes[5] << 8) | bytes[4]; +} + +void USFSMAX::ACC_getADC() +{ + uint8_t bytes[6]; + + _i2c->readBytes(MAX32660_SLV_ADDR, A_X_L, 6, bytes); + accADC[_sensornum][0] = ((int16_t)bytes[1] << 8) | bytes[0]; + accADC[_sensornum][1] = ((int16_t)bytes[3] << 8) | bytes[2]; + accADC[_sensornum][2] = ((int16_t)bytes[5] << 8) | bytes[4]; +} + +void USFSMAX::MAG_getADC() +{ + uint8_t bytes[6]; + + _i2c->readBytes(MAX32660_SLV_ADDR, M_X_L, 6, bytes); + magADC[_sensornum][0] = ((int16_t)bytes[1] << 8) | bytes[0]; + magADC[_sensornum][1] = ((int16_t)bytes[3] << 8) | bytes[2]; + magADC[_sensornum][2] = ((int16_t)bytes[5] << 8) | bytes[4]; +} + +void USFSMAX::GetMxMy() +{ + uint8_t bytes[4]; + int16_t H[2]; + + _i2c->readBytes(MAX32660_SLV_ADDR, MX_L, 4, bytes); + + H[0] = ((int16_t)bytes[1] << 8) | bytes[0]; + H[1] = ((int16_t)bytes[3] << 8) | bytes[2]; + + Mx[_sensornum] = ((float)H[0])*UT_per_Count; + My[_sensornum] = ((float)H[1])*UT_per_Count; +} + +void USFSMAX::getQUAT() +{ + uint8_t bytes[16]; + + _i2c->readBytes(MAX32660_SLV_ADDR, Q0_BYTE0, 16, bytes); + qt[_sensornum][0] = uint32_reg_to_float (&bytes[0]); + qt[_sensornum][1] = uint32_reg_to_float (&bytes[4]); + qt[_sensornum][2] = uint32_reg_to_float (&bytes[8]); + qt[_sensornum][3] = uint32_reg_to_float (&bytes[12]); +} + +void USFSMAX::getEULER() +{ + uint8_t bytes[12]; + + _i2c->readBytes(MAX32660_SLV_ADDR, YAW_BYTE0, 12, bytes); + heading[_sensornum] = uint32_reg_to_float (&bytes[0]); + angle[_sensornum][0] = uint32_reg_to_float (&bytes[4]); + angle[_sensornum][1] = uint32_reg_to_float (&bytes[8]); +} + +void USFSMAX::getQUAT_Lin() +{ + uint8_t bytes[28]; + + _i2c->readBytes(MAX32660_SLV_ADDR, Q0_BYTE0, 28, bytes); + qt[_sensornum][0] = uint32_reg_to_float (&bytes[0]); + qt[_sensornum][1] = uint32_reg_to_float (&bytes[4]); + qt[_sensornum][2] = uint32_reg_to_float (&bytes[8]); + qt[_sensornum][3] = uint32_reg_to_float (&bytes[12]); + accLIN[_sensornum][0] = ((int16_t)bytes[17] << 8) | bytes[16]; + accLIN[_sensornum][1] = ((int16_t)bytes[19] << 8) | bytes[18]; + accLIN[_sensornum][2] = ((int16_t)bytes[21] << 8) | bytes[20]; + grav[_sensornum][0] = ((int16_t)bytes[23] << 8) | bytes[22]; + grav[_sensornum][1] = ((int16_t)bytes[25] << 8) | bytes[24]; + grav[_sensornum][2] = ((int16_t)bytes[27] << 8) | bytes[26]; +} + +void USFSMAX::LIN_ACC_getADC() +{ + uint8_t bytes[12]; + + _i2c->readBytes(MAX32660_SLV_ADDR, LIN_X_L, 12, bytes); + accLIN[_sensornum][0] = ((int16_t)bytes[1] << 8) | bytes[0]; + accLIN[_sensornum][1] = ((int16_t)bytes[3] << 8) | bytes[2]; + accLIN[_sensornum][2] = ((int16_t)bytes[5] << 8) | bytes[4]; + grav[_sensornum][0] = ((int16_t)bytes[7] << 8) | bytes[6]; + grav[_sensornum][1] = ((int16_t)bytes[9] << 8) | bytes[8]; + grav[_sensornum][2] = ((int16_t)bytes[11] << 8) | bytes[10]; +} + +void USFSMAX::BARO_getADC() +{ + uint8_t bytes[3]; + + _i2c->readBytes(MAX32660_SLV_ADDR, BARO_XL, 3, bytes); + baroADC[_sensornum] = (int32_t)bytes[2] << 16 | (int32_t)bytes[1] << 8 | bytes[0]; +} + +void USFSMAX::getDHI_Rsq() +{ + uint8_t bytes[2]; + + _i2c->readBytes(MAX32660_SLV_ADDR, DHI_RSQ_L, 2, bytes); + Rsq = ((float)((int16_t)bytes[1] << 8 | bytes[0]))/10000.0f; +} + +void USFSMAX::Reset_DHI() +{ + if(USE_2D_DHI_CORRECTOR) + { + _i2c->writeByte(MAX32660_SLV_ADDR, CALIBRATION_REQUEST, 0x20); // Assert DHI Reset-true + delay(100); + _i2c->writeByte(MAX32660_SLV_ADDR, CALIBRATION_REQUEST, 0x50); //Assert DHI Enable=true, 2D corrector=true (0x40|0x10) + } else + { + _i2c->writeByte(MAX32660_SLV_ADDR, CALIBRATION_REQUEST, 0x20); // Assert DHI Reset-true + delay(100); + _i2c->writeByte(MAX32660_SLV_ADDR, CALIBRATION_REQUEST, 0x10); //Assert DHI Enable=true + } +} + +void USFSMAX::Retreive_cfg() +{ + _i2c->readBytes(MAX32660_SLV_ADDR, COPRO_CFG_DATA0, 30, &cfg_buff[0]); + delay(100); + _i2c->readBytes(MAX32660_SLV_ADDR, COPRO_CFG_DATA1, (sizeof(CoProcessorConfig_t) - 30), &cfg_buff[30]); + memcpy(&Cfg[_sensornum + 1], cfg_buff, sizeof(CoProcessorConfig_t)); +} + +void USFSMAX::Retreive_full_accelcal() +{ + _i2c->readBytes(MAX32660_SLV_ADDR, ACCEL_CAL_DATA0, 30, &AccelCal_buff[0]); + delay(100); + _i2c->readBytes(MAX32660_SLV_ADDR, ACCEL_CAL_DATA1, (sizeof(full_adv_cal_t) - 30), &AccelCal_buff[30]); + memcpy(&accelcal[_sensornum], AccelCal_buff, sizeof(full_adv_cal_t)); +} + +void USFSMAX::Upload_full_accelcal(full_adv_cal_t Cal) +{ + // Future functionality +} + +void USFSMAX::Retreive_ellip_magcal() +{ + _i2c->readBytes(MAX32660_SLV_ADDR, ELLIP_MAG_CAL_DATA0, 30, &EllipMagCal_buff[0]); + delay(100); + _i2c->readBytes(MAX32660_SLV_ADDR, ELLIP_MAG_CAL_DATA1, (sizeof(full_adv_cal_t) - 30), &EllipMagCal_buff[30]); + memcpy(&ellipsoid_magcal[_sensornum], EllipMagCal_buff, sizeof(full_adv_cal_t)); +} + +void USFSMAX::Upload_ellip_magcal(full_adv_cal_t Cal) +{ + // Future functionality +} + +void USFSMAX::Retreive_final_magcal() +{ + _i2c->readBytes(MAX32660_SLV_ADDR, FINE_MAG_CAL_DATA0, 30, &FineMagCal_buff[0]); + delay(100); + _i2c->readBytes(MAX32660_SLV_ADDR, FINE_MAG_CAL_DATA1, (sizeof(full_adv_cal_t) - 30), &FineMagCal_buff[30]); + memcpy(&final_magcal[_sensornum], FineMagCal_buff, sizeof(full_adv_cal_t)); +} + +void USFSMAX::Upload_final_magcal(full_adv_cal_t Cal) +{ + // Future functionality +} + +void USFSMAX::Retreive_full_gyrocal() +{ + _i2c->readBytes(MAX32660_SLV_ADDR, GYRO_CAL_DATA0, 30, &GyroCal_buff[0]); + delay(100); + _i2c->readBytes(MAX32660_SLV_ADDR, GYRO_CAL_DATA1, (sizeof(full_adv_cal_t) - 30), &GyroCal_buff[30]); + memcpy(&gyrocal[_sensornum], GyroCal_buff, sizeof(full_adv_cal_t)); +} + +void USFSMAX::Upload_full_gyrocal(full_adv_cal_t Cal) +{ + // Future functionality +} + +float USFSMAX::uint32_reg_to_float (uint8_t *buf) +{ + union + { + uint32_t ui32; + float f; + } u; + + u.ui32 = (((uint32_t)buf[0]) + + (((uint32_t)buf[1]) << 8) + + (((uint32_t)buf[2]) << 16) + + (((uint32_t)buf[3]) << 24)); + return u.f; +} diff --git a/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/USFSMAX.h b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/USFSMAX.h new file mode 100644 index 0000000..e5dd186 --- /dev/null +++ b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/USFSMAX.h @@ -0,0 +1,217 @@ +/* + * Copyright (c) 2020 Gregory Tomasch. All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to + * deal with the Software without restriction, including without limitation the + * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or + * sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimers. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimers in the + * documentation and/or other materials provided with the distribution. + * 3. The names of Gregory Tomasch and his successors + * may not be used to endorse or promote products derived from this Software + * without specific prior written permission. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * WITH THE SOFTWARE. + */ + +#ifndef USFSMAX_h +#define USFSMAX_h + +#include "I2Cdev.h" +#include "Alarms.h" +#include "def.h" +#include "config.h" +#include "Types.h" + +#define SENS_ERR_STAT 0x00 +#define CALIBRATION_STATUS 0x01 +#define ACCEL_CAL_POS 0x02 +#define FUSION_STATUS 0x03 +#define COMBO_DRDY_STAT 0x04 + +#define G_X_L 0x05 +#define G_X_H 0x06 +#define G_Y_L 0x07 +#define G_Y_H 0x08 +#define G_Z_L 0x09 +#define G_Z_H 0x0A +#define A_X_L 0x0B +#define A_X_H 0x0C +#define A_Y_L 0x0D +#define A_Y_H 0x0E +#define A_Z_L 0x0F +#define A_Z_H 0x10 +#define M_X_L 0x11 +#define M_X_H 0x12 +#define M_Y_L 0x13 +#define M_Y_H 0x14 +#define M_Z_L 0x15 +#define M_Z_H 0x16 +#define BARO_XL 0x17 +#define BARO_L 0x18 +#define BARO_H 0x19 +#define Q0_BYTE0 0x1A +#define Q0_BYTE1 0x1B +#define Q0_BYTE2 0x1C +#define Q0_BYTE3 0x1D +#define Q1_BYTE0 0x1E +#define Q1_BYTE1 0x1F +#define Q1_BYTE2 0x20 +#define Q1_BYTE3 0x21 +#define Q2_BYTE0 0x22 +#define Q2_BYTE1 0x23 +#define Q2_BYTE2 0x24 +#define Q2_BYTE3 0x25 +#define Q3_BYTE0 0x26 +#define Q3_BYTE1 0x27 +#define Q3_BYTE2 0x28 +#define Q3_BYTE3 0x29 +#define LIN_X_L 0x2A +#define LIN_X_H 0x2B +#define LIN_Y_L 0x2C +#define LIN_Y_H 0x2D +#define LIN_Z_L 0x2E +#define LIN_Z_H 0x2F +#define GRAV_X_L 0x30 +#define GRAV_X_H 0x31 +#define GRAV_Y_L 0x32 +#define GRAV_Y_H 0x33 +#define GRAV_Z_L 0x34 +#define GRAV_Z_H 0x35 +#define YAW_BYTE0 0x36 +#define YAW_BYTE1 0x37 +#define YAW_BYTE2 0x38 +#define YAW_BYTE3 0x39 +#define PITCH_BYTE0 0x3A +#define PITCH_BYTE1 0x3B +#define PITCH_BYTE2 0x3C +#define PITCH_BYTE3 0x3D +#define ROLL_BYTE0 0x3E +#define ROLL_BYTE1 0x3F +#define ROLL_BYTE2 0x40 +#define ROLL_BYTE3 0x41 +#define AG_TEMP_L 0x42 +#define AG_TEMP_H 0x43 +#define M_TEMP_L 0x44 +#define M_TEMP_H 0x45 +#define B_TEMP_L 0x46 +#define B_TEMP_H 0x47 +#define AUX_1_X_L 0x48 +#define AUX_1_X_H 0x49 +#define AUX_1_Y_L 0x4A +#define AUX_1_Y_H 0x4B +#define AUX_1_Z_L 0x4C +#define AUX_1_Z_H 0x4D +#define AUX_2_X_L 0x4E +#define AUX_2_X_H 0x4F +#define AUX_2_Y_L 0x50 +#define AUX_2_Y_H 0x51 +#define AUX_2_Z_L 0x52 +#define AUX_2_Z_H 0x53 +#define AUX_3_X_L 0x54 +#define AUX_3_X_H 0x55 +#define AUX_3_Y_L 0x56 +#define AUX_3_Y_H 0x57 +#define AUX_3_Z_L 0x58 +#define AUX_3_Z_H 0x59 +#define MX_L 0x5A +#define MX_H 0x5B +#define MY_L 0x5C +#define MY_H 0x5D +#define DHI_RSQ_L 0x5E +#define DHI_RSQ_H 0x5F + +#define FUSION_START_STOP 0x60 +#define CALIBRATION_REQUEST 0x61 + +#define COPRO_CFG_DATA0 0x62 +#define COPRO_CFG_DATA1 0x63 +#define GYRO_CAL_DATA0 0x64 +#define GYRO_CAL_DATA1 0x65 +#define ACCEL_CAL_DATA0 0x66 +#define ACCEL_CAL_DATA1 0x67 +#define ELLIP_MAG_CAL_DATA0 0x68 +#define ELLIP_MAG_CAL_DATA1 0x69 +#define FINE_MAG_CAL_DATA0 0x6A +#define FINE_MAG_CAL_DATA1 0x6B +#define NEW_I2C_SLAVE_ADDR 0x6C +#define GO_TO_SLEEP 0x6D +#define FIRMWARE_ID 0x7F + +#define FUSION_RUNNING_MASK 0x01 +#define HI_CORRECTOR_MASK 0x10 + +extern CoProcessorConfig_t Cfg[2]; +extern uint8_t cfg_buff[sizeof(CoProcessorConfig_t)]; +extern full_adv_cal_t gyrocal[2]; +extern full_adv_cal_t ellipsoid_magcal[2]; +extern full_adv_cal_t accelcal[2]; +extern full_adv_cal_t final_magcal[2]; +extern uint8_t GyroCal_buff[sizeof(full_adv_cal_t)]; +extern uint8_t EllipMagCal_buff[sizeof(full_adv_cal_t)]; +extern uint8_t AccelCal_buff[sizeof(full_adv_cal_t)]; +extern uint8_t FineMagCal_buff[sizeof(full_adv_cal_t)]; +extern uint8_t EulerQuatFlag; +extern uint8_t ScaledSensorDataFlag; +extern int16_t gyroADC[2][3]; +extern int16_t accADC[2][3]; +extern int16_t magADC[2][3]; +extern float Mx[2], My[2]; +extern float angle[2][2]; +extern float heading[2]; +extern float UT_per_Count; +extern float qt[2][4]; +extern int16_t accLIN[2][3]; +extern int16_t grav[2][3]; +extern int32_t baroADC[2]; +extern float Rsq; + +class USFSMAX +{ + public: + USFSMAX(I2Cdev*, uint8_t); + void init_USFSMAX(); + void GoToSleep(); + void GyroAccelMagBaro_getADC(); + void GyroAccel_getADC(); + void MagBaro_getADC(); + void Gyro_getADC(); + void ACC_getADC(); + void MAG_getADC(); + void GetMxMy(); + void getQUAT(); + void getEULER(); + void LIN_ACC_getADC(); + void getQUAT_Lin(); + void BARO_getADC(); + void Reset_DHI(); + void getDHI_Rsq(); + void Retreive_cfg(); + void Upload_cfg(CoProcessorConfig_t Cfg); + void Retreive_full_accelcal(); + void Upload_full_accelcal(full_adv_cal_t Cal); + void Retreive_ellip_magcal(); + void Upload_ellip_magcal(full_adv_cal_t Cal); + void Retreive_final_magcal(); + void Upload_final_magcal(full_adv_cal_t Cal); + void Retreive_full_gyrocal(); + void Upload_full_gyrocal(full_adv_cal_t Cal); + private: + I2Cdev* _i2c; + uint8_t _sensornum; + float uint32_reg_to_float (uint8_t *buf); +}; + +#endif // USFSMAX_h diff --git a/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/config.h b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/config.h new file mode 100644 index 0000000..9a51063 --- /dev/null +++ b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/config.h @@ -0,0 +1,235 @@ +/* + * Copyright (c) 2020 Gregory Tomasch. All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to + * deal with the Software without restriction, including without limitation the + * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or + * sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimers. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimers in the + * documentation and/or other materials provided with the distribution. + * 3. The names of Gregory Tomasch and his successors + * may not be used to endorse or promote products derived from this Software + * without specific prior written permission. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * WITH THE SOFTWARE. + */ + +#ifndef config_h +#define config_h + +/*************************************************************************************************/ +/************* ***************/ +/************* SECTION 1 - BASIC SETUP ***************/ +/************* ***************/ +/*************************************************************************************************/ + #define MAX32660_SLV_ADDR (0x57) // USFS MAX I2C slave address + + #define INT_PIN 6 // Uno DRDY pin + #define LED_PIN 13 // Uno LED pin + #define WAKE_PIN 14 // ESP32 GPIO pin for USFSMAX wakeup pin + #define RESET_PIN 36 // ESP32 GPIO pin for USFSMAX reset pin + #define SDA_PIN 16 // ESP32 I2C SDA pin + #define SCL_PIN 15 // ESP32 I2C SCL pin + + + // Dynamic Hard Iron corrector (Uncomment one only) + //#define ENABLE_DHI_CORRECTOR 0x01 + #define ENABLE_DHI_CORRECTOR 0x00 // Default DHI to off. Enable when you are ready to properly train the corrector + + // Dynamic Hard Iron Corrector allgorithm (Uncomment one only) + //#define USE_2D_DHI_CORRECTOR 0x01 // Define as "1" to use the 2D HI corrector instead of the 3D corrector + #define USE_2D_DHI_CORRECTOR 0x00 + + #define SERIAL_DEBUG // Uncomment to see the verbose screen update; comment out for spreadsheet or "MotionCal" GUI output + //#define MOTION_CAL_GUI_ENABLED // Uncomment to visualize the magnetometer response surface on the "MotionCal" GUI (https://www.pjrc.com/store/prop_shield.html) + #define UPDATE_PERIOD 100 // Serial update period (ms) + #define CAL_POINTS 2048 // Number or data points collected for gyro and accel/fine mag calibrations + #define AHRS_AVERAGING_POINTS 300 // Number of screen updates to be averaged for AHRS summary data + + #define SENSOR_0_WIRE_INSTANCE Wire // I2C Wire Library instance + + // I2C Clock Speed (Uncomment one only) + //#define I2C_CLOCK 100000 // 100kHz + #define I2C_CLOCK 400000 // 400kHz + //#define I2C_CLOCK 1000000 // 1MHz + +/******************** Alternate Fusion Filter **********************/ + /* Uncomment one only */ + //#define MADGWICK_9DOF + #define MAHONY_9DOF + + +/******************** MAX32660 IMU Board **********************/ + /* Add boards as they become available */ + #define USFS_MAX // Pesky Products USFSMAX + + /* Uncomment one only */ + //#define OUTPUT_EULER_ANGLES 0x01 // Output Euler angles + #define OUTPUT_EULER_ANGLES 0x00 // Output Quaternions + + + #define SCALED_SENSOR_DATA 0x01 // CoPro sensor data is scaled/calibrated/oriented + //#define SCALED_SENSOR_DATA 0x00 // CoPro sensor data is raw/not oriented + +/************************** Sensor Data Rates ****************************/ + /* LSM6DSM Acc Output data rate. Uncomment only one option */ + //#define ACC_ODR 0x0A // 6660Hz + //#define ACC_ODR 0x09 // 3330Hz + //#define ACC_ODR 0x08 // 1660Hz + #define ACC_ODR 0x07 // 834Hz + //#define ACC_ODR 0x06 // 416Hz + //#define ACC_ODR 0x05 // 208Hz + //#define ACC_ODR 0x04 // 104Hz + //#define ACC_ODR 0x03 // 52Hz + //#define ACC_ODR 0x02 // 26Hz + //#define ACC_ODR 0x01 // 12.5Hz + + /* LSM6DSM Gyro Output data rate. Uncomment only one option */ + //#define GYRO_ODR 0x0A // 6660Hz + //#define GYRO_ODR 0x09 // 3330Hz + //#define GYRO_ODR 0x08 // 1660Hz + #define GYRO_ODR 0x07 // 834Hz + //#define GYRO_ODR 0x06 // 416Hz + //#define GYRO_ODR 0x05 // 208Hz + //#define GYRO_ODR 0x04 // 104Hz + //#define GYRO_ODR 0x03 // 52Hz + //#define GYRO_ODR 0x02 // 26Hz + //#define GYRO_ODR 0x01 // 12.5Hz + + /* MMC5983MA Mag Output data rate. Uncomment only one option */ + #define MAG_ODR 0x04 // 50Hz + //#define MAG_ODR 0x03 // 20Hz + //#define MAG_ODR 0x02 // 10Hz + + /* BMP280 Baro Output data rate. Uncomment only one option */ + //#define BARO_ODR 0x05 // 75Hz + #define BARO_ODR 0x04 // 50Hz + //#define BARO_ODR 0x03 // 25Hz + //#define BARO_ODR 0x02 // 10Hz + //#define BARO_ODR 0x01 // 1Hz + + /* AUX1 Output data rate. Uncomment only one option */ + #define AUX1_ODR 0x00 // Future option + + /* AUX2 Output data rate. Uncomment only one option */ + #define AUX2_ODR 0x00 // Future option + + /* AUX2 Output data rate. Uncomment only one option */ + #define AUX3_ODR 0x00 // Future option + + /* Quaternion rate divisor; quaternion rate is the gyro ODR (in Hz) divided by the quaternion rate divisor. Uncomment only one option */ + //#define QUAT_DIV 0x0F // 16 + //#define QUAT_DIV 0x09 // 10 + #define QUAT_DIV 0x07 // 8 + //#define QUAT_DIV 0x05 // 6 + //#define QUAT_DIV 0x04 // 5 + //#define QUAT_DIV 0x03 // 4 + //#define QUAT_DIV 0x02 // 3 + //#define QUAT_DIV 0x01 // 2 + //#define QUAT_DIV 0x00 // 1 + +/************************** Sensor Scales ****************************/ + /* LSM6DSM Acc Output Scale. Uncomment only one option */ + //#define ACC_SCALE_2 // +/-2g + //#define ACC_SCALE_4 // +/-4g + //#define ACC_SCALE_8 // +/-8g + #define ACC_SCALE_16 // +/-16g + + /* LSM6DSM Gyro Output Scale. Uncomment only one option */ + //#define GYRO_SCALE_125 // +/-125DPS + //#define GYRO_SCALE_250 // +/-250DPS + //#define GYRO_SCALE_500 // +/-500DPS + //#define GYRO_SCALE_1000 // +/-1000DPS + #define GYRO_SCALE_2000 // +/-2000DPS + + /* LSM6DSM Acc Output Scale. Uncomment only one option */ + #define MAG_SCALE 0x00 // Not adjustable + + /* LSM6DSM Acc Output Scale. Uncomment only one option */ + #define BARO_SCALE 0x00 // Not adjustable + + /* AUX1 Output Scale. Uncomment only one option */ + #define AUX1_SCALE 0x00 // Future option + + /* AUX2 Output Scale. Uncomment only one option */ + #define AUX2_SCALE 0x00 // Future option + + /* AUX3 Output Scale. Uncomment only one option */ + #define AUX3_SCALE 0x00 // Future option + +/************************** Sensor Filters ****************************/ + /* LSM6DSM Gyro low pass filter setting. Uncomment only one option */ + #define LSM6DSM_GYRO_LPF_167 + //#define LSM6DSM_GYRO_LPF_223 + //#define LSM6DSM_GYRO_LPF_314 + //#define LSM6DSM_GYRO_LPF_655 + + /* LSM6DSM Gyro high pass filter setting. Uncomment only one option */ + #define LSM6DSM_GYRO_DHPF_CFG 0x00 // Future option + + /* LSM6DSM Acc low pass filter setting. Uncomment only one option */ + //#define LSM6DSM_ACC_LPF_ODR_DIV2 + //#define LSM6DSM_ACC_LPF_ODR_DIV4 + #define LSM6DSM_ACC_LPF_ODR_DIV9 + //#define LSM6DSM_ACC_LPF_ODR_DIV50 + //#define LSM6DSM_ACC_LPF_ODR_DIV100 + //#define LSM6DSM_ACC_LPF_ODR_DIV400 + + /* LSM6DSM Acc high pass filter setting. Uncomment only one option */ + #define LSM6DSM_ACC_DHPF_CFG 0x00 // Future option + + /* MMC5983MA Mag low pass filter setting. Uncomment only one option */ + #define MMC5983MA_MAG_LPF 0x00 // Not adjustable + + /* MMC5983MA Mag high pass filter setting. Uncomment only one option */ + #define MMC5983MA_MAG_HPF 0x00 // Future option + + /* LPS22HB Baro low pass filter setting. Uncomment only one option */ + //#define LPS22HB_BARO_LPF 0x00 // ODR/2 + //#define LPS22HB_BARO_LPF 0x08 // ODR/9 + #define LPS22HB_BARO_LPF 0x0C // ODR/20 + + /* LPS22HB Mag high pass filter setting. Uncomment only one option */ + #define LPS22HB_BARO_HPF 0x00 // Future option + + /* Aux_1 low pass filter setting. Uncomment only one option */ + #define AUX1_LPF 0x00 // Future option + + /* Aux_1 high pass filter setting. Uncomment only one option */ + #define AUX1_HPF 0x00 // Future option + + /* Aux_2 low pass filter setting. Uncomment only one option */ + #define AUX2_LPF 0x00 // Future option + + /* Aux_2 high pass filter setting. Uncomment only one option */ + #define AUX2_HPF 0x00 // Future option + + /* Aux_3 low pass filter setting. Uncomment only one option */ + #define AUX3_LPF 0x00 // Future option + + /* Aux_3 high pass filter setting. Uncomment only one option */ + #define AUX3_HPF 0x00 // Future option + +/******************** Magnetic Constants ***********************/ + + //#define KELSEYVILLE_CA_USA + //#define DANVILLE_CA_USA + //#define YUBA_CITY_CA_USA + #define SUNNYVALE_CA_USA + //#define MISSISSAUGA_ON_CA + +/************************** End Configuration ****************************/ + +#endif diff --git a/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/def.h b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/def.h new file mode 100644 index 0000000..c321fa6 --- /dev/null +++ b/MMC_USFS_MAX_Module_Uno_Simple_Host_Utility_v0.0/def.h @@ -0,0 +1,188 @@ +/* + * Copyright (c) 2020 Gregory Tomasch. All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to + * deal with the Software without restriction, including without limitation the + * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or + * sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimers. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimers in the + * documentation and/or other materials provided with the distribution. + * 3. The names of Gregory Tomasch and his successors + * may not be used to endorse or promote products derived from this Software + * without specific prior written permission. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * WITH THE SOFTWARE. + */ + +#ifndef def_h +#define def_h + +#include "config.h" + +/*************************************************************************************************/ +/************* ***************/ +/************* IMU Orientations and Sensor Definitions ***************/ +/************* ***************/ +/*************************************************************************************************/ +#if defined(USFS_MAX) + + // AHRS filters follow ENU convention EAST NORTH UP + #define ACC_ORIENTATION(X, Y, Z) {accData[sensorNUM][EAST] = +X; accData[sensorNUM][NORTH] = +Y; accData[sensorNUM][UP] = +Z;} + + // ENU rotation axes PITCH (EAST) axis gyro (nose-up +) ROLL (NORTH) axis gyro (CW +) YAW (UP) axis gyro (E of N +) + #define GYRO_ORIENTATION(X, Y, Z) {gyroData[sensorNUM][PITCH] = +X; gyroData[sensorNUM][ROLL] = +Y; gyroData[sensorNUM][YAW] = -Z;} + + // Mag follows NED convention FWD RIGHT DOWN // Check component sign with NOAA calculator + #define MAG_ORIENTATION(X, Y, Z) {magData[sensorNUM][0] = +X; magData[sensorNUM][1] = -Y; magData[sensorNUM][2] = +Z;} // Z is multiplied by an additional factor of -1 because this Goddam sensor is left-handed! +#endif + +#if defined(LSM6DSM_GYRO_LPF_167) || defined(LSM6DSM_GYRO_LPF_223) || defined(LSM6DSM_GYRO_LPF_314) \ + || defined(LSM6DSM_GYRO_LPF_655) + + #if defined(LSM6DSM_GYRO_LPF_167) + #define LSM6DSM_GYRO_DLPF_CFG 0x02 + #endif + #if defined(LSM6DSM_GYRO_LPF_223) + #define LSM6DSM_GYRO_DLPF_CFG 0x01 + #endif + #if defined(LSM6DSM_GYRO_LPF_314) + #define LSM6DSM_GYRO_DLPF_CFG 0x00 + #endif + #if defined(LSM6DSM_GYRO_LPF_655) + #define LSM6DSM_GYRO_DLPF_CFG 0x03 + #endif + #else + //Default settings LPF 314Hz + #define LSM6DSM_GYRO_DLPF_CFG 0x00 +#endif + +#if defined(LSM6DSM_ACC_LPF_ODR_DIV2) || defined(LSM6DSM_ACC_LPF_ODR_DIV4) || defined(LSM6DSM_ACC_LPF_ODR_DIV9) \ + || defined(LSM6DSM_ACC_LPF_ODR_DIV50) || defined(LSM6DSM_ACC_LPF_ODR_DIV100) || defined(LSM6DSM_ACC_LPF_ODR_DIV400) + + #if defined(LSM6DSM_ACC_LPF_ODR_DIV2) + #define LSM6DSM_ACC_DLPF_CFG 0x00 + #endif + #if defined(LSM6DSM_ACC_LPF_ODR_DIV4) + #define LSM6DSM_ACC_DLPF_CFG 0x01 + #endif + #if defined(LSM6DSM_ACC_LPF_ODR_DIV9) + #define LSM6DSM_ACC_DLPF_CFG 0x02 + #endif + #if defined(LSM6DSM_ACC_LPF_ODR_DIV50) + #define LSM6DSM_ACC_DLPF_CFG 0x03 + #endif + #if defined(LSM6DSM_ACC_LPF_ODR_DIV100) + #define LSM6DSM_ACC_DLPF_CFG 0x04 + #endif + #if defined(LSM6DSM_ACC_LPF_ODR_DIV400) + #define LSM6DSM_ACC_DLPF_CFG 0x05 + #endif +#else + //Default settings LPF ODR/9 + #define LSM6DSM_ACC_DLPF_CFG 0x02 +#endif + +#if defined(ACC_SCALE_2) || defined(ACC_SCALE_4) || defined(ACC_SCALE_8) || defined(ACC_SCALE_16) + + #if defined(ACC_SCALE_2) + #define ACC_SCALE 0x00 + #define G_PER_COUNT 0.0000610f + #endif + #if defined(ACC_SCALE_4) + #define ACC_SCALE 0x02 + #define G_PER_COUNT 0.0001220f + #endif + #if defined(ACC_SCALE_8) + #define ACC_SCALE 0x03 + #define G_PER_COUNT 0.0002440f + #endif + #if defined(ACC_SCALE_16) + #define ACC_SCALE 0x01 + #define G_PER_COUNT 0.0004880f + #endif +#endif + +#if defined(GYRO_SCALE_125) || defined(GYRO_SCALE_250) || defined(GYRO_SCALE_500) || defined(GYRO_SCALE_1000) \ + || defined(GYRO_SCALE_2000) + + #if defined(GYRO_SCALE_125) + #define GYRO_SCALE 0x02 + #define DPS_PER_COUNT 0.004375f + #endif + #if defined(GYRO_SCALE_250) + #define GYRO_SCALE 0x00 + #define DPS_PER_COUNT 0.00875f + #endif + #if defined(GYRO_SCALE_500) + #define GYRO_SCALE 0x04 + #define DPS_PER_COUNT 0.0175f + #endif + #if defined(GYRO_SCALE_1000) + #define GYRO_SCALE 0x08 + #define DPS_PER_COUNT 0.035f + #endif + #if defined(GYRO_SCALE_2000) + #define GYRO_SCALE 0x0C + #define DPS_PER_COUNT 0.070f + #endif +#endif + +#define MMC5983MA_UT_PER_COUNT 0.006103515625f +#define RPS_PER_DPS 0.01745329 + +/*************************************************************************************************/ +/************* ***************/ +/************* Magnetic Constants ***************/ +/************* ***************/ +/*************************************************************************************************/ +/* Geomagnetic field data: https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml#igrfwmm + Units are uT, angles are in decimal degrees (Not DMS)*/ + +#ifdef KELSEYVILLE_CA_USA + #define M_V 42.9631f + #define M_H 22.7568f + #define MAG_DECLINIATION 13.7433f +#endif +#ifdef DANVILLE_CA_USA + #define M_V 42.2089f + #define M_H 23.0939f + #define MAG_DECLINIATION 13.2197f +#endif +#ifdef YUBA_CITY_CA_USA + #define M_V 43.3972f + #define M_H 22.6066f + #define MAG_DECLINIATION 13.5336f +#endif +#ifdef SUNNYVALE_CA_USA + #define M_V 41.8128f + #define M_H 23.2519f + #define MAG_DECLINIATION 13.2197f +#endif +#ifdef MISSISSAUGA_ON_CA + #define M_V 50.1566f + #define M_H 18.8467f + #define MAG_DECLINIATION -10.1406f +#endif + +/*************************************************************************************************/ +/************* ***************/ +/************* Error Checking Section ***************/ +/************* ***************/ +/*************************************************************************************************/ +#ifndef USFS_MAX + #error "Is this board a USFSMAX?" +#endif + +#endif // def_h