From 58636c780c604f84683c7cc23369727a12c51e0a Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Wed, 16 Apr 2014 13:53:30 +0400
Subject: [PATCH] LPC11C24: Simple UAVCAN node. Does nothing. There's some
 problem with TX reordering, it was solved temporarily by setting number of TX
 slots to one.

---
 libuavcan_drivers/lpc11c24/driver/src/can.cpp |  4 +-
 .../test_olimex_lpc_p11c24/src/main.cpp       | 96 +++++++------------
 2 files changed, 38 insertions(+), 62 deletions(-)

diff --git a/libuavcan_drivers/lpc11c24/driver/src/can.cpp b/libuavcan_drivers/lpc11c24/driver/src/can.cpp
index 8a3e3268f3..18db7540a6 100644
--- a/libuavcan_drivers/lpc11c24/driver/src/can.cpp
+++ b/libuavcan_drivers/lpc11c24/driver/src/can.cpp
@@ -29,10 +29,10 @@ namespace
 /**
  * Hardware message objects are allocated as follows:
  *  - 0..NumTxMsgObjects - TX objects
- *  - NumRxMsgObjects..32 - RX objects
+ *  - NumTxMsgObjects..32 - RX objects
  */
 const unsigned NumMsgObjects = 32;
-const unsigned NumTxMsgObjects = 3;
+const unsigned NumTxMsgObjects = 1;
 const unsigned NumRxMsgObjects = NumMsgObjects - NumTxMsgObjects;
 
 /**
diff --git a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp
index 3e8ee9ce81..8f97dd0b58 100644
--- a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp
@@ -1,83 +1,59 @@
 /*
- * Pavel Kirienko, 2014 <pavel.kirienko@gmail.com>
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
  */
 
 #include <cstdio>
 #include <board.hpp>
 #include <chip.h>
-#include <uavcan_lpc11c24/can.hpp>
-#include <uavcan_lpc11c24/clock.hpp>
+#include <uavcan_lpc11c24/uavcan_lpc11c24.hpp>
 
 #define ENFORCE(x)        \
     if ((x) == 0) {       \
         while (true) { board::setErrorLed(true); }  \
     }
 
-int main()
+namespace
+{
+
+typedef uavcan::Node<2048> Node;
+
+Node& getNode()
+{
+    static Node node(uavcan_lpc11c24::CanDriver::instance(), uavcan_lpc11c24::SystemClock::instance());
+    return node;
+}
+
+__attribute__((noreturn))
+void die()
+{
+    while (true) { }
+}
+
+void init()
 {
     if (uavcan_lpc11c24::CanDriver::instance().init(1000000) < 0)
     {
-        while (true) { }
+        die();
     }
 
-    uavcan_lpc11c24::clock::init();
+    getNode().setNodeID(72);
+    getNode().setName("org.uavcan.lpc11c24_test");
 
-    uavcan::MonotonicTime prev_mono;
-    uavcan::MonotonicTime prev_time_pub_at;
+    while (getNode().start() < 0)
+    {
+    }
+}
+
+}
+
+int main()
+{
+    init();
 
     while (true)
     {
-        const uavcan::MonotonicTime ts_mono = uavcan_lpc11c24::clock::getMonotonic();
-
-        while (prev_mono >= ts_mono)
-        {
-        }
-        prev_mono = ts_mono;
-
-        board::setErrorLed(uavcan_lpc11c24::CanDriver::instance().getErrorCount() > 0 ||
-                           uavcan_lpc11c24::CanDriver::instance().hadActivity());
-
-        uavcan::CanSelectMasks masks;
-        masks.read = 1;
-        masks.write = 1;
-        uavcan_lpc11c24::CanDriver::instance().select(masks, ts_mono + uavcan::MonotonicDuration::fromMSec(10));
-
-        if (masks.read == 1)
-        {
-            board::setStatusLed(true);
-            uavcan::CanFrame frm;
-            uavcan::UtcTime utc;
-            uavcan::MonotonicTime mono;
-            uavcan::CanIOFlags flags;
-            ENFORCE(1 == uavcan_lpc11c24::CanDriver::instance().receive(frm, mono, utc, flags));
-
-            if (masks.write == 1)
-            {
-                frm.id += 0x100;
-                ENFORCE(1 == uavcan_lpc11c24::CanDriver::instance().send(frm, mono, 0));
-            }
-        }
-        else
-        {
-            board::setStatusLed(false);
-        }
-
-        masks.read = 0;
-        masks.write = 1;
-        uavcan_lpc11c24::CanDriver::instance().select(masks, ts_mono + uavcan::MonotonicDuration::fromMSec(10));
-
-        if ((ts_mono - prev_time_pub_at).toMSec() >= 1000 && (masks.write == 1))
-        {
-            prev_time_pub_at = ts_mono;
-
-            uavcan::CanFrame frm;
-            frm.dlc = 8;
-            std::fill_n(frm.data, 8, 0);
-            snprintf(reinterpret_cast<char*>(frm.data), 8, "%u", unsigned(ts_mono.toMSec()));
-
-            frm.id = ts_mono.toMSec() | uavcan::CanFrame::FlagEFF;
-
-            ENFORCE(1 == uavcan_lpc11c24::CanDriver::instance().send(frm, ts_mono, 0));
-        }
+        const int res = getNode().spin(uavcan::MonotonicDuration::fromMSec(25));
+        board::setErrorLed(res < 0);
+        board::setStatusLed(uavcan_lpc11c24::CanDriver::instance().hadActivity());
     }
 }