diff --git a/core/frontend/package.json b/core/frontend/package.json index 3b32cf4d57..169381b531 100644 --- a/core/frontend/package.json +++ b/core/frontend/package.json @@ -30,6 +30,7 @@ "date-fns": "^2.23.0", "file-saver": "^2.0.5", "fuse.js": "^6.6.2", + "gsap": "^3.12.3", "http-status-codes": "^2.2.0", "image-js": "^0.35.3", "is-ip": "^5.0.0", diff --git a/core/frontend/src/components/vehiclesetup/Configure.vue b/core/frontend/src/components/vehiclesetup/Configure.vue index e83a2f1c2a..1bc3389524 100644 --- a/core/frontend/src/components/vehiclesetup/Configure.vue +++ b/core/frontend/src/components/vehiclesetup/Configure.vue @@ -29,6 +29,8 @@ import Vue from 'vue' import autopilot from '@/store/autopilot_manager' +import SpinningLogo from '../common/SpinningLogo.vue' +import ArdupilotMavlinkCompassSetup from './configuration/compass/ArdupilotMavlinkCompassSetup.vue' import LightsConfigration from './configuration/lights.vue' import ParamSets from './overview/ParamSets.vue' @@ -44,6 +46,8 @@ export default Vue.extend({ components: { ParamSets, LightsConfigration, + ArdupilotMavlinkCompassSetup, + SpinningLogo, }, data() { return { @@ -51,7 +55,7 @@ export default Vue.extend({ pages: [ { title: 'Parameters', component: ParamSets }, { title: 'Accelerometer', component: undefined }, - { title: 'Compass', component: undefined }, + { title: 'Compass', component: ArdupilotMavlinkCompassSetup }, { title: 'Baro', component: undefined }, { title: 'Gripper', component: undefined }, { title: 'Lights', component: LightsConfigration, filter: () => autopilot.vehicle_type === 'Submarine' }, diff --git a/core/frontend/src/components/vehiclesetup/configuration/compass/ArdupilotMavlinkCompassSetup.vue b/core/frontend/src/components/vehiclesetup/configuration/compass/ArdupilotMavlinkCompassSetup.vue new file mode 100644 index 0000000000..dadacc71ce --- /dev/null +++ b/core/frontend/src/components/vehiclesetup/configuration/compass/ArdupilotMavlinkCompassSetup.vue @@ -0,0 +1,417 @@ + + + diff --git a/core/frontend/src/components/vehiclesetup/configuration/compass/CompassDisplay.vue b/core/frontend/src/components/vehiclesetup/configuration/compass/CompassDisplay.vue new file mode 100644 index 0000000000..97d9d72c7c --- /dev/null +++ b/core/frontend/src/components/vehiclesetup/configuration/compass/CompassDisplay.vue @@ -0,0 +1,302 @@ + + + + + diff --git a/core/frontend/src/components/vehiclesetup/configuration/compass/CompassLearn.vue b/core/frontend/src/components/vehiclesetup/configuration/compass/CompassLearn.vue new file mode 100644 index 0000000000..29a1531362 --- /dev/null +++ b/core/frontend/src/components/vehiclesetup/configuration/compass/CompassLearn.vue @@ -0,0 +1,152 @@ + + + + + diff --git a/core/frontend/src/components/vehiclesetup/configuration/compass/CompassMaskPicker.vue b/core/frontend/src/components/vehiclesetup/configuration/compass/CompassMaskPicker.vue new file mode 100644 index 0000000000..7ffa90dc8a --- /dev/null +++ b/core/frontend/src/components/vehiclesetup/configuration/compass/CompassMaskPicker.vue @@ -0,0 +1,67 @@ + + + diff --git a/core/frontend/src/components/vehiclesetup/configuration/compass/CompassParams.vue b/core/frontend/src/components/vehiclesetup/configuration/compass/CompassParams.vue new file mode 100644 index 0000000000..e18a1fc053 --- /dev/null +++ b/core/frontend/src/components/vehiclesetup/configuration/compass/CompassParams.vue @@ -0,0 +1,101 @@ + + + + + diff --git a/core/frontend/src/components/vehiclesetup/configuration/compass/LargeVehicleCompassCalibrator.vue b/core/frontend/src/components/vehiclesetup/configuration/compass/LargeVehicleCompassCalibrator.vue new file mode 100644 index 0000000000..966b82526d --- /dev/null +++ b/core/frontend/src/components/vehiclesetup/configuration/compass/LargeVehicleCompassCalibrator.vue @@ -0,0 +1,130 @@ + + + + + diff --git a/core/frontend/src/utils/math.ts b/core/frontend/src/utils/math.ts new file mode 100644 index 0000000000..fe983ba6f6 --- /dev/null +++ b/core/frontend/src/utils/math.ts @@ -0,0 +1,186 @@ +export function degrees (radians: number): number { + return radians * 180 / Math.PI +} + +export class Vector3 { + x: number; + y: number; + z: number; + + constructor (x: number, y: number, z: number) { + this.x = x; + this.y = y; + this.z = z; + } + + subtract (v: Vector3): Vector3 { + this.x = this.x - v.x; + this.y = this.y - v.y; + this.z = this.z - v.z; + return this; + } + + add (v: Vector3): Vector3 { + this.x = this.x + v.x; + this.y = this.y + v.y; + this.z = this.z + v.z; + return this; + } + + multiply (s: number): Vector3 { + this.x = this.x * s; + this.y = this.y * s; + this.z = this.z * s; + return this; + } + + length (): number { + return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z); + } + + equals (v: Vector3): boolean { + return this.x === v.x && this.y === v.y && this.z === v.z; + } +} + +export class Matrix3 { + private _elements: number[]; + + constructor ( + i11: number | Vector3 = 0, + i12: number | Vector3 = 0, + i13: number | Vector3 = 0, + i21: number = 0, + i22: number = 0, + i23: number = 0, + i31: number = 0, + i32: number = 0, + i33: number = 0 + ) { + // check if we're receiving 3 vector3, if so, use its elements: + if (i11 instanceof Vector3 && + i12 instanceof Vector3 && + i13 instanceof Vector3) { + this._elements = [ + i11.x, i11.y, i11.z, + i12.x, i12.y, i12.z, + i13.x, i13.y, i13.z + ] + return + } + this._elements = [i11 as number, i12 as number, i13 as number, i21, i22, i23, i31, i32, i33] + } + + get a (): Vector3 { + return new Vector3( + this._elements[0], + this._elements[1], + this._elements[2] + ) + } + + get b (): Vector3 { + return new Vector3( + this._elements[3], + this._elements[4], + this._elements[5] + ) + } + + + get c () { + return new Vector3( + this._elements[6], + this._elements[7], + this._elements[8] + ) + } + + fromEuler (roll: number, pitch: number, yaw: number) { + this._elements = [] + const cp = Math.cos(pitch) + const sp = Math.sin(pitch) + const sr = Math.sin(roll) + const cr = Math.cos(roll) + const sy = Math.sin(yaw) + const cy = Math.cos(yaw) + this._elements.push(cp * cy) + this._elements.push((sr * sp * cy) - (cr * sy)) + this._elements.push((cr * sp * cy) + (sr * sy)) + this._elements.push(cp * sy) + this._elements.push((sr * sp * sy) + (cr * cy)) + this._elements.push((cr * sp * sy) - (sr * cy)) + this._elements.push(-sp) + this._elements.push(sr * cp) + this._elements.push(cr * cp) + return this + } + + e (i) { + // validate? + return this._elements[i] + } + + times (vector: Vector3): Vector3 { + return new Vector3( + this._elements[0] * vector.x + this._elements[1] * vector.y + this._elements[2] * vector.z, + this._elements[3] * vector.x + this._elements[4] * vector.y + this._elements[5] * vector.z, + this._elements[6] * vector.x + this._elements[7] * vector.y + this._elements[8] * vector.z + ) + } + + multiply (matrix: Matrix3): Matrix3 { + const m = this._elements + const n = matrix._elements + return new Matrix3( + m[0] * n[0] + m[1] * n[3] + m[2] * n[6], + m[0] * n[1] + m[1] * n[4] + m[2] * n[7], + m[0] * n[2] + m[1] * n[5] + m[2] * n[8], + m[3] * n[0] + m[4] * n[3] + m[5] * n[6], + m[3] * n[1] + m[4] * n[4] + m[5] * n[7], + m[3] * n[2] + m[4] * n[5] + m[5] * n[8], + m[6] * n[0] + m[7] * n[3] + m[8] * n[6], + m[6] * n[1] + m[7] * n[4] + m[8] * n[7], + m[6] * n[2] + m[7] * n[5] + m[8] * n[8] + ) + } + + transposed (): Matrix3 { + return new Matrix3( + this._elements[0], this._elements[3], this._elements[6], + this._elements[1], this._elements[4], this._elements[7], + this._elements[2], this._elements[5], this._elements[8] + ) + } + + determinant (): number { + const m = this._elements + return m[0] * m[4] * m[8] - + m[0] * m[5] * m[7] - + m[1] * m[3] * m[8] + + m[1] * m[5] * m[6] + + m[2] * m[3] * m[7] - + m[2] * m[4] * m[6] + } + + invert (): Matrix3 | null { + const d = this.determinant() + if (d === 0) { + return null + } + const invD = 1 / d + const m = this._elements + return new Matrix3( + invD * (m[8] * m[4] - m[7] * m[5]), + invD * -(m[8] * m[1] - m[7] * m[2]), + invD * (m[5] * m[1] - m[4] * m[2]), + invD * -(m[8] * m[3] - m[6] * m[5]), + invD * (m[8] * m[0] - m[6] * m[2]), + invD * -(m[5] * m[0] - m[3] * m[2]), + invD * (m[7] * m[3] - m[6] * m[4]), + invD * -(m[7] * m[0] - m[6] * m[1]), + invD * (m[4] * m[0] - m[3] * m[1]) + ) + } +} + diff --git a/core/frontend/src/utils/mavlink_math.ts b/core/frontend/src/utils/mavlink_math.ts new file mode 100644 index 0000000000..390eead525 --- /dev/null +++ b/core/frontend/src/utils/mavlink_math.ts @@ -0,0 +1,21 @@ +// ported from https://github.com/ArduPilot/pymavlink/blob/master/mavextra.py#L60 + +import { Matrix3, Vector3, degrees } from "./math" + +export function mag_heading(RawImu: Vector3, attitude: Vector3, declination: number) { + // calculate heading from raw magnetometer + let magX = RawImu.x + let magY = RawImu.y + let magZ = RawImu.z + + // go via a DCM matrix to match the APM calculation + const dcmMatrix = (new Matrix3()).fromEuler(attitude.x, attitude.y, attitude.z) + const cosPitchSqr = 1.0 - (dcmMatrix.e(6) * dcmMatrix.e(6)) + const headY = magY * dcmMatrix.e(8) - magZ * dcmMatrix.e(7) + const headX = magX * cosPitchSqr - dcmMatrix.e(6) * (magY * dcmMatrix.e(7) + magZ * dcmMatrix.e(8)) + let heading = degrees(Math.atan2(-headY, headX)) + declination + if (while < -180) { + heading += 360 + } + return heading +}