Merge branch 'master' of github.com:wingtra/Firmware into control_state

Makefile

@@ -89,7 +89,7 @@ endif
 # --------------------------------------------------------------------
 # describe how to build a cmake config
 define cmake-build
-+@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi
++@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then git submodule update --init --recursive --force && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi
 +$(PX4_MAKE) -C $(PWD)/build_$@ $(PX4_MAKE_ARGS) $(ARGS)
 endef
 
@@ -104,13 +104,13 @@ endef
 # --------------------------------------------------------------------
 #  Do not put any spaces between function arguments.
 
-px4fmu-v1_default: git-init
+px4fmu-v1_default:
 	$(call cmake-build,nuttx_px4fmu-v1_default)
 
-px4fmu-v2_default: git-init
+px4fmu-v2_default:
 	$(call cmake-build,nuttx_px4fmu-v2_default)
 
-px4fmu-v2_simple: git-init
+px4fmu-v2_simple:
 	$(call cmake-build,nuttx_px4fmu-v2_simple)
 
 nuttx_sim_simple:
@@ -172,19 +172,8 @@ check_format:
 
 clean:
 	@rm -rf build_*/
-
-distclean: clean
-	@cd NuttX
-	@git clean -d -f -x
-	@cd ..
-	@cd src/modules/uavcan/libuavcan
-	@git clean -d -f -x
-	@cd ../../../..
-
-# XXX this is not the right way to fix it, but we need a temporary solution
-# for average joe
-git-init:
-	@git submodule update --init --recursive
+	@(cd NuttX && git clean -d -f -x)
+	@(cd src/modules/uavcan/libuavcan && git clean -d -f -x)
 
 # targets handled by cmake
 cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak libuavcan

Tools/check_submodules.sh

@@ -1,144 +0,0 @@
-#!/bin/sh
-
-[ -n "$GIT_SUBMODULES_ARE_EVIL" ] && {
-    # GIT_SUBMODULES_ARE_EVIL is set, meaning user doesn't want submodules
-    echo "Skipping submodules. NUTTX_SRC is set to $NUTTX_SRC"
-    exit 0
-}
-
-if [ -d NuttX/nuttx ];
-	then
-	STATUSRETVAL=$(git submodule summary | grep -A20 -i "NuttX" | grep "<")
-	if [ -z "$STATUSRETVAL" ]; then
-		echo "Checked NuttX submodule, correct version found"
-	else
-		echo ""
-		echo ""
-		echo "New commits required:"
-		echo "$(git submodule summary)"
-		echo ""
-		echo ""
-		echo "   NuttX sub repo not at correct version. Try 'git submodule update'"
-		echo "   or follow instructions on http://pixhawk.org/dev/git/submodules"
-		echo ""
-		echo "   DO NOT FORGET TO RUN 'make distclean && make archives' AFTER EACH NUTTX UPDATE!"
-		exit 1
-	fi
-else
-	git submodule update --init --recursive
-fi
-
-
-if [ -d mavlink/include/mavlink/v1.0 ];
-	then
-	STATUSRETVAL=$(git submodule summary | grep -A20 -i "mavlink/include/mavlink/v1.0" | grep "<")
-	if [ -z "$STATUSRETVAL" ]; then
-		echo "Checked mavlink submodule, correct version found"
-	else
-		echo ""
-		echo ""
-		echo "New commits required:"
-		echo "$(git submodule summary)"
-		echo ""
-		echo ""
-		echo "mavlink sub repo not at correct version. Try 'git submodule update'"
-		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
-		exit 1
-	fi
-else
-	git submodule update --init --recursive
-fi
-
-
-if [ -d uavcan ]
-then
-	STATUSRETVAL=$(git submodule summary | grep -A20 -i uavcan | grep "<")
-	if [ -z "$STATUSRETVAL" ]
-	then
-		echo "Checked uavcan submodule, correct version found"
-	else
-		echo ""
-		echo ""
-		echo "New commits required:"
-		echo "$(git submodule summary)"
-		echo ""
-		echo ""
-		echo "uavcan sub repo not at correct version. Try 'git submodule update'"
-		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
-		exit 1
-	fi
-else
-	git submodule update --init --recursive
-fi
-
-if [ -d src/lib/eigen ]
-then
-	echo "ARG = $1"
-	if [ $1 = "qurt" ]
-	then
-		# QuRT needs to use Eigen 3.2 because the toolchain doews not support C++11
-		STATUSRETVAL=$(true)
-	else
-		STATUSRETVAL=$(git submodule summary | grep -A20 -i eigen | grep "<")
-		if [ -z "$STATUSRETVAL" ]
-		then
-			echo "Checked Eigen submodule, correct version found"
-		else
-			echo ""
-			echo ""
-			echo "New commits required:"
-			echo "$(git submodule summary)"
-			echo ""
-			echo ""
-			echo "eigen sub repo not at correct version. Try 'git submodule update'"
-			echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
-			exit 1
-		fi
-	fi
-else
-	git submodule update --init --recursive
-fi
-
-if [ -d Tools/gencpp ]
-then
-	STATUSRETVAL=$(git submodule summary | grep -A20 -i gencpp | grep "<")
-	if [ -z "$STATUSRETVAL" ]
-	then
-		echo "Checked gencpp submodule, correct version found"
-	else
-		echo ""
-		echo ""
-		echo "New commits required:"
-		echo "$(git submodule summary)"
-		echo ""
-		echo ""
-		echo "gencpp sub repo not at correct version. Try 'git submodule update'"
-		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
-		exit 1
-	fi
-else
-	git submodule update --init --recursive
-fi
-
-if [ -d Tools/genmsg ]
-then
-	STATUSRETVAL=$(git submodule summary | grep -A20 -i genmsg | grep "<")
-	if [ -z "$STATUSRETVAL" ]
-	then
-		echo "Checked genmsg submodule, correct version found"
-	else
-		echo ""
-		echo ""
-		echo "New commits required:"
-		echo "$(git submodule summary)"
-		echo ""
-		echo ""
-		echo "genmsg sub repo not at correct version. Try 'git submodule update'"
-		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
-		exit 1
-	fi
-else
-	git submodule update --init --recursive
-fi
-
-exit 0

Tools/generate_listener.py

