Merge remote-tracking branch 'upstream/master' into ros_messagelayer_merge_nuttx_bringup_posctrl

Conflicts:
	nuttx-configs/px4fmu-v2/nsh/defconfig

10 files changed, 126 insertions, 48 deletions

diff --git a/Makefile b/Makefile
index 1f50770a5..e975fb8d4 100644
--- a/Makefile
+++ b/Makefile
@@ -169,16 +169,18 @@ ifneq ($(filter archives,$(MAKECMDGOALS)),)
 .NOTPARALLEL:
 endif
 
+J?=1
+
 $(ARCHIVE_DIR)%.export:	board = $(notdir $(basename $@))
 $(ARCHIVE_DIR)%.export:	configuration = nsh
 $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC)
 	@$(ECHO) %% Configuring NuttX for $(board)
 	$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
-	$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
+	$(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean
 	$(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .)
 	$(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration))
 	@$(ECHO) %% Exporting NuttX for $(board)
-	$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export
+	$(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export
 	$(Q) $(MKDIR) -p $(dir $@)
 	$(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@
 	$(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board))
@@ -218,11 +220,11 @@ BOARD			 = $(BOARDS)
 menuconfig: $(NUTTX_SRC)
 	@$(ECHO) %% Configuring NuttX for $(BOARD)
 	$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
-	$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
+	$(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean
 	$(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .)
 	$(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh)
 	@$(ECHO) %% Running menuconfig for $(BOARD)
-	$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig
+	$(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) menuconfig
 	@$(ECHO) %% Saving configuration file
 	$(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig
 else
diff --git a/Tools/make_color.sh b/Tools/make_color.sh
new file mode 100755
index 000000000..81316a932
--- /dev/null
+++ b/Tools/make_color.sh
@@ -0,0 +1,36 @@
+#!/bin/sh
+# make_color.sh
+# 
+#   Author: Simon Wilks (simon@uaventure.com)
+#
+# A compiler color coder.
+#
+# To invoke this script everytime you run make simply create the alias:
+#
+#     alias make='<your-firmware-directory>/Tools/make_color.sh'
+#
+# Color codes:
+#
+# white        "\033[1,37m"
+# yellow       "\033[1,33m"
+# green        "\033[1,32m"
+# blue         "\033[1,34m"
+# cyan         "\033[1,36m"
+# red          "\033[1,31m"
+# magenta      "\033[1,35m"
+# black        "\033[1,30m"
+# darkwhite    "\033[0,37m"
+# darkyellow   "\033[0,33m"
+# darkgreen    "\033[0,32m"
+# darkblue     "\033[0,34m"
+# darkcyan     "\033[0,36m"
+# darkred      "\033[0,31m"
+# darkmagenta  "\033[0,35m"
+# off          "\033[0,0m"
+#
+OFF="\o033[0m"
+WARN="\o033[1;33m"
+ERROR="\o033[1;31m" 
+INFO="\o033[0;37m"
+
+make ${@} 2>&1 | sed "s/make\[[0-9]\].*/$INFO & $OFF/;s/.*: warning: .*/$WARN & $OFF/;s/.*: error: .*/$ERROR & $OFF/" 
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index a48b3115e..e741f2271 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -37,11 +37,11 @@ MODULES		+= drivers/hott/hott_telemetry
 MODULES		+= drivers/hott/hott_sensors
 MODULES		+= drivers/blinkm
 MODULES		+= drivers/airspeed
-MODULES		+= drivers/ets_airspeed
+# MODULES		+= drivers/ets_airspeed
 MODULES		+= drivers/meas_airspeed
-MODULES		+= drivers/frsky_telemetry
+# MODULES		+= drivers/frsky_telemetry
 MODULES		+= modules/sensors
-MODULES		+= drivers/mkblctrl
+# MODULES		+= drivers/mkblctrl
 MODULES		+= drivers/px4flow
 
 #
@@ -70,7 +70,7 @@ MODULES		+= modules/commander
 MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
 MODULES		+= modules/gpio_led
-MODULES		+= modules/uavcan
+# MODULES		+= modules/uavcan
 MODULES 	+= modules/land_detector
 
 #
@@ -120,7 +120,7 @@ MODULES		+= lib/launchdetection
 #
 # OBC challenge
 #
-MODULES		+= modules/bottle_drop
+# MODULES		+= modules/bottle_drop
 
 #
 # Demo apps
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index ba39a3125..d0f198b33 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -114,7 +114,7 @@ CONFIG_ARCH_FPU=y
 # CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set
 # CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set
 CONFIG_ARMV7M_TOOLCHAIN_GNU_EABIL=y
-# CONFIG_ARMV7M_STACKCHECK is not set
+CONFIG_ARMV7M_STACKCHECK=y
 # CONFIG_ARMV7M_ITMSYSLOG is not set
 CONFIG_SDIO_DMA=y
 CONFIG_SDIO_DMAPRIO=0x00010000
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 562033db1..f20bba52e 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -35,6 +35,10 @@
  * @file mc_att_control_main.cpp
  * Multicopter attitude controller.
  *
+ * Publication for the desired attitude tracking:
+ * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors.
+ * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011.
+ *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 2c2cae8e4..b9692ffbf 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -35,6 +35,12 @@
  * @file mc_pos_control_main.cpp
  * Multicopter position controller.
  *
+ * Original publication for the desired attitude generation:
+ * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors.
+ * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011
+ *
+ * Also inspired by https://pixhawk.org/firmware/apps/fw_pos_control_l1
+ *
  * The controller has two loops: P loop for position error and PID loop for velocity error.
  * Output of velocity controller is thrust vector that splitted to thrust direction
  * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself).
diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c
index 4bcf95784..24f4e4207 100644
--- a/src/modules/systemlib/mcu_version.c
+++ b/src/modules/systemlib/mcu_version.c
@@ -47,7 +47,8 @@
 #ifdef CONFIG_ARCH_CHIP_STM32
 #include <up_arch.h>
 
-#define DBGMCU_IDCODE	0xE0042000
+#define DBGMCU_IDCODE	0xE0042000  //STM DocID018909 Rev 8 Sect 38.18 (MCU device ID code)
+#define UNIQUE_ID    	0x1FFF7A10  //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register)
 
 #define STM32F40x_41x	0x413
 #define STM32F42x_43x	0x419
@@ -57,7 +58,13 @@
 
 #endif
 
-
+/** Copy the 96bit MCU Unique ID into the provided pointer */
+void mcu_unique_id(uint32_t *uid_96_bit)
+{
+	uid_96_bit[0] = getreg32(UNIQUE_ID);
+	uid_96_bit[1] = getreg32(UNIQUE_ID+4);
+	uid_96_bit[2] = getreg32(UNIQUE_ID+8);
+}
 
 int mcu_version(char* rev, char** revstr)
 {
diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h
index 1b3d0aba9..c8a0bf5cd 100644
--- a/src/modules/systemlib/mcu_version.h
+++ b/src/modules/systemlib/mcu_version.h
@@ -33,6 +33,8 @@
 
 #pragma once
 
+#include <stdint.h>
+
 /* magic numbers from reference manual */
 enum MCU_REV {
 	MCU_REV_STM32F4_REV_A = 0x1000,
@@ -42,6 +44,15 @@ enum MCU_REV {
 	MCU_REV_STM32F4_REV_3 = 0x2001
 };
 
+
+/**
+ * Reports the microcontroller unique id.
+ *
+ * This ID is guaranteed to be unique for every mcu. 
+ * @param uid_96_bit A uint32_t[3] array to copy the data to.
+ */
+__EXPORT void mcu_unique_id(uint32_t *uid_96_bit);
+
 /**
  * Reports the microcontroller version of the main CPU.
  *
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
index 077bc47c9..9f13edb18 100644
--- a/src/systemcmds/config/config.c
+++ b/src/systemcmds/config/config.c
@@ -36,7 +36,7 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
  *
- * config tool.
+ * config tool. Takes the device name as the first parameter.
  */
 
 #include <nuttx/config.h>
@@ -71,18 +71,18 @@ int
 config_main(int argc, char *argv[])
 {
 	if (argc >= 2) {
-		if (!strcmp(argv[1], "gyro")) {
-			do_gyro(argc - 2, argv + 2);
-		} else if (!strcmp(argv[1], "accel")) {
-			do_accel(argc - 2, argv + 2);
-		} else if (!strcmp(argv[1], "mag")) {
-			do_mag(argc - 2, argv + 2);
+		if (!strncmp(argv[1], "/dev/gyro",9)) {
+			do_gyro(argc - 1, argv + 1);
+		} else if (!strncmp(argv[1], "/dev/accel",10)) {
+			do_accel(argc - 1, argv + 1);
+		} else if (!strncmp(argv[1], "/dev/mag",8)) {
+			do_mag(argc - 1, argv + 1);
 		} else {
 			do_device(argc - 1, argv + 1);
 		}
 	}
 	
-	errx(1, "expected a command, try 'gyro', 'accel', 'mag'");
+	errx(1, "expected a device, try '/dev/gyro', '/dev/accel', '/dev/mag'");
 }
 
 static void
@@ -133,41 +133,41 @@ do_gyro(int argc, char *argv[])
 {
 	int	fd;
 
-	fd = open(GYRO_DEVICE_PATH, 0);
+	fd = open(argv[0], 0);
 
 	if (fd < 0) {
-		warn("%s", GYRO_DEVICE_PATH);
+		warn("%s", argv[0]);
 		errx(1, "FATAL: no gyro found");
 
 	} else {
 
 		int ret;
 
-		if (argc == 2 && !strcmp(argv[0], "sampling")) {
+		if (argc == 3 && !strcmp(argv[1], "sampling")) {
 
 			/* set the gyro internal sampling rate up to at least i Hz */
-			ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[2], NULL, 0));
 
 			if (ret)
 				errx(ret,"sampling rate could not be set");
 
-		} else if (argc == 2 && !strcmp(argv[0], "rate")) {
+		} else if (argc == 3 && !strcmp(argv[1], "rate")) {
 
 			/* set the driver to poll at i Hz */
-			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0));
 
 			if (ret)
 				errx(ret,"pollrate could not be set");
 
-		} else if (argc == 2 && !strcmp(argv[0], "range")) {
+		} else if (argc == 3 && !strcmp(argv[1], "range")) {
 
 			/* set the range to i dps */
-			ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[2], NULL, 0));
 
 			if (ret)
 				errx(ret,"range could not be set");
 
-		} else if (argc == 1 && !strcmp(argv[0], "check")) {
+		} else if (argc == 2 && !strcmp(argv[1], "check")) {
 			ret = ioctl(fd, GYROIOCSELFTEST, 0);
 
 			if (ret) {
@@ -206,41 +206,41 @@ do_mag(int argc, char *argv[])
 {
 	int fd;
 
-	fd = open(MAG_DEVICE_PATH, 0);
+	fd = open(argv[0], 0);
 
 	if (fd < 0) {
-		warn("%s", MAG_DEVICE_PATH);
+		warn("%s", argv[0]);
 		errx(1, "FATAL: no magnetometer found");
 
 	} else {
 
 		int ret;
 
-		if (argc == 2 && !strcmp(argv[0], "sampling")) {
+		if (argc == 3 && !strcmp(argv[1], "sampling")) {
 
 			/* set the mag internal sampling rate up to at least i Hz */
-			ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[2], NULL, 0));
 
 			if (ret)
 				errx(ret,"sampling rate could not be set");
 
-		} else if (argc == 2 && !strcmp(argv[0], "rate")) {
+		} else if (argc == 3 && !strcmp(argv[1], "rate")) {
 
 			/* set the driver to poll at i Hz */
-			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0));
 
 			if (ret)
 				errx(ret,"pollrate could not be set");
 
-		} else if (argc == 2 && !strcmp(argv[0], "range")) {
+		} else if (argc == 3 && !strcmp(argv[1], "range")) {
 
 			/* set the range to i G */
-			ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[2], NULL, 0));
 
 			if (ret)
 				errx(ret,"range could not be set");
 
-		} else if(argc == 1 && !strcmp(argv[0], "check")) {
+		} else if(argc == 2 && !strcmp(argv[1], "check")) {
 			ret = ioctl(fd, MAGIOCSELFTEST, 0);
 
 			if (ret) {
@@ -282,41 +282,41 @@ do_accel(int argc, char *argv[])
 {
 	int	fd;
 
-	fd = open(ACCEL_DEVICE_PATH, 0);
+	fd = open(argv[0], 0);
 
 	if (fd < 0) {
-		warn("%s", ACCEL_DEVICE_PATH);
+		warn("%s", argv[0]);
 		errx(1, "FATAL: no accelerometer found");
 
 	} else {
 
 		int ret;
 
-		if (argc == 2 && !strcmp(argv[0], "sampling")) {
+		if (argc == 3 && !strcmp(argv[1], "sampling")) {
 
 			/* set the accel internal sampling rate up to at least i Hz */
-			ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[2], NULL, 0));
 
 			if (ret)
 				errx(ret,"sampling rate could not be set");
 
-		} else if (argc == 2 && !strcmp(argv[0], "rate")) {
+		} else if (argc == 3 && !strcmp(argv[1], "rate")) {
 
 			/* set the driver to poll at i Hz */
-			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0));
 
 			if (ret)
 				errx(ret,"pollrate could not be set");
 
-		} else if (argc == 2 && !strcmp(argv[0], "range")) {
+		} else if (argc == 3 && !strcmp(argv[1], "range")) {
 
 			/* set the range to i G */
-			ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[2], NULL, 0));
 
 			if (ret)
 				errx(ret,"range could not be set");
 
-		} else if(argc == 1 && !strcmp(argv[0], "check")) {
+		} else if(argc == 2 && !strcmp(argv[1], "check")) {
 			ret = ioctl(fd, ACCELIOCSELFTEST, 0);
 
 			if (ret) {
diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index 2ead3e632..087eb52e3 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -54,6 +54,7 @@ static const char sz_ver_bdate_str[] = "bdate";
 static const char sz_ver_gcc_str[] 	= "gcc";
 static const char sz_ver_all_str[] 	= "all";
 static const char mcu_ver_str[]		= "mcu";
+static const char mcu_uid_str[]		= "uid";
 
 static void usage(const char *reason)
 {
@@ -61,7 +62,7 @@ static void usage(const char *reason)
 		printf("%s\n", reason);
 	}
 
-	printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu}\n\n");
+	printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu|uid}\n\n");
 }
 
 __EXPORT int ver_main(int argc, char *argv[]);
@@ -141,6 +142,17 @@ int ver_main(int argc, char *argv[])
 				ret = 0;
 			}
 
+			if (show_all || !strncmp(argv[1], mcu_uid_str, sizeof(mcu_uid_str))) {
+				uint32_t uid[3];
+
+				mcu_unique_id(uid);
+
+				printf("UID: %X:%X:%X \n",uid[0],uid[1],uid[2]);
+
+				ret = 0;
+			}
+
+
 			if (ret == 1) {
 				errx(1, "unknown command.\n");
 			}