From 7a8b285792469ef1497e80e468892955d7139d32 Mon Sep 17 00:00:00 2001
From: Oleksiy Protas <elfy@ecognize.me>
Date: Fri, 15 Nov 2024 13:51:41 +0200
Subject: [PATCH] Telemetry log endpoint feature

---
 README.md        |  3 +++
 endpoint_tlog.go | 34 ++++++++++++++++++++++++++++++++++
 main.go          | 15 +++++++++++++++
 3 files changed, 52 insertions(+)
 create mode 100644 endpoint_tlog.go

diff --git a/README.md b/README.md
index addf0c8..01a7a66 100644
--- a/README.md
+++ b/README.md
@@ -17,6 +17,7 @@ Features:
   * serial
   * UDP (client, server or broadcast mode)
   * TCP (client or server mode)
+  * Telemetry log
 * Support Mavlink 2.0 and 1.0, support any dialect
 * Emit heartbeats
 * Automatically request streams to Ardupilot devices and block stream requests from ground stations
@@ -128,6 +129,8 @@ Arguments:
 
                        udps:listen_ip:port (udp, server mode)
 
+                       tlog:filename (telemetry log)
+
 Flags:
   -h, --help                     Show context-sensitive help.
       --version                  print version.
diff --git a/endpoint_tlog.go b/endpoint_tlog.go
new file mode 100644
index 0000000..d0e6452
--- /dev/null
+++ b/endpoint_tlog.go
@@ -0,0 +1,34 @@
+package main
+
+import (
+	"io"
+	"os"
+	"time"
+	"encoding/binary"
+)
+
+type TlogWriter struct {
+	file *os.File
+	io.Closer
+}
+
+// can't read from the log, but we don't want to error out either
+func (t *TlogWriter) Read(_ []byte) (int, error) {
+	// FIXME: this runs in a gorountine so it's somewhat safe to lock up
+	// TODO: but we need a way to get out of there too
+	for {
+	}
+	return 0, nil
+}
+
+func (t *TlogWriter) Write(b []byte) (n int, e error) {
+	// 64 bit timestamp in microseconds (big endian)
+	var usec uint64 = uint64(time.Now().UnixNano() / 1000)
+	e = binary.Write(t.file, binary.BigEndian, usec)
+	if e != nil {
+		return 0, e
+	}
+
+	n, e  = t.file.Write(b)
+	return n + 8, e
+}
diff --git a/main.go b/main.go
index 35ed28a..73282e1 100644
--- a/main.go
+++ b/main.go
@@ -108,6 +108,21 @@ var endpointTypes = map[string]endpointType{
 			return gomavlib.EndpointTCPClient{Address: args}, nil
 		},
 	},
+	"tlog": {
+		"filename",
+		"telemetry log",
+		func(args string) (gomavlib.EndpointConf, error) {
+			fp, err := os.Create(args)
+			if err != nil {
+				return nil, fmt.Errorf("Unable to open file %s", args)
+			}
+			tlog := &TlogWriter{
+				file: fp,
+				Closer: fp,
+			}
+			return gomavlib.EndpointCustom{ReadWriteCloser: tlog}, nil
+		},
+	},
 }
 
 func generateEndpointConfs(endpoints []string) ([]gomavlib.EndpointConf, error) {