/* "compass_cal.pde" (c) 2019 lingib https://www.instructables.com/member/lingib/instructables/ Last update: 28 November 2019 -------- ABOUT -------- This program calculates hard-iron and soft-iron offsets that are required to calibrating the magnetometer inside an MPU-9250 accelerometer|gyro|magnetometer package. Temperature compensation is not applied. -------- NOTES: -------- (1) All data values are maintained at 100% in the array[][] All data points are scaled to fit the 3D display Change the arrayLength in code-line 63 if you need greater accuracy. (2) An arrayLength of 12000 provides sufficient data points for creating a sphere but slows everything down. An arrayLength 0f 6000 provides sufficient time to create six circles each of 1000 data points. (3) Orientate the MPU-9250 sensor with the Y-axis pointing downwards before starting then follow the onscreen instructions. ---------- COPYRIGHT ---------- This is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This software is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License. If not, see . */ /* */ import processing.serial.*; Serial myPort; // create Serial port instance PrintWriter rotateXdown; // create Printwriter instance PrintWriter rotateXup; // create Printwriter instance PrintWriter rotateXall; // create Printwriter instance PrintWriter rotateYdown; // create Printwriter instance PrintWriter rotateYup; // create Printwriter instance PrintWriter rotateYall; // create Printwriter instance PrintWriter rotateZdown; // create Printwriter instance PrintWriter rotateZup; // create Printwriter instance PrintWriter rotateZall; // create Printwriter instance PrintWriter rotateXYZ; // create Printwriter instance //final int arrayLength = 12000; // use this value if you intend to plot a shere final int arrayLength = 6000; // use this value if you intend to plot circles float [][] array = new float [arrayLength+1][3]; // 6000 rows of 3 columns (xPos, yPos, zPos) final int arrayLimit1 = int((float)arrayLength*1/6); final int arrayLimit2 = int((float)arrayLength*2/6); final int arrayLimit3 = int((float)arrayLength*3/6); final int arrayLimit4 = int((float)arrayLength*4/6); final int arrayLimit5 = int((float)arrayLength*5/6); int arraySegment = 0; // which array[][] data segment are we talking about int drawIndex = 0; // used by the draw() loop to scan the array 60 times per second int captureIndex = 0; // used by the serialEvent() routine which captures the data at a slower rate String inputString = ""; //for incoming data float xPos; // scratch pads for raw array[][] data float yPos; float zPos; float xPrev; // scratch pads for validating the incoming data. float yPrev; float zPrev; float xyzLimit = 10; // data range for validation float xMax = -32768.0; // Dummy raw data extremes float yMax = -32768.0; float zMax = -32768.0; float xMin = 32767.0; float yMin = 32767.0; float zMin = 32767.0; float xOffset; // hard-iron offsets float yOffset; float zOffset; float xChord; // scratchPads for calculating soft-iron scale factors float yChord; float zChord; float avgChord; float xScale = 1.0; // soft-iron scale factors float yScale = 1.0; float zScale = 1.0; // ----- flags boolean connected = false; // used for synching data boolean dataValid = true; // used to eliminate false readings boolean offsetsValid = false; // used during data aquisition & 3D display boolean pauseFlag = false; // used during data aquisition boolean scanComplete = false; // used during data aquisition boolean debug = true; // ========================== // setup() // ========================== void setup() { // ----- define drawing area size size(600, 600, P3D); // P3D allows rotation about the Z axis background(0); // initial background color // ----- create 10 data files in the sketch directory rotateXdown = createWriter("rotateXdown.csv"); rotateXup = createWriter("rotateXup.csv"); rotateXall = createWriter("rotateXall.csv"); rotateYdown = createWriter("rotateYdown.csv"); rotateYup = createWriter("rotateYup.csv"); rotateYall = createWriter("rotateYall.csv"); rotateZdown = createWriter("rotateZdown.csv"); rotateZup = createWriter("rotateZup.csv"); rotateZall = createWriter("rotateZall.csv"); rotateXYZ = createWriter("rotateXYZ.csv"); // ----- configure the serial port printArray(Serial.list()); myPort = new Serial(this, Serial.list()[0], 115200); myPort.bufferUntil('\n'); // serialEvent() won't trigger until buffer has "\n" myPort.clear(); } // ========================== // draw() // ========================== void draw() { background(0); for (drawIndex = 10; drawIndex < arrayLength; drawIndex++) { // drawIndex=10 skips any stray 9999 sync values plotData(drawIndex); } } // ======================= // serial event (called with each Arduino data string) // ======================= void serialEvent(Serial myPort) { // ----- wait for a line-feed inputString = myPort.readStringUntil('\n'); // ----- validate input data if (inputString != null) { // ----- establish connection inputString = trim(inputString); // remove leading/trailing whitespace if (connected == false) { if (inputString.equals("S")) { connected = true; // connection established pauseFlag = true; // spacebar must be pressed to continue println(""); println("Place your compass on a flat surface with the RED arrows DOWN"); println("Press the spacebar to continue ..."); } } else { if (pauseFlag == false) { // ----- request data if (connected && !inputString.equals("S")) { inputString = trim(inputString); // remove leading/trailing whitespace // ----- process the data String[] str = split(inputString, ","); // convert inputString into string array xPos = float(str[0]); yPos = float(str[1]); zPos = float(str[2]); // ----- debug if (debug == true) { println(inputString); //print(xPos); //print("\t"); //print(yPos); //print("\t"); //println(zPos); } // ----- validate the data /* Extraneous data points sometime appear. Data points do not tend to change much from one reading to the next. This routine flags any data point that changes by more than 10 counts Three zeros not possible */ if (abs(int(xPos - xPrev))> xyzLimit) dataValid = false; if (abs(int(yPos - yPrev))> xyzLimit) dataValid = false; if (abs(int(zPos - zPrev))> xyzLimit) dataValid = false; if ((int(xPos) == 0) && (int(yPos) == 0) && (int(zPos) == 0)) dataValid = false; xPrev = xPos; yPrev = yPos; zPrev = zPos; // ----- store data set in array[][] if (dataValid == true) { dataValid = false; array[captureIndex][0] = xPos; array[captureIndex][1] = yPos; array[captureIndex][2] = zPos; // ----- point to next storage location captureIndex++; } // ----- do we need to change axis if ((captureIndex == arrayLimit1) && (arraySegment == 0)) { pauseFlag = true; // this flag is cleared in "keyPressed()" arraySegment++; println("Change axis ... RED arrows UP"); println("Press the spacebar to continue ..."); } if ((captureIndex == arrayLimit2) && (arraySegment == 1)) { pauseFlag = true; // this flag is cleared in "keyPressed()" arraySegment++; println("Change axis ... GREEN arrows DOWN"); println("Press the spacebar to continue ..."); } if ((captureIndex == arrayLimit3) && (arraySegment == 2)) { pauseFlag = true; // this flag is cleared in "keyPressed()" arraySegment++; println("Change axis ... GREEN arrows UP"); println("Press the spacebar to continue ..."); } if ((captureIndex == arrayLimit4) && (arraySegment == 3)) { pauseFlag = true; // this flag is cleared in "keyPressed()" arraySegment++; println("Change axis ... BLUE arrows DOWN"); println("Press the spacebar to continue ..."); } if ((captureIndex == arrayLimit5) && (arraySegment == 4)) { pauseFlag = true; // this flag is cleared in "keyPressed()" arraySegment++; println("Change axis ... BLUE arrows UP"); println("Press the spacebar to continue ..."); } if (captureIndex == arrayLength) { scanComplete = true; pauseFlag = true; println("Scan complete"); println(""); calculateOffsets(); // calculate hard-iron offsets and soft-iron scale-factors createCSV(); // create CSV data files in the sketch folder } // ----- send data requests until the array[][] is full if ((pauseFlag == false) && (scanComplete == false)) { // data requests stop when scanComplete flag set dataValid = true; myPort.clear(); //clear the receive buffer myPort.write("S"); } } } } } } // ========================== // keyPressed() // ========================== void keyPressed() { //println(keyCode); // uncomment this line to find your spacebar keycode // ----- look for spacebar if ((pauseFlag == true) && (scanComplete == false)) { if (key == 32) { // spacebar keyCode=32 println("Rotate about the new axis ..."); println(""); delay(1000); pauseFlag = false; myPort.write("S"); // (S)end more data } } // ----- toggle offsets on|off if (scanComplete) { if ((key == 'o')||(key == 'O')) { offsetsValid = !offsetsValid; } } } // ========================== // plot3D(int scanRow) // ========================== void plotData(int scanRow) { // ----- locals float axesSF = 0.7; // 3D axes (S)cale (F)actor //float dataSF = 0.5; // data (S)cale (F)actor float dataSF = 0.3; // data (S)cale (F)actor pushMatrix(); translate(width/2, width/2, -width/2); // set origin to cube center scale(axesSF); // reduce axis sizes if (scanComplete == true) { // ----- rotate box using mouse rotateX(map(mouseX, 0, height, 0, PI)); // change viewing angle rotateY(map(mouseY, 0, height, 0, PI)); // change viewing angle rotateZ(radians(0)); } else { // ----- fixed view while capturing data rotateX(radians(20)); // change viewing angle rotateY(radians(-30)); // change viewing angle rotateZ(radians(-20)); } // ----- draw XYZ axes stroke(255, 0, 0); // X-axis (red) line(-width/2, 0, 0, width/2, 0, 0); stroke(0, 255, 0); // y-axis (green) line(0, -width/2, 0, 0, width/2, 0); stroke(0, 0, 255); // z-axis (blue) line(0, 0, -width/2, 0, 0, width/2); // ----- draw wire frame stroke(64); // line(x1,y1,z1 x2,y2,z3); line(-width/2, 0, -width/2, width/2, 0, -width/2); // XY plane (Z center) line(-width/2, 0, width/2, width/2, 0, width/2); line(-width/2, 0, -width/2, -width/2, 0, width/2); line(width/2, 0, -width/2, width/2, 0, width/2); line(-width/2, -width/2, -width/2, width/2, -width/2, -width/2); // XY plane (-Z) line(-width/2, -width/2, +width/2, width/2, -width/2, width/2); line(-width/2, -width/2, -width/2, -width/2, -width/2, width/2); line(width/2, -width/2, -width/2, width/2, -width/2, width/2); line( 0, -width/2, -width/2, 0, -width/2, width/2); line(-width/2, -width/2, 0, width/2, -width/2, 0); line(-width/2, width/2, width/2, width/2, width/2, width/2); // XY plane (+Z) line(-width/2, width/2, 0, width/2, width/2, 0); line(-width/2, width/2, -width/2, width/2, width/2, -width/2); line(-width/2, width/2, -width/2, -width/2, width/2, width/2); line( 0, width/2, -width/2, 0, width/2, width/2); line(+width/2, width/2, -width/2, width/2, width/2, width/2); line(-width/2, width/2, -width/2, -width/2, -width/2, -width/2); // YZ plane (verticals) line(-width/2, width/2, 0, -width/2, -width/2, 0); line(-width/2, width/2, width/2, -width/2, -width/2, width/2); line(0, width/2, -width/2, 0, -width/2, -width/2); line(0, width/2, width/2, 0, -width/2, width/2); line(width/2, width/2, -width/2, width/2, -width/2, -width/2); line(width/2, width/2, 0, width/2, -width/2, 0); line(width/2, width/2, width/2, width/2, -width/2, width/2); // ----- apply data corrections if (offsetsValid == false) { // ----- Read raw data (no offsets applied) xPos = array[scanRow][0] * dataSF; // dataSF reduces the size of the 3D balls yPos = array[scanRow][1] * dataSF; // dataSF reduces the size of the 3D balls zPos = array[scanRow][2] * dataSF; // dataSF reduces the size of the 3D balls } else { // ----- read raw data and apply the offsets xPos = (array[scanRow][0] - xOffset) * xScale * dataSF; yPos = (array[scanRow][1] - yOffset) * yScale * dataSF; zPos = (array[scanRow][2] - zOffset) * zScale * dataSF; } // ----- display the three 3D-spheres noStroke(); // no wire-frame lights(); // 3D shadows // ----- rotation about X-axis (YZ plane if (scanRow = arrayLimit2) && (scanRow = arrayLimit4) && (scanRow < arrayLength)) { fill(0, 0, 255); // blue dots translate(xPos, yPos, zPos); } sphere(4); // draw the sphere noLights(); popMatrix(); } // ========================== // calculateOffsets() // ========================== void calculateOffsets() { // ----- search array for max|min values for (int i=10; i xMax) xMax = array[i][0]; if (array[i][1] > yMax) yMax = array[i][1]; if (array[i][2] > zMax) zMax = array[i][2]; // ----- find minimums if (array[i][0] < xMin) xMin = array[i][0]; if (array[i][1] < yMin) yMin = array[i][1]; if (array[i][2] < zMin) zMin = array[i][2]; } // ----- calculate the hard-iron offsets xOffset = (xMax + xMin)/2.0; yOffset = (yMax + yMin)/2.0; zOffset = (zMax + zMin)/2.0; // ----- calculate the soft-iron offsets xChord = (xMax - xMin)/2.0; yChord = (yMax - yMin)/2.0; zChord = (zMax - zMin)/2.0; avgChord = (xChord + yChord + zChord)/3.0; xScale = avgChord/xChord; yScale = avgChord/yChord; zScale = avgChord/zChord; // ----- print the results println("----------------"); println(" Max|min values "); println("----------------"); print("xMax: "); print(xMax); print("\t"); print("xMin: "); println(xMin); print("yMax: "); print(yMax); print("\t"); print("yMin: "); println(yMin); print("zMax: "); print(zMax); print("\t"); print("zMin: "); println(zMin); println(""); println("----------"); println(" Offsets"); println("----------"); print("xOffset: "); println(xOffset); print("yOffset: "); println(yOffset); print("zOffset: "); println(zOffset); println(""); println("---------------"); println(" Scale Factors"); println("---------------"); print("xScale: "); println(xScale); print("yScale: "); println(yScale); print("zScale: "); println(zScale); println(""); println("----------------------------------------"); println("Copy&Paste the following code into your "); println("Arduino header then delete the old code."); println("----------------------------------------"); println(""); println("float"); print("Mag_x_offset = "); print(xOffset); println(","); print("Mag_y_offset = "); print(yOffset); println(","); print("Mag_z_offset = "); print(zOffset); println(","); print("Mag_x_scale = "); print(xScale); println(","); print("Mag_y_scale = "); print(yScale); println(","); print("Mag_z_scale = "); print(zScale); println(";"); } // ========================== // createCSV() // ========================== void createCSV() { for (int i=10; i