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

.gitignore

@@ -46,3 +46,5 @@ Firmware.zip
 unittests/build
 *.generated.h
 .vagrant
+*.pretty
+

Makefile

@@ -262,6 +262,10 @@ testbuild:
 tests:	generateuorbtopicheaders
 	$(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) unittests)
 
+.PHONY: format check_format
+check_format:
+	$(Q) (./Tools/check_code_style.sh | sort -n)
+
 #
 # Cleanup targets.  'clean' should remove all built products and force
 # a complete re-compilation, 'distclean' should remove everything

NuttX

@@ -1 +1 @@
-Subproject commit 787aca971a86219d4e791100646b54ed8245a733
+Subproject commit 11afcdfee6a3961952dd92f02c1abaa4756b115f

ROMFS/px4fmu_common/init.d/rcS

@@ -436,7 +436,7 @@ then
 	then
 		if param compare SYS_COMPANION 921600
 		then
-			mavlink start -d /dev/ttyS2 -b 921600 -m onboard
+			mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000
 		fi
 	fi
 

Tools/check_code_style.sh

@@ -0,0 +1,31 @@
+#!/usr/bin/env bash
+set -eu
+failed=0
+for fn in $(find . -path './src/lib/uavcan' -prune -o \
+                   -path './src/lib/mathlib/CMSIS' -prune -o \
+                   -path './src/modules/attitude_estimator_ekf/codegen/' -prune -o \
+                   -path './NuttX' -prune -o \
+                   -path './Build' -prune -o \
+                   -path './mavlink' -prune -o \
+                   -path './unittests/gtest' -prune -o \
+                   -path './unittests/build' -prune -o \
+                   -name '*.c' -o -name '*.cpp' -o -name '*.hpp' -o -name '*.h' -type f); do
+  if [ -f "$fn" ];
+  then
+    ./Tools/fix_code_style.sh --quiet < $fn > $fn.pretty
+    diffsize=$(diff -y --suppress-common-lines $fn $fn.pretty | wc -l)
+    rm -f $fn.pretty
+    if [ $diffsize -ne 0 ]; then
+      failed=1
+      echo $diffsize $fn
+    fi
+  fi
+done
+
+if [ $failed -eq 0 ]; then
+    echo "Format checks passed"
+    exit 0
+else
+    echo "Format checks failed; please run ./Tools/fix_code_style.sh on each file"
+    exit 1
+fi

Tools/fix_code_style.sh

@@ -18,4 +18,5 @@ astyle \
     --exclude=EASTL		\
     --add-brackets		\
     --max-code-length=120	\
+    --preserve-date             \
     $*

Tools/pre-commit

@@ -0,0 +1,26 @@
+#!/bin/sh
+if git rev-parse --verify HEAD >/dev/null 2>&1
+then
+  against=HEAD
+else
+  # Initial commit: diff against an empty tree object
+  against=4b825dc642cb6eb9a060e54bf8d69288fbee4904
+fi
+
+# Redirect output to stderr.
+exec 1>&2
+
+CHANGED_FILES=`git diff --cached --name-only --diff-filter=ACM $against | grep '\.c\|\.cpp\|\.h\|\.hpp'`
+FAILED=0
+if [ ! -z "$CHANGED_FILES" -a "$CHANGED_FILES" != " " ]; then
+  for FILE in $CHANGED_FILES; do
+    ./Tools/fix_code_style.sh --quiet < $FILE > $FILE.pretty
+    diff -u $FILE $FILE.pretty || FAILED=1
+    rm -f $FILE.pretty
+    if [ $FAILED -ne 0 ]; then
+      echo "There are code formatting errors. Please fix them by running ./Tools/fix_code_style.sh $FILE"
+      exit $FAILED
+    fi
+  done
+fi
+exit 0

Tools/px_uploader.py

@@ -416,7 +416,7 @@ class uploader(object):
         def upload(self, fw):
                 # Make sure we are doing the right thing
                 if self.board_type != fw.property('board_id'):
-                        raise RuntimeError("Firmware not suitable for this board")
+                        raise IOError("Firmware not suitable for this board")
                 if self.fw_maxsize < fw.property('image_size'):
                         raise RuntimeError("Firmware image is too large for this board")
 
@@ -564,10 +564,13 @@ try:
                             up.upload(fw)
 
                     except RuntimeError as ex:
-
                             # print the error
                             print("\nERROR: %s" % ex.args)
 
+                    except IOError as e:
+                            up.close();
+                            continue
+
                     finally:
                             # always close the port
                             up.close()

integrationtests/demo_tests/manual_input.py

