Auto alignment tool

_locales/en/messages.json

@@ -5591,5 +5591,11 @@
     },
     "ezTuneNote": {
         "message": "<strong>Important</strong> Ez Tune is enabled. All settings on this tab are set and controlled by the Ez Tune. To use PID Tuning tab you have to disable Ez Tune. To do it, uncheck the <strong>Enabled</strong> checkbox on the Ez Tune tab."
+    },
+    "accAlignTitle" : {
+        "message": "Automatic Alignment Wizard"
+    },
+    "accAlignFinished" : {
+        "message": "Board alignment <span style=\"color: #37a8db\">finished</span>"
     }
 }

tabs/magnetometer-ai.js

@@ -0,0 +1,137 @@
+function accComputeYaw(changed, upside) {
+    // Convert the changed pitch and roll into a yaw angle
+    // When nose is lifted, the accelerometer measures which direction was "up"
+    // This tells us the aircraft's yaw orientation
+
+    let pitch_change = changed[0];
+    let roll_change = changed[1];
+
+    // Normalize angles to -180 to 180 range for atan2
+    if (pitch_change > 180) pitch_change -= 360;
+    if (roll_change > 180) roll_change -= 360;
+
+    // Calculate yaw based on which axis changed when nose was lifted
+    let yaw = Math.atan2(-pitch_change, roll_change) * (180 / Math.PI);
+
+    // Adjust for upside down mounting
+    if (upside === 'down') {
+        yaw = (yaw + 180) % 360;
+    }
+
+    // Normalize to 0-360 range
+    yaw = (yaw + 360) % 360;
+
+    // Check for 45° mounting (25.5mm boards)
+    let corner_raised = false;
+    let pitch_change = Math.abs(raw_changed[0]);
+    let roll_change = Math.abs(raw_changed[1]);
+
+    if (pitch_change > 20 && pitch_change < 40 &&
+        roll_change > 20 && roll_change < 40 &&
+        Math.abs(pitch_change - roll_change) < 15) {  
+            corner_raised = true;
+    }
+
+    if (corner_raised) {
+        yaw = Math.round(yaw / 22.5) * 22.5;
+    } else {
+        yaw = Math.round(yaw / 45) * 45;
+    }
+
+    return (yaw + 360) % 360;
+}
+
+function accAutoAlignCompass() {
+    let heading_change = (SENSOR_DATA.kinematics[2] - this.heading_flat + 360) % 360;
+    let yaw_correction_needed = 0;
+    let roll_correction_needed = 0;
+
+    var correction_needed = (450 - FC.SENSOR_DATA.kinematics[2]) % 360;
+
+    heading_change = Math.round(heading_change / 90) * 90;
+    if ( typeof modal != "undefined" ) {
+        modal.close();
+    }
+
+    // If a 90 degree turn caused a 270 degree change, it's upside down.
+    if (heading_change > 180) {
+        console.log("mag upside down");
+        roll_correction_needed = 180;
+    }
+
+    // Determine yaw correction needed
+    if ( (Math.abs(this.heading_flat % 45) < 15) && Math.abs(correction_needed % 45) < 15 ) {
+        yaw_correction_needed = ( Math.round(correction_needed / 45) * 45 + 360 ) % 360;
+    } else {
+        yaw_correction_needed = ( Math.round(correction_needed / 90) * 90 + 360 ) % 360;
+    }
+
+    console.log("heading_flat: " + this.heading_flat + ", change: " + heading_change + ", correction: " + correction_needed % 360);
+
+    // SOLUTION: Apply the inverse of the IMU rotation change to the compass alignment
+
+    // Create the NEW compass alignment (what we want in absolute terms)
+    var newCompassRotation = new THREE.Euler(
+        -THREE.Math.degToRad(self.mag_saved_pitch),
+        THREE.Math.degToRad(-180 - yaw_correction_needed), 
+        THREE.Math.degToRad(roll_correction_needed), 
+        'YXZ'
+    );
+    var matrixCompass = (new THREE.Matrix4()).makeRotationFromEuler(newCompassRotation);
+
+    // Create the CHANGE in IMU rotation (new - old)
+    var oldBoardRotation = new THREE.Euler(
+        THREE.Math.degToRad(self.boardAlignmentConfig.saved_pitch),
+        THREE.Math.degToRad(-self.boardAlignmentConfig.saved_yaw),
+        THREE.Math.degToRad(self.boardAlignmentConfig.saved_roll),
+        'YXZ'
+    );
+    var newBoardRotation = new THREE.Euler(
+        THREE.Math.degToRad(self.boardAlignmentConfig.pitch),
+        THREE.Math.degToRad(-self.boardAlignmentConfig.yaw),
+        THREE.Math.degToRad(self.boardAlignmentConfig.roll),
+        'YXZ'
+    );
+
+    var matrixOldBoard = (new THREE.Matrix4()).makeRotationFromEuler(oldBoardRotation);
+    var matrixNewBoard = (new THREE.Matrix4()).makeRotationFromEuler(newBoardRotation);
+
+    // The delta rotation: how much the IMU frame changed
+    var matrixBoardDelta = (new THREE.Matrix4()).copy(matrixNewBoard).multiply(
+        (new THREE.Matrix4()).copy(matrixOldBoard).invert()
+    );
+
+    // Apply the INVERSE of the board rotation change to the compass
+    // This keeps the compass pointing the same absolute direction
+    matrixCompass.premultiply((new THREE.Matrix4()).copy(matrixBoardDelta).invert());
+
+    // Extract the final compass alignment angles
+    var finalCompassEuler = new THREE.Euler();
+    finalCompassEuler.setFromRotationMatrix(matrixCompass, 'YXZ');
+
+    var finalPitch = Math.round(THREE.Math.radToDeg(-finalCompassEuler.x) + 180);
+    var finalYaw = Math.round(-180 - THREE.Math.radToDeg(finalCompassEuler.y));
+    var finalRoll = Math.round(THREE.Math.radToDeg(finalCompassEuler.z));
+
+    // Normalize to proper ranges
+    finalPitch = ((finalPitch % 360) + 360) % 360;
+    finalYaw = ((finalYaw % 360) + 360) % 360;
+    finalRoll = ((finalRoll % 360) + 360) % 360;
+
+    updatePitchAxis(finalPitch);
+    updateRollAxis(finalRoll);
+    updateYawAxis(finalYaw);
+
+    $("#modal-compass-align-setting").text(
+        finalRoll + ", " + finalPitch + ", " + finalYaw
+    );
+
+    modal = new jBox('Modal', {
+        width: 460,
+        height: 360,
+        animation: false,
+        closeOnClick: true,
+        content: $('#modal-acc-align-done')
+    }).open();
+}
+

