Add EGL init code

Makefile

@@ -1,13 +1,13 @@
 CPPFLAGS+= -g -Wall -std=gnu99 -I/opt/vc/include/ -I/opt/vc/include/interface/vcos/pthreads -I/opt/vc/include/interface/vmcs_host/linux
 LDFLAGS+= -lfreetype -lz
-LDFLAGS+=-L/opt/vc/lib/ -lGLESv2 -lEGL -lopenmaxil -lbcm_host -lvcos -lvchiq_arm -lpthread -lrt -lm -lshapes
+LDFLAGS+=-L/opt/vc/lib/ -lGLESv2 -lEGL -lopenmaxil -lbcm_host -lvcos -lvchiq_arm -lpthread -lrt -lm
 
 all: osd
 
 %.o: %.c
 	gcc -c -o $@ $< $(CPPFLAGS)
 
-osd: 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
+osd: 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
 	gcc -o $@ $^ $(LDFLAGS)
 
 clean:

eglstate.h

@@ -0,0 +1,11 @@
+typedef struct {
+	uint32_t screen_width;
+	uint32_t screen_height;
+	// OpenGL|ES objects
+	EGLDisplay display;
+
+	EGLSurface surface;
+	EGLContext context;
+} STATE_T;
+
+extern void oglinit(STATE_T *);

graphengine.c

