Skip to content

Commit

Permalink
Add DebugWidget and working GPSWidget
Browse files Browse the repository at this point in the history 
Also, validated that Mavlink works on real hardware
  • Loading branch information
@seriyps
seriyps committed Nov 6, 2024
1 parent 16710b9 commit cfddeac
Show file tree
Hide file tree
Showing 4 changed files with 224 additions and 37 deletions.
81 changes: 63 additions & 18 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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

Expand Down
37 changes: 36 additions & 1 deletion config_osd.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
},
Expand Down Expand Up @@ -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",
Expand Down
30 changes: 15 additions & 15 deletions src/mavlink.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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];
Expand All @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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;
Expand Down
113 changes: 110 additions & 3 deletions src/osd.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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
);
Expand All @@ -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);
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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);
}
Expand Down

0 comments on commit cfddeac

Please sign in to comment.