@@ -24,11 +24,16 @@ for index,m in enumerate(raw_messages):
 			if ('float32[' in line.split(' ')[0]):
 				num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0])
 				temp_list.append(("float_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats))
-			if ('uint64[' in line.split(' ')[0]):
+			elif ('float64[' in line.split(' ')[0]):
+				num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0])
+				temp_list.append(("double_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats))
+			elif ('uint64[' in line.split(' ')[0]):
 				num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0])
 				temp_list.append(("uint64_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats))
 			elif(line.split(' ')[0] == "float32"):
 				temp_list.append(("float",line.split(' ')[1].split('\t')[0].split('\n')[0]))
+			elif(line.split(' ')[0] == "float64"):
+				temp_list.append(("double",line.split(' ')[1].split('\t')[0].split('\n')[0]))
 			elif(line.split(' ')[0] == "uint64") and len(line.split('=')) == 1:
 				temp_list.append(("uint64",line.split(' ')[1].split('\t')[0].split('\n')[0]))
 			elif(line.split(' ')[0] == "uint32") and len(line.split('=')) == 1:
@@ -114,6 +119,10 @@ print("""
 #define PRIu64 "llu"
 #endif
 
+#ifndef PRI64
+#define PRI64 "lld"
+#endif
+
 """)
 for m in messages:
 	print("#include <uORB/topics/%s.h>" % m)
@@ -151,11 +160,19 @@ for index,m in enumerate(messages[1:]):
 	print("\t\t\torb_copy(ID,sub,&container);")
 	for item in message_elements[index+1]:
 		if item[0] == "float":
-			print("\t\t\tprintf(\"%s: %%f\\n\",(double)container.%s);" % (item[1], item[1]))
+			print("\t\t\tprintf(\"%s: %%8.4f\\n\",(double)container.%s);" % (item[1], item[1]))
 		elif item[0] == "float_array":
 			print("\t\t\tprintf(\"%s: \");" % item[1])
 			print("\t\t\tfor (int j = 0; j < %d; j++) {" % item[2])
-			print("\t\t\t\tprintf(\"%%f \",(double)container.%s[j]);" % item[1])
+			print("\t\t\t\tprintf(\"%%8.4f \",(double)container.%s[j]);" % item[1])
+			print("\t\t\t}")
+			print("\t\t\tprintf(\"\\n\");")
+		elif item[0] == "double":
+			print("\t\t\tprintf(\"%s: %%8.4f\\n\",(double)container.%s);" % (item[1], item[1]))
+		elif item[0] == "double_array":
+			print("\t\t\tprintf(\"%s: \");" % item[1])
+			print("\t\t\tfor (int j = 0; j < %d; j++) {" % item[2])
+			print("\t\t\t\tprintf(\"%%8.4f \",(double)container.%s[j]);" % item[1])
 			print("\t\t\t}")
 			print("\t\t\tprintf(\"\\n\");")
 		elif item[0] == "uint64":

Tools/px4airframes/rcout.py

@@ -1,5 +1,6 @@
 from xml.sax.saxutils import escape
 import codecs
+import os
 
 class RCOutput():
     def __init__(self, groups, board):
@@ -30,7 +31,7 @@ class RCOutput():
         for group in groups:
             result += "# GROUP: %s\n\n" % group.GetName()
             for param in group.GetParams():
-                path = param.GetPath().rsplit('/', 1)[1]
+                path = os.path.split(param.GetPath())[1]
                 id_val = param.GetId()
                 name = param.GetFieldValue("short_desc")
                 long_desc = param.GetFieldValue("long_desc")

Tools/px4airframes/srcparser.py

@@ -1,5 +1,6 @@
 import sys
 import re
+import os
 
 class ParameterGroup(object):
     """
@@ -160,7 +161,7 @@ class SourceParser(object):
         """
 
         airframe_id = None
-        airframe_id = path.rsplit('/',1)[1].split('_',1)[0]
+        airframe_id = os.path.split(path)[1].split('_',1)[0]
 
         # Skip if not numeric
         if (not self.IsNumber(airframe_id)):

cmake/common/px4_base.cmake

@@ -136,14 +136,17 @@ function(px4_add_git_submodule)
 		REQUIRED TARGET PATH
 		ARGN ${ARGN})
 	string(REPLACE "/" "_" NAME ${PATH})
-	add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/git_${NAME}.stamp
+	add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp
 		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
-		COMMAND git submodule update --init --recursive -f ${PATH}
-		COMMAND touch ${CMAKE_BINARY_DIR}/git_${NAME}.stamp
+		COMMAND git submodule init ${PATH}
+		COMMAND touch ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp
+		DEPENDS ${CMAKE_SOURCE_DIR}/.gitmodules
 		)
 	add_custom_target(${TARGET}
-		DEPENDS git_${NAME}.stamp
-	)
+		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+		COMMAND git submodule update --recursive ${PATH}
+		DEPENDS ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp
+		)
 endfunction()
 
 #=============================================================================
@@ -418,7 +421,8 @@ function(px4_add_upload)
 			)
 	elseif(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Windows")
 		foreach(port RANGE 32 0)
-			list(APPEND "COM${port}")
+			list(APPEND serial_ports
+				"COM${port}")
 		endforeach()
 	endif()
 	px4_join(OUT serial_ports LIST "${serial_ports}" GLUE ",")
@@ -543,6 +547,7 @@ function(px4_add_common_flags)
 	endif()
 
 	set(c_warnings
+		-Wbad-function-cast
 		-Wstrict-prototypes
 		-Wmissing-prototypes
 		-Wnested-externs

cmake/nuttx/px4_impl_nuttx.cmake

@@ -213,8 +213,8 @@ function(px4_nuttx_add_export)
 	# copy
 	add_custom_command(OUTPUT nuttx_copy_${CONFIG}.stamp
 		COMMAND ${MKDIR} -p ${CMAKE_BINARY_DIR}/${CONFIG}
-		COMMAND ${RM} -rf ${nuttx_src}
-		COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/NuttX ${nuttx_src}
+		COMMAND ${MKDIR} -p ${nuttx_src}
+		COMMAND ${CP} -a ${CMAKE_SOURCE_DIR}/NuttX/. ${nuttx_src}/
 		COMMAND ${RM} -rf ${nuttx_src}/.git
 		COMMAND ${TOUCH} nuttx_copy_${CONFIG}.stamp
 		DEPENDS ${DEPENDS})
@@ -229,7 +229,7 @@ function(px4_nuttx_add_export)
 		COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG} ${nuttx_src}/nuttx/configs
 		COMMAND cd ${nuttx_src}/nuttx/tools && ./configure.sh ${CONFIG}/nsh
 		COMMAND ${ECHO} Exporting NuttX for ${CONFIG}
-		COMMAND ${MAKE} --no-print-directory --quiet -C ${nuttx_src}/nuttx -j${THREADS} -r CONFIG_ARCH_BOARD=${CONFIG} export
+		COMMAND ${MAKE} --no-print-directory --quiet -C ${nuttx_src}/nuttx -j${THREADS} -r CONFIG_ARCH_BOARD=${CONFIG} export > /dev/null
 		COMMAND ${CP} -r ${nuttx_src}/nuttx/nuttx-export.zip ${CMAKE_BINARY_DIR}/${CONFIG}.export
 		DEPENDS ${config_files} ${DEPENDS} __nuttx_copy_${CONFIG})
 