@@ -39,6 +39,7 @@ import sys
 import rospy
 
 from px4.msg import manual_control_setpoint
+from px4.msg import offboard_control_mode
 from mav_msgs.msg import CommandAttitudeThrust
 from std_msgs.msg import Header
 
@@ -46,13 +47,15 @@ from std_msgs.msg import Header
 # Manual input control helper
 #
 # FIXME: this is not the way to do it! ATM it fakes input to iris/command/attitude because else
-# the simulator does not instantiate our controller.
+# the simulator does not instantiate our controller. Probably this whole class will disappear once
+# arming works correctly.
 #
 class ManualInput:
 
     def __init__(self):
         rospy.init_node('test_node', anonymous=True)
         self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10)
+        self.pubOff = rospy.Publisher('px4_multicopter/offboard_control_mode', offboard_control_mode, queue_size=10)
         self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10)
 
     def arm(self):
@@ -119,9 +122,35 @@ class ManualInput:
             rate.sleep()
             count = count + 1
 
-    def offboard(self):
+
+    def offboard_attctl(self):
+        self.offboard(False, False, True, True, True, True)
+
+    def offboard_posctl(self):
+        self.offboard(False, False, True, False, True, True)
+
+    # Trigger offboard and set offboard control mode before
+    def offboard(self, ignore_thrust=False, ignore_attitude=False, ignore_bodyrate=True,
+                        ignore_position=False, ignore_velocity=True, ignore_acceleration_force=True):
         rate = rospy.Rate(10) # 10hz
 
+        mode = offboard_control_mode()
+        mode.ignore_thrust = ignore_thrust
+        mode.ignore_attitude = ignore_attitude
+        mode.ignore_bodyrate = ignore_bodyrate
+        mode.ignore_position = ignore_position
+        mode.ignore_velocity = ignore_velocity
+        mode.ignore_acceleration_force = ignore_acceleration_force
+
+        count = 0
+        while not rospy.is_shutdown() and count < 5:
+            rospy.loginfo("setting offboard mode")
+            time = rospy.get_rostime().now()
+            mode.timestamp = time.secs * 1e6 + time.nsecs / 1000
+            self.pubOff.publish(mode)
+            rate.sleep()
+            count = count + 1
+
         # triggers offboard
         pos = manual_control_setpoint()
         pos.x = 0

integrationtests/demo_tests/mavros_offboard_attctl_test.py

@@ -100,6 +100,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
         # FIXME: this must go ASAP when arming is implemented
         manIn = ManualInput()
         manIn.arm()
+        manIn.offboard_attctl()
 
         self.assertTrue(self.arm(), "Could not arm")
         self.rateSec.sleep()
@@ -111,7 +112,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
         att.header = Header() 
         att.header.frame_id = "base_footprint"
         att.header.stamp = rospy.Time.now()
-        quaternion = quaternion_from_euler(0.2, 0.2, 0)
+        quaternion = quaternion_from_euler(0.15, 0.15, 0)
         att.pose.orientation = Quaternion(*quaternion)
 
         throttle = Float64()

integrationtests/demo_tests/mavros_offboard_posctl_test.py

@@ -136,7 +136,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
         # FIXME: this must go ASAP when arming is implemented
         manIn = ManualInput()
         manIn.arm()
-        manIn.offboard()
+        manIn.offboard_posctl()
 
         self.assertTrue(self.arm(), "Could not arm")
         self.rateSec.sleep()

nuttx-configs/aerocore/nsh/defconfig

@@ -332,7 +332,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717
 CONFIG_DRAM_START=0x20000000
 CONFIG_DRAM_SIZE=262144
 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
-CONFIG_ARCH_INTERRUPTSTACK=4096
+CONFIG_ARCH_INTERRUPTSTACK=1500
 
 #
 # Boot options
@@ -415,8 +415,8 @@ CONFIG_PREALLOC_TIMERS=50
 #
 # Stack and heap information
 #
-CONFIG_IDLETHREAD_STACKSIZE=6000
-CONFIG_USERMAIN_STACKSIZE=4096
+CONFIG_IDLETHREAD_STACKSIZE=1000
+CONFIG_USERMAIN_STACKSIZE=3000
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
 

nuttx-configs/px4fmu-v1/nsh/defconfig

@@ -325,7 +325,8 @@ CONFIG_BOARD_LOOPSPERMSEC=16717
 CONFIG_DRAM_START=0x20000000
 CONFIG_DRAM_SIZE=196608
 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
