diff --git a/.gitignore b/.gitignore index 611325444e..0e553fa365 100644 --- a/.gitignore +++ b/.gitignore @@ -46,3 +46,5 @@ Firmware.zip unittests/build *.generated.h .vagrant +*.pretty + diff --git a/Makefile b/Makefile index 7620099f98..201187e021 100644 --- a/Makefile +++ b/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 diff --git a/NuttX b/NuttX index 787aca971a..11afcdfee6 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 787aca971a86219d4e791100646b54ed8245a733 +Subproject commit 11afcdfee6a3961952dd92f02c1abaa4756b115f diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ea3f721f34..9118f26013 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/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 diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh new file mode 100755 index 0000000000..8d1ab6363b --- /dev/null +++ b/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 diff --git a/Tools/fix_code_style.sh b/Tools/fix_code_style.sh index 5995d428ea..e73a5a8af2 100755 --- a/Tools/fix_code_style.sh +++ b/Tools/fix_code_style.sh @@ -18,4 +18,5 @@ astyle \ --exclude=EASTL \ --add-brackets \ --max-code-length=120 \ + --preserve-date \ $* diff --git a/Tools/pre-commit b/Tools/pre-commit new file mode 100755 index 0000000000..13cd4aaddb --- /dev/null +++ b/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 diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index f4e317cfa8..859c821e5b 100755 --- a/Tools/px_uploader.py +++ b/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() diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index cf139bb1da..9b2471a00e 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/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 diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index 27885635a1..a52f7ffc1b 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/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() diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index a1f1cf3c55..a3739ae5ce 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/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() diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig index 29dff64aad..08ff4a9885 100644 --- a/nuttx-configs/aerocore/nsh/defconfig +++ b/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 diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 86ed041fff..cd410051c7 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/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 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 6a1aec22bd..4ccc5dacb6 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/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 diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 38400beef5..d28966fca6 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/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 */ diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 74ebe0ae43..f832f761ef 100644 --- a/src/modules/commander/commander.cpp +++ b/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, ¶m); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0132a3193d..f8e819ce7a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/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); diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index f9d30dcbea..e82b8bd935 100644 --- a/src/modules/mavlink/module.mk +++ b/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 diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 5ec6780331..272e4b14f3 100644 --- a/src/modules/sensors/sensor_params.c +++ b/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. *