msg/uavcan_parameter_request.msg

@@ -0,0 +1,8 @@
+# UAVCAN-MAVLink parameter bridge request type
+uint8 message_type		# MAVLink message type: PARAM_REQUEST_READ, PARAM_REQUEST_LIST, PARAM_SET
+uint8 node_id			# UAVCAN node ID mapped from MAVLink component ID
+char[17] param_id		# MAVLink/UAVCAN parameter name
+int16 param_index		# -1 if the param_id field should be used as identifier
+uint8 param_type		# MAVLink parameter type
+int64 int_value			# current value if param_type is int-like
+float32 real_value		# current value if param_type is float-like

msg/uavcan_parameter_value.msg

@@ -0,0 +1,8 @@
+# UAVCAN-MAVLink parameter bridge response type
+uint8 node_id			# UAVCAN node ID mapped from MAVLink component ID
+char[17] param_id		# MAVLink/UAVCAN parameter name
+int16 param_index		# parameter index, if known
+uint16 param_count		# number of parameters exposed by the node
+uint8 param_type		# MAVLink parameter type
+int64 int_value			# current value if param_type is int-like
+float32 real_value		# current value if param_type is float-like

msg/vehicle_command.msg

@@ -53,6 +53,7 @@ uint32 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003            # Enable or disable on-b
 uint32 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000    # Command VTOL transition
 uint32 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001	# Prepare a payload deployment in the flight plan
 uint32 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002	# Control a pre-programmed payload deployment
+uint32 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243		# UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started 
 
 uint32 VEHICLE_CMD_RESULT_ACCEPTED = 0			# Command ACCEPTED and EXECUTED |
 uint32 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1	# Command TEMPORARY REJECTED/DENIED |

nuttx-configs/px4fmu-v2/nsh/defconfig

@@ -801,11 +801,11 @@ CONFIG_SCHED_WORKQUEUE=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_WORKPRIORITY=192
 CONFIG_SCHED_WORKPERIOD=5000
-CONFIG_SCHED_WORKSTACKSIZE=1800
+CONFIG_SCHED_WORKSTACKSIZE=1600
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKPERIOD=50000
-CONFIG_SCHED_LPWORKSTACKSIZE=1800
+CONFIG_SCHED_LPWORKSTACKSIZE=1600
 # CONFIG_LIB_KBDCODEC is not set
 # CONFIG_LIB_SLCDCODEC is not set
 

src/drivers/drv_sensor.h

@@ -121,5 +121,10 @@
  */
 #define SENSORIOCGROTATION	_SENSORIOC(6)
 
+/**
+ * Test the sensor calibration
+ */
+#define SENSORIOCCALTEST	_SENSORIOC(7)
+
 #endif /* _DRV_SENSOR_H */
 

src/examples/hwtest/hwtest.c

@@ -57,8 +57,10 @@ __EXPORT int ex_hwtest_main(int argc, char *argv[]);
 
 int ex_hwtest_main(int argc, char *argv[])
 {
-	warnx("DO NOT FORGET TO STOP THE COMMANDER APP!");
-	warnx("(run <commander stop> to do so)");
+	warnx("DO NOT FORGET TO STOP THE DEFAULT CONTROL APPS!");
+	warnx("(run <commander stop>,)");
+	warnx("(    <mc_att_control stop> and)");
+	warnx("(    <fw_att_control stop> to do so)");
 	warnx("usage: http://px4.io/dev/examples/write_output");
 
 	struct actuator_controls_s actuators;

src/modules/commander/PreflightCheck.cpp

@@ -71,7 +71,50 @@
 
 namespace Commander
 {
-static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
+
+static int check_calibration(int fd, const char* param_template, int &devid);
+
+int check_calibration(int fd, const char* param_template, int &devid)
+{
+	bool calibration_found;
+
+	/* new style: ask device for calibration state */
+	int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0);
+
+	calibration_found = (ret == OK);
+	
+	devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
+
+	char s[20];
+	int instance = 0;
+
+	/* old style transition: check param values */
+	while (!calibration_found) {
+		sprintf(s, param_template, instance);
+		param_t parm = param_find(s);
+
+		/* if the calibration param is not present, abort */
+		if (parm == PARAM_INVALID) {
+			break;
+		}
+
+		/* if param get succeeds */
+		int calibration_devid;
+		if (!param_get(parm, &(calibration_devid))) {
+
+			/* if the devid matches, exit early */
+			if (devid == calibration_devid) {
+				calibration_found = true;
+				break;
+			}
+		}
+		instance++;
+	}
+
+	return !calibration_found;
+}
+
+static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
 {
 	bool success = true;
 
@@ -88,13 +131,9 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
 		return false;
 	}
 
-	int calibration_devid;
-	int ret;
-	int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-	sprintf(s, "CAL_MAG%u_ID", instance);
-	param_get(param_find(s), &(calibration_devid));
+	int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id);
 
-	if (devid != calibration_devid) {
+	if (ret) {
 		mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
 		success = false;
@@ -115,7 +154,7 @@ out:
 	return success;
 }
 
-static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic)
+static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id)
 {
 	bool success = true;
 
@@ -132,13 +171,9 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 		return false;
 	}
 
-	int calibration_devid;
-	int ret;
-	int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-	sprintf(s, "CAL_ACC%u_ID", instance);
-	param_get(param_find(s), &(calibration_devid));
+	int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id);
 
-	if (devid != calibration_devid) {
+	if (ret) {
 		mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
 		success = false;
@@ -184,7 +219,7 @@ out:
 	return success;
 }
 
-static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
+static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
 {
 	bool success = true;
 
@@ -201,13 +236,9 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
 		return false;
 	}
 
-	int calibration_devid;
-	int ret;
-	int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-	sprintf(s, "CAL_GYRO%u_ID", instance);
-	param_get(param_find(s), &(calibration_devid));
+	int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id);
 
-	if (devid != calibration_devid) {
+	if (ret) {
 		mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
 		success = false;
@@ -228,7 +259,7 @@ out:
 	return success;
 }
 
-static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
+static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
 {
 	bool success = true;
 
@@ -245,6 +276,20 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
 		return false;
 	}
 
+	device_id = -1000;
+
+	// TODO: There is no baro calibration yet, since no external baros exist
+	// int ret = check_calibration(fd, "CAL_BARO%u_ID");
+
+	// if (ret) {
+	// 	mavlink_and_console_log_critical(mavlink_fd,
+	// 					 "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance);
+	// 	success = false;
+	// 	goto out;
+	// }
+
+//out:
+
 	px4_close(fd);
 	return success;
 }
@@ -311,49 +356,110 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 
 	/* ---- MAG ---- */
 	if (checkMag) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_MAG_PRIME"), &prime_id);
