From 1f24515c8a969f440b263a1791d576c740ec09b0 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Mon, 11 Dec 2023 19:12:42 -0300 Subject: [PATCH] core: frontend: create compass configuration --- core/frontend/package.json | 1 + .../src/components/common/ParameterSwitch.vue | 52 +++ .../components/common/StatusTextWatcher.vue | 53 +++ .../src/components/vehiclesetup/Configure.vue | 8 +- .../compass/ArdupilotMavlinkCompassSetup.vue | 442 ++++++++++++++++++ .../configuration/compass/CompassDisplay.vue | 303 ++++++++++++ .../configuration/compass/CompassLearn.vue | 227 +++++++++ .../compass/CompassMaskPicker.vue | 65 +++ .../configuration/compass/CompassParams.vue | 119 +++++ .../compass/FullCompassCalibrator.vue | 166 +++++++ .../compass/LargeVehicleCompassCalibrator.vue | 166 +++++++ core/frontend/src/utils/deviceid_decoder.ts | 5 + core/frontend/src/utils/math.ts | 186 ++++++++ core/frontend/src/utils/mavlink_math.ts | 22 + core/start-blueos-core | 6 + 15 files changed, 1819 insertions(+), 2 deletions(-) create mode 100644 core/frontend/src/components/common/ParameterSwitch.vue create mode 100644 core/frontend/src/components/common/StatusTextWatcher.vue create mode 100644 core/frontend/src/components/vehiclesetup/configuration/compass/ArdupilotMavlinkCompassSetup.vue create mode 100644 core/frontend/src/components/vehiclesetup/configuration/compass/CompassDisplay.vue create mode 100644 core/frontend/src/components/vehiclesetup/configuration/compass/CompassLearn.vue create mode 100644 core/frontend/src/components/vehiclesetup/configuration/compass/CompassMaskPicker.vue create mode 100644 core/frontend/src/components/vehiclesetup/configuration/compass/CompassParams.vue create mode 100644 core/frontend/src/components/vehiclesetup/configuration/compass/FullCompassCalibrator.vue create mode 100644 core/frontend/src/components/vehiclesetup/configuration/compass/LargeVehicleCompassCalibrator.vue create mode 100644 core/frontend/src/utils/math.ts create mode 100644 core/frontend/src/utils/mavlink_math.ts 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/common/ParameterSwitch.vue b/core/frontend/src/components/common/ParameterSwitch.vue new file mode 100644 index 0000000000..b08d751cb0 --- /dev/null +++ b/core/frontend/src/components/common/ParameterSwitch.vue @@ -0,0 +1,52 @@ + + diff --git a/core/frontend/src/components/common/StatusTextWatcher.vue b/core/frontend/src/components/common/StatusTextWatcher.vue new file mode 100644 index 0000000000..75348c4459 --- /dev/null +++ b/core/frontend/src/components/common/StatusTextWatcher.vue @@ -0,0 +1,53 @@ + + + diff --git a/core/frontend/src/components/vehiclesetup/Configure.vue b/core/frontend/src/components/vehiclesetup/Configure.vue index a1e2ee0e15..583fbf2c8f 100644 --- a/core/frontend/src/components/vehiclesetup/Configure.vue +++ b/core/frontend/src/components/vehiclesetup/Configure.vue @@ -27,6 +27,8 @@ + 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..743771dc7c --- /dev/null +++ b/core/frontend/src/components/vehiclesetup/configuration/compass/CompassDisplay.vue @@ -0,0 +1,303 @@ + + + + + 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..5c20487579 --- /dev/null +++ b/core/frontend/src/components/vehiclesetup/configuration/compass/CompassLearn.vue @@ -0,0 +1,227 @@ + + + + + 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..66a330cd54 --- /dev/null +++ b/core/frontend/src/components/vehiclesetup/configuration/compass/CompassMaskPicker.vue @@ -0,0 +1,65 @@ + + + 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..85cf455156 --- /dev/null +++ b/core/frontend/src/components/vehiclesetup/configuration/compass/CompassParams.vue @@ -0,0 +1,119 @@ + + + + + diff --git a/core/frontend/src/components/vehiclesetup/configuration/compass/FullCompassCalibrator.vue b/core/frontend/src/components/vehiclesetup/configuration/compass/FullCompassCalibrator.vue new file mode 100644 index 0000000000..6758ee3270 --- /dev/null +++ b/core/frontend/src/components/vehiclesetup/configuration/compass/FullCompassCalibrator.vue @@ -0,0 +1,166 @@ + + + + + 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..84483df400 --- /dev/null +++ b/core/frontend/src/components/vehiclesetup/configuration/compass/LargeVehicleCompassCalibrator.vue @@ -0,0 +1,166 @@ + + + + + diff --git a/core/frontend/src/utils/deviceid_decoder.ts b/core/frontend/src/utils/deviceid_decoder.ts index 5eaa381863..b88220ce70 100755 --- a/core/frontend/src/utils/deviceid_decoder.ts +++ b/core/frontend/src/utils/deviceid_decoder.ts @@ -116,6 +116,7 @@ function toHex(value: number): string { export interface deviceId { param: string paramValue: number + deviceIdNumber: number deviceName?: string busType: string bus: number @@ -128,6 +129,9 @@ export default function decode(device: string, devid: number): deviceId { const bus = devid >> 3 & 0x1F const address = devid >> 8 & 0xFF const devtype = devid >> 16 + // set deviceIdNumber to the last number of the "device" string or 1 if it's not a number + const deviceIdNumber = parseInt(device.slice(-1), 10) || 1 + let decodedDevname = 'UNKNOWN' if (device.startsWith('COMPASS')) { @@ -163,6 +167,7 @@ export default function decode(device: string, devid: number): deviceId { bus, address: toHex(address), devtype, + deviceIdNumber, paramValue: devid, } } 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..a5093e967e --- /dev/null +++ b/core/frontend/src/utils/mavlink_math.ts @@ -0,0 +1,22 @@ +import { Matrix3, Vector3, degrees } from "./math" + +export function mag_heading(RawImu: Vector3, attitude: Vector3, declination: number) { + // calculate heading from raw magnetometer + if (declination === undefined) { + throw new Error('declination is undefined') + } + 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 (heading < -180) { + heading += 360 + } + return heading +} diff --git a/core/start-blueos-core b/core/start-blueos-core index 4917cf57d7..38dc4e4e78 100755 --- a/core/start-blueos-core +++ b/core/start-blueos-core @@ -48,6 +48,12 @@ else echo "Error: blueos static system configuration folder does not exist!" fi +# Define the log file path +LOG_FILE="/var/logs/start-blueos-core/service_restarts.log" + +# Ensure the log directory exists +mkdir -p $(dirname "$LOG_FILE") + # This is a workaround to make the container's `resolv.conf` synchronized with the host's # `resolv.conf`. To do that, we disallow docker from managing the container's DNS nameservers # by unmounting the container's `/etc/resolv.conf`, and soft linking it to the binded host's