Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
46 changes: 36 additions & 10 deletions gcs/src/components/mapComponents/missionItems.jsx
Original file line number Diff line number Diff line change
Expand Up @@ -31,33 +31,53 @@ export default function MissionItems({
filterMissionItems(missionItems),
)
const [listOfLineCoords, setListOfLineCoords] = useState([])
const [listOfDottedLineCoords, setListOfDottedLineCoords] = useState([])

useEffect(() => {
setFilteredMissionItems(filterMissionItems(missionItems))
}, [missionItems])

useEffect(() => {
setListOfLineCoords(getListOfLineCoordinates(filteredMissionItems))
const { solid: solidLineCoords, dotted: dottedLineCoords } =
getListOfLineCoordinates(filteredMissionItems)

setListOfLineCoords(solidLineCoords)
setListOfDottedLineCoords(dottedLineCoords)
}, [filteredMissionItems])

function getListOfLineCoordinates(filteredMissionItems) {
if (filteredMissionItems.length === 0) return []
if (filteredMissionItems.length === 0) return { solid: [], dotted: [] }

const lineCoordsList = []

filteredMissionItems.forEach((item) => {
const dottedLineCoordsList = []

// Stop processing waypoints after a land command
const landCommandIndex = filteredMissionItems.findIndex((item) =>
[21, 189].includes(item.command),
)
const itemsToProcess =
landCommandIndex === -1
? filteredMissionItems
: filteredMissionItems.slice(0, landCommandIndex + 1)

itemsToProcess.forEach((item) => {
lineCoordsList.push([intToCoord(item.y), intToCoord(item.x)])
})

// Join the last item to first item if aircraft does not land
// Join the last item to first item if aircraft does not land, with a
// dotted line
if (
![21, 189].includes(
filteredMissionItems[filteredMissionItems.length - 1].command,
itemsToProcess[itemsToProcess.length - 1].command, // Use itemsToProcess here
)
) {
lineCoordsList.push([
intToCoord(filteredMissionItems[0].y),
intToCoord(filteredMissionItems[0].x),
dottedLineCoordsList.push([
intToCoord(itemsToProcess[0].y), // Use itemsToProcess here
intToCoord(itemsToProcess[0].x),
])
dottedLineCoordsList.push([
intToCoord(itemsToProcess[itemsToProcess.length - 1].y), // Use itemsToProcess here
intToCoord(itemsToProcess[itemsToProcess.length - 1].x),
])
}

Expand All @@ -80,7 +100,7 @@ export default function MissionItems({
lineCoordsList.push([intToCoord(nextItem.y), intToCoord(nextItem.x)])
})

return lineCoordsList
return { solid: lineCoordsList, dotted: dottedLineCoordsList }
}

return (
Expand All @@ -107,6 +127,12 @@ export default function MissionItems({
coordinates={listOfLineCoords}
colour={tailwindColors.yellow[400]}
/>

<DrawLineCoordinates
coordinates={listOfDottedLineCoords}
colour={tailwindColors.yellow[400]}
lineProps={{ "line-dasharray": [2, 2] }}
/>
</>
)
}
9 changes: 7 additions & 2 deletions gcs/src/components/missions/missionItemsTable.jsx
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,11 @@ function MissionItemsTableNonMemo({
missionItems,
aircraftType,
updateMissionItem,
deleteMissionItem,
updateMissionItemOrder,
}) {
return (
<Table striped withTableBorder withColumnBorders>
<Table striped withTableBorder withColumnBorders stickyHeader>
<Table.Thead>
<Table.Tr>
<Table.Th></Table.Th>
Expand All @@ -22,9 +24,10 @@ function MissionItemsTableNonMemo({
<Table.Th>Param 3</Table.Th>
<Table.Th>Param 4</Table.Th>
<Table.Th>Lat</Table.Th>
<Table.Th>Long</Table.Th>
<Table.Th>Lng</Table.Th>
<Table.Th>Alt</Table.Th>
<Table.Th>Frame</Table.Th>
<Table.Th></Table.Th>
</Table.Tr>
</Table.Thead>
<Table.Tbody>
Expand All @@ -45,6 +48,8 @@ function MissionItemsTableNonMemo({
aircraftType={aircraftType}
missionItem={missionItem}
updateMissionItem={updateMissionItem}
deleteMissionItem={deleteMissionItem}
updateMissionItemOrder={updateMissionItemOrder}
/>
)
})}
Expand Down
29 changes: 28 additions & 1 deletion gcs/src/components/missions/missionItemsTableRow.jsx
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,14 @@
This component displays the row for a mission item in a table.
*/

import { NumberInput, Select, TableTd, TableTr } from "@mantine/core"
import {
ActionIcon,
NumberInput,
Select,
TableTd,
TableTr,
} from "@mantine/core"
import { IconArrowDown, IconArrowUp, IconTrash } from "@tabler/icons-react"
import { useEffect, useState } from "react"
import { coordToInt, intToCoord } from "../../helpers/dataFormatters"
import {
Expand All @@ -18,6 +25,8 @@ export default function MissionItemsTableRow({
aircraftType,
missionItem,
updateMissionItem,
deleteMissionItem,
updateMissionItemOrder,
}) {
const [missionItemData, setMissionItemData] = useState(missionItem)

Expand Down Expand Up @@ -131,6 +140,24 @@ export default function MissionItemsTableRow({
/>
</TableTd>
<TableTd>{getFrameName(missionItemData.frame)}</TableTd>
<TableTd className="flex flex-row gap-2">
<ActionIcon
onClick={() => updateMissionItemOrder(missionItemData.id, -1)}
>
<IconArrowUp size={20} />
</ActionIcon>
<ActionIcon
onClick={() => updateMissionItemOrder(missionItemData.id, 1)}
>
<IconArrowDown size={20} />
</ActionIcon>
<ActionIcon
onClick={() => deleteMissionItem(missionItemData.id)}
color="red"
>
<IconTrash size={20} />
</ActionIcon>
Comment thread
1Blademaster marked this conversation as resolved.
</TableTd>
</TableTr>
)
}
7 changes: 7 additions & 0 deletions gcs/src/components/missions/missionsMap.jsx
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ function MapSectionNonMemo({
currentTab,
markerDragEndCallback,
rallyDragEndCallback,
addNewMissionItem,
mapId = "dashboard",
}) {
const [connected] = useSessionStorage({
Expand Down Expand Up @@ -198,6 +199,12 @@ function MapSectionNonMemo({
},
})
}}
onClick={(e) => {
setClicked(false)
let lat = e.lngLat.lat
let lon = e.lngLat.lng
addNewMissionItem(lat, lon)
}}
cursor="default"
>
{/* Show marker on map if the position is set */}
Expand Down
4 changes: 2 additions & 2 deletions gcs/src/components/missions/rallyItemsTable.jsx
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@ import RallyItemsTableRow from "./rallyItemsTableRow"

function RallyItemsTableNonMemo({ rallyItems, updateRallyItem }) {
return (
<Table striped withTableBorder withColumnBorders>
<Table striped withTableBorder withColumnBorders stickyHeader>
<Table.Thead>
<Table.Tr>
<Table.Th></Table.Th>
Expand All @@ -17,7 +17,7 @@ function RallyItemsTableNonMemo({ rallyItems, updateRallyItem }) {
<Table.Th>Param 3</Table.Th>
<Table.Th>Param 4</Table.Th>
<Table.Th>Lat</Table.Th>
<Table.Th>Long</Table.Th>
<Table.Th>Lng</Table.Th>
<Table.Th>Alt</Table.Th>
<Table.Th>Frame</Table.Th>
</Table.Tr>
Expand Down
95 changes: 92 additions & 3 deletions gcs/src/missions.jsx
Original file line number Diff line number Diff line change
Expand Up @@ -17,10 +17,11 @@ import MissionItemsTable from "./components/missions/missionItemsTable"
import MissionsMapSection from "./components/missions/missionsMap"
import RallyItemsTable from "./components/missions/rallyItemsTable"
import NoDroneConnected from "./components/noDroneConnected"
import { intToCoord } from "./helpers/dataFormatters"
import { coordToInt, intToCoord } from "./helpers/dataFormatters"
import {
COPTER_MODES_FLIGHT_MODE_MAP,
MAV_AUTOPILOT_INVALID,
MAV_FRAME_LIST,
PLANE_MODES_FLIGHT_MODE_MAP,
} from "./helpers/mavlinkConstants"
import {
Expand Down Expand Up @@ -60,6 +61,12 @@ export default function Missions() {
key: "homePosition",
defaultValue: null,
})
const [targetInfo, setTargetInfo] = useSessionStorage({
key: "targetInfo",
defaultValue: { target_component: 0, target_system: 255 },
})

const newMissionItemAltitude = 30 // TODO: Make this configurable

// Heartbeat data
const [heartbeatData, setHeartbeatData] = useState({ system_status: 0 })
Expand Down Expand Up @@ -92,6 +99,7 @@ export default function Missions() {
} else {
socket.emit("set_state", { state: "missions" })
socket.emit("get_home_position")
socket.emit("get_target_info")
}

socket.on("incoming_msg", (msg) => {
Expand All @@ -108,14 +116,18 @@ export default function Missions() {
}
})

socket.on("target_info", (data) => {
if (data) {
setTargetInfo(data)
}
})

socket.on("current_mission", (data) => {
if (!data.success) {
showErrorNotification(data.message)
return
}

console.log(data)

if (data.mission_type === "mission") {
const missionItemsWithIds = []
for (let missionItem of data.items) {
Expand Down Expand Up @@ -146,6 +158,7 @@ export default function Missions() {
return () => {
socket.off("incoming_msg")
socket.off("home_position_result")
socket.off("target_info")
socket.off("current_mission")
socket.off("write_mission_result")
}
Expand All @@ -168,6 +181,34 @@ export default function Missions() {
return missionItem
}

function addNewMissionItem(lat, lon) {
const newMissionItem = {
id: uuidv4(),
seq: missionItems.length,
x: coordToInt(lat),
y: coordToInt(lon),
z: newMissionItemAltitude,
frame: parseInt(
Object.keys(MAV_FRAME_LIST).find(
(key) => MAV_FRAME_LIST[key] === "MAV_FRAME_GLOBAL_RELATIVE_ALT",
),
),
command: 16, // MAV_CMD_NAV_WAYPOINT
Comment thread
1Blademaster marked this conversation as resolved.
param1: 0,
param2: 0,
param3: 0,
param4: 0,
current: 0,
autocontinue: 1,
target_component: targetInfo.target_component,
target_system: targetInfo.target_system,
Comment thread
1Blademaster marked this conversation as resolved.
mission_type: 0,
mavpackettype: "MISSION_ITEM_INT",
}

setMissionItems((prevItems) => [...prevItems, newMissionItem])
}

function updateMissionItem(updatedMissionItem) {
setMissionItems((prevItems) =>
prevItems.map((item) =>
Expand All @@ -187,6 +228,51 @@ export default function Missions() {
)
}

function deleteMissionItem(missionItemId) {
setMissionItems((prevItems) => {
const updatedItems = prevItems.filter((item) => item.id !== missionItemId)

return updatedItems.map((item, index) => ({
...item,
seq: index, // Reassign seq based on the new order
}))
})
}

function updateMissionItemOrder(missionItemId, indexIncrement) {
setMissionItems((prevItems) => {
const currentIndex = prevItems.findIndex(
(item) => item.id === missionItemId,
)

// Ensure the item exists and the swap is within bounds
if (
currentIndex === -1 ||
(indexIncrement === -1 && currentIndex === 0) ||
(indexIncrement === 1 && currentIndex === prevItems.length - 1)
) {
return prevItems // No changes if out of bounds
}

// Calculate the new index
const newIndex = currentIndex + indexIncrement

// Create a copy of the items array
const updatedItems = [...prevItems]

// Swap the items
const temp = updatedItems[currentIndex]
updatedItems[currentIndex] = updatedItems[newIndex]
updatedItems[newIndex] = temp

// Update the seq values
updatedItems[currentIndex].seq = currentIndex
updatedItems[newIndex].seq = newIndex

return updatedItems
})
}

function readMissionFromDrone() {
socket.emit("get_current_mission", { type: activeTab })
}
Expand Down Expand Up @@ -317,6 +403,7 @@ export default function Missions() {
currentTab={activeTab}
markerDragEndCallback={updateMissionItem}
rallyDragEndCallback={updateRallyItem}
addNewMissionItem={addNewMissionItem}
mapId="missions"
/>
</div>
Expand Down Expand Up @@ -350,6 +437,8 @@ export default function Missions() {
missionItems={missionItems}
aircraftType={aircraftType}
updateMissionItem={updateMissionItem}
deleteMissionItem={deleteMissionItem}
updateMissionItemOrder={updateMissionItemOrder}
/>
</Tabs.Panel>
<Tabs.Panel value="fence"></Tabs.Panel>
Expand Down
17 changes: 17 additions & 0 deletions radio/app/endpoints/connections.py
Original file line number Diff line number Diff line change
Expand Up @@ -28,3 +28,20 @@ def isConnectedToDrone() -> None:
Handle client asking if we're connected to the drone or not
"""
socketio.emit("is_connected_to_drone", bool(droneStatus.drone))


@socketio.on("get_target_info")
def getTargetInfo() -> None:
"""
Return the target component and target system
"""
if droneStatus.drone:
socketio.emit(
"target_info",
{
"target_component": droneStatus.drone.target_component,
"target_system": droneStatus.drone.target_system,
},
)
else:
socketio.emit("target_info", None)
Loading
Loading