From 7813566e6680f4940989fb91760ddb0782b05858 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Tue, 6 May 2014 19:30:45 +0400
Subject: [PATCH] Initial UAVCAN integration. The library compiles
 successfully, CAN driver appears to be working properly. There is one
 hardcoded path in the module makefile that needs to be fixed; plus the
 compilation will likely fail unless arch/math.h contains log2l()

---
 .gitignore                            |   1 +
 makefiles/config_px4fmu-v2_default.mk |   1 +
 makefiles/setup.mk                    |   1 +
 makefiles/toolchain_gnu-arm-eabi.mk   |   2 +-
 src/modules/uavcan/.gitignore         |   1 +
 src/modules/uavcan/module.mk          |  74 +++++++++++++++++++
 src/modules/uavcan/uavcan_clock.cpp   |  65 +++++++++++++++++
 src/modules/uavcan/uavcan_main.cpp    | 100 ++++++++++++++++++++++++++
 src/modules/uavcan/uavcan_main.hpp    |  39 ++++++++++
 9 files changed, 283 insertions(+), 1 deletion(-)
 create mode 100644 src/modules/uavcan/.gitignore
 create mode 100644 src/modules/uavcan/module.mk
 create mode 100644 src/modules/uavcan/uavcan_clock.cpp
 create mode 100644 src/modules/uavcan/uavcan_main.cpp
 create mode 100644 src/modules/uavcan/uavcan_main.hpp

diff --git a/.gitignore b/.gitignore
index 5b345b34a8..8c3bb1bb55 100644
--- a/.gitignore
+++ b/.gitignore
@@ -37,3 +37,4 @@ mavlink/include/mavlink/v0.9/
 tags
 .tags_sorted_by_file
 .pydevproject
+/uavcan
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index e13421acc5..09b5bf7c6e 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -73,6 +73,7 @@ MODULES		+= modules/commander
 MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
 MODULES		+= modules/gpio_led
+MODULES		+= modules/uavcan
 
 #
 # Estimation modules (EKF/ SO3 / other filters)
diff --git a/makefiles/setup.mk b/makefiles/setup.mk
index 183b143d6c..a5271c6569 100644
--- a/makefiles/setup.mk
+++ b/makefiles/setup.mk
@@ -48,6 +48,7 @@ export PX4_MK_DIR	 = $(abspath $(PX4_BASE)/makefiles)/
 export NUTTX_SRC	 = $(abspath $(PX4_BASE)/NuttX/nuttx)/
 export NUTTX_APP_SRC	 = $(abspath $(PX4_BASE)/NuttX/apps)/
 export MAVLINK_SRC	 = $(abspath $(PX4_BASE)/mavlink)/
+export UAVCAN_DIR	 = $(abspath $(PX4_BASE)/uavcan)/
 export ROMFS_SRC	 = $(abspath $(PX4_BASE)/ROMFS)/
 export IMAGE_DIR	 = $(abspath $(PX4_BASE)/Images)/
 export BUILD_DIR	 = $(abspath $(PX4_BASE)/Build)/
diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
index b519e0e7a1..3c00e77f11 100644
--- a/makefiles/toolchain_gnu-arm-eabi.mk
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -111,7 +111,7 @@ INSTRUMENTATIONDEFINES	 = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
 # Language-specific flags
 #
 ARCHCFLAGS		 = -std=gnu99
-ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
+ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics
 
 # Generic warnings
 #