@@ -19,19 +19,17 @@
  * Tau Labs - Brain FPV Flight Controller(https://github.com/BrainFPV/TauLabs)
  */
 
-#include "bcm_host.h"
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
 #include <assert.h>
+#include <bcm_host.h>
 #include "VG/openvg.h"
 #include "VG/vgu.h"
-#include "fontinfo.h"
-#include "shapes.h"
 #include <math.h>
 #include <EGL/egl.h>
 #include <EGL/eglext.h>
-#include <stdio.h>
+#include "eglstate.h"
 
 #include "graphengine.h"
 #include "math3d.h"
@@ -40,13 +38,16 @@
 #include "font8x10.h"
 
 
-int screen_width = 0, screen_height = 0;
 static uint8_t* video_buf = NULL;
+STATE_T ogl_state;
 
+void render_init()
+{
+    bcm_host_init();
+    memset(&ogl_state, 0, sizeof(ogl_state));
+    oglinit(&ogl_state);
 
-void render_init() {
-    init(&screen_width, &screen_height);
-    printf("Screen HW %dx%d, virtual %dx%d \n", screen_width, screen_height, GRAPHICS_WIDTH, GRAPHICS_HEIGHT);
+    printf("Screen HW %dx%d, virtual %dx%d \n", ogl_state.screen_width, ogl_state.screen_height, GRAPHICS_WIDTH, GRAPHICS_HEIGHT);
     video_buf = malloc(GRAPHICS_WIDTH * GRAPHICS_HEIGHT * 4); // ARGB
 }
 
@@ -55,13 +56,39 @@ void clearGraphics(void) {
 }
 
 void displayGraphics(void) {
-    Start(screen_width, screen_height);
+    VGfloat color1[4] = { 255, 255, 255, 0 };
+    VGfloat color2[4] = { 0, 0, 0, 0 };
+
+    vgSetfv(VG_CLEAR_COLOR, 4, color1);
+    vgClear(0, 0, ogl_state.screen_width, ogl_state.screen_height);
+
+    // set fill
+    VGPaint fillPaint = vgCreatePaint();
+    vgSetParameteri(fillPaint, VG_PAINT_TYPE, VG_PAINT_TYPE_COLOR);
+    vgSetParameterfv(fillPaint, VG_PAINT_COLOR, 4, color2);
+    vgSetPaint(fillPaint, VG_FILL_PATH);
+    vgDestroyPaint(fillPaint);
+
+    // set stroke
+    VGPaint strokePaint = vgCreatePaint();
+    vgSetParameteri(strokePaint, VG_PAINT_TYPE, VG_PAINT_TYPE_COLOR);
+    vgSetParameterfv(strokePaint, VG_PAINT_COLOR, 4, color2);
+    vgSetPaint(strokePaint, VG_STROKE_PATH);
+    vgDestroyPaint(strokePaint);
+
+    // set stroke width
+    vgSetf(VG_STROKE_LINE_WIDTH, 0);
+    vgSeti(VG_STROKE_CAP_STYLE, VG_CAP_BUTT);
+    vgSeti(VG_STROKE_JOIN_STYLE, VG_JOIN_MITER);
+
+    vgLoadIdentity();
 
+    // convert video buffer into image
     unsigned int dstride = GRAPHICS_WIDTH * 4;
     VGImageFormat rgbaFormat = VG_sABGR_8888;
     VGImage img = vgCreateImage(rgbaFormat, GRAPHICS_WIDTH, GRAPHICS_HEIGHT, VG_IMAGE_QUALITY_BETTER);
-    float screen_scale_x = (float)screen_width / GRAPHICS_WIDTH;
-    float screen_scale_y = (float)screen_height / GRAPHICS_HEIGHT;
+    float screen_scale_x = (float)ogl_state.screen_width / GRAPHICS_WIDTH;
+    float screen_scale_y = (float)ogl_state.screen_height / GRAPHICS_HEIGHT;
     float screen_scale = MIN(screen_scale_x, screen_scale_y);
 
     vgImageSubData(img, (void *)video_buf, dstride, rgbaFormat, 0, 0, GRAPHICS_WIDTH, GRAPHICS_HEIGHT);
@@ -72,7 +99,11 @@ void displayGraphics(void) {
     vgDrawImage(img);
     vgSeti(VG_MATRIX_MODE, VG_MATRIX_PATH_USER_TO_SURFACE);
     vgDestroyImage(img);
-    End();
+
+    // display
+    assert(vgGetError() == VG_NO_ERROR);
+    eglSwapBuffers(ogl_state.display, ogl_state.surface);
+    assert(eglGetError() == EGL_SUCCESS);
 }
 
 //void drawArrow(uint16_t x, uint16_t y, uint16_t angle, uint16_t size_quarter)

oglinit.c

@@ -0,0 +1,98 @@
+#include <EGL/egl.h>
+#include <GLES/gl.h>
+#include "eglstate.h"
+#include <bcm_host.h>
+#include <assert.h>
+
+// oglinit sets the display, OpenGL|ES context and screen information
+// state holds the OGLES model information
+extern void oglinit(STATE_T * state) {
+	int32_t success = 0;
+	EGLBoolean result;
+	EGLint num_config;
+
+	static EGL_DISPMANX_WINDOW_T nativewindow;
+
+	DISPMANX_ELEMENT_HANDLE_T dispman_element;
+	DISPMANX_DISPLAY_HANDLE_T dispman_display;
+	DISPMANX_UPDATE_HANDLE_T dispman_update;
+	VC_RECT_T dst_rect;
+	VC_RECT_T src_rect;
+
+	static const EGLint attribute_list[] = {
+		EGL_RED_SIZE, 8,
+		EGL_GREEN_SIZE, 8,
+		EGL_BLUE_SIZE, 8,
+		EGL_ALPHA_SIZE, 8,
+		EGL_SURFACE_TYPE, EGL_WINDOW_BIT,
+		EGL_NONE
+	};
+
+	EGLConfig config;
+
+	// get an EGL display connection
+	state->display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
+	assert(state->display != EGL_NO_DISPLAY);
+
+	// initialize the EGL display connection
+	result = eglInitialize(state->display, NULL, NULL);
+	assert(EGL_FALSE != result);
+
+	// bind OpenVG API
+	eglBindAPI(EGL_OPENVG_API);
+
+	// get an appropriate EGL frame buffer configuration
+	result = eglChooseConfig(state->display, attribute_list, &config, 1, &num_config);
+	assert(EGL_FALSE != result);
+
+	// create an EGL rendering context
+	state->context = eglCreateContext(state->display, config, EGL_NO_CONTEXT, NULL);
+	assert(state->context != EGL_NO_CONTEXT);
+
+	// create an EGL window surface
+	success = graphics_get_display_size(0 /* LCD */ , &state->screen_width,
+					    &state->screen_height);
+	assert(success >= 0);
+
+	dst_rect.x = 0;
+	dst_rect.y = 0;
+	dst_rect.width = state->screen_width;
+	dst_rect.height = state->screen_height;
+
+	src_rect.x = 0;
+	src_rect.y = 0;
+	src_rect.width = state->screen_width << 16;
+	src_rect.height = state->screen_height << 16;
+
+	dispman_display = vc_dispmanx_display_open(0 /* LCD */ );
+	dispman_update = vc_dispmanx_update_start(0);
+
+	dispman_element = vc_dispmanx_element_add(dispman_update, dispman_display, 1 /*layer */ , &dst_rect, 0 /*src */ ,
+						  &src_rect, DISPMANX_PROTECTION_NONE, 0 /*alpha */ , 0 /*clamp */ ,
+						  0 /*transform */ );
+
+	nativewindow.element = dispman_element;
+	nativewindow.width = state->screen_width;
+	nativewindow.height = state->screen_height;
+	vc_dispmanx_update_submit_sync(dispman_update);
+
+	state->surface = eglCreateWindowSurface(state->display, config, &nativewindow, NULL);
+	assert(state->surface != EGL_NO_SURFACE);
+
+	// preserve the buffers on swap
+	result = eglSurfaceAttrib(state->display, state->surface, EGL_SWAP_BEHAVIOR, EGL_BUFFER_PRESERVED);
+	assert(EGL_FALSE != result);
+
+	// connect the context to the surface
+	result = eglMakeCurrent(state->display, state->surface, state->surface, state->context);
+	assert(EGL_FALSE != result);
+
+	// set up screen ratio
+	glViewport(0, 0, (GLsizei) state->screen_width, (GLsizei) state->screen_height);
+
+	glMatrixMode(GL_PROJECTION);
+	glLoadIdentity();
+
+	float ratio = (float)state->screen_width / (float)state->screen_height;
+	glFrustumf(-ratio, ratio, -1.0f, 1.0f, 1.0f, 10.0f);
+}