+
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_mag_count; i++) {
 			bool required = (i < max_mandatory_mag_count);
+			int device_id = -1;
 
-			if (!magnometerCheck(mavlink_fd, i, !required) && required) {
+			if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found && prime_id != 0) {
+			mavlink_log_critical(mavlink_fd, "warning: primary compass not operational");
+			failed = true;
 		}
 	}
 
 	/* ---- ACCEL ---- */
 	if (checkAcc) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_ACC_PRIME"), &prime_id);
+
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_accel_count; i++) {
 			bool required = (i < max_mandatory_accel_count);
+			int device_id = -1;
 
-			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) {
+			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found && prime_id != 0) {
+			mavlink_log_critical(mavlink_fd, "warning: primary accelerometer not operational");
+			failed = true;
 		}
 	}
 
 	/* ---- GYRO ---- */
 	if (checkGyro) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_GYRO_PRIME"), &prime_id);
+
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_gyro_count; i++) {
 			bool required = (i < max_mandatory_gyro_count);
+			int device_id = -1;
 
-			if (!gyroCheck(mavlink_fd, i, !required) && required) {
+			if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found && prime_id != 0) {
+			mavlink_log_critical(mavlink_fd, "warning: primary gyro not operational");
+			failed = true;
 		}
 	}
 
 	/* ---- BARO ---- */
 	if (checkBaro) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_BARO_PRIME"), &prime_id);
+
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_baro_count; i++) {
 			bool required = (i < max_mandatory_baro_count);
+			int device_id = -1;
 
-			if (!baroCheck(mavlink_fd, i, !required) && required) {
+			if (!baroCheck(mavlink_fd, i, !required, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		// TODO there is no logic in place to calibrate the primary baro yet
+		// // check if the primary device is present 
+		if (!prime_found && prime_id != 0) {
+			mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational");
+			failed = true;
 		}
 	}
 
@@ -367,12 +473,13 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 	/* ---- RC CALIBRATION ---- */
 	if (checkRC) {
 		if (rc_calibration_check(mavlink_fd) != OK) {
+			mavlink_log_critical(mavlink_fd, "RC calibration check failed");
 			failed = true;
 		}
 	}
 
 	/* ---- Global Navigation Satellite System receiver ---- */
-	if(checkGNSS) {
+	if (checkGNSS) {
 		if(!gnssCheck(mavlink_fd)) {
 			failed = true;
 		}

src/modules/commander/accelerometer_calibration.cpp

@@ -156,6 +156,10 @@ static const int ERROR = -1;
 
 static const char *sensor_name = "accel";
 
+static int32_t device_id[max_accel_sens];
+static int device_prio_max = 0;
+static int32_t device_id_primary = 0;
+
 calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
 calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
 int mat_invert3(float src[3][3], float dst[3][3]);
@@ -172,7 +176,6 @@ typedef struct  {
 int do_accel_calibration(int mavlink_fd)
 {
 	int fd;
-	int32_t device_id[max_accel_sens];
 
 	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
@@ -259,6 +262,8 @@ int do_accel_calibration(int mavlink_fd)
 		
 		bool failed = false;
 
+		failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
+
 		/* set parameters */
 		(void)sprintf(str, "CAL_ACC%u_XOFF", i);
 		failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
@@ -370,6 +375,15 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
 		struct accel_report arp = {};
 		(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
 		timestamps[i] = arp.timestamp;
+
+		// Get priority
+		int32_t prio;
+		orb_priority(worker_data.subs[i], &prio);
+
+		if (prio > device_prio_max) {
+			device_prio_max = prio;
+			device_id_primary = device_id[i];
+		}
 	}
 
 	if (result == calibrate_return_ok) {

src/modules/commander/commander.cpp

@@ -802,6 +802,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE:
+	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:

src/modules/commander/gyro_calibration.cpp

@@ -168,6 +168,9 @@ int do_gyro_calibration(int mavlink_fd)
 		1.0f,	// z scale
 	};
 	
+	int device_prio_max = 0;
+	int32_t device_id_primary = 0;
+
 	for (unsigned s = 0; s < max_gyros; s++) {
 		char str[30];
 		
@@ -199,6 +202,15 @@ int do_gyro_calibration(int mavlink_fd)
 	
 	for (unsigned s = 0; s < max_gyros; s++) {
 		worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
+
+		// Get priority
+		int32_t prio;
+		orb_priority(worker_data.gyro_sensor_sub[s], &prio);
+
+		if (prio > device_prio_max) {
+			device_prio_max = prio;
+			device_id_primary = worker_data.device_id[s];
+		}
 	}
 
 	int cancel_sub  = calibrate_cancel_subscribe();
@@ -258,9 +270,12 @@ int do_gyro_calibration(int mavlink_fd)
 	}
 
 	if (res == OK) {
+
 		/* set offset parameters to new values */
 		bool failed = false;
 
+		failed = failed || (OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary)));
+
 		for (unsigned s = 0; s < max_gyros; s++) {
 			if (worker_data.device_id[s] != 0) {
 				char str[30];

src/modules/commander/mag_calibration.cpp

@@ -73,6 +73,10 @@ static constexpr unsigned int calibration_sides = 6;			///< The total number of
 static constexpr unsigned int calibration_total_points = 240;		///< The total points per magnetometer
 static constexpr unsigned int calibraton_duration_seconds = 42; 	///< The total duration the routine is allowed to take
 
+int32_t	device_ids[max_mags];
+int device_prio_max = 0;
+int32_t device_id_primary = 0;
+
 calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
 
 /// Data passed to calibration worker routine
@@ -108,7 +112,6 @@ int do_mag_calibration(int mavlink_fd)
 	
 	// Determine which mags are available and reset each
 
-	int32_t	device_ids[max_mags];
 	char str[30];
 
 	for (size_t i=0; i<max_mags; i++) {
@@ -434,6 +437,15 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 					result = calibrate_return_error;
 					break;
 				}
+
+				// Get priority
+				int32_t prio;
+				orb_priority(worker_data.sub_mag[cur_mag], &prio);
+
+				if (prio > device_prio_max) {
+					device_prio_max = prio;
+					device_id_primary = device_ids[cur_mag];
+				}
 			}
 		}
 	}
@@ -550,6 +562,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 	}
 	
 	if (result == calibrate_return_ok) {
+
+		(void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary));
+
 		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
 			if (device_ids[cur_mag] != 0) {
 				int fd_mag = -1;

src/modules/ekf_att_pos_estimator/CMakeLists.txt

@@ -39,6 +39,7 @@ endif()
 px4_add_module(
 	MODULE modules__ekf_att_pos_estimator
 	MAIN ekf_att_pos_estimator
+	STACK 3000
 	COMPILE_FLAGS ${MODULE_CFLAGS}
 	SRCS
 		ekf_att_pos_estimator_main.cpp

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -1194,7 +1194,7 @@ int AttitudePositionEstimatorEKF::start()
 	_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 20,
-					 4200,
+					 3900,
 					 (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
 					 nullptr);
 
@@ -1235,6 +1235,7 @@ void AttitudePositionEstimatorEKF::print_status()
 	PX4_INFO("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y,
 	       (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y,
 	       (double)_ekf->correctedDelAng.z);
+
 	PX4_INFO("states (quat)        [0-3]: %8.4f, %8.4f, %8.4f, %8.4f", (double)_ekf->states[0], (double)_ekf->states[1],
 	       (double)_ekf->states[2], (double)_ekf->states[3]);
 	PX4_INFO("states (vel m/s)     [4-6]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[4], (double)_ekf->states[5],
@@ -1338,7 +1339,7 @@ void AttitudePositionEstimatorEKF::pollData()
 		} else {
 			_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]);
 			_ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]);
-			_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]) + 9.80665f;
+			_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]);
 		}
 
 		_ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0];
