Render Betaflight VTX temperature element

msposd.c

@@ -54,7 +54,7 @@ extern char* recording_dir;
 // libevent base main loop
 struct event_base *base = NULL;
 
-int serial_fd = 0;
+int serial_fd = -1;
 int in_sock = 0;
 int MSPUDPPort = 0;
 int MSP_PollRate = 20;
@@ -1182,7 +1182,7 @@ static void poll_msp(evutil_socket_t sock, short event, void *arg) {
 
 static int handle_data(const char *port_name, int baudrate, const char *out_addr) {
 	struct event *sig_int = NULL, *in_ev = NULL, *temp_tmr = NULL, *msp_tmr = NULL;
-	struct event *sig_term;
+	struct event *sig_term = NULL;
 	int ret = EXIT_SUCCESS;
 
 	// Read from UDP
@@ -1217,6 +1217,10 @@ static int handle_data(const char *port_name, int baudrate, const char *out_addr
 		printf("Listening UART on %s...\n", port_name);
 	}
 
+	// In UDP mode there is no UART; do not apply raw termios settings to stdin.
+	if (serial_fd < 0)
+		goto uart_configured;
+
 	struct termios options;
 	tcgetattr(serial_fd, &options);
 	cfsetspeed(&options, speed_by_value(baudrate));
@@ -1253,6 +1257,8 @@ static int handle_data(const char *port_name, int baudrate, const char *out_addr
 		msp_set_vtx_config(serial_fd);
 	}
 
+uart_configured:
+
 	if (strlen(out_addr) > 1) {
 		out_sock = socket(AF_INET, SOCK_DGRAM, 0);
 
@@ -1280,7 +1286,7 @@ static int handle_data(const char *port_name, int baudrate, const char *out_addr
 	// Test inject a simple packet to test malvink communication Camera to Ground
 	signal(SIGUSR1, sendtestmsg);
 
-	if (serial_fd > 0 && !enable_simple_uart) { // if UART opened and we need to read it via events
+	if (serial_fd >= 0 && !enable_simple_uart) { // if UART opened and we need to read it via events
 		serial_bev = bufferevent_socket_new(base, serial_fd, 0);
 
 		// Trigger the read callback only whenever there is at least 16 bytes of data in the buffer.
@@ -1355,7 +1361,7 @@ static int handle_data(const char *port_name, int baudrate, const char *out_addr
 
 	// MSP_PollRate
 	if (ParseMSP && msp_tmr == NULL &&
-		serial_fd > 0) { // Only if we are on Cam, on ground no need to poll
+		serial_fd >= 0) { // Only if we are on Cam, on ground no need to poll
 		msp_tmr = event_new(base, -1, EV_PERSIST, poll_msp, &serial_fd);
 		// Set poll interval to 50 milliseconds if pollrate is 20
 		struct timeval interval = {
@@ -1390,14 +1396,16 @@ static int handle_data(const char *port_name, int baudrate, const char *out_addr
 
 	if (sig_int)
 		event_free(sig_int);
+	if (sig_term)
+		event_free(sig_term);
+
+	CloseMSP();
 
 	if (base)
 		event_base_free(base);
 
 	libevent_global_shutdown();
 
-	CloseMSP();
-
 	return ret;
 }
 

osd.c

@@ -333,6 +333,138 @@ char font_load_name[255];
 
 bool LoadFont();
 
+// Betaflight MSP DisplayPort system-element rendering.
+#define BETAFLIGHT_SYM_TEMPERATURE 0x7A
+#define MAX_SYSTEM_ELEMENT_TEXT 30
+
+typedef bool (*system_element_formatter_t)(char *buffer, size_t buffer_size);
+
+static int GetBoardTempForOSD() {
+#if __SIGMASTAR__
+	return GetTempSigmaStar();
+#elif defined(__GOKE__)
+	return GetTempGoke();
+#else
+	return last_board_temp;
+#endif
+}
+
+static bool GetVtxTemperature(int *temperature) {
+	int board_temp = GetBoardTempForOSD();
+	int tx_temp = GetTXTemp();
+
+	bool has_board_temp = board_temp > 0;
+	bool has_tx_temp = tx_temp > 0;
+	if (!has_board_temp && !has_tx_temp)
+		return false;
+
+	if (!has_board_temp)
+		board_temp = 0;
+	if (!has_tx_temp)
+		tx_temp = 0;
+
+	int max_temp = board_temp > tx_temp ? board_temp : tx_temp;
+	if (max_temp > 999)
+		max_temp = 999;
+	*temperature = max_temp;
+	return true;
+}
+
+static bool FormatVtxTemperature(char *buffer, size_t buffer_size) {
+	int temperature;
+	if (!GetVtxTemperature(&temperature))
+		return false;
+
+	snprintf(buffer, buffer_size, "%c% 3d", BETAFLIGHT_SYM_TEMPERATURE, temperature);
+	return true;
+}
+
+static bool GetVideoBitrateMbps(float *megabits) {
+#if __SIGMASTAR__
+	static const char *const command =
+#ifdef __INFINITY6C__
+		"cat /proc/mi_modules/mi_venc/mi_venc0 | grep Fps_1s -A 1 | awk 'NR==2 {print $7, $8}'";
+#else
+		"cat /proc/mi_modules/mi_venc/mi_venc0 | grep Fps10s -A 1 | awk 'NR==2 {print $9, $10}'";
+#endif
+	static uint64_t last_read_time;
+	static float cached_megabits;
+
+	if (cached_megabits > 0 && get_time_ms() - last_read_time < 1000) {
+		*megabits = cached_megabits;
+		return true;
+	}
+
+	FILE *stat = popen(command, "r");
+	if (stat == NULL)
+		return false;
+
+	unsigned int bitrate_kbps;
+	bool success = fscanf(stat, "%*f %u", &bitrate_kbps) == 1 && bitrate_kbps > 0;
+	pclose(stat);
+	if (!success)
+		return false;
+
+	cached_megabits = bitrate_kbps / 1000.0f;
+	last_read_time = get_time_ms();
+	*megabits = cached_megabits;
+	return true;
+#else
+	(void)megabits;
+	return false;
+#endif
+}
+
+static bool FormatBitrate(char *buffer, size_t buffer_size) {
+	float megabits;
+	if (!GetVideoBitrateMbps(&megabits))
+		return false;
+
+	snprintf(buffer, buffer_size, "%02.1fM", megabits);
+	return true;
+}
+
+static const system_element_formatter_t system_element_renderers[MSP_DISPLAYPORT_SYS_COUNT] = {
+	[MSP_DISPLAYPORT_SYS_BITRATE] = FormatBitrate,
+	[MSP_DISPLAYPORT_SYS_VTX_TEMP] = FormatVtxTemperature,
+};
+
+static bool FormatSystemElement(uint8_t element, char *buffer, size_t buffer_size) {
+	if (element >= MSP_DISPLAYPORT_SYS_COUNT)
+		return false;
+
+	system_element_formatter_t formatter = system_element_renderers[element];
+#if defined(_x86) || defined(__ROCKCHIP__)
+	(void)formatter;
+	(void)buffer;
+	(void)buffer_size;
+	return false;
+#else
+	return formatter && formatter(buffer, buffer_size);
+#endif
+}
+
+static bool InjectSystemElement(msp_msg_t *msp_message) {
+	if (msp_message->size < 4 || msp_message->payload[0] != MSP_DISPLAYPORT_DRAW_SYSTEM)
+		return false;
+
+	char text[MAX_SYSTEM_ELEMENT_TEXT + 1];
+	if (!FormatSystemElement(msp_message->payload[3], text, sizeof(text)))
+		return false;
+
+	size_t text_len = strlen(text);
+	if (text_len >= MAX_SYSTEM_ELEMENT_TEXT)
+		return false;
+
+	msp_message->payload[0] = MSP_DISPLAYPORT_DRAW_STRING;
+	msp_message->payload[3] = 0; // draw string attributes
+	memcpy(&msp_message->payload[4], text, text_len);
+	msp_message->payload[4 + text_len] = '\0';
+	msp_message->size = 4 + text_len;
+
+	return true;
+}
+
 static bool InjectChars(char *payload) {
 	char *str = payload + 4;
 	// string starts at 4 payload[0]==MSP_subtype
@@ -442,6 +574,9 @@ static void rx_msp_callback(msp_msg_t *msp_message) {
 		msp_message->cmd == MSP_CMD_DISPLAYPORT &&
 		msp_message->payload[0] == MSP_DISPLAYPORT_DRAW_STRING)
 		InjectChars(&msp_message->payload[0]);
+	if (msp_message->cmd == MSP_CMD_DISPLAYPORT && msp_message->direction == MSP_INBOUND &&
+		msp_message->payload[0] == MSP_DISPLAYPORT_DRAW_SYSTEM)
+		InjectSystemElement(msp_message);
 
 	// if (out_sock>0){//No need to cache MSP if we won't send it later
 	uint16_t size = msp_data_from_msg(message_buffer, msp_message);

osd/msp/msp_displayport.h

@@ -14,6 +14,24 @@ typedef enum {
 	MSP_DISPLAYPORT_INFO_MSG // custom type added for sending status text
 } msp_displayport_cmd_e;
 
+// Values sent in the fourth byte of MSP_DISPLAYPORT_DRAW_SYSTEM by
+// Betaflight. The display device, rather than the flight controller, owns
+// the data presented by these elements.
+typedef enum {
+	MSP_DISPLAYPORT_SYS_GOGGLE_VOLTAGE,
+	MSP_DISPLAYPORT_SYS_VTX_VOLTAGE,
+	MSP_DISPLAYPORT_SYS_BITRATE,
+	MSP_DISPLAYPORT_SYS_DELAY,
+	MSP_DISPLAYPORT_SYS_DISTANCE,
+	MSP_DISPLAYPORT_SYS_LQ,
+	MSP_DISPLAYPORT_SYS_GOGGLE_DVR,
+	MSP_DISPLAYPORT_SYS_VTX_DVR,
+	MSP_DISPLAYPORT_SYS_WARNINGS,
+	MSP_DISPLAYPORT_SYS_VTX_TEMP,
+	MSP_DISPLAYPORT_SYS_FAN_SPEED,
+	MSP_DISPLAYPORT_SYS_COUNT,
+} msp_displayport_system_element_e;
+
 typedef enum {
 	MSP_SD_OPTION_30_16,
 	MSP_HD_OPTION_50_18,

osd/util/Render_gs.c

@@ -516,9 +516,10 @@ void FlushDrawing() {
     if (x11_event == NULL && base != NULL) {
         // Attach X11 display's file descriptor to the existing msposd
         // event_base
-        struct event *x11_event =
+        x11_event =
             event_new(base, ConnectionNumber(display), EV_READ | EV_PERSIST, event_callback, NULL);
-        event_add(x11_event, NULL);
+        if (x11_event)
+            event_add(x11_event, NULL);
 
         XGrabKey(display, XKeysymToKeycode(display, XK_Up), Mod1Mask, RootWindow, True,
             GrabModeAsync,
@@ -565,15 +566,54 @@ void FlushDrawing() {
 
 void Close() {
 	// Clean up resources
-	cairo_destroy(cr);
-	cairo_destroy(cr_back);
-	cairo_surface_destroy(image_surface);
-	cairo_surface_destroy(surface);
-	cairo_surface_destroy(surface_back);
+	if (image_surface) {
+		cairo_surface_destroy(image_surface);
+		image_surface = NULL;
+	}
+#if defined(_x86)
+	if (x11_event) {
+		event_del(x11_event);
+		event_free(x11_event);
+		x11_event = NULL;
+	}
+
+	if (!shm_image) {
+		if (cr) {
+			cairo_destroy(cr);
+			cr = NULL;
+		}
+		if (surface) {
+			cairo_surface_destroy(surface);
+			surface = NULL;
+		}
+	} else {
+		cr = NULL;
+		surface = NULL;
+	}
+#else
+	if (cr) {
+		cairo_destroy(cr);
+		cr = NULL;
+	}
+	if (surface) {
+		cairo_surface_destroy(surface);
+		surface = NULL;
+	}
+#endif
+	if (cr_back) {
+		cairo_destroy(cr_back);
+		cr_back = NULL;
+	}
+	if (surface_back) {
+		cairo_surface_destroy(surface_back);
+		surface_back = NULL;
+	}
+
 #if defined(_x86)
 	// Clean up XShm triple-buffer resources
 	if (shm_image) {
-		XShmDetach(display, &shm_seg);
+		if (display)
+			XShmDetach(display, &shm_seg);
 		XDestroyImage(shm_image);
 		shm_image = NULL;
 	}
@@ -604,12 +644,13 @@ void Close() {
 		shm_sysv_id = -1;
 	}
 
-	XDestroyWindow(display, window);
-	XCloseDisplay(display);
-
-	// Cleanup
-	event_free(x11_event);
-	event_base_free(base);
+	if (display) {
+		if (window)
+			XDestroyWindow(display, window);
+		XCloseDisplay(display);
+		display = NULL;
+		window = 0;
+	}
 #endif
 }