Added ECHO cmd and fixed some timing issues

goTool/cmd_echo_ping.go

@@ -0,0 +1,26 @@
+package main
+
+import (
+	"flag"
+	"fmt"
+)
+
+func runEchoPing(sp *serialPort, args []string) error {
+	fs := flag.NewFlagSet("echo-ping", flag.ExitOnError)
+	clientID := fs.Uint("client", 0, "slave client id from `clients`")
+	if err := fs.Parse(args); err != nil {
+		return err
+	}
+	if *clientID == 0 {
+		return fmt.Errorf("client id required (see `gotool clients`)")
+	}
+
+	r, err := sp.echoPing(uint32(*clientID))
+	if err != nil {
+		return err
+	}
+
+	fmt.Printf("echo ping: success=%v client_id=%d rtt_ms=%.3f esp_rtt_us=%d\n",
+		r.Success, r.ClientID, r.RttMs, r.EspRttUs)
+	return nil
+}