-
Notifications
You must be signed in to change notification settings - Fork 23
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 Takeoff and Land #627
Add Takeoff and Land #627
Changes from all commits
b2234b4
dd634d4
0155183
13239fb
26be491
55df666
4d5eb56
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,16 @@ | ||
<template> | ||
<button | ||
class="relative flex items-center justify-center w-32 p-1 rounded-md shadow-inner h-9 bg-slate-800/60 hover:bg-slate-400/60" | ||
@click="vehicleStore.flying ? vehicleStore.takeoff() : vehicleStore.land()" | ||
> | ||
<span class="inline-block font-extrabold align-middle text-white"> | ||
{{ vehicleStore.flying === undefined ? '...' : vehicleStore.flying ? 'Takeoff' : 'Land' }} | ||
</span> | ||
</button> | ||
</template> | ||
|
||
<script setup lang="ts"> | ||
import { useMainVehicleStore } from '@/stores/mainVehicle' | ||
|
||
const vehicleStore = useMainVehicleStore() | ||
</script> |
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -80,6 +80,7 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo | |
_messages: MAVLinkMessageDictionary = new Map() | ||
|
||
onMAVLinkMessage = new SignalTyped() | ||
_flying = false | ||
|
||
/** | ||
* Function for subclass inheritance | ||
|
@@ -296,6 +297,8 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo | |
|
||
this._isArmed = Boolean(heartbeat.base_mode.bits & MavModeFlag.MAV_MODE_FLAG_SAFETY_ARMED) | ||
this.onArm.emit() | ||
this._flying = heartbeat.system_status.type === MavState.MAV_STATE_ACTIVE | ||
this.onTakeoff.emit() | ||
break | ||
} | ||
case MAVLinkType.SYS_STATUS: { | ||
|
@@ -385,6 +388,44 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo | |
return true | ||
} | ||
|
||
/** | ||
* Helper function for commanding takeoff | ||
* @param {number} altitude (in meters) | ||
*/ | ||
_takeoff(altitude: number): void { | ||
this.sendCommandLong(MavCmd.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, altitude) | ||
} | ||
|
||
/** | ||
* Takeoff | ||
* @returns {void} | ||
*/ | ||
takeoff(): void { | ||
const guidedMode = this.modesAvailable().get('GUIDED') | ||
if (guidedMode === undefined) { | ||
return | ||
} | ||
|
||
this.setMode(guidedMode as Modes) | ||
this.arm() | ||
this._takeoff(10) | ||
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. Should we wait after setting guided mode and also after arming to send the takeoff command, or is it ok to send them continuously? 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. That should be mostly ok |
||
this.onTakeoff.emit() | ||
return | ||
} | ||
|
||
/** | ||
* Land | ||
* @returns {void} | ||
*/ | ||
land(): void { | ||
const landMode = this.modesAvailable().get('LAND') | ||
if (landMode === undefined) { | ||
return | ||
} | ||
this.setMode(landMode as Modes) | ||
return | ||
} | ||
|
||
/** | ||
* Return vehicle altitude-related data | ||
* @returns {Altitude} | ||
|
@@ -442,6 +483,14 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo | |
return this._isArmed | ||
} | ||
|
||
/** | ||
* Check if the vehicle is flying | ||
* @returns {boolean} | ||
*/ | ||
flying(): boolean { | ||
return !this._flying | ||
} | ||
|
||
/** | ||
* Return metadata from the vehicle | ||
* @returns {MetadataFile} | ||
|
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -104,6 +104,7 @@ export const useMainVehicleStore = defineStore('main-vehicle', () => { | |
const velocity: Velocity = reactive({} as Velocity) | ||
const mainVehicle = ref<ArduPilot | undefined>(undefined) | ||
const isArmed = ref<boolean | undefined>(undefined) | ||
const flying = ref<boolean | undefined>(undefined) | ||
const icon = ref<string | undefined>(undefined) | ||
const configurationPages = ref<PageDescription[]>([]) | ||
const timeNow = useTimestamp({ interval: 100 }) | ||
|
@@ -136,6 +137,19 @@ export const useMainVehicleStore = defineStore('main-vehicle', () => { | |
function disarm(): void { | ||
mainVehicle.value?.disarm() | ||
} | ||
/** | ||
* Takeoff the vehicle | ||
*/ | ||
function takeoff(): void { | ||
mainVehicle.value?.takeoff() | ||
} | ||
|
||
/** | ||
* Land the vehicle | ||
*/ | ||
function land(): void { | ||
mainVehicle.value?.land() | ||
} | ||
Comment on lines
+140
to
+152
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. I have mixed feelings with such functions on mainVehicle, that's not a blocker, but just something for us to have in mind for future rework. The idea of cockpit is to be generic, the concept of takeoff and landing applies mostly to aerial vehicles, having such functions here in mainVehicle is a bit complicated. I would recommend in the future for us to have a generic callback map that the vehicle could specify such "logic". We need to put more thought on it, there is now no way for us to check if the vehicle does support such functions. 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. Yep. Same thoughts. But as we don't have discussed yet how to go on that, I think we should just add that now and discuss changes after. |
||
|
||
/** | ||
* Configure the vehicle somehow | ||
|
@@ -237,6 +251,9 @@ export const useMainVehicleStore = defineStore('main-vehicle', () => { | |
mainVehicle.value.onArm.add((armed: boolean) => { | ||
isArmed.value = armed | ||
}) | ||
mainVehicle.value.onTakeoff.add((inAir: boolean) => { | ||
flying.value = inAir | ||
}) | ||
mainVehicle.value.onCpuLoad.add((newCpuLoad: number) => { | ||
cpuLoad.value = newCpuLoad | ||
}) | ||
|
@@ -350,6 +367,8 @@ export const useMainVehicleStore = defineStore('main-vehicle', () => { | |
|
||
return { | ||
arm, | ||
takeoff, | ||
land, | ||
disarm, | ||
modesAvailable, | ||
setFlightMode, | ||
|
@@ -376,6 +395,7 @@ export const useMainVehicleStore = defineStore('main-vehicle', () => { | |
mode, | ||
modes, | ||
isArmed, | ||
flying, | ||
isVehicleOnline, | ||
icon, | ||
configurationPages, | ||
|
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.
Is this MAV_STATE_ACTIVE shared between all vehicles? @ES-Alexander maybe can help answering that one.
If so, maybe we can have a better name that also works for subs/boats/rovers, but if we don't find one, flying is also ok. I usually say or subs are flying 😅