Add UI for accelerometer calibration

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

@@ -63,6 +63,7 @@ import autopilot from '@/store/autopilot_manager'
 import settings from '@/store/settings'
 
 import SpinningLogo from '../common/SpinningLogo.vue'
+import ArdupilotAccelerometerSetup from './configuration/accelerometer/ArdupilotAccelerometerSetup.vue'
 import ArdupilotMavlinkCompassSetup from './configuration/compass/ArdupilotMavlinkCompassSetup.vue'
 import LightsConfigration from './configuration/lights.vue'
 import BaroCalib from './overview/BaroCalib.vue'
@@ -91,14 +92,15 @@ export default Vue.extend({
     ParamSets,
     LightsConfigration,
     ArdupilotMavlinkCompassSetup,
+    ArdupilotAccelerometerSetup,
     SpinningLogo,
   },
   data() {
     return {
       page_selected: null as string | null,
       pages: [
         { title: 'Parameters', component: ParamSets },
-        { title: 'Accelerometer', component: undefined },
+        { title: 'Accelerometer', component: ArdupilotAccelerometerSetup },
         { title: 'Compass', component: ArdupilotMavlinkCompassSetup },
         { title: 'Baro', component: BaroCalib },
         { title: 'Gripper', component: undefined },

core/frontend/src/components/vehiclesetup/configuration/accelerometer/ArdupilotAccelerometerSetup.vue

@@ -0,0 +1,116 @@
+<template>
+  <div class="d-flex flex-col justify-center">
+    <v-card v-if="params_finished_loaded" outline class="pa-5 mt-4 mr-2 mb-2">
+      <v-simple-table>
+        <thead>
+          <tr>
+            <th>IMU</th>
+            <th>Calibration</th>
+          </tr>
+        </thead>
+        <tbody>
+          <tr v-for="imu in imus" :key="imu.deviceIdNumber">
+            <td>{{ imu.deviceName }}</td>
+            <td>
+              <v-icon
+                v-if="imu_is_calibrated[imu.param]"
+                v-tooltip="'Sensor is calibrated and good to use'"
+                color="green"
+              >
+                mdi-emoticon-happy-outline
+              </v-icon>
+              <v-icon
+                v-else
+                v-tooltip="'Sensor needs to be calibrated'"
+                color="red"
+              >
+                mdi-emoticon-sad-outline
+              </v-icon>
+              {{ imu_is_calibrated[imu.param] ? 'Calibrated' : 'Needs calibration' }}
+              <v-icon
+                v-if="imu_temperature_is_calibrated[imu.param].calibrated"
+                v-tooltip="`Sensor was calibrated at ${getCalibrationTemperature(imu)} ºC`"
+                color="green"
+              >
+                mdi-thermometer-check
+              </v-icon>
+              <v-icon
+                v-else
+                v-tooltip="'Sensor thermometer needs to be calibrated'"
+                color="red"
+              >
+                mdi-thermometer-off
+              </v-icon>
+            </td>
+          </tr>
+        </tbody>
+      </v-simple-table>
+      <v-card-actions class="justify-center">
+        <FullAccelerometerCalibration />
+        <QuickAccelerometerCalibration />
+      </v-card-actions>
+    </v-card>
+    <spinning-logo
+      v-else
+      size="50%"
+      :subtitle="`${loaded_params}/${total_params} parameters loaded`"
+    />
+  </div>
+</template>
+<script lang="ts">
+import Vue from 'vue'
+
+// eslint-disable-next-line
+import FullAccelerometerCalibration from '@/components/vehiclesetup/configuration/accelerometer/FullAccelerometerCalibration.vue'
+import autopilot_data from '@/store/autopilot'
+import { Dictionary } from '@/types/common'
+import decode, { deviceId } from '@/utils/deviceid_decoder'
+
+import { imu_is_calibrated, imu_temperature_is_calibrated } from '../common'
+import QuickAccelerometerCalibration from './QuickAccelerometerCalibration.vue'
+
+export default Vue.extend({
+  name: 'ArdupilotAccelerometerSetup',
+  components: {
+    FullAccelerometerCalibration,
+    QuickAccelerometerCalibration,
+  },
+  data() {
+    return {
+    }
+  },
+  computed: {
+    params_finished_loaded(): boolean {
+      return autopilot_data.finished_loading
+    },
+    loaded_params(): number {
+      return autopilot_data.parameters_loaded
+    },
+    total_params(): number {
+      return autopilot_data.parameters_total
+    },
+    imus() : deviceId[] {
+      return autopilot_data.parameterRegex('^INS_ACC.*_ID')
+        .filter((param) => param.value !== 0)
+        .map((parameter) => decode(parameter.name, parameter.value))
+    },
+    imu_is_calibrated(): Dictionary<boolean> {
+      return imu_is_calibrated(this.imus, autopilot_data)
+    },
+    imu_temperature_is_calibrated(): Dictionary<{ calibrated: boolean, calibrationTemperature: number }> {
+      return imu_temperature_is_calibrated(this.imus, autopilot_data)
+    },
+  },
+  methods: {
+    getCalibrationTemperature(imu: deviceId) {
+      return this.imu_temperature_is_calibrated[imu.param].calibrationTemperature.toFixed(0)
+    },
+  },
+})
+</script>
+<style scoped="true">
+td {
+  padding-left: 5px !important;
+  padding-right: 5px !important;
+}
+</style>

core/frontend/src/components/vehiclesetup/configuration/accelerometer/FullAccelerometerCalibration.vue

@@ -0,0 +1,228 @@
+<template>
+  <v-dialog
+    v-model="dialog"
+    persistent
+    width="500"
+  >
+    <template #activator="{ on, attrs }">
+      <v-btn
+        color="primary"
+        class="ma-2"
+        v-bind="attrs"
+        v-on="on"
+      >
+        Full Calibration
+      </v-btn>
+    </template>
+
+    <v-card>
+      <v-card-title class="text-h5 grey lighten-2">
+        Accelerometer Calibration
+      </v-card-title>
+      <div class="ma-6">
+        <p>
+          Full accelerometer calibration requires you to place the vehicle onto all of its sides and keep
+          it still for a few seconds.
+          This will allow the autopilot to calibrate the accelerometer.
+        </p>
+      </div>
+      <v-alert
+        v-if="![states.IDLE, states.WAITING_FOR_VEHICLE_RESPONSE].includes(state)"
+        dense
+        :type="current_state_type"
+      >
+        <strong>{{ current_state_text }}</strong>
+      </v-alert>
+      <v-card-actions class="justify-center">
+        <v-btn v-if="show_start_button" :loading="start_button_loading" class="primary" @click="startCalibration">
+          Start Calibration
+        </v-btn>
+        <v-btn v-if="show_next_button" class="primary" @click="nextStep">
+          Next
+        </v-btn>
+        <v-btn v-if="show_ok_button" class="primary" @click="cleanup">
+          Ok
+        </v-btn>
+      </v-card-actions>
+    </v-card>
+  </v-dialog>
+</template>
+
+<script lang="ts">
+
+import mavlink2rest from '@/libs/MAVLink2Rest'
+import { MavCmd } from '@/libs/MAVLink2Rest/mavlink2rest-ts/messages/mavlink2rest-enum'
+import autopilot_data from '@/store/autopilot'
+
+import { calibrator, PreflightCalibration } from '../../calibration'
+
+// Ideally these would come from mavlink2rest-enum
+// but we don't have the numbering information there
+enum AccelcalVehiclePos {
+  ACCELCAL_VEHICLE_POS_LEVEL = 1,
+  ACCELCAL_VEHICLE_POS_LEFT = 2,
+  ACCELCAL_VEHICLE_POS_RIGHT = 3,
+  ACCELCAL_VEHICLE_POS_NOSEDOWN = 4,
+  ACCELCAL_VEHICLE_POS_NOSEUP = 5,
+  ACCELCAL_VEHICLE_POS_BACK = 6,
+  ACCELCAL_VEHICLE_POS_SUCCESS = 16777215,
+  ACCELCAL_VEHICLE_POS_FAILED = 16777216
+}
+
+enum CalState {
+  IDLE,
+  WAITING_FOR_VEHICLE_RESPONSE,
+  WAITING_FOR_LEVEL_POSITION,
+  WAITING_FOR_LEFT_POSITION,
+  WAITING_FOR_RIGHT_POSITION,
+  WAITING_FOR_NOSEDOWN_POSITION,
+  WAITING_FOR_NOSEUP_POSITION,
+  WAITING_FOR_BACK_POSITION,
+  SUCCESS,
+  FAIL,
+}
+
+export default {
+  name: 'FullAccelerometerCalibration',
+  components: {
+  },
+  data() {
+    return {
+      dialog: false,
+      state: CalState.IDLE,
+    }
+  },
+  computed: {
+    states() {
+      return CalState
+    },
+    show_next_button() {
+      return ![
+        CalState.IDLE,
+        CalState.FAIL,
+        CalState.SUCCESS,
+        CalState.WAITING_FOR_VEHICLE_RESPONSE,
+      ].includes(this.state)
+    },
+    show_ok_button() {
+      return [CalState.SUCCESS, CalState.FAIL].includes(this.state)
+    },
+    show_start_button() {
+      return [CalState.IDLE, CalState.WAITING_FOR_VEHICLE_RESPONSE].includes(this.state)
+    },
+    start_button_loading() {
+      return this.state === CalState.WAITING_FOR_VEHICLE_RESPONSE
+    },
+    current_state_text() {
+      const AccelStateText: { [key: number]: string } = {
+        [CalState.WAITING_FOR_LEVEL_POSITION]: 'Place the vehicle on a level surface',
+        [CalState.WAITING_FOR_LEFT_POSITION]: 'Place the vehicle on its left side',
+        [CalState.WAITING_FOR_RIGHT_POSITION]: 'Place the vehicle on its right side',
+        [CalState.WAITING_FOR_NOSEDOWN_POSITION]: 'Place the vehicle with its nose down',
+        [CalState.WAITING_FOR_NOSEUP_POSITION]: 'Place the vehicle with its nose up',
+        [CalState.WAITING_FOR_BACK_POSITION]: 'Place the vehicle on its back',
+        [CalState.SUCCESS]: 'Calibration Successful',
+        [CalState.FAIL]: 'Calibration Failed',
+      }
+      return AccelStateText[Number(this.state)] ?? ''
+    },
+    current_state_type() {
+      if (this.state === CalState.SUCCESS) {
+        return 'success'
+      }
+      if (this.state === CalState.FAIL) {
+        return 'error'
+      }
+      return 'info'
+    },
+  },
+  methods: {
+    nextStep() {
+      // param needs to match the current step
+      const param = [
+        CalState.WAITING_FOR_LEVEL_POSITION,
+        CalState.WAITING_FOR_LEFT_POSITION,
+        CalState.WAITING_FOR_RIGHT_POSITION,
+        CalState.WAITING_FOR_NOSEDOWN_POSITION,
+        CalState.WAITING_FOR_NOSEUP_POSITION,
+        CalState.WAITING_FOR_BACK_POSITION,
+      ].indexOf(this.state) + 1
+
+      mavlink2rest.sendMessage({
+        header: {
+          system_id: 255,
+          component_id: 1,
+          sequence: 1,
+        },
+        message: {
+          type: 'COMMAND_LONG',
+          param1: param,
+          param2: 0,
+          param3: 0,
+          param4: 0,
+          param5: 0,
+          param6: 0,
+          param7: 0,
+          command: {
+            type: MavCmd.MAV_CMD_ACCELCAL_VEHICLE_POS,
+          },
+          target_system: autopilot_data.system_id,
+          target_component: 1,
+          confirmation: 1,
+        },
+      })
+    },
+    cleanup() {
+      this.dialog = false
+      // delay resetting the state to wait for the close animation to finish
+      setTimeout(() => { this.state = CalState.IDLE }, 500)
+    },
+    async startCalibration() {
+      const ack_listener = mavlink2rest.startListening('COMMAND_LONG').setCallback((message) => {
+        if (message.message.command.type === 'MAV_CMD_ACCELCAL_VEHICLE_POS') {
+          switch (message.message.param1) {
+            case AccelcalVehiclePos.ACCELCAL_VEHICLE_POS_LEVEL:
+              this.state = CalState.WAITING_FOR_LEVEL_POSITION
+              break
+            case AccelcalVehiclePos.ACCELCAL_VEHICLE_POS_LEFT:
+              this.state = CalState.WAITING_FOR_LEFT_POSITION
+              break
+            case AccelcalVehiclePos.ACCELCAL_VEHICLE_POS_RIGHT:
+              this.state = CalState.WAITING_FOR_RIGHT_POSITION
+              break
+            case AccelcalVehiclePos.ACCELCAL_VEHICLE_POS_NOSEDOWN:
+              this.state = CalState.WAITING_FOR_NOSEDOWN_POSITION
+              break
+            case AccelcalVehiclePos.ACCELCAL_VEHICLE_POS_NOSEUP:
+              this.state = CalState.WAITING_FOR_NOSEUP_POSITION
+              break
+            case AccelcalVehiclePos.ACCELCAL_VEHICLE_POS_BACK:
+              this.state = CalState.WAITING_FOR_BACK_POSITION
+              break
+            case AccelcalVehiclePos.ACCELCAL_VEHICLE_POS_SUCCESS:
+              this.state = CalState.SUCCESS
+              this.cleanup()
+              ack_listener.discard()
+              break
+            case AccelcalVehiclePos.ACCELCAL_VEHICLE_POS_FAILED:
+              this.state = CalState.FAIL
+              ack_listener.discard()
+              break
+            default:
+              console.error('Unknown vehicle position:', message.message.param1)
+              break
+          }
+        }
+      })
+      for await (const value of calibrator.calibrate(PreflightCalibration.ACCELEROMETER)) {
+        console.log(value)
+      }
+      this.state = CalState.WAITING_FOR_VEHICLE_RESPONSE
+    },
+  },
+}
+</script>
+
+  <style scoped>
+
+  </style>