tabs/magnetometer.html

@@ -1,5 +1,85 @@
+
+
+
 <div class="tab-magnetometer toolbar_fixed_bottom">
     <div class="content_wrapper">
+
+                                      <div class="default_btn">
+                                        <div id="fc_align_button" style="width: 20%">
+                                            <a id="fc-align-start-button" class="calibrate" href="#">FC Auto Align</a>
+                                        </div>
+                                    </div>
+
+
+
+<div id="modal-acc-align-start" class="is-hidden">
+    <div class="modal__content">
+        <h1 class="modal__title" data-i18n="accAlignTitle"></h1>
+        <div class="modal__text">Place the aircraft flat on the table, facing roughly north (+/- 20°).<br />
+        For best results, try to put it as far from the computer screen as you can.
+        Click OK after the aircraft is flat and pointing north.
+        </div>
+    </div>
+    <div class="modal__buttons">
+        <a id="modal-acc-align-2" class="modal__button modal__button--main" data-i18n="OK"></a>
+    </div>
+</div>
+
+
+<div id="modal-acc-align-45" class="is-hidden">
+    <div class="modal__content">
+        <h1 class="modal__title" data-i18n="accAlignTitle"></h1>
+        <div class="modal__text">Please tilt the aircraft to about 45 degrees nose up. Then click the OK button while the aircraft
+        is pitched nose up.<img src="../images/acc-align-60.png"></div>
+    </div>
+    <div class="modal__buttons">
+        <a id="modal-acc-align-3" class="modal__button modal__button--main" data-i18n="OK"></a>
+    </div>
+</div>
+
+
+<div id="modal-acc-align-east" class="is-hidden">
+    <div class="modal__content">
+        <h1 class="modal__title" data-i18n="accAlignTitle"></h1>
+        <div class="modal__text">Place the aircraft flat and pointing roughly <em>east</em>, then click OK.</div>
+    </div>
+    <div class="modal__buttons">
+        <a id="modal-acc-align-4" class="modal__button modal__button--main" data-i18n="OK"></a>
+    </div>
+</div>
+
+<div id="modal-acc-align-done" class="is-hidden">
+    <div class="modal__content">
+        <h1 class="modal__title" data-i18n="accAlignFinished"></h1>
+        <div class="modal__text">
+            <div data-i18n="accAlignFinished">accAlignFinished></div>
+            Board alignment set to <span id="modal-acc-align-setting"></span><br />
+            Compass alignment set to <span id="modal-compass-align-setting"></span>
+        </div>
+    </div>
+    <div class="modal__buttons">
+        <!-- <a class="save" href="#" data-i18n="configurationButtonSave"></a> -->
+        <a id="modal-acc-align-done-ok" class="modal__button modal__button--main" data-i18n="OK"></a>
+    </div>
+</div>
+
+<div id="modal-acc-align-calibration-error" class="is-hidden">
+    <div class="modal__content">
+        <h1 class="modal__title" data-i18n="accAlignTitle"></h1>
+        <div class="modal__text">
+            <div data-i18n="accAlignCalibrationError">Wrong Planet</div>
+            Accelerometer is not correctly calibrated for the current planet.
+            Please use the calibration tab to calibrate for the local gravity.
+        </div>
+    </div>
+    <div class="modal__buttons">
+        <!-- <a class="save" href="#" data-i18n="configurationButtonSave"></a> -->
+        <a id="modal-acc-align-error-calibration-ok" class="modal__button modal__button--main" data-i18n="OK"></a>
+    </div>
+</div>
+
+
+
         <div class="tab_title" data-i18n="tabMagnetometer">Magnetometer</div>
         <div class="note spacebottom">
             <div class="note_spacer">
@@ -229,4 +309,4 @@
             </td>
         </tr>
     </table>
-</div>
+</div>