@@ -1414,21 +1415,21 @@ void AttitudePositionEstimatorEKF::pollData()
 	static hrt_abstime lastprint = 0;
 
 	if (hrt_elapsed_time(&lastprint) > 1000000) {
-		warnx("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
+		PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
 			(double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
 			dtoverflow5, dtoverflow10);
 
-		warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
+		PX4_WARN("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
 		(double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z,
 		(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z);
 
-		warnx("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
+		PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
 		(double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]),
 		(double)(_sensor_combined.accelerometer_integral_m_s[0]),
 		(double)(_sensor_combined.accelerometer_integral_m_s[1]),
 		(double)(_sensor_combined.accelerometer_integral_m_s[2]));
 
-		warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
+		PX4_WARN("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
 		(double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
 		(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT),
 		(double)(_sensor_combined.accelerometer_m_s2[1] * deltaT),
@@ -1738,8 +1739,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "status")) {
-		PX4_INFO("running");
-
 		estimator::g_estimator->print_status();
 
 		return 0;

src/modules/fw_att_control/CMakeLists.txt

@@ -38,7 +38,6 @@ px4_add_module(
 		-Os
 	SRCS
 		fw_att_control_main.cpp
-		fw_att_control_params.c
 	DEPENDS
 		platforms__common
 	)

src/modules/fw_att_control/fw_att_control_params.c

@@ -40,11 +40,6 @@
  * @author Thomas Gubler <thomas@px4.io>
  */
 
-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
-
 /*
  * Controller parameters, accessible via MAVLink
  *

src/modules/fw_pos_control_l1/CMakeLists.txt

@@ -39,11 +39,9 @@ px4_add_module(
 		-Os
 	SRCS
 		fw_pos_control_l1_main.cpp
-		fw_pos_control_l1_params.c
 		landingslope.cpp
 		mtecs/mTecs.cpp
 		mtecs/limitoverride.cpp
-		mtecs/mTecs_params.c
 	DEPENDS
 		platforms__common
 		lib__external_lgpl

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -39,9 +39,6 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
-#include <px4_config.h>
-#include <systemlib/param/param.h>
-
 /*
  * Controller parameters, accessible via MAVLink
  */

src/modules/fw_pos_control_l1/mtecs/mTecs_params.c

@@ -39,9 +39,6 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <px4_config.h>
-#include <systemlib/param/param.h>
-
 /*
  * Controller parameters, accessible via MAVLink
  */

src/modules/mavlink/mavlink_parameters.cpp

@@ -41,27 +41,29 @@
 
 #include <stdio.h>
 
+#include <uORB/topics/uavcan_parameter_request.h>
+#include <uORB/topics/uavcan_parameter_value.h>
+
 #include "mavlink_parameters.h"
 #include "mavlink_main.h"
 
+ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s);
+ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s);
 #define HASH_PARAM "_HASH_CHECK"
 
 MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
 	_send_all_index(-1),
 	_rc_param_map_pub(nullptr),
-	_rc_param_map()
+	_rc_param_map(),
+	_uavcan_parameter_request_pub(nullptr),
+	_uavcan_parameter_value_sub(-1)
 {
 }
 
 unsigned
 MavlinkParametersManager::get_size()
 {
-	if (_send_all_index >= 0) {
-		return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-
-	} else {
-		return 0;
-	}
+	return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 }
 
 void
@@ -78,36 +80,75 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 
 				_send_all_index = 0;
 			}
+
+			if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 &&
+			    (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
+				// publish list request to UAVCAN driver via uORB.
+				uavcan_parameter_request_s req;
+				req.message_type = msg->msgid;
+				req.node_id = req_list.target_component;
+				req.param_index = 0;
+
+				if (_uavcan_parameter_request_pub == nullptr) {
+					_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
+				} else {
+					orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
+				}
+			}
 			break;
 		}
 
 	case MAVLINK_MSG_ID_PARAM_SET: {
 			/* set parameter */
-			if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
-				mavlink_param_set_t set;
-				mavlink_msg_param_set_decode(msg, &set);
+			mavlink_param_set_t set;
+			mavlink_msg_param_set_decode(msg, &set);
 
-				if (set.target_system == mavlink_system.sysid &&
-				    (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
+			if (set.target_system == mavlink_system.sysid &&
+			    (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
 
-					/* local name buffer to enforce null-terminated string */
-					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
-					strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-					/* enforce null termination */
-					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
-					/* attempt to find parameter, set and send it */
-					param_t param = param_find_no_notification(name);
+				/* local name buffer to enforce null-terminated string */
+				char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
+				strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+				/* enforce null termination */
+				name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+				/* attempt to find parameter, set and send it */
+				param_t param = param_find_no_notification(name);
 
-					if (param == PARAM_INVALID) {
-						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
-						sprintf(buf, "[pm] unknown param: %s", name);
-						_mavlink->send_statustext_info(buf);
+				if (param == PARAM_INVALID) {
+					char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
+					sprintf(buf, "[pm] unknown param: %s", name);
+					_mavlink->send_statustext_info(buf);
 
-					} else {
-						/* set and send parameter */
-						param_set(param, &(set.param_value));
-						send_param(param);
-					}
+				} else {
+					/* set and send parameter */
+					param_set(param, &(set.param_value));
+					send_param(param);
+				}
+			}
+
+			if (set.target_system == mavlink_system.sysid && set.target_component < 127 &&
+			    (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
+				// publish set request to UAVCAN driver via uORB.
+				uavcan_parameter_request_s req;
+				req.message_type = msg->msgid;
+				req.node_id = set.target_component;
+				req.param_index = -1;
+				strncpy(req.param_id, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1);
+				req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+				if (set.param_type == MAV_PARAM_TYPE_REAL32) {
+					req.param_type = MAV_PARAM_TYPE_REAL32;
+					req.real_value = set.param_value;
+				} else {
+					int32_t val;
+					memcpy(&val, &set.param_value, sizeof(int32_t));
+					req.param_type = MAV_PARAM_TYPE_INT64;
+					req.int_value = val;
+				}
+
+				if (_uavcan_parameter_request_pub == nullptr) {
+					_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
+				} else {
+					orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
 				}
 			}
 			break;
@@ -160,6 +201,23 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 					}
 				}
 			}
+
+			if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 &&
+				    (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
+				// publish set request to UAVCAN driver via uORB.
+				uavcan_parameter_request_s req;
+				req.message_type = msg->msgid;
+				req.node_id = req_read.target_component;
+				req.param_index = req_read.param_index;
+				strncpy(req.param_id, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1);
+				req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+
+				if (_uavcan_parameter_request_pub == nullptr) {
+					_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
+				} else {
+					orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
+				}
+			}
 			break;
 		}
 
@@ -208,11 +266,38 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 void
 MavlinkParametersManager::send(const hrt_abstime t)
 {
-	/* send all parameters if requested, but only after the system has booted */
-	if (_send_all_index >= 0 && _mavlink->boot_complete()) {
+	bool space_available = _mavlink->get_free_tx_buf() >= get_size();
+
+	/* Send parameter values received from the UAVCAN topic */
+	if (_uavcan_parameter_value_sub < 0) {
+		_uavcan_parameter_value_sub = orb_subscribe(ORB_ID(uavcan_parameter_value));
+	}
+
+	bool param_value_ready;
+	orb_check(_uavcan_parameter_value_sub, &param_value_ready);
+	if (space_available && param_value_ready) {
+		struct uavcan_parameter_value_s value;
+		orb_copy(ORB_ID(uavcan_parameter_value), _uavcan_parameter_value_sub, &value);
+
+		mavlink_param_value_t msg;
+		msg.param_count = value.param_count;
+		msg.param_index = value.param_index;
+		strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+		if (value.param_type == MAV_PARAM_TYPE_REAL32) {
+			msg.param_type = MAVLINK_TYPE_FLOAT;
+			msg.param_value = value.real_value;
+		} else {
+			int32_t val;
+			val = (int32_t)value.int_value;
+			memcpy(&msg.param_value, &val, sizeof(int32_t));
+			msg.param_type = MAVLINK_TYPE_INT32_T;
+		}
+		_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg, value.node_id);
+	} else if (_send_all_index >= 0 && _mavlink->boot_complete()) {
+		/* send all parameters if requested, but only after the system has booted */
 
 		/* skip if no space is available */
-		if (_mavlink->get_free_tx_buf() < get_size()) {
+		if (!space_available) {
 			return;
 		}
 

src/modules/mavlink/mavlink_parameters.h

@@ -118,4 +118,7 @@ protected:
 
 	orb_advert_t _rc_param_map_pub;
 	struct rc_parameter_map_s _rc_param_map;
+
+	orb_advert_t _uavcan_parameter_request_pub;
+	int _uavcan_parameter_value_sub;
 };

src/modules/mavlink/mavlink_receiver.cpp

@@ -1444,12 +1444,12 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
 		hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
 		hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2;
-		hil_sensors.accelerometer_m_s2[0] = ((imu.xacc * dt + _hil_prev_accel[0]) / 2.0f) / dt;
-		hil_sensors.accelerometer_m_s2[1] = ((imu.yacc * dt + _hil_prev_accel[1]) / 2.0f) / dt;
-		hil_sensors.accelerometer_m_s2[2] = (((imu.zacc + 9.80665f) * dt + _hil_prev_accel[2]) / 2.0f) / dt - 9.80665f;
+		hil_sensors.accelerometer_m_s2[0] = imu.xacc;
+		hil_sensors.accelerometer_m_s2[1] = imu.yacc;
+		hil_sensors.accelerometer_m_s2[2] = imu.zacc;
 		hil_sensors.accelerometer_integral_m_s[0] = (imu.xacc * dt + _hil_prev_accel[0]) / 2.0f;
 		hil_sensors.accelerometer_integral_m_s[1] = (imu.yacc * dt + _hil_prev_accel[1]) / 2.0f;
-		hil_sensors.accelerometer_integral_m_s[2] = ((imu.zacc + 9.80665f) * dt + _hil_prev_accel[2]) / 2.0f;
+		hil_sensors.accelerometer_integral_m_s[2] = (imu.zacc * dt + _hil_prev_accel[2]) / 2.0f;
 		memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_integral_m_s[0], sizeof(_hil_prev_accel));
 		hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f;
 		hil_sensors.accelerometer_mode[0] = 0; // TODO what is this?

src/modules/mavlink/mavlink_tests/module.mk

@@ -1,53 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
-#
-# 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.
-#
-############################################################################
-
-#
-# System state machine tests.
-#
-
-MODULE_COMMAND		= mavlink_tests
-SRCS			= mavlink_tests.cpp \
-			mavlink_ftp_test.cpp \
-			../mavlink_stream.cpp \
-			../mavlink_ftp.cpp \
-			../mavlink.c
-
-INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
-
-MODULE_STACKSIZE = 5000
-
-MAXOPTIMIZATION	 = -Os
-
-EXTRACXXFLAGS	= -Weffc++ -DMAVLINK_FTP_UNIT_TEST -Wno-attributes -Wno-packed
-
-EXTRACFLAGS	= -Wno-packed

src/modules/navigator/CMakeLists.txt

@@ -39,24 +39,17 @@ px4_add_module(
 		-Os
 	SRCS
 		navigator_main.cpp
-		navigator_params.c
 		navigator_mode.cpp
 		mission_block.cpp
 		mission.cpp
-		mission_params.c
 		loiter.cpp
 		rtl.cpp
-		rtl_params.c
 		mission_feasibility_checker.cpp
 		geofence.cpp
-		geofence_params.c
 		datalinkloss.cpp
-		datalinkloss_params.c
 		rcloss.cpp
-		rcloss_params.c
 		enginefailure.cpp
 		gpsfailure.cpp
-		gpsfailure_params.c
 	DEPENDS
 		platforms__common
 	)

src/modules/navigator/datalinkloss_params.c

@@ -39,10 +39,6 @@
  * @author Thomas Gubler <thomas@px4.io>
  */
 
-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
 /*
  * Data Link Loss parameters, accessible via MAVLink
  */

src/modules/navigator/geofence_params.c

@@ -39,10 +39,6 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
 /*
  * Geofence parameters, accessible via MAVLink
  */

src/modules/navigator/gpsfailure_params.c

@@ -39,10 +39,6 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
 /*
  * GPS Failure Navigation Mode parameters, accessible via MAVLink
  */

src/modules/navigator/mission_feasibility_checker.cpp

@@ -336,7 +336,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
 
 					} else {
 						/* item is too far from home */
-						mavlink_log_critical(_mavlink_fd, "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)dist_first_wp);
+						mavlink_log_critical(_mavlink_fd, "First waypoint too far: %d m,refusing mission", (int)dist_to_1wp, (int)dist_first_wp);
 						warning_issued = true;
 						return false;
 					}

src/modules/navigator/mission_params.c

@@ -39,10 +39,6 @@
  * @author Julian Oes <joes@student.ethz.ch>
  */
 
-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
 /*
  * Mission parameters, accessible via MAVLink
  */

src/modules/navigator/navigator_params.c

@@ -40,10 +40,6 @@
  * @author Thomas Gubler <thomas@px4.io>
  */
 
-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
 /**
  * Loiter radius (FW only)
  *

src/modules/navigator/rcloss_params.c

@@ -39,10 +39,6 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
 /*
  * OBC RC Loss mode parameters, accessible via MAVLink
  */

src/modules/navigator/rtl_params.c

@@ -39,10 +39,6 @@
  * @author Julian Oes <julian@oes.ch>
  */
 
-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
 /*
  * RTL parameters, accessible via MAVLink
  */

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -897,7 +897,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		}
 
 		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
-		dt = fmaxf(fminf(0.02, dt), 0.002);		// constrain dt from 2 to 20 ms
+		dt = fmaxf(fminf(0.02, dt), 0.0002);		// constrain dt from 0.2 to 20 ms
 		t_prev = t;
 
 		/* increase EPH/EPV on each step */

src/modules/sdlog2/logbuffer.c

@@ -50,8 +50,8 @@ int logbuffer_init(struct logbuffer_s *lb, int size)
 	lb->size  = size;
 	lb->write_ptr = 0;
 	lb->read_ptr = 0;
-	lb->data = malloc(lb->size);
-	return (lb->data == 0) ? PX4_ERROR : PX4_OK;
+	lb->data = NULL;
+	return PX4_OK;
 }
 
 int logbuffer_count(struct logbuffer_s *lb)
