├── .github ├── ISSUE_TEMPLATE │ ├── bug_report.md │ └── question.md └── workflows │ ├── build_debian.yml │ ├── build_ubuntu.yml │ └── codeql-analysis.yml ├── .gitignore ├── .gitmodules ├── LICENSE ├── Makefile ├── README.md ├── UAVObj.c ├── UAVObj.h ├── appsrc.c ├── docker ├── Dockerfile ├── qemu-binfmt-conf.sh ├── qemu.patch └── src │ └── .gitignore ├── drm_output.c ├── eglstate.h ├── font12x18.h ├── font8x10.h ├── font_outlined8x14.c ├── font_outlined8x14.h ├── font_outlined8x8.c ├── font_outlined8x8.h ├── fonts.c ├── fonts.h ├── fpv_video ├── Makefile ├── Makefile.include ├── fpv_video.c └── fpv_video.sh ├── graphengine.c ├── graphengine.h ├── gst-compat.c ├── m2dlib.c ├── m2dlib.h ├── main.c ├── math3d.c ├── math3d.h ├── oglinit.c ├── osdconfig.c ├── osdconfig.h ├── osdmavlink.c ├── osdmavlink.h ├── osdrender.c ├── osdrender.h ├── osdvar.c ├── osdvar.h ├── px4_custom_mode.h ├── rpi_setup.sh ├── scr1.png ├── scr2.png ├── setup.py ├── stdeb.cfg └── version.py /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: "[BUG]" 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **To Reproduce** 14 | Steps to reproduce the behavior: 15 | 16 | **Expected behavior** 17 | A clear and concise description of what you expected to happen. 18 | 19 | **Screenshots** 20 | If applicable, add screenshots to help explain your problem. 21 | 22 | **Your setup (please complete the following information):** 23 | - OS [e.g. Ubuntu 18.04, Raspbian] 24 | - Hardware [e.g. x86, rpi, nanopi neo2 ] 25 | - WiFi cards [e.g. rt28xx, rtl8812au] 26 | - WiFi drivers [e.g. stock, patched from http://.. ] 27 | 28 | **Additional context** 29 | Add any other context about the problem here. 30 | 31 | **Confirm you read** 32 | - [ ] https://github.com/svpcom/wfb-ng/blob/master/README.md 33 | - [ ] https://github.com/svpcom/wfb-ng/wiki/Setup-HOWTO 34 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/question.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: ⛔ Support Question 3 | about: See https://github.com/svpcom/wfb-ng/discussions for questions about using WFB. 4 | 5 | --- 6 | 7 | We use GitHub issues only to discuss WFB bugs and new features. For 8 | questions about using WFB or related components, please use https://github.com/svpcom/wfb-ng/discussions 9 | 10 | Thanks! 11 | 12 | 13 | -------------------------------------------------------------------------------- /.github/workflows/build_debian.yml: -------------------------------------------------------------------------------- 1 | name: WFB-ng-osd package builder for debian 2 | 3 | on: 4 | push: 5 | branches: [ master, release-25.01 ] 6 | pull_request: 7 | # The branches below must be a subset of the branches above 8 | branches: [ master, stable ] 9 | 10 | jobs: 11 | build_and_test: 12 | runs-on: ubuntu-22.04 13 | strategy: 14 | fail-fast: false 15 | matrix: 16 | version: [11, 12] 17 | arch: [ "armhf.arm32v7.max", "arm64.arm64v8.cortex-a53", "amd64.amd64.max"] 18 | 19 | steps: 20 | - name: Checkout repository 21 | uses: actions/checkout@v4 22 | - name: build package generic 23 | run: | 24 | curl -s -L https://github.com/svpcom/wfb-ng/releases/download/wifibroadcast-17.10/qemu-7.2.15-fixed.tar.gz | sudo tar xzv -C / 25 | git submodule update --init 26 | make deb_docker mode=gst QEMU_CPU=$(echo ${{ matrix.arch }} | cut -f3 -d.) DOCKER_ARCH=$(echo ${{ matrix.arch}} | cut -f1 -d.) DOCKER_SRC_IMAGE=$(echo ${{ matrix.arch }} | cut -f2 -d.)/debian:${{ matrix.version }} 27 | mv deb_dist/*.deb . 28 | make deb_docker mode=rockchip QEMU_CPU=$(echo ${{ matrix.arch }} | cut -f3 -d.) DOCKER_ARCH=$(echo ${{ matrix.arch}} | cut -f1 -d.) DOCKER_SRC_IMAGE=$(echo ${{ matrix.arch }} | cut -f2 -d.)/debian:${{ matrix.version }} 29 | mv deb_dist/*.deb . 30 | - name: build package rpi3 31 | if: matrix.arch == 'armhf.arm32v7.max' 32 | run: | 33 | make deb_docker mode=rpi3 QEMU_CPU=$(echo ${{ matrix.arch }} | cut -f3 -d.) DOCKER_ARCH=$(echo ${{ matrix.arch}} | cut -f1 -d.) DOCKER_SRC_IMAGE=$(echo ${{ matrix.arch }} | cut -f2 -d.)/debian:${{ matrix.version }} 34 | mv deb_dist/*.deb . 35 | - name: Archive production artifacts 36 | uses: actions/upload-artifact@v4 37 | if: github.event_name != 'pull_request' 38 | id: artifact-upload-step 39 | with: 40 | name: wfb-ng-osd-${{ github.ref_name }}-debian${{ matrix.version }}-${{ matrix.arch }} 41 | path: | 42 | *.deb 43 | -------------------------------------------------------------------------------- /.github/workflows/build_ubuntu.yml: -------------------------------------------------------------------------------- 1 | name: WFB-ng-osd package builder for ubuntu 2 | 3 | on: 4 | push: 5 | branches: [ master, release-25.01 ] 6 | pull_request: 7 | # The branches below must be a subset of the branches above 8 | branches: [ master, stable ] 9 | 10 | jobs: 11 | build_and_test: 12 | runs-on: ubuntu-22.04 13 | strategy: 14 | fail-fast: false 15 | matrix: 16 | version: [ "20.04", "22.04", "24.04" ] 17 | arch: [ "armhf.arm32v7.max", "arm64.arm64v8.cortex-a53", "amd64.amd64.max"] 18 | 19 | steps: 20 | - name: Checkout repository 21 | uses: actions/checkout@v4 22 | - name: build package 23 | run: | 24 | curl -s -L https://github.com/svpcom/wfb-ng/releases/download/wifibroadcast-17.10/qemu-7.2.15-fixed.tar.gz | sudo tar xzv -C / 25 | git submodule update --init 26 | make deb_docker mode=gst QEMU_CPU=$(echo ${{ matrix.arch }} | cut -f3 -d.) DOCKER_ARCH=$(echo ${{ matrix.arch}} | cut -f1 -d.) DOCKER_SRC_IMAGE=$(echo ${{ matrix.arch }} | cut -f2 -d.)/ubuntu:${{ matrix.version }} 27 | mv deb_dist/*.deb . 28 | make deb_docker mode=rockchip QEMU_CPU=$(echo ${{ matrix.arch }} | cut -f3 -d.) DOCKER_ARCH=$(echo ${{ matrix.arch}} | cut -f1 -d.) DOCKER_SRC_IMAGE=$(echo ${{ matrix.arch }} | cut -f2 -d.)/ubuntu:${{ matrix.version }} 29 | mv deb_dist/*.deb . 30 | - name: Archive production artifacts 31 | uses: actions/upload-artifact@v4 32 | if: github.event_name != 'pull_request' 33 | id: artifact-upload-step 34 | with: 35 | name: wfb-ng-osd-${{ github.ref_name }}-ubuntu${{ matrix.version }}-${{ matrix.arch }} 36 | path: | 37 | *.deb 38 | -------------------------------------------------------------------------------- /.github/workflows/codeql-analysis.yml: -------------------------------------------------------------------------------- 1 | # For most projects, this workflow file will not need changing; you simply need 2 | # to commit it to your repository. 3 | # 4 | # You may wish to alter this file to override the set of languages analyzed, 5 | # or to provide custom queries or build logic. 6 | # 7 | # ******** NOTE ******** 8 | # We have attempted to detect the languages in your repository. Please check 9 | # the `language` matrix defined below to confirm you have the correct set of 10 | # supported CodeQL languages. 11 | # 12 | name: "CodeQL" 13 | 14 | on: 15 | push: 16 | branches: [ master, stable ] 17 | pull_request: 18 | # The branches below must be a subset of the branches above 19 | branches: [ master, stable ] 20 | schedule: 21 | - cron: '41 12 * * 4' 22 | 23 | jobs: 24 | analyze: 25 | name: Analyze 26 | runs-on: ubuntu-24.04 27 | 28 | strategy: 29 | fail-fast: false 30 | matrix: 31 | language: [ 'cpp' ] 32 | # CodeQL supports [ 'cpp', 'csharp', 'go', 'java', 'javascript', 'python' ] 33 | # Learn more: 34 | # https://docs.github.com/en/free-pro-team@latest/github/finding-security-vulnerabilities-and-errors-in-your-code/configuring-code-scanning#changing-the-languages-that-are-analyzed 35 | 36 | steps: 37 | - name: Checkout repository 38 | uses: actions/checkout@v4 39 | 40 | # Initializes the CodeQL tools for scanning. 41 | - name: Initialize CodeQL 42 | uses: github/codeql-action/init@v3 43 | with: 44 | languages: ${{ matrix.language }} 45 | # If you wish to specify custom queries, you can do so here or in a config file. 46 | # By default, queries listed here will override any specified in a config file. 47 | # Prefix the list here with "+" to use these queries and those in the config file. 48 | # queries: ./path/to/local/query, your-org/your-repo/queries@main 49 | 50 | - run: | 51 | sudo apt update && sudo apt -y install build-essential libgstreamer-plugins-base1.0-dev libgstreamer1.0-dev 52 | git submodule update --init 53 | make osd 54 | 55 | - name: Perform CodeQL Analysis 56 | uses: github/codeql-action/analyze@v3 57 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.o 2 | *.a 3 | osd.gst 4 | osd.rockchip 5 | osd.rpi3 6 | docker/src/ 7 | dist/ 8 | deb_dist/ 9 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "mavlink"] 2 | path = mavlink 3 | url = https://github.com/mavlink/c_library_v2.git 4 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | mode ?= gst 2 | PYTHON ?= python3 3 | SHELL = /bin/bash 4 | ENV ?= $(PWD)/env 5 | OS_CODENAME ?= $(shell lsb_release -cs) 6 | STDEB ?= "git+https://github.com/svpcom/stdeb" 7 | DOCKER_ARCH ?= amd64 8 | DOCKER_SRC_IMAGE ?= "p2ptech/cross-build:2023-02-21-raspios-bullseye-armhf-lite" 9 | QEMU_CPU ?= "max" 10 | 11 | ifneq ("$(wildcard .git)","") 12 | RELEASE ?= $(or $(shell git rev-parse --abbrev-ref HEAD | grep -v '^stable$$'),\ 13 | $(shell git describe --all --match 'release-*' --match 'origin/release-*' --abbrev=0 HEAD 2>/dev/null | grep -o '[^/]*$$'),\ 14 | unknown) 15 | COMMIT ?= $(shell git rev-parse HEAD) 16 | SOURCE_DATE_EPOCH ?= $(or $(shell git show -s --format="%ct" $(COMMIT)), $(shell date "+%s")) 17 | VERSION ?= $(shell $(PYTHON) ./version.py $(SOURCE_DATE_EPOCH) $(RELEASE)) 18 | else 19 | COMMIT ?= release 20 | SOURCE_DATE_EPOCH ?= $(shell date "+%s") 21 | VERSION ?= $(or $(shell basename $(PWD) | grep -E -o '[0-9]+.[0-9]+(.[0-9]+)?$$'), 0.0.0) 22 | endif 23 | 24 | export VERSION COMMIT SOURCE_DATE_EPOCH 25 | export OSD_MODE=$(mode) 26 | 27 | $(ENV): 28 | $(PYTHON) -m virtualenv --download $(ENV) 29 | $$(PATH=$(ENV)/bin:$(ENV)/local/bin:$(PATH) which python3) -m pip install --upgrade pip setuptools $(STDEB) 30 | 31 | CFLAGS += -DWFB_OSD_VERSION='"$(VERSION)-$(shell /bin/bash -c '_tmp=$(COMMIT); echo $${_tmp::8}')"' 32 | 33 | ifeq ($(mode), gst) 34 | CFLAGS += -Wall -pthread -std=gnu99 -D__GST_OPENGL__ -fPIC $(shell pkg-config --cflags glib-2.0) $(shell pkg-config --cflags gstreamer-1.0) 35 | LDFLAGS += $(shell pkg-config --libs glib-2.0) $(shell pkg-config --libs gstreamer-1.0) $(shell pkg-config --libs gstreamer-video-1.0) -lgstapp-1.0 -lpthread -lrt -lm 36 | OBJS = main.o osdrender.o osdmavlink.o graphengine.o UAVObj.o m2dlib.o math3d.o osdconfig.o osdvar.o fonts.o font_outlined8x14.o font_outlined8x8.o appsrc.o gst-compat.o 37 | else ifeq ($(mode), rockchip) 38 | CFLAGS += -Wall -pthread -std=gnu99 -D__DRM_ROCKCHIP__ -fPIC $(shell pkg-config --cflags libdrm) 39 | LDFLAGS += $(shell pkg-config --libs libdrm) -lpthread -lrt -lm 40 | OBJS = main.o osdrender.o osdmavlink.o graphengine.o UAVObj.o m2dlib.o math3d.o osdconfig.o osdvar.o fonts.o font_outlined8x14.o font_outlined8x8.o drm_output.o 41 | else ifeq ($(mode), rpi3) 42 | CFLAGS += -Wall -pthread -std=gnu99 -D__BCM_OPENVG__ -I/opt/vc/include/ -I/opt/vc/include/interface/vcos/pthreads -I/opt/vc/include/interface/vmcs_host/linux 43 | LDFLAGS += -L/opt/vc/lib/ -lbrcmGLESv2 -lbrcmEGL -lopenmaxil -lbcm_host -lvcos -lvchiq_arm -lpthread -lrt -lm 44 | OBJS = main.o osdrender.o osdmavlink.o graphengine.o UAVObj.o m2dlib.o math3d.o osdconfig.o osdvar.o fonts.o font_outlined8x14.o font_outlined8x8.o oglinit.o 45 | else 46 | $(error Valid modes are: gst, rockchip or rpi3) 47 | endif 48 | 49 | all: osd 50 | 51 | osd: osd.$(mode) 52 | 53 | %.o: %.c 54 | $(CC) $(CFLAGS) -c -o $@ $< 55 | 56 | osd.$(mode): $(OBJS) 57 | $(CC) -o $@ $^ $(LDFLAGS) 58 | 59 | deb: osd $(ENV) 60 | rm -rf deb_dist 61 | $$(PATH=$(ENV)/bin:$(ENV)/local/bin:$(PATH) which python3) ./setup.py --command-packages=stdeb.command sdist_dsc --debian-version 0~$(OS_CODENAME) --package3 wfb-ng-osd-$(mode) bdist_deb 62 | rm -rf wfb_ng_osd.egg-info/ wfb-ng-osd-$(VERSION).tar.gz 63 | 64 | deb_docker: /opt/qemu/bin 65 | @if ! [ -d /opt/qemu ]; then echo "Docker cross build requires patched QEMU!\nApply ./docker/qemu.patch to qemu-7.2.0 and build it:\n ./configure --prefix=/opt/qemu --static --disable-system && make && sudo make install"; exit 1; fi 66 | if ! ls /proc/sys/fs/binfmt_misc | grep -q qemu ; then sudo ./docker/qemu-binfmt-conf.sh --qemu-path /opt/qemu/bin --persistent yes; fi 67 | cp -a Makefile docker/src/ 68 | TAG="wfb-ng-osd:build-`date +%s`"; docker build --platform linux/$(DOCKER_ARCH) -t $$TAG docker --build-arg SRC_IMAGE=$(DOCKER_SRC_IMAGE) --build-arg QEMU_CPU=$(QEMU_CPU) && \ 69 | docker run -i --rm --platform linux/$(DOCKER_ARCH) -v $(PWD):/build $$TAG bash -c "trap 'chown -R --reference=. .' EXIT; export VERSION=$(VERSION) COMMIT=$(COMMIT) SOURCE_DATE_EPOCH=$(SOURCE_DATE_EPOCH) && cd /build && make clean && make deb mode=$(mode)" 70 | docker image ls -q "wfb-ng-osd:build-*" | uniq | tail -n+6 | while read i ; do docker rmi -f $$i; done 71 | 72 | osd_docker: deb_docker 73 | 74 | clean: 75 | rm -rf osd.{rockchip,gst,rpi3} deb_dist *.o *~ 76 | make -C fpv_video clean 77 | 78 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | This project started from https://github.com/TobiasBales/PlayuavOSD.git 2 | 3 | Supported platforms: 4 | ------------------- 5 | * Raspberry Pi 0-3 -- use hardware overlay mode (OpenVG) 6 | * Radxa Zero 3W/3E -- use hardware overlay mode (libdrm) 7 | * OrangePi 5 -- use hardware overlay mode (libdrm) 8 | * Any other Linux with X11/Wayland and GPU -- use GStreamer OpenGL mixer 9 | 10 | Supported autopilots: 11 | --------------------- 12 | 13 | * PX4 -- full support 14 | * Ardupilot -- should work, but not tested 15 | * any mavlink-based -- should work with small fixes 16 | 17 | Building: 18 | --------- 19 | 20 | 1. Build for Linux (X11 or Wayland) (native build): 21 | * `apt-get install gstreamer1.0-tools pkg-config libgstreamer1.0-dev gstreamer1.0-plugins-base gstreamer1.0-plugins-bad gstreamer1.0-plugins-good gstreamer1.0-plugins-ugly libgstreamer-plugins-base1.0-dev` 22 | * `make osd` 23 | 24 | 2. Build for Raspberry PI 0-3 (OpenVG) (native build): 25 | * `make osd mode=rpi3` 26 | 27 | 3. Build for Radxa or OrangePi (libdrm) (native build): 28 | * `apt-get install libdrm-dev pkg-config` 29 | * `make osd mode=rockchip` 30 | 31 | Running: 32 | -------- 33 | 34 | Default mavlink port is UDP 14551. 35 | Default RTP video port is UDP 5600. 36 | 37 | * Run `./osd` 38 | * You should got screen like this: 39 | ![gstreamer](scr1.png) 40 | 41 | 42 | Screenshots: 43 | ------------ 44 | ![px4](scr2.png) 45 | 46 | Wiki: [![Ask DeepWiki](https://deepwiki.com/badge.svg)](https://deepwiki.com/svpcom/wfb-ng-osd) 47 | ------------ 48 | -------------------------------------------------------------------------------- /UAVObj.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This program is free software; you can redistribute it and/or modify 3 | * it under the terms of the GNU General Public License as published by 4 | * the Free Software Foundation; either version 3 of the License, or 5 | * (at your option) any later version. 6 | * 7 | * This program is distributed in the hope that it will be useful, but 8 | * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 9 | * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 10 | * for more details. 11 | * 12 | * You should have received a copy of the GNU General Public License along 13 | * with this program; if not, write to the Free Software Foundation, Inc., 14 | * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include "UAVObj.h" 22 | #include "osdconfig.h" 23 | #include "graphengine.h" 24 | #include "osdvar.h" 25 | 26 | 27 | POLYGON2D uav2D; 28 | POLYGON2D rollscale2D; 29 | POLYGON2D simple_attitude; 30 | POLYGON2D home_direction; 31 | POLYGON2D home_direction_outline; 32 | 33 | 34 | void simple_attitude_init(void) { 35 | simple_attitude.state = 1; 36 | simple_attitude.num_verts = 4; 37 | simple_attitude.x0 = osd_params.Atti_mp_posX; 38 | simple_attitude.y0 = osd_params.Atti_mp_posY; 39 | const int line_length = 60; 40 | const int line_spacing = 20; 41 | int x = 0; 42 | int i = 0; 43 | for (int index = 0; index < simple_attitude.num_verts; index += 4) { 44 | x = 12 + i * line_length + i * line_spacing + line_spacing; 45 | VECTOR2D_INITXYZ(&(simple_attitude.vlist_local[index]), -x, 0); 46 | VECTOR2D_INITXYZ(&(simple_attitude.vlist_local[index + 1]), -x - line_length, 0); 47 | VECTOR2D_INITXYZ(&(simple_attitude.vlist_local[index + 2]), x, 0); 48 | VECTOR2D_INITXYZ(&(simple_attitude.vlist_local[index + 3]), x + line_length, 0); 49 | i++; 50 | } 51 | 52 | Scale_Polygon2D(&simple_attitude, atti_mp_scale, atti_mp_scale); 53 | } 54 | 55 | void home_direction_init(void) { 56 | home_direction.state = 1; 57 | home_direction.num_verts = 24; 58 | home_direction.x0 = osd_params.HomeDirection_posX + 10; 59 | home_direction.y0 = osd_params.HomeDirection_posY + 10; 60 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[0]), -2, -2); 61 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[1]), -2, 8); 62 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[2]), -1, -2); 63 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[3]), -1, 8); 64 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[4]), 0, -2); 65 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[5]), 0, 8); 66 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[6]), 1, -2); 67 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[7]), 1, 8); 68 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[8]), 2, -2); 69 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[9]), 2, 8); 70 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[10]), -5, -3); 71 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[11]), 5, -3); 72 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[12]), -4, -4); 73 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[13]), 4, -4); 74 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[14]), -3, -5); 75 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[15]), 3, -5); 76 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[16]), -2, -6); 77 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[17]), 2, -6); 78 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[18]), -1, -7); 79 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[19]), 1, -7); 80 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[20]), 0, -8); 81 | VECTOR2D_INITXYZ(&(home_direction.vlist_local[21]), 0, -8); 82 | 83 | home_direction_outline.state = 1; 84 | home_direction_outline.num_verts = 14; 85 | home_direction_outline.x0 = osd_params.HomeDirection_posX; 86 | home_direction_outline.y0 = osd_params.HomeDirection_posY; 87 | VECTOR2D_INITXYZ(&(home_direction_outline.vlist_local[0]), -7, -2); 88 | VECTOR2D_INITXYZ(&(home_direction_outline.vlist_local[1]), 0, -9); 89 | VECTOR2D_INITXYZ(&(home_direction_outline.vlist_local[2]), 0, -9); 90 | VECTOR2D_INITXYZ(&(home_direction_outline.vlist_local[3]), 7, -2); 91 | VECTOR2D_INITXYZ(&(home_direction_outline.vlist_local[4]), 7, -2); 92 | VECTOR2D_INITXYZ(&(home_direction_outline.vlist_local[5]), 3, -2); 93 | VECTOR2D_INITXYZ(&(home_direction_outline.vlist_local[6]), -7, -2); 94 | VECTOR2D_INITXYZ(&(home_direction_outline.vlist_local[7]), -3, -2); 95 | VECTOR2D_INITXYZ(&(home_direction_outline.vlist_local[8]), 3, -2); 96 | VECTOR2D_INITXYZ(&(home_direction_outline.vlist_local[9]), 3, 9); 97 | VECTOR2D_INITXYZ(&(home_direction_outline.vlist_local[10]), -3, -2); 98 | VECTOR2D_INITXYZ(&(home_direction_outline.vlist_local[11]), -3, 9); 99 | VECTOR2D_INITXYZ(&(home_direction_outline.vlist_local[12]), -3, 9); 100 | VECTOR2D_INITXYZ(&(home_direction_outline.vlist_local[13]), 3, 9); 101 | } 102 | 103 | 104 | void uav2D_init(void) { 105 | // initialize uav2d 106 | uav2D.state = 1; // turn it on 107 | uav2D.num_verts = 38; 108 | uav2D.x0 = osd_params.Atti_mp_posX; // position it 109 | uav2D.y0 = osd_params.Atti_mp_posY; 110 | 111 | int index = 0, i = 0; 112 | const int lX = 5; 113 | const int stepY = 11; 114 | const int hX = 22; 115 | for (index = 0; index < uav2D.num_verts / 2 - 1; ) 116 | { 117 | VECTOR2D_INITXYZ(&(uav2D.vlist_local[index]), -lX, stepY * (i + 1)); 118 | VECTOR2D_INITXYZ(&(uav2D.vlist_local[index + 1]), lX, stepY * (i + 1)); 119 | index += 2; 120 | i++; 121 | } 122 | VECTOR2D_INITXYZ(&(uav2D.vlist_local[index]), -hX, 0); 123 | VECTOR2D_INITXYZ(&(uav2D.vlist_local[index + 1]), hX, 0); 124 | index += 2; 125 | 126 | i = 0; 127 | for (; index < uav2D.num_verts; ) 128 | { 129 | VECTOR2D_INITXYZ(&(uav2D.vlist_local[index]), -lX, -stepY * (i + 1)); 130 | VECTOR2D_INITXYZ(&(uav2D.vlist_local[index + 1]), lX, -stepY * (i + 1)); 131 | index += 2; 132 | i++; 133 | } 134 | 135 | // do a quick scale on the vertices 136 | 137 | Scale_Polygon2D(&uav2D, atti_mp_scale, atti_mp_scale); 138 | 139 | // initialize roll scale 140 | rollscale2D.state = 1; // turn it on 141 | rollscale2D.num_verts = 13; 142 | rollscale2D.x0 = osd_params.Atti_mp_posX; // position it 143 | rollscale2D.y0 = osd_params.Atti_mp_posY; 144 | 145 | 146 | int x, y, theta; 147 | int mp = (rollscale2D.num_verts - 1) / 2; 148 | i = mp; 149 | int radio = 38; 150 | int arcStep = (mp * 10) / 6; 151 | for (index = 0; index < mp; index++) 152 | { 153 | theta = i * arcStep; 154 | x = radio * Fast_Sin(theta); 155 | y = radio * Fast_Cos(theta); 156 | VECTOR2D_INITXYZ(&(rollscale2D.vlist_local[index]), -x, -y); 157 | VECTOR2D_INITXYZ(&(rollscale2D.vlist_local[rollscale2D.num_verts - 1 - index]), x, -y); 158 | i--; 159 | } 160 | 161 | VECTOR2D_INITXYZ(&(rollscale2D.vlist_local[index]), 0, -radio); 162 | Scale_Polygon2D(&rollscale2D, atti_mp_scale, atti_mp_scale); 163 | } 164 | -------------------------------------------------------------------------------- /UAVObj.h: -------------------------------------------------------------------------------- 1 | #ifndef __UAVOBJ_H_ 2 | #define __UAVOBJ_H_ 3 | 4 | #include "m2dlib.h" 5 | 6 | extern POLYGON2D uav2D; 7 | extern POLYGON2D rollscale2D; 8 | extern POLYGON2D simple_attitude; 9 | extern POLYGON2D home_direction; 10 | extern POLYGON2D home_direction_outline; 11 | 12 | void uav2D_init(void); 13 | void simple_attitude_init(void); 14 | void home_direction_init(void); 15 | 16 | #endif //__UAVOBJ_H_ 17 | -------------------------------------------------------------------------------- /appsrc.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Сopyright (C) 2024 Vasily Evseenko 3 | * 4 | * This library is free software; you can redistribute it and/or 5 | * modify it under the terms of the GNU Library General Public 6 | * License as published by the Free Software Foundation; either 7 | * version 2 of the License, or (at your option) any later version. 8 | * 9 | * This library is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | * Library General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Library General Public 15 | * License along with this library; if not, write to the 16 | * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, 17 | * Boston, MA 02110-1301, USA. 18 | */ 19 | 20 | #define _GNU_SOURCE 21 | 22 | // Local camera overlay 23 | #define LOCAL_CAMERA_SUPPORT 0 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "graphengine.h" 35 | 36 | // For gstreamer < 1.18 37 | GstClockTime gst_element_get_current_running_time (GstElement * element); 38 | 39 | 40 | static gboolean 41 | on_message (GstBus * bus, GstMessage * message, gpointer user_data) 42 | { 43 | GMainLoop *loop = (GMainLoop *) user_data; 44 | 45 | switch (GST_MESSAGE_TYPE (message)) { 46 | case GST_MESSAGE_ERROR: 47 | { 48 | GError *err = NULL; 49 | gchar *debug; 50 | 51 | gst_message_parse_error (message, &err, &debug); 52 | g_critical ("Got ERROR: %s (%s)", err->message, GST_STR_NULL (debug)); 53 | g_main_loop_quit (loop); 54 | break; 55 | } 56 | 57 | case GST_MESSAGE_WARNING: 58 | { 59 | GError *err = NULL; 60 | gchar *debug; 61 | 62 | gst_message_parse_warning (message, &err, &debug); 63 | g_warning ("Got WARNING: %s (%s)", err->message, GST_STR_NULL (debug)); 64 | g_main_loop_quit (loop); 65 | break; 66 | } 67 | 68 | case GST_MESSAGE_STATE_CHANGED: 69 | { 70 | GstState old_state, new_state; 71 | 72 | gst_message_parse_state_changed (message, &old_state, &new_state, NULL); 73 | g_print ("gst %s: %s -> %s\n", 74 | GST_OBJECT_NAME (message->src), 75 | gst_element_state_get_name (old_state), 76 | gst_element_state_get_name (new_state)); 77 | break; 78 | } 79 | 80 | case GST_MESSAGE_EOS: 81 | g_main_loop_quit (loop); 82 | break; 83 | 84 | default: 85 | break; 86 | } 87 | 88 | return TRUE; 89 | } 90 | 91 | static void cb_need_data (GstElement *appsrc, guint unused_size, gpointer user_data) 92 | { 93 | GMainLoop *loop = (GMainLoop *) user_data; 94 | 95 | pthread_mutex_lock(&video_mutex); 96 | GstBuffer *buffer = render(); 97 | pthread_mutex_unlock(&video_mutex); 98 | 99 | GST_BUFFER_PTS (buffer) = gst_element_get_current_running_time(appsrc); 100 | 101 | // set to min supported fps, 102 | // but low value will increase latency. 103 | // If fps lower than selected then cpu usage will increase a lot 104 | GST_BUFFER_DURATION (buffer) = gst_util_uint64_scale_int (1, GST_SECOND, 30); 105 | GST_BUFFER_FLAG_SET(buffer, GST_BUFFER_FLAG_LIVE); 106 | GST_BUFFER_FLAG_SET(buffer, GST_BUFFER_FLAG_DROPPABLE); 107 | 108 | //GstFlowReturn ret = gst_app_src_push_buffer(appsrc, buffer); 109 | GstFlowReturn ret; 110 | g_signal_emit_by_name (appsrc, "push-buffer", buffer, &ret); 111 | gst_buffer_unref (buffer); 112 | 113 | 114 | if (ret != GST_FLOW_OK) { 115 | /* something wrong, stop pushing */ 116 | g_main_loop_quit (loop); 117 | } 118 | } 119 | 120 | static const char* select_osd_render(osd_render_t osd_render) 121 | { 122 | switch(osd_render) 123 | { 124 | case OSD_RENDER_XV: 125 | return osd_debug ? \ 126 | "glcolorconvert ! gldownload ! clockoverlay text=XV valignment=center ! xvimagesink" : \ 127 | "glcolorconvert ! gldownload ! xvimagesink"; 128 | 129 | case OSD_RENDER_GL: 130 | return osd_debug ? \ 131 | "clockoverlay text=GL valignment=center ! glimagesink" : \ 132 | "glimagesink"; 133 | 134 | case OSD_RENDER_KMS: 135 | return osd_debug ? \ 136 | "glcolorconvert ! gldownload ! clockoverlay text=Auto valignment=center ! kmssink" : \ 137 | "glcolorconvert ! gldownload ! kmssink"; 138 | 139 | case OSD_RENDER_AUTO: 140 | default: 141 | return osd_debug ? \ 142 | "glcolorconvert ! gldownload ! clockoverlay text=Auto valignment=center ! autovideosink" : \ 143 | "glcolorconvert ! gldownload ! autovideosink"; 144 | } 145 | } 146 | 147 | 148 | int gst_main(int rtp_port, char *codec, int rtp_jitter, osd_render_t osd_render, int screen_width, char *rtsp_src) 149 | { 150 | int screen_height = screen_width * 9 / 16; 151 | 152 | // Fix for intel hd graphics 153 | setenv("GST_GL_PLATFORM", "egl", 0); 154 | 155 | /* init GStreamer */ 156 | gst_init (NULL, NULL); 157 | 158 | GMainLoop *loop = g_main_loop_new (NULL, FALSE); 159 | GstElement *pipeline = NULL; 160 | 161 | /* setup pipeline */ 162 | { 163 | char *pipeline_str = NULL; 164 | char *src_str = NULL; 165 | GError *error = NULL; 166 | 167 | if(rtsp_src != NULL) 168 | { 169 | asprintf(&src_str, 170 | "rtspsrc latency=%d location=\"%s\"", rtp_jitter, rtsp_src); 171 | } else { 172 | asprintf(&src_str, 173 | "udpsrc port=%d caps=\"application/x-rtp,media=(string)video, clock-rate=(int)90000, encoding-name=(string)H%s\"", 174 | rtp_port, codec + 1); 175 | } 176 | 177 | char *codecs[] = {"nv%sdec", "v4l2%sdec", "avdec_%s"}; 178 | char *decoder = NULL; 179 | 180 | for(int i = 0; i < sizeof(codecs) / sizeof(codecs[0]); i++) 181 | { 182 | char *buf = NULL; 183 | asprintf(&buf, codecs[i], codec); 184 | GstElement *tmp = gst_element_factory_make(buf, "decoder"); 185 | 186 | if(tmp != NULL) 187 | { 188 | gst_object_unref(tmp); 189 | decoder = buf; 190 | break; 191 | } 192 | 193 | free(buf); 194 | } 195 | 196 | if(decoder == NULL) 197 | { 198 | fprintf(stderr, "No decoder for %s was found\n", codec); 199 | exit(1); 200 | } 201 | 202 | asprintf(&pipeline_str, 203 | "%s ! " 204 | "rtp%sdepay ! " 205 | "%sparse config-interval=1 disable-passthrough=true ! " 206 | "%s qos=false ! " 207 | "queue leaky=downstream max-size-buffers=1 max-size-bytes=0 ! " 208 | "glupload ! glcolorconvert ! " 209 | "glvideomixerelement emit-signals=true start-time-selection=1 name=osd_mixer " 210 | "sink_0::emit-signals=true sink_0::width=%d sink_0::height=%d sink_0::zorder=-2 " 211 | "sink_1::emit-signals=true sink_1::width=%d sink_1::height=%d sink_1::zorder=0 " 212 | #if LOCAL_CAMERA_SUPPORT 213 | "sink_2::emit-signals=true sink_2::width=640 sink_2::height=360 sink_2::zorder=-1 " 214 | #endif 215 | "! %s sync=true " 216 | "appsrc name=osd_src stream-type=0 format=time min-latency=0 ! " 217 | "video/x-raw,format=RGBA,width=%d,height=%d,framerate=0/1 ! glupload ! glcolorconvert ! osd_mixer. " 218 | #if LOCAL_CAMERA_SUPPORT 219 | "v4l2src device=/dev/video2 ! video/x-raw,width=640,height=360,framerate=30/1 ! queue ! glupload ! glcolorconvert ! osd_mixer." 220 | #endif 221 | , 222 | src_str, codec, codec, decoder, 223 | screen_width, screen_height, screen_width, screen_height, 224 | select_osd_render(osd_render), 225 | GRAPHICS_WIDTH, GRAPHICS_HEIGHT); 226 | 227 | free(src_str); 228 | free(decoder); 229 | 230 | printf("GST pipeline: %s\n", pipeline_str); 231 | 232 | pipeline = gst_parse_launch(pipeline_str, &error); 233 | free(pipeline_str); 234 | 235 | if(error != NULL) 236 | { 237 | fprintf (stderr, "GST Error: %s\n", error->message); 238 | g_error_free (error); 239 | exit(1); 240 | } 241 | } 242 | 243 | g_assert(pipeline); 244 | 245 | /* setup */ 246 | GstElement *appsrc = gst_bin_get_by_name(GST_BIN(pipeline), "osd_src"); 247 | g_signal_connect (appsrc, "need-data", G_CALLBACK (cb_need_data), loop); 248 | 249 | // Set message handler 250 | { 251 | GstBus *bus = gst_pipeline_get_bus (GST_PIPELINE (pipeline)); 252 | gst_bus_add_signal_watch (bus); 253 | g_signal_connect (G_OBJECT (bus), "message", G_CALLBACK (on_message), loop); 254 | gst_object_unref (GST_OBJECT (bus)); 255 | } 256 | 257 | // main loop 258 | gst_element_set_state (pipeline, GST_STATE_PLAYING); 259 | g_main_loop_run (loop); 260 | gst_element_set_state (pipeline, GST_STATE_NULL); 261 | 262 | gst_object_unref (pipeline); 263 | 264 | return 0; 265 | } 266 | -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | ARG SRC_IMAGE 2 | FROM $SRC_IMAGE 3 | 4 | ARG QEMU_CPU 5 | ENV QEMU_CPU=$QEMU_CPU 6 | 7 | RUN apt-get update && \ 8 | DEBIAN_FRONTEND=noninteractive \ 9 | apt-get install -y lsb-release build-essential git wget \ 10 | python3-all python3-all-dev python3-setuptools \ 11 | virtualenv dh-python fakeroot debhelper \ 12 | libgstreamer-plugins-base1.0-dev libgstreamer1.0-dev \ 13 | libdrm-dev pkg-config 14 | 15 | RUN git clone -b oldstable --depth 1 https://github.com/raspberrypi/firmware /tmp/firmware && mv /tmp/firmware/opt/vc /opt && rm -rf /tmp/firmware 16 | 17 | COPY src/Makefile /tmp 18 | ENV ENV=/opt/env 19 | ENV PYTHON=python3 20 | RUN cd /tmp && make $ENV 21 | -------------------------------------------------------------------------------- /docker/qemu-binfmt-conf.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # Enable automatic program execution by the kernel. 3 | 4 | qemu_target_list="i386 i486 alpha arm armeb sparc sparc32plus sparc64 \ 5 | ppc ppc64 ppc64le m68k mips mipsel mipsn32 mipsn32el mips64 mips64el \ 6 | sh4 sh4eb s390x aarch64 aarch64_be hppa riscv32 riscv64 xtensa xtensaeb \ 7 | microblaze microblazeel or1k x86_64 hexagon loongarch64" 8 | 9 | i386_magic='\x7fELF\x01\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x03\x00' 10 | i386_mask='\xff\xff\xff\xff\xff\xfe\xfe\x00\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff' 11 | i386_family=i386 12 | 13 | i486_magic='\x7fELF\x01\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x06\x00' 14 | i486_mask='\xff\xff\xff\xff\xff\xfe\xfe\x00\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff' 15 | i486_family=i386 16 | 17 | x86_64_magic='\x7fELF\x02\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x3e\x00' 18 | x86_64_mask='\xff\xff\xff\xff\xff\xfe\xfe\x00\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff' 19 | x86_64_family=i386 20 | 21 | alpha_magic='\x7fELF\x02\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x26\x90' 22 | alpha_mask='\xff\xff\xff\xff\xff\xfe\xfe\x00\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff' 23 | alpha_family=alpha 24 | 25 | arm_magic='\x7fELF\x01\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x28\x00' 26 | arm_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff' 27 | arm_family=arm 28 | 29 | armeb_magic='\x7fELF\x01\x02\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x28' 30 | armeb_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff' 31 | armeb_family=armeb 32 | 33 | sparc_magic='\x7fELF\x01\x02\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x02' 34 | sparc_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff' 35 | sparc_family=sparc 36 | 37 | sparc32plus_magic='\x7fELF\x01\x02\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x12' 38 | sparc32plus_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff' 39 | sparc32plus_family=sparc 40 | 41 | sparc64_magic='\x7fELF\x02\x02\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x2b' 42 | sparc64_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff' 43 | sparc64_family=sparc 44 | 45 | ppc_magic='\x7fELF\x01\x02\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x14' 46 | ppc_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff' 47 | ppc_family=ppc 48 | 49 | ppc64_magic='\x7fELF\x02\x02\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x15' 50 | ppc64_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff' 51 | ppc64_family=ppc 52 | 53 | ppc64le_magic='\x7fELF\x02\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x15\x00' 54 | ppc64le_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\x00' 55 | ppc64le_family=ppcle 56 | 57 | m68k_magic='\x7fELF\x01\x02\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x04' 58 | m68k_mask='\xff\xff\xff\xff\xff\xff\xfe\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff' 59 | m68k_family=m68k 60 | 61 | # FIXME: We could use the other endianness on a MIPS host. 62 | 63 | mips_magic='\x7fELF\x01\x02\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x08\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00' 64 | mips_mask='\xff\xff\xff\xff\xff\xff\xff\x00\x00\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff\xff\xff\xff\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x20' 65 | mips_family=mips 66 | 67 | mipsel_magic='\x7fELF\x01\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x08\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00' 68 | mipsel_mask='\xff\xff\xff\xff\xff\xff\xff\x00\x00\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff\xff\xff\xff\xff\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x20\x00\x00\x00' 69 | mipsel_family=mips 70 | 71 | mipsn32_magic='\x7fELF\x01\x02\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x08\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x20' 72 | mipsn32_mask='\xff\xff\xff\xff\xff\xff\xff\x00\x00\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff\xff\xff\xff\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x20' 73 | mipsn32_family=mips 74 | 75 | mipsn32el_magic='\x7fELF\x01\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x08\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x20\x00\x00\x00' 76 | mipsn32el_mask='\xff\xff\xff\xff\xff\xff\xff\x00\x00\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff\xff\xff\xff\xff\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x20\x00\x00\x00' 77 | mipsn32el_family=mips 78 | 79 | mips64_magic='\x7fELF\x02\x02\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x08' 80 | mips64_mask='\xff\xff\xff\xff\xff\xff\xff\x00\x00\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff' 81 | mips64_family=mips 82 | 83 | mips64el_magic='\x7fELF\x02\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x08\x00' 84 | mips64el_mask='\xff\xff\xff\xff\xff\xff\xff\x00\x00\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff' 85 | mips64el_family=mips 86 | 87 | sh4_magic='\x7fELF\x01\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x2a\x00' 88 | sh4_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff' 89 | sh4_family=sh4 90 | 91 | sh4eb_magic='\x7fELF\x01\x02\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x2a' 92 | sh4eb_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff' 93 | sh4eb_family=sh4 94 | 95 | s390x_magic='\x7fELF\x02\x02\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x16' 96 | s390x_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff' 97 | s390x_family=s390x 98 | 99 | aarch64_magic='\x7fELF\x02\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\xb7\x00' 100 | aarch64_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff' 101 | aarch64_family=arm 102 | 103 | aarch64_be_magic='\x7fELF\x02\x02\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\xb7' 104 | aarch64_be_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff' 105 | aarch64_be_family=armeb 106 | 107 | hppa_magic='\x7f\x45\x4c\x46\x01\x02\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x0f' 108 | hppa_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff' 109 | hppa_family=hppa 110 | 111 | riscv32_magic='\x7fELF\x01\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\xf3\x00' 112 | riscv32_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff' 113 | riscv32_family=riscv 114 | 115 | riscv64_magic='\x7fELF\x02\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\xf3\x00' 116 | riscv64_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff' 117 | riscv64_family=riscv 118 | 119 | xtensa_magic='\x7fELF\x01\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x5e\x00' 120 | xtensa_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff' 121 | xtensa_family=xtensa 122 | 123 | xtensaeb_magic='\x7fELF\x01\x02\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x5e' 124 | xtensaeb_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff' 125 | xtensaeb_family=xtensaeb 126 | 127 | microblaze_magic='\x7fELF\x01\x02\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\xba\xab' 128 | microblaze_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff' 129 | microblaze_family=microblaze 130 | 131 | microblazeel_magic='\x7fELF\x01\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\xab\xba' 132 | microblazeel_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff' 133 | microblazeel_family=microblazeel 134 | 135 | or1k_magic='\x7fELF\x01\x02\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x5c' 136 | or1k_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff' 137 | or1k_family=or1k 138 | 139 | hexagon_magic='\x7fELF\x01\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\xa4\x00' 140 | hexagon_mask='\xff\xff\xff\xff\xff\xff\xff\x00\xff\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff' 141 | hexagon_family=hexagon 142 | 143 | loongarch64_magic='\x7fELF\x02\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x02\x00\x02\x01' 144 | loongarch64_mask='\xff\xff\xff\xff\xff\xff\xff\xfc\x00\xff\xff\xff\xff\xff\xff\xff\xfe\xff\xff\xff' 145 | loongarch64_family=loongarch 146 | 147 | qemu_get_family() { 148 | cpu=${HOST_ARCH:-$(uname -m)} 149 | case "$cpu" in 150 | amd64|i386|i486|i586|i686|i86pc|BePC|x86_64) 151 | echo "i386" 152 | ;; 153 | mips*) 154 | echo "mips" 155 | ;; 156 | "Power Macintosh"|ppc64|powerpc|ppc) 157 | echo "ppc" 158 | ;; 159 | ppc64el|ppc64le) 160 | echo "ppcle" 161 | ;; 162 | arm|armel|armhf|arm64|armv[4-9]*l|aarch64) 163 | echo "arm" 164 | ;; 165 | armeb|armv[4-9]*b|aarch64_be) 166 | echo "armeb" 167 | ;; 168 | sparc*) 169 | echo "sparc" 170 | ;; 171 | riscv*) 172 | echo "riscv" 173 | ;; 174 | loongarch*) 175 | echo "loongarch" 176 | ;; 177 | *) 178 | echo "$cpu" 179 | ;; 180 | esac 181 | } 182 | 183 | usage() { 184 | cat <&2 233 | exit 1 234 | fi 235 | } 236 | 237 | qemu_check_bintfmt_misc() { 238 | # load the binfmt_misc module 239 | if [ ! -d /proc/sys/fs/binfmt_misc ]; then 240 | if ! /sbin/modprobe binfmt_misc ; then 241 | exit 1 242 | fi 243 | fi 244 | if [ ! -f /proc/sys/fs/binfmt_misc/register ]; then 245 | if ! mount binfmt_misc -t binfmt_misc /proc/sys/fs/binfmt_misc ; then 246 | exit 1 247 | fi 248 | fi 249 | 250 | qemu_check_access /proc/sys/fs/binfmt_misc/register 251 | } 252 | 253 | installed_dpkg() { 254 | dpkg --status "$1" > /dev/null 2>&1 255 | } 256 | 257 | qemu_check_debian() { 258 | if [ ! -e /etc/debian_version ] ; then 259 | echo "WARNING: your system is not a Debian based distro" 1>&2 260 | elif ! installed_dpkg binfmt-support ; then 261 | echo "WARNING: package binfmt-support is needed" 1>&2 262 | fi 263 | qemu_check_access "$EXPORTDIR" 264 | } 265 | 266 | qemu_check_systemd() { 267 | if ! systemctl -q is-enabled systemd-binfmt.service ; then 268 | echo "WARNING: systemd-binfmt.service is missing or disabled" 1>&2 269 | fi 270 | qemu_check_access "$EXPORTDIR" 271 | } 272 | 273 | qemu_generate_register() { 274 | flags="" 275 | if [ "$CREDENTIAL" = "yes" ] ; then 276 | flags="OC" 277 | fi 278 | if [ "$PERSISTENT" = "yes" ] ; then 279 | flags="${flags}F" 280 | fi 281 | if [ "$PRESERVE_ARG0" = "yes" ] ; then 282 | flags="${flags}P" 283 | fi 284 | 285 | echo ":qemu-$cpu:M::$magic:$mask:$qemu:$flags" 286 | } 287 | 288 | qemu_register_interpreter() { 289 | echo "Setting $qemu as binfmt interpreter for $cpu" 290 | qemu_generate_register > /proc/sys/fs/binfmt_misc/register 291 | } 292 | 293 | qemu_generate_systemd() { 294 | echo "Setting $qemu as binfmt interpreter for $cpu for systemd-binfmt.service" 295 | qemu_generate_register > "$EXPORTDIR/qemu-$cpu.conf" 296 | } 297 | 298 | qemu_generate_debian() { 299 | cat > "$EXPORTDIR/qemu-$cpu" <&2 323 | continue 324 | fi 325 | 326 | qemu="$QEMU_PATH/qemu-$cpu" 327 | if [ "$cpu" = "i486" ] ; then 328 | qemu="$QEMU_PATH/qemu-i386" 329 | fi 330 | 331 | qemu="$qemu$QEMU_SUFFIX" 332 | if [ "$host_family" != "$family" ] ; then 333 | $BINFMT_SET 334 | fi 335 | done 336 | } 337 | 338 | CHECK=qemu_check_bintfmt_misc 339 | BINFMT_SET=qemu_register_interpreter 340 | 341 | SYSTEMDDIR="/etc/binfmt.d" 342 | DEBIANDIR="/usr/share/binfmts" 343 | 344 | QEMU_PATH=/usr/local/bin 345 | CREDENTIAL=no 346 | PERSISTENT=no 347 | PRESERVE_ARG0=no 348 | QEMU_SUFFIX="" 349 | 350 | _longopts="debian,systemd:,qemu-path:,qemu-suffix:,exportdir:,help,credential:,\ 351 | persistent:,preserve-argv0:" 352 | options=$(getopt -o ds:Q:S:e:hc:p:g:F: -l ${_longopts} -- "$@") 353 | eval set -- "$options" 354 | 355 | while true ; do 356 | case "$1" in 357 | -d|--debian) 358 | CHECK=qemu_check_debian 359 | BINFMT_SET=qemu_generate_debian 360 | EXPORTDIR=${EXPORTDIR:-$DEBIANDIR} 361 | ;; 362 | -s|--systemd) 363 | CHECK=qemu_check_systemd 364 | BINFMT_SET=qemu_generate_systemd 365 | EXPORTDIR=${EXPORTDIR:-$SYSTEMDDIR} 366 | shift 367 | # check given cpu is in the supported CPU list 368 | if [ "$1" != "ALL" ] ; then 369 | for cpu in ${qemu_target_list} ; do 370 | if [ "$cpu" = "$1" ] ; then 371 | break 372 | fi 373 | done 374 | 375 | if [ "$cpu" = "$1" ] ; then 376 | qemu_target_list="$1" 377 | else 378 | echo "ERROR: unknown CPU \"$1\"" 1>&2 379 | usage 380 | exit 1 381 | fi 382 | fi 383 | ;; 384 | -Q|--qemu-path) 385 | shift 386 | QEMU_PATH="$1" 387 | ;; 388 | -F|--qemu-suffix) 389 | shift 390 | QEMU_SUFFIX="$1" 391 | ;; 392 | -e|--exportdir) 393 | shift 394 | EXPORTDIR="$1" 395 | ;; 396 | -h|--help) 397 | usage 398 | exit 1 399 | ;; 400 | -c|--credential) 401 | shift 402 | CREDENTIAL="$1" 403 | ;; 404 | -p|--persistent) 405 | shift 406 | PERSISTENT="$1" 407 | ;; 408 | -g|--preserve-argv0) 409 | shift 410 | PRESERVE_ARG0="$1" 411 | ;; 412 | *) 413 | break 414 | ;; 415 | esac 416 | shift 417 | done 418 | 419 | $CHECK 420 | qemu_set_binfmts 421 | -------------------------------------------------------------------------------- /docker/qemu.patch: -------------------------------------------------------------------------------- 1 | diff -urw qemu-7.2.0/linux-user/generic/sockbits.h qemu-7.2.0.new/linux-user/generic/sockbits.h 2 | --- qemu-7.2.0/linux-user/generic/sockbits.h 2022-12-14 19:28:44.000000000 +0300 3 | +++ qemu-7.2.0.new/linux-user/generic/sockbits.h 2023-03-26 09:28:12.461589808 +0300 4 | @@ -58,4 +58,6 @@ 5 | 6 | #define TARGET_SO_PROTOCOL 38 7 | #define TARGET_SO_DOMAIN 39 8 | +#define TARGET_SO_RXQ_OVFL 40 9 | + 10 | #endif 11 | diff -urw qemu-7.2.0/linux-user/mips/sockbits.h qemu-7.2.0.new/linux-user/mips/sockbits.h 12 | --- qemu-7.2.0/linux-user/mips/sockbits.h 2022-12-14 19:28:44.000000000 +0300 13 | +++ qemu-7.2.0.new/linux-user/mips/sockbits.h 2023-03-26 09:42:20.146917902 +0300 14 | @@ -70,6 +70,7 @@ 15 | #define TARGET_SO_SNDBUFFORCE 31 16 | #define TARGET_SO_RCVBUFFORCE 33 17 | #define TARGET_SO_PASSSEC 34 18 | +#define TARGET_SO_RXQ_OVFL 40 19 | 20 | /** sock_type - Socket types 21 | * 22 | diff -urw qemu-7.2.0/linux-user/strace.c qemu-7.2.0.new/linux-user/strace.c 23 | --- qemu-7.2.0/linux-user/strace.c 2022-12-14 19:28:45.000000000 +0300 24 | +++ qemu-7.2.0.new/linux-user/strace.c 2023-03-26 09:53:40.414754941 +0300 25 | @@ -2737,6 +2737,9 @@ 26 | case TARGET_SO_REUSEPORT: 27 | qemu_log("SO_REUSEPORT,"); 28 | goto print_optint; 29 | + case TARGET_SO_RXQ_OVFL: 30 | + qemu_log("SO_RXQ_OVFL,"); 31 | + goto print_optint; 32 | case TARGET_SO_TYPE: 33 | qemu_log("SO_TYPE,"); 34 | goto print_optint; 35 | diff -urw qemu-7.2.0/linux-user/syscall.c qemu-7.2.0.new/linux-user/syscall.c 36 | --- qemu-7.2.0/linux-user/syscall.c 2022-12-14 19:28:45.000000000 +0300 37 | +++ qemu-7.2.0.new/linux-user/syscall.c 2023-03-26 14:16:24.811649762 +0300 38 | @@ -2477,6 +2477,9 @@ 39 | optname = SO_REUSEPORT; 40 | break; 41 | #endif 42 | + case TARGET_SO_RXQ_OVFL: 43 | + optname = SO_RXQ_OVFL; 44 | + break; 45 | case TARGET_SO_TYPE: 46 | optname = SO_TYPE; 47 | break; 48 | -------------------------------------------------------------------------------- /docker/src/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/svpcom/wfb-ng-osd/0170afc3fd6b59768c794ca5acca30e51353ad43/docker/src/.gitignore -------------------------------------------------------------------------------- /eglstate.h: -------------------------------------------------------------------------------- 1 | typedef struct { 2 | uint32_t screen_width; 3 | uint32_t screen_height; 4 | // OpenGL|ES objects 5 | EGLDisplay display; 6 | 7 | EGLSurface surface; 8 | EGLContext context; 9 | } STATE_T; 10 | 11 | extern void oglinit(STATE_T *); 12 | -------------------------------------------------------------------------------- /font_outlined8x14.c: -------------------------------------------------------------------------------- 1 | /** 2 | * Super OSD, software revision 3 3 | * Copyright (C) 2010 Thomas Oldbury 4 | * 5 | * Please note the font data here is covered under Creative 6 | * Commons licenses (version 3.0 BY-SA) and *not* the GPL. 7 | */ 8 | 9 | // This data is generated by a Python script from 10 | // the original font .pngs. 11 | 12 | // 0xff = indicates character not present 13 | // any other byte contains the index of the character. 14 | const char font_lookup_outlined8x14[256] = { 15 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 16 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 17 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 18 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 19 | 0x00, 0x01, 0x02, 0x03, 0xff, 0x04, 0x05, 0x06, 20 | 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 21 | 0x0f, 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 22 | 0x17, 0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 23 | 0x1f, 0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 24 | 0x27, 0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 25 | 0x2f, 0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 26 | 0x37, 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 27 | 0x3f, 0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 28 | 0x47, 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, 29 | 0x4f, 0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 30 | 0x57, 0x58, 0x59, 0x5a, 0x5b, 0x5c, 0x5d, 0xff, 31 | 0x5e, 0x5f, 0x60, 0x61, 0x62, 0x63, 0x64, 0x65, 32 | 0x66, 0x67, 0x68, 0x69, 0x6a, 0x6b, 0x6c, 0xff, 33 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 34 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 35 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 36 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 37 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 38 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 39 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 40 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 41 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 42 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 43 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 44 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 45 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 46 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff 47 | }; 48 | 49 | const char font_data_outlined8x14[] = { 50 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 51 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x20 52 | 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 53 | 0x00, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, // 0x21 54 | 0x66, 0x66, 0x66, 0x66, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 55 | 0x00, 0x24, 0x24, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x22 56 | 0x7e, 0xff, 0xff, 0xff, 0x7e, 0xff, 0xff, 0xff, 0x7e, 0xff, 0xff, 0xff, 0x7e, 0x00, 57 | 0x00, 0x24, 0x7e, 0x24, 0x24, 0x24, 0x7e, 0x24, 0x24, 0x24, 0x7e, 0x24, 0x00, 0x00, // 0x23 58 | 0x00, 0x70, 0x70, 0x72, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0x4e, 0x0e, 0x0e, 0x00, 59 | 0x00, 0x00, 0x20, 0x00, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x00, 0x04, 0x00, 0x00, // 0x25 60 | 0x38, 0x3c, 0x7c, 0x7c, 0x7c, 0xfc, 0xee, 0xef, 0xef, 0xff, 0xff, 0x7e, 0x00, 0x00, 61 | 0x00, 0x18, 0x28, 0x10, 0x28, 0x28, 0x44, 0x44, 0x46, 0x44, 0x3a, 0x00, 0x00, 0x00, // 0x26 62 | 0x38, 0x38, 0x38, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 63 | 0x00, 0x10, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x27 64 | 0x1e, 0x3e, 0x7e, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x7e, 0x3e, 0x1e, 0x00, 65 | 0x00, 0x1c, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x1c, 0x00, 0x00, // 0x28 66 | 0x78, 0x7c, 0x7e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x7e, 0x7c, 0x78, 0x00, 67 | 0x00, 0x38, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x38, 0x00, 0x00, // 0x29 68 | 0x38, 0x7c, 0xfe, 0xfe, 0xfe, 0xee, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 69 | 0x00, 0x10, 0x7c, 0x28, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x2a 70 | 0x00, 0x00, 0x00, 0x38, 0x38, 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x00, 0x00, 0x00, 0x00, 71 | 0x00, 0x00, 0x00, 0x00, 0x10, 0x10, 0x7c, 0x10, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x2b 72 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x38, 0x00, 73 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x10, 0x00, 0x00, // 0x2c 74 | 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 75 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x2d 76 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 77 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, // 0x2e 78 | 0x00, 0x07, 0x07, 0x0f, 0x0e, 0x1c, 0x1c, 0x38, 0x38, 0x70, 0xf0, 0xe0, 0xe0, 0x00, 79 | 0x00, 0x00, 0x02, 0x04, 0x04, 0x08, 0x08, 0x10, 0x10, 0x20, 0x20, 0x40, 0x00, 0x00, // 0x2f 80 | 0x7e, 0xff, 0xff, 0xe7, 0xef, 0xff, 0xff, 0xf7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, 81 | 0x00, 0x3c, 0x42, 0x42, 0x46, 0x4a, 0x52, 0x62, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x30 82 | 0x3c, 0x7c, 0xfc, 0xfc, 0x5c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0xff, 0xff, 0xff, 0x00, 83 | 0x00, 0x38, 0x68, 0x48, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x7e, 0x00, 0x00, // 0x31 84 | 0x3e, 0x7f, 0xff, 0xe7, 0x47, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0xff, 0xff, 0xff, 0x00, 85 | 0x00, 0x3c, 0x62, 0x42, 0x02, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x7e, 0x00, 0x00, // 0x32 86 | 0xfe, 0xff, 0xff, 0x07, 0x07, 0x7f, 0x7e, 0x7f, 0x07, 0x07, 0xff, 0xff, 0xfe, 0x00, 87 | 0x00, 0x7c, 0x02, 0x02, 0x02, 0x04, 0x38, 0x04, 0x02, 0x02, 0x02, 0x7c, 0x00, 0x00, // 0x33 88 | 0x04, 0x0e, 0x1e, 0x3e, 0x7e, 0xff, 0xff, 0xff, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x00, 89 | 0x00, 0x04, 0x0c, 0x14, 0x24, 0x44, 0x7e, 0x04, 0x04, 0x04, 0x04, 0x04, 0x00, 0x00, // 0x34 90 | 0xff, 0xff, 0xff, 0xe0, 0xfc, 0xfe, 0xff, 0x0f, 0x07, 0x0f, 0xff, 0xfe, 0xfc, 0x00, 91 | 0x00, 0x7e, 0x40, 0x40, 0x40, 0x78, 0x04, 0x02, 0x02, 0x02, 0x04, 0x78, 0x00, 0x00, // 0x35 92 | 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, 93 | 0x00, 0x3c, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x36 94 | 0xff, 0xff, 0xff, 0x07, 0x07, 0x0e, 0x0e, 0x1c, 0x1c, 0x38, 0x38, 0x70, 0x70, 0x00, 95 | 0x00, 0x7e, 0x02, 0x02, 0x02, 0x04, 0x04, 0x08, 0x08, 0x10, 0x10, 0x20, 0x00, 0x00, // 0x37 96 | 0x3c, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0x7e, 0x3c, 0x00, 97 | 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x38 98 | 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x07, 0x7f, 0x7f, 0x7f, 0x00, 99 | 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3e, 0x02, 0x02, 0x02, 0x02, 0x3c, 0x00, 0x00, // 0x39 100 | 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, 101 | 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, // 0x3a 102 | 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x38, 0x00, 103 | 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x10, 0x00, 0x00, // 0x3b 104 | 0x00, 0x00, 0x04, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0x70, 0x38, 0x1c, 0x0e, 0x04, 0x00, 105 | 0x00, 0x00, 0x00, 0x04, 0x08, 0x10, 0x20, 0x40, 0x20, 0x10, 0x08, 0x04, 0x00, 0x00, // 0x3c 106 | 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x00, 0x7e, 0x7e, 0x7e, 0x00, 0x00, 0x00, 0x00, 107 | 0x00, 0x00, 0x00, 0x00, 0x3c, 0x00, 0x00, 0x00, 0x3c, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x3d 108 | 0x00, 0x00, 0x20, 0x70, 0x38, 0x1c, 0x0e, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0x20, 0x00, 109 | 0x00, 0x00, 0x00, 0x20, 0x10, 0x08, 0x04, 0x02, 0x04, 0x08, 0x10, 0x20, 0x00, 0x00, // 0x3e 110 | 0x3c, 0xfe, 0xff, 0xe7, 0xe7, 0x0e, 0x1c, 0x38, 0x38, 0x00, 0x38, 0x38, 0x38, 0x00, 111 | 0x00, 0x3c, 0x42, 0x42, 0x02, 0x04, 0x08, 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, // 0x3f 112 | 0x18, 0x3c, 0x7e, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x7c, 0x3e, 0x1e, 0x00, 113 | 0x00, 0x18, 0x24, 0x42, 0x42, 0x42, 0x4a, 0x52, 0x52, 0x4c, 0x20, 0x1e, 0x00, 0x00, // 0x40 114 | 0x7e, 0xff, 0xff, 0xff, 0xe7, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, 115 | 0x00, 0x3c, 0x66, 0x42, 0x42, 0x42, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x41 116 | 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0x00, 117 | 0x00, 0x7c, 0x46, 0x42, 0x42, 0x42, 0x7c, 0x42, 0x42, 0x42, 0x46, 0x7c, 0x00, 0x00, // 0x42 118 | 0x7f, 0xff, 0xff, 0xf7, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xf7, 0xff, 0xff, 0x7f, 0x00, 119 | 0x00, 0x3e, 0x62, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x62, 0x3e, 0x00, 0x00, // 0x43 120 | 0xfe, 0xff, 0xff, 0xef, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xef, 0xff, 0xff, 0xfe, 0x00, 121 | 0x00, 0x7c, 0x46, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x46, 0x7c, 0x00, 0x00, // 0x44 122 | 0xff, 0xff, 0xff, 0xe0, 0xe0, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xff, 0xff, 0xff, 0x00, 123 | 0x00, 0x7e, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x7e, 0x00, 0x00, // 0x45 124 | 0xff, 0xff, 0xff, 0xe0, 0xe0, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, 125 | 0x00, 0x7e, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x46 126 | 0x7f, 0xff, 0xff, 0xf0, 0xe0, 0xef, 0xef, 0xef, 0xe7, 0xf7, 0xff, 0xff, 0x7f, 0x00, 127 | 0x00, 0x3e, 0x60, 0x40, 0x40, 0x40, 0x46, 0x42, 0x42, 0x42, 0x62, 0x3e, 0x00, 0x00, // 0x47 128 | 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, 129 | 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x48 130 | 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0xfe, 0xfe, 0xfe, 0x00, 131 | 0x00, 0x7c, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x7c, 0x00, 0x00, // 0x49 132 | 0xff, 0xff, 0xff, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x3c, 0xfc, 0xfc, 0xf8, 0x00, 133 | 0x00, 0x7e, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x18, 0x70, 0x00, 0x00, // 0x4a 134 | 0xe7, 0xef, 0xff, 0xfe, 0xfc, 0xf8, 0xf0, 0xf8, 0xfc, 0xfe, 0xff, 0xef, 0xe7, 0x00, 135 | 0x00, 0x42, 0x44, 0x48, 0x50, 0x60, 0x40, 0x60, 0x50, 0x48, 0x44, 0x42, 0x00, 0x00, // 0x4b 136 | 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xff, 0xff, 0xff, 0x00, 137 | 0x00, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x7e, 0x00, 0x00, // 0x4c 138 | 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, 139 | 0x00, 0x42, 0x66, 0x5a, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x4d 140 | 0xe7, 0xf7, 0xff, 0xff, 0xff, 0xff, 0xef, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, 141 | 0x00, 0x42, 0x62, 0x52, 0x4a, 0x46, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x4e 142 | 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, 143 | 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x4f 144 | 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, 145 | 0x00, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x7e, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x50 146 | 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x07, 0x00, 147 | 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x02, 0x02, 0x00, 0x00, // 0x51 148 | 0xfe, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0xf8, 0xfc, 0xee, 0xe7, 0xe3, 0x00, 149 | 0x00, 0x7e, 0x42, 0x42, 0x42, 0x42, 0x7e, 0x60, 0x50, 0x48, 0x44, 0x42, 0x00, 0x00, // 0x52 150 | 0xff, 0xff, 0xff, 0xe0, 0xf0, 0xfe, 0xff, 0x7f, 0x0f, 0x07, 0xff, 0xff, 0xff, 0x00, 151 | 0x00, 0x7e, 0x40, 0x40, 0x40, 0x60, 0x3c, 0x06, 0x02, 0x02, 0x02, 0x7e, 0x00, 0x00, // 0x53 152 | 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, 153 | 0x00, 0x7c, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x54 154 | 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0x7e, 0x3c, 0x00, 155 | 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x55 156 | 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xfe, 0x7e, 0x7e, 0x7e, 0x3c, 0x3c, 0x00, 157 | 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x24, 0x24, 0x24, 0x18, 0x18, 0x00, 0x00, // 0x56 158 | 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0x00, 159 | 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x5a, 0x66, 0x42, 0x00, 0x00, // 0x57 160 | 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0x7f, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0x00, 161 | 0x00, 0x42, 0x42, 0x42, 0x66, 0x3c, 0x18, 0x3c, 0x66, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x58 162 | 0xee, 0xee, 0xfe, 0xfe, 0x7c, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, 163 | 0x00, 0x44, 0x44, 0x28, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x59 164 | 0xff, 0xff, 0xff, 0x07, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0xff, 0xff, 0xff, 0x00, 165 | 0x00, 0x7e, 0x02, 0x02, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x40, 0x7e, 0x00, 0x00, // 0x5a 166 | 0x7e, 0x7e, 0x7e, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x7e, 0x7e, 0x7e, 0x00, 167 | 0x00, 0x3c, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x3c, 0x00, 0x00, // 0x5b 168 | 0x00, 0xe0, 0xe0, 0xf0, 0x70, 0x38, 0x38, 0x1c, 0x1c, 0x0e, 0x0f, 0x07, 0x07, 0x00, 169 | 0x00, 0x00, 0x40, 0x20, 0x20, 0x10, 0x10, 0x08, 0x08, 0x04, 0x04, 0x02, 0x00, 0x00, // 0x5c 170 | 0x7e, 0x7e, 0x7e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x7e, 0x7e, 0x7e, 0x00, 171 | 0x00, 0x3c, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x3c, 0x00, 0x00, // 0x5d 172 | 0x3c, 0x7e, 0x7e, 0x7e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 173 | 0x00, 0x18, 0x24, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x5e 174 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x00, 175 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x00, 0x00, // 0x5f 176 | 0x30, 0x38, 0x1c, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 177 | 0x00, 0x10, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x60 178 | 0x00, 0x00, 0x00, 0x00, 0x7c, 0xfc, 0xfe, 0xee, 0xfe, 0xfe, 0xff, 0xff, 0xff, 0x00, 179 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x38, 0x44, 0x04, 0x04, 0x3c, 0x44, 0x7a, 0x00, 0x00, // 0x61 180 | 0x00, 0xe0, 0xe0, 0xe0, 0xe0, 0xfe, 0xff, 0xff, 0xf7, 0xe7, 0xff, 0xff, 0x7e, 0x00, 181 | 0x00, 0x00, 0x40, 0x40, 0x40, 0x40, 0x5c, 0x62, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x62 182 | 0x00, 0x00, 0x00, 0x00, 0x3f, 0x7f, 0xff, 0xf0, 0xe0, 0xf0, 0xff, 0x7f, 0x3f, 0x00, 183 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x1e, 0x20, 0x40, 0x40, 0x40, 0x20, 0x1e, 0x00, 0x00, // 0x63 184 | 0x00, 0x07, 0x07, 0x07, 0x07, 0x7f, 0xff, 0xff, 0xef, 0xe7, 0xff, 0xff, 0x7e, 0x00, 185 | 0x00, 0x00, 0x02, 0x02, 0x02, 0x02, 0x3a, 0x46, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x64 186 | 0x00, 0x00, 0x00, 0x00, 0x7e, 0xff, 0xff, 0xe7, 0xff, 0xff, 0xff, 0xff, 0x7f, 0x00, 187 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x42, 0x42, 0x42, 0x5c, 0x40, 0x3e, 0x00, 0x00, // 0x65 188 | 0x3f, 0x7f, 0xff, 0xf0, 0xe0, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, 189 | 0x00, 0x1e, 0x20, 0x40, 0x40, 0x40, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x66 190 | 0x7c, 0xfe, 0xff, 0xef, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x0f, 0xff, 0xfe, 0xfc, 0x00, 191 | 0x00, 0x38, 0x44, 0x42, 0x42, 0x42, 0x3e, 0x02, 0x02, 0x02, 0x04, 0x78, 0x00, 0x00, // 0x67 192 | 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0xfc, 0xfe, 0xff, 0xef, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, 193 | 0x00, 0x40, 0x40, 0x40, 0x40, 0x40, 0x78, 0x44, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x68 194 | 0x1c, 0x1c, 0x1c, 0x00, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x00, 195 | 0x00, 0x08, 0x00, 0x00, 0x00, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x00, 0x00, // 0x69 196 | 0x1c, 0x1c, 0x1c, 0x00, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x3c, 0xfc, 0xfc, 0xf0, 0x00, 197 | 0x00, 0x08, 0x00, 0x00, 0x00, 0x08, 0x08, 0x08, 0x08, 0x08, 0x10, 0x60, 0x00, 0x00, // 0x6a 198 | 0x00, 0xe2, 0xe7, 0xee, 0xfc, 0xf8, 0xf0, 0xf0, 0xf8, 0xfc, 0xee, 0xe7, 0xe2, 0x00, 199 | 0x00, 0x00, 0x42, 0x44, 0x48, 0x50, 0x60, 0x60, 0x50, 0x48, 0x44, 0x42, 0x00, 0x00, // 0x6b 200 | 0xf0, 0xf0, 0xf0, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x78, 0x7f, 0x3f, 0x1f, 0x00, 201 | 0x00, 0x60, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x10, 0x0e, 0x00, 0x00, // 0x6c 202 | 0x00, 0x00, 0x00, 0x00, 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, 203 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x66, 0x5a, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x6d 204 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0x00, 205 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x42, 0x00, 0x00, // 0x6e 206 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0xff, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, 207 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x6f 208 | 0x3c, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0xfe, 0xe0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, 209 | 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x7c, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x70 210 | 0x3c, 0x7e, 0xff, 0xe7, 0xe7, 0xff, 0xff, 0x7f, 0x07, 0x07, 0x07, 0x07, 0x07, 0x00, 211 | 0x00, 0x3c, 0x42, 0x42, 0x42, 0x42, 0x3e, 0x02, 0x02, 0x02, 0x03, 0x02, 0x00, 0x00, // 0x71 212 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x7f, 0xff, 0xf0, 0xe0, 0xe0, 0xe0, 0xe0, 0x00, 213 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1e, 0x20, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, // 0x72 214 | 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xfe, 0xff, 0x7f, 0xff, 0xff, 0xff, 0x00, 215 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x40, 0x40, 0x3c, 0x02, 0x02, 0x7e, 0x00, 0x00, // 0x73 216 | 0x00, 0xe0, 0xe0, 0xe0, 0xe0, 0xfc, 0xfc, 0xfc, 0xe0, 0xf0, 0xfe, 0xfe, 0x7e, 0x00, 217 | 0x00, 0x00, 0x40, 0x40, 0x40, 0x40, 0x78, 0x40, 0x40, 0x40, 0x60, 0x3c, 0x00, 0x00, // 0x74 218 | 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x00, 219 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x3c, 0x00, 0x00, // 0x75 220 | 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0x3c, 0x00, 221 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x42, 0x42, 0x42, 0x24, 0x18, 0x00, 0x00, // 0x76 222 | 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0xff, 0xe7, 0x00, 223 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x42, 0x42, 0x5a, 0x66, 0x42, 0x00, 0x00, // 0x77 224 | 0x00, 0x00, 0x00, 0x00, 0xe7, 0xe7, 0xff, 0xff, 0x7e, 0xff, 0xe7, 0xe7, 0xe7, 0x00, 225 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x42, 0x24, 0x18, 0x24, 0x42, 0x42, 0x00, 0x00, // 0x78 226 | 0xe7, 0xe7, 0xe7, 0xe7, 0xff, 0xff, 0xff, 0x07, 0x07, 0x0f, 0xff, 0xfe, 0xfc, 0x00, 227 | 0x00, 0x42, 0x42, 0x42, 0x42, 0x7c, 0x02, 0x02, 0x02, 0x02, 0x04, 0x78, 0x00, 0x00, // 0x79 228 | 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x0e, 0x1c, 0x38, 0x70, 0xff, 0xff, 0xff, 0x00, 229 | 0x00, 0x00, 0x00, 0x00, 0x7e, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x7e, 0x00, 0x00, // 0x7a 230 | 0x00, 0x3f, 0x7f, 0x7f, 0x70, 0xf0, 0xf0, 0xf0, 0xf0, 0x70, 0x7f, 0x7f, 0x3f, 0x00, 231 | 0x00, 0x00, 0x1e, 0x20, 0x20, 0x20, 0x40, 0x40, 0x20, 0x20, 0x20, 0x1e, 0x00, 0x00, // 0x7b 232 | 0x00, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, 233 | 0x00, 0x00, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x7c 234 | 0x00, 0xfc, 0xfe, 0xfe, 0x0e, 0x0f, 0x0f, 0x0f, 0x0f, 0x0e, 0xfe, 0xfe, 0xfc, 0x00, 235 | 0x00, 0x00, 0x78, 0x04, 0x04, 0x04, 0x02, 0x02, 0x04, 0x04, 0x04, 0x78, 0x00, 0x00, // 0x7d 236 | 0x00, 0x10, 0x3a, 0x7f, 0x2e, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 237 | 0x00, 0x00, 0x10, 0x2a, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x7e 238 | 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, 239 | 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x80 240 | 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, 241 | 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x81 242 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, 243 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x82 244 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, 245 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x83 246 | 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, 247 | 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x84 248 | 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, 249 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x85 250 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, 251 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x86 252 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, 253 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x87 254 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, 255 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x3c, 0x00, 0x00, // 0x88 256 | 0x00, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x7e, 0x7e, 0x3c, 0x00, 257 | 0x00, 0x00, 0x18, 0x08, 0x18, 0x08, 0x18, 0x08, 0x18, 0x08, 0x3c, 0x3c, 0x00, 0x00, // 0x89 258 | 0x00, 0xff, 0xff, 0xff, 0xff, 0x1f, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 259 | 0x00, 0x00, 0x6e, 0x68, 0x08, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x8a 260 | 0x00, 0xff, 0xff, 0xff, 0xff, 0x1f, 0x1f, 0x1f, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x00, 261 | 0x00, 0x00, 0x69, 0x6a, 0x0c, 0x0c, 0x0a, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x8b 262 | 0x00, 0xff, 0xff, 0xff, 0xff, 0x1f, 0x1f, 0x1c, 0x1c, 0x1c, 0x00, 0x00, 0x00, 0x00, 263 | 0x00, 0x00, 0x6e, 0x68, 0x08, 0x0e, 0x08, 0x08, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x8c 264 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x7c, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x7e, 0x00, 265 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x38, 0x3c, 0x3c, 0x3c, 0x3c, 0x3c, 0x00, 0x00, // 0x8d 266 | 0x00, 0x00, 0x00, 0x3e, 0x7f, 0x7f, 0x7f, 0x7f, 0x7f, 0x3e, 0x00, 0x00, 0x00, 0x00, 267 | 0x00, 0x00, 0x00, 0x00, 0x36, 0x36, 0x32, 0x3e, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x00 // 0x8e 268 | }; 269 | -------------------------------------------------------------------------------- /font_outlined8x14.h: -------------------------------------------------------------------------------- 1 | #ifndef FONT_OUTLINE8X14_H__ 2 | #define FONT_OUTLINE8X14_H__ 3 | 4 | /** 5 | * Super OSD, software revision 3 6 | * Copyright (C) 2010 Thomas Oldbury 7 | * 8 | * Please note the font data here is covered under Creative 9 | * Commons licenses (version 3.0 BY-SA) and *not* the GPL. 10 | */ 11 | 12 | // Lookup table. 13 | extern const char font_lookup_outlined8x14[256]; 14 | // Data. 15 | extern const char font_data_outlined8x14[]; 16 | 17 | #endif //FONT_OUTLINE8X14_H__ 18 | -------------------------------------------------------------------------------- /font_outlined8x8.c: -------------------------------------------------------------------------------- 1 | /** 2 | * Super OSD, software revision 3 3 | * Copyright (C) 2010 Thomas Oldbury 4 | * 5 | * Please note the font data here is covered under Creative 6 | * Commons licenses (version 3.0 BY-SA) and *not* the GPL. 7 | */ 8 | 9 | // This data is generated by a Python script from 10 | // the original font .pngs. 11 | 12 | // 0xff = indicates character not present 13 | // any other byte contains the index of the character. 14 | const char font_lookup_outlined8x8[256] = { 15 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 16 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 17 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 18 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 19 | 0x00, 0x01, 0x02, 0x03, 0xff, 0xff, 0xff, 0x04, 20 | 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 21 | 0x0d, 0x0e, 0x0f, 0x10, 0x11, 0x12, 0x13, 0x14, 22 | 0x15, 0x16, 0x17, 0xff, 0x18, 0x19, 0x1a, 0x1b, 23 | 0xff, 0x1c, 0x1d, 0x1e, 0x1f, 0x20, 0x21, 0x22, 24 | 0x23, 0x24, 0x25, 0x26, 0x27, 0x28, 0x29, 0x2a, 25 | 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 0x30, 0x31, 0x32, 26 | 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0xff, 0x39, 27 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 28 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 29 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 30 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 31 | 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f, 0x40, 0x41, 32 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 33 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 34 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 35 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 36 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 37 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 38 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 39 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 40 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 41 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 42 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 43 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 44 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 45 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 46 | 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff 47 | }; 48 | 49 | const char font_data_outlined8x8[] = { 50 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 51 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x20 52 | 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, 53 | 0x00, 0x10, 0x10, 0x10, 0x00, 0x10, 0x00, 0x00, // 0x21 54 | 0x44, 0xee, 0xee, 0x44, 0x00, 0x00, 0x00, 0x00, 55 | 0x00, 0x44, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x22 56 | 0x28, 0x7c, 0xfe, 0x7c, 0xfe, 0x7c, 0x28, 0x00, 57 | 0x00, 0x28, 0x7c, 0x28, 0x7c, 0x28, 0x00, 0x00, // 0x23 58 | 0x10, 0x38, 0x38, 0x10, 0x00, 0x00, 0x00, 0x00, 59 | 0x00, 0x10, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, // 0x27 60 | 0x10, 0x38, 0x70, 0x70, 0x70, 0x38, 0x10, 0x00, 61 | 0x00, 0x10, 0x20, 0x20, 0x20, 0x10, 0x00, 0x00, // 0x28 62 | 0x20, 0x70, 0x38, 0x38, 0x38, 0x70, 0x20, 0x00, 63 | 0x00, 0x20, 0x10, 0x10, 0x10, 0x20, 0x00, 0x00, // 0x29 64 | 0x28, 0x7c, 0x38, 0x7c, 0x28, 0x00, 0x00, 0x00, 65 | 0x00, 0x28, 0x10, 0x28, 0x00, 0x00, 0x00, 0x00, // 0x2a 66 | 0x38, 0x38, 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x00, 67 | 0x00, 0x10, 0x10, 0x7c, 0x10, 0x10, 0x00, 0x00, // 0x2b 68 | 0x00, 0x00, 0x78, 0x78, 0x78, 0x78, 0x38, 0x00, 69 | 0x00, 0x00, 0x00, 0x30, 0x30, 0x10, 0x00, 0x00, // 0x2c 70 | 0x00, 0x00, 0xfe, 0xfe, 0xfe, 0x00, 0x00, 0x00, 71 | 0x00, 0x00, 0x00, 0x7c, 0x00, 0x00, 0x00, 0x00, // 0x2d 72 | 0x00, 0x00, 0x00, 0x78, 0x78, 0x78, 0x78, 0x00, 73 | 0x00, 0x00, 0x00, 0x00, 0x30, 0x30, 0x00, 0x00, // 0x2e 74 | 0x04, 0x0e, 0x1c, 0x38, 0x70, 0xe0, 0x40, 0x00, 75 | 0x00, 0x04, 0x08, 0x10, 0x20, 0x40, 0x00, 0x00, // 0x2f 76 | 0x7c, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x7c, 0x00, 77 | 0x00, 0x38, 0x44, 0x54, 0x44, 0x38, 0x00, 0x00, // 0x30 78 | 0x3c, 0x7c, 0xfc, 0xfc, 0xfe, 0xfe, 0xfe, 0x00, 79 | 0x00, 0x18, 0x28, 0x48, 0x08, 0x7c, 0x00, 0x00, // 0x31 80 | 0xfc, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x00, 81 | 0x00, 0x78, 0x04, 0x78, 0x40, 0x7c, 0x00, 0x00, // 0x32 82 | 0xfe, 0xfe, 0xfe, 0x7e, 0xfe, 0xfe, 0xfe, 0x00, 83 | 0x00, 0x7c, 0x04, 0x3c, 0x04, 0x7c, 0x00, 0x00, // 0x33 84 | 0xee, 0xee, 0xfe, 0xfe, 0x7e, 0x0e, 0x0e, 0x00, 85 | 0x00, 0x44, 0x44, 0x3c, 0x04, 0x04, 0x00, 0x00, // 0x34 86 | 0xfc, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0x00, 87 | 0x00, 0x7c, 0x40, 0x78, 0x04, 0x78, 0x00, 0x00, // 0x35 88 | 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x00, 89 | 0x00, 0x7c, 0x40, 0x7c, 0x44, 0x7c, 0x00, 0x00, // 0x36 90 | 0xfe, 0xfe, 0xfe, 0x38, 0x70, 0xe0, 0xc0, 0x00, 91 | 0x00, 0x7c, 0x08, 0x10, 0x20, 0x40, 0x00, 0x00, // 0x37 92 | 0x7c, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x7c, 0x00, 93 | 0x00, 0x38, 0x44, 0x7c, 0x44, 0x38, 0x00, 0x00, // 0x38 94 | 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x7e, 0x7e, 0x00, 95 | 0x00, 0x7c, 0x44, 0x7c, 0x04, 0x3c, 0x00, 0x00, // 0x39 96 | 0x78, 0x78, 0x78, 0x78, 0x78, 0x78, 0x78, 0x00, 97 | 0x00, 0x30, 0x30, 0x00, 0x30, 0x30, 0x00, 0x00, // 0x3a 98 | 0x08, 0x1c, 0x38, 0x70, 0x38, 0x1c, 0x08, 0x00, 99 | 0x00, 0x08, 0x10, 0x20, 0x10, 0x08, 0x00, 0x00, // 0x3c 100 | 0xfe, 0xfe, 0xfe, 0x00, 0xfe, 0xfe, 0xfe, 0x00, 101 | 0x00, 0x7c, 0x00, 0x00, 0x00, 0x7c, 0x00, 0x00, // 0x3d 102 | 0x20, 0x70, 0x38, 0x1c, 0x38, 0x70, 0x20, 0x00, 103 | 0x00, 0x20, 0x10, 0x08, 0x10, 0x20, 0x00, 0x00, // 0x3e 104 | 0x7c, 0xfe, 0xfe, 0xfe, 0x3c, 0x38, 0x38, 0x00, 105 | 0x00, 0x38, 0x44, 0x18, 0x00, 0x10, 0x00, 0x00, // 0x3f 106 | 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xee, 0xee, 0x00, 107 | 0x00, 0x7c, 0x44, 0x7c, 0x44, 0x44, 0x00, 0x00, // 0x41 108 | 0xfc, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0x00, 109 | 0x00, 0x78, 0x44, 0x7c, 0x44, 0x78, 0x00, 0x00, // 0x42 110 | 0xfe, 0xfe, 0xfe, 0xe0, 0xfe, 0xfe, 0xfe, 0x00, 111 | 0x00, 0x7c, 0x40, 0x40, 0x40, 0x7c, 0x00, 0x00, // 0x43 112 | 0xfc, 0xfe, 0xfe, 0xee, 0xfe, 0xfe, 0xfc, 0x00, 113 | 0x00, 0x78, 0x44, 0x44, 0x44, 0x78, 0x00, 0x00, // 0x44 114 | 0xfe, 0xfe, 0xfe, 0xfc, 0xfe, 0xfe, 0xfe, 0x00, 115 | 0x00, 0x7c, 0x40, 0x78, 0x40, 0x7c, 0x00, 0x00, // 0x45 116 | 0xfe, 0xfe, 0xfe, 0xfc, 0xfc, 0xe0, 0xe0, 0x00, 117 | 0x00, 0x7c, 0x40, 0x78, 0x40, 0x40, 0x00, 0x00, // 0x46 118 | 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x00, 119 | 0x00, 0x7c, 0x40, 0x5c, 0x44, 0x7c, 0x00, 0x00, // 0x47 120 | 0xee, 0xee, 0xfe, 0xfe, 0xfe, 0xee, 0xee, 0x00, 121 | 0x00, 0x44, 0x44, 0x7c, 0x44, 0x44, 0x00, 0x00, // 0x48 122 | 0xfe, 0xfe, 0xfe, 0x38, 0xfe, 0xfe, 0xfe, 0x00, 123 | 0x00, 0x7c, 0x10, 0x10, 0x10, 0x7c, 0x00, 0x00, // 0x49 124 | 0xfe, 0xfe, 0xfe, 0x1c, 0xfc, 0xfc, 0xfc, 0x00, 125 | 0x00, 0x7c, 0x08, 0x08, 0x08, 0x78, 0x00, 0x00, // 0x4a 126 | 0xe6, 0xee, 0xfc, 0xf8, 0xfc, 0xee, 0xe6, 0x00, 127 | 0x00, 0x44, 0x48, 0x70, 0x48, 0x44, 0x00, 0x00, // 0x4b 128 | 0xe0, 0xe0, 0xe0, 0xe0, 0xfe, 0xfe, 0xfe, 0x00, 129 | 0x00, 0x40, 0x40, 0x40, 0x40, 0x7c, 0x00, 0x00, // 0x4c 130 | 0x6c, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xee, 0x00, 131 | 0x00, 0x28, 0x54, 0x54, 0x54, 0x44, 0x00, 0x00, // 0x4d 132 | 0xee, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xee, 0x00, 133 | 0x00, 0x44, 0x64, 0x54, 0x4c, 0x44, 0x00, 0x00, // 0x4e 134 | 0xfe, 0xfe, 0xfe, 0xee, 0xfe, 0xfe, 0xfe, 0x00, 135 | 0x00, 0x7c, 0x44, 0x44, 0x44, 0x7c, 0x00, 0x00, // 0x4f 136 | 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xe0, 0xe0, 0x00, 137 | 0x00, 0x7c, 0x44, 0x7c, 0x40, 0x40, 0x00, 0x00, // 0x50 138 | 0xfe, 0xfe, 0xfe, 0xee, 0xfc, 0xfe, 0xf6, 0x00, 139 | 0x00, 0x7c, 0x44, 0x44, 0x48, 0x74, 0x00, 0x00, // 0x51 140 | 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0xe8, 0x00, 141 | 0x00, 0x7c, 0x44, 0x7c, 0x50, 0x48, 0x00, 0x00, // 0x52 142 | 0x7e, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, 0x00, 143 | 0x00, 0x3c, 0x40, 0x7c, 0x04, 0x78, 0x00, 0x00, // 0x53 144 | 0xfe, 0xfe, 0xfe, 0x38, 0x38, 0x38, 0x38, 0x00, 145 | 0x00, 0x7c, 0x10, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x54 146 | 0xee, 0xee, 0xee, 0xee, 0xfe, 0xfe, 0x7c, 0x00, 147 | 0x00, 0x44, 0x44, 0x44, 0x44, 0x38, 0x00, 0x00, // 0x55 148 | 0xee, 0xee, 0xee, 0xfe, 0xfe, 0x7c, 0x38, 0x00, 149 | 0x00, 0x44, 0x44, 0x44, 0x28, 0x10, 0x00, 0x00, // 0x56 150 | 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0x6c, 0x00, 151 | 0x00, 0x54, 0x54, 0x54, 0x54, 0x28, 0x00, 0x00, // 0x57 152 | 0xee, 0xfe, 0xfe, 0x7c, 0xfe, 0xfe, 0xee, 0x00, 153 | 0x00, 0x44, 0x28, 0x10, 0x28, 0x44, 0x00, 0x00, // 0x58 154 | 0xee, 0xfe, 0xfe, 0x7c, 0x38, 0x38, 0x38, 0x00, 155 | 0x00, 0x44, 0x28, 0x10, 0x10, 0x10, 0x00, 0x00, // 0x59 156 | 0xfe, 0xfe, 0xfe, 0x78, 0xfe, 0xfe, 0xfe, 0x00, 157 | 0x00, 0x7c, 0x08, 0x10, 0x20, 0x7c, 0x00, 0x00, // 0x5a 158 | 0x38, 0x7c, 0x78, 0x70, 0x78, 0x7c, 0x38, 0x00, 159 | 0x00, 0x38, 0x20, 0x20, 0x20, 0x38, 0x00, 0x00, // 0x5b 160 | 0x40, 0xe0, 0x70, 0x38, 0x1c, 0x0e, 0x04, 0x00, 161 | 0x00, 0x40, 0x20, 0x10, 0x08, 0x04, 0x00, 0x00, // 0x5c 162 | 0x38, 0x7c, 0x3c, 0x1c, 0x3c, 0x7c, 0x38, 0x00, 163 | 0x00, 0x38, 0x08, 0x08, 0x08, 0x38, 0x00, 0x00, // 0x5d 164 | 0x00, 0x00, 0x00, 0x00, 0xfe, 0xfe, 0xfe, 0x00, 165 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x7c, 0x00, 0x00, // 0x5f 166 | 0x10, 0x38, 0x7c, 0xfe, 0xfe, 0x38, 0x38, 0x00, 167 | 0x00, 0x10, 0x38, 0x54, 0x10, 0x10, 0x00, 0x00, // 0x80 168 | 0x3e, 0x3e, 0x1e, 0x3e, 0x76, 0xe0, 0x40, 0x00, 169 | 0x00, 0x1c, 0x0c, 0x14, 0x20, 0x40, 0x00, 0x00, // 0x81 170 | 0x10, 0x38, 0xfc, 0xfe, 0xfc, 0x38, 0x10, 0x00, 171 | 0x00, 0x10, 0x08, 0x7c, 0x08, 0x10, 0x00, 0x00, // 0x82 172 | 0x40, 0xe0, 0x76, 0x3e, 0x1e, 0x3e, 0x3e, 0x00, 173 | 0x00, 0x40, 0x20, 0x14, 0x0c, 0x1c, 0x00, 0x00, // 0x83 174 | 0x38, 0x38, 0xfe, 0xfe, 0x7c, 0x38, 0x10, 0x00, 175 | 0x00, 0x10, 0x10, 0x54, 0x38, 0x10, 0x00, 0x00, // 0x84 176 | 0x04, 0x0e, 0xdc, 0xf8, 0xf0, 0xf8, 0xf8, 0x00, 177 | 0x00, 0x04, 0x08, 0x50, 0x60, 0x70, 0x00, 0x00, // 0x85 178 | 0x10, 0x38, 0x7e, 0xfe, 0x7e, 0x38, 0x10, 0x00, 179 | 0x00, 0x10, 0x20, 0x7c, 0x20, 0x10, 0x00, 0x00, // 0x86 180 | 0xf8, 0xf8, 0xf0, 0xf8, 0xdc, 0x0e, 0x04, 0x00, 181 | 0x00, 0x70, 0x60, 0x50, 0x08, 0x04, 0x00, 0x00 // 0x87 182 | }; 183 | -------------------------------------------------------------------------------- /font_outlined8x8.h: -------------------------------------------------------------------------------- 1 | #ifndef FONT_OUTLINE8X8_H__ 2 | #define FONT_OUTLINE8X8_H__ 3 | 4 | /** 5 | * Super OSD, software revision 3 6 | * Copyright (C) 2010 Thomas Oldbury 7 | * 8 | * Please note the font data here is covered under Creative 9 | * Commons licenses (version 3.0 BY-SA) and *not* the GPL. 10 | */ 11 | 12 | // Lookup table. 13 | extern const char font_lookup_outlined8x8[256]; 14 | // Data. 15 | extern const char font_data_outlined8x8[]; 16 | 17 | #endif //FONT_OUTLINE8X8_H__ 18 | -------------------------------------------------------------------------------- /fonts.c: -------------------------------------------------------------------------------- 1 | /** 2 | * Super OSD, software revision 3 3 | * Copyright (C) 2010 Thomas Oldbury 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 2 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, write to the Free Software Foundation, Inc., 17 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 18 | */ 19 | 20 | #include "fonts.h" 21 | 22 | struct FontEntry fonts[NUM_FONTS + 1] = { 23 | { 0, 8, 14, "Outlined8x14", font_lookup_outlined8x14, font_data_outlined8x14, 0 }, 24 | { 1, 8, 8, "Outlined8x8", font_lookup_outlined8x8, font_data_outlined8x8, FONT_UPPERCASE_ONLY }, 25 | { 2, 8, 10, "font8x10", 0, 0, 0 }, 26 | { 3, 12, 18, "font12x18", 0, 0, 0 }, 27 | { -1, 0, 0, "", 0, 0, 0 }// ends font table 28 | }; 29 | -------------------------------------------------------------------------------- /fonts.h: -------------------------------------------------------------------------------- 1 | #ifndef OSD_FONT_H__ 2 | #define OSD_FONT_H__ 3 | 4 | // New fonts need a .c file (with the data) and a .h file with 5 | // the definitions. Add the .h files here only. 6 | #include "font_outlined8x14.h" 7 | #include "font_outlined8x8.h" 8 | 9 | // This number must also be incremented for each new font. 10 | #define NUM_FONTS 4 11 | 12 | // Flags for fonts. 13 | #define FONT_LOWERCASE_ONLY 1 14 | #define FONT_UPPERCASE_ONLY 2 15 | 16 | // Font table. (Actual list of fonts in fonts.c.) 17 | struct FontEntry { 18 | int id; 19 | unsigned char width, height; 20 | const char *name; 21 | const char *lookup; 22 | const char *data; 23 | int flags; 24 | }; 25 | 26 | extern struct FontEntry fonts[NUM_FONTS + 1]; 27 | 28 | #endif //OSD_FONT_H__ 29 | 30 | -------------------------------------------------------------------------------- /fpv_video/Makefile: -------------------------------------------------------------------------------- 1 | OBJS=fpv_video.o 2 | BIN=fpv_video 3 | LDFLAGS+=-lilclient 4 | CFLAGS+=-DDISPLAY_SET_NOASPECT 5 | 6 | include Makefile.include 7 | 8 | -------------------------------------------------------------------------------- /fpv_video/Makefile.include: -------------------------------------------------------------------------------- 1 | CFLAGS+=-DSTANDALONE -D__STDC_CONSTANT_MACROS -D__STDC_LIMIT_MACROS -DTARGET_POSIX -D_LINUX -fPIC -DPIC -D_REENTRANT -D_LARGEFILE64_SOURCE -D_FILE_OFFSET_BITS=64 -U_FORTIFY_SOURCE -Wall -g -DHAVE_LIBOPENMAX=2 -DOMX -DOMX_SKIP64BIT -ftree-vectorize -pipe -DUSE_EXTERNAL_OMX -DHAVE_LIBBCM_HOST -DUSE_EXTERNAL_LIBBCM_HOST -DUSE_VCHIQ_ARM -Wno-psabi 2 | 3 | LDFLAGS+=-L$(SDKSTAGE)/opt/vc/lib/ -lbrcmGLESv2 -lbrcmEGL -lopenmaxil -lbcm_host -lvcos -lvchiq_arm -lpthread -lrt -lm -L$(SDKSTAGE)/opt/vc/src/hello_pi/libs/ilclient -L$(SDKSTAGE)/opt/vc/src/hello_pi/libs/vgfont 4 | 5 | INCLUDES+=-I$(SDKSTAGE)/opt/vc/include/ -I$(SDKSTAGE)/opt/vc/include/interface/vcos/pthreads -I$(SDKSTAGE)/opt/vc/include/interface/vmcs_host/linux -I./ -I$(SDKSTAGE)/opt/vc/src/hello_pi/libs/ilclient -I$(SDKSTAGE)/opt/vc/src/hello_pi/libs/vgfont 6 | 7 | all: $(BIN) $(LIB) 8 | 9 | %.o: %.c 10 | @rm -f $@ 11 | $(CC) $(CFLAGS) $(INCLUDES) -g -c $< -o $@ -Wno-deprecated-declarations 12 | 13 | %.o: %.cpp 14 | @rm -f $@ 15 | $(CXX) $(CFLAGS) $(INCLUDES) -g -c $< -o $@ -Wno-deprecated-declarations 16 | 17 | $(BIN): $(OBJS) 18 | $(CC) -o $@ -Wl,--whole-archive $(OBJS) $(LDFLAGS) -Wl,--no-whole-archive -rdynamic 19 | 20 | %.a: $(OBJS) 21 | $(AR) r $@ $^ 22 | 23 | clean: 24 | for i in $(OBJS); do (if test -e "$$i"; then ( rm -f $$i ); fi ); done 25 | @rm -f $(BIN) $(LIB) 26 | 27 | 28 | -------------------------------------------------------------------------------- /fpv_video/fpv_video.c: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2012, 2019, Broadcom Europe Ltd 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the copyright holder nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | // Video deocode demo using OpenMAX IL though the ilcient helper library 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "bcm_host.h" 35 | #include "ilclient.h" 36 | 37 | #define OMX_INIT_STRUCTURE(a) \ 38 | memset(&(a), 0, sizeof(a)); \ 39 | (a).nSize = sizeof(a); \ 40 | (a).nVersion.s.nVersionMajor = OMX_VERSION_MAJOR; \ 41 | (a).nVersion.s.nVersionMinor = OMX_VERSION_MINOR; \ 42 | (a).nVersion.s.nRevision = OMX_VERSION_REVISION; \ 43 | (a).nVersion.s.nStep = OMX_VERSION_STEP 44 | 45 | 46 | static int video_decode_test() 47 | { 48 | OMX_VIDEO_PARAM_PORTFORMATTYPE format; 49 | OMX_TIME_CONFIG_CLOCKSTATETYPE cstate; 50 | 51 | COMPONENT_T *video_decode = NULL, *video_scheduler = NULL, *video_render = NULL, *clock = NULL; 52 | COMPONENT_T *list[5]; 53 | TUNNEL_T tunnel[4]; 54 | ILCLIENT_T *client; 55 | 56 | int status = 0; 57 | 58 | memset(list, 0, sizeof(list)); 59 | memset(tunnel, 0, sizeof(tunnel)); 60 | 61 | if((client = ilclient_init()) == NULL) 62 | { 63 | fprintf(stderr, "ilclient_init failed\n"); 64 | return -1; 65 | } 66 | 67 | if(OMX_Init() != OMX_ErrorNone) 68 | { 69 | ilclient_destroy(client); 70 | fprintf(stderr, "OMX_init failed\n"); 71 | return -1; 72 | } 73 | 74 | // create video_decode 75 | if(ilclient_create_component(client, &video_decode, "video_decode", ILCLIENT_DISABLE_ALL_PORTS | ILCLIENT_ENABLE_INPUT_BUFFERS) != 0) 76 | { 77 | fprintf(stderr, "video_decode create failed\n"); 78 | return -1; 79 | } 80 | 81 | list[0] = video_decode; 82 | 83 | // create video_render 84 | if(ilclient_create_component(client, &video_render, "video_render", ILCLIENT_DISABLE_ALL_PORTS) != 0) 85 | { 86 | fprintf(stderr, "video_render create failed\n"); 87 | return -1; 88 | } 89 | 90 | list[1] = video_render; 91 | 92 | // create clock 93 | if(ilclient_create_component(client, &clock, "clock", ILCLIENT_DISABLE_ALL_PORTS) != 0) 94 | { 95 | fprintf(stderr, "clock create failed\n"); 96 | return -1; 97 | } 98 | 99 | list[2] = clock; 100 | 101 | memset(&cstate, 0, sizeof(cstate)); 102 | cstate.nSize = sizeof(cstate); 103 | cstate.nVersion.nVersion = OMX_VERSION; 104 | cstate.eState = OMX_TIME_ClockStateWaitingForStartTime; 105 | cstate.nWaitMask = 1; 106 | 107 | if(clock != NULL && OMX_SetParameter(ILC_GET_HANDLE(clock), OMX_IndexConfigTimeClockState, &cstate) != OMX_ErrorNone) 108 | { 109 | fprintf(stderr, "OMX set clock state failed\n"); 110 | return -1; 111 | } 112 | 113 | // create video_scheduler 114 | if(ilclient_create_component(client, &video_scheduler, "video_scheduler", ILCLIENT_DISABLE_ALL_PORTS) != 0) 115 | { 116 | fprintf(stderr, "create video_scheduler failed\n"); 117 | return -1; 118 | } 119 | 120 | list[3] = video_scheduler; 121 | 122 | set_tunnel(tunnel, video_decode, 131, video_scheduler, 10); 123 | set_tunnel(tunnel+1, video_scheduler, 11, video_render, 90); 124 | set_tunnel(tunnel+2, clock, 80, video_scheduler, 12); 125 | 126 | // setup clock tunnel first 127 | if(ilclient_setup_tunnel(tunnel + 2, 0, 0) != 0) 128 | { 129 | fprintf(stderr, "ilclient_setup_tunnel2 failed\n"); 130 | return -1; 131 | } 132 | 133 | ilclient_change_component_state(clock, OMX_StateExecuting); 134 | ilclient_change_component_state(video_decode, OMX_StateIdle); 135 | 136 | memset(&format, 0, sizeof(OMX_VIDEO_PARAM_PORTFORMATTYPE)); 137 | format.nSize = sizeof(OMX_VIDEO_PARAM_PORTFORMATTYPE); 138 | format.nVersion.nVersion = OMX_VERSION; 139 | format.nPortIndex = 130; 140 | format.eCompressionFormat = OMX_VIDEO_CodingAVC; 141 | format.xFramerate = 30 << 16; 142 | 143 | if(OMX_SetParameter(ILC_GET_HANDLE(video_decode), OMX_IndexParamVideoPortFormat, &format) == OMX_ErrorNone && 144 | ilclient_enable_port_buffers(video_decode, 130, NULL, NULL, NULL) == 0) 145 | { 146 | OMX_BUFFERHEADERTYPE *buf; 147 | int first_packet = 1; 148 | 149 | ilclient_change_component_state(video_decode, OMX_StateExecuting); 150 | 151 | #ifdef DISPLAY_SET_NOASPECT 152 | { 153 | OMX_CONFIG_DISPLAYREGIONTYPE display; 154 | OMX_ERRORTYPE omx_err; 155 | 156 | OMX_INIT_STRUCTURE(display); 157 | display.nPortIndex = 90; 158 | display.set = (OMX_DISPLAYSETTYPE)OMX_DISPLAY_SET_NOASPECT; 159 | display.noaspect = OMX_TRUE; 160 | 161 | omx_err = OMX_SetParameter(ILC_GET_HANDLE(video_render), OMX_IndexConfigDisplayRegion, &display); 162 | 163 | if (omx_err != OMX_ErrorNone) 164 | { 165 | fprintf(stderr, "Unable to set aspect: 0x%x\n", omx_err); 166 | return -1; 167 | } 168 | } 169 | #endif 170 | 171 | void psc_callback(void *userdata, COMPONENT_T *comp, OMX_U32 data) 172 | { 173 | //printf("got event %p %p %d\n", userdata, comp, data); 174 | 175 | if(comp == video_decode && data == 131) 176 | { 177 | if(ilclient_setup_tunnel(tunnel, 0, 0) != 0) 178 | { 179 | status = -1; 180 | fprintf(stderr, "ilclient_setup_tunnel0 failed\n"); 181 | return; 182 | } 183 | 184 | ilclient_change_component_state(video_scheduler, OMX_StateExecuting); 185 | 186 | // now setup tunnel to video_render 187 | if(ilclient_setup_tunnel(tunnel+1, 0, 1000) != 0) 188 | { 189 | status = -1; 190 | fprintf(stderr, "ilclient_setup_tunnel1 failed\n"); 191 | return; 192 | } 193 | 194 | ilclient_change_component_state(video_render, OMX_StateExecuting); 195 | } 196 | } 197 | 198 | ilclient_set_port_settings_callback(client, psc_callback, NULL); 199 | 200 | while(status == 0 && (buf = ilclient_get_input_buffer(video_decode, 130, 1)) != NULL) 201 | { 202 | int data_len = read(STDIN_FILENO, buf->pBuffer, buf->nAllocLen); 203 | if(data_len <= 0) break; 204 | 205 | buf->nFilledLen = data_len; 206 | buf->nOffset = 0; 207 | 208 | if(first_packet) 209 | { 210 | buf->nFlags = OMX_BUFFERFLAG_STARTTIME; 211 | first_packet = 0; 212 | } 213 | else 214 | buf->nFlags = OMX_BUFFERFLAG_TIME_UNKNOWN; 215 | 216 | if(OMX_EmptyThisBuffer(ILC_GET_HANDLE(video_decode), buf) != OMX_ErrorNone) 217 | { 218 | status = -1; 219 | fprintf(stderr, "OMX_EmptyThisBuffer failed\n"); 220 | break; 221 | } 222 | } 223 | 224 | if(buf != NULL) 225 | { 226 | buf->nFilledLen = 0; 227 | buf->nFlags = OMX_BUFFERFLAG_TIME_UNKNOWN | OMX_BUFFERFLAG_EOS; 228 | OMX_EmptyThisBuffer(ILC_GET_HANDLE(video_decode), buf); 229 | } 230 | 231 | // need to flush the renderer to allow video_decode to disable its input port 232 | ilclient_flush_tunnels(tunnel, 0); 233 | } 234 | 235 | 236 | ilclient_disable_tunnel(tunnel); 237 | ilclient_disable_tunnel(tunnel+1); 238 | ilclient_disable_tunnel(tunnel+2); 239 | ilclient_disable_port_buffers(video_decode, 130, NULL, NULL, NULL); 240 | ilclient_teardown_tunnels(tunnel); 241 | 242 | ilclient_state_transition(list, OMX_StateIdle); 243 | ilclient_state_transition(list, OMX_StateLoaded); 244 | 245 | ilclient_cleanup_components(list); 246 | 247 | OMX_Deinit(); 248 | 249 | ilclient_destroy(client); 250 | return status; 251 | } 252 | 253 | int main (int argc, char **argv) 254 | { 255 | bcm_host_init(); 256 | return video_decode_test(); 257 | } 258 | -------------------------------------------------------------------------------- /fpv_video/fpv_video.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | set -o pipefail 4 | gst-launch-1.0 udpsrc port=5600 caps='application/x-rtp, media=(string)video, clock-rate=(int)90000, encoding-name=(string)H264' ! rtph264depay ! 'video/x-h264,stream-format=byte-stream' ! fdsink | fpv_video 5 | -------------------------------------------------------------------------------- /graphengine.h: -------------------------------------------------------------------------------- 1 | #ifndef GRAPH_ENGINE_H__ 2 | #define GRAPH_ENGINE_H__ 3 | 4 | #include 5 | #include "fonts.h" 6 | 7 | extern int osd_debug; 8 | 9 | typedef enum 10 | { 11 | OSD_RENDER_AUTO = 0, 12 | OSD_RENDER_XV, 13 | OSD_RENDER_GL, 14 | OSD_RENDER_KMS, 15 | } osd_render_t; 16 | 17 | // Size of an array (num items.) 18 | #define SIZEOF_ARRAY(x) (sizeof(x) / sizeof((x)[0])) 19 | 20 | 21 | #define SETUP_STROKE_FILL(stroke, fill, mode) \ 22 | stroke = 0; fill = 0; \ 23 | if (mode == 0) { stroke = 0; fill = color; } \ 24 | if (mode == 1) { stroke = color; fill = 0; } \ 25 | 26 | // Line endcaps (for horizontal and vertical lines.) 27 | #define ENDCAP_NONE 0 28 | #define ENDCAP_ROUND 1 29 | #define ENDCAP_FLAT 2 30 | 31 | #define DRAW_ENDCAP_HLINE(e, x, y, s, f, l) \ 32 | if ((e) == ENDCAP_ROUND) /* single pixel endcap */ \ 33 | { write_pixel_lm(x, y, f, l); } \ 34 | else if ((e) == ENDCAP_FLAT) /* flat endcap: FIXME, quicker to draw a vertical line(?) */ \ 35 | { write_pixel_lm(x, y - 1, s, l); write_pixel_lm(x, y, s, l); write_pixel_lm(x, y + 1, s, l); } 36 | 37 | #define DRAW_ENDCAP_VLINE(e, x, y, s, f, l) \ 38 | if ((e) == ENDCAP_ROUND) /* single pixel endcap */ \ 39 | { write_pixel_lm(x, y, f, l); } \ 40 | else if ((e) == ENDCAP_FLAT) /* flat endcap: FIXME, quicker to draw a horizontal line(?) */ \ 41 | { write_pixel_lm(x - 1, y, s, l); write_pixel_lm(x, y, s, l); write_pixel_lm(x + 1, y, s, l); } 42 | 43 | // Macros for writing pixels in a midpoint circle algorithm. 44 | #define CIRCLE_PLOT_8(cx, cy, x, y, color, mode) \ 45 | CIRCLE_PLOT_4(cx, cy, x, y, mode, color); \ 46 | if ((x) != (y)) { CIRCLE_PLOT_4(cx, cy, y, x, mode, color); } 47 | 48 | #define CIRCLE_PLOT_4(cx, cy, x, y, color, mode) \ 49 | write_pixel_lm((cx) + (x), (cy) + (y), mode, color); \ 50 | write_pixel_lm((cx) - (x), (cy) + (y), mode, color); \ 51 | write_pixel_lm((cx) + (x), (cy) - (y), mode, color); \ 52 | write_pixel_lm((cx) - (x), (cy) - (y), mode, color); 53 | 54 | 55 | // Font flags. 56 | #define FONT_BOLD 1 // bold text (no outline) 57 | #define FONT_INVERT 2 // invert: border white, inside black 58 | // Text alignments. 59 | #define TEXT_VA_TOP 0 60 | #define TEXT_VA_MIDDLE 1 61 | #define TEXT_VA_BOTTOM 2 62 | #define TEXT_HA_LEFT 0 63 | #define TEXT_HA_CENTER 1 64 | #define TEXT_HA_RIGHT 2 65 | 66 | // Text dimension structures. 67 | struct FontDimensions { 68 | int width, height; 69 | }; 70 | 71 | // to convert metric -> imperial 72 | // for speeds see http://en.wikipedia.org/wiki/Miles_per_hour 73 | typedef struct { // from metric imperial 74 | float m_to_m_feet; // m m 1.0 feet 3.280840 75 | float ms_to_ms_fts; // m/s m/s 1.0 ft/s 3.280840 76 | float ms_to_kmh_mph; // m/s km/h 3.6 mph 2.236936 77 | uint8_t char_m_feet; // char 'm' 'f' 78 | uint8_t char_ms_fts; // char 'm/s' 'ft/s' 79 | } Unit; 80 | 81 | // Home position for calculations 82 | typedef struct { 83 | int32_t Latitude; 84 | int32_t Longitude; 85 | float Altitude; 86 | uint8_t GotHome; 87 | uint32_t Distance; 88 | uint16_t Direction; 89 | } HomePosition; 90 | 91 | // ADC values filtered 92 | typedef struct { 93 | double rssi; 94 | double flight; 95 | double video; 96 | double volt; 97 | double curr; 98 | } ADCfiltered; 99 | 100 | // Max/Min macros 101 | #define MAX3(a, b, c) MAX(a, MAX(b, c)) 102 | #define MIN3(a, b, c) MIN(a, MIN(b, c)) 103 | #define LIMIT(x, l, h) MAX(l, MIN(x, h)) 104 | 105 | 106 | extern int screen_width, screen_height; 107 | 108 | // PAL 109 | #define GRAPHICS_WIDTH 640 110 | #define GRAPHICS_HEIGHT 360 111 | #define GRAPHICS_LEFT 0 112 | #define GRAPHICS_TOP 0 113 | #define GRAPHICS_RIGHT (GRAPHICS_WIDTH - 1) 114 | #define GRAPHICS_BOTTOM (GRAPHICS_HEIGHT - 1) 115 | 116 | #define GRAPHICS_X_MIDDLE (GRAPHICS_WIDTH / 2) 117 | #define GRAPHICS_Y_MIDDLE (GRAPHICS_HEIGHT / 2) 118 | 119 | // Check if coordinates are valid. If not, return. Assumes signed coordinates for working correct also with values lesser than 0. 120 | #define CHECK_COORDS(x, y) if (x < GRAPHICS_LEFT || x > GRAPHICS_RIGHT || y < GRAPHICS_TOP || y > GRAPHICS_BOTTOM) { return; } 121 | #define CHECK_COORD_X(x) if (x < GRAPHICS_LEFT || x > GRAPHICS_RIGHT) { return; } 122 | #define CHECK_COORD_Y(y) if (y < GRAPHICS_TOP || y > GRAPHICS_BOTTOM) { return; } 123 | 124 | // Clip coordinates out of range. Assumes signed coordinates for working correct also with values lesser than 0. 125 | #define CLIP_COORDS(x, y) { CLIP_COORD_X(x); CLIP_COORD_Y(y); } 126 | #define CLIP_COORD_X(x) { x = x < GRAPHICS_LEFT ? GRAPHICS_LEFT : x > GRAPHICS_RIGHT ? GRAPHICS_RIGHT : x; } 127 | #define CLIP_COORD_Y(y) { y = y < GRAPHICS_TOP ? GRAPHICS_TOP : y > GRAPHICS_BOTTOM ? GRAPHICS_BOTTOM : y; } 128 | 129 | // Macro to swap two variables using XOR swap. 130 | #define SWAP(a, b) { a ^= b; b ^= a; a ^= b; } 131 | 132 | uint8_t getCharData(uint16_t charPos); 133 | 134 | void* render(void); 135 | void render_init(int shift_x, int shift_y, float scale_x, float scale_y); 136 | void clearGraphics(void); 137 | void* displayGraphics(void); 138 | 139 | //void drawArrow(uint16_t x, uint16_t y, uint16_t angle, uint16_t size); 140 | void drawBox(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2); 141 | 142 | void write_pixel_lm(int x, int y, int opaq, int color); 143 | 144 | void write_hline_lm(int x0, int x1, int y, int color, int opaq); 145 | 146 | void write_hline_outlined(int x0, int x1, int y, int endcap0, int endcap1, int mode, int opaq, int color); 147 | 148 | void write_vline_lm(int x, int y0, int y1, int color, int opaq); 149 | void write_vline_outlined(int x, int y0, int y1, int endcap0, int endcap1, int mode, int opaq, int color); 150 | 151 | void write_filled_rectangle_lm(int x, int y, int width, int height, int color, int opaq); 152 | void write_rectangle_outlined(int x, int y, int width, int height, int mode, int opaq); 153 | 154 | void write_circle_outlined(int cx, int cy, int r, int dashp, int bmode, int mode, int opaq, int color); 155 | 156 | void write_line_lm(int x0, int y0, int x1, int y1, int opaq, int color); 157 | void write_line_outlined(int x0, int y0, int x1, int y1, int endcap0, int endcap1, int mode, int opaq); 158 | void write_line_outlined_dashed(int x0, int y0, int x1, int y1, int endcap0, int endcap1, int mode, int opaq, int dots); 159 | 160 | void write_triangle_filled(int x0, int y0, int x1, int y1, int x2, int y2); 161 | void write_triangle_wire(int x0, int y0, int x1, int y1, int x2, int y2); 162 | 163 | void write_char16(char ch, int x, int y, int font, int color); 164 | void write_char(char ch, int x, int y, int flags, int font, int color); 165 | 166 | void write_string(char *str, int x, int y, int xs, int ys, int va, int ha, int flags, int font); 167 | void write_color_string(char *str, int x, int y, int xs, int ys, int va, int ha, int flags, int font, int color); 168 | 169 | int fetch_font_info(uint8_t ch, int font, struct FontEntry *font_info, char *lookup); 170 | void calc_text_dimensions(char *str, struct FontEntry font, int xs, int ys, struct FontDimensions *dim); 171 | 172 | extern uint8_t* video_buf_ext; 173 | extern pthread_mutex_t video_mutex; 174 | 175 | #endif 176 | -------------------------------------------------------------------------------- /gst-compat.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | /** 5 | * gst_element_get_current_clock_time: 6 | * @element: a #GstElement. 7 | * 8 | * Returns the current clock time of the element, as in, the time of the 9 | * element's clock, or GST_CLOCK_TIME_NONE if there is no clock. 10 | * 11 | * Returns: the clock time of the element, or GST_CLOCK_TIME_NONE if there is 12 | * no clock. 13 | * 14 | * Since: 1.18 15 | */ 16 | GstClockTime 17 | gst_element_get_current_clock_time (GstElement * element) 18 | { 19 | GstClock *clock = NULL; 20 | GstClockTime ret; 21 | 22 | g_return_val_if_fail (GST_IS_ELEMENT (element), GST_CLOCK_TIME_NONE); 23 | 24 | clock = gst_element_get_clock (element); 25 | 26 | if (!clock) { 27 | GST_DEBUG_OBJECT (element, "Element has no clock"); 28 | return GST_CLOCK_TIME_NONE; 29 | } 30 | 31 | ret = gst_clock_get_time (clock); 32 | gst_object_unref (clock); 33 | 34 | return ret; 35 | } 36 | 37 | /** 38 | * gst_element_get_current_running_time: 39 | * @element: a #GstElement. 40 | * 41 | * Returns the running time of the element. The running time is the 42 | * element's clock time minus its base time. Will return GST_CLOCK_TIME_NONE 43 | * if the element has no clock, or if its base time has not been set. 44 | * 45 | * Returns: the running time of the element, or GST_CLOCK_TIME_NONE if the 46 | * element has no clock or its base time has not been set. 47 | * 48 | * Since: 1.18 49 | */ 50 | GstClockTime 51 | gst_element_get_current_running_time (GstElement * element) 52 | { 53 | GstClockTime base_time, clock_time; 54 | 55 | g_return_val_if_fail (GST_IS_ELEMENT (element), GST_CLOCK_TIME_NONE); 56 | 57 | base_time = gst_element_get_base_time (element); 58 | 59 | if (!GST_CLOCK_TIME_IS_VALID (base_time)) { 60 | GST_DEBUG_OBJECT (element, "Could not determine base time"); 61 | return GST_CLOCK_TIME_NONE; 62 | } 63 | 64 | clock_time = gst_element_get_current_clock_time (element); 65 | 66 | if (!GST_CLOCK_TIME_IS_VALID (clock_time)) { 67 | return GST_CLOCK_TIME_NONE; 68 | } 69 | 70 | if (clock_time < base_time) { 71 | GST_DEBUG_OBJECT (element, "Got negative current running time"); 72 | return GST_CLOCK_TIME_NONE; 73 | } 74 | 75 | return clock_time - base_time; 76 | } 77 | -------------------------------------------------------------------------------- /m2dlib.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This program is free software; you can redistribute it and/or modify 3 | * it under the terms of the GNU General Public License as published by 4 | * the Free Software Foundation; either version 3 of the License, or 5 | * (at your option) any later version. 6 | * 7 | * This program is distributed in the hope that it will be useful, but 8 | * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 9 | * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 10 | * for more details. 11 | * 12 | * You should have received a copy of the GNU General Public License along 13 | * with this program; if not, write to the Free Software Foundation, Inc., 14 | * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 15 | */ 16 | /* 17 | * With Grateful Acknowledgements to the books writen by Andre LaMothe: 18 | * 19 | * 20 | */ 21 | #include "m2dlib.h" 22 | #include "osdvar.h" 23 | 24 | //Reset 2d object, just copy local verts to transfer verts 25 | void Reset_Polygon2D(POLYGON2D_PTR poly) { 26 | for (int curr_vert = 0; curr_vert < poly->num_verts; curr_vert++) { 27 | VECTOR2D_COPY(&(poly->vlist_trans[curr_vert]), 28 | &(poly->vlist_local[curr_vert])); 29 | } 30 | } 31 | 32 | int Transform_Polygon2D(POLYGON2D_PTR poly, float roate, float tx, float ty) { 33 | // test for valid pointer 34 | if (!poly) 35 | return (0); 36 | 37 | // test for negative rotation angle 38 | if (roate < 0) 39 | roate += 360; 40 | 41 | // loop and rotate each point, very crude, no lookup!!! 42 | for (int curr_vert = 0; curr_vert < poly->num_verts; curr_vert++) { 43 | poly->vlist_trans[curr_vert].x += tx; 44 | poly->vlist_trans[curr_vert].y += ty; 45 | // perform rotation 46 | float xr = (float) poly->vlist_trans[curr_vert].x * Fast_Cos(roate) 47 | - (float) poly->vlist_trans[curr_vert].y * Fast_Sin(roate); 48 | 49 | float yr = (float) poly->vlist_trans[curr_vert].x * Fast_Sin(roate) 50 | + (float) poly->vlist_trans[curr_vert].y * Fast_Cos(roate); 51 | 52 | // store result back 53 | poly->vlist_trans[curr_vert].x = xr; 54 | poly->vlist_trans[curr_vert].y = yr; 55 | 56 | } // end for curr_vert 57 | 58 | // return success 59 | return (1); 60 | } 61 | 62 | int Translate_Polygon2D(POLYGON2D_PTR poly, float dx, float dy) { 63 | // this function translates the center of a polygon 64 | 65 | // test for valid pointer 66 | if (!poly) 67 | return (0); 68 | 69 | // translate 70 | poly->x0 += dx; 71 | poly->y0 += dy; 72 | 73 | // return success 74 | return (1); 75 | 76 | } // end Translate_Polygon2D 77 | 78 | int Rotate_Polygon2D(POLYGON2D_PTR poly, float theta) { 79 | // this function rotates the local coordinates of the polygon 80 | 81 | // test for valid pointer 82 | if (!poly) 83 | return (0); 84 | 85 | // test for negative rotation angle 86 | if (theta < 0) 87 | theta += 360; 88 | 89 | // loop and rotate each point, very crude, no lookup!!! 90 | for (int curr_vert = 0; curr_vert < poly->num_verts; curr_vert++) { 91 | // perform rotation 92 | float xr = (float) poly->vlist_trans[curr_vert].x * Fast_Cos(theta) 93 | - (float) poly->vlist_trans[curr_vert].y * Fast_Sin(theta); 94 | 95 | float yr = (float) poly->vlist_trans[curr_vert].x * Fast_Sin(theta) 96 | + (float) poly->vlist_trans[curr_vert].y * Fast_Cos(theta); 97 | 98 | // store result back 99 | poly->vlist_trans[curr_vert].x = xr; 100 | poly->vlist_trans[curr_vert].y = yr; 101 | 102 | } // end for curr_vert 103 | 104 | // return success 105 | return (1); 106 | 107 | } // end Rotate_Polygon2D 108 | 109 | int Scale_Polygon2D(POLYGON2D_PTR poly, float sx, float sy) { 110 | // this function scalesthe local coordinates of the polygon 111 | 112 | // test for valid pointer 113 | if (!poly) 114 | return (0); 115 | 116 | // loop and scale each point 117 | for (int curr_vert = 0; curr_vert < poly->num_verts; curr_vert++) { 118 | // scale and store result back 119 | poly->vlist_local[curr_vert].x *= sx; 120 | poly->vlist_local[curr_vert].y *= sy; 121 | 122 | } // end for curr_vert 123 | 124 | // return success 125 | return (1); 126 | 127 | } // end Scale_Polygon2D 128 | 129 | //return 1: visible 130 | // 0: invisible 131 | int Clip_Line(VECTOR4D_PTR v) { 132 | // this function clips the sent line using the globally defined clipping 133 | // region 134 | 135 | // internal clipping codes 136 | #define CLIP_CODE_C 0x0000 137 | #define CLIP_CODE_N 0x0008 138 | #define CLIP_CODE_S 0x0004 139 | #define CLIP_CODE_E 0x0002 140 | #define CLIP_CODE_W 0x0001 141 | 142 | #define CLIP_CODE_NE 0x000a 143 | #define CLIP_CODE_SE 0x0006 144 | #define CLIP_CODE_NW 0x0009 145 | #define CLIP_CODE_SW 0x0005 146 | 147 | int x1 = v->x, y1 = v->y, x2 = v->z, y2 = v->w; 148 | 149 | int xc1 = x1, yc1 = y1, xc2 = x2, yc2 = y2; 150 | 151 | int p1_code = 0, p2_code = 0; 152 | 153 | // determine codes for p1 and p2 154 | if (y1 < atti_3d_min_clipY) 155 | p1_code |= CLIP_CODE_N; 156 | else if (y1 > atti_3d_max_clipY) 157 | p1_code |= CLIP_CODE_S; 158 | 159 | if (x1 < atti_3d_min_clipX) 160 | p1_code |= CLIP_CODE_W; 161 | else if (x1 > atti_3d_max_clipX) 162 | p1_code |= CLIP_CODE_E; 163 | 164 | if (y2 < atti_3d_min_clipY) 165 | p2_code |= CLIP_CODE_N; 166 | else if (y2 > atti_3d_max_clipY) 167 | p2_code |= CLIP_CODE_S; 168 | 169 | if (x2 < atti_3d_min_clipX) 170 | p2_code |= CLIP_CODE_W; 171 | else if (x2 > atti_3d_max_clipX) 172 | p2_code |= CLIP_CODE_E; 173 | 174 | // try and trivially reject 175 | if ((p1_code & p2_code)) 176 | return (0); 177 | 178 | // test for totally visible, if so leave points untouched 179 | if (p1_code == 0 && p2_code == 0) 180 | return (1); 181 | 182 | // determine end clip point for p1 183 | switch (p1_code) { 184 | case CLIP_CODE_C: 185 | break; 186 | 187 | case CLIP_CODE_N: { 188 | yc1 = atti_3d_min_clipY; 189 | xc1 = x1 + 0.5 + (atti_3d_min_clipY - y1) * (x2 - x1) / (y2 - y1); 190 | } 191 | break; 192 | case CLIP_CODE_S: { 193 | yc1 = atti_3d_max_clipY; 194 | xc1 = x1 + 0.5 + (atti_3d_max_clipY - y1) * (x2 - x1) / (y2 - y1); 195 | } 196 | break; 197 | 198 | case CLIP_CODE_W: { 199 | xc1 = atti_3d_min_clipX; 200 | yc1 = y1 + 0.5 + (atti_3d_min_clipX - x1) * (y2 - y1) / (x2 - x1); 201 | } 202 | break; 203 | 204 | case CLIP_CODE_E: { 205 | xc1 = atti_3d_max_clipX; 206 | yc1 = y1 + 0.5 + (atti_3d_max_clipX - x1) * (y2 - y1) / (x2 - x1); 207 | } 208 | break; 209 | 210 | // these cases are more complex, must compute 2 intersections 211 | case CLIP_CODE_NE: { 212 | // north hline intersection 213 | yc1 = atti_3d_min_clipY; 214 | xc1 = x1 + 0.5 + (atti_3d_min_clipY - y1) * (x2 - x1) / (y2 - y1); 215 | 216 | // test if intersection is valid, of so then done, else compute next 217 | if (xc1 < atti_3d_min_clipX || xc1 > atti_3d_max_clipX) { 218 | // east vline intersection 219 | xc1 = atti_3d_max_clipX; 220 | yc1 = y1 + 0.5 + (atti_3d_max_clipX - x1) * (y2 - y1) / (x2 - x1); 221 | } // end if 222 | 223 | } 224 | break; 225 | 226 | case CLIP_CODE_SE: { 227 | // south hline intersection 228 | yc1 = atti_3d_max_clipY; 229 | xc1 = x1 + 0.5 + (atti_3d_max_clipY - y1) * (x2 - x1) / (y2 - y1); 230 | 231 | // test if intersection is valid, of so then done, else compute next 232 | if (xc1 < atti_3d_min_clipX || xc1 > atti_3d_max_clipX) { 233 | // east vline intersection 234 | xc1 = atti_3d_max_clipX; 235 | yc1 = y1 + 0.5 + (atti_3d_max_clipX - x1) * (y2 - y1) / (x2 - x1); 236 | } // end if 237 | 238 | } 239 | break; 240 | 241 | case CLIP_CODE_NW: { 242 | // north hline intersection 243 | yc1 = atti_3d_min_clipY; 244 | xc1 = x1 + 0.5 + (atti_3d_min_clipY - y1) * (x2 - x1) / (y2 - y1); 245 | 246 | // test if intersection is valid, of so then done, else compute next 247 | if (xc1 < atti_3d_min_clipX || xc1 > atti_3d_max_clipX) { 248 | xc1 = atti_3d_min_clipX; 249 | yc1 = y1 + 0.5 + (atti_3d_min_clipX - x1) * (y2 - y1) / (x2 - x1); 250 | } // end if 251 | 252 | } 253 | break; 254 | 255 | case CLIP_CODE_SW: { 256 | // south hline intersection 257 | yc1 = atti_3d_max_clipY; 258 | xc1 = x1 + 0.5 + (atti_3d_max_clipY - y1) * (x2 - x1) / (y2 - y1); 259 | 260 | // test if intersection is valid, of so then done, else compute next 261 | if (xc1 < atti_3d_min_clipX || xc1 > atti_3d_max_clipX) { 262 | xc1 = atti_3d_min_clipX; 263 | yc1 = y1 + 0.5 + (atti_3d_min_clipX - x1) * (y2 - y1) / (x2 - x1); 264 | } // end if 265 | 266 | } 267 | break; 268 | 269 | default: 270 | break; 271 | 272 | } // end switch 273 | 274 | // determine clip point for p2 275 | switch (p2_code) { 276 | case CLIP_CODE_C: 277 | break; 278 | 279 | case CLIP_CODE_N: { 280 | yc2 = atti_3d_min_clipY; 281 | xc2 = x2 + (atti_3d_min_clipY - y2) * (x1 - x2) / (y1 - y2); 282 | } 283 | break; 284 | 285 | case CLIP_CODE_S: { 286 | yc2 = atti_3d_max_clipY; 287 | xc2 = x2 + (atti_3d_max_clipY - y2) * (x1 - x2) / (y1 - y2); 288 | } 289 | break; 290 | 291 | case CLIP_CODE_W: { 292 | xc2 = atti_3d_min_clipX; 293 | yc2 = y2 + (atti_3d_min_clipX - x2) * (y1 - y2) / (x1 - x2); 294 | } 295 | break; 296 | 297 | case CLIP_CODE_E: { 298 | xc2 = atti_3d_max_clipX; 299 | yc2 = y2 + (atti_3d_max_clipX - x2) * (y1 - y2) / (x1 - x2); 300 | } 301 | break; 302 | 303 | // these cases are more complex, must compute 2 intersections 304 | case CLIP_CODE_NE: { 305 | // north hline intersection 306 | yc2 = atti_3d_min_clipY; 307 | xc2 = x2 + 0.5 + (atti_3d_min_clipY - y2) * (x1 - x2) / (y1 - y2); 308 | 309 | // test if intersection is valid, of so then done, else compute next 310 | if (xc2 < atti_3d_min_clipX || xc2 > atti_3d_max_clipX) { 311 | // east vline intersection 312 | xc2 = atti_3d_max_clipX; 313 | yc2 = y2 + 0.5 + (atti_3d_max_clipX - x2) * (y1 - y2) / (x1 - x2); 314 | } // end if 315 | 316 | } 317 | break; 318 | 319 | case CLIP_CODE_SE: { 320 | // south hline intersection 321 | yc2 = atti_3d_max_clipY; 322 | xc2 = x2 + 0.5 + (atti_3d_max_clipY - y2) * (x1 - x2) / (y1 - y2); 323 | 324 | // test if intersection is valid, of so then done, else compute next 325 | if (xc2 < atti_3d_min_clipX || xc2 > atti_3d_max_clipX) { 326 | // east vline intersection 327 | xc2 = atti_3d_max_clipX; 328 | yc2 = y2 + 0.5 + (atti_3d_max_clipX - x2) * (y1 - y2) / (x1 - x2); 329 | } // end if 330 | 331 | } 332 | break; 333 | 334 | case CLIP_CODE_NW: { 335 | // north hline intersection 336 | yc2 = atti_3d_min_clipY; 337 | xc2 = x2 + 0.5 + (atti_3d_min_clipY - y2) * (x1 - x2) / (y1 - y2); 338 | 339 | // test if intersection is valid, of so then done, else compute next 340 | if (xc2 < atti_3d_min_clipX || xc2 > atti_3d_max_clipX) { 341 | xc2 = atti_3d_min_clipX; 342 | yc2 = y2 + 0.5 + (atti_3d_min_clipX - x2) * (y1 - y2) / (x1 - x2); 343 | } // end if 344 | 345 | } 346 | break; 347 | 348 | case CLIP_CODE_SW: { 349 | // south hline intersection 350 | yc2 = atti_3d_max_clipY; 351 | xc2 = x2 + 0.5 + (atti_3d_max_clipY - y2) * (x1 - x2) / (y1 - y2); 352 | 353 | // test if intersection is valid, of so then done, else compute next 354 | if (xc2 < atti_3d_min_clipX || xc2 > atti_3d_max_clipX) { 355 | xc2 = atti_3d_min_clipX; 356 | yc2 = y2 + 0.5 + (atti_3d_min_clipX - x2) * (y1 - y2) / (x1 - x2); 357 | } // end if 358 | 359 | } 360 | break; 361 | 362 | default: 363 | break; 364 | 365 | } // end switch 366 | 367 | // do bounds check 368 | if ((xc1 < atti_3d_min_clipX) || (xc1 > atti_3d_max_clipX) 369 | || (yc1 < atti_3d_min_clipY) || (yc1 > atti_3d_max_clipY) 370 | || (xc2 < atti_3d_min_clipX) || (xc2 > atti_3d_max_clipX) 371 | || (yc2 < atti_3d_min_clipY) || (yc2 > atti_3d_max_clipY)) { 372 | return (0); 373 | } // end if 374 | 375 | // store vars back 376 | v->x = xc1; 377 | v->y = yc1; 378 | v->z = xc2; 379 | v->w = yc2; 380 | 381 | return (1); 382 | 383 | } // end Clip_Line 384 | 385 | -------------------------------------------------------------------------------- /m2dlib.h: -------------------------------------------------------------------------------- 1 | #ifndef M2DLIB_H__ 2 | #define M2DLIB_H__ 3 | 4 | #include "math3d.h" 5 | 6 | // DEFINES //////////////////////////////////////////////////// 7 | 8 | #define OBJECT2DV1_MAX_VERTICES 80 9 | 10 | 11 | // a 2D polygon 12 | typedef struct POLYGON2D_TYP 13 | { 14 | int state; // state of polygon 15 | int num_verts; // number of vertices 16 | int x0, y0; // position of center of polygon 17 | VERTEX2DF vlist_local[OBJECT2DV1_MAX_VERTICES]; // pointer to vertex list 18 | VERTEX2DF vlist_trans[OBJECT2DV1_MAX_VERTICES]; // pointer to vertex list 19 | 20 | } POLYGON2D, *POLYGON2D_PTR; 21 | 22 | void Reset_Polygon2D(POLYGON2D_PTR poly); 23 | int Transform_Polygon2D(POLYGON2D_PTR poly, float roate, float tx, float ty); 24 | int Translate_Polygon2D(POLYGON2D_PTR poly, float dx, float dy); 25 | int Rotate_Polygon2D(POLYGON2D_PTR poly, float theta); 26 | int Scale_Polygon2D(POLYGON2D_PTR poly, float sx, float sy); 27 | int Clip_Line(VECTOR4D_PTR v); 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /main.c: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2017, 2021 Vasily Evseenko 3 | based on PlayuavOSD https://github.com/TobiasBales/PlayuavOSD.git 4 | */ 5 | 6 | /* 7 | * This program is free software; you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation; either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 14 | * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 15 | * for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License along 18 | * with this program; if not, write to the Free Software Foundation, Inc., 19 | * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 20 | */ 21 | 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include "osdrender.h" 42 | #include "osdmavlink.h" 43 | #include "osdvar.h" 44 | #include "osdconfig.h" 45 | #include "UAVObj.h" 46 | #include "graphengine.h" 47 | 48 | 49 | #ifdef __GST_OPENGL__ 50 | int gst_main(int rtp_port, char *codec, int rtp_jitter, osd_render_t osd_render, int screen_width, char *rtsp_url); 51 | #endif 52 | 53 | static volatile uint8_t finished = 0; 54 | int osd_debug = 0; 55 | 56 | void sigterm_handler(int signum) 57 | { 58 | finished = 1; 59 | } 60 | 61 | int open_udp_socket_for_rx(int port) 62 | { 63 | struct sockaddr_in saddr; 64 | int fd = socket(AF_INET, SOCK_DGRAM, 0); 65 | if (fd < 0){ 66 | perror("Error opening socket"); 67 | exit(1); 68 | } 69 | 70 | int optval = 1; 71 | setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, (const void *)&optval , sizeof(int)); 72 | 73 | bzero((char *) &saddr, sizeof(saddr)); 74 | saddr.sin_family = AF_INET; 75 | saddr.sin_addr.s_addr = htonl(INADDR_ANY); 76 | saddr.sin_port = htons((unsigned short)port); 77 | 78 | if (bind(fd, (struct sockaddr *) &saddr, sizeof(saddr)) < 0) 79 | { 80 | perror("Bind error"); 81 | exit(1); 82 | } 83 | return fd; 84 | } 85 | 86 | int main(int argc, char **argv) 87 | { 88 | int opt; 89 | int osd_port = 14551; 90 | int rtp_port = 5600; 91 | char* codec = "h264"; 92 | int rtp_jitter = 5; 93 | osd_render_t osd_render = OSD_RENDER_GL; 94 | int screen_width = 1920; 95 | char *rtsp_url = NULL; 96 | 97 | uint64_t render_ts = 0; 98 | uint64_t cur_ts = 0; 99 | uint8_t buf[65536]; // Max UDP packet size 100 | int fd; 101 | struct pollfd fds[1]; 102 | 103 | while ((opt = getopt(argc, argv, "hdp:P:R:45j:xakw:")) != -1) { 104 | switch (opt) { 105 | case 'p': 106 | osd_port = atoi(optarg); 107 | break; 108 | 109 | case 'P': 110 | rtp_port = atoi(optarg); 111 | break; 112 | 113 | case 'R': 114 | rtsp_url = strdup(optarg); 115 | break; 116 | 117 | case '4': 118 | codec = "h264"; 119 | break; 120 | 121 | case '5': 122 | codec = "h265"; 123 | break; 124 | 125 | case 'j': 126 | rtp_jitter = atoi(optarg); 127 | break; 128 | 129 | case 'x': 130 | osd_render = OSD_RENDER_XV; 131 | break; 132 | 133 | case 'a': 134 | osd_render = OSD_RENDER_AUTO; 135 | break; 136 | 137 | case 'k': 138 | osd_render = OSD_RENDER_KMS; 139 | break; 140 | 141 | case 'w': 142 | screen_width = atoi(optarg); 143 | break; 144 | 145 | case 'd': 146 | osd_debug = 1; 147 | break; 148 | 149 | case 'h': 150 | default: 151 | show_usage: 152 | 153 | #ifdef __GST_OPENGL__ 154 | fprintf(stderr, "%s [-p mavlink_port] [-P rtp_port] [ -R rtsp_url ] [-4] [-5] [-j rtp_jitter] [-x] [-a] [-w screen_width] \n", argv[0]); 155 | fprintf(stderr, "Default: mavlink_port=%d, rtp_port=%d, rtsp_url=%s, codec=%s, rtp_jitter=%d, screen_width=%d\n", 156 | osd_port, rtp_port, 157 | rtsp_url != NULL ? rtsp_url : "none", 158 | codec, rtp_jitter, screen_width); 159 | #else 160 | fprintf(stderr, "%s [-p mavlink_port]\n", argv[0]); 161 | fprintf(stderr, "Default: mavlink_port=%d\n", osd_port); 162 | #endif 163 | fprintf(stderr, "WFB-ng OSD version " WFB_OSD_VERSION "\n"); 164 | fprintf(stderr, "WFB-ng home page: \n"); 165 | exit(1); 166 | } 167 | } 168 | 169 | if (optind > argc) { 170 | goto show_usage; 171 | } 172 | 173 | #ifdef __GST_OPENGL__ 174 | printf("Use: mavlink_port=%d, rtp_port=%d, rtsp_url=%s, codec=%s, rtp_jitter=%d, osd_render=%d, screen_width=%d\n", 175 | osd_port, rtp_port, 176 | rtsp_url != NULL ? rtsp_url : "none", 177 | codec, rtp_jitter, osd_render, screen_width); 178 | 179 | osd_init(0, 0, 1, 1); 180 | fd = open_udp_socket_for_rx(osd_port); 181 | 182 | void* gst_thread_start(void *arg) 183 | { 184 | gst_main(rtp_port, codec, rtp_jitter, osd_render, screen_width, rtsp_url); 185 | fprintf(stderr, "gst thread exited\n"); 186 | exit(1); 187 | } 188 | 189 | pthread_t tid; 190 | pthread_create(&tid, NULL, gst_thread_start, NULL); 191 | 192 | while(1) 193 | { 194 | ssize_t rsize; 195 | while((rsize = recv(fd, buf, sizeof(buf), 0)) >= 0) 196 | { 197 | // Avoid race with rendering in gstreamer 198 | pthread_mutex_lock(&video_mutex); 199 | parse_mavlink_packet(buf, rsize); 200 | pthread_mutex_unlock(&video_mutex); 201 | } 202 | 203 | if (rsize < 0 && errno != EINTR) 204 | { 205 | perror("Error receiving packet"); 206 | exit(1); 207 | } 208 | } 209 | 210 | #else 211 | printf("Use mavlink_port=%d\n", osd_port); 212 | 213 | osd_init(0, 0, 1, 1); 214 | fd = open_udp_socket_for_rx(osd_port); 215 | 216 | if(fcntl(fd, F_SETFL, fcntl(fd, F_GETFL, 0) | O_NONBLOCK) < 0) 217 | { 218 | perror("Unable to set socket into nonblocked mode"); 219 | exit(1); 220 | } 221 | 222 | memset(fds, '\0', sizeof(fds)); 223 | fds[0].fd = fd; 224 | fds[0].events = POLLIN; 225 | 226 | signal(SIGTERM, sigterm_handler); 227 | signal(SIGINT, sigterm_handler); 228 | 229 | fprintf(stderr, "Starting event loop\n"); 230 | while(!finished) 231 | { 232 | cur_ts = GetSystimeMS(); 233 | uint64_t sleep_ts = render_ts > cur_ts ? render_ts - cur_ts : 0; 234 | int rc = poll(fds, 1, sleep_ts); 235 | 236 | if (rc < 0){ 237 | if (errno == EINTR || errno == EAGAIN) continue; 238 | perror("Poll error"); 239 | exit(1); 240 | } 241 | 242 | if (fds[0].revents & (POLLERR | POLLNVAL)) 243 | { 244 | fprintf(stderr, "socket error!"); 245 | exit(1); 246 | } 247 | 248 | if (fds[0].revents & POLLIN){ 249 | ssize_t rsize; 250 | while((rsize = recv(fd, buf, sizeof(buf), 0)) >= 0) 251 | { 252 | parse_mavlink_packet(buf, rsize); 253 | } 254 | if (rsize < 0 && errno != EWOULDBLOCK){ 255 | perror("Error receiving packet"); 256 | exit(1); 257 | } 258 | } 259 | 260 | cur_ts = GetSystimeMS(); 261 | if (render_ts <= cur_ts) 262 | { 263 | render_ts = cur_ts + 1000 / 30; // 30Hz osd refresh rate 264 | render(); 265 | } 266 | } 267 | fprintf(stderr, "Event loop finished\n"); 268 | #endif 269 | return 0; 270 | } 271 | -------------------------------------------------------------------------------- /math3d.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This program is free software; you can redistribute it and/or modify 3 | * it under the terms of the GNU General Public License as published by 4 | * the Free Software Foundation; either version 3 of the License, or 5 | * (at your option) any later version. 6 | * 7 | * This program is distributed in the hope that it will be useful, but 8 | * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 9 | * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 10 | * for more details. 11 | * 12 | * You should have received a copy of the GNU General Public License along 13 | * with this program; if not, write to the Free Software Foundation, Inc., 14 | * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 15 | */ 16 | /* 17 | * With Grateful Acknowledgements to the books writen by Andre LaMothe: 18 | * 19 | * 20 | */ 21 | #include "math3d.h" 22 | 23 | float my_cos_look[361]; // 1 extra element so we can store 0-360 inclusive 24 | float my_sin_look[361]; 25 | 26 | ////////////////////////////////////////////////////////////// 27 | 28 | float VECTOR3D_Length(VECTOR3D_PTR va) { 29 | // computes the magnitude of a vector, slow 30 | 31 | return ((float) sqrtf(va->x * va->x + va->y * va->y + va->z * va->z)); 32 | 33 | } // end VECTOR3D_Length 34 | 35 | /////////////////////////////////////////////////////////////// 36 | 37 | void VECTOR3D_Normalize(VECTOR3D_PTR va, VECTOR3D_PTR vn) { 38 | // normalizes the sent vector and returns the result in vn 39 | 40 | VECTOR3D_ZERO(vn); 41 | 42 | // compute length 43 | float length = VECTOR3D_Length(va); 44 | 45 | // test for zero length vector 46 | // if found return zero vector 47 | if (length < EPSILON_E5) 48 | return; 49 | 50 | float length_inv = 1.0 / length; 51 | 52 | // compute normalized version of vector 53 | vn->x = va->x * length_inv; 54 | vn->y = va->y * length_inv; 55 | vn->z = va->z * length_inv; 56 | 57 | } // end VECTOR3D_Normalize 58 | 59 | /////////////////////////////////////////////////////////////////////////////// 60 | 61 | float Fast_Distance_3D(float fx, float fy, float fz) { 62 | // this function computes the distance from the origin to x,y,z 63 | 64 | int32_t temp; // used for swaping 65 | int32_t x, y, z; // used for algorithm 66 | 67 | // make sure values are all positive 68 | x = fabs(fx) * 1024; 69 | y = fabs(fy) * 1024; 70 | z = fabs(fz) * 1024; 71 | 72 | // sort values 73 | if (y < x) 74 | SWAP3D(x, y, temp) 75 | 76 | if (z < y) 77 | SWAP3D(y, z, temp) 78 | 79 | if (y < x) 80 | SWAP3D(x, y, temp) 81 | 82 | int32_t dist = (z + 11 * (y >> 5) + (x >> 2)); 83 | 84 | // compute distance with 8% error 85 | return ((float) (dist >> 10)); 86 | 87 | } // 88 | 89 | //////////////////////////////////////////////////////////////// 90 | 91 | // these are the 4D version of the vector functions, they 92 | // assume that the vectors are 3D with a w, so w is left 93 | // out of all the operations 94 | 95 | void VECTOR4D_Build(VECTOR4D_PTR init, VECTOR4D_PTR term, VECTOR4D_PTR result) { 96 | // build a 4d vector 97 | result->x = term->x - init->x; 98 | result->y = term->y - init->y; 99 | result->z = term->z - init->z; 100 | result->w = 1; 101 | 102 | } // end VECTOR4D_Build 103 | 104 | //////////////////////////////////////////////////////////////// 105 | 106 | void VECTOR4D_Add(VECTOR4D_PTR va, VECTOR4D_PTR vb, VECTOR4D_PTR vsum) { 107 | // this function adds va+vb and return it in vsum 108 | vsum->x = va->x + vb->x; 109 | vsum->y = va->y + vb->y; 110 | vsum->z = va->z + vb->z; 111 | vsum->w = 1; 112 | 113 | } // end VECTOR4D_Add 114 | 115 | //////////////////////////////////////////////////////////// 116 | 117 | VECTOR4D VECTOR4D_Add1(VECTOR4D_PTR va, VECTOR4D_PTR vb) { 118 | // this function adds va+vb and returns the result on 119 | // the stack 120 | VECTOR4D vsum; 121 | 122 | vsum.x = va->x + vb->x; 123 | vsum.y = va->y + vb->y; 124 | vsum.z = va->z + vb->z; 125 | vsum.w = 1; 126 | 127 | // return result 128 | return (vsum); 129 | 130 | } // end VECTOR4D_Add 131 | 132 | //////////////////////////////////////////////////////////// 133 | 134 | void VECTOR4D_Sub(VECTOR4D_PTR va, VECTOR4D_PTR vb, VECTOR4D_PTR vdiff) { 135 | // this function subtracts va-vb and return it in vdiff 136 | // the stack 137 | vdiff->x = va->x - vb->x; 138 | vdiff->y = va->y - vb->y; 139 | vdiff->z = va->z - vb->z; 140 | vdiff->w = 1; 141 | 142 | } // end VECTOR4D_Sub 143 | 144 | //////////////////////////////////////////////////////////// 145 | 146 | VECTOR4D VECTOR4D_Sub1(VECTOR4D_PTR va, VECTOR4D_PTR vb) { 147 | // this function subtracts va-vb and returns the result on 148 | // the stack 149 | VECTOR4D vdiff; 150 | 151 | vdiff.x = va->x - vb->x; 152 | vdiff.y = va->y - vb->y; 153 | vdiff.z = va->z - vb->z; 154 | vdiff.w = 1; 155 | 156 | // return result 157 | return (vdiff); 158 | 159 | } // end VECTOR4D_Sub 160 | 161 | //////////////////////////////////////////////////////////// 162 | 163 | void VECTOR4D_Scale(float k, VECTOR4D_PTR va) { 164 | // this function scales a vector by the constant k, 165 | // in place , note w is left unchanged 166 | 167 | // multiply each component by scaling factor 168 | va->x *= k; 169 | va->y *= k; 170 | va->z *= k; 171 | va->w = 1; 172 | 173 | } // end VECTOR4D_Scale 174 | 175 | ///////////////////////////////////////////////////////////// 176 | 177 | void VECTOR4D_Scale1(float k, VECTOR4D_PTR va, VECTOR4D_PTR vscaled) { 178 | // this function scales a vector by the constant k, 179 | // leaves the original unchanged, and returns the result 180 | // in vres as well as on the stack 181 | 182 | // multiply each component by scaling factor 183 | vscaled->x = k * va->x; 184 | vscaled->y = k * va->y; 185 | vscaled->z = k * va->z; 186 | vscaled->w = 1; 187 | 188 | } // end VECTOR4D_Scale 189 | 190 | ////////////////////////////////////////////////////////////// 191 | 192 | float VECTOR4D_Dot(VECTOR4D_PTR va, VECTOR4D_PTR vb) { 193 | // computes the dot product between va and vb 194 | return ((va->x * vb->x) + (va->y * vb->y) + (va->z * vb->z)); 195 | } // end VECTOR4D_DOT 196 | 197 | ///////////////////////////////////////////////////////////// 198 | 199 | void VECTOR4D_Cross(VECTOR4D_PTR va, VECTOR4D_PTR vb, VECTOR4D_PTR vn) { 200 | // this function computes the cross product between va and vb 201 | // and returns the vector that is perpendicular to each in vn 202 | 203 | vn->x = ((va->y * vb->z) - (va->z * vb->y)); 204 | vn->y = -((va->x * vb->z) - (va->z * vb->x)); 205 | vn->z = ((va->x * vb->y) - (va->y * vb->x)); 206 | vn->w = 1; 207 | 208 | } // end VECTOR4D_Cross 209 | 210 | ///////////////////////////////////////////////////////////// 211 | 212 | VECTOR4D VECTOR4D_Cross1(VECTOR4D_PTR va, VECTOR4D_PTR vb) { 213 | // this function computes the cross product between va and vb 214 | // and returns the vector that is perpendicular to each 215 | 216 | VECTOR4D vn; 217 | 218 | vn.x = ((va->y * vb->z) - (va->z * vb->y)); 219 | vn.y = -((va->x * vb->z) - (va->z * vb->x)); 220 | vn.z = ((va->x * vb->y) - (va->y * vb->x)); 221 | vn.w = 1; 222 | 223 | // return result 224 | return (vn); 225 | 226 | } // end VECTOR4D_Cross 227 | 228 | ////////////////////////////////////////////////////////////// 229 | 230 | float VECTOR4D_Length(VECTOR4D_PTR va) { 231 | // computes the magnitude of a vector, slow 232 | 233 | return (sqrtf(va->x * va->x + va->y * va->y + va->z * va->z)); 234 | 235 | } // end VECTOR4D_Length 236 | 237 | /////////////////////////////////////////////////////////////// 238 | 239 | float VECTOR4D_Length_Fast(VECTOR4D_PTR va) { 240 | // computes the magnitude of a vector using an approximation 241 | // very fast 242 | return (Fast_Distance_3D(va->x, va->y, va->z)); 243 | 244 | } // end VECTOR4D_Length_Fast 245 | 246 | /////////////////////////////////////////////////////////////// 247 | 248 | void VECTOR4D_Normalize(VECTOR4D_PTR va) { 249 | // normalizes the sent vector and returns the result 250 | 251 | // compute length 252 | float length = sqrtf(va->x * va->x + va->y * va->y + va->z * va->z); 253 | 254 | // test for zero length vector 255 | // if found return zero vector 256 | if (length < EPSILON_E5) 257 | return; 258 | 259 | float length_inv = 1.0 / length; 260 | 261 | // compute normalized version of vector 262 | va->x *= length_inv; 263 | va->y *= length_inv; 264 | va->z *= length_inv; 265 | va->w = 1; 266 | 267 | } // end VECTOR4D_Normalize 268 | 269 | /////////////////////////////////////////////////////////////// 270 | 271 | void VECTOR4D_Normalize1(VECTOR4D_PTR va, VECTOR4D_PTR vn) { 272 | // normalizes the sent vector and returns the result in vn 273 | 274 | VECTOR4D_ZERO(vn); 275 | 276 | // compute length 277 | float length = sqrt(va->x * va->x + va->y * va->y + va->z * va->z); 278 | 279 | // test for zero length vector 280 | // if found return zero vector 281 | if (length < EPSILON_E5) 282 | return; 283 | 284 | float length_inv = 1.0 / length; 285 | 286 | // compute normalized version of vector 287 | vn->x = va->x * length_inv; 288 | vn->y = va->y * length_inv; 289 | vn->z = va->z * length_inv; 290 | vn->w = 1; 291 | 292 | } // end VECTOR4D_Normalize 293 | 294 | /////////////////////////////////////////////////////////////// 295 | 296 | float VECTOR4D_CosTh(VECTOR4D_PTR va, VECTOR4D_PTR vb) { 297 | // this function returns the cosine of the angle between 298 | // two vectors. Note, we could compute the actual angle, 299 | // many many times, in further calcs we will want ultimately 300 | // compute cos of the angle, so why not just leave it! 301 | return (VECTOR4D_Dot(va, vb) / (VECTOR4D_Length(va) * VECTOR4D_Length(vb))); 302 | 303 | } // end VECTOR4D_CosTh 304 | 305 | /////////////////////////////////////////////////////////////// 306 | int Mat_Mul1X2_3X2(MATRIX1X2_PTR ma, MATRIX3X2_PTR mb, MATRIX1X2_PTR mprod) { 307 | // this function multiplies a 1x2 matrix against a 308 | // 3x2 matrix - ma*mb and stores the result 309 | // using a dummy element for the 3rd element of the 1x2 310 | // to make the matrix multiply valid i.e. 1x3 X 3x2 311 | int col = 0; 312 | int index = 0; 313 | for (col = 0; col < 2; col++) { 314 | // compute dot product from row of ma 315 | // and column of mb 316 | 317 | float sum = 0; // used to hold result 318 | 319 | for (index = 0; index < 2; index++) { 320 | // add in next product pair 321 | sum += (ma->M[index] * mb->M[index][col]); 322 | } // end for index 323 | 324 | // add in last element * 1 325 | sum += mb->M[index][col]; 326 | 327 | // insert resulting col element 328 | mprod->M[col] = sum; 329 | 330 | } // end for col 331 | 332 | return (1); 333 | 334 | } // end Mat_Mul_1X2_3X2 335 | 336 | /////////////////////////////////////////////////////////////// 337 | 338 | void Mat_Add_4X4(MATRIX4X4_PTR ma, MATRIX4X4_PTR mb, MATRIX4X4_PTR msum) { 339 | // this function adds two 4x4 matrices together and 340 | // and stores the result 341 | 342 | for (int32_t row = 0; row < 4; row++) { 343 | for (int32_t col = 0; col < 4; col++) { 344 | // insert resulting row,col element 345 | msum->M[row][col] = ma->M[row][col] + mb->M[row][col]; 346 | } // end for col 347 | 348 | } // end for row 349 | 350 | } // end Mat_Add_4X4 351 | 352 | /////////////////////////////////////////////////////////////// 353 | 354 | void Mat_Mul_4X4(MATRIX4X4_PTR ma, MATRIX4X4_PTR mb, MATRIX4X4_PTR mprod) { 355 | // this function multiplies two 4x4 matrices together and 356 | // and stores the result in mprod 357 | // note later we will take advantage of the fact that we know 358 | // that w=1 always, and that the last column of a 4x4 is 359 | // always 0 360 | 361 | for (int32_t row = 0; row < 4; row++) { 362 | for (int32_t col = 0; col < 4; col++) { 363 | // compute dot product from row of ma 364 | // and column of mb 365 | 366 | float sum = 0; // used to hold result 367 | 368 | for (int32_t index = 0; index < 4; index++) { 369 | // add in next product pair 370 | sum += (ma->M[row][index] * mb->M[index][col]); 371 | } // end for index 372 | 373 | // insert resulting row,col element 374 | mprod->M[row][col] = sum; 375 | 376 | } // end for col 377 | 378 | } // end for row 379 | 380 | } // end Mat_Mul_4X4 381 | 382 | //////////////////////////////////////////////////////////////// 383 | 384 | void Mat_Mul_1X4_4X4(MATRIX1X4_PTR ma, MATRIX4X4_PTR mb, MATRIX1X4_PTR mprod) { 385 | // this function multiplies a 1x4 matrix against a 386 | // 4x4 matrix - ma*mb and stores the result 387 | // no tricks or assumptions here, just a straight multiply 388 | 389 | for (int32_t col = 0; col < 4; col++) { 390 | // compute dot product from row of ma 391 | // and column of mb 392 | float sum = 0; // used to hold result 393 | 394 | for (int32_t row = 0; row < 4; row++) { 395 | // add in next product pair 396 | sum += (ma->M[row] * mb->M[row][col]); 397 | } // end for index 398 | 399 | // insert resulting col element 400 | mprod->M[col] = sum; 401 | 402 | } // end for col 403 | 404 | } // end Mat_Mul_1X4_4X4 405 | 406 | //////////////////////////////////////////////////////////////////// 407 | 408 | void Mat_Mul_VECTOR3D_4X4(VECTOR3D_PTR va, MATRIX4X4_PTR mb, VECTOR3D_PTR vprod) { 409 | // this function multiplies a VECTOR3D against a 410 | // 4x4 matrix - ma*mb and stores the result in mprod 411 | // the function assumes that the vector refers to a 412 | // 4D homogenous vector, thus the function assumes that 413 | // w=1 to carry out the multiply, also the function 414 | // does not carry out the last column multiply since 415 | // we are assuming w=1, there is no point 416 | int32_t col, row; 417 | for (col = 0; col < 3; col++) { 418 | // compute dot product from row of ma 419 | // and column of mb 420 | float sum = 0; // used to hold result 421 | 422 | for (row = 0; row < 3; row++) { 423 | // add in next product pair 424 | sum += (va->M[row] * mb->M[row][col]); 425 | } // end for index 426 | 427 | // add in last element in column or w*mb[3][col] 428 | sum += mb->M[row][col]; 429 | 430 | // insert resulting col element 431 | vprod->M[col] = sum; 432 | 433 | } // end for col 434 | 435 | } // end Mat_Mul_VECTOR3D_4X4 436 | 437 | /////////////////////////////////////////////////////////////// 438 | 439 | void Mat_Mul_VECTOR3D_4X3(VECTOR3D_PTR va, MATRIX4X3_PTR mb, VECTOR3D_PTR vprod) { 440 | // this function multiplies a VECTOR3D against a 441 | // 4x3 matrix - ma*mb and stores the result in mprod 442 | // the function assumes that the vector refers to a 443 | // 4D homogenous vector, thus the function assumes that 444 | // w=1 to carry out the multiply, also the function 445 | // does not carry out the last column multiply since 446 | // we are assuming w=1, there is no point 447 | int32_t col, row; 448 | for (col = 0; col < 3; col++) { 449 | // compute dot product from row of ma 450 | // and column of mb 451 | float sum = 0; // used to hold result 452 | 453 | for (row = 0; row < 3; row++) { 454 | // add in next product pair 455 | sum += (va->M[row] * mb->M[row][col]); 456 | } // end for index 457 | 458 | // add in last element in column or w*mb[3][col] 459 | sum += mb->M[row][col]; 460 | 461 | // insert resulting col element 462 | vprod->M[col] = sum; 463 | 464 | } // end for col 465 | 466 | } // end Mat_Mul_VECTOR3D_4X3 467 | 468 | //////////////////////////////////////////////////////////////////// 469 | 470 | void Mat_Mul_VECTOR4D_4X4(VECTOR4D_PTR va, MATRIX4X4_PTR mb, VECTOR4D_PTR vprod) { 471 | // this function multiplies a VECTOR4D against a 472 | // 4x4 matrix - ma*mb and stores the result in mprod 473 | // the function makes no assumptions 474 | 475 | for (int32_t col = 0; col < 4; col++) { 476 | // compute dot product from row of ma 477 | // and column of mb 478 | float sum = 0; // used to hold result 479 | 480 | for (int32_t row = 0; row < 4; row++) { 481 | // add in next product pair 482 | sum += (va->M[row] * mb->M[row][col]); 483 | } // end for index 484 | 485 | // insert resulting col element 486 | vprod->M[col] = sum; 487 | 488 | } // end for col 489 | 490 | } // end Mat_Mul_VECTOR4D_4X4 491 | 492 | //////////////////////////////////////////////////////////////////// 493 | 494 | void Mat_Mul_VECTOR4D_4X3(VECTOR4D_PTR va, MATRIX4X4_PTR mb, VECTOR4D_PTR vprod) { 495 | // this function multiplies a VECTOR4D against a 496 | // 4x3 matrix - ma*mb and stores the result in mprod 497 | // the function assumes that the last column of 498 | // mb is [0 0 0 1]t , thus w just gets replicated 499 | // from the vector [x y z w] 500 | 501 | for (int32_t col = 0; col < 3; col++) { 502 | // compute dot product from row of ma 503 | // and column of mb 504 | float sum = 0; // used to hold result 505 | 506 | for (int32_t row = 0; row < 4; row++) { 507 | // add in next product pair 508 | sum += (va->M[row] * mb->M[row][col]); 509 | } // end for index 510 | 511 | // insert resulting col element 512 | vprod->M[col] = sum; 513 | 514 | } // end for col 515 | 516 | // copy back w element 517 | vprod->M[3] = va->M[3]; 518 | 519 | } // end Mat_Mul_VECTOR4D_4X3 520 | 521 | /////////////////////////////////////////////////////////////// 522 | 523 | void Mat_Init_4X4(MATRIX4X4_PTR ma, float m00, float m01, float m02, float m03, 524 | float m10, float m11, float m12, float m13, float m20, float m21, 525 | float m22, float m23, float m30, float m31, float m32, float m33) { 526 | // this function fills a 4x4 matrix with the sent data in 527 | // row major form 528 | ma->M00 = m00; 529 | ma->M01 = m01; 530 | ma->M02 = m02; 531 | ma->M03 = m03; 532 | ma->M10 = m10; 533 | ma->M11 = m11; 534 | ma->M12 = m12; 535 | ma->M13 = m13; 536 | ma->M20 = m20; 537 | ma->M21 = m21; 538 | ma->M22 = m22; 539 | ma->M23 = m23; 540 | ma->M30 = m30; 541 | ma->M31 = m31; 542 | ma->M32 = m32; 543 | ma->M33 = m33; 544 | 545 | } // end Mat_Init_4X4 546 | 547 | //////////////////////////////////////////////////////////////// 548 | 549 | int32_t Mat_Inverse_4X4(MATRIX4X4_PTR m, MATRIX4X4_PTR mi) { 550 | // computes the inverse of a 4x4, assumes that the last 551 | // column is [0 0 0 1]t 552 | 553 | float det = (m->M00 * (m->M11 * m->M22 - m->M12 * m->M21) 554 | - m->M01 * (m->M10 * m->M22 - m->M12 * m->M20) 555 | + m->M02 * (m->M10 * m->M21 - m->M11 * m->M20)); 556 | 557 | // test determinate to see if it's 0 558 | if (fabs(det) < EPSILON_E5) 559 | return (0); 560 | 561 | float det_inv = 1.0f / det; 562 | 563 | mi->M00 = det_inv * (m->M11 * m->M22 - m->M12 * m->M21); 564 | mi->M01 = -det_inv * (m->M01 * m->M22 - m->M02 * m->M21); 565 | mi->M02 = det_inv * (m->M01 * m->M12 - m->M02 * m->M11); 566 | mi->M03 = 0.0f; // always 0 567 | 568 | mi->M10 = -det_inv * (m->M10 * m->M22 - m->M12 * m->M20); 569 | mi->M11 = det_inv * (m->M00 * m->M22 - m->M02 * m->M20); 570 | mi->M12 = -det_inv * (m->M00 * m->M12 - m->M02 * m->M10); 571 | mi->M13 = 0.0f; // always 0 572 | 573 | mi->M20 = det_inv * (m->M10 * m->M21 - m->M11 * m->M20); 574 | mi->M21 = -det_inv * (m->M00 * m->M21 - m->M01 * m->M20); 575 | mi->M22 = det_inv * (m->M00 * m->M11 - m->M01 * m->M10); 576 | mi->M23 = 0.0f; // always 0 577 | 578 | mi->M30 = -(m->M30 * mi->M00 + m->M31 * mi->M10 + m->M32 * mi->M20); 579 | mi->M31 = -(m->M30 * mi->M01 + m->M31 * mi->M11 + m->M32 * mi->M21); 580 | mi->M32 = -(m->M30 * mi->M02 + m->M31 * mi->M12 + m->M32 * mi->M22); 581 | mi->M33 = 1.0f; // always 0 582 | 583 | // return success 584 | return (1); 585 | 586 | } // end Mat_Inverse_4X4 587 | 588 | /////////////////////////////////////////////////////////////// 589 | 590 | void PLANE3D_Init(PLANE3D_PTR plane, POINT3D_PTR p0, VECTOR3D_PTR normal, 591 | int32_t normalize) { 592 | // this function initializes a 3d plane 593 | 594 | // copy the point 595 | POINT3D_COPY(&plane->p0, p0); 596 | 597 | // if normalize is 1 then the normal is made into a unit vector 598 | if (!normalize) 599 | VECTOR3D_COPY(&plane->n, normal); 600 | else { 601 | // make normal into unit vector 602 | VECTOR3D_Normalize(normal, &plane->n); 603 | } // end else 604 | 605 | } // end PLANE3D_Init 606 | 607 | void Build_Sin_Cos_Tables(void) { 608 | 609 | // create sin/cos lookup table 610 | // note the creation of one extra element; 360 611 | // this helps with logic in using the tables 612 | 613 | // generate the tables 0 - 360 inclusive 614 | for (int ang = 0; ang <= 360; ang++) { 615 | // convert ang to radians 616 | float theta = ((float) ang) * XPI / 180.0; 617 | // insert next entry into table 618 | my_cos_look[ang] = cosf(theta); 619 | my_sin_look[ang] = sinf(theta); 620 | 621 | } // end for ang 622 | 623 | } // end Build_Sin_Cos_Tables 624 | 625 | float Fast_Sin(float theta) { 626 | // this function uses the sin_look[] lookup table, but 627 | // has logic to handle negative angles as well as fractional 628 | // angles via interpolation, use this for a more robust 629 | // sin computation that the blind lookup, but with with 630 | // a slight hit in speed 631 | 632 | // convert angle to 0-359 633 | theta = fmodf(theta, 360); 634 | 635 | // make angle positive 636 | if (theta < 0) 637 | theta += 360.0; 638 | 639 | // compute floor of theta and fractional part to interpolate 640 | int theta_int = (int) theta; 641 | float theta_frac = theta - theta_int; 642 | 643 | // now compute the value of sin(angle) using the lookup tables 644 | // and interpolating the fractional part, note that if theta_int 645 | // is equal to 359 then theta_int+1=360, but this is fine since the 646 | // table was made with the entries 0-360 inclusive 647 | return (my_sin_look[theta_int] 648 | + theta_frac * (my_sin_look[theta_int + 1] - my_sin_look[theta_int])); 649 | 650 | } // end Fast_Sin 651 | 652 | /////////////////////////////////////////////////////////////// 653 | 654 | float Fast_Cos(float theta) { 655 | // this function uses the cos_look[] lookup table, but 656 | // has logic to handle negative angles as well as fractional 657 | // angles via interpolation, use this for a more robust 658 | // cos computation that the blind lookup, but with with 659 | // a slight hit in speed 660 | 661 | // convert angle to 0-359 662 | theta = fmodf(theta, 360); 663 | 664 | // make angle positive 665 | if (theta < 0) 666 | theta += 360.0; 667 | 668 | // compute floor of theta and fractional part to interpolate 669 | int theta_int = (int) theta; 670 | float theta_frac = theta - theta_int; 671 | 672 | // now compute the value of sin(angle) using the lookup tables 673 | // and interpolating the fractional part, note that if theta_int 674 | // is equal to 359 then theta_int+1=360, but this is fine since the 675 | // table was made with the entries 0-360 inclusive 676 | return (my_cos_look[theta_int] 677 | + theta_frac * (my_cos_look[theta_int + 1] - my_cos_look[theta_int])); 678 | 679 | } // end Fast_Cos 680 | 681 | -------------------------------------------------------------------------------- /math3d.h: -------------------------------------------------------------------------------- 1 | #ifndef __MATH3D_H_ 2 | #define __MATH3D_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #define XPI 3.141592653589793f //3.14159265358979323846 9 | // some math macros 10 | #define DEG_TO_RAD(ang) ((ang) * XPI / 180.0) 11 | #define RAD_TO_DEG(rads) ((rads) * 180.0 / XPI) 12 | 13 | // storage for our lookup tables 14 | 15 | void Build_Sin_Cos_Tables(void); 16 | float Fast_Sin(float theta); 17 | float Fast_Cos(float theta); 18 | 19 | // a 2D vertex 20 | typedef struct VERTEX2DF_TYP 21 | { 22 | float x, y; // the vertex 23 | } VERTEX2DF, *VERTEX2DF_PTR; 24 | 25 | // note that 1x2 has the same memory layout as a VERTEX2DF, hence we 26 | // can use the matrix function written for a MATRIX1X2 to multiply a 27 | // VERTEX2DF by casting 28 | typedef struct MATRIX1X2_TYP 29 | { 30 | float M[2]; // data storage 31 | } MATRIX1X2, *MATRIX1X2_PTR; 32 | 33 | 34 | typedef struct MATRIX3X2_TYP 35 | { 36 | float M[3][2]; // data storage 37 | } MATRIX3X2, *MATRIX3X2_PTR; 38 | 39 | // 3D vector, point without the w //////////////////////// 40 | typedef struct VECTOR3D_TYP 41 | { 42 | union 43 | { 44 | float M[3]; // array indexed storage 45 | 46 | // explicit names 47 | struct 48 | { 49 | float x, y, z; 50 | }; // end struct 51 | 52 | }; // end union 53 | 54 | } VECTOR3D, POINT3D, *VECTOR3D_PTR, *POINT3D_PTR; 55 | 56 | // 4D homogenous vector, point with w //////////////////// 57 | typedef struct VECTOR4D_TYP 58 | { 59 | union 60 | { 61 | float M[4]; // array indexed storage 62 | 63 | // explicit names 64 | struct 65 | { 66 | float x, y, z, w; 67 | }; // end struct 68 | }; // end union 69 | 70 | } VECTOR4D, POINT4D, *VECTOR4D_PTR, *POINT4D_PTR; 71 | 72 | // 3D plane /////////////////////////////////////////////////// 73 | typedef struct PLANE3D_TYP 74 | { 75 | POINT3D p0; // point on the plane 76 | VECTOR3D n; // normal to the plane (not necessarily a unit vector) 77 | } PLANE3D, *PLANE3D_PTR; 78 | 79 | // 1x4 matrix ///////////////////////////////////////////// 80 | typedef struct MATRIX1X4_TYP 81 | { 82 | union 83 | { 84 | float M[4]; // array indexed data storage 85 | 86 | // storage in row major form with explicit names 87 | struct 88 | { 89 | float M00, M01, M02, M03; 90 | }; // end explicit names 91 | 92 | }; // end union 93 | } MATRIX1X4, *MATRIX1X4_PTR; 94 | 95 | // 4x4 matrix ///////////////////////////////////////////// 96 | typedef struct MATRIX4X4_TYP 97 | { 98 | union 99 | { 100 | float M[4][4]; // array indexed data storage 101 | 102 | // storage in row major form with explicit names 103 | struct 104 | { 105 | float M00, M01, M02, M03; 106 | float M10, M11, M12, M13; 107 | float M20, M21, M22, M23; 108 | float M30, M31, M32, M33; 109 | }; // end explicit names 110 | 111 | }; // end union 112 | 113 | } MATRIX4X4, *MATRIX4X4_PTR; 114 | 115 | // 4x3 matrix ///////////////////////////////////////////// 116 | typedef struct MATRIX4X3_TYP 117 | { 118 | union 119 | { 120 | float M[4][3]; // array indexed data storage 121 | 122 | // storage in row major form with explicit names 123 | struct 124 | { 125 | float M00, M01, M02; 126 | float M10, M11, M12; 127 | float M20, M21, M22; 128 | float M30, M31, M32; 129 | }; // end explicit names 130 | 131 | }; // end union 132 | 133 | } MATRIX4X3, *MATRIX4X3_PTR; 134 | 135 | 136 | // used to compute the min and max of two expresions 137 | #define MIN(a, b) (((a) < (b)) ? (a) : (b)) 138 | #define MAX(a, b) (((a) > (b)) ? (a) : (b)) 139 | 140 | // bit manipulation macros 141 | #define SET_BIT_MATH3D(word, bit_flag) ((word) = ((word) | (bit_flag))) 142 | #define RESET_BIT_MATH3D(word, bit_flag) ((word) = ((word) & (~bit_flag))) 143 | 144 | // used for swapping algorithm 145 | #define SWAP3D(a, b, t) { t = a; a = b; b = t; } 146 | #define EPSILON_E5 (float)(1E-5) 147 | float Fast_Distance_3D(float fx, float fy, float fz); 148 | 149 | static inline void VECTOR2D_INITXYZ(VERTEX2DF_PTR v, float x, float y) { 150 | (v)->x = (x); (v)->y = (y); 151 | } 152 | 153 | static inline void VECTOR2D_COPY(VERTEX2DF_PTR vdst, VERTEX2DF_PTR vsrc) { 154 | (vdst)->x = (vsrc)->x; (vdst)->y = (vsrc)->y; 155 | } 156 | 157 | static inline void VECTOR3D_ZERO(VECTOR3D_PTR v) { 158 | (v)->x = (v)->y = (v)->z = 0.0; 159 | } 160 | 161 | static inline void POINT3D_COPY(POINT3D_PTR vdst, POINT3D_PTR vsrc) { 162 | (vdst)->x = (vsrc)->x; (vdst)->y = (vsrc)->y; (vdst)->z = (vsrc)->z; 163 | } 164 | 165 | static inline void VECTOR3D_COPY(VECTOR3D_PTR vdst, VECTOR3D_PTR vsrc) { 166 | (vdst)->x = (vsrc)->x; (vdst)->y = (vsrc)->y; (vdst)->z = (vsrc)->z; 167 | } 168 | 169 | static inline void VECTOR3D_INITXYZ(VECTOR3D_PTR v, float x, float y, float z) { 170 | (v)->x = (x); (v)->y = (y); (v)->z = (z); 171 | } 172 | 173 | static inline void VECTOR4D_ZERO(VECTOR4D_PTR v) { 174 | (v)->x = (v)->y = (v)->z = 0.0; (v)->w = 1.0; 175 | } 176 | 177 | static inline void VECTOR4D_COPY(VECTOR4D_PTR vdst, VECTOR4D_PTR vsrc) { 178 | (vdst)->x = (vsrc)->x; (vdst)->y = (vsrc)->y; 179 | (vdst)->z = (vsrc)->z; (vdst)->w = (vsrc)->w; 180 | } 181 | 182 | static inline void VECTOR4D_INITXYZ(VECTOR4D_PTR v, float x, float y, float z) { 183 | (v)->x = (x); (v)->y = (y); (v)->z = (z); (v)->w = 1.0; 184 | } 185 | 186 | static inline void VECTOR4D_INITXYZW(VECTOR4D_PTR v, float x, float y, float z, float w) { 187 | (v)->x = (x); (v)->y = (y); (v)->z = (z); (v)->w = (w); 188 | } 189 | 190 | // used to convert from 4D homogenous to 4D non-homogenous 191 | static inline void VECTOR4D_DIV_BY_W(VECTOR4D_PTR v) { 192 | (v)->x /= (v)->w; (v)->y /= (v)->w; (v)->z /= (v)->w; 193 | } 194 | 195 | // 3d vector functions 196 | float VECTOR3D_Length(VECTOR3D_PTR va); 197 | void VECTOR3D_Normalize(VECTOR3D_PTR va, VECTOR3D_PTR vn); 198 | 199 | static inline int Mat_Init_3X2(MATRIX3X2_PTR ma, 200 | float m00, float m01, 201 | float m10, float m11, 202 | float m20, float m21) { 203 | // this function fills a 3x2 matrix with the sent data in row major form 204 | ma->M[0][0] = m00; ma->M[0][1] = m01; 205 | ma->M[1][0] = m10; ma->M[1][1] = m11; 206 | ma->M[2][0] = m20; ma->M[2][1] = m21; 207 | 208 | // return success 209 | return(1); 210 | 211 | } // end Mat_Init_3X2 212 | 213 | int Mat_Mul1X2_3X2(MATRIX1X2_PTR ma, 214 | MATRIX3X2_PTR mb, 215 | MATRIX1X2_PTR mprod); 216 | 217 | // 4d vector functions 218 | void VECTOR4D_Add(VECTOR4D_PTR va, VECTOR4D_PTR vb, VECTOR4D_PTR vsum); 219 | VECTOR4D VECTOR4D_Add1(VECTOR4D_PTR va, VECTOR4D_PTR vb); 220 | void VECTOR4D_Sub(VECTOR4D_PTR va, VECTOR4D_PTR vb, VECTOR4D_PTR vdiff); 221 | VECTOR4D VECTOR4D_Sub1(VECTOR4D_PTR va, VECTOR4D_PTR vb); 222 | void VECTOR4D_Scale(float k, VECTOR4D_PTR va); 223 | void VECTOR4D_Scale1(float k, VECTOR4D_PTR va, VECTOR4D_PTR vscaled); 224 | float VECTOR4D_Dot(VECTOR4D_PTR va, VECTOR4D_PTR vb); 225 | void VECTOR4D_Cross(VECTOR4D_PTR va, VECTOR4D_PTR vb, VECTOR4D_PTR vn); 226 | VECTOR4D VECTOR4D_Cross1(VECTOR4D_PTR va, VECTOR4D_PTR vb); 227 | float VECTOR4D_Length(VECTOR4D_PTR va); 228 | float VECTOR4D_Length_Fast(VECTOR4D_PTR va); 229 | void VECTOR4D_Normalize(VECTOR4D_PTR va); 230 | void VECTOR4D_Normalize1(VECTOR4D_PTR va, VECTOR4D_PTR vn); 231 | void VECTOR4D_Build(VECTOR4D_PTR init, VECTOR4D_PTR term, VECTOR4D_PTR result); 232 | float VECTOR4D_CosTh(VECTOR4D_PTR va, VECTOR4D_PTR vb); 233 | 234 | // 4x4 identity matrix 235 | const static MATRIX4X4 IMAT_4X4 = { 1, 0, 0, 0, 236 | 0, 1, 0, 0, 237 | 0, 0, 1, 0, 238 | 0, 0, 0, 1 }; 239 | // macros to set the identity matrix 240 | #define MAT_IDENTITY_4X4(m) { memcpy((void *)(m), (void *)&IMAT_4X4, sizeof(MATRIX4X4)); } 241 | #define MAT_COPY_4X4(src_mat, dest_mat) { memcpy((void *)(dest_mat), (void *)(src_mat), sizeof(MATRIX4X4) ); } 242 | 243 | // 4x4 matrix functions 244 | void Mat_Add_4X4(MATRIX4X4_PTR ma, MATRIX4X4_PTR mb, MATRIX4X4_PTR msum); 245 | void Mat_Mul_4X4(MATRIX4X4_PTR ma, MATRIX4X4_PTR mb, MATRIX4X4_PTR mprod); 246 | void Mat_Mul_1X4_4X4(MATRIX1X4_PTR ma, MATRIX4X4_PTR mb, MATRIX1X4_PTR mprod); 247 | void Mat_Mul_VECTOR3D_4X4(VECTOR3D_PTR va, MATRIX4X4_PTR mb, VECTOR3D_PTR vprod); 248 | void Mat_Mul_VECTOR3D_4X3(VECTOR3D_PTR va, MATRIX4X3_PTR mb, VECTOR3D_PTR vprod); 249 | void Mat_Mul_VECTOR4D_4X4(VECTOR4D_PTR va, MATRIX4X4_PTR mb, VECTOR4D_PTR vprod); 250 | void Mat_Mul_VECTOR4D_4X3(VECTOR4D_PTR va, MATRIX4X4_PTR mb, VECTOR4D_PTR vprod); 251 | int32_t Mat_Inverse_4X4(MATRIX4X4_PTR m, MATRIX4X4_PTR mi); 252 | void Mat_Init_4X4(MATRIX4X4_PTR ma, 253 | float m00, float m01, float m02, float m03, 254 | float m10, float m11, float m12, float m13, 255 | float m20, float m21, float m22, float m23, 256 | float m30, float m31, float m32, float m33); 257 | 258 | // 3d plane functions 259 | void PLANE3D_Init(PLANE3D_PTR plane, POINT3D_PTR p0, 260 | VECTOR3D_PTR normal, int32_t normalize); 261 | #endif //__MATH3D_H_ 262 | -------------------------------------------------------------------------------- /oglinit.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "eglstate.h" 4 | #include 5 | #include 6 | 7 | // oglinit sets the display, OpenGL|ES context and screen information 8 | // state holds the OGLES model information 9 | extern void oglinit(STATE_T * state) { 10 | int32_t success = 0; 11 | EGLBoolean result; 12 | EGLint num_config; 13 | 14 | static EGL_DISPMANX_WINDOW_T nativewindow; 15 | 16 | DISPMANX_ELEMENT_HANDLE_T dispman_element; 17 | DISPMANX_DISPLAY_HANDLE_T dispman_display; 18 | DISPMANX_UPDATE_HANDLE_T dispman_update; 19 | VC_RECT_T dst_rect; 20 | VC_RECT_T src_rect; 21 | 22 | static const EGLint attribute_list[] = { 23 | EGL_RED_SIZE, 8, 24 | EGL_GREEN_SIZE, 8, 25 | EGL_BLUE_SIZE, 8, 26 | EGL_ALPHA_SIZE, 8, 27 | EGL_SURFACE_TYPE, EGL_WINDOW_BIT, 28 | EGL_NONE 29 | }; 30 | 31 | EGLConfig config; 32 | 33 | // get an EGL display connection 34 | state->display = eglGetDisplay(EGL_DEFAULT_DISPLAY); 35 | assert(state->display != EGL_NO_DISPLAY); 36 | 37 | // initialize the EGL display connection 38 | result = eglInitialize(state->display, NULL, NULL); 39 | assert(EGL_FALSE != result); 40 | 41 | // bind OpenVG API 42 | eglBindAPI(EGL_OPENVG_API); 43 | 44 | // get an appropriate EGL frame buffer configuration 45 | result = eglChooseConfig(state->display, attribute_list, &config, 1, &num_config); 46 | assert(EGL_FALSE != result); 47 | 48 | // create an EGL rendering context 49 | state->context = eglCreateContext(state->display, config, EGL_NO_CONTEXT, NULL); 50 | assert(state->context != EGL_NO_CONTEXT); 51 | 52 | // create an EGL window surface 53 | success = graphics_get_display_size(0 /* LCD */ , &state->screen_width, 54 | &state->screen_height); 55 | assert(success >= 0); 56 | 57 | dst_rect.x = 0; 58 | dst_rect.y = 0; 59 | dst_rect.width = state->screen_width; 60 | dst_rect.height = state->screen_height; 61 | 62 | src_rect.x = 0; 63 | src_rect.y = 0; 64 | src_rect.width = state->screen_width << 16; 65 | src_rect.height = state->screen_height << 16; 66 | 67 | dispman_display = vc_dispmanx_display_open(0 /* LCD */ ); 68 | dispman_update = vc_dispmanx_update_start(0); 69 | 70 | dispman_element = vc_dispmanx_element_add(dispman_update, dispman_display, 1 /*layer */ , &dst_rect, 0 /*src */ , 71 | &src_rect, DISPMANX_PROTECTION_NONE, 0 /*alpha */ , 0 /*clamp */ , 72 | 0 /*transform */ ); 73 | 74 | nativewindow.element = dispman_element; 75 | nativewindow.width = state->screen_width; 76 | nativewindow.height = state->screen_height; 77 | vc_dispmanx_update_submit_sync(dispman_update); 78 | 79 | state->surface = eglCreateWindowSurface(state->display, config, &nativewindow, NULL); 80 | assert(state->surface != EGL_NO_SURFACE); 81 | 82 | // preserve the buffers on swap 83 | result = eglSurfaceAttrib(state->display, state->surface, EGL_SWAP_BEHAVIOR, EGL_BUFFER_PRESERVED); 84 | assert(EGL_FALSE != result); 85 | 86 | // connect the context to the surface 87 | result = eglMakeCurrent(state->display, state->surface, state->surface, state->context); 88 | assert(EGL_FALSE != result); 89 | 90 | // set up screen ratio 91 | glViewport(0, 0, (GLsizei) state->screen_width, (GLsizei) state->screen_height); 92 | 93 | glMatrixMode(GL_PROJECTION); 94 | glLoadIdentity(); 95 | 96 | float ratio = (float)state->screen_width / (float)state->screen_height; 97 | glFrustumf(-ratio, ratio, -1.0f, 1.0f, 1.0f, 10.0f); 98 | } 99 | -------------------------------------------------------------------------------- /osdconfig.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This program is free software; you can redistribute it and/or modify 3 | * it under the terms of the GNU General Public License as published by 4 | * the Free Software Foundation; either version 3 of the License, or 5 | * (at your option) any later version. 6 | * 7 | * This program is distributed in the hope that it will be useful, but 8 | * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 9 | * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 10 | * for more details. 11 | * 12 | * You should have received a copy of the GNU General Public License along 13 | * with this program; if not, write to the Free Software Foundation, Inc., 14 | * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 15 | */ 16 | 17 | #include "osdconfig.h" 18 | #include "graphengine.h" 19 | 20 | osd_params_t osd_params = { 21 | .Arm_en=1, 22 | .Arm_panel=1, 23 | .Arm_posX=GRAPHICS_RIGHT - 10, 24 | .Arm_posY=40, 25 | .Arm_fontsize=0, 26 | .Arm_align=2, 27 | .BattVolt_en=1, 28 | .BattVolt_panel=1, 29 | .BattVolt_posX=10, 30 | .BattVolt_posY=GRAPHICS_BOTTOM - 60, 31 | .BattVolt_fontsize=0, 32 | .BattVolt_align=0, 33 | .BattCurrent_en=1, 34 | .BattCurrent_panel=1, 35 | .BattCurrent_posX=2, 36 | .BattCurrent_posY=GRAPHICS_BOTTOM - 48, 37 | .BattCurrent_fontsize=0, 38 | .BattCurrent_align=0, 39 | .BattRemaining_en=1, 40 | .BattRemaining_panel=1, 41 | .BattRemaining_posX=18, 42 | .BattRemaining_posY=GRAPHICS_BOTTOM - 78, 43 | .BattRemaining_fontsize=0, 44 | .BattRemaining_align=0, 45 | .FlightMode_en=1, 46 | .FlightMode_panel=1, 47 | .FlightMode_posX=GRAPHICS_X_MIDDLE, 48 | .FlightMode_posY=64, 49 | .FlightMode_fontsize=1, 50 | .FlightMode_align=1, 51 | .GpsStatus_en=1, 52 | .GpsStatus_panel=1, 53 | .GpsStatus_posX=72, 54 | .GpsStatus_posY=GRAPHICS_BOTTOM - 78, 55 | .GpsStatus_fontsize=0, 56 | .GpsStatus_align=0, 57 | .GpsHDOP_en=0, 58 | .GpsHDOP_panel=1, 59 | .GpsHDOP_posX=72, 60 | .GpsHDOP_posY=GRAPHICS_BOTTOM - 68, 61 | .GpsHDOP_fontsize=0, 62 | .GpsHDOP_align=0, 63 | .GpsLat_en=1, 64 | .GpsLat_panel=1, 65 | .GpsLat_posX=72, 66 | .GpsLat_posY=GRAPHICS_BOTTOM - 60, 67 | .GpsLat_fontsize=0, 68 | .GpsLat_align=0, 69 | .GpsLon_en=1, 70 | .GpsLon_panel=1, 71 | .GpsLon_posX=72, 72 | .GpsLon_posY=GRAPHICS_BOTTOM - 48, 73 | .GpsLon_fontsize=0, 74 | .GpsLon_align=0, 75 | .Gps2Status_en=1, 76 | .Gps2Status_panel=2, 77 | .Gps2Status_posX=0, 78 | .Gps2Status_posY=GRAPHICS_BOTTOM - 38, 79 | .Gps2Status_fontsize=0, 80 | .Gps2Status_align=0, 81 | .Gps2HDOP_en=1, 82 | .Gps2HDOP_panel=2, 83 | .Gps2HDOP_posX=70, 84 | .Gps2HDOP_posY=GRAPHICS_BOTTOM - 38, 85 | .Gps2HDOP_fontsize=0, 86 | .Gps2HDOP_align=0, 87 | .Gps2Lat_en=1, 88 | .Gps2Lat_panel=2, 89 | .Gps2Lat_posX=GRAPHICS_RIGHT - 88, 90 | .Gps2Lat_posY=GRAPHICS_BOTTOM - 38, 91 | .Gps2Lat_fontsize=0, 92 | .Gps2Lat_align=0, 93 | .Gps2Lon_en=1, 94 | .Gps2Lon_panel=2, 95 | .Gps2Lon_posX=GRAPHICS_RIGHT - 8, 96 | .Gps2Lon_posY=GRAPHICS_BOTTOM - 38, 97 | .Gps2Lon_fontsize=0, 98 | .Gps2Lon_align=0, 99 | .Time_en=0, 100 | .Time_panel=1, 101 | .Time_posX=GRAPHICS_RIGHT - 10, 102 | .Time_posY=15, 103 | .Time_fontsize=0, 104 | .Time_align=2, 105 | .TALT_en=1, 106 | .TALT_panel=2, 107 | .TALT_posX=5, 108 | .TALT_posY=10, 109 | .TALT_fontsize=0, 110 | .TALT_align=0, 111 | .Alt_Scale_en=1, 112 | .Alt_Scale_panel=1, 113 | .Alt_Scale_posX=GRAPHICS_RIGHT - 160, 114 | .Alt_Scale_posY=GRAPHICS_Y_MIDDLE, 115 | .Alt_Scale_align=1, 116 | .Alt_Scale_source=1, 117 | .TSPD_en=1, 118 | .TSPD_panel=3, 119 | .TSPD_posX=125, 120 | .TSPD_posY=GRAPHICS_BOTTOM - 99, 121 | .TSPD_fontsize=0, 122 | .TSPD_align=1, 123 | .Speed_scale_en=1, 124 | .Speed_scale_panel=1, 125 | .Speed_scale_posX=160, 126 | .Speed_scale_align=0, 127 | .Speed_scale_source=0, 128 | .Speed_scale_posY=GRAPHICS_Y_MIDDLE, 129 | .Throt_en=1, 130 | .Throt_panel=1, 131 | .Throt_scale_en=1, 132 | .Throt_posX=GRAPHICS_X_MIDDLE, 133 | .Throt_posY=GRAPHICS_BOTTOM - 105, 134 | .CWH_home_dist_en=1, 135 | .CWH_home_dist_panel=1, 136 | .CWH_home_dist_posX=25, 137 | .CWH_home_dist_posY=90, 138 | .CWH_home_dist_fontsize=0, 139 | .CWH_home_dist_align=0, 140 | .CWH_wp_dist_en=0, 141 | .CWH_wp_dist_panel=0, 142 | .CWH_wp_dist_posX=40, 143 | .CWH_wp_dist_posY=24, 144 | .CWH_wp_dist_fontsize=0, 145 | .CWH_wp_dist_align=0, 146 | .CWH_Tmode_en=1, 147 | .CWH_Tmode_panel=1, 148 | .CWH_Tmode_posY=15, 149 | .CWH_Nmode_en=1, 150 | .CWH_Nmode_panel=1, 151 | .CWH_Nmode_posX=45, 152 | .CWH_Nmode_posY=45, 153 | .CWH_Nmode_radius=30, 154 | .CWH_Nmode_home_radius=37, 155 | .CWH_Nmode_wp_radius=37, 156 | .Atti_mp_en=1, 157 | .Atti_mp_panel=1, 158 | .Atti_mp_mode=0, 159 | .Atti_3D_en=1, 160 | .Atti_3D_panel=2, 161 | .Units_mode=0, 162 | .Max_panels=1, 163 | .PWM_Video_en=1, 164 | .PWM_Video_ch=8, 165 | .PWM_Video_value=1200, 166 | .PWM_Panel_en=0, 167 | .PWM_Panel_ch=0, 168 | .PWM_Panel_value=1200, 169 | .Alarm_posX=GRAPHICS_X_MIDDLE, 170 | .Alarm_posY=GRAPHICS_TOP + 90, 171 | .Alarm_fontsize=1, 172 | .Alarm_align=1, 173 | .Alarm_GPS_status_en=1, 174 | .Alarm_low_batt_en=1, 175 | .Alarm_low_batt=20, 176 | .Alarm_low_speed_en=0, 177 | .Alarm_low_speed=2, 178 | .Alarm_over_speed_en=0, 179 | .Alarm_over_speed=100, 180 | .Alarm_low_alt_en=0, 181 | .Alarm_low_alt=10, 182 | .Alarm_over_alt_en=0, 183 | .Alarm_over_alt=1000, 184 | .Alarm_rc_status_en=0, 185 | .Alarm_wfb_status_en=1, 186 | .ClimbRate_en=1, 187 | .ClimbRate_panel=1, 188 | .ClimbRate_posX=GRAPHICS_RIGHT - 150, 189 | .ClimbRate_posY=GRAPHICS_Y_MIDDLE, 190 | .ClimbRate_fontsize=0, 191 | .RSSI_en=0, 192 | .RSSI_type=0, 193 | .RSSI_panel=1, 194 | .RSSI_posX=72, 195 | .RSSI_posY=GRAPHICS_BOTTOM - 38, 196 | .RSSI_fontsize=0, 197 | .RSSI_align=0, 198 | .RSSI_min=0, 199 | .RSSI_max=100, 200 | .RSSI_raw_en=0, 201 | .FC_Protocol=0, 202 | .Wind_en=1, 203 | .Wind_panel=2, 204 | .Wind_posX=10, 205 | .Wind_posY=100, 206 | .Time_type=0, 207 | .Throttle_Scale_Type=1, 208 | .Atti_mp_posX=GRAPHICS_X_MIDDLE, 209 | .Atti_mp_posY=GRAPHICS_Y_MIDDLE, 210 | .Atti_mp_scale_real=1, 211 | .Atti_mp_scale_frac=0, 212 | .Atti_3D_posX=GRAPHICS_X_MIDDLE, 213 | .Atti_3D_posY=GRAPHICS_Y_MIDDLE, 214 | .Atti_3D_scale_real=1, 215 | .Atti_3D_scale_frac=0, 216 | .Atti_3D_map_radius=40, 217 | .osd_offsetY=0, 218 | .osd_offsetX=0, 219 | .firmware_ver=10, 220 | .video_mode=1, 221 | .BattConsumed_panel=1, 222 | .BattConsumed_posX=65, 223 | .BattConsumed_posY=GRAPHICS_BOTTOM - 38, 224 | .BattConsumed_fontsize=0, 225 | .BattConsumed_align=0, 226 | .TotalTripDist_panel=0, 227 | .TotalTripDist_posX=GRAPHICS_RIGHT - 10, 228 | .TotalTripDist_posY=GRAPHICS_BOTTOM - 78, 229 | .TotalTripDist_fontsize=0, 230 | .TotalTripDist_align=2, 231 | .Map_en=1, 232 | .Map_panel=4, 233 | .Map_radius=120, 234 | .Map_fontsize=1, 235 | .Map_H_align=0, 236 | .Map_V_align=0, 237 | .Relative_ALT_en=1, 238 | .Relative_ALT_panel=2, 239 | .Relative_ALT_posX=5, 240 | .Relative_ALT_posY=25, 241 | .Relative_ALT_fontsize=0, 242 | .Relative_ALT_align=0, 243 | .Alt_Scale_type=1, 244 | .Air_Speed_en=1, 245 | .Air_Speed_panel=2, 246 | .Air_Speed_posX=160, 247 | .Air_Speed_posY=GRAPHICS_BOTTOM - 88, 248 | .Air_Speed_fontsize=0, 249 | .Air_Speed_align=0, 250 | .Spd_Scale_type=0, 251 | .osd_offsetX_sign=0, 252 | .uart_bandrate=7, 253 | .Atti_mp_type=1, 254 | .Efficiency_en=0, 255 | .Efficiency_panel=0, 256 | .Efficiency_posX=GRAPHICS_RIGHT - 10, 257 | .Efficiency_posY=GRAPHICS_BOTTOM - 88, 258 | .Efficiency_fontsize=0, 259 | .Efficiency_align=2, 260 | .PWM_Video_mode=1, 261 | .PWM_Panel_mode=1, 262 | .LinkQuality_en=0, 263 | .LinkQuality_panel=0, 264 | .LinkQuality_posX=150, 265 | .LinkQuality_posY=GRAPHICS_BOTTOM - 53, 266 | .LinkQuality_fontsize=0, 267 | .LinkQuality_align=0, 268 | .LinkQuality_chan=5, 269 | .LinkQuality_min=1000, 270 | .LinkQuality_max=2000, 271 | .LinkQuality_type=0, 272 | .Vario_Graph_enabled=0, 273 | .Vario_Graph_panel=0, 274 | .Vario_Graph_posX=GRAPHICS_BOTTOM - 88, 275 | .Vario_Graph_posY=GRAPHICS_BOTTOM - 88, 276 | .HomeDirection_enabled=0, 277 | .HomeDirection_panel=1, 278 | .HomeDirection_posX=35, 279 | .HomeDirection_posY=65, 280 | .HomeLatitude_enabled=0, 281 | .HomeLatitude_panel=0, 282 | .HomeLatitude_posX=184, 283 | .HomeLatitude_posY=GRAPHICS_BOTTOM - 48, 284 | .HomeLatitude_fontsize=0, 285 | .HomeLatitude_align=0, 286 | .HomeLongitude_enabled=0, 287 | .HomeLongitude_panel=0, 288 | .HomeLongitude_posX=GRAPHICS_BOTTOM - 24, 289 | .HomeLongitude_posY=GRAPHICS_BOTTOM - 48, 290 | .HomeLongitude_fontsize=0, 291 | .HomeLongitude_align=0, 292 | .WFBState_en=1, 293 | .WFBState_panel=1, 294 | .WFBState_posX=GRAPHICS_RIGHT - 10, 295 | .WFBState_posY=15, 296 | .WFBState_fontsize=0, 297 | .WFBState_align=2, 298 | .OSDMessages_en=1, 299 | .OSDMessages_panel=1, 300 | .OSDMessages_posX=180, 301 | .OSDMessages_posY=285, 302 | }; 303 | -------------------------------------------------------------------------------- /osdconfig.h: -------------------------------------------------------------------------------- 1 | #ifndef __OSD_CONFIG_H 2 | #define __OSD_CONFIG_H 3 | 4 | #include 5 | 6 | typedef struct 7 | { 8 | uint16_t Arm_en; 9 | uint16_t Arm_panel; 10 | uint16_t Arm_posX; 11 | uint16_t Arm_posY; 12 | uint16_t Arm_fontsize; 13 | uint16_t Arm_align; 14 | 15 | uint16_t BattVolt_en; 16 | uint16_t BattVolt_panel; 17 | uint16_t BattVolt_posX; 18 | uint16_t BattVolt_posY; 19 | uint16_t BattVolt_fontsize; 20 | uint16_t BattVolt_align; 21 | 22 | uint16_t BattCurrent_en; 23 | uint16_t BattCurrent_panel; 24 | uint16_t BattCurrent_posX; 25 | uint16_t BattCurrent_posY; 26 | uint16_t BattCurrent_fontsize; 27 | uint16_t BattCurrent_align; 28 | 29 | uint16_t BattRemaining_en; 30 | uint16_t BattRemaining_panel; 31 | uint16_t BattRemaining_posX; 32 | uint16_t BattRemaining_posY; 33 | uint16_t BattRemaining_fontsize; 34 | uint16_t BattRemaining_align; 35 | 36 | uint16_t FlightMode_en; 37 | uint16_t FlightMode_panel; 38 | uint16_t FlightMode_posX; 39 | uint16_t FlightMode_posY; 40 | uint16_t FlightMode_fontsize; 41 | uint16_t FlightMode_align; 42 | 43 | uint16_t GpsStatus_en; 44 | uint16_t GpsStatus_panel; 45 | uint16_t GpsStatus_posX; 46 | uint16_t GpsStatus_posY; 47 | uint16_t GpsStatus_fontsize; 48 | uint16_t GpsStatus_align; 49 | 50 | uint16_t GpsHDOP_en; 51 | uint16_t GpsHDOP_panel; 52 | uint16_t GpsHDOP_posX; 53 | uint16_t GpsHDOP_posY; 54 | uint16_t GpsHDOP_fontsize; 55 | uint16_t GpsHDOP_align; 56 | 57 | uint16_t GpsLat_en; 58 | uint16_t GpsLat_panel; 59 | uint16_t GpsLat_posX; 60 | uint16_t GpsLat_posY; 61 | uint16_t GpsLat_fontsize; 62 | uint16_t GpsLat_align; 63 | 64 | uint16_t GpsLon_en; 65 | uint16_t GpsLon_panel; 66 | uint16_t GpsLon_posX; 67 | uint16_t GpsLon_posY; 68 | uint16_t GpsLon_fontsize; 69 | uint16_t GpsLon_align; 70 | 71 | uint16_t Gps2Status_en; 72 | uint16_t Gps2Status_panel; 73 | uint16_t Gps2Status_posX; 74 | uint16_t Gps2Status_posY; 75 | uint16_t Gps2Status_fontsize; 76 | uint16_t Gps2Status_align; 77 | 78 | uint16_t Gps2HDOP_en; 79 | uint16_t Gps2HDOP_panel; 80 | uint16_t Gps2HDOP_posX; 81 | uint16_t Gps2HDOP_posY; 82 | uint16_t Gps2HDOP_fontsize; 83 | uint16_t Gps2HDOP_align; 84 | 85 | uint16_t Gps2Lat_en; 86 | uint16_t Gps2Lat_panel; 87 | uint16_t Gps2Lat_posX; 88 | uint16_t Gps2Lat_posY; 89 | uint16_t Gps2Lat_fontsize; 90 | uint16_t Gps2Lat_align; 91 | 92 | uint16_t Gps2Lon_en; 93 | uint16_t Gps2Lon_panel; 94 | uint16_t Gps2Lon_posX; 95 | uint16_t Gps2Lon_posY; 96 | uint16_t Gps2Lon_fontsize; 97 | uint16_t Gps2Lon_align; 98 | 99 | uint16_t Time_en; 100 | uint16_t Time_panel; 101 | uint16_t Time_posX; 102 | uint16_t Time_posY; 103 | uint16_t Time_fontsize; 104 | uint16_t Time_align; 105 | 106 | uint16_t TALT_en; 107 | uint16_t TALT_panel; 108 | uint16_t TALT_posX; 109 | uint16_t TALT_posY; 110 | uint16_t TALT_fontsize; 111 | uint16_t TALT_align; 112 | uint16_t Alt_Scale_en; 113 | uint16_t Alt_Scale_panel; 114 | uint16_t Alt_Scale_posX; 115 | uint16_t Alt_Scale_align; 116 | uint16_t Alt_Scale_source; 117 | 118 | uint16_t TSPD_en; 119 | uint16_t TSPD_panel; 120 | uint16_t TSPD_posX; 121 | uint16_t TSPD_posY; 122 | uint16_t TSPD_fontsize; 123 | uint16_t TSPD_align; 124 | uint16_t Speed_scale_en; 125 | uint16_t Speed_scale_panel; 126 | uint16_t Speed_scale_posX; 127 | uint16_t Speed_scale_align; 128 | uint16_t Speed_scale_source; 129 | 130 | uint16_t Throt_en; 131 | uint16_t Throt_panel; 132 | uint16_t Throt_scale_en; 133 | uint16_t Throt_posX; 134 | uint16_t Throt_posY; 135 | 136 | uint16_t CWH_home_dist_en; 137 | uint16_t CWH_home_dist_panel; 138 | uint16_t CWH_home_dist_posX; 139 | uint16_t CWH_home_dist_posY; 140 | uint16_t CWH_home_dist_fontsize; 141 | uint16_t CWH_home_dist_align; 142 | 143 | uint16_t CWH_wp_dist_en; 144 | uint16_t CWH_wp_dist_panel; 145 | uint16_t CWH_wp_dist_posX; 146 | uint16_t CWH_wp_dist_posY; 147 | uint16_t CWH_wp_dist_fontsize; 148 | uint16_t CWH_wp_dist_align; 149 | 150 | uint16_t CWH_Tmode_en; 151 | uint16_t CWH_Tmode_panel; 152 | uint16_t CWH_Tmode_posY; 153 | uint16_t CWH_Nmode_en; 154 | uint16_t CWH_Nmode_panel; 155 | uint16_t CWH_Nmode_posX; 156 | uint16_t CWH_Nmode_posY; 157 | uint16_t CWH_Nmode_radius; 158 | uint16_t CWH_Nmode_home_radius; 159 | uint16_t CWH_Nmode_wp_radius; 160 | 161 | uint16_t Atti_mp_en; 162 | uint16_t Atti_mp_panel; 163 | uint16_t Atti_mp_mode; 164 | uint16_t Atti_3D_en; 165 | uint16_t Atti_3D_panel; 166 | 167 | //misc 168 | uint16_t Units_mode; 169 | uint16_t Max_panels; 170 | 171 | uint16_t PWM_Video_en; 172 | uint16_t PWM_Video_ch; 173 | uint16_t PWM_Video_value; 174 | uint16_t PWM_Panel_en; 175 | uint16_t PWM_Panel_ch; 176 | uint16_t PWM_Panel_value; 177 | 178 | uint16_t Alarm_posX; 179 | uint16_t Alarm_posY; 180 | uint16_t Alarm_fontsize; 181 | uint16_t Alarm_align; 182 | uint16_t Alarm_GPS_status_en; 183 | uint16_t Alarm_low_batt_en; 184 | uint16_t Alarm_low_batt; 185 | uint16_t Alarm_low_speed_en; 186 | uint16_t Alarm_low_speed; 187 | uint16_t Alarm_over_speed_en; 188 | uint16_t Alarm_over_speed; 189 | uint16_t Alarm_low_alt_en; 190 | uint16_t Alarm_low_alt; 191 | uint16_t Alarm_over_alt_en; 192 | uint16_t Alarm_over_alt; 193 | uint16_t Alarm_rc_status_en; 194 | uint16_t Alarm_wfb_status_en; 195 | 196 | uint16_t ClimbRate_en; 197 | uint16_t ClimbRate_panel; 198 | uint16_t ClimbRate_posX; 199 | uint16_t ClimbRate_posY; 200 | uint16_t ClimbRate_fontsize; 201 | //uint16_t ClimbRate_align; 202 | 203 | uint16_t RSSI_en; 204 | uint16_t RSSI_type; 205 | uint16_t RSSI_panel; 206 | uint16_t RSSI_posX; 207 | uint16_t RSSI_posY; 208 | uint16_t RSSI_fontsize; 209 | uint16_t RSSI_align; 210 | uint16_t RSSI_min; 211 | uint16_t RSSI_max; 212 | uint16_t RSSI_raw_en; 213 | 214 | uint16_t FC_Protocol; 215 | 216 | uint16_t Wind_en; 217 | uint16_t Wind_panel; 218 | uint16_t Wind_posX; 219 | uint16_t Wind_posY; 220 | 221 | uint16_t Time_type; 222 | 223 | uint16_t Throttle_Scale_Type; 224 | 225 | uint16_t Atti_mp_posX; 226 | uint16_t Atti_mp_posY; 227 | uint16_t Atti_mp_scale_real; 228 | uint16_t Atti_mp_scale_frac; 229 | uint16_t Atti_3D_posX; 230 | uint16_t Atti_3D_posY; 231 | uint16_t Atti_3D_scale_real; 232 | uint16_t Atti_3D_scale_frac; 233 | uint16_t Atti_3D_map_radius; 234 | 235 | uint16_t osd_offsetY; 236 | uint16_t osd_offsetX; 237 | 238 | /*from firmware version 6*/ 239 | uint16_t firmware_ver; 240 | uint16_t video_mode; 241 | 242 | /*from firmware version 7*/ 243 | uint16_t Speed_scale_posY; 244 | uint16_t Alt_Scale_posY; 245 | 246 | uint16_t BattConsumed_en; // total current drawn since startup in amp-hours 247 | uint16_t BattConsumed_panel; 248 | uint16_t BattConsumed_posX; 249 | uint16_t BattConsumed_posY; 250 | uint16_t BattConsumed_fontsize; 251 | uint16_t BattConsumed_align; 252 | 253 | uint16_t TotalTripDist_en; // total trip distance since startup, calculated in meter 254 | uint16_t TotalTripDist_panel; 255 | uint16_t TotalTripDist_posX; 256 | uint16_t TotalTripDist_posY; 257 | uint16_t TotalTripDist_fontsize; 258 | uint16_t TotalTripDist_align; 259 | 260 | uint16_t Map_en; 261 | uint16_t Map_panel; 262 | uint16_t Map_radius; 263 | uint16_t Map_fontsize; 264 | uint16_t Map_H_align; 265 | uint16_t Map_V_align; 266 | 267 | //v1.0.9 268 | uint16_t Relative_ALT_en; 269 | uint16_t Relative_ALT_panel; 270 | uint16_t Relative_ALT_posX; 271 | uint16_t Relative_ALT_posY; 272 | uint16_t Relative_ALT_fontsize; 273 | uint16_t Relative_ALT_align; 274 | 275 | uint16_t Alt_Scale_type; 276 | 277 | uint16_t Air_Speed_en; 278 | uint16_t Air_Speed_panel; 279 | uint16_t Air_Speed_posX; 280 | uint16_t Air_Speed_posY; 281 | uint16_t Air_Speed_fontsize; 282 | uint16_t Air_Speed_align; 283 | 284 | uint16_t Spd_Scale_type; 285 | 286 | //v1.1.0 287 | uint16_t osd_offsetX_sign; 288 | uint16_t uart_bandrate; 289 | 290 | uint16_t Atti_mp_type; 291 | 292 | uint16_t Efficiency_en; 293 | uint16_t Efficiency_panel; 294 | uint16_t Efficiency_posX; 295 | uint16_t Efficiency_posY; 296 | uint16_t Efficiency_fontsize; 297 | uint16_t Efficiency_align; 298 | 299 | uint16_t PWM_Video_mode; 300 | uint16_t PWM_Panel_mode; 301 | 302 | uint16_t LinkQuality_en; 303 | uint16_t LinkQuality_panel; 304 | uint16_t LinkQuality_posX; 305 | uint16_t LinkQuality_posY; 306 | uint16_t LinkQuality_fontsize; 307 | uint16_t LinkQuality_align; 308 | uint16_t LinkQuality_chan; 309 | uint16_t LinkQuality_min; 310 | uint16_t LinkQuality_max; 311 | uint16_t LinkQuality_type; 312 | 313 | uint16_t Vario_Graph_enabled; 314 | uint16_t Vario_Graph_panel; 315 | uint16_t Vario_Graph_posX; 316 | uint16_t Vario_Graph_posY; 317 | 318 | uint16_t HomeDirection_enabled; 319 | uint16_t HomeDirection_panel; 320 | uint16_t HomeDirection_posX; 321 | uint16_t HomeDirection_posY; 322 | 323 | uint16_t HomeLatitude_enabled; 324 | uint16_t HomeLatitude_panel; 325 | uint16_t HomeLatitude_posX; 326 | uint16_t HomeLatitude_posY; 327 | uint16_t HomeLatitude_fontsize; 328 | uint16_t HomeLatitude_align; 329 | 330 | uint16_t HomeLongitude_enabled; 331 | uint16_t HomeLongitude_panel; 332 | uint16_t HomeLongitude_posX; 333 | uint16_t HomeLongitude_posY; 334 | uint16_t HomeLongitude_fontsize; 335 | uint16_t HomeLongitude_align; 336 | 337 | uint16_t WFBState_en; 338 | uint16_t WFBState_panel; 339 | uint16_t WFBState_posX; 340 | uint16_t WFBState_posY; 341 | uint16_t WFBState_fontsize; 342 | uint16_t WFBState_align; 343 | 344 | uint16_t OSDMessages_en; 345 | uint16_t OSDMessages_panel; 346 | uint16_t OSDMessages_posX; 347 | uint16_t OSDMessages_posY; 348 | 349 | 350 | } osd_params_t; 351 | 352 | extern osd_params_t osd_params; 353 | 354 | #endif //__OSD_CONFIG_H 355 | -------------------------------------------------------------------------------- /osdmavlink.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This program is free software; you can redistribute it and/or modify 3 | * it under the terms of the GNU General Public License as published by 4 | * the Free Software Foundation; either version 3 of the License, or 5 | * (at your option) any later version. 6 | * 7 | * This program is distributed in the hope that it will be useful, but 8 | * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 9 | * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 10 | * for more details. 11 | * 12 | * You should have received a copy of the GNU General Public License along 13 | * with this program; if not, write to the Free Software Foundation, Inc., 14 | * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 15 | */ 16 | 17 | /* 18 | * With Grateful Acknowledgements to the projects: 19 | * MinimOSD - arducam-osd Controller(https://code.google.com/p/arducam-osd/) 20 | */ 21 | 22 | #include "osdmavlink.h" 23 | #include "osdvar.h" 24 | #include "osdconfig.h" 25 | #include "osdrender.h" 26 | 27 | float Rad2Deg(float x) 28 | { 29 | return x * (180.0F / M_PI); 30 | } 31 | 32 | void parse_mavlink_packet(uint8_t *buf, int buflen) 33 | { 34 | mavlink_status_t status; 35 | mavlink_message_t msg; 36 | uint8_t mavtype; 37 | 38 | for(int i = 0; i < buflen; i++) 39 | { 40 | uint8_t c = buf[i]; 41 | if (mavlink_parse_char(0, c, &msg, &status)) 42 | { 43 | //handle msg 44 | switch (msg.msgid) 45 | { 46 | case MAVLINK_MSG_ID_HEARTBEAT: 47 | { 48 | if ((msg.compid != 1) && (msg.compid != 50)) { 49 | // MAVMSG not from ardupilot(component ID:1) or pixhawk(component ID:50) 50 | break; 51 | } 52 | 53 | mavtype = mavlink_msg_heartbeat_get_type(&msg); 54 | if (mavtype == MAV_TYPE_GCS) { 55 | // MAVMSG from GCS 56 | break; 57 | } 58 | 59 | mav_system = msg.sysid; 60 | mav_component = msg.compid; 61 | mav_type = mavtype; 62 | autopilot = mavlink_msg_heartbeat_get_autopilot(&msg); 63 | base_mode = mavlink_msg_heartbeat_get_base_mode(&msg); 64 | custom_mode = mavlink_msg_heartbeat_get_custom_mode(&msg); 65 | 66 | last_motor_armed = motor_armed; 67 | motor_armed = base_mode & MAV_MODE_FLAG_SAFETY_ARMED; 68 | 69 | if (!last_motor_armed && motor_armed) { 70 | armed_start_time = GetSystimeMS(); 71 | } 72 | 73 | if (last_motor_armed && !motor_armed) { 74 | total_armed_time = GetSystimeMS() - armed_start_time + total_armed_time; 75 | armed_start_time = 0; 76 | } 77 | } 78 | break; 79 | 80 | case MAVLINK_MSG_ID_HOME_POSITION: 81 | { 82 | osd_home_lat = mavlink_msg_home_position_get_latitude(&msg) / 1e7; 83 | osd_home_lon = mavlink_msg_home_position_get_longitude(&msg) / 1e7; 84 | osd_home_alt = mavlink_msg_home_position_get_altitude(&msg) / 1000; 85 | osd_got_home = 1; 86 | break; 87 | } 88 | 89 | case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: 90 | { 91 | vtol_state = mavlink_msg_extended_sys_state_get_vtol_state(&msg); 92 | break; 93 | } 94 | 95 | case MAVLINK_MSG_ID_SYS_STATUS: 96 | { 97 | osd_vbat_A = (mavlink_msg_sys_status_get_voltage_battery(&msg) / 1000.0f); //Battery voltage, in millivolts (1 = 1 millivolt) 98 | osd_curr_A = mavlink_msg_sys_status_get_current_battery(&msg); //Battery current, in 10*milliamperes (1 = 10 milliampere) 99 | osd_battery_remaining_A = mavlink_msg_sys_status_get_battery_remaining(&msg); //Remaining battery energy: (0%: 0, 100%: 100) 100 | //custom_mode = mav_component;//Debug 101 | //osd_nav_mode = mav_system;//Debug 102 | } 103 | break; 104 | 105 | case MAVLINK_MSG_ID_BATTERY_STATUS: 106 | { 107 | osd_curr_consumed_mah = mavlink_msg_battery_status_get_current_consumed(&msg); 108 | } 109 | break; 110 | 111 | case MAVLINK_MSG_ID_GPS_RAW_INT: 112 | { 113 | osd_lat = mavlink_msg_gps_raw_int_get_lat(&msg) / 10000000.0; 114 | osd_lon = mavlink_msg_gps_raw_int_get_lon(&msg) / 10000000.0; 115 | osd_fix_type = mavlink_msg_gps_raw_int_get_fix_type(&msg); 116 | osd_hdop = mavlink_msg_gps_raw_int_get_eph(&msg); 117 | osd_satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(&msg); 118 | } 119 | break; 120 | 121 | case MAVLINK_MSG_ID_GPS2_RAW: 122 | { 123 | osd_lat2 = mavlink_msg_gps2_raw_get_lat(&msg) / 10000000.0; 124 | osd_lon2 = mavlink_msg_gps2_raw_get_lon(&msg) / 10000000.0; 125 | osd_fix_type2 = mavlink_msg_gps2_raw_get_fix_type(&msg); 126 | osd_hdop2 = mavlink_msg_gps2_raw_get_eph(&msg); 127 | osd_satellites_visible2 = mavlink_msg_gps2_raw_get_satellites_visible(&msg); 128 | } 129 | break; 130 | 131 | case MAVLINK_MSG_ID_VFR_HUD: 132 | { 133 | osd_airspeed = mavlink_msg_vfr_hud_get_airspeed(&msg); 134 | osd_groundspeed = mavlink_msg_vfr_hud_get_groundspeed(&msg); 135 | osd_heading = mavlink_msg_vfr_hud_get_heading(&msg); // 0..360 deg, 0=north 136 | osd_throttle = mavlink_msg_vfr_hud_get_throttle(&msg); 137 | osd_alt = mavlink_msg_vfr_hud_get_alt(&msg); 138 | osd_climb = mavlink_msg_vfr_hud_get_climb(&msg); 139 | } 140 | break; 141 | 142 | // Workaround for ardupilot 143 | case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: 144 | { 145 | mavlink_global_position_int_t global_position; 146 | mavlink_msg_global_position_int_decode(&msg, &global_position); 147 | osd_alt = global_position.alt / 1000.0; 148 | osd_rel_alt = global_position.relative_alt / 1000.0; 149 | } 150 | break; 151 | 152 | case MAVLINK_MSG_ID_ALTITUDE: 153 | { 154 | osd_bottom_clearance = mavlink_msg_altitude_get_bottom_clearance(&msg); 155 | osd_rel_alt = mavlink_msg_altitude_get_altitude_relative(&msg); 156 | } 157 | break; 158 | 159 | case MAVLINK_MSG_ID_ATTITUDE: 160 | { 161 | osd_pitch = Rad2Deg(mavlink_msg_attitude_get_pitch(&msg)); 162 | osd_roll = Rad2Deg(mavlink_msg_attitude_get_roll(&msg)); 163 | osd_yaw = Rad2Deg(mavlink_msg_attitude_get_yaw(&msg)); 164 | } 165 | break; 166 | 167 | case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: 168 | { 169 | nav_roll = mavlink_msg_nav_controller_output_get_nav_roll(&msg); 170 | nav_pitch = mavlink_msg_nav_controller_output_get_nav_pitch(&msg); 171 | nav_bearing = mavlink_msg_nav_controller_output_get_nav_bearing(&msg); 172 | wp_target_bearing = mavlink_msg_nav_controller_output_get_target_bearing(&msg); 173 | wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(&msg); 174 | alt_error = mavlink_msg_nav_controller_output_get_alt_error(&msg); 175 | aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(&msg); 176 | xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(&msg); 177 | } 178 | break; 179 | 180 | case MAVLINK_MSG_ID_MISSION_CURRENT: 181 | { 182 | wp_number = (uint8_t)mavlink_msg_mission_current_get_seq(&msg); 183 | } 184 | break; 185 | 186 | case MAVLINK_MSG_ID_RC_CHANNELS_RAW: 187 | { 188 | if (!osd_chan_cnt_above_eight) 189 | { 190 | osd_chan1_raw = mavlink_msg_rc_channels_raw_get_chan1_raw(&msg); 191 | osd_chan2_raw = mavlink_msg_rc_channels_raw_get_chan2_raw(&msg); 192 | osd_chan3_raw = mavlink_msg_rc_channels_raw_get_chan3_raw(&msg); 193 | osd_chan4_raw = mavlink_msg_rc_channels_raw_get_chan4_raw(&msg); 194 | osd_chan5_raw = mavlink_msg_rc_channels_raw_get_chan5_raw(&msg); 195 | osd_chan6_raw = mavlink_msg_rc_channels_raw_get_chan6_raw(&msg); 196 | osd_chan7_raw = mavlink_msg_rc_channels_raw_get_chan7_raw(&msg); 197 | osd_chan8_raw = mavlink_msg_rc_channels_raw_get_chan8_raw(&msg); 198 | osd_rssi = mavlink_msg_rc_channels_raw_get_rssi(&msg); 199 | } 200 | } 201 | break; 202 | 203 | case MAVLINK_MSG_ID_RC_CHANNELS: 204 | { 205 | osd_chan_cnt_above_eight = true; 206 | osd_chan1_raw = mavlink_msg_rc_channels_get_chan1_raw(&msg); 207 | osd_chan2_raw = mavlink_msg_rc_channels_get_chan2_raw(&msg); 208 | osd_chan3_raw = mavlink_msg_rc_channels_get_chan3_raw(&msg); 209 | osd_chan4_raw = mavlink_msg_rc_channels_get_chan4_raw(&msg); 210 | osd_chan5_raw = mavlink_msg_rc_channels_get_chan5_raw(&msg); 211 | osd_chan6_raw = mavlink_msg_rc_channels_get_chan6_raw(&msg); 212 | osd_chan7_raw = mavlink_msg_rc_channels_get_chan7_raw(&msg); 213 | osd_chan8_raw = mavlink_msg_rc_channels_get_chan8_raw(&msg); 214 | osd_chan9_raw = mavlink_msg_rc_channels_get_chan9_raw(&msg); 215 | osd_chan10_raw = mavlink_msg_rc_channels_get_chan10_raw(&msg); 216 | osd_chan11_raw = mavlink_msg_rc_channels_get_chan11_raw(&msg); 217 | osd_chan12_raw = mavlink_msg_rc_channels_get_chan12_raw(&msg); 218 | osd_chan13_raw = mavlink_msg_rc_channels_get_chan13_raw(&msg); 219 | osd_chan14_raw = mavlink_msg_rc_channels_get_chan14_raw(&msg); 220 | osd_chan15_raw = mavlink_msg_rc_channels_get_chan15_raw(&msg); 221 | osd_chan16_raw = mavlink_msg_rc_channels_get_chan16_raw(&msg); 222 | osd_rssi = mavlink_msg_rc_channels_get_rssi(&msg); 223 | } 224 | break; 225 | 226 | case MAVLINK_MSG_ID_RADIO_STATUS: 227 | { 228 | if ((msg.sysid != 3) || (msg.compid != 68)) { 229 | break; 230 | } 231 | 232 | wfb_rssi = (int8_t)mavlink_msg_radio_status_get_rssi(&msg); 233 | wfb_errors = mavlink_msg_radio_status_get_rxerrors(&msg); 234 | wfb_fec_fixed = mavlink_msg_radio_status_get_fixed(&msg); 235 | wfb_flags = mavlink_msg_radio_status_get_remnoise(&msg); 236 | } 237 | break; 238 | 239 | case MAVLINK_MSG_ID_STATUSTEXT: 240 | { 241 | osd_message_queue_tail = (osd_message_queue_tail + 1) % OSD_MAX_MESSAGES; 242 | osd_message_t *item = osd_message_queue + osd_message_queue_tail; 243 | item->severity = mavlink_msg_statustext_get_severity(&msg); 244 | mavlink_msg_statustext_get_text(&msg, item->message); 245 | item->message[sizeof(item->message) - 1] = '\0'; 246 | printf("Message: %s\n", item->message); 247 | } 248 | break; 249 | 250 | 251 | default: 252 | //Do nothing 253 | break; 254 | } //end switch(msg.msgid) 255 | } 256 | } 257 | } 258 | 259 | -------------------------------------------------------------------------------- /osdmavlink.h: -------------------------------------------------------------------------------- 1 | #ifndef __OSD_MAVLINK_H 2 | #define __OSD_MAVLINK_H 3 | 4 | #include "mavlink/common/mavlink.h" 5 | 6 | void parse_mavlink_packet(uint8_t *buf, int buflen); 7 | 8 | #endif //__OSD_MAVLINK_H 9 | -------------------------------------------------------------------------------- /osdrender.h: -------------------------------------------------------------------------------- 1 | #ifndef __RENDER_H__ 2 | #define __RENDER_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "graphengine.h" 20 | #include "mavlink/ardupilotmega/mavlink.h" 21 | 22 | void osd_init(int shift_x, int shift_y, float scale_x, float scale_y); 23 | 24 | /// GPS status codes 25 | enum GPS_Status { 26 | NO_GPS = 0, ///< No GPS connected/detected 27 | NO_FIX = 1, ///< Receiving valid GPS messages but no lock 28 | GPS_OK_FIX_2D = 2, ///< Receiving valid messages and 2D lock 29 | GPS_OK_FIX_3D = 3, ///< Receiving valid messages and 3D lock 30 | GPS_OK_FIX_3D_DGPS = 4, ///< Receiving valid messages and 3D lock with differential improvements 31 | GPS_OK_FIX_3D_RTK = 5, ///< Receiving valid messages and 3D lock, with relative-positioning improvements 32 | }; 33 | 34 | 35 | uint64_t GetSystimeMS(void); 36 | void RenderScreen(void); 37 | 38 | void draw_uav3d(void); 39 | void draw_uav2d(void); 40 | void draw_throttle(void); 41 | void draw_simple_attitude(void); 42 | void draw_home_direction(void); 43 | void draw_radar(void); 44 | void draw_flight_mode(void); 45 | void draw_arm_state(void); 46 | void draw_battery_voltage(void); 47 | void draw_battery_current(void); 48 | void draw_battery_remaining(void); 49 | void draw_battery_consumed(void); 50 | void draw_altitude_scale(void); 51 | void draw_absolute_altitude(void); 52 | void draw_relative_altitude(void); 53 | void draw_speed_scale(void); 54 | void draw_ground_speed(void); 55 | void draw_air_speed(void); 56 | void draw_vtol_speed(void); 57 | void draw_home_latitude(void); 58 | void draw_home_longitude(void); 59 | void draw_gps_status(void); 60 | void draw_gps_hdop(void); 61 | void draw_gps_latitude(void); 62 | void draw_gps_longitude(void); 63 | void draw_gps2_status(void); 64 | void draw_gps2_hdop(void); 65 | void draw_gps2_latitude(void); 66 | void draw_gps2_longitude(void); 67 | void draw_total_trip(void); 68 | void draw_time(void); 69 | void draw_climb_rate(void); 70 | void draw_rssi(void); 71 | void draw_wfb_state(void); 72 | void draw_link_quality(void); 73 | void draw_efficiency(void); 74 | void draw_wind(void); 75 | void draw_map(void); 76 | void draw_panel_changed(void); 77 | void draw_warning(void); 78 | void draw_CWH(void); 79 | void draw_head_wp_home(void); 80 | void draw_osd_messages(void); 81 | 82 | void cal_vars(void); 83 | 84 | 85 | void draw_vertical_scale(float v, int range, int halign, int x, int y, int height, int mintick_step, 86 | int majtick_step, int mintick_len, int majtick_len, 87 | int boundtick_len, __attribute__((unused)) int max_val, int flags, int min_val); 88 | 89 | void draw_linear_compass(int v, int home_dir, int range, int width, int x, int y, int mintick_step, 90 | int majtick_step, int mintick_len, int majtick_len, 91 | __attribute__((unused)) int flags); 92 | 93 | 94 | 95 | #endif 96 | -------------------------------------------------------------------------------- /osdvar.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This program is free software; you can redistribute it and/or modify 3 | * it under the terms of the GNU General Public License as published by 4 | * the Free Software Foundation; either version 3 of the License, or 5 | * (at your option) any later version. 6 | * 7 | * This program is distributed in the hope that it will be useful, but 8 | * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 9 | * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 10 | * for more details. 11 | * 12 | * You should have received a copy of the GNU General Public License along 13 | * with this program; if not, write to the Free Software Foundation, Inc., 14 | * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 15 | */ 16 | /* 17 | * With Grateful Acknowledgements to the projects: 18 | * MinimOSD - arducam-osd Controller(https://code.google.com/p/arducam-osd/) 19 | */ 20 | #include "osdvar.h" 21 | 22 | ///////////////////////////////////////////////////////////////////////// 23 | uint64_t lastWritePanel = 0; 24 | uint8_t mav_type; 25 | uint8_t mav_system; 26 | uint8_t mav_component; 27 | uint64_t sys_start_time = 0; 28 | uint64_t armed_start_time = 0; 29 | uint64_t total_armed_time = 0; 30 | uint8_t vtol_state = 0; 31 | 32 | ///////////////////////////////////////////////////////////////////////// 33 | float osd_vbat_A = 0.0f; // Battery A voltage in milivolt 34 | int16_t osd_curr_A = 0; // Battery A current 35 | int8_t osd_battery_remaining_A = 0; // 0 to 100 <=> 0 to 1000 36 | uint32_t osd_curr_consumed_mah = 0; 37 | 38 | float osd_pitch = 0.0f; // pitch from DCM 39 | float osd_roll = 0.0f; // roll from DCM 40 | float osd_yaw = 0.0f; // relative heading form DCM 41 | float osd_heading = 0.0f; // ground course heading from GPS 42 | 43 | double osd_lat = 0.0f; // latidude 44 | double osd_lon = 0.0f; // longitude 45 | uint8_t osd_satellites_visible = 0; // number of satelites 46 | uint8_t osd_fix_type = 0; // GPS lock 0-1=no fix, 2=2D, 3=3D 47 | double osd_hdop = 0.0f; 48 | 49 | double osd_lat2 = 0.0f; // latidude 50 | double osd_lon2 = 0.0f; // longitude 51 | uint8_t osd_satellites_visible2 = 0; // number of satelites 52 | uint8_t osd_fix_type2 = 0; // GPS lock 0-1=no fix, 2=2D, 3=3D 53 | double osd_hdop2 = 0.0f; 54 | 55 | float osd_airspeed = -1.0f; // airspeed 56 | float osd_groundspeed = 0.0f; // ground speed 57 | float osd_downVelocity = 0.0f; 58 | uint16_t osd_throttle = 0; // throtle 59 | float osd_alt = 0.0f; // altitude 60 | float osd_rel_alt = 0.0f; // relative altitude // jmmods 61 | float osd_bottom_clearance = NAN; 62 | float osd_climb = 0.0f; 63 | float osd_climb_ma[10]; 64 | int osd_climb_ma_index = 0; 65 | float osd_total_trip_dist = 0; 66 | 67 | float nav_roll = 0.0f; // Current desired roll in degrees 68 | float nav_pitch = 0.0f; // Current desired pitch in degrees 69 | int16_t nav_bearing = 0; // Current desired heading in degrees 70 | int16_t wp_target_bearing = 0; // Bearing to current MISSION/target in degrees 71 | int8_t wp_target_bearing_rotate_int = 0; 72 | uint16_t wp_dist = 0; // Distance to active MISSION in meters 73 | uint8_t wp_number = 0; // Current waypoint number 74 | float alt_error = 0.0f; // Current altitude error in meters 75 | float aspd_error = 0.0f; // Current airspeed error in meters/second 76 | float xtrack_error = 0.0f; // Current crosstrack error on x-y plane in meters 77 | float eff = 0.0f; //Efficiency 78 | uint8_t osd_linkquality = 0; 79 | 80 | bool motor_armed = false; 81 | bool last_motor_armed = false; 82 | uint8_t autopilot = 0; 83 | uint8_t base_mode = 0; 84 | uint32_t custom_mode = 0; 85 | 86 | bool osd_chan_cnt_above_eight = false; 87 | uint16_t osd_chan1_raw = 0; 88 | uint16_t osd_chan2_raw = 0; 89 | uint16_t osd_chan3_raw = 0; 90 | uint16_t osd_chan4_raw = 0; 91 | uint16_t osd_chan5_raw = 0; 92 | uint16_t osd_chan6_raw = 0; 93 | uint16_t osd_chan7_raw = 0; 94 | uint16_t osd_chan8_raw = 0; 95 | uint16_t osd_chan9_raw = 0; 96 | uint16_t osd_chan10_raw = 0; 97 | uint16_t osd_chan11_raw = 0; 98 | uint16_t osd_chan12_raw = 0; 99 | uint16_t osd_chan13_raw = 0; 100 | uint16_t osd_chan14_raw = 0; 101 | uint16_t osd_chan15_raw = 0; 102 | uint16_t osd_chan16_raw = 0; 103 | uint8_t osd_rssi = 0; //raw value from mavlink 104 | 105 | int8_t wfb_rssi = -128; //WFB rssi -128dBm 106 | uint16_t wfb_errors = 0; 107 | uint16_t wfb_fec_fixed = 0; 108 | int8_t wfb_flags = WFB_LINK_LOST; 109 | bool rc_lost = true; 110 | 111 | uint8_t osd_got_home = 0; // tels if got home position or not 112 | double osd_home_lat = 0.0f; // home latidude 113 | double osd_home_lon = 0.0f; // home longitude 114 | float osd_home_alt = 0.0f; 115 | long osd_home_distance = 0; // distance from home 116 | uint32_t osd_home_bearing = 0; 117 | uint8_t osd_alt_cnt = 0; // counter for stable osd_alt 118 | float osd_alt_prev = 0.0f; // previous altitude 119 | 120 | float osd_windSpeed = 0.0f; 121 | float osd_windDir = 0.0f; 122 | 123 | volatile uint8_t current_panel = 1; 124 | 125 | float atti_mp_scale = 0.0; 126 | float atti_3d_scale = 0.0; 127 | uint32_t atti_3d_min_clipX = 0; 128 | uint32_t atti_3d_max_clipX = 0; 129 | uint32_t atti_3d_min_clipY = 0; 130 | uint32_t atti_3d_max_clipY = 0; 131 | 132 | uint8_t got_mission_counts = 0; 133 | uint8_t enable_mission_count_request = 0; 134 | uint16_t mission_counts = 0; 135 | uint8_t enable_mission_item_request = 0; 136 | uint16_t current_mission_item_req_index = 0; 137 | 138 | uint16_t wp_counts = 0; 139 | uint8_t got_all_wps = 0; 140 | WAYPOINT wp_list[MAX_WAYPOINTS]; 141 | 142 | int8_t osd_offset_Y = 0; 143 | int8_t osd_offset_X = 0; 144 | 145 | osd_message_t osd_message_queue[OSD_MAX_MESSAGES]; 146 | int osd_message_queue_tail = -1; 147 | -------------------------------------------------------------------------------- /osdvar.h: -------------------------------------------------------------------------------- 1 | #ifndef __OSDVAR_H 2 | #define __OSDVAR_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #define WFB_LINK_LOST 1 9 | #define WFB_LINK_JAMMED 2 10 | 11 | ///////////////////////////////////////////////////////////////////////// 12 | extern uint64_t lastWritePanel; 13 | extern uint8_t mav_type; 14 | extern uint8_t mav_system; 15 | extern uint8_t mav_component; 16 | extern uint64_t sys_start_time; 17 | extern uint64_t armed_start_time; 18 | extern uint64_t total_armed_time; 19 | extern uint8_t vtol_state; 20 | 21 | ///////////////////////////////////////////////////////////////////////// 22 | extern float osd_vbat_A; // Battery A voltage in milivolt 23 | extern int16_t osd_curr_A; // Battery A current 24 | extern int8_t osd_battery_remaining_A; // 0 to 100 <=> 0 to 1000 25 | extern uint32_t osd_curr_consumed_mah; // total current drawn since startup in amp-hours 26 | 27 | extern float osd_pitch; // pitch from DCM 28 | extern float osd_roll; // roll from DCM 29 | extern float osd_yaw; // relative heading form DCM 30 | extern float osd_heading; // ground course heading from GPS 31 | 32 | extern double osd_lat; // latidude 33 | extern double osd_lon; // longitude 34 | extern uint8_t osd_satellites_visible; // number of satelites 35 | extern uint8_t osd_fix_type; // GPS lock 0-1=no fix, 2=2D, 3=3D 36 | extern double osd_hdop; 37 | 38 | extern double osd_lat2; // latidude 39 | extern double osd_lon2; // longitude 40 | extern uint8_t osd_satellites_visible2; // number of satelites 41 | extern uint8_t osd_fix_type2; // GPS lock 0-1=no fix, 2=2D, 3=3D 42 | extern double osd_hdop2; 43 | 44 | extern float osd_airspeed; // airspeed 45 | extern float osd_groundspeed; // ground speed 46 | extern float osd_downVelocity; // ground speed 47 | extern uint16_t osd_throttle; // throtle 48 | extern float osd_alt; // altitude 49 | extern float osd_rel_alt; // relative altitude // jmmods 50 | extern float osd_bottom_clearance; // relative altitude // jmmods 51 | extern float osd_climb; 52 | extern float osd_climb_ma[10]; 53 | extern int osd_climb_ma_index; 54 | extern float osd_total_trip_dist; //total trip distance since startup, calculated in meter 55 | 56 | extern float nav_roll; // Current desired roll in degrees 57 | extern float nav_pitch; // Current desired pitch in degrees 58 | extern int16_t nav_bearing; // Current desired heading in degrees 59 | extern int16_t wp_target_bearing; // Bearing to current MISSION/target in degrees 60 | extern int8_t wp_target_bearing_rotate_int; 61 | extern uint16_t wp_dist; // Distance to active MISSION in meters 62 | extern uint8_t wp_number; // Current waypoint number 63 | extern float alt_error; // Current altitude error in meters 64 | extern float aspd_error; // Current airspeed error in meters/second 65 | extern float xtrack_error; // Current crosstrack error on x-y plane in meters 66 | extern float eff; //Efficiency 67 | 68 | extern uint32_t custom_mode; 69 | extern bool motor_armed; 70 | extern bool last_motor_armed; 71 | extern uint8_t base_mode; 72 | extern uint8_t autopilot; 73 | 74 | extern bool osd_chan_cnt_above_eight; 75 | extern uint16_t osd_chan1_raw; 76 | extern uint16_t osd_chan2_raw; 77 | extern uint16_t osd_chan3_raw; 78 | extern uint16_t osd_chan4_raw; 79 | extern uint16_t osd_chan5_raw; 80 | extern uint16_t osd_chan6_raw; 81 | extern uint16_t osd_chan7_raw; 82 | extern uint16_t osd_chan8_raw; 83 | extern uint16_t osd_chan9_raw; 84 | extern uint16_t osd_chan10_raw; 85 | extern uint16_t osd_chan11_raw; 86 | extern uint16_t osd_chan12_raw; 87 | extern uint16_t osd_chan13_raw; 88 | extern uint16_t osd_chan14_raw; 89 | extern uint16_t osd_chan15_raw; 90 | extern uint16_t osd_chan16_raw; 91 | extern uint8_t osd_rssi; //raw value from mavlink 92 | 93 | extern int8_t wfb_rssi; //WFB rssi 94 | extern uint16_t wfb_errors; 95 | extern uint16_t wfb_fec_fixed; 96 | extern int8_t wfb_flags; 97 | extern bool rc_lost; 98 | 99 | extern uint8_t osd_got_home; // tels if got home position or not 100 | extern double osd_home_lat; // home latidude 101 | extern double osd_home_lon; // home longitude 102 | extern float osd_home_alt; 103 | extern long osd_home_distance; // distance from home 104 | extern uint32_t osd_home_bearing; 105 | extern uint8_t osd_alt_cnt; // counter for stable osd_alt 106 | extern float osd_alt_prev; // previous altitude 107 | 108 | extern float osd_windSpeed; 109 | extern float osd_windDir; 110 | 111 | extern volatile uint8_t current_panel; 112 | 113 | extern float atti_mp_scale; 114 | extern float atti_3d_scale; 115 | extern uint32_t atti_3d_min_clipX; 116 | extern uint32_t atti_3d_max_clipX; 117 | extern uint32_t atti_3d_min_clipY; 118 | extern uint32_t atti_3d_max_clipY; 119 | 120 | #define MAX_WAYPOINTS 20 121 | 122 | extern uint8_t got_mission_counts; 123 | extern uint8_t enable_mission_count_request; 124 | extern uint16_t mission_counts; 125 | extern uint8_t enable_mission_item_request; 126 | extern uint16_t current_mission_item_req_index; 127 | 128 | extern uint16_t wp_counts; 129 | extern uint8_t got_all_wps; 130 | // a self contained waypoint list 131 | typedef struct WAYPOINT_TYP { 132 | // float para1; 133 | // float para2; 134 | // float para3; 135 | // float para4; 136 | 137 | float x; 138 | float y; 139 | float z; 140 | 141 | uint16_t seq; 142 | uint16_t cmd; 143 | 144 | // uint8_t frame; 145 | uint8_t current; 146 | // uint8_t autocontinue; 147 | } WAYPOINT, *WAYPOINT_PTR; 148 | 149 | extern WAYPOINT wp_list[MAX_WAYPOINTS]; 150 | 151 | #define OSD_MAX_MESSAGES 6 152 | 153 | typedef struct 154 | { 155 | uint8_t severity; 156 | char message[51]; 157 | } osd_message_t; 158 | 159 | extern osd_message_t osd_message_queue[OSD_MAX_MESSAGES]; 160 | extern int osd_message_queue_tail; 161 | 162 | extern int8_t osd_offset_Y; 163 | extern int8_t osd_offset_X; 164 | #endif 165 | -------------------------------------------------------------------------------- /px4_custom_mode.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @file px4_custom_mode.h 36 | * PX4 custom flight modes 37 | * 38 | * @author Anton Babushkin 39 | */ 40 | 41 | #ifndef PX4_CUSTOM_MODE_H_ 42 | #define PX4_CUSTOM_MODE_H_ 43 | 44 | #include 45 | 46 | enum PX4_CUSTOM_MAIN_MODE { 47 | PX4_CUSTOM_MAIN_MODE_MANUAL = 1, 48 | PX4_CUSTOM_MAIN_MODE_ALTCTL, 49 | PX4_CUSTOM_MAIN_MODE_POSCTL, 50 | PX4_CUSTOM_MAIN_MODE_AUTO, 51 | PX4_CUSTOM_MAIN_MODE_ACRO, 52 | PX4_CUSTOM_MAIN_MODE_OFFBOARD, 53 | PX4_CUSTOM_MAIN_MODE_STABILIZED, 54 | PX4_CUSTOM_MAIN_MODE_RATTITUDE 55 | }; 56 | 57 | enum PX4_CUSTOM_SUB_MODE_AUTO { 58 | PX4_CUSTOM_SUB_MODE_AUTO_READY = 1, 59 | PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, 60 | PX4_CUSTOM_SUB_MODE_AUTO_LOITER, 61 | PX4_CUSTOM_SUB_MODE_AUTO_MISSION, 62 | PX4_CUSTOM_SUB_MODE_AUTO_RTL, 63 | PX4_CUSTOM_SUB_MODE_AUTO_LAND, 64 | PX4_CUSTOM_SUB_MODE_AUTO_RTGS, 65 | PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET 66 | }; 67 | 68 | union px4_custom_mode { 69 | struct { 70 | uint16_t reserved; 71 | uint8_t main_mode; 72 | uint8_t sub_mode; 73 | }__attribute__((packed)); 74 | uint32_t data; 75 | float data_float; 76 | }__attribute__((packed)); 77 | 78 | #endif /* PX4_CUSTOM_MODE_H_ */ 79 | -------------------------------------------------------------------------------- /rpi_setup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | set -x 4 | 5 | apt-get update 6 | apt-get install build-essential libraspberrypi0 libraspberrypi-bin gstreamer1.0-tools gstreamer1.0-plugins-good 7 | 8 | make mode=rpi3 9 | 10 | cp -a osd fpv_video/{fpv_video,fpv_video.sh} /usr/bin 11 | 12 | cat > /etc/systemd/system/osd.service < /etc/systemd/system/fpv-video.service < 3 | -------------------------------------------------------------------------------- /version.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | import sys 5 | import time 6 | import datetime 7 | 8 | 9 | def main(): 10 | # last commit timestamp + branch name 11 | ttuple = time.gmtime(int(sys.argv[1])) 12 | branch = sys.argv[2] 13 | 14 | if branch.startswith('release-'): 15 | release = [int(i) for i in branch.split('-')[1].split('.')] 16 | delta = datetime.datetime(*ttuple[:6]) - datetime.datetime(2000 + release[0], release[1], 1) 17 | 18 | print('%d.%d.%s.%d' % (release[0], release[1], '0.%d' % (999 + delta.days,) if delta.days < 0 else (delta.days + 1), 19 | ttuple.tm_hour * 3600 + ttuple.tm_min * 60 + ttuple.tm_sec)) 20 | else: 21 | print('%d.%d.%d.%d' % (ttuple.tm_year - 2000, ttuple.tm_mon, ttuple.tm_mday, 22 | ttuple.tm_hour * 3600 + ttuple.tm_min * 60 + ttuple.tm_sec)) 23 | 24 | 25 | if __name__ == '__main__': 26 | main() 27 | --------------------------------------------------------------------------------