diff --git a/gcs/ekfStatus.html b/gcs/ekfStatus.html new file mode 100644 index 000000000..d7c0a6610 --- /dev/null +++ b/gcs/ekfStatus.html @@ -0,0 +1,12 @@ + + + + + + + + +
+ + + diff --git a/gcs/electron/main.ts b/gcs/electron/main.ts index 2bd38fc71..ab0d5c7fb 100644 --- a/gcs/electron/main.ts +++ b/gcs/electron/main.ts @@ -22,6 +22,9 @@ import registerAboutIPC, { destroyAboutWindow, openAboutPopout, } from "./modules/aboutWindow" +import registerEkfStatusIPC, { + destroyEkfStatusWindow, +} from "./modules/ekfStatus" import registerLinkStatsIPC, { destroyLinkStatsWindow, openLinkStatsWindow, @@ -220,6 +223,7 @@ function createWindow() { registerWebcamIPC(win) registerAboutIPC() registerLinkStatsIPC() + registerEkfStatusIPC() // Open links in browser, not within the electron window. // Note, links must have target="_blank" @@ -371,14 +375,19 @@ function startBackend() { }) } +function closeWindows() { + destroyWebcamWindow() + destroyAboutWindow() + destroyLinkStatsWindow() + destroyEkfStatusWindow() +} + // Quit when all windows are closed, except on macOS. There, it's common // for applications and their menu bar to stay active until the user quits // explicitly with Cmd + Q. function closeWithBackend() { // Always close all popout windows first - destroyWebcamWindow() - destroyAboutWindow() - destroyLinkStatsWindow() + closeWindows() console.log("Killing backend") // kill any processes with the name "fgcs_backend.exe" // Windows @@ -400,9 +409,7 @@ app.on("before-quit", () => { console.log("Stopping backend") spawnSync("pkill", ["-f", "fgcs_backend"]) pythonBackend = null - destroyWebcamWindow() - destroyAboutWindow() - destroyLinkStatsWindow() + closeWindows() } }) diff --git a/gcs/electron/modules/ekfStatus.ts b/gcs/electron/modules/ekfStatus.ts new file mode 100644 index 000000000..ea84640ca --- /dev/null +++ b/gcs/electron/modules/ekfStatus.ts @@ -0,0 +1,61 @@ +import { BrowserWindow, ipcMain } from "electron" +import path from "path" + +const VITE_DEV_SERVER_URL = process.env["VITE_DEV_SERVER_URL"] + +let ekfStatusWin: BrowserWindow | null = null + +export function openEkfStatusWindow() { + if (ekfStatusWin === null) { + ekfStatusWin = new BrowserWindow({ + width: 700, + height: 400, + frame: true, + icon: path.join(process.env.VITE_PUBLIC, "app_icon.ico"), + show: false, + title: "EKF Status", + webPreferences: { + preload: path.join(__dirname, "preload.js"), + contextIsolation: true, + }, + fullscreen: false, + fullscreenable: false, + alwaysOnTop: true, + }) + } + + if (VITE_DEV_SERVER_URL) { + ekfStatusWin?.loadURL(VITE_DEV_SERVER_URL + "ekfStatus.html") + } else { + ekfStatusWin?.loadFile(path.join(process.env.DIST, "ekfStatus.html")) + } + + ekfStatusWin.on("close", () => { + ekfStatusWin = null + }) + ekfStatusWin.setMenuBarVisibility(false) + ekfStatusWin.show() +} + +export function closeEkfStatusWindow() { + destroyEkfStatusWindow() +} + +export function destroyEkfStatusWindow() { + ekfStatusWin?.close() + ekfStatusWin = null +} + +export default function registerEkfStatusIPC() { + ipcMain.removeHandler("app:open-ekf-status-window") + ipcMain.removeHandler("app:close-ekf-status-window") + ipcMain.removeHandler("app:update-ekf-status") + + ipcMain.handle("app:open-ekf-status-window", () => { + openEkfStatusWindow() + }) + ipcMain.handle("app:close-ekf-status-window", () => closeEkfStatusWindow()) + ipcMain.handle("app:update-ekf-status", (_, ekfStatusData) => { + ekfStatusWin?.webContents.send("app:send-ekf-status", ekfStatusData) + }) +} diff --git a/gcs/electron/modules/webcam.ts b/gcs/electron/modules/webcam.ts index aa8b40ef0..bd0a7f7ca 100644 --- a/gcs/electron/modules/webcam.ts +++ b/gcs/electron/modules/webcam.ts @@ -49,6 +49,7 @@ export function openWebcamPopout( }, fullscreen: false, fullscreenable: false, + alwaysOnTop: true, }) } else { console.warn("2nd webcam window requested, ignoring") @@ -85,7 +86,6 @@ export function openWebcamPopout( } export function closeWebcamPopout(mainWindow: BrowserWindow | null) { - console.log("Destroying webcam window") destroyWebcamWindow() mainWindow?.webContents.send("app:webcam-closed") } diff --git a/gcs/electron/preload.js b/gcs/electron/preload.js index 8e090d95d..1fa7dbfaf 100644 --- a/gcs/electron/preload.js +++ b/gcs/electron/preload.js @@ -20,6 +20,8 @@ const ALLOWED_INVOKE_CHANNELS = [ "app:close-link-stats-window", "app:update-link-stats", "window:select-file-in-explorer", + "app:update-ekf-status", + "app:open-ekf-status-window", ] const ALLOWED_SEND_CHANNELS = [ @@ -41,6 +43,7 @@ const ALLOWED_ON_CHANNELS = [ "app:webcam-closed", "app:send-link-stats", "fla:log-parse-progress", + "app:send-ekf-status", ] contextBridge.exposeInMainWorld("ipcRenderer", { diff --git a/gcs/src/components/dashboard/ekfDisplay.jsx b/gcs/src/components/dashboard/ekfDisplay.jsx new file mode 100644 index 000000000..d603faee8 --- /dev/null +++ b/gcs/src/components/dashboard/ekfDisplay.jsx @@ -0,0 +1,37 @@ +import { useMemo } from "react" +import { useSelector } from "react-redux" +import { selectEkfCalculatedStatus } from "../../redux/slices/droneInfoSlice" + +// Tailwind styling +import resolveConfig from "tailwindcss/resolveConfig" +import tailwindConfig from "../../../tailwind.config" +const tailwindColors = resolveConfig(tailwindConfig).theme.colors + +const RED = tailwindColors.red[500] +const YELLOW = tailwindColors.yellow[500] + +export default function EkfDisplay({ telemetryFontSize }) { + const ekfCalculatedStatus = useSelector(selectEkfCalculatedStatus) + + const textColour = useMemo(() => { + if (ekfCalculatedStatus > 0.8) return RED + if (ekfCalculatedStatus > 0.5) return YELLOW + return "" + }, [ekfCalculatedStatus]) + + return ( +
{ + window.ipcRenderer.invoke("app:open-ekf-status-window") + }} + > + EKF +
+ ) +} diff --git a/gcs/src/components/dashboard/telemetry.jsx b/gcs/src/components/dashboard/telemetry.jsx index 9e54356ee..49e52b293 100644 --- a/gcs/src/components/dashboard/telemetry.jsx +++ b/gcs/src/components/dashboard/telemetry.jsx @@ -4,9 +4,9 @@ */ // Custom Components +import { MAV_STATE } from "../../helpers/mavlinkConstants" import { AttitudeIndicator, HeadingIndicator } from "./indicator" import TelemetryValueDisplay from "./telemetryValueDisplay" -import { MAV_STATE } from "../../helpers/mavlinkConstants" // Redux import { useSelector } from "react-redux" @@ -21,6 +21,7 @@ import { selectPrearmEnabled, selectTelemetry, } from "../../redux/slices/droneInfoSlice" +import EkfDisplay from "./ekfDisplay" export default function TelemetrySection({ calcIndicatorSize, @@ -175,36 +176,51 @@ export default function TelemetrySection({ - {/* Battery information */} -
-

