-
Notifications
You must be signed in to change notification settings - Fork 64
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Add UI for accelerometer calibration #2451
Merged
Merged
Changes from all commits
Commits
Show all changes
2 commits
Select commit
Hold shift + click to select a range
File filter
Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
116 changes: 116 additions & 0 deletions
116
...d/src/components/vehiclesetup/configuration/accelerometer/ArdupilotAccelerometerSetup.vue
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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> |
228 changes: 228 additions & 0 deletions
228
.../src/components/vehiclesetup/configuration/accelerometer/FullAccelerometerCalibration.vue
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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', | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. The message isn't clear enough: we must say what position we expect the vehicle to be. A level surface can mean anything. |
||
[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> |
Oops, something went wrong.
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Differently from the Quick Calibration, I can't close this dialogue.