diff --git a/msposd.c b/msposd.c index d3270d0..12b687e 100644 --- a/msposd.c +++ b/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,40 +1217,43 @@ static int handle_data(const char *port_name, int baudrate, const char *out_addr printf("Listening UART on %s...\n", port_name); } - struct termios options; - tcgetattr(serial_fd, &options); - cfsetspeed(&options, speed_by_value(baudrate)); + // In UDP mode there is no UART; do not apply raw termios settings to stdin. + if (serial_fd >= 0) { + struct termios options; + tcgetattr(serial_fd, &options); + cfsetspeed(&options, speed_by_value(baudrate)); - options.c_cflag &= ~CSIZE; // Mask the character size bits - options.c_cflag |= CS8; // 8 bit data - options.c_cflag &= ~PARENB; // set parity to no - options.c_cflag &= ~PARODD; // set parity to no - options.c_cflag &= ~CSTOPB; // set one stop bit + options.c_cflag &= ~CSIZE; // Mask the character size bits + options.c_cflag |= CS8; // 8 bit data + options.c_cflag &= ~PARENB; // set parity to no + options.c_cflag &= ~PARODD; // set parity to no + options.c_cflag &= ~CSTOPB; // set one stop bit - options.c_cflag |= (CLOCAL | CREAD); - options.c_oflag &= ~OPOST; + options.c_cflag |= (CLOCAL | CREAD); + options.c_oflag &= ~OPOST; - options.c_lflag &= 0; - options.c_iflag &= 0; // disable software flow controll - options.c_oflag &= 0; + options.c_lflag &= 0; + options.c_iflag &= 0; // disable software flow controll + options.c_oflag &= 0; - if (enable_simple_uart) - options.c_iflag |= (UART_FCR_TRIGGER_RX_L3); //#define UART_FCR_TRIGGER_RX_L3 0x10000 - // will work only on patched kernel driver + if (enable_simple_uart) + options.c_iflag |= (UART_FCR_TRIGGER_RX_L3); //#define UART_FCR_TRIGGER_RX_L3 0x10000 + // will work only on patched kernel driver - cfmakeraw(&options); - tcsetattr(serial_fd, TCSANOW, &options); + cfmakeraw(&options); + tcsetattr(serial_fd, TCSANOW, &options); #ifdef _x86 - usleep(500 * 1000); // flush all data in the Rx buffer - int tttt = tcflush(serial_fd, TCIOFLUSH); + usleep(500 * 1000); // flush all data in the Rx buffer + int tttt = tcflush(serial_fd, TCIOFLUSH); #endif - if (enable_simple_uart) - tcflush(serial_fd, TCIOFLUSH); + if (enable_simple_uart) + tcflush(serial_fd, TCIOFLUSH); - // tell the fc what vtx config we support - if (mspVTXenabled) { - printf("Setup mspVTX ...\n"); - msp_set_vtx_config(serial_fd); + // tell the fc what vtx config we support + if (mspVTXenabled) { + printf("Setup mspVTX ...\n"); + msp_set_vtx_config(serial_fd); + } } if (strlen(out_addr) > 1) { @@ -1280,7 +1283,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 +1358,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 +1393,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; } diff --git a/osd.c b/osd.c index b546c72..f15335c 100644 --- a/osd.c +++ b/osd.c @@ -333,6 +333,66 @@ char font_load_name[255]; bool LoadFont(); +// Betaflight MSP DisplayPort system element and font symbols. +#define BETAFLIGHT_SYS_VTX_TEMP 9 +#define BETAFLIGHT_SYM_TEMPERATURE 0x7A +#define BETAFLIGHT_SYM_C 0x0E +#define INVALID_TEMP_LIMIT -90 + +static int GetBoardTempForOSD() { +#if __SIGMASTAR__ + return GetTempSigmaStar(); +#elif defined(__GOKE__) + return GetTempGoke(); +#else + return last_board_temp; +#endif +} + +static int GetMaxVtxTempForOSD() { + int board_temp = GetBoardTempForOSD(); + int tx_temp = GetTXTemp(); + + if (board_temp < INVALID_TEMP_LIMIT) + board_temp = 0; + if (tx_temp < INVALID_TEMP_LIMIT) + tx_temp = 0; + if (board_temp < 0) + board_temp = 0; + if (tx_temp < 0) + tx_temp = 0; + + int max_temp = board_temp > tx_temp ? board_temp : tx_temp; + if (max_temp > 999) + max_temp = 999; + return max_temp; +} + +static void FormatVtxTemperature(char *buffer, size_t buffer_size) { + snprintf(buffer, buffer_size, "%c%d%c", + BETAFLIGHT_SYM_TEMPERATURE, GetMaxVtxTempForOSD(), BETAFLIGHT_SYM_C); +} + +static bool InjectSystemElement(msp_msg_t *msp_message) { + if (msp_message->size < 4 || + msp_message->payload[0] != MSP_DISPLAYPORT_DRAW_SYSTEM || + msp_message->payload[3] != BETAFLIGHT_SYS_VTX_TEMP) { + return false; + } + + char temp_string[8]; + FormatVtxTemperature(temp_string, sizeof(temp_string)); + uint8_t temp_string_len = strlen(temp_string); + + msp_message->payload[0] = MSP_DISPLAYPORT_DRAW_STRING; + msp_message->payload[3] = 0; // draw string attributes + memcpy(&msp_message->payload[4], temp_string, temp_string_len); + msp_message->payload[4 + temp_string_len] = '\0'; + msp_message->size = 4 + temp_string_len; + + return true; +} + static bool InjectChars(char *payload) { char *str = payload + 4; // string starts at 4 payload[0]==MSP_subtype @@ -438,10 +498,12 @@ static void rx_msp_callback(msp_msg_t *msp_message) { // return; } // Here it will replace custom text messages for configurator screen - if (msp_message->cmd == MSP_CMD_DISPLAYPORT && msp_message->direction == MSP_INBOUND && - 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) { + if (msp_message->payload[0] == MSP_DISPLAYPORT_DRAW_STRING) + InjectChars(&msp_message->payload[0]); + else if (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); diff --git a/osd/util/Render_gs.c b/osd/util/Render_gs.c index 2c98b41..a075095 100644 --- a/osd/util/Render_gs.c +++ b/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; } @@ -603,13 +643,14 @@ void Close() { shmctl(shm_sysv_id, IPC_RMID, NULL); 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 }