From cfddeac44147e45a1fac658772ebda5ea04258b8 Mon Sep 17 00:00:00 2001 From: Sergey Prokhorov Date: Wed, 6 Nov 2024 01:40:59 +0100 Subject: [PATCH] Add DebugWidget and working GPSWidget Also, validated that Mavlink works on real hardware --- README.md | 81 ++++++++++++++++++++++++++-------- config_osd.json | 37 +++++++++++++++- src/mavlink.c | 30 ++++++------- src/osd.cpp | 113 ++++++++++++++++++++++++++++++++++++++++++++++-- 4 files changed, 224 insertions(+), 37 deletions(-) diff --git a/README.md b/README.md index 34743c4..7cc8a28 100644 --- a/README.md +++ b/README.md @@ -58,7 +58,50 @@ pixelpilot --help ### OSD config -TODO +OSD is set-up declaratively in `/etc/pixelpilot/config_osd.json` file (or whatever is set via `--osd-config` +command line key. +OSD is described as an array of widgets which may subscribe to fact updates (they receive each fact +update they subscribe to) and those widgets are periodically rendered on the screen (in the order they +declared in config). So the goal is that widgets would decide how they should be rendered based on +the values of the facts they are subscribed to. If widget needs to render not the latest value of the +fact, but some processed value (like average / max / total etc), the widget should keep the necessary +state for that. There is a helper class `MovingAverage` that would be helpful to calculate common +statistical parameters. +Each fact has a specific datatype: one of `int` (signed integer) / `uint` (unsigned integer) / +`double` (floating point) / `bool` (true/false) / `string` (text). Type cast is currently not +implemented, so it is important to use the right type in the widget code and templates. +Facts may also have tags: a set of string key->value pairs. Widget may filter facts by tags as well as by name. +Currently there are several generic OSD widgets and several specific ad-hoc ones. There are quite a +lot of facts to which widgets can subscribe to: + +| Fact | Type | Description | +|:-------------------------------|:-----|:--------------------------------------------------------------------------| +| `dvr.recording` | bool | Is DVR currently recording? | +| `video.width` | uint | The width of the video stream | +| `video.height` | uint | The height of the video stream | +| `video.displayed_frame` | uint | Published with value "1" each time a new video frame is displayed | +| `video.decode_and_handover_ms` | uint | Time from the moment packet is received to time it is displayed on screen | +| `video.decoder_feed_time_ms` | uint | Time to feed the video packet to hardware decoder | +| `gstreamer.received_bytes` | uint | Number of bytes received from gstreamer (published for each packet) | + +There are many facts based on Mavlink telemetry, see `mavlink.c`. All of them have tags "sysid" and +"compid", but some have extra tags. +Currently implemented fact categories are grouped by Mavlink message types: + +| Fact | Type | Description | +|:------------------------------------|:---------|:----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| `mavlink.heartbeet.base_mode.armed` | bool | Is drone armed? | +| `mavlink.raw_imu.*` | int | Raw values from gyroscope and accelerometer, See [RAW_IMU](https://mavlink.io/en/messages/common.html#RAW_IMU) | +| `mavlink.sys_status.*` | int/uint | Some of the fields from [SYS_STATUS](https://mavlink.io/en/messages/common.html#SYS_STATUS) | +| `mavlink.battery_status.*` | int | Some of the fields from [BATTERY_STATUS](https://mavlink.io/en/messages/common.html#BATTERY_STATUS) | +| `mavlink.rc_channels_raw.chanN` | uint | Raw values of remote control chanels (N is from 1 to 8) | +| `mavlink.gps_raw.*` | int/uint | Raw data from GNSS sensor, see [GPS_RAW_INT](https://mavlink.io/en/messages/common.html#GPS_RAW_INT) | +| `mavlink.vfr_hud.*` | double | Metrics common for fixed wing OSDs, see [VFR_HUD](https://mavlink.io/en/messages/common.html#VFR_HUD) | +| `mavlink.global_position_int.*` | int | Position estimation based on sensor fusion, see [GLOBAL_POSITION_INT](https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT) | +| `mavlink.attitude.*` | double | See [ATTITUDE](https://mavlink.io/en/messages/common.html#ATTITUDE) | +| `mavlink.radio_status.*` | uint/int | Status of various radio equipment. Tags `{sysid: 3, compid: 68}` encode the [injected status of WFB-ng receiver](https://github.com/svpcom/wfb-ng/blob/4ea700606c259960ea169bad1f55fde77850013d/wfb_ng/conf/master.cfg#L227-L228) | + +More can be easily added later. You can use `DebugWidget` to inspect the current raw value of the fact(s). ## Known issues @@ -67,39 +110,41 @@ TODO ## The way it works -It uses `mpp` library to decode MPEG frames using Rockchip hardware decoder. It uses `gstreamer` to read the RTP media stream from video UDP. -It uses `mavlink` decoder to read Mavlink telemetry from telemetry UDP (if enabled), see `mavlink.c` -It uses `cairo` library to draw OSD elements (if enabled), see `osd.c`. +It uses `mpp` library to decode MPEG frames using Rockchip hardware decoder. It uses [Direct Rendering Manager (DRM)](https://en.wikipedia.org/wiki/Direct_Rendering_Manager) to display video on the screen, see `drm.c`. -It writes raw MPEG stream to file as DVR (if enabled) using `minimp4.h` library. +It uses `mavlink` decoder to read Mavlink telemetry from telemetry UDP (if enabled), see `mavlink.c` +It uses `cairo` library to draw OSD elements (if enabled), see `osd.c`. +It writes non-decoded MPEG stream to file as DVR (if enabled) using `minimp4.h` library. Pixelpilot starts several threads: -* main thread +* main thread: controls gstreamer which reads RTP, extracts MPEG frames and - feeds them to MPP hardware decoder - sends them to DVR thread via mutex-protected `std::queue` (if enabled) -* DVR_THREAD (if enabled) +* DVR_THREAD (if enabled): reads frames from main thread via `std::queue` and writes them to disk using `minimp4` library it yields on a condition variable for DVR queue or `kill` signal variable -* FRAME_THREAD +* FRAME_THREAD: reads decoded video frames from MPP hardware decoder and forwards them to `DISPLAY_THREAD` - through DRM `output_list` protected by `video_mutex` + through DRM `output_list` protected by `video_mutex`. Seems that thread vields on `mpi->decode_get_frame()` call waiting for HW decoder to return a new frame -* DISPLAY_THREAD +* DISPLAY_THREAD: reads frames and OSD from `video_mutex`-protected `output_list` and calls `drm*` functions to - render them on the screen + render them on the screen. The loop yields on `video_mutex` and `video_cond` waiting for a new frame to display from FRAME_THREAD -* MAVLINK_THREAD (if OSD and mavlink configured) - reads mavlink packets from UDP, decodes and updates `osd_vars` (without any mutex) - The loop yields on UDP read -* OSD_THREAD (if OSD is enabled) - takes `drm_fd` and `output_list` as thread parameters - draws telemetry on a buffer inside `output_list` based on `osd_vars` using Cairo library - The loop yelds on explicit `sleep` to control refresh rate +* MAVLINK_THREAD (if OSD and mavlink configured): + reads mavlink packets from UDP, decodes and updates `osd_vars` (without any mutex). + The loop yields on UDP read. +* OSD_THREAD (if OSD is enabled): + takes `drm_fd`, `output_list` and JSON config as thread parameters, + receives Facts through mutex-with-timeout-protected `std::queue`, feeds Facts to widgets and + periodically draws widgets on a buffer inside `output_list` using Cairo library. + There exists legacy OSD, is based on `osd_vars`, draws using Cairo library, to be removed. + The loop yields on queue mutex with timeout (timeout in order to re-draw OSD at fixed intervals). ## Release diff --git a/config_osd.json b/config_osd.json index 502e27f..f295234 100644 --- a/config_osd.json +++ b/config_osd.json @@ -208,8 +208,10 @@ "type": "GPSWidget", "x": 10, "y": 140, - "template": "GPS:%u,%u", "facts": [ + { + "name": "mavlink.gps_raw.fix_type" + }, { "name": "mavlink.gps_raw.lat" }, @@ -243,6 +245,39 @@ } ] }, + { + "name": "Dump raw facts to the scren", + "type": "---DebugWidget", + "x": 10, + "y": 250, + "facts": [ + { + "name": "mavlink.heartbeet.base_mode.armed" + }, + { + "name": "mavlink.radio_status.rssi", + "tags": { + "sysid": "3", + "compid": "68" + } + }, + { + "name": "mavlink.gps_raw.lat" + }, + { + "name": "mavlink.gps_raw.lon" + }, + { + "name": "mavlink.gps_raw.fix_type" + }, + { + "name": "mavlink.global_position_int.lat" + }, + { + "name": "mavlink.global_position_int.lon" + } + ] + }, { "name": "Distance", "type": "DistanceWidget", diff --git a/src/mavlink.c b/src/mavlink.c index 4638431..f20e529 100644 --- a/src/mavlink.c +++ b/src/mavlink.c @@ -130,7 +130,7 @@ void* __MAVLINK_THREAD__(void* arg) { // Credit to openIPC:https://github.com/OpenIPC/silicon_research/blob/master/vdec/main.c#L1020 mavlink_message_t message; mavlink_status_t status; - static bool current_arm_state = false; + static int current_arm_state = -1; for (int i = 0; i < ret; ++i) { if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &message, &status) == 1) { osd_tag tags[3]; @@ -143,7 +143,7 @@ void* __MAVLINK_THREAD__(void* arg) { { mavlink_heartbeat_t heartbeat = {}; mavlink_msg_heartbeat_decode(&message, &heartbeat); - bool received_arm_state = (heartbeat.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) != 0; + int received_arm_state = (heartbeat.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) != 0; if (current_arm_state != received_arm_state) { osd_publish_bool_fact("mavlink.heartbeet.base_mode.armed", tags, 2, received_arm_state); current_arm_state = received_arm_state; @@ -300,11 +300,11 @@ void* __MAVLINK_THREAD__(void* arg) { osd_vars.s3_double = strtod(osd_vars.s3, &osd_vars.ptr); osd_vars.s4_double = strtod(osd_vars.s4, &osd_vars.ptr); } - osd_add_int_fact(batch, "mavlink.gps_raw.lat", tags, 2, (long) gps.lat); - osd_add_int_fact(batch, "mavlink.gps_raw.lon", tags, 2, (long) gps.lon); - osd_add_int_fact(batch, "mavlink.gps_raw.alt", tags, 2, (long) gps.alt); - osd_add_uint_fact(batch, "mavlink.gps_raw.vel", tags, 2, (ulong) gps.vel); - osd_add_uint_fact(batch, "mavlink.gps_raw.cog", tags, 2, (ulong) gps.cog); + osd_add_int_fact(batch, "mavlink.gps_raw.lat", tags, 2, (long) gps.lat); //degE7 + osd_add_int_fact(batch, "mavlink.gps_raw.lon", tags, 2, (long) gps.lon); //degE7 + osd_add_int_fact(batch, "mavlink.gps_raw.alt", tags, 2, (long) gps.alt); //mm + osd_add_uint_fact(batch, "mavlink.gps_raw.vel", tags, 2, (ulong) gps.vel); //cm/s + osd_add_uint_fact(batch, "mavlink.gps_raw.cog", tags, 2, (ulong) gps.cog); //cdeg osd_add_uint_fact(batch, "mavlink.gps_raw.satellites_visible", tags, 2, (ulong) gps.satellites_visible); // Fix type: https://mavlink.io/en/messages/common.html#GPS_FIX_TYPE osd_add_uint_fact(batch, "mavlink.gps_raw.fix_type", tags, 2, (ulong) gps.fix_type); @@ -337,14 +337,14 @@ void* __MAVLINK_THREAD__(void* arg) { void *batch = osd_batch_init(8); mavlink_msg_global_position_int_decode( &message, &global_position_int); osd_vars.telemetry_hdg = global_position_int.hdg / 100; - osd_add_int_fact(batch, "mavlink.global_position_int.lat", tags, 2, (long) global_position_int.lat); - osd_add_int_fact(batch, "mavlink.global_position_int.lon", tags, 2, (long) global_position_int.lon); - osd_add_int_fact(batch, "mavlink.global_position_int.alt", tags, 2, (long) global_position_int.alt); - osd_add_int_fact(batch, "mavlink.global_position_int.relative_alt", tags, 2, (long) global_position_int.relative_alt); - osd_add_int_fact(batch, "mavlink.global_position_int.vx", tags, 2, (long) global_position_int.vx); - osd_add_int_fact(batch, "mavlink.global_position_int.vy", tags, 2, (long) global_position_int.vy); - osd_add_int_fact(batch, "mavlink.global_position_int.vz", tags, 2, (long) global_position_int.vz); - osd_add_uint_fact(batch, "mavlink.global_position_int.hdg", tags, 2, (ulong) global_position_int.hdg); + osd_add_int_fact(batch, "mavlink.global_position_int.lat", tags, 2, (long) global_position_int.lat); //degE7 + osd_add_int_fact(batch, "mavlink.global_position_int.lon", tags, 2, (long) global_position_int.lon); //degE7 + osd_add_int_fact(batch, "mavlink.global_position_int.alt", tags, 2, (long) global_position_int.alt); //mm + osd_add_int_fact(batch, "mavlink.global_position_int.relative_alt", tags, 2, (long) global_position_int.relative_alt); //mm + osd_add_int_fact(batch, "mavlink.global_position_int.vx", tags, 2, (long) global_position_int.vx); //cm/s + osd_add_int_fact(batch, "mavlink.global_position_int.vy", tags, 2, (long) global_position_int.vy); //cm/s + osd_add_int_fact(batch, "mavlink.global_position_int.vz", tags, 2, (long) global_position_int.vz); //cm/s + osd_add_uint_fact(batch, "mavlink.global_position_int.hdg", tags, 2, (ulong) global_position_int.hdg); //cdeg osd_publish_batch(batch); } break; diff --git a/src/osd.cpp b/src/osd.cpp index 21a6dc3..6ba7800 100644 --- a/src/osd.cpp +++ b/src/osd.cpp @@ -163,6 +163,28 @@ class Fact { FactTags getTags() { return meta.getTags(); } + + std::string asString() { + switch(type) { + case T_UNDEF: + return "(undefined)"; + case T_BOOL: + if (getBoolValue()) { + return "true"; + } else { + return "false"; + }; + case T_INT: + return std::to_string(getIntValue()); + case T_UINT: + return std::to_string(getUintValue()); + case T_DOUBLE: + return std::to_string(getDoubleValue()); + case T_STRING: + return getStrValue(); + } + return "(unknown)"; + } private: Type type = T_UNDEF; @@ -614,14 +636,14 @@ class BarChartWidget: public Widget { cairo_set_source_rgba(cr, 200.0, 200.0, 200.0, 0.8); double scale = max - min; - SPDLOG_DEBUG("Scale: {}, min {}, max {}", scale, min, max); + SPDLOG_TRACE("Scale: {}, min {}, max {}", scale, min, max); uint legend_w = 65; uint chart_w = w - legend_w; uint bar_pad = 4; uint bar_w = (chart_w - (bar_pad * num_buckets)) / num_buckets; uint bar_x = x + legend_w; - SPDLOG_DEBUG( + SPDLOG_TRACE( "chart_w {} bar_w {}, bar_x {}", chart_w, bar_w, bar_x ); @@ -630,7 +652,7 @@ class BarChartWidget: public Widget { double bar_h = -1.0 * (normalized * (h - 10)) / scale; // h -> max-min // ? -> normalized - SPDLOG_DEBUG("val {}, cairo_rectangle(cr, {}, {}, {}, {})", + SPDLOG_TRACE("val {}, cairo_rectangle(cr, {}, {}, {}, {})", val, bar_x, y + h, bar_w, bar_h); cairo_rectangle(cr, bar_x, y + h, bar_w, bar_h - 2); cairo_fill(cr); @@ -789,6 +811,87 @@ class VideoDecodeLatencyWidget: public IconTplTextWidget { }; +class GPSWidget: public Widget { +public: + GPSWidget(int pos_x, int pos_y, uint num_args) : + Widget(pos_x, pos_y, num_args) { + assert(num_args == 3); + }; + + void draw(cairo_t *cr) { + if( !(args[0].isDefined() && args[1].isDefined() && args[2].isDefined()) ) return; + auto [x, y] = xy(cr); + std::string fix_type = "undef"; + char buf[64]; + switch (args[0].getUintValue()) { + case 0: + fix_type = "no GPS"; + break; + case 1: + fix_type = "no fix"; + break; + case 2: + fix_type = "2D fix"; + break; + case 3: + fix_type = "3D fix"; + break; + case 4: + fix_type = "DGPS/SBAS 3D"; + break; + case 5: + fix_type = "RTK float 3D"; + break; + case 6: + fix_type = "RTK Fixed 3D"; + break; + case 7: + fix_type = "Static fixed"; + break; + case 8: + fix_type = "PPP 3D"; + break; + } + double lat = args[1].getIntValue() * 1.0e-7; + double lon = args[2].getIntValue() * 1.0e-7; + snprintf(buf, sizeof(buf), "%s Lat:%f, Lon:%f", fix_type.c_str(), lat, lon); + cairo_set_source_rgba(cr, 255.0, 255.0, 255.0, 1); + cairo_move_to(cr, x, y); + cairo_show_text(cr, buf); + } +}; + + +class DebugWidget: public Widget { +public: + DebugWidget(int pos_x, int pos_y, uint num_args) : + Widget(pos_x, pos_y, num_args) {}; + + void draw(cairo_t *cr) { + auto [x, y] = xy(cr); + auto y_offset = y; + for (Fact &fact : args) { + std::ostringstream oss; + if (!fact.isDefined()) { + oss << "undef"; + } else { + oss << fact.getName() << " (" << fact.getTypeName() << ") {"; + for (const auto &tag : fact.getTags()) { + oss << tag.first << "=>" << tag.second << ", "; + } + oss << "} = " << fact.asString(); + } + std::string text = oss.str(); + cairo_set_source_rgba(cr, 255.0, 50.0, 50.0, 1); + cairo_move_to(cr, x, y_offset); + cairo_show_text(cr, text.c_str()); + y_offset += 20; + SPDLOG_INFO("dbg draw {}", text); + } + } +}; + + class Osd { public: void loadConfig(json cfg) { @@ -908,6 +1011,10 @@ class Osd { } addWidget(new BarChartWidget(x, y, width, height, window_s, num_buckets, stats_kind), matchers); + } else if (type == "GPSWidget") { + addWidget(new GPSWidget(x, y, (uint)matchers.size()), matchers); + } else if(type == "DebugWidget") { + addWidget(new DebugWidget(x, y, (uint)matchers.size()), matchers); } else { spdlog::warn("Widget '{}': unknown type: {}", name, type); }