@@ -72,6 +72,16 @@ int logbuffer_is_empty(struct logbuffer_s *lb)
 
 bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size)
 {
+	// allocate buffer if not yet present
+	if (lb->data == NULL) {
+		lb->data = malloc(lb->size);
+	}
+
+	// allocation failed, bail out
+	if (lb->data == NULL) {
+		return false;
+	}
+
 	// bytes available to write
 	int available = lb->read_ptr - lb->write_ptr - 1;
 

src/modules/sensors/CMakeLists.txt

@@ -40,7 +40,6 @@ px4_add_module(
 		-O3
 	SRCS
 		sensors.cpp
-		sensor_params.c
 	DEPENDS
 		platforms__common
 	)

src/modules/sensors/sensor_params.c

@@ -585,6 +585,34 @@ PARAM_DEFINE_FLOAT(CAL_ACC2_YSCALE, 1.0f);
  */
 PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 1.0f);
 
+/**
+ * Primary accel ID
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_ACC_PRIME, 0);
+
+/**
+ * Primary gyro ID
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_GYRO_PRIME, 0);
+
+/**
+ * Primary mag ID
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0);
+
+/**
+ * Primary baro ID
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_BARO_PRIME, 0);
+
 /**
  * Differential pressure sensor offset
  *

src/modules/sensors/sensors.cpp

@@ -1412,6 +1412,8 @@ Sensors::parameter_update_poll(bool forced)
 				(void)sprintf(str, "CAL_MAG%u_ID", i);
 				int device_id;
 				failed = failed || (OK != param_get(param_find(str), &device_id));
+				(void)sprintf(str, "CAL_MAG%u_ROT", i);
+				(void)param_find(str);
 
 				if (failed) {
 					px4_close(fd);
@@ -2020,6 +2022,11 @@ Sensors::task_main()
 	 * do subscriptions
 	 */
 