diff --git a/src/modules/uavcan/.gitignore b/src/modules/uavcan/.gitignore
new file mode 100644
index 0000000000..24fbf171f0
--- /dev/null
+++ b/src/modules/uavcan/.gitignore
@@ -0,0 +1 @@
+./dsdlc_generated/
diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk
new file mode 100644
index 0000000000..5caa8d401a
--- /dev/null
+++ b/src/modules/uavcan/module.mk
@@ -0,0 +1,74 @@
+############################################################################
+#
+#   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+#   Author: Pavel Kirienko <pavel.kirienko@gmail.com>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# UAVCAN <--> uORB bridge
+#
+
+MODULE_COMMAND = uavcan
+
+MAXOPTIMIZATION = -Os
+
+SRCS += uavcan_main.cpp \
+        uavcan_clock.cpp
+
+#
+# libuavcan
+#
+include $(UAVCAN_DIR)/libuavcan/include.mk
+SRCS += $(LIBUAVCAN_SRC)
+# TODO fix include path
+INCLUDE_DIRS += $(LIBUAVCAN_INC) /media/storage/px4/Firmware/Build/px4fmu-v2_default.build/nuttx-export/include/cxx/
+EXTRADEFINES += -DUAVCAN_MEM_POOL_BLOCK_SIZE=56 \
+                -DUAVCAN_TOSTRING=0 \
+                -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \
+                -DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1
+
+#
+# libuavcan drivers for STM32
+#
+include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk
+SRCS += $(LIBUAVCAN_STM32_SRC)
+INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC)
+EXTRADEFINES += -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=1
+
+#
+# Invoke DSDL compiler
+# TODO: Add make target for this, or invoke dsdlc manually.
+#       The second option assumes that the generated headers shall be saved
+#       under the version control, which may be undesirable.
+#       The first option requires python3 and python3-mako for the sources to be built.
+#
+$(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR)))
+INCLUDE_DIRS += dsdlc_generated
diff --git a/src/modules/uavcan/uavcan_clock.cpp b/src/modules/uavcan/uavcan_clock.cpp
new file mode 100644
index 0000000000..2a5e889890
--- /dev/null
+++ b/src/modules/uavcan/uavcan_clock.cpp
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Author: Pavel Kirienko <pavel.kirienko@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <uavcan_stm32/uavcan_stm32.hpp>
+#include <drivers/drv_hrt.h>
+
+namespace uavcan_stm32
+{
+namespace clock
+{
+
+uavcan::MonotonicTime getMonotonic()
+{
+	return uavcan::MonotonicTime::fromUSec(hrt_absolute_time());
+}
+
+uavcan::UtcTime getUtc()
+{
+	return uavcan::UtcTime();
+}
+
+void adjustUtc(uavcan::UtcDuration adjustment)
+{
+	(void)adjustment;
+}
+
+uavcan::uint64_t getUtcUSecFromCanInterrupt()
+{
+	return 0;
+}
+
+}
+}
+
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
new file mode 100644
index 0000000000..fbb2e174b8
--- /dev/null
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Author: Pavel Kirienko <pavel.kirienko@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <cstdlib>
+#include <cstring>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+#include <arch/board/board.h>
+#include "uavcan_main.hpp"
+
+extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
+
+namespace
+{
+
+uavcan_stm32::CanInitHelper<> can_driver;
+
+void print_usage()
+{
+	warnx("usage: uavcan start [can_bitrate]");
+}
+
+int test_thread(int argc, char *argv[])
+{
+	stm32_configgpio(GPIO_CAN1_RX);
+	stm32_configgpio(GPIO_CAN1_TX);
+	stm32_configgpio(GPIO_CAN2_RX);
+	stm32_configgpio(GPIO_CAN2_TX);
+	int res = can_driver.init(1000000);
+	if (res < 0)
+	{
+		errx(res, "CAN driver init failed");
+	}
+	while (true)
+	{
+		::sleep(1);
+		auto iface = static_cast<uavcan::ICanIface*>(can_driver.driver.getIface(0));
+		res = iface->send(uavcan::CanFrame(), uavcan_stm32::clock::getMonotonic(), 0);
+		warnx("published %i, pending %u", res, can_driver.driver.getIface(0)->getRxQueueLength());
+	}
+	return 0;
+}
+
+}
+
+int uavcan_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		print_usage();
+		::exit(1);
+	}
+
+	if (!std::strcmp(argv[1], "start")) {
+		static bool started = false;
+		if (started)
+		{
+			warnx("already started");
+			::exit(1);
+		}
+		started = true;
+		(void)task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 3000,
+			static_cast<main_t>(&test_thread), const_cast<const char**>(argv));
+		return 0;
+	} else {
+		print_usage();
+		::exit(1);
+	}
+	return 0;
+}
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
new file mode 100644
index 0000000000..7fcc992d04
--- /dev/null
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -0,0 +1,39 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Author: Pavel Kirienko <pavel.kirienko@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <uavcan_stm32/uavcan_stm32.hpp>
+
+// ...