Add first calibration abstraction

core/frontend/src/components/vehiclesetup/Configure.vue

@@ -65,6 +65,7 @@ import settings from '@/store/settings'
 import SpinningLogo from '../common/SpinningLogo.vue'
 import ArdupilotMavlinkCompassSetup from './configuration/compass/ArdupilotMavlinkCompassSetup.vue'
 import LightsConfigration from './configuration/lights.vue'
+import BaroCalib from './overview/BaroCalib.vue'
 import GripperInfo from './overview/gripper.vue'
 import LeakInfo from './overview/LeakInfo.vue'
 import LightsInfo from './overview/LightsInfo.vue'
@@ -99,7 +100,7 @@ export default Vue.extend({
         { title: 'Parameters', component: ParamSets },
         { title: 'Accelerometer', component: undefined },
         { title: 'Compass', component: ArdupilotMavlinkCompassSetup },
-        { title: 'Baro', component: undefined },
+        { title: 'Baro', component: BaroCalib },
         { title: 'Gripper', component: undefined },
         { title: 'Lights', component: LightsConfigration, filter: () => autopilot.vehicle_type === 'Submarine' },
         { title: 'Camera Mount', component: undefined },

core/frontend/src/components/vehiclesetup/calibration.ts

@@ -0,0 +1,149 @@
+import mavlink2rest from '@/libs/MAVLink2Rest'
+import {
+  MavCmd, MAVLinkType, MavResult,
+} from '@/libs/MAVLink2Rest/mavlink2rest-ts/messages/mavlink2rest-enum'
+import autopilot_data from '@/store/autopilot'
+import { sleep } from '@/utils/helper_functions'
+
+enum PreflightCalibration {
+  GYROSCOPE,
+  GYROSCOPE_TEMPERATURE,
+  MAGNETOMETER,
+  PRESSURE,
+  RC,
+  RC_TRIM,
+  ACCELEROMETER,
+  BOARD_LEVEL,
+  ACCELEROMETER_TEMPERATURE,
+  SIMPLE_ACCELEROMETER,
+  COMPASS_MOTOR_INTERFERENCE,
+  AIRPSEED,
+  ESC,
+  BAROMETER_TEMPERATURE,
+}
+
+class Calibrator {
+  private static instance: Calibrator
+
+  private calibrating: PreflightCalibration | undefined
+
+  private calibrationStatus: MavResult | undefined
+
+  constructor() {
+    mavlink2rest.startListening(MAVLinkType.COMMAND_ACK).setCallback((content) => {
+      const { header, message } = content
+      if (header.system_id !== autopilot_data.system_id || header.component_id !== 1) {
+        return
+      }
+      if (message.command.type === MavCmd.MAV_CMD_PREFLIGHT_CALIBRATION) {
+        this.calibrationStatus = message.result.type
+      }
+    }).setFrequency(0)
+  }
+
+  /**
+   * Singleton access
+   * @returns Calibrator
+   */
+  public static getInstance(): Calibrator {
+    if (!Calibrator.instance) {
+      Calibrator.instance = new Calibrator()
+    }
+    return Calibrator.instance
+  }
+
+  /**
+   * Start calibration process
+   * @param {PreflightCalibration} type
+   */
+  private static start(type: PreflightCalibration): void {
+    mavlink2rest.sendMessage({
+      header: {
+        system_id: 255,
+        component_id: 0,
+        sequence: 0,
+      },
+      message: {
+        type: MAVLinkType.COMMAND_LONG,
+        param1: {
+          [PreflightCalibration.GYROSCOPE]: 1,
+          [PreflightCalibration.GYROSCOPE_TEMPERATURE]: 3,
+        }[type] || 0,
+        param2: type === PreflightCalibration.MAGNETOMETER ? 1 : 0,
+        param3: type === PreflightCalibration.PRESSURE ? 1 : 0,
+        param4: {
+          [PreflightCalibration.RC]: 1,
+          [PreflightCalibration.RC_TRIM]: 2,
+        }[type] || 0,
+        param5: {
+          [PreflightCalibration.ACCELEROMETER]: 1,
+          [PreflightCalibration.BOARD_LEVEL]: 2,
+          [PreflightCalibration.ACCELEROMETER_TEMPERATURE]: 3,
+          [PreflightCalibration.SIMPLE_ACCELEROMETER]: 4,
+        }[type] || 0,
+        param6: {
+          [PreflightCalibration.COMPASS_MOTOR_INTERFERENCE]: 1,
+          [PreflightCalibration.AIRPSEED]: 2,
+        }[type] || 0,
+        param7: {
+          [PreflightCalibration.ESC]: 1,
+          [PreflightCalibration.BAROMETER_TEMPERATURE]: 3,
+        }[type] || 0,
+        command: {
+          type: MavCmd.MAV_CMD_PREFLIGHT_CALIBRATION,
+        },
+        target_system: autopilot_data.system_id,
+        target_component: 1,
+        confirmation: 0,
+      },
+    })
+  }
+
+  /**
+   * Generator function for calibration status with timeout
+   * @param {PreflightCaliration} type
+   * @param {number} timeout in seconds
+   */
+  public async* calibrate(type: PreflightCalibration, timeout = 6): AsyncGenerator<MavResult | string> {
+    const startTime = Date.now()
+    Calibrator.start(type)
+
+    while (true) {
+      await sleep(200)
+
+      switch (this.calibrationStatus) {
+        case MavResult.MAV_RESULT_ACCEPTED:
+          this.calibrationStatus = undefined
+          yield 'Calibration done.'
+          return
+
+        case MavResult.MAV_RESULT_IN_PROGRESS:
+          this.calibrationStatus = undefined
+          yield 'In progress..'
+          continue
+
+        case MavResult.MAV_RESULT_CANCELLED:
+        case MavResult.MAV_RESULT_DENIED:
+        case MavResult.MAV_RESULT_FAILED:
+        case MavResult.MAV_RESULT_TEMPORARILY_REJECTED:
+        case MavResult.MAV_RESULT_UNSUPPORTED:
+          yield `Calibration failed with status: ${this.calibrationStatus}`
+          this.calibrationStatus = undefined
+          return
+
+        default:
+          // Handle any other potential cases if needed
+          yield 'Waiting for vehicle..'
+      }
+
+      // Check for timeout
+      if (Date.now() - startTime > timeout * 1000) {
+        yield `Calibration timed out after ${timeout} seconds.`
+        return
+      }
+    }
+  }
+}
+
+const calibrator = Calibrator.getInstance()
+export { calibrator, PreflightCalibration }

core/frontend/src/components/vehiclesetup/overview/BaroCalib.vue

@@ -0,0 +1,133 @@
+<template>
+  <v-card class="ma-2 pa-2">
+    <v-card-title class="align-center">
+      Calibrate Barometer
+    </v-card-title>
+    <v-card-text>
+      <v-simple-table dense>
+        <template #default>
+          <thead>
+            <tr>
+              <th class="text-left">
+                Sensor
+              </th>
+              <th class="text-left">
+                Type
+              </th>
+              <th class="text-left">
+                Bus
+              </th>
+              <th class="text-left">
+                Address
+              </th>
+              <th class="text-left">
+                Status
+              </th>
+              <th class="text-left">
+                Calibrated at
+              </th>
+            </tr>
+          </thead>
+          <tbody>
+            <tr
+              v-for="baro in baros"
+              :key="baro.param"
+            >
+              <td><b>{{ baro.deviceName ?? 'UNKNOWN' }}</b></td>
+              <td v-tooltip="'Used to estimate altitude/depth'">
+                {{ get_pressure_type[baro.param] }} Pressure
+              </td>
+              <td>{{ baro.busType }} {{ baro.bus }}</td>
+              <td>{{ `0x${baro.address}` }}</td>
+              <td>{{ baro_status[baro.param] }}</td>
+              <td>{{ baro_ground_pressure[baro.param] }}</td>
+            </tr>
+          </tbody>
+        </template>
+      </v-simple-table>
+    </v-card-text>
+    <v-card-actions class="justify-center pt-4">
+      {{ calibration_status }}
+    </v-card-actions>
+    <v-card-actions class="justify-center pa-2">
+      <v-btn
+        v-tooltip="'Calibrate all barometers to ground level'"
+        color="primary"
+        :disabled="disable_button"
+        @click="calibrate"
+      >
+        Calibrate
+      </v-btn>
+    </v-card-actions>
+  </v-card>
+</template>
+
+<script lang="ts">
+import Vue from 'vue'
+
+import autopilot_data from '@/store/autopilot'
+import mavlink from '@/store/mavlink'
+import { printParam } from '@/types/autopilot/parameter'
+import { Dictionary } from '@/types/common'
+import decode, { deviceId } from '@/utils/deviceid_decoder'
+import mavlink_store_get from '@/utils/mavlink'
+
+import { calibrator, PreflightCalibration } from '../calibration'
+
+export default Vue.extend({
+  name: 'BaroCalibrate',
+  data() {
+    return {
+      calibration_status: '',
+      disable_button: false,
+    }
+  },
+  computed: {
+    baros(): deviceId[] {
+      return autopilot_data.parameterRegex('^BARO.*_DEVID')
+        .filter((param) => param.value !== 0)
+        .map((parameter) => decode(parameter.name, parameter.value))
+    },
+    baro_status(): Dictionary<string> {
+      const results = {} as Dictionary<string>
+      for (const baro of this.baros) {
+        const radix = baro.param.replace('_DEVID', '')
+        const number = parseInt(radix.replace('BARO', ''), 10)
+        const msg = number === 1 ? 'SCALED_PRESSURE' : `SCALED_PRESSURE${number}`
+        const value = mavlink_store_get(mavlink, `${msg}.messageData.message.press_abs`) as number
+        results[baro.param] = `${value ? value.toFixed(2) : '--'} hPa`
+      }
+      return results
+    },
+    baro_ground_pressure(): Dictionary<string> {
+      const results = {} as Dictionary<string>
+      for (const baro of this.baros) {
+        const radix = baro.param.replace('_DEVID', '')
+        const calibrated_param = autopilot_data.parameter(`${radix}_GND_PRESS`)
+        const pretty_value = (printParam(calibrated_param) / 100).toFixed(2)
+        results[baro.param] = `${pretty_value} hPa`
+      }
+      return results
+    },
+    get_pressure_type(): Dictionary<string> {
+      const results = {} as Dictionary<string>
+      for (const barometer of this.baros) {
+        // BARO_SPEC_GRAV Only exist for underwater vehicles
+        const spec_gravity_param = autopilot_data.parameter('BARO_SPEC_GRAV')
+        results[barometer.param] = (spec_gravity_param && printParam(spec_gravity_param)) ?? 'Barometric'
+      }
+      return results
+    },
+  },
+  methods: {
+    async calibrate() {
+      this.disable_button = true
+      this.calibration_status = 'Starting calibration..'
+      for await (const value of calibrator.calibrate(PreflightCalibration.PRESSURE)) {
+        this.calibration_status = value
+      }
+      this.disable_button = false
+    },
+  },
+})
+</script>