From 4b4f5ed1c01b77cb599b3c40c0317b0b3eeb17e5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 26 Nov 2013 11:57:59 +1100
Subject: [PATCH] HAL_PX4: support get_system_id() on PX4

---
 libraries/AP_HAL_PX4/Util.cpp | 24 ++++++++++++++++++++++++
 libraries/AP_HAL_PX4/Util.h   |  5 +++++
 2 files changed, 29 insertions(+)

diff --git a/libraries/AP_HAL_PX4/Util.cpp b/libraries/AP_HAL_PX4/Util.cpp
index c180ae15dd..3f5a4519d9 100644
--- a/libraries/AP_HAL_PX4/Util.cpp
+++ b/libraries/AP_HAL_PX4/Util.cpp
@@ -11,6 +11,7 @@
 #include "UARTDriver.h"
 #include <uORB/uORB.h>
 #include <uORB/topics/safety.h>
+#include <systemlib/otp.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -97,4 +98,27 @@ void PX4Util::set_system_clock(uint64_t time_utc_usec)
     clock_settime(CLOCK_REALTIME, &ts);    
 }
 
+/*
+  display PX4 system identifer - board type and serial number
+ */
+bool PX4Util::get_system_id(char buf[40])
+{
+    uint8_t serialid[12];
+    memset(serialid, 0, sizeof(serialid));
+    val_read(serialid, (const void *)UDID_START, sizeof(serialid));
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+    const char *board_type = "PX4v1";
+#else
+    const char *board_type = "PX4v2";
+#endif
+    // this format is chosen to match the human_readable_serial()
+    // function in auth.c
+    snprintf(buf, 40, "%s %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X",
+             board_type,
+             (unsigned)serialid[0], (unsigned)serialid[1], (unsigned)serialid[2], (unsigned)serialid[3], 
+             (unsigned)serialid[4], (unsigned)serialid[5], (unsigned)serialid[6], (unsigned)serialid[7], 
+             (unsigned)serialid[8], (unsigned)serialid[9], (unsigned)serialid[10],(unsigned)serialid[11]); 
+    return true;
+}
+
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4
diff --git a/libraries/AP_HAL_PX4/Util.h b/libraries/AP_HAL_PX4/Util.h
index 43a520bf3f..5f64ef5a06 100644
--- a/libraries/AP_HAL_PX4/Util.h
+++ b/libraries/AP_HAL_PX4/Util.h
@@ -17,6 +17,11 @@ public:
      */
     void set_system_clock(uint64_t time_utc_usec);
 
+    /*
+      get system identifier (STM32 serial number)
+     */
+    bool get_system_id(char buf[40]);
+
 private:
     int _safety_handle;
 };