-CONFIG_ARCH_INTERRUPTSTACK=4096
+# The actual usage is 420 bytes
+CONFIG_ARCH_INTERRUPTSTACK=1500
 
 #
 # Boot options
@@ -416,8 +417,8 @@ CONFIG_PREALLOC_TIMERS=50
 #
 # Stack and heap information
 #
-CONFIG_IDLETHREAD_STACKSIZE=3500
-CONFIG_USERMAIN_STACKSIZE=2600
+CONFIG_IDLETHREAD_STACKSIZE=1000
+CONFIG_USERMAIN_STACKSIZE=3000
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
 

nuttx-configs/px4fmu-v2/nsh/defconfig

@@ -367,7 +367,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717
 CONFIG_DRAM_START=0x20000000
 CONFIG_DRAM_SIZE=262144
 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
-CONFIG_ARCH_INTERRUPTSTACK=4096
+CONFIG_ARCH_INTERRUPTSTACK=1500
 
 #
 # Boot options
@@ -451,8 +451,8 @@ CONFIG_PREALLOC_TIMERS=50
 #
 # Stack and heap information
 #
-CONFIG_IDLETHREAD_STACKSIZE=3500
-CONFIG_USERMAIN_STACKSIZE=2600
+CONFIG_IDLETHREAD_STACKSIZE=1000
+CONFIG_USERMAIN_STACKSIZE=3000
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
 

src/lib/mathlib/math/Quaternion.hpp

@@ -115,6 +115,35 @@ public:
 		return Vector<3>(&data[1]);
 	}
 
+	/**
+	 * inverse of quaternion
+	 */
+	math::Quaternion inverse() {
+		Quaternion res;
+		memcpy(res.data,data,sizeof(res.data));
+		res.data[1] = -res.data[1];
+		res.data[2] = -res.data[2];
+		res.data[3] = -res.data[3];
+		return res;
+	}
+
+
+	/**
+	* rotate vector by quaternion
+	*/
+	Vector<3> rotate(const Vector<3> &w) {
+		Quaternion q_w;	// extend vector to quaternion
+		Quaternion q = {data[0],data[1],data[2],data[3]};
+		Quaternion q_rotated;	// quaternion representation of rotated vector
+		q_w(0) = 0;
+		q_w(1) = w.data[0];
+		q_w(2) = w.data[1];
+		q_w(3) = w.data[2];
+		q_rotated = q*q_w*q.inverse();
+		Vector<3> res = {q_rotated.data[1],q_rotated.data[2],q_rotated.data[3]};
+		return res;
+	}
+
 	/**
 	 * set quaternion to rotation defined by euler angles
 	 */

src/modules/commander/commander.cpp

@@ -966,7 +966,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	pthread_attr_t commander_low_prio_attr;
 	pthread_attr_init(&commander_low_prio_attr);
-	pthread_attr_setstacksize(&commander_low_prio_attr, 2600);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 2400);
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);

src/modules/mavlink/mavlink_main.cpp

@@ -1621,7 +1621,7 @@ Mavlink::start(int argc, char *argv[])
 	task_spawn_cmd(buf,
 		       SCHED_DEFAULT,
 		       SCHED_PRIORITY_DEFAULT,
-		       2700,
+		       2400,
 		       (main_t)&Mavlink::start_helper,
 		       (char * const *)argv);
 

src/modules/mavlink/module.mk

@@ -51,7 +51,7 @@ INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 
 MAXOPTIMIZATION	 = -Os
 
-MODULE_STACKSIZE = 1024
+MODULE_STACKSIZE = 1200
 
 EXTRACXXFLAGS	= -Weffc++ -Wno-attributes -Wno-packed
 

src/modules/sensors/sensor_params.c

@@ -993,6 +993,36 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
 PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124);	/* scaling for 3DR power brick */
 
 
+/**
+ * RC channel count
+ *
+ * This parameter is used by Ground Station software to save the number
+ * of channels which were used during RC calibration. It is only meant
+ * for ground station use.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+
+PARAM_DEFINE_INT32(RC_CHAN_CNT, 0);
+
+/**
+ * RC mode switch threshold automaic distribution
+ *
+ * This parameter is used by Ground Station software to specify whether
+ * the threshold values for flight mode switches were automatically calculated.
+ * 0 indicates that the threshold values were set by the user. Any other value
+ * indicates that the threshold value where automatically set by the ground
+ * station software. It is only meant for ground station use.
+ *
+ * @min 0
+ * @max 1
+ * @group Radio Calibration
+ */
+
+PARAM_DEFINE_INT32(RC_TH_USER, 1);
+
 /**
  * Roll control channel mapping.
  *