Telemetry log endpoint feature

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.

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
+}

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) {