From ef9525da7d0d0452934044038d54a9ec077e8f8d Mon Sep 17 00:00:00 2001 From: Tyler Lentz Date: Fri, 3 May 2024 17:45:48 -0700 Subject: [PATCH] add flight mode display --- deployments/broach-docker-compose.yml | 11 ++++--- deployments/docker-compose.yml | 30 +++++++++++++---- houston/src/pages/Control.tsx | 7 ++-- houston/src/pages/Drop.tsx | 16 +++++++-- houston/src/pages/Input.tsx | 3 -- houston/src/utilities/pull_telemetry.ts | 43 +++++++++++++++++++++++++ internal/mavlink/eventframe_handlers.go | 2 +- 7 files changed, 91 insertions(+), 21 deletions(-) diff --git a/deployments/broach-docker-compose.yml b/deployments/broach-docker-compose.yml index 28cebc5c..361c527c 100644 --- a/deployments/broach-docker-compose.yml +++ b/deployments/broach-docker-compose.yml @@ -7,11 +7,11 @@ services: #ports: # - 5000:5000 environment: - - OBC_ADDR=localhost:5010 + - OBC_ADDR=192.168.1.51:5010 - ANTENNA_TRACKER_IP=192.168.1.36 - ANTENNA_TRACKER_PORT=4000 - MAV_DEVICE=serial:/dev/ttyUSB0 - - MAV_OUTPUT1=udp:192.168.1.2:14551 + - MAV_OUTPUT1=udp:192.168.1.3:14551 - MAV_OUTPUT2=udp:192.168.1.7:14551 - MAV_OUTPUT3="tcp:localhost:14551" - MAV_OUTPUT4="" @@ -19,7 +19,7 @@ services: - INFLUXDB_BUCKET=mavlink - INFLUXDB_ORG=TritonUAS - INFLUXDB_TOKEN=influxdbToken - - INFLUXDB_URI=http://influxdb:8086 + - INFLUXDB_URI=http://localhost:8086 - HUB_PATH=/go/src/github.com/tritonuas/gcs volumes: - missions:/go/src/github.com/tritonuas/gcs/missions @@ -31,12 +31,14 @@ services: influxdb: image: influxdb:2.0-alpine + network_mode: "host" ports: - 8086:8086 volumes: - influxdb_data:/var/lib/influxdb grafana: image: grafana/grafana:7.5.5 + network_mode: "host" ports: - 3000:3000 environment: @@ -57,6 +59,7 @@ services: depends_on: - influxdb image: influxdb:2.0-alpine + network_mode: "host" entrypoint: > influx setup --bucket mavlink @@ -64,7 +67,7 @@ services: --token influxdbToken --username tritons --password tritonuas - --host http://influxdb:8086 + --host http://localhost:8086 --force restart: on-failure:20 dozzle: diff --git a/deployments/docker-compose.yml b/deployments/docker-compose.yml index 66063231..8bdf8e8c 100644 --- a/deployments/docker-compose.yml +++ b/deployments/docker-compose.yml @@ -11,7 +11,8 @@ services: - OUT1=--out=tcpin:127.0.0.1:14552 # gcs - OUT2=--out=tcpin:127.0.0.1:14551 - - OUT3= + # qgroundcontrol + - OUT3=--out=tcpin:127.0.0.1:14553 - OUT4= - OUT5= - OUT6= @@ -87,15 +88,30 @@ services: --force restart: on-failure:20 sitl: - image: tritonuas/plane.rascal + image: radarku/ardupilot-sitl network_mode: "host" + # platform: linux/amd64 + tty: true # ports: # - 5760:5760 environment: - - "SITL_HOME=38.31542593549111,-76.55062632801757,8,0" - - SITL_SPEEDUP=1 - volumes: - - sitl:/app/logs + - VEHICLE=ArduPlane + - MODEL=quadplane + - LAT=38.31542593549111 + - LON=-76.55062632801757 + - ALT=8 + - DIR=180 + - SPEEDUP=2 + # sitl: + # image: tritonuas/plane.rascal + # network_mode: "host" + # # ports: + # # - 5760:5760 + # environment: + # - "SITL_HOME=38.31542593549111,-76.55062632801757,8,0" + # - SITL_SPEEDUP=1 + # volumes: + # - sitl:/app/logs dozzle: container_name: dozzle image: amir20/dozzle:latest @@ -106,5 +122,5 @@ services: volumes: grafana_data: missions: - sitl: + # sitl: influxdb_data: diff --git a/houston/src/pages/Control.tsx b/houston/src/pages/Control.tsx index c9c8afa0..684a52ab 100644 --- a/houston/src/pages/Control.tsx +++ b/houston/src/pages/Control.tsx @@ -227,7 +227,6 @@ function Control({settings, planeCoordinates}:{settings: SettingsConfig, planeCo FEET_TO_METERS(settings.minAltitudeAGL_feet) + FEET_TO_METERS(settings.groundAltitude_feet), FEET_TO_METERS(settings.maxAltitudeAGL_feet) + FEET_TO_METERS(settings.groundAltitude_feet), ]; - console.log(altitudeMSLThreshold); const motorBatteryThreshold: Threshold = [ settings.minVoltsPerCell, @@ -279,6 +278,8 @@ function Control({settings, planeCoordinates}:{settings: SettingsConfig, planeCo const [superSecret, setSuperSecret] = useState(false); + const [flightMode, setFlightMode] = useState("???"); + useEffect(() => { const interval = setInterval(() => pullTelemetry( settings, @@ -290,7 +291,8 @@ function Control({settings, planeCoordinates}:{settings: SettingsConfig, planeCo setMotorBattery, setPixhawkBattery, setESCtemperature, - setSuperSecret + setSuperSecret, + setFlightMode ), 1000); return () => { @@ -298,7 +300,6 @@ function Control({settings, planeCoordinates}:{settings: SettingsConfig, planeCo } }, [settings]); - const flightMode = ''; const flightModeColor = { backgroundColor: 'var(--warning-text)' }; const handleClick = (setter: Dispatch>) => { diff --git a/houston/src/pages/Drop.tsx b/houston/src/pages/Drop.tsx index 20088094..0b8b23c7 100644 --- a/houston/src/pages/Drop.tsx +++ b/houston/src/pages/Drop.tsx @@ -1,6 +1,6 @@ import { ChangeEvent, useState } from "react" import { BottleDropIndex, BottleSwap } from "../protos/obc.pb"; - +import video from "../assets/IAMTHEANGRYPUMPKIN.mp4" /** * Page that lets the user perform a manual drop @@ -8,6 +8,7 @@ import { BottleDropIndex, BottleSwap } from "../protos/obc.pb"; */ function Drop() { const [bottle, setBottle] = useState(BottleDropIndex.A); + const [playing, setPlaying] = useState(false); const handleChange = (event: ChangeEvent) => { const value = event.target.value; @@ -37,13 +38,21 @@ function Drop() { const body = { index: bottle } as BottleSwap; + + const video = document.getElementById('pumpkin') as HTMLVideoElement; + video.play(); + setPlaying(true); fetch("/api/plane/dodropnow", { method: "POST", body: JSON.stringify(body) }) .then(resp => resp.text()) - .then(resp => alert(resp)) + .then(resp => { + console.log(resp) + video.play(); + setPlaying(true); + }) } return( @@ -53,9 +62,10 @@ function Drop() { + ); } -export default Drop; \ No newline at end of file +export default Drop; diff --git a/houston/src/pages/Input.tsx b/houston/src/pages/Input.tsx index 6b4b7fab..942fee4a 100644 --- a/houston/src/pages/Input.tsx +++ b/houston/src/pages/Input.tsx @@ -112,7 +112,6 @@ function FormTable( setMapData(mapData => { if (data !== undefined) { const temp = (data.slice(0, i).concat(data.slice(i+1))); - console.log(temp); return new Map(mapData.set(mapMode, temp)); } else { return mapData; // should never happen @@ -547,8 +546,6 @@ function Input() { Waypoints: mapDataToGpsCoords(MapMode.Waypoint), }; - console.log(mission); - fetch("/api/mission", { method: "POST", headers: { diff --git a/houston/src/utilities/pull_telemetry.ts b/houston/src/utilities/pull_telemetry.ts index f64343ad..48107742 100644 --- a/houston/src/utilities/pull_telemetry.ts +++ b/houston/src/utilities/pull_telemetry.ts @@ -15,6 +15,7 @@ import { SettingsConfig } from "./settings"; * @param setPixhawkBatteryVal state setter * @param setESCtemperatureVal state setter * @param setSuperSecret state setter + * @param setFlightMode state setter */ export function pullTelemetry( settings: SettingsConfig, @@ -27,6 +28,7 @@ export function pullTelemetry( setPixhawkBatteryVal: Dispatch>, setESCtemperatureVal: Dispatch>, setSuperSecret: Dispatch>, + setFlightMode: Dispatch>, ) { fetch('/api/plane/telemetry?id=74&field=groundspeed,airspeed,heading') .then(resp => resp.json()) @@ -67,6 +69,47 @@ export function pullTelemetry( .catch(_ => { setESCtemperatureVal(param => param.getErrorValue()); }) + fetch('/api/plane/telemetry?id=0') + .then(resp => resp.json()) + .then(json => { + // some respectable individual can come in later on and refactor this into + // its own function if they so desire... + // reference: https://mavlink.io/en/messages/common.html#MAV_MODE_FLAG + const SAFETY_ARMED: number = 128; + const MANUAL_INPUT_ENABLED: number = 64; + const HIL_ENABLED: number = 32; + const STABILIZE_ENABLED: number = 16; + const GUIDED_ENABLED: number = 8; + const AUTO_ENABLED: number = 4; + // MAV_MODE_FLAG_TEST_ENABLED = 2 + // MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 + + const base_mode = Number(json["base_mode"]) + if ((base_mode & SAFETY_ARMED) != SAFETY_ARMED) { + setFlightMode("Unarmed"); + } else { + // we know the system is armed + + // i dont really know how all of these options combine together. + // i.e. is stablize / HIL also on while we are in auto? + // so i have ordered them from in an order I feel makes sense, reporting + // the most important bits first (manual, auto) then the other ones + // im less sure about how they will be set exactly + if ((base_mode & MANUAL_INPUT_ENABLED) == MANUAL_INPUT_ENABLED) { + setFlightMode("Manual"); + } else if ((base_mode & AUTO_ENABLED) == AUTO_ENABLED) { + setFlightMode("Auto"); + } else if ((base_mode & STABILIZE_ENABLED) == STABILIZE_ENABLED) { + setFlightMode("Stablize"); + } else if ((base_mode & HIL_ENABLED) == HIL_ENABLED) { + setFlightMode("HIL"); + } else if ((base_mode & GUIDED_ENABLED) == GUIDED_ENABLED) { + setFlightMode("Guided"); // i think this will always be on if we are in auto? + } else { + setFlightMode("Armed"); // idk if this ever actually happens + } + } + }) fetch('/api/plane/voltage') .then(resp => resp.json()) .then(json => { diff --git a/internal/mavlink/eventframe_handlers.go b/internal/mavlink/eventframe_handlers.go index 56fa360e..e8718708 100644 --- a/internal/mavlink/eventframe_handlers.go +++ b/internal/mavlink/eventframe_handlers.go @@ -103,7 +103,7 @@ func (c *Client) writeMsgToInfluxDB(evt *gomavlib.EventFrame, node *gomavlib.Nod data["type"] = msg.Type data["autopilot"] = msg.Autopilot - data["base_mode"] = msg.BaseMode + data["base_mode"] = (uint32)(msg.BaseMode) data["custom_mode"] = msg.CustomMode data["system_status"] = msg.SystemStatus data["mavlink_version"] = msg.MavlinkVersion