+	unsigned gcount_prev = _gyro_count;
+	unsigned mcount_prev = _mag_count;
+	unsigned acount_prev = _accel_count;
+	unsigned bcount_prev = _baro_count;
+
 	_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
 		&raw.gyro_priority[0], &raw.gyro_errcount[0]);
 
@@ -2032,6 +2039,15 @@ Sensors::task_main()
 	_baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
 		&raw.baro_priority[0], &raw.baro_errcount[0]);
 
+	if (gcount_prev != _gyro_count ||
+	    mcount_prev != _mag_count ||
+	    acount_prev != _accel_count ||
+	    bcount_prev != _baro_count) {
+
+		/* reload calibration params */
+		parameter_update_poll(true);
+	}
+
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
 	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));

src/modules/uavcan/CMakeLists.txt

@@ -63,7 +63,7 @@ px4_add_module(
 	COMPILE_FLAGS
 		-Wframe-larger-than=1500
 		-Wno-deprecated-declarations
-		-O3
+		-Os
 	SRCS
 		# Main
 		uavcan_main.cpp
@@ -81,7 +81,7 @@ px4_add_module(
 
 	DEPENDS
 		platforms__common
-		uavcan	
+		uavcan
 	)
 
 ## vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/uavcan/uavcan_main.hpp

@@ -94,7 +94,7 @@ class UavcanNode : public device::CDev
 	 */
 
 	static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At
-	static constexpr unsigned StackSize          = 1600;
+	static constexpr unsigned StackSize          = 1800;
 
 public:
 	typedef uavcan::Node<MemPoolSize> Node;

src/modules/uavcan/uavcan_servers.hpp

@@ -42,16 +42,20 @@
 #include <uavcan/node/sub_node.hpp>
 #include <uavcan/protocol/node_status_monitor.hpp>
 
-# include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
-# include <uavcan/protocol/node_info_retriever.hpp>
-# include <uavcan_posix/basic_file_server_backend.hpp>
-# include <uavcan/protocol/firmware_update_trigger.hpp>
-# include <uavcan/protocol/file_server.hpp>
-# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
-# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
-# include <uavcan_posix/firmware_version_checker.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
+#include <uavcan/protocol/node_info_retriever.hpp>
+#include <uavcan_posix/basic_file_server_backend.hpp>
+#include <uavcan/protocol/firmware_update_trigger.hpp>
+#include <uavcan/protocol/file_server.hpp>
+#include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
+#include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
+#include <uavcan_posix/firmware_version_checker.hpp>
+#include <uavcan/equipment/esc/RawCommand.hpp>
+#include <uavcan/equipment/indication/BeepCommand.hpp>
+#include <uavcan/protocol/enumeration/Begin.hpp>
+#include <uavcan/protocol/enumeration/Indication.hpp>
 
-# include "uavcan_virtual_can_driver.hpp"
+#include "uavcan_virtual_can_driver.hpp"
 
 /**
  * @file uavcan_servers.hpp
@@ -65,6 +69,9 @@
 #define UAVCAN_DEVICE_PATH	"/dev/uavcan/esc"
 #define UAVCAN_NODE_DB_PATH     "/fs/microsd/uavcan.db"
 #define UAVCAN_FIRMWARE_PATH    "/fs/microsd/fw"
+#define UAVCAN_ROMFS_FW_PATH "/etc/uavcan/fw"
+#define UAVCAN_ROMFS_FW_PREFIX "romfs_"
+#define UAVCAN_MAX_PATH_LENGTH (128 + 40)
 #define UAVCAN_LOG_FILE         UAVCAN_NODE_DB_PATH"/trace.log"
 
 /**
@@ -88,7 +95,7 @@ class UavcanServers
 	static constexpr unsigned QueuePoolSize =
 		(NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer);
 
-	static constexpr unsigned StackSize  = 3500;
+	static constexpr unsigned StackSize  = 6000;
 	static constexpr unsigned Priority  =  120;
 
 	typedef uavcan::SubNode<MemPoolSize> SubNode;
@@ -140,7 +147,84 @@ private:
 	uavcan::FirmwareUpdateTrigger   _fw_upgrade_trigger;
 	uavcan::BasicFileServer         _fw_server;
 
+	/*
+	 * The MAVLink parameter bridge needs to know the maximum parameter index
+	 * of each node so that clients can determine when parameter listings have
+	 * finished. We do that by querying a node's entire parameter set whenever
+	 * a parameter message is received for a node with a zero _param_count,
+	 * and storing the count here. If a node doesn't respond, or doesn't have
+	 * any parameters, its count will stay at zero and we'll try to query the
+	 * set again next time.
+	 *
+	 * The node's UAVCAN ID is used as the index into the _param_counts array.
+	 */
+	uint8_t _param_counts[128];
+	bool _count_in_progress;
+	uint8_t _count_index;
+
+	bool _param_in_progress;
+	uint8_t _param_index;
+	bool _param_list_in_progress;
+	bool _param_list_all_nodes;
+	uint8_t _param_list_node_id;
+
+	uint32_t _param_dirty_bitmap[4];
+	uint8_t _param_save_opcode;
+
+	bool _cmd_in_progress;
+
+	// uORB topic handle for MAVLink parameter responses
+	orb_advert_t _param_response_pub;
+
+	typedef uavcan::MethodBinder<UavcanServers *,
+		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;
+	typedef uavcan::MethodBinder<UavcanServers *,
+		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &)> ExecuteOpcodeCallback;
+	typedef uavcan::MethodBinder<UavcanServers *,
+		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
+	void cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
+	void cb_count(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
+	void cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
+	void cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result);
+
+	uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _param_getset_client;
+	uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _param_opcode_client;
+	uavcan::ServiceClient<uavcan::protocol::RestartNode, RestartNodeCallback> _param_restartnode_client;
+	void param_count(uavcan::NodeID node_id);
+	void param_opcode(uavcan::NodeID node_id);
+
+	uint8_t get_next_active_node_id(uint8_t base);
+	uint8_t get_next_dirty_node_id(uint8_t base);
+	void set_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] |= 1 << (node_id & 31); }
+	void clear_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] &= ~(1 << (node_id & 31)); }
+	bool are_node_params_dirty(uint8_t node_id) const { return bool((_param_dirty_bitmap[node_id >> 5] >> (node_id & 31)) & 1); }
+
 	bool _mutex_inited;
 	volatile bool _check_fw;
 