BATTERY

- - - - {batteryData.map((battery) => ( - - - - - - - ))} - -
BATTERY{battery.id} - {(battery.voltages ? battery.voltages[0] / 1000 : 0).toFixed( - 2, - )} - V - - {(battery.current_battery - ? battery.current_battery / 100 - : 0 - ).toFixed(2)} - A - - {battery.battery_remaining ? battery.battery_remaining : 0}% -
+ {/* EKF and VIBE labels */} +
+ +
+ VIBE +
+ + {/* Battery information */} + {batteryData.length > 0 && ( +
+ + + {batteryData.map((battery) => ( + + + + + + + ))} + +
BATTERY{battery.id} + {(battery.voltages + ? battery.voltages[0] / 1000 + : 0 + ).toFixed(2)} + V + + {(battery.current_battery + ? battery.current_battery / 100 + : 0 + ).toFixed(2)} + A + + {battery.battery_remaining ? battery.battery_remaining : 0}% +
+
+ )}
) } diff --git a/gcs/src/components/ekfStatusWindow/ekfStatusWindow.jsx b/gcs/src/components/ekfStatusWindow/ekfStatusWindow.jsx new file mode 100644 index 000000000..5157825c9 --- /dev/null +++ b/gcs/src/components/ekfStatusWindow/ekfStatusWindow.jsx @@ -0,0 +1,104 @@ +"use client" + +import { Progress } from "@mantine/core" +import { useEffect, useMemo, useState } from "react" +import { + EKF_STATUS_FLAGS, + getActiveEKFFlags, +} from "../../helpers/mavlinkConstants" + +// Tailwind styling +import resolveConfig from "tailwindcss/resolveConfig" +import tailwindConfig from "../../../tailwind.config" +const tailwindColors = resolveConfig(tailwindConfig).theme.colors + +// EKF Flags that are good when enabled +const FLAGS_ENABLED_BAD = ["EKF_UNINITIALIZED", "EKF_GPS_GLITCHING"] + +const EKF_VARIANCE_STRINGS = { + compass_variance: "Compass Variance", + pos_horiz_variance: "Horizontal Pos Variance", + pos_vert_variance: "Vertical Pos Variance", + terrain_alt_variance: "Terrain Alt Variance", + velocity_variance: "Velocity Variance", +} + +const RED = tailwindColors.red[500] +const YELLOW = tailwindColors.yellow[500] +const GREEN = tailwindColors.green[500] + +function getPercentageFromValue(value) { + return (value * 100).toFixed(2) +} + +function getColourFromValue(value) { + if (value > 0.8) return RED + if (value > 0.5) return YELLOW + return "" +} + +export default function EkfStatusWindow() { + const [ekfVariances, setEkfVariances] = useState({ + compass_variance: 0, + pos_horiz_variance: 0, + pos_vert_variance: 0, + terrain_alt_variance: 0, + velocity_variance: 0, + }) + const [ekfFlags, setEkfFlags] = useState(0) + + const activeFlags = useMemo(() => { + return getActiveEKFFlags(ekfFlags) + }, [ekfFlags]) + + useEffect(() => { + window.ipcRenderer.on("app:send-ekf-status", (_event, status) => { + setEkfVariances({ + compass_variance: status.compass_variance, + pos_horiz_variance: status.pos_horiz_variance, + pos_vert_variance: status.pos_vert_variance, + terrain_alt_variance: status.terrain_alt_variance, + velocity_variance: status.velocity_variance, + }) + setEkfFlags(status.flags) + }) + }, []) + + return ( +
+
+ {Object.entries(ekfVariances).map(([key, value]) => ( +
+

+ {EKF_VARIANCE_STRINGS[key]} - {value.toFixed(2)} +

+ + + +
+ ))} +
+
+ {Object.values(EKF_STATUS_FLAGS).map((flag) => ( +

+ {flag} +

+ ))} +
+
+ ) +} diff --git a/gcs/src/ekfStatus.jsx b/gcs/src/ekfStatus.jsx new file mode 100644 index 000000000..7b860c8ff --- /dev/null +++ b/gcs/src/ekfStatus.jsx @@ -0,0 +1,25 @@ +import "./css/index.css" // Needs to be at the top of the file +import "./css/resizable.css" + +// Style imports +import "@mantine/code-highlight/styles.css" +import "@mantine/core/styles.css" +import "@mantine/notifications/styles.css" +import "@mantine/spotlight/styles.css" +import "@mantine/tiptap/styles.css" + +import { MantineProvider } from "@mantine/core" +import React from "react" +import ReactDOM from "react-dom/client" +import { CustomMantineTheme } from "./components/customMantineTheme" +import EkfStatusWindow from "./components/ekfStatusWindow/ekfStatusWindow" + +ReactDOM.createRoot(document.getElementById("root")).render( + + + + + , +) + +postMessage({ payload: "removeLoading" }, "*") diff --git a/gcs/src/helpers/mavlinkConstants.js b/gcs/src/helpers/mavlinkConstants.js index 72c74fff3..7a9acc625 100644 --- a/gcs/src/helpers/mavlinkConstants.js +++ b/gcs/src/helpers/mavlinkConstants.js @@ -478,3 +478,31 @@ export const MAV_FRAME_LIST = { 22: "MAV_FRAME_LOCAL_FLU", 23: "MAV_FRAME_ENUM_END", } + +export const EKF_STATUS_FLAGS = { + 1: "EKF_ATTITUDE", + 2: "EKF_VELOCITY_HORIZ", + 4: "EKF_VELOCITY_VERT", + 8: "EKF_POS_HORIZ_REL", + 16: "EKF_POS_HORIZ_ABS", + 32: "EKF_POS_VERT_ABS", + 64: "EKF_POS_VERT_AGL", + 128: "EKF_CONST_POS_MODE", + 256: "EKF_PRED_POS_HORIZ_REL", + 512: "EKF_PRED_POS_HORIZ_ABS", + 1024: "EKF_UNINITIALIZED", + 32768: "EKF_GPS_GLITCHING", +} + +export function getActiveEKFFlags(statusValue) { + const activeFlags = [] + + for (const [flag, name] of Object.entries(EKF_STATUS_FLAGS)) { + const flagValue = parseInt(flag) + if (statusValue & flagValue) { + activeFlags.push(name) + } + } + + return activeFlags +} diff --git a/gcs/src/redux/middleware/socketMiddleware.js b/gcs/src/redux/middleware/socketMiddleware.js index f9aa83210..0d5823e63 100644 --- a/gcs/src/redux/middleware/socketMiddleware.js +++ b/gcs/src/redux/middleware/socketMiddleware.js @@ -53,6 +53,7 @@ import { setAttitudeData, setBatteryData, setDroneAircraftType, + setEkfStatusReportData, setExtraData, setGpsData, setGpsRawIntData, @@ -206,6 +207,19 @@ const socketMiddleware = (store) => { case "BATTERY_STATUS": store.dispatch(setBatteryData(msg)) break + case "EKF_STATUS_REPORT": { + const data = { + compass_variance: msg.compass_variance, + pos_horiz_variance: msg.pos_horiz_variance, + pos_vert_variance: msg.pos_vert_variance, + terrain_alt_variance: msg.terrain_alt_variance, + velocity_variance: msg.velocity_variance, + flags: msg.flags, + } + store.dispatch(setEkfStatusReportData(data)) + window.ipcRenderer.invoke("app:update-ekf-status", data) + break + } } } diff --git a/gcs/src/redux/slices/droneInfoSlice.js b/gcs/src/redux/slices/droneInfoSlice.js index aa9888cb5..c0a1c0571 100644 --- a/gcs/src/redux/slices/droneInfoSlice.js +++ b/gcs/src/redux/slices/droneInfoSlice.js @@ -1,6 +1,10 @@ import { createSelector, createSlice } from "@reduxjs/toolkit" import { defaultDataMessages } from "../../helpers/dashboardDefaultDataMessages" -import { getFlightModeMap, MAV_STATE } from "../../helpers/mavlinkConstants" +import { + getActiveEKFFlags, + getFlightModeMap, + MAV_STATE, +} from "../../helpers/mavlinkConstants" const droneInfoSlice = createSlice({ name: "droneInfo", @@ -53,6 +57,15 @@ const droneInfoSlice = createSlice({ lon: 0, // Stored in coords not int alt: 0, }, + ekfStatusReportData: { + compass_variance: 0, + pos_horiz_variance: 0, + pos_vert_variance: 0, + terrain_alt_variance: 0, + velocity_variance: 0, + flags: 0, + }, + ekfCalculatedStatus: 0, graphs: { selectedGraphs: { graph_a: null, @@ -170,6 +183,35 @@ const droneInfoSlice = createSlice({ setGuidedModePinData: (state, action) => { state.guidedModePinData = action.payload }, + setEkfStatusReportData: (state, action) => { + state.ekfStatusReportData = action.payload + + // Calculate EKF status value ranging from 0-1 + // https://github.com/ArduPilot/MissionPlanner/blob/4d441bd4b1dbc08adce4d8b26e078e93760da3a7/ExtLibs/ArduPilot/CurrentState.cs#L2645-L2647 + const vel = state.ekfStatusReportData.velocity_variance + const comp = state.ekfStatusReportData.compass_variance + const posHor = state.ekfStatusReportData.pos_horiz_variance + const posVer = state.ekfStatusReportData.pos_vert_variance + const terAlt = state.ekfStatusReportData.terrain_alt_variance + state.ekfCalculatedStatus = Math.max(vel, comp, posHor, posVer, terAlt) + + // Check EKF flags to handle critical errors + // https://github.com/ArduPilot/MissionPlanner/blob/4d441bd4b1dbc08adce4d8b26e078e93760da3a7/ExtLibs/ArduPilot/CurrentState.cs#L2674-L2736 + const activeFlags = getActiveEKFFlags(state.ekfStatusReportData.flags) + if (!activeFlags.includes("EKF_ATTITUDE")) { + // If we have no attitude solution + state.ekfCalculatedStatus = 1 + } else if (!activeFlags.includes("EKF_VELOCITY_HORIZ")) { + // If we have GPS but no horizontal velocity solution + const gpsStatus = state.gpsRawIntData.fixType + if (gpsStatus > 0) { + state.ekfCalculatedStatus = 1 + } + } else if (activeFlags.includes("EKF_UNINITIALIZED")) { + // EKF not initialized at all + state.ekfCalculatedStatus = 1 + } + }, }, selectors: { selectAttitude: (state) => state.attitudeData, @@ -203,6 +245,8 @@ const droneInfoSlice = createSlice({ selectStatusText: (state) => state.statusText, selectGraphValues: (state) => state.graphs.selectedGraphs, selectLastGraphMessage: (state) => state.graphs.lastGraphResultsMessage, + selectEkfStatusReportData: (state) => state.ekfStatusReportData, + selectEkfCalculatedStatus: (state) => state.ekfCalculatedStatus, }, }) @@ -225,6 +269,7 @@ export const { setLastGraphMessage, setLoiterRadius, setGuidedModePinData, + setEkfStatusReportData, } = droneInfoSlice.actions // Memoized selectors because redux is a bitch @@ -298,6 +343,8 @@ export const { selectExtraDroneData, selectGraphValues, selectLastGraphMessage, + selectEkfStatusReportData, + selectEkfCalculatedStatus, } = droneInfoSlice.selectors export default droneInfoSlice diff --git a/radio/app/endpoints/states.py b/radio/app/endpoints/states.py index 4aec449cd..861d210fb 100644 --- a/radio/app/endpoints/states.py +++ b/radio/app/endpoints/states.py @@ -46,6 +46,7 @@ def set_state(data: SetStateType) -> None: "RC_CHANNELS", "ESC_TELEMETRY_5_TO_8", "MISSION_CURRENT", + "EKF_STATUS_REPORT", ], "missions": ["GLOBAL_POSITION_INT", "NAV_CONTROLLER_OUTPUT", "HEARTBEAT"], "graphs": ["VFR_HUD", "ATTITUDE", "SYS_STATUS"], diff --git a/radio/tests/test_states.py b/radio/tests/test_states.py index 8e407b40d..c26e8bb0a 100644 --- a/radio/tests/test_states.py +++ b/radio/tests/test_states.py @@ -28,7 +28,7 @@ def test_setState(socketio_client: SocketIOTestClient, droneStatus) -> None: # Success on changing state to dashboard socketio_client.emit("set_state", {"state": "dashboard"}) assert len(socketio_client.get_received()) == 0 - assert len(droneStatus.drone.message_listeners) == 13 + assert len(droneStatus.drone.message_listeners) == 14 droneStatus.drone.message_listeners = {}