+	// ESC enumeration
+	bool _esc_enumeration_active;
+	uint8_t _esc_enumeration_ids[uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize];
+	uint8_t _esc_enumeration_index;
+	uint8_t _esc_set_index;
+	uint8_t _esc_count;
+
+	typedef uavcan::MethodBinder<UavcanServers *,
+		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &)> EnumerationBeginCallback;
+	typedef uavcan::MethodBinder<UavcanServers *,
+		void (UavcanServers::*)(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication>&)>
+		EnumerationIndicationCallback;
+	void cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result);
+	void cb_enumeration_indication(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication> &msg);
+	void cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
+	void cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
+
+	uavcan::Publisher<uavcan::equipment::indication::BeepCommand> _beep_pub;
+	uavcan::Subscriber<uavcan::protocol::enumeration::Indication, EnumerationIndicationCallback> _enumeration_indication_sub;
+	uavcan::ServiceClient<uavcan::protocol::enumeration::Begin, EnumerationBeginCallback> _enumeration_client;
+	uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _enumeration_getset_client;
+	uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _enumeration_save_client;
+
+	void unpackFwFromROMFS(const char* sd_path, const char* romfs_path);
+	int copyFw(const char* dst, const char* src);
 };

src/platforms/px4_log.h

@@ -130,7 +130,7 @@ __EXPORT extern int __px4_log_level_current;
 __END_DECLS
 
 // __px4_log_level_current will be initialized to PX4_LOG_LEVEL_AT_RUN_TIME
-#define PX4_LOG_LEVEL_AT_RUN_TIME	_PX4_LOG_LEVEL_WARN
+#define PX4_LOG_LEVEL_AT_RUN_TIME	_PX4_LOG_LEVEL_ERROR
 
 /****************************************************************************
  * Implementation of log section formatting based on printf
@@ -357,8 +357,8 @@ __END_DECLS
 /****************************************************************************
  * Non-verbose settings for a Release build to minimize strings in build
  ****************************************************************************/
-#define PX4_PANIC(FMT, ...)	__px4_log_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__)
-#define PX4_ERR(FMT, ...)	__px4_log_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__)
+#define PX4_PANIC(FMT, ...)	__px4_log(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__)
+#define PX4_ERR(FMT, ...)	__px4_log(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__)
 #define PX4_WARN(FMT, ...) 	__px4_log_omit(_PX4_LOG_LEVEL_WARN,  FMT, ##__VA_ARGS__)
 #define PX4_DEBUG(FMT, ...) 	__px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__)
 
@@ -366,9 +366,9 @@ __END_DECLS
 /****************************************************************************
  * Medium verbose settings for a default build
  ****************************************************************************/
-#define PX4_PANIC(FMT, ...)	__px4_log_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__)
-#define PX4_ERR(FMT, ...)	__px4_log_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__)
-#define PX4_WARN(FMT, ...) 	__px4_log_file_and_line(_PX4_LOG_LEVEL_WARN,  FMT, ##__VA_ARGS__)
+#define PX4_PANIC(FMT, ...)	__px4_log(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__)
+#define PX4_ERR(FMT, ...)	__px4_log(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__)
+#define PX4_WARN(FMT, ...) 	__px4_log(_PX4_LOG_LEVEL_WARN,  FMT, ##__VA_ARGS__)
 #define PX4_DEBUG(FMT, ...) 	__px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__)
 
 #endif