diff options
author | Marco Bauer <marco@wtns.de> | 2015-03-06 15:32:30 +0100 |
---|---|---|
committer | Marco Bauer <marco@wtns.de> | 2015-03-06 15:32:30 +0100 |
commit | 4a977af8708e3f59b38793dfe501b75842efa08d (patch) | |
tree | 3788fec32ba7757f61cb8b659f97b5df480caaaa | |
parent | 3de63dee6c3eac5fec7959ececf012abe12ab3a4 (diff) | |
parent | 9d4b4ab0492c4fb2f42ee1e6940c8f85c473f2ad (diff) | |
download | px4-firmware-4a977af8708e3f59b38793dfe501b75842efa08d.tar.gz px4-firmware-4a977af8708e3f59b38793dfe501b75842efa08d.tar.bz2 px4-firmware-4a977af8708e3f59b38793dfe501b75842efa08d.zip |
Merge pull request #1 from PX4/master
From original
98 files changed, 4531 insertions, 939 deletions
diff --git a/.gitignore b/.gitignore index 611325444..0e553fa36 100644 --- a/.gitignore +++ b/.gitignore @@ -46,3 +46,5 @@ Firmware.zip unittests/build *.generated.h .vagrant +*.pretty + diff --git a/.travis.yml b/.travis.yml index a994d3471..d659bf650 100644 --- a/.travis.yml +++ b/.travis.yml @@ -37,6 +37,7 @@ env: # Email address: $PX4_EMAIL - secure: "ei3hKAw6Pk+vEkQBI5Y2Ak74BRAaXcK2UHVnVadviBHI4EVPwn1YGP6A4Y0wnLe4U7ETTl0UiijRoVxyDW0Mq896Pv0siw02amNpjSZZYu+RfN1+//MChB48OxsLDirUdHVrULhl/bOARM02h2Bg28jDE2g7IqmJwg3em3oMbjU=" - PX4_REPORT=report.txt + - BUILD_URI=https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip script: - arm-none-eabi-gcc --version @@ -58,11 +59,14 @@ after_script: # upload newest build for this branch with s3 index - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ # archive newest build by date with s3 index - - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/ + - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/ - ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ # upload top level index.html and timestamp.html - ./CI-Tools/s3cmd-put CI-Tools/index.html index.html - ./CI-Tools/s3cmd-put CI-Tools/timestamp.html timestamp.html + - echo "" + - echo "Binaries have been posted to:" + - echo https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip deploy: provider: releases diff --git a/CMakeLists.txt b/CMakeLists.txt index 0c81607b0..457a0bfa7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,6 +3,7 @@ project(px4) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") add_definitions(-D__PX4_ROS) add_definitions(-D__EXPORT=) +add_definitions(-DMAVLINK_DIALECT=common) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) @@ -11,11 +12,14 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs + geometry_msgs message_generation cmake_modules gazebo_msgs sensor_msgs mav_msgs + libmavconn + tf ) find_package(Eigen REQUIRED) @@ -74,6 +78,8 @@ add_message_files( position_setpoint_triplet.msg vehicle_local_position_setpoint.msg vehicle_global_velocity_setpoint.msg + offboard_control_mode.msg + vehicle_force_setpoint.msg ) ## Generate services in the 'srv' folder @@ -109,7 +115,7 @@ generate_messages( catkin_package( INCLUDE_DIRS src/include LIBRARIES px4 - CATKIN_DEPENDS message_runtime roscpp rospy std_msgs + CATKIN_DEPENDS message_runtime roscpp rospy std_msgs libmavconn DEPENDS system_lib ) @@ -128,6 +134,7 @@ include_directories( src/ src/lib ${EIGEN_INCLUDE_DIRS} + integrationtests ) ## generate multiplatform wrapper headers @@ -231,15 +238,42 @@ target_link_libraries(mc_mixer px4 ) -## Commander +## Commander dummy add_executable(commander src/platforms/ros/nodes/commander/commander.cpp) -add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) +add_dependencies(commander ${PROJECT_NAME}_generate_messages_cpp_cpp) target_link_libraries(commander ${catkin_LIBRARIES} px4 ) +## Mavlink dummy +add_executable(mavlink + src/platforms/ros/nodes/mavlink/mavlink.cpp) +add_dependencies(mavlink ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(mavlink + ${catkin_LIBRARIES} + px4 +) + +## Offboard Position Setpoint Demo +add_executable(demo_offboard_position_setpoints + src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp) +add_dependencies(demo_offboard_position_setpoints ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(demo_offboard_position_setpoints + ${catkin_LIBRARIES} + px4 +) + +## Offboard Attitude Setpoint Demo +add_executable(demo_offboard_attitude_setpoints + src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp) +add_dependencies(demo_offboard_attitude_setpoints ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(demo_offboard_attitude_setpoints + ${catkin_LIBRARIES} + px4 +) + ############# ## Install ## ############# @@ -287,3 +321,11 @@ install(TARGETS ${PROJECT_NAME} ## Add folders to be run by python nosetests # catkin_add_nosetests(test) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest(integrationtests/demo_tests/direct_tests.launch) + add_rostest(integrationtests/demo_tests/mavros_tests.launch) +endif() + + @@ -124,7 +124,7 @@ $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4 .PHONY: $(FIRMWARES) $(BUILD_DIR)%.build/firmware.px4: config = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@) $(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/ -$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: checksubmodules generateuorbtopicheaders +$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: generateuorbtopicheaders checksubmodules @$(ECHO) %%%% @$(ECHO) %%%% Building $(config) in $(work_dir) @$(ECHO) %%%% @@ -236,7 +236,7 @@ GENMSG_PYTHONPATH = $(PX4_BASE)Tools/genmsg/src GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src .PHONY: generateuorbtopicheaders -generateuorbtopicheaders: +generateuorbtopicheaders: checksubmodules @$(ECHO) "Generating uORB topic headers" $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH):$(PYTHONPATH) $(PYTHON) \ $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ @@ -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 @@ -13,7 +13,7 @@ This repository contains the PX4 Flight Core, with the main applications located * [Fixed wing](http://px4.io/platforms/planes/start) * [VTOL](http://px4.io/platforms/vtol/start) * Binaries (always up-to-date from master): - * [Downloads](http://px4.io/downloads) + * [Downloads](http://px4.io/firmware/downloads) * Mailing list: [Google Groups](http://groups.google.com/group/px4users) ### Users ### diff --git a/ROMFS/px4fmu_common/init.d/50001_rover b/ROMFS/px4fmu_common/init.d/50001_rover index e9df3904b..c2f94705c 100644 --- a/ROMFS/px4fmu_common/init.d/50001_rover +++ b/ROMFS/px4fmu_common/init.d/50001_rover @@ -3,8 +3,8 @@ # Generic rover # +#load some defaults e.g. PWM values sh /etc/init.d/rc.rover_defaults +#choose a mixer, for rover control we need a plain passthrough to the servos set MIXER IO_pass - -set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 2ecc104df..6517e026a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -4,9 +4,15 @@ # att & pos estimator, att & pos control. # -attitude_estimator_ekf start -#ekf_att_pos_estimator start -position_estimator_inav start +# previously (2014) the system was relying on +# INAV, which defaults to 0 now. +if param compare INAV_ENABLED 1 +then + attitude_estimator_ekf start + position_estimator_inav start +else + ekf_att_pos_estimator start +fi if mc_att_control start then diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_defaults index fc68472a6..23bf47df1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.rover_defaults @@ -2,12 +2,27 @@ set VEHICLE_TYPE rover -if [ $AUTOCNF == yes ] -then - # param set MC_ROLL_P 7.0 -fi +# This section can be enabled once tuning parameters for this particular +# rover model are known. It allows to configure default gains via the GUI +#if [ $AUTOCNF == yes ] +#then +# # param set MC_ROLL_P 7.0 +#fi +# PWM Hz - 50 Hz is the normal rate in RC cars, higher rates +# may damage analog servos. set PWM_RATE 50 -set PWM_DISARMED 1100 -set PWM_MIN 1100 -set PWM_MAX 1900 + +# PWM default value for "disarmed" mode +# this centers the steering and throttle, which means no motion +# for a rover +set PWM_DISARMED 1500 + +# PWM range +set PWM_MIN 1200 +set PWM_MAX 1800 + +# Enable servo output on pins 3 and 4 (steering and thrust) +# but also include 1+2 as they form together one output group +# and need to be set together. +set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 580043a1d..1d21d7772 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -198,8 +198,17 @@ then tone_alarm MLL32CP8MB - px4io start - px4io safety_on + if px4io start + then + # try to safe px4 io so motor outputs dont go crazy + if px4io safety_on + then + # success! no-op + else + # px4io did not respond to the safety command + px4io stop + fi + fi if px4io forceupdate 14662 ${IO_FILE} then @@ -432,7 +441,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 000000000..8d1ab6363 --- /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 5995d428e..e73a5a8af 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 000000000..13cd4aadd --- /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 95a3d4046..859c821e5 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -169,8 +169,8 @@ class uploader(object): INFO_BOARD_REV = b'\x03' # board revision INFO_FLASH_SIZE = b'\x04' # max firmware size in bytes - PROG_MULTI_MAX = 60 # protocol max is 255, must be multiple of 4 - READ_MULTI_MAX = 60 # protocol max is 255, something overflows with >= 64 + PROG_MULTI_MAX = 252 # protocol max is 255, must be multiple of 4 + READ_MULTI_MAX = 252 # protocol max is 255 NSH_INIT = bytearray(b'\x0d\x0d\x0d') NSH_REBOOT_BL = b"reboot -b\n" @@ -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") @@ -501,75 +501,87 @@ print("Loaded firmware for %x,%x, size: %d bytes, waiting for the bootloader..." print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.") # Spin waiting for a device to show up -while True: - portlist = [] - patterns = args.port.split(",") - # on unix-like platforms use glob to support wildcard ports. This allows - # the use of /dev/serial/by-id/usb-3D_Robotics on Linux, which prevents the upload from - # causing modem hangups etc - if "linux" in _platform or "darwin" in _platform: - import glob - for pattern in patterns: - portlist += glob.glob(pattern) - else: - portlist = patterns - - for port in portlist: - - #print("Trying %s" % port) - - # create an uploader attached to the port - try: - if "linux" in _platform: - # Linux, don't open Mac OS and Win ports - if not "COM" in port and not "tty.usb" in port: - up = uploader(port, args.baud) - elif "darwin" in _platform: - # OS X, don't open Windows and Linux ports - if not "COM" in port and not "ACM" in port: - up = uploader(port, args.baud) - elif "win" in _platform: - # Windows, don't open POSIX ports - if not "/" in port: - up = uploader(port, args.baud) - except Exception: - # open failed, rate-limit our attempts - time.sleep(0.05) - - # and loop to the next port - continue - - # port is open, try talking to it - try: - # identify the bootloader - up.identify() - print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) +try: + while True: + portlist = [] + patterns = args.port.split(",") + # on unix-like platforms use glob to support wildcard ports. This allows + # the use of /dev/serial/by-id/usb-3D_Robotics on Linux, which prevents the upload from + # causing modem hangups etc + if "linux" in _platform or "darwin" in _platform: + import glob + for pattern in patterns: + portlist += glob.glob(pattern) + else: + portlist = patterns + + for port in portlist: + + #print("Trying %s" % port) + + # create an uploader attached to the port + try: + if "linux" in _platform: + # Linux, don't open Mac OS and Win ports + if not "COM" in port and not "tty.usb" in port: + up = uploader(port, args.baud) + elif "darwin" in _platform: + # OS X, don't open Windows and Linux ports + if not "COM" in port and not "ACM" in port: + up = uploader(port, args.baud) + elif "win" in _platform: + # Windows, don't open POSIX ports + if not "/" in port: + up = uploader(port, args.baud) + except Exception: + # open failed, rate-limit our attempts + time.sleep(0.05) + + # and loop to the next port + continue + + # port is open, try talking to it + try: + # identify the bootloader + up.identify() + print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) - except Exception: - # most probably a timeout talking to the port, no bootloader, try to reboot the board - print("attempting reboot on %s..." % port) - print("if the board does not respond, unplug and re-plug the USB connector.") - up.send_reboot() + except Exception: + # most probably a timeout talking to the port, no bootloader, try to reboot the board + print("attempting reboot on %s..." % port) + print("if the board does not respond, unplug and re-plug the USB connector.") + up.send_reboot() - # wait for the reboot, without we might run into Serial I/O Error 5 - time.sleep(0.5) + # wait for the reboot, without we might run into Serial I/O Error 5 + time.sleep(0.5) - # always close the port - up.close() - continue + # always close the port + up.close() + continue - try: - # ok, we have a bootloader, try flashing it - up.upload(fw) + try: + # ok, we have a bootloader, try flashing it + up.upload(fw) + + except RuntimeError as ex: + # print the error + print("\nERROR: %s" % ex.args) + + except IOError as e: + up.close(); + continue - except RuntimeError as ex: + finally: + # always close the port + up.close() - # print the error - print("\nERROR: %s" % ex.args) + # we could loop here if we wanted to wait for more boards... + sys.exit(0) - finally: - # always close the port - up.close() + # Delay retries to < 20 Hz to prevent spin-lock from hogging the CPU + time.sleep(0.05) - # we could loop here if we wanted to wait for more boards... - sys.exit(0) +# CTRL+C aborts the upload/spin-lock by interrupt mechanics +except KeyboardInterrupt: + print("\n Upload aborted by user.") + sys.exit(0) diff --git a/integrationtests/demo_tests/direct_manual_input_test.py b/integrationtests/demo_tests/direct_manual_input_test.py new file mode 100755 index 000000000..6d115316b --- /dev/null +++ b/integrationtests/demo_tests/direct_manual_input_test.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 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. +# +#***************************************************************************/ + +# +# @author Andreas Antener <andreas@uaventure.com> +# +PKG = 'px4' + +import sys +import unittest +import rospy + +from px4.msg import actuator_armed +from px4.msg import vehicle_control_mode +from manual_input import ManualInput + +# +# Tests if commander reacts to manual input and sets control flags accordingly +# +class ManualInputTest(unittest.TestCase): + + # + # General callback functions used in tests + # + def actuator_armed_callback(self, data): + self.actuatorStatus = data + + def vehicle_control_mode_callback(self, data): + self.controlMode = data + + # + # Test arming + # + def test_manual_input(self): + rospy.init_node('test_node', anonymous=True) + rospy.Subscriber('px4_multicopter/actuator_armed', actuator_armed, self.actuator_armed_callback) + rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + + man = ManualInput() + + # Test arming + man.arm() + self.assertEquals(self.actuatorStatus.armed, True, "did not arm") + + # Test posctl + man.posctl() + self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") + + # Test offboard + man.offboard() + self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") + + +if __name__ == '__main__': + import rostest + rostest.rosrun(PKG, 'direct_manual_input_test', ManualInputTest) diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py new file mode 100755 index 000000000..e09550bbc --- /dev/null +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -0,0 +1,167 @@ +#!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 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. +# +#***************************************************************************/ + +# +# @author Andreas Antener <andreas@uaventure.com> +# +PKG = 'px4' + +import sys +import unittest +import rospy + +from numpy import linalg +import numpy as np + +from px4.msg import vehicle_local_position +from px4.msg import vehicle_control_mode +from px4.msg import actuator_armed +from px4.msg import position_setpoint_triplet +from px4.msg import position_setpoint +from sensor_msgs.msg import Joy +from std_msgs.msg import Header + +from manual_input import ManualInput +from flight_path_assertion import FlightPathAssertion + +# +# Tests flying a path in offboard control by directly sending setpoints +# to the position controller (position_setpoint_triplet). +# +# For the test to be successful it needs to stay on the predefined path +# and reach all setpoints in a certain time. +# +class DirectOffboardPosctlTest(unittest.TestCase): + + def setUp(self): + rospy.init_node('test_node', anonymous=True) + rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) + self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) + self.rate = rospy.Rate(10) # 10hz + + def tearDown(self): + if (self.fpa): + self.fpa.stop() + + # + # General callback functions used in tests + # + def position_callback(self, data): + self.hasPos = True + self.localPosition = data + + def vehicle_control_mode_callback(self, data): + self.controlMode = data + + + # + # Helper methods + # + def is_at_position(self, x, y, z, offset): + rospy.logdebug("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) + desired = np.array((x, y, z)) + pos = np.array((self.localPosition.x, self.localPosition.y, self.localPosition.z)) + return linalg.norm(desired - pos) < offset + + def reach_position(self, x, y, z, timeout): + # set a position setpoint + pos = position_setpoint() + pos.valid = True + pos.x = x + pos.y = y + pos.z = z + pos.position_valid = True + stp = position_setpoint_triplet() + stp.current = pos + self.pubSpt.publish(stp) + + # does it reach the position in X seconds? + count = 0 + while(count < timeout): + if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + break + count = count + 1 + self.rate.sleep() + + self.assertTrue(count < timeout, "took too long to get to position") + + # + # Test offboard position control + # + def test_posctl(self): + manIn = ManualInput() + + # arm and go into offboard + manIn.arm() + manIn.offboard() + self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set") + self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") + self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") + + # prepare flight path + positions = ( + (0,0,0), + (2,2,-2), + (2,-2,-2), + (-2,-2,-2), + (2,2,-2)) + + # flight path assertion + self.fpa = FlightPathAssertion(positions, 1, 0) + self.fpa.start() + + for i in range(0, len(positions)): + self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) + self.assertFalse(self.fpa.failed, "breached flight path tunnel (%d)" % i) + + # does it hold the position for Y seconds? + positionHeld = True + count = 0 + timeout = 50 + while(count < timeout): + if(not self.is_at_position(2, 2, -2, 0.5)): + positionHeld = False + break + count = count + 1 + self.rate.sleep() + + self.assertTrue(count == timeout, "position could not be held") + self.fpa.stop() + + +if __name__ == '__main__': + import rostest + rostest.rosrun(PKG, 'direct_offboard_posctl_test', DirectOffboardPosctlTest) + #unittest.main() diff --git a/integrationtests/demo_tests/direct_tests.launch b/integrationtests/demo_tests/direct_tests.launch new file mode 100644 index 000000000..1a7d843fd --- /dev/null +++ b/integrationtests/demo_tests/direct_tests.launch @@ -0,0 +1,18 @@ +<launch> + <arg name="headless" default="true"/> + <arg name="gui" default="false"/> + <arg name="enable_logging" default="false"/> + <arg name="enable_ground_truth" default="false"/> + <arg name="log_file" default="iris"/> + + <include file="$(find px4)/launch/gazebo_iris_empty_world.launch"> + <arg name="headless" value="$(arg headless)"/> + <arg name="gui" value="$(arg gui)"/> + <arg name="enable_logging" value="$(arg enable_logging)" /> + <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" /> + <arg name="log_file" value="$(arg log_file)"/> + </include> + + <test test-name="direct_manual_input_test" pkg="px4" type="direct_manual_input_test.py" /> + <test test-name="direct_offboard_posctl_test" pkg="px4" type="direct_offboard_posctl_test.py" /> +</launch> diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/demo_tests/flight_path_assertion.py new file mode 100644 index 000000000..485de8c41 --- /dev/null +++ b/integrationtests/demo_tests/flight_path_assertion.py @@ -0,0 +1,171 @@ +#!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 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. +# +#***************************************************************************/ + +# +# @author Andreas Antener <andreas@uaventure.com> +# +import sys +import rospy +import threading + +from px4.msg import vehicle_local_position +from gazebo_msgs.srv import SpawnModel +from gazebo_msgs.srv import SetModelState +from gazebo_msgs.srv import DeleteModel +from geometry_msgs.msg import Pose +from geometry_msgs.msg import Twist + +from numpy import linalg +import numpy as np +import math + +# +# Helper to test if vehicle stays on expected flight path. +# +class FlightPathAssertion(threading.Thread): + + # + # Arguments + # - positions: tuple of tuples in the form (x, y, z, heading) + # + # TODO: yaw validation + # TODO: fail main test thread + # + def __init__(self, positions, tunnelRadius = 1, yawOffset = 0.2): + threading.Thread.__init__(self) + rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) + self.spawnModel = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) + self.setModelState = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) + self.deleteModel = rospy.ServiceProxy('gazebo/delete_model', DeleteModel) + + self.positions = positions + self.tunnelRadius = tunnelRadius + self.yawOffset = yawOffset + self.hasPos = False + self.shouldStop = False + self.center = positions[0] + self.endOfSegment = False + self.failed = False + + def position_callback(self, data): + self.hasPos = True + self.localPosition = data + + def spawn_indicator(self): + self.deleteModel("indicator") + xml = "<?xml version='1.0'?><sdf version='1.4'><model name='indicator'><static>true</static><link name='link'><visual name='visual'><transparency>0.7</transparency><geometry><sphere><radius>%f</radius></sphere></geometry><material><ambient>1 0 0 0.5</ambient><diffuse>1 0 0 0.5</diffuse></material></visual></link></model></sdf>" % self.tunnelRadius + self.spawnModel("indicator", xml, "", Pose(), "") + + def position_indicator(self): + state = SetModelState() + state.model_name = "indicator" + pose = Pose() + pose.position.x = self.center[0] + pose.position.y = (-1) * self.center[1] + pose.position.z = (-1) * self.center[2] + state.pose = pose + state.twist = Twist() + state.reference_frame = "" + self.setModelState(state) + + def distance_to_line(self, a, b, pos): + v = b - a + w = pos - a + + c1 = np.dot(w, v) + if c1 <= 0: # before a + self.center = a + return linalg.norm(pos - a) + + c2 = np.dot(v, v) + if c2 <= c1: # after b + self.center = b + self.endOfSegment = True + return linalg.norm(pos - b) + + x = c1 / c2 + l = a + x * v + self.center = l + return linalg.norm(pos - l) + + def stop(self): + self.shouldStop = True + + def run(self): + rate = rospy.Rate(10) # 10hz + self.spawn_indicator() + + current = 0 + + while not self.shouldStop: + if (self.hasPos): + # calculate distance to line segment between first two points + # if distances > tunnelRadius + # exit with error + # advance current pos if not on the line anymore or distance to next point < tunnelRadius + # exit if current pos is now the last position + + self.position_indicator() + + pos = np.array((self.localPosition.x, + self.localPosition.y, + self.localPosition.z)) + aPos = np.array((self.positions[current][0], + self.positions[current][1], + self.positions[current][2])) + bPos = np.array((self.positions[current + 1][0], + self.positions[current + 1][1], + self.positions[current + 1][2])) + + dist = self.distance_to_line(aPos, bPos, pos) + bDist = linalg.norm(pos - bPos) + + rospy.logdebug("distance to line: %f, distance to end: %f" % (dist, bDist)) + + if (dist > self.tunnelRadius): + msg = "left tunnel at position (%f, %f, %f)" % (self.localPosition.x, self.localPosition.y, self.localPosition.z) + rospy.logerr(msg) + self.failed = True + break + + if (self.endOfSegment or bDist < self.tunnelRadius): + rospy.loginfo("next segment") + self.endOfSegment = False + current = current + 1 + + if (current == len(self.positions) - 1): + rospy.loginfo("no more positions") + break + + rate.sleep() diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py new file mode 100755 index 000000000..9b2471a00 --- /dev/null +++ b/integrationtests/demo_tests/manual_input.py @@ -0,0 +1,175 @@ +#!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 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. +# +#***************************************************************************/ + +# +# @author Andreas Antener <andreas@uaventure.com> +# +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 + +# +# 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. 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): + rate = rospy.Rate(10) # 10hz + + att = CommandAttitudeThrust() + att.header = Header() + + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 3 + pos.return_switch = 3 + pos.posctl_switch = 3 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 3 + + count = 0 + while not rospy.is_shutdown() and count < 5: + rospy.loginfo("zeroing") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + # Fake input to iris commander + self.pubAtt.publish(att) + rate.sleep() + count = count + 1 + + pos.r = 1 + count = 0 + while not rospy.is_shutdown() and count < 5: + rospy.loginfo("arming") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + rate.sleep() + count = count + 1 + + def posctl(self): + rate = rospy.Rate(10) # 10hz + + # triggers posctl + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 2 + pos.return_switch = 3 + pos.posctl_switch = 1 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 3 + + count = 0 + while not rospy.is_shutdown() and count < 5: + rospy.loginfo("triggering posctl") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + rate.sleep() + count = count + 1 + + + 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 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 3 + pos.return_switch = 3 + pos.posctl_switch = 3 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 1 + + count = 0 + while not rospy.is_shutdown() and count < 5: + rospy.loginfo("triggering offboard") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + rate.sleep() + count = count + 1 + diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py new file mode 100755 index 000000000..a52f7ffc1 --- /dev/null +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 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. +# +#***************************************************************************/ + +# +# @author Andreas Antener <andreas@uaventure.com> +# +PKG = 'px4' + +import sys +import unittest +import rospy +import math + +from numpy import linalg +import numpy as np + +from px4.msg import vehicle_control_mode +from std_msgs.msg import Header +from std_msgs.msg import Float64 +from geometry_msgs.msg import PoseStamped, Quaternion +from tf.transformations import quaternion_from_euler +from mavros.srv import CommandBool + +from manual_input import ManualInput + +# +# Tests flying a path in offboard control by sending position setpoints +# over MAVROS. +# +# For the test to be successful it needs to reach all setpoints in a certain time. +# FIXME: add flight path assertion (needs transformation from ROS frame to NED) +# +class MavrosOffboardAttctlTest(unittest.TestCase): + + def setUp(self): + rospy.init_node('test_node', anonymous=True) + rospy.wait_for_service('mavros/cmd/arming', 30) + rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback) + self.pubAtt = rospy.Publisher('mavros/setpoint/attitude', PoseStamped, queue_size=10) + self.pubThr = rospy.Publisher('mavros/setpoint/att_throttle', Float64, queue_size=10) + self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) + self.rate = rospy.Rate(10) # 10hz + self.rateSec = rospy.Rate(1) + self.hasPos = False + self.controlMode = vehicle_control_mode() + + # + # General callback functions used in tests + # + def position_callback(self, data): + self.hasPos = True + self.localPosition = data + + def vehicle_control_mode_callback(self, data): + self.controlMode = data + + + # + # Helper methods + # + def arm(self): + return self.cmdArm(value=True) + + # + # Test offboard position control + # + def test_attctl(self): + # 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() + self.rateSec.sleep() + self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds") + + # set some attitude and thrust + att = PoseStamped() + att.header = Header() + att.header.frame_id = "base_footprint" + att.header.stamp = rospy.Time.now() + quaternion = quaternion_from_euler(0.15, 0.15, 0) + att.pose.orientation = Quaternion(*quaternion) + + throttle = Float64() + throttle.data = 0.6 + + # does it cross expected boundaries in X seconds? + count = 0 + timeout = 120 + while(count < timeout): + # update timestamp for each published SP + att.header.stamp = rospy.Time.now() + self.pubAtt.publish(att) + self.pubThr.publish(throttle) + + if (self.localPosition.pose.position.x > 5 + and self.localPosition.pose.position.z > 5 + and self.localPosition.pose.position.y < -5): + break + count = count + 1 + self.rate.sleep() + + self.assertTrue(count < timeout, "took too long to cross boundaries") + + +if __name__ == '__main__': + import rostest + rostest.rosrun(PKG, 'mavros_offboard_attctl_test', MavrosOffboardAttctlTest) + #unittest.main() diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py new file mode 100755 index 000000000..a3739ae5c --- /dev/null +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -0,0 +1,174 @@ +#!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 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. +# +#***************************************************************************/ + +# +# @author Andreas Antener <andreas@uaventure.com> +# +PKG = 'px4' + +import sys +import unittest +import rospy +import math + +from numpy import linalg +import numpy as np + +from px4.msg import vehicle_control_mode +from std_msgs.msg import Header +from geometry_msgs.msg import PoseStamped, Quaternion +from tf.transformations import quaternion_from_euler +from mavros.srv import CommandBool + +from manual_input import ManualInput + +# +# Tests flying a path in offboard control by sending position setpoints +# over MAVROS. +# +# For the test to be successful it needs to reach all setpoints in a certain time. +# FIXME: add flight path assertion (needs transformation from ROS frame to NED) +# +class MavrosOffboardPosctlTest(unittest.TestCase): + + def setUp(self): + rospy.init_node('test_node', anonymous=True) + rospy.wait_for_service('mavros/cmd/arming', 30) + rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback) + self.pubSpt = rospy.Publisher('mavros/setpoint/local_position', PoseStamped, queue_size=10) + self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) + self.rate = rospy.Rate(10) # 10hz + self.rateSec = rospy.Rate(1) + self.hasPos = False + self.controlMode = vehicle_control_mode() + + # + # General callback functions used in tests + # + def position_callback(self, data): + self.hasPos = True + self.localPosition = data + + def vehicle_control_mode_callback(self, data): + self.controlMode = data + + + # + # Helper methods + # + def is_at_position(self, x, y, z, offset): + if(not self.hasPos): + return False + + rospy.logdebug("current position %f, %f, %f" % (self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z)) + desired = np.array((x, y, z)) + pos = np.array((self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z)) + return linalg.norm(desired - pos) < offset + + def reach_position(self, x, y, z, timeout): + # set a position setpoint + pos = PoseStamped() + pos.header = Header() + pos.header.frame_id = "base_footprint" + pos.pose.position.x = x + pos.pose.position.y = y + pos.pose.position.z = z + + # For demo purposes we will lock yaw/heading to north. + yaw_degrees = 0 # North + yaw = math.radians(yaw_degrees) + quaternion = quaternion_from_euler(0, 0, yaw) + pos.pose.orientation = Quaternion(*quaternion) + + # does it reach the position in X seconds? + count = 0 + while(count < timeout): + # update timestamp for each published SP + pos.header.stamp = rospy.Time.now() + self.pubSpt.publish(pos) + + if(self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5)): + break + count = count + 1 + self.rate.sleep() + + self.assertTrue(count < timeout, "took too long to get to position") + + def arm(self): + return self.cmdArm(value=True) + + # + # Test offboard position control + # + def test_posctl(self): + # FIXME: this must go ASAP when arming is implemented + manIn = ManualInput() + manIn.arm() + manIn.offboard_posctl() + + self.assertTrue(self.arm(), "Could not arm") + self.rateSec.sleep() + self.rateSec.sleep() + self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds") + + # prepare flight path + positions = ( + (0,0,0), + (2,2,2), + (2,-2,2), + (-2,-2,2), + (2,2,2)) + + for i in range(0, len(positions)): + self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) + + # does it hold the position for Y seconds? + positionHeld = True + count = 0 + timeout = 50 + while(count < timeout): + if(not self.is_at_position(2, 2, 2, 0.5)): + positionHeld = False + break + count = count + 1 + self.rate.sleep() + + self.assertTrue(count == timeout, "position could not be held") + + +if __name__ == '__main__': + import rostest + rostest.rosrun(PKG, 'mavros_offboard_posctl_test', MavrosOffboardPosctlTest) + #unittest.main() diff --git a/integrationtests/demo_tests/mavros_tests.launch b/integrationtests/demo_tests/mavros_tests.launch new file mode 100644 index 000000000..4651f0dc9 --- /dev/null +++ b/integrationtests/demo_tests/mavros_tests.launch @@ -0,0 +1,19 @@ +<launch> + <arg name="headless" default="true"/> + <arg name="gui" default="false"/> + <arg name="enable_logging" default="false"/> + <arg name="enable_ground_truth" default="false"/> + <arg name="log_file" default="iris"/> + + <include file="$(find px4)/launch/gazebo_iris_empty_world.launch"> + <arg name="headless" value="$(arg headless)"/> + <arg name="gui" value="$(arg gui)"/> + <arg name="enable_logging" value="$(arg enable_logging)" /> + <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" /> + <arg name="log_file" value="$(arg log_file)"/> + </include> + <include file="$(find px4)/launch/mavros_sitl.launch" /> + + <test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" /> + <test test-name="mavros_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" /> +</launch> diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch index 9d19fe5f9..22bfb0c79 100644 --- a/launch/gazebo_ardrone_empty_world.launch +++ b/launch/gazebo_ardrone_empty_world.launch @@ -3,7 +3,7 @@ <arg name="headless" default="false"/> <arg name="gui" default="true"/> <arg name="enable_logging" default="false"/> - <arg name="enable_ground_truth" default="true"/> + <arg name="enable_ground_truth" default="false"/> <arg name="log_file" default="ardrone"/> <include file="$(find rotors_gazebo)/launch/ardrone_empty_world_with_joy.launch"> diff --git a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch new file mode 100644 index 000000000..717244abf --- /dev/null +++ b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch @@ -0,0 +1,9 @@ + +<launch> + +<include file="$(find px4)/launch/gazebo_ardrone_empty_world.launch" /> +<include file="$(find px4)/launch/mavros_sitl.launch" /> + +<node pkg="px4" name="demo_offboard_attitude_setpoints" type="demo_offboard_attitude_setpoints"/> + +</launch> diff --git a/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch new file mode 100644 index 000000000..9ff7f7d1f --- /dev/null +++ b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch @@ -0,0 +1,9 @@ + +<launch> + +<include file="$(find px4)/launch/gazebo_ardrone_empty_world.launch" /> +<include file="$(find px4)/launch/mavros_sitl.launch" /> + +<node pkg="px4" name="demo_offboard_position_setpoints" type="demo_offboard_position_setpoints"/> + +</launch> diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch new file mode 100644 index 000000000..40010a273 --- /dev/null +++ b/launch/mavros_sitl.launch @@ -0,0 +1,21 @@ +<launch> + <!-- vim: set ft=xml noet : --> + <!-- example launch script for PX4 based FCU's --> + + <arg name="fcu_url" default="udp://localhost:14560@localhost:14565" /> + <arg name="gcs_url" default="" /> + <arg name="tgt_system" default="1" /> + <arg name="tgt_component" default="50" /> + + <param name="mavros/setpoint/attitude/listen_twist" type="bool" value="false" /> + + <include file="$(find mavros)/launch/node.launch"> + <arg name="blacklist_yaml" value="$(find mavros)/launch/px4_blacklist.yaml" /> + <arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" /> + + <arg name="fcu_url" value="$(arg fcu_url)" /> + <arg name="gcs_url" value="$(arg gcs_url)" /> + <arg name="tgt_system" value="$(arg tgt_system)" /> + <arg name="tgt_component" value="$(arg tgt_component)" /> + </include> +</launch> diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 95400bd82..bc0e37771 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -9,6 +9,7 @@ <node pkg="px4" name="position_estimator" type="position_estimator"/> <node pkg="px4" name="mc_att_control" type="mc_att_control"/> <node pkg="px4" name="mc_pos_control" type="mc_pos_control"/> + <node pkg="px4" name="mavlink" type="mavlink"/> <!-- <node pkg="rosbag" type="record" name="record" output="screen" args="-a -O px4_multicopter"/> --> </group> diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 0347492eb..86925f9ff 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -43,6 +43,7 @@ MODULES += drivers/frsky_telemetry MODULES += modules/sensors MODULES += drivers/mkblctrl MODULES += drivers/px4flow +MODULES += drivers/oreoled # # System commands diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 -Subproject 68a88824741ac26838ec0c8c38d67b51dc84b79 +Subproject 7ae438b86ea983e95af5f092e45a5d0f9d093c7 diff --git a/msg/offboard_control_mode.msg b/msg/offboard_control_mode.msg new file mode 100644 index 000000000..e2d06963b --- /dev/null +++ b/msg/offboard_control_mode.msg @@ -0,0 +1,9 @@ +# Off-board control mode +uint64 timestamp + +bool ignore_thrust +bool ignore_attitude +bool ignore_bodyrate +bool ignore_position +bool ignore_velocity +bool ignore_acceleration_force diff --git a/msg/pwm_input.msg b/msg/pwm_input.msg new file mode 100644 index 000000000..beb82021d --- /dev/null +++ b/msg/pwm_input.msg @@ -0,0 +1,5 @@ + +uint64 timestamp # Microseconds since system boot +uint64 error_count +uint32 pulse_width # Pulse width, timer counts +uint32 period # Period, timer counts diff --git a/msg/vehicle_force_setpoint.msg b/msg/vehicle_force_setpoint.msg new file mode 100644 index 000000000..9e2322005 --- /dev/null +++ b/msg/vehicle_force_setpoint.msg @@ -0,0 +1,8 @@ +# Definition of force (NED) setpoint uORB topic. Typically this can be used +# by a position control app together with an attitude control app. + + +float32 x # in N NED +float32 y # in N NED +float32 z # in N NED +float32 yaw # right-hand rotation around downward axis (rad, equivalent to Tait-Bryan yaw) diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig index 29dff64aa..08ff4a988 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 86ed041ff..cd410051c 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 6a1aec22b..4ccc5dacb 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/package.xml b/package.xml index 666200390..2da49096f 100644 --- a/package.xml +++ b/package.xml @@ -44,10 +44,15 @@ <build_depend>rospy</build_depend> <build_depend>std_msgs</build_depend> <build_depend>eigen</build_depend> + <build_depend>libmavconn</build_depend> + <build_depend>tf</build_depend> + <build_depend>rostest</build_depend> <run_depend>roscpp</run_depend> <run_depend>rospy</run_depend> <run_depend>std_msgs</run_depend> <run_depend>eigen</run_depend> + <run_depend>libmavconn</run_depend> + <run_depend>tf</run_depend> <!-- The export tag contains other, unspecified, tags --> diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 604ce35c5..a958fc60d 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -452,10 +452,7 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_ uint8_t pec = get_PEC(reg, true, buff, bufflen + 1); if (pec != buff[bufflen + 1]) { - // debug - warnx("CurrPEC:%x vs MyPec:%x", (int)buff[bufflen + 1], (int)pec); return 0; - } // copy data diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 2fd8bc724..273af1023 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -207,6 +207,11 @@ __BEGIN_DECLS #define HRT_TIMER 8 /* use timer8 for the HRT */ #define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ +#define PWMIN_TIMER 4 +#define PWMIN_TIMER_CHANNEL 2 +#define GPIO_PWM_IN GPIO_TIM4_CH2IN_2 + /**************************************************************************************************** * Public Types ****************************************************************************************************/ diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index bacafe1dc..be9604b6e 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -159,4 +159,6 @@ #define GPIO_SENSOR_RAIL_RESET GPIOC(13) +#define GPIO_PERIPHERAL_RAIL_RESET GPIOC(14) + #endif /* _DRV_GPIO_H */ diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h new file mode 100644 index 000000000..0dcb10a7b --- /dev/null +++ b/src/drivers/drv_oreoled.h @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 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. + * + ****************************************************************************/ + +/** + * @file drv_oreoled.h + * + * Oreo led device API + */ + +#pragma once + +#include <stdint.h> +#include <sys/ioctl.h> + +/* oreoled device path */ +#define OREOLED0_DEVICE_PATH "/dev/oreoled0" + +/* + * ioctl() definitions + */ + +#define _OREOLEDIOCBASE (0x2d00) +#define _OREOLEDIOC(_n) (_IOC(_OREOLEDIOCBASE, _n)) + +/** set constant RGB values */ +#define OREOLED_SET_RGB _OREOLEDIOC(1) + +/** run macro */ +#define OREOLED_RUN_MACRO _OREOLEDIOC(2) + +/** send bytes */ +#define OREOLED_SEND_BYTES _OREOLEDIOC(3) + +/* Oreo LED driver supports up to 4 leds */ +#define OREOLED_NUM_LEDS 4 + +/* instance of 0xff means apply to all instances */ +#define OREOLED_ALL_INSTANCES 0xff + +/* maximum command length that can be sent to LEDs */ +#define OREOLED_CMD_LENGTH_MAX 24 + +/* enum passed to OREOLED_SET_MODE ioctl() + * defined by hardware */ +enum oreoled_pattern { + OREOLED_PATTERN_OFF = 0, + OREOLED_PATTERN_SINE = 1, + OREOLED_PATTERN_SOLID = 2, + OREOLED_PATTERN_SIREN = 3, + OREOLED_PATTERN_STROBE = 4, + OREOLED_PATTERN_FADEIN = 5, + OREOLED_PATTERN_FADEOUT = 6, + OREOLED_PATTERN_PARAMUPDATE = 7, + OREOLED_PATTERN_ENUM_COUNT +}; + +/* enum passed to OREOLED_SET_MODE ioctl() + * defined by hardware */ +enum oreoled_param { + OREOLED_PARAM_BIAS_RED = 0, + OREOLED_PARAM_BIAS_GREEN = 1, + OREOLED_PARAM_BIAS_BLUE = 2, + OREOLED_PARAM_AMPLITUDE_RED = 3, + OREOLED_PARAM_AMPLITUDE_GREEN = 4, + OREOLED_PARAM_AMPLITUDE_BLUE = 5, + OREOLED_PARAM_PERIOD = 6, + OREOLED_PARAM_REPEAT = 7, + OREOLED_PARAM_PHASEOFFSET = 8, + OREOLED_PARAM_MACRO = 9, + OREOLED_PARAM_ENUM_COUNT +}; + +/* enum of available macros + * defined by hardware */ +enum oreoled_macro { + OREOLED_PARAM_MACRO_RESET = 0, + OREOLED_PARAM_MACRO_COLOUR_CYCLE = 1, + OREOLED_PARAM_MACRO_BREATH = 2, + OREOLED_PARAM_MACRO_STROBE = 3, + OREOLED_PARAM_MACRO_FADEIN = 4, + OREOLED_PARAM_MACRO_FADEOUT = 5, + OREOLED_PARAM_MACRO_RED = 6, + OREOLED_PARAM_MACRO_GREEN = 7, + OREOLED_PARAM_MACRO_BLUE = 8, + OREOLED_PARAM_MACRO_YELLOW = 9, + OREOLED_PARAM_MACRO_WHITE = 10, + OREOLED_PARAM_MACRO_ENUM_COUNT +}; + +/* + structure passed to OREOLED_SET_RGB ioctl() + Note that the driver scales the brightness to 0 to 255, regardless + of the hardware scaling + */ +typedef struct { + uint8_t instance; + oreoled_pattern pattern; + uint8_t red; + uint8_t green; + uint8_t blue; +} oreoled_rgbset_t; + +/* + structure passed to OREOLED_RUN_MACRO ioctl() + */ +typedef struct { + uint8_t instance; + oreoled_macro macro; +} oreoled_macrorun_t; + +/* + structure passed to send_bytes method (only used for testing) + */ +typedef struct { + uint8_t led_num; + uint8_t num_bytes; + uint8_t buff[OREOLED_CMD_LENGTH_MAX]; +} oreoled_cmd_t; + diff --git a/src/drivers/drv_pwm_input.h b/src/drivers/drv_pwm_input.h new file mode 100644 index 000000000..778d9fcf5 --- /dev/null +++ b/src/drivers/drv_pwm_input.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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. + * + ****************************************************************************/ + +#pragma once + +#include <sys/types.h> +#include <stdbool.h> + +#include <time.h> +#include <queue.h> + +#define PWMIN0_DEVICE_PATH "/dev/pwmin0" + +__BEGIN_DECLS + +/* + * Initialise the timer + */ +__EXPORT extern int pwm_input_main(int argc, char * argv[]); + +__END_DECLS diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index e197059db..f42c968d3 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -45,7 +45,7 @@ * (rework, add ubx7+ compatibility) * * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf - * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_%28UBX-13003221%29.pdf + * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf */ #include <assert.h> diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 4aa05a980..b48ea8577 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1881,6 +1881,7 @@ MPU6000 *g_dev_int; // on internal bus MPU6000 *g_dev_ext; // on external bus void start(bool, enum Rotation); +void stop(bool); void test(bool); void reset(bool); void info(bool); @@ -1946,6 +1947,20 @@ fail: errx(1, "driver start failed"); } +void +stop(bool external_bus) +{ + MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + if (*g_dev_ptr != nullptr) { + delete *g_dev_ptr; + *g_dev_ptr = nullptr; + } else { + /* warn, but not an error */ + warnx("already stopped."); + } + exit(0); +} + /** * Perform some basic functional tests on the driver; * make sure we can collect data from the sensor in polled @@ -2111,7 +2126,7 @@ factorytest(bool external_bus) void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'factorytest', 'testerror'"); + warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -2147,38 +2162,50 @@ mpu6000_main(int argc, char *argv[]) * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { mpu6000::start(external_bus, rotation); + } + + if (!strcmp(verb, "stop")) { + mpu6000::stop(external_bus); + } /* * Test the driver/device. */ - if (!strcmp(verb, "test")) + if (!strcmp(verb, "test")) { mpu6000::test(external_bus); + } /* * Reset the driver. */ - if (!strcmp(verb, "reset")) + if (!strcmp(verb, "reset")) { mpu6000::reset(external_bus); + } /* * Print driver information. */ - if (!strcmp(verb, "info")) + if (!strcmp(verb, "info")) { mpu6000::info(external_bus); + } /* * Print register information. */ - if (!strcmp(verb, "regdump")) + if (!strcmp(verb, "regdump")) { mpu6000::regdump(external_bus); + } - if (!strcmp(verb, "factorytest")) + if (!strcmp(verb, "factorytest")) { mpu6000::factorytest(external_bus); + } - if (!strcmp(verb, "testerror")) + if (!strcmp(verb, "testerror")) { mpu6000::testerror(external_bus); + } - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'regdump', 'factorytest' or 'testerror'"); + mpu6000::usage(); + exit(1); } diff --git a/src/drivers/oreoled/module.mk b/src/drivers/oreoled/module.mk new file mode 100644 index 000000000..96afdd5fd --- /dev/null +++ b/src/drivers/oreoled/module.mk @@ -0,0 +1,8 @@ +# +# Oreo LED driver +# + +MODULE_COMMAND = oreoled +SRCS = oreoled.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp new file mode 100644 index 000000000..b44c4b720 --- /dev/null +++ b/src/drivers/oreoled/oreoled.cpp @@ -0,0 +1,700 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Randy Mackay <rmackay9@yahoo.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file oreoled.cpp + * + * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. + * + */ + +#include <nuttx/config.h> + +#include <drivers/device/i2c.h> +#include <drivers/drv_hrt.h> + +#include <sys/types.h> +#include <stdint.h> +#include <string.h> +#include <stdlib.h> +#include <stdbool.h> +#include <fcntl.h> +#include <unistd.h> +#include <stdio.h> +#include <ctype.h> + +#include <nuttx/arch.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +#include <board_config.h> + +#include <drivers/drv_oreoled.h> +#include <drivers/device/ringbuffer.h> + +#define OREOLED_NUM_LEDS 4 ///< maximum number of LEDs the oreo led driver can support +#define OREOLED_BASE_I2C_ADDR 0x68 ///< base i2c address (7-bit) +#define OREOLED_TIMEOUT_MS 10000000U ///< timeout looking for battery 10seconds after startup +#define OREOLED_GENERALCALL_US 4000000U ///< general call sent every 4 seconds +#define OREOLED_GENERALCALL_CMD 0x00 ///< general call command sent at regular intervals + +#define OREOLED_STARTUP_INTERVAL_US (1000000U / 10U) ///< time in microseconds, measure at 10hz +#define OREOLED_UPDATE_INTERVAL_US (1000000U / 50U) ///< time in microseconds, measure at 10hz + +#define OREOLED_CMD_QUEUE_SIZE 10 ///< up to 10 messages can be queued up to send to the LEDs + +class OREOLED : public device::I2C +{ +public: + OREOLED(int bus, int i2c_addr); + virtual ~OREOLED(); + + virtual int init(); + virtual int probe(); + virtual int info(); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /* send general call on I2C bus to syncronise all LEDs */ + int send_general_call(); + + /* send cmd to an LEDs (used for testing only) */ + int send_cmd(oreoled_cmd_t sb); + +private: + + /** + * Start periodic updates to the LEDs + */ + void start(); + + /** + * Stop periodic updates to the LEDs + */ + void stop(); + + /** + * static function that is called by worker queue + */ + static void cycle_trampoline(void *arg); + + /** + * update the colours displayed by the LEDs + */ + void cycle(); + + /* internal variables */ + work_s _work; ///< work queue for scheduling reads + bool _healthy[OREOLED_NUM_LEDS]; ///< health of each LED + uint8_t _num_healthy; ///< number of healthy LEDs + RingBuffer *_cmd_queue; ///< buffer of commands to send to LEDs + uint64_t _last_gencall; + uint64_t _start_time; ///< system time we first attempt to communicate with battery +}; + +/* for now, we only support one OREOLED */ +namespace +{ +OREOLED *g_oreoled = nullptr; +} + +void oreoled_usage(); + +extern "C" __EXPORT int oreoled_main(int argc, char *argv[]); + +/* constructor */ +OREOLED::OREOLED(int bus, int i2c_addr) : + I2C("oreoled", OREOLED0_DEVICE_PATH, bus, i2c_addr, 100000), + _work{}, + _num_healthy(0), + _cmd_queue(nullptr), + _last_gencall(0) +{ + /* initialise to unhealthy */ + memset(_healthy, 0, sizeof(_healthy)); + + /* capture startup time */ + _start_time = hrt_absolute_time(); +} + +/* destructor */ +OREOLED::~OREOLED() +{ + /* make sure we are truly inactive */ + stop(); + + /* clear command queue */ + if (_cmd_queue != nullptr) { + delete _cmd_queue; + } +} + +int +OREOLED::init() +{ + int ret; + + /* initialise I2C bus */ + ret = I2C::init(); + + if (ret != OK) { + return ret; + } + + /* allocate command queue */ + _cmd_queue = new RingBuffer(OREOLED_CMD_QUEUE_SIZE, sizeof(oreoled_cmd_t)); + + if (_cmd_queue == nullptr) { + return ENOTTY; + + } else { + /* start work queue */ + start(); + } + + return OK; +} + +int +OREOLED::probe() +{ + /* always return true */ + return OK; +} + +int +OREOLED::info() +{ + /* print health info on each LED */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (!_healthy[i]) { + log("oreo %u: BAD", (int)i); + + } else { + log("oreo %u: OK", (int)i); + } + } + + return OK; +} + +void +OREOLED::start() +{ + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, 1); +} + +void +OREOLED::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +OREOLED::cycle_trampoline(void *arg) +{ + OREOLED *dev = (OREOLED *)arg; + + /* check global oreoled and cycle */ + if (g_oreoled != nullptr) { + dev->cycle(); + } +} + +void +OREOLED::cycle() +{ + /* check time since startup */ + uint64_t now = hrt_absolute_time(); + bool startup_timeout = (now - _start_time > OREOLED_TIMEOUT_MS); + + /* if not leds found during start-up period, exit without rescheduling */ + if (startup_timeout && _num_healthy == 0) { + warnx("did not find oreoled"); + return; + } + + /* during startup period keep searching for unhealthy LEDs */ + if (!startup_timeout && _num_healthy < OREOLED_NUM_LEDS) { + /* prepare command to turn off LED*/ + uint8_t msg[] = {OREOLED_PATTERN_OFF}; + + /* attempt to contact each unhealthy LED */ + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + if (!_healthy[i]) { + /* set I2C address */ + set_address(OREOLED_BASE_I2C_ADDR + i); + + /* send I2C command and record health*/ + if (transfer(msg, sizeof(msg), nullptr, 0) == OK) { + _healthy[i] = true; + _num_healthy++; + warnx("oreoled %d ok", (unsigned)i); + } + } + } + + /* schedule another attempt in 0.1 sec */ + work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, + USEC2TICK(OREOLED_STARTUP_INTERVAL_US)); + return; + } + + /* get next command from queue */ + oreoled_cmd_t next_cmd; + + while (_cmd_queue->get(&next_cmd, sizeof(oreoled_cmd_t))) { + /* send valid messages to healthy LEDs */ + if ((next_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[next_cmd.led_num] + && (next_cmd.num_bytes <= OREOLED_CMD_LENGTH_MAX)) { + /* set I2C address */ + set_address(OREOLED_BASE_I2C_ADDR + next_cmd.led_num); + /* send I2C command */ + transfer(next_cmd.buff, next_cmd.num_bytes, nullptr, 0); + } + } + + /* send general call every 4 seconds*/ + if ((now - _last_gencall) > OREOLED_GENERALCALL_US) { + send_general_call(); + } + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, + USEC2TICK(OREOLED_UPDATE_INTERVAL_US)); +} + +int +OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = -ENODEV; + oreoled_cmd_t new_cmd; + + switch (cmd) { + case OREOLED_SET_RGB: + /* set the specified color */ + new_cmd.led_num = ((oreoled_rgbset_t *) arg)->instance; + new_cmd.buff[0] = ((oreoled_rgbset_t *) arg)->pattern; + new_cmd.buff[1] = OREOLED_PARAM_BIAS_RED; + new_cmd.buff[2] = ((oreoled_rgbset_t *) arg)->red; + new_cmd.buff[3] = OREOLED_PARAM_BIAS_GREEN; + new_cmd.buff[4] = ((oreoled_rgbset_t *) arg)->green; + new_cmd.buff[5] = OREOLED_PARAM_BIAS_BLUE; + new_cmd.buff[6] = ((oreoled_rgbset_t *) arg)->blue; + new_cmd.num_bytes = 7; + + /* special handling for request to set all instances rgb values */ + if (new_cmd.led_num == OREOLED_ALL_INSTANCES) { + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + /* add command to queue for all healthy leds */ + if (_healthy[i]) { + new_cmd.led_num = i; + _cmd_queue->force(&new_cmd); + ret = OK; + } + } + + } else if (new_cmd.led_num < OREOLED_NUM_LEDS) { + /* request to set individual instance's rgb value */ + if (_healthy[new_cmd.led_num]) { + _cmd_queue->force(&new_cmd); + ret = OK; + } + } + + return ret; + + case OREOLED_RUN_MACRO: + /* run a macro */ + new_cmd.led_num = ((oreoled_macrorun_t *) arg)->instance; + new_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; + new_cmd.buff[1] = OREOLED_PARAM_MACRO; + new_cmd.buff[2] = ((oreoled_macrorun_t *) arg)->macro; + new_cmd.num_bytes = 3; + + /* special handling for request to set all instances */ + if (new_cmd.led_num == OREOLED_ALL_INSTANCES) { + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + /* add command to queue for all healthy leds */ + if (_healthy[i]) { + new_cmd.led_num = i; + _cmd_queue->force(&new_cmd); + ret = OK; + } + } + + } else if (new_cmd.led_num < OREOLED_NUM_LEDS) { + /* request to set individual instance's rgb value */ + if (_healthy[new_cmd.led_num]) { + _cmd_queue->force(&new_cmd); + ret = OK; + } + } + + return ret; + + case OREOLED_SEND_BYTES: + /* send bytes */ + new_cmd = *((oreoled_cmd_t *) arg); + + /* special handling for request to set all instances */ + if (new_cmd.led_num == OREOLED_ALL_INSTANCES) { + for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { + /* add command to queue for all healthy leds */ + if (_healthy[i]) { + new_cmd.led_num = i; + _cmd_queue->force(&new_cmd); + ret = OK; + } + } + + } else if (new_cmd.led_num < OREOLED_NUM_LEDS) { + /* request to set individual instance's rgb value */ + if (_healthy[new_cmd.led_num]) { + _cmd_queue->force(&new_cmd); + ret = OK; + } + } + + return ret; + + default: + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filp, cmd, arg); + break; + } + + return ret; +} + +/* send general call on I2C bus to syncronise all LEDs */ +int +OREOLED::send_general_call() +{ + int ret = -ENODEV; + + /* set I2C address to zero */ + set_address(0); + + /* prepare command : 0x01 = general hardware call, 0x00 = I2C address of master (but we don't act as a slave so set to zero)*/ + uint8_t msg[] = {0x01, 0x00}; + + /* send I2C command */ + if (transfer(msg, sizeof(msg), nullptr, 0) == OK) { + ret = OK; + } + + /* record time */ + _last_gencall = hrt_absolute_time(); + + return ret; +} + +/* send a cmd to an LEDs (used for testing only) */ +int +OREOLED::send_cmd(oreoled_cmd_t new_cmd) +{ + int ret = -ENODEV; + + /* sanity check led number, health and cmd length */ + if ((new_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[new_cmd.led_num] && (new_cmd.num_bytes < OREOLED_CMD_LENGTH_MAX)) { + /* set I2C address */ + set_address(OREOLED_BASE_I2C_ADDR + new_cmd.led_num); + + /* add to queue */ + _cmd_queue->force(&new_cmd); + ret = OK; + } + + return ret; +} + +void +oreoled_usage() +{ + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50' 'macro 4' 'gencall' 'bytes <lednum> 7 9 6'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); + warnx(" -a addr (0x%x)", OREOLED_BASE_I2C_ADDR); +} + +int +oreoled_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int i2c_addr = OREOLED_BASE_I2C_ADDR; /* 7bit */ + + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + i2c_addr = (int)strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = (int)strtol(optarg, NULL, 0); + break; + + default: + oreoled_usage(); + exit(0); + } + } + + if (optind >= argc) { + oreoled_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + int ret; + + /* start driver */ + if (!strcmp(verb, "start")) { + if (g_oreoled != nullptr) { + errx(1, "already started"); + } + + /* by default use LED bus */ + if (i2cdevice == -1) { + i2cdevice = PX4_I2C_BUS_LED; + } + + /* instantiate driver */ + g_oreoled = new OREOLED(i2cdevice, i2c_addr); + + /* check if object was created */ + if (g_oreoled == nullptr) { + errx(1, "failed to allocated memory for driver"); + } + + /* check object was created successfully */ + if (g_oreoled->init() != OK) { + delete g_oreoled; + g_oreoled = nullptr; + errx(1, "failed to start driver"); + } + + exit(0); + } + + /* need the driver past this point */ + if (g_oreoled == nullptr) { + warnx("not started"); + oreoled_usage(); + exit(1); + } + + if (!strcmp(verb, "test")) { + int fd = open(OREOLED0_DEVICE_PATH, O_RDWR); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + /* structure to hold desired colour */ + oreoled_rgbset_t rgb_set_red = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, 0xFF, 0x0, 0x0}; + oreoled_rgbset_t rgb_set_blue = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, 0x0, 0x0, 0xFF}; + oreoled_rgbset_t rgb_set_off = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_OFF, 0x0, 0x0, 0x0}; + + /* flash red and blue for 3 seconds */ + for (uint8_t i = 0; i < 30; i++) { + /* red */ + if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_red)) != OK) { + errx(1, " failed to update rgb"); + } + + /* sleep for 0.05 seconds */ + usleep(50000); + + /* blue */ + if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_blue)) != OK) { + errx(1, " failed to update rgb"); + } + + /* sleep for 0.05 seconds */ + usleep(50000); + } + + /* turn off LED */ + if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_off)) != OK) { + errx(1, " failed to turn off led"); + } + + close(fd); + exit(ret); + } + + /* display driver status */ + if (!strcmp(verb, "info")) { + g_oreoled->info(); + exit(0); + } + + if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + /* turn off LED */ + oreoled_rgbset_t rgb_set_off = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_OFF, 0x0, 0x0, 0x0}; + ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_off); + + close(fd); + + /* delete the oreoled object if stop was requested, in addition to turning off the LED. */ + if (!strcmp(verb, "stop")) { + OREOLED *tmp_oreoled = g_oreoled; + g_oreoled = nullptr; + delete tmp_oreoled; + exit(0); + } + + exit(ret); + } + + /* send rgb request to all LEDS */ + if (!strcmp(verb, "rgb")) { + if (argc < 5) { + errx(1, "Usage: oreoled rgb <red> <green> <blue>"); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + uint8_t red = (uint8_t)strtol(argv[2], NULL, 0); + uint8_t green = (uint8_t)strtol(argv[3], NULL, 0); + uint8_t blue = (uint8_t)strtol(argv[4], NULL, 0); + oreoled_rgbset_t rgb_set = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, red, green, blue}; + + if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set)) != OK) { + errx(1, "failed to set rgb"); + } + + close(fd); + exit(ret); + } + + /* send macro request to all LEDS */ + if (!strcmp(verb, "macro")) { + if (argc < 3) { + errx(1, "Usage: oreoled macro <macro_num>"); + } + + int fd = open(OREOLED0_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " OREOLED0_DEVICE_PATH); + } + + uint8_t macro = (uint8_t)strtol(argv[2], NULL, 0); + + /* sanity check macro number */ + if (macro > OREOLED_PARAM_MACRO_ENUM_COUNT) { + errx(1, "invalid macro number %d", (int)macro); + exit(ret); + } + + oreoled_macrorun_t macro_run = {OREOLED_ALL_INSTANCES, (enum oreoled_macro)macro}; + + if ((ret = ioctl(fd, OREOLED_RUN_MACRO, (unsigned long)¯o_run)) != OK) { + errx(1, "failed to run macro"); + } + + close(fd); + exit(ret); + } + + /* send general hardware call to all LEDS */ + if (!strcmp(verb, "gencall")) { + ret = g_oreoled->send_general_call(); + warnx("sent general call"); + exit(ret); + } + + /* send a string of bytes to an LED using send_bytes function */ + if (!strcmp(verb, "bytes")) { + if (argc < 3) { + errx(1, "Usage: oreoled bytes <led_num> <byte1> <byte2> <byte3> ..."); + } + + /* structure to be sent */ + oreoled_cmd_t sendb; + + /* maximum of 20 bytes can be sent */ + if (argc > 20 + 3) { + errx(1, "Max of 20 bytes can be sent"); + } + + /* check led num */ + sendb.led_num = (uint8_t)strtol(argv[2], NULL, 0); + + if (sendb.led_num > 3) { + errx(1, "led number must be between 0 ~ 3"); + } + + /* get bytes */ + sendb.num_bytes = argc - 3; + uint8_t byte_count; + + for (byte_count = 0; byte_count < sendb.num_bytes; byte_count++) { + sendb.buff[byte_count] = (uint8_t)strtol(argv[byte_count + 3], NULL, 0); + } + + /* send bytes */ + if ((ret = g_oreoled->send_cmd(sendb)) != OK) { + errx(1, "failed to send command"); + + } else { + warnx("sent %d bytes", (int)sendb.num_bytes); + } + + exit(ret); + } + + oreoled_usage(); + exit(0); +} diff --git a/src/drivers/pwm_input/module.mk b/src/drivers/pwm_input/module.mk new file mode 100644 index 000000000..04f04d6cb --- /dev/null +++ b/src/drivers/pwm_input/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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. +# +############################################################################ + +# +# Makefile to build the PWM input driver. +# + +MODULE_COMMAND = pwm_input + +SRCS = pwm_input.cpp + diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp new file mode 100644 index 000000000..2d771266c --- /dev/null +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -0,0 +1,603 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Andrew Tridgell. 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. + * + ****************************************************************************/ + +/** + * @file pwm_input.cpp + * + * PWM input driver based on earlier driver from Evan Slatyer, + * which in turn was based on drv_hrt.c + */ + +#include <nuttx/config.h> +#include <nuttx/arch.h> +#include <nuttx/irq.h> + +#include <sys/types.h> +#include <stdbool.h> + +#include <assert.h> +#include <debug.h> +#include <time.h> +#include <queue.h> +#include <errno.h> +#include <string.h> +#include <math.h> +#include <stdio.h> +#include <stdlib.h> + +#include <board_config.h> +#include <drivers/drv_pwm_input.h> +#include <drivers/drv_hrt.h> + +#include "chip.h" +#include "up_internal.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_gpio.h" +#include "stm32_tim.h" +#include <systemlib/err.h> + +#include <uORB/uORB.h> +#include <uORB/topics/pwm_input.h> +#include <uORB/topics/subsystem_info.h> + +#include <drivers/drv_device.h> +#include <drivers/device/device.h> +#include <drivers/device/ringbuffer.h> + +#include <sys/types.h> +#include <sys/stat.h> +#include <fcntl.h> + +#if HRT_TIMER == PWMIN_TIMER +#error cannot share timer between HRT and PWMIN +#endif + +#if !defined(GPIO_PWM_IN) || !defined(PWMIN_TIMER) || !defined(PWMIN_TIMER_CHANNEL) +#error PWMIN defines are needed in board_config.h for this board +#endif + +/* PWMIN configuration */ +#if PWMIN_TIMER == 1 +# define PWMIN_TIMER_BASE STM32_TIM1_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB2ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM1EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1CC +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM1_CLKIN +#elif PWMIN_TIMER == 2 +# define PWMIN_TIMER_BASE STM32_TIM2_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM2EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM2 +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM2_CLKIN +#elif PWMIN_TIMER == 3 +# define PWMIN_TIMER_BASE STM32_TIM3_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB1ENR_TIM3EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM3 +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM3_CLKIN +#elif PWMIN_TIMER == 4 +# define PWMIN_TIMER_BASE STM32_TIM4_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB1ENR_TIM4EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM4 +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM4_CLKIN +#elif PWMIN_TIMER == 5 +# define PWMIN_TIMER_BASE STM32_TIM5_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM5EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM5 +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM5_CLKIN +#elif PWMIN_TIMER == 8 +# define PWMIN_TIMER_BASE STM32_TIM8_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB2ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM8EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM8CC +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM8_CLKIN +#elif PWMIN_TIMER == 9 +# define PWMIN_TIMER_BASE STM32_TIM9_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM9EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1BRK +# define PWMIN_TIMER_CLOCK STM32_APB1_TIM9_CLKIN +#elif PWMIN_TIMER == 10 +# define PWMIN_TIMER_BASE STM32_TIM10_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1UP +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM10_CLKIN +#elif PWMIN_TIMER == 11 +# define PWMIN_TIMER_BASE STM32_TIM11_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM11_CLKIN +#elif PWMIN_TIMER == 12 +# define PWMIN_TIMER_BASE STM32_TIM12_BASE +# define PWMIN_TIMER_POWER_REG STM32_RCC_APB1ENR +# define PWMIN_TIMER_POWER_BIT RCC_APB2ENR_TIM12EN +# define PWMIN_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM +# define PWMIN_TIMER_CLOCK STM32_APB2_TIM12_CLKIN +#else +# error PWMIN_TIMER must be a value between 1 and 12 +#endif + +/* + * HRT clock must be at least 1MHz + */ +#if PWMIN_TIMER_CLOCK <= 1000000 +# error PWMIN_TIMER_CLOCK must be greater than 1MHz +#endif + +/* + * Timer register accessors + */ +#define REG(_reg) (*(volatile uint32_t *)(PWMIN_TIMER_BASE + _reg)) + +#define rCR1 REG(STM32_GTIM_CR1_OFFSET) +#define rCR2 REG(STM32_GTIM_CR2_OFFSET) +#define rSMCR REG(STM32_GTIM_SMCR_OFFSET) +#define rDIER REG(STM32_GTIM_DIER_OFFSET) +#define rSR REG(STM32_GTIM_SR_OFFSET) +#define rEGR REG(STM32_GTIM_EGR_OFFSET) +#define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET) +#define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET) +#define rCCER REG(STM32_GTIM_CCER_OFFSET) +#define rCNT REG(STM32_GTIM_CNT_OFFSET) +#define rPSC REG(STM32_GTIM_PSC_OFFSET) +#define rARR REG(STM32_GTIM_ARR_OFFSET) +#define rCCR1 REG(STM32_GTIM_CCR1_OFFSET) +#define rCCR2 REG(STM32_GTIM_CCR2_OFFSET) +#define rCCR3 REG(STM32_GTIM_CCR3_OFFSET) +#define rCCR4 REG(STM32_GTIM_CCR4_OFFSET) +#define rDCR REG(STM32_GTIM_DCR_OFFSET) +#define rDMAR REG(STM32_GTIM_DMAR_OFFSET) + +/* + * Specific registers and bits used by HRT sub-functions + */ +#if PWMIN_TIMER_CHANNEL == 1 + #define rCCR_PWMIN_A rCCR1 /* compare register for PWMIN */ + #define DIER_PWMIN_A (GTIM_DIER_CC1IE) /* interrupt enable for PWMIN */ + #define SR_INT_PWMIN_A GTIM_SR_CC1IF /* interrupt status for PWMIN */ + #define rCCR_PWMIN_B rCCR2 /* compare register for PWMIN */ + #define SR_INT_PWMIN_B GTIM_SR_CC2IF /* interrupt status for PWMIN */ + #define CCMR1_PWMIN ((0x02 << GTIM_CCMR1_CC2S_SHIFT) | (0x01 << GTIM_CCMR1_CC1S_SHIFT)) + #define CCMR2_PWMIN 0 + #define CCER_PWMIN (GTIM_CCER_CC2P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) + #define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) + #define SMCR_PWMIN_1 (0x05 << GTIM_SMCR_TS_SHIFT) + #define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) +#elif PWMIN_TIMER_CHANNEL == 2 + #define rCCR_PWMIN_A rCCR2 /* compare register for PWMIN */ + #define DIER_PWMIN_A (GTIM_DIER_CC2IE) /* interrupt enable for PWMIN */ + #define SR_INT_PWMIN_A GTIM_SR_CC2IF /* interrupt status for PWMIN */ + #define rCCR_PWMIN_B rCCR1 /* compare register for PWMIN */ + #define DIER_PWMIN_B GTIM_DIER_CC1IE /* interrupt enable for PWMIN */ + #define SR_INT_PWMIN_B GTIM_SR_CC1IF /* interrupt status for PWMIN */ + #define CCMR1_PWMIN ((0x01 << GTIM_CCMR1_CC2S_SHIFT) | (0x02 << GTIM_CCMR1_CC1S_SHIFT)) + #define CCMR2_PWMIN 0 + #define CCER_PWMIN (GTIM_CCER_CC1P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) + #define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) + #define SMCR_PWMIN_1 (0x06 << GTIM_SMCR_TS_SHIFT) + #define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) +#else + #error PWMIN_TIMER_CHANNEL must be either 1 and 2. +#endif + + +class PWMIN : device::CDev +{ +public: + PWMIN(); + virtual ~PWMIN(); + + virtual int init(); + virtual int open(struct file *filp); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + void _publish(uint16_t status, uint32_t period, uint32_t pulse_width); + void _print_info(void); + +private: + uint32_t error_count; + uint32_t pulses_captured; + uint32_t last_period; + uint32_t last_width; + RingBuffer *reports; + bool timer_started; + + void timer_init(void); +}; + +static int pwmin_tim_isr(int irq, void *context); +static void pwmin_start(void); +static void pwmin_info(void); +static void pwmin_test(void); +static void pwmin_reset(void); + +static PWMIN *g_dev; + +PWMIN::PWMIN() : + CDev("pwmin", PWMIN0_DEVICE_PATH), + error_count(0), + pulses_captured(0), + last_period(0), + last_width(0), + reports(nullptr), + timer_started(false) +{ +} + +PWMIN::~PWMIN() +{ + if (reports != nullptr) + delete reports; +} + +/* + initialise the driver. This doesn't actually start the timer (that + is done on open). We don't start the timer to allow for this driver + to be started in init scripts when the user may be using the input + pin as PWM output + */ +int +PWMIN::init() +{ + // we just register the device in /dev, and only actually + // activate the timer when requested to when the device is opened + CDev::init(); + + reports = new RingBuffer(2, sizeof(struct pwm_input_s)); + if (reports == nullptr) { + return -ENOMEM; + } + + return OK; +} + +/* + * Initialise the timer we are going to use. + */ +void PWMIN::timer_init(void) +{ + // run with interrupts disabled in case the timer is already + // setup. We don't want it firing while we are doing the setup + irqstate_t flags = irqsave(); + stm32_configgpio(GPIO_PWM_IN); + + /* claim our interrupt vector */ + irq_attach(PWMIN_TIMER_VECTOR, pwmin_tim_isr); + + /* Clear no bits, set timer enable bit.*/ + modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT); + + /* disable and configure the timer */ + rCR1 = 0; + rCR2 = 0; + rSMCR = 0; + rDIER = DIER_PWMIN_A; + rCCER = 0; /* unlock CCMR* registers */ + rCCMR1 = CCMR1_PWMIN; + rCCMR2 = CCMR2_PWMIN; + rSMCR = SMCR_PWMIN_1; /* Set up mode */ + rSMCR = SMCR_PWMIN_2; /* Enable slave mode controller */ + rCCER = CCER_PWMIN; + rDCR = 0; + + // for simplicity scale by the clock in MHz. This gives us + // readings in microseconds which is typically what is needed + // for a PWM input driver + uint32_t prescaler = PWMIN_TIMER_CLOCK/1000000UL; + + /* + * define the clock speed. We want the highest possible clock + * speed that avoids overflows. + */ + rPSC = prescaler - 1; + + /* run the full span of the counter. All timers can handle + * uint16 */ + rARR = UINT16_MAX; + + /* generate an update event; reloads the counter, all registers */ + rEGR = GTIM_EGR_UG; + + /* enable the timer */ + rCR1 = GTIM_CR1_CEN; + + /* enable interrupts */ + up_enable_irq(PWMIN_TIMER_VECTOR); + + irqrestore(flags); + + timer_started = true; +} + +/* + hook for open of the driver. We start the timer at this point, then + leave it running + */ +int +PWMIN::open(struct file *filp) +{ + if (g_dev == nullptr) { + return -EIO; + } + int ret = CDev::open(filp); + if (ret == OK && !timer_started) { + g_dev->timer_init(); + } + return ret; +} + + +/* + handle ioctl requests + */ +int +PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 500)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return reports->size(); + + case SENSORIOCRESET: + /* user has asked for the timer to be reset. This may + be needed if the pin was used for a different + purpose (such as PWM output) + */ + timer_init(); + return OK; + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + + +/* + read some samples from the device + */ +ssize_t +PWMIN::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct pwm_input_s); + struct pwm_input_s *buf = reinterpret_cast<struct pwm_input_s *>(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + while (count--) { + if (reports->get(buf)) { + ret += sizeof(struct pwm_input_s); + buf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; +} + +/* + publish some data from the ISR in the ring buffer + */ +void PWMIN::_publish(uint16_t status, uint32_t period, uint32_t pulse_width) +{ + /* if we missed an edge, we have to give up */ + if (status & SR_OVF_PWMIN) { + error_count++; + return; + } + + struct pwm_input_s pwmin_report; + pwmin_report.timestamp = hrt_absolute_time(); + pwmin_report.error_count = error_count; + pwmin_report.period = period; + pwmin_report.pulse_width = pulse_width; + + reports->force(&pwmin_report); + + last_period = period; + last_width = pulse_width; + pulses_captured++; +} + +/* + print information on the last captured + */ +void PWMIN::_print_info(void) +{ + if (!timer_started) { + printf("timer not started - try the 'test' command\n"); + } else { + printf("count=%u period=%u width=%u\n", + (unsigned)pulses_captured, + (unsigned)last_period, + (unsigned)last_width); + } +} + + +/* + Handle the interupt, gathering pulse data + */ +static int pwmin_tim_isr(int irq, void *context) +{ + uint16_t status = rSR; + uint32_t period = rCCR_PWMIN_A; + uint32_t pulse_width = rCCR_PWMIN_B; + + /* ack the interrupts we just read */ + rSR = 0; + + if (g_dev != nullptr) { + g_dev->_publish(status, period, pulse_width); + } + + return OK; +} + +/* + start the driver + */ +static void pwmin_start(void) +{ + if (g_dev != nullptr) { + printf("driver already started\n"); + exit(1); + } + g_dev = new PWMIN(); + if (g_dev == nullptr) { + errx(1, "driver allocation failed"); + } + if (g_dev->init() != OK) { + errx(1, "driver init failed"); + } + exit(0); +} + +/* + test the driver + */ +static void pwmin_test(void) +{ + int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); + if (fd == -1) { + errx(1, "Failed to open device"); + } + uint64_t start_time = hrt_absolute_time(); + + printf("Showing samples for 5 seconds\n"); + + while (hrt_absolute_time() < start_time+5U*1000UL*1000UL) { + struct pwm_input_s buf; + if (::read(fd, &buf, sizeof(buf)) == sizeof(buf)) { + printf("period=%u width=%u error_count=%u\n", + (unsigned)buf.period, + (unsigned)buf.pulse_width, + (unsigned)buf.error_count); + } + } + close(fd); + exit(0); +} + +/* + reset the timer + */ +static void pwmin_reset(void) +{ + int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); + if (fd == -1) { + errx(1, "Failed to open device"); + } + if (ioctl(fd, SENSORIOCRESET, 0) != OK) { + errx(1, "reset failed"); + } + close(fd); + exit(0); +} + +/* + show some information on the driver + */ +static void pwmin_info(void) +{ + if (g_dev == nullptr) { + printf("driver not started\n"); + exit(1); + } + g_dev->_print_info(); + exit(0); +} + + +/* + driver entry point + */ +int pwm_input_main(int argc, char * argv[]) +{ + const char *verb = argv[1]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + pwmin_start(); + } + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) { + pwmin_info(); + } + + /* + * print test results + */ + if (!strcmp(verb, "test")) { + pwmin_test(); + } + + /* + * reset the timer + */ + if (!strcmp(verb, "reset")) { + pwmin_reset(); + } + + errx(1, "unrecognized command, try 'start', 'info', 'reset' or 'test'"); + return 0; +} diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 3a9246bf2..7b09a4676 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -182,6 +182,7 @@ private: void gpio_reset(void); void sensor_reset(int ms); + void peripheral_reset(int ms); void gpio_set_function(uint32_t gpios, int function); void gpio_write(uint32_t gpios, int function); uint32_t gpio_read(void); @@ -1376,6 +1377,29 @@ PX4FMU::sensor_reset(int ms) #endif } +void +PX4FMU::peripheral_reset(int ms) +{ +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + + if (ms < 1) { + ms = 10; + } + + /* set the peripheral rails off */ + stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); + stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 1); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0); +#endif +} void PX4FMU::gpio_reset(void) @@ -1488,6 +1512,10 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) sensor_reset(arg); break; + case GPIO_PERIPHERAL_RAIL_RESET: + peripheral_reset(arg); + break; + case GPIO_SET_OUTPUT: case GPIO_SET_INPUT: case GPIO_SET_ALT_1: @@ -1529,6 +1557,7 @@ enum PortMode { PORT_GPIO_AND_SERIAL, PORT_PWM_AND_SERIAL, PORT_PWM_AND_GPIO, + PORT_PWM4, }; PortMode g_port_mode; @@ -1564,6 +1593,13 @@ fmu_new_mode(PortMode new_mode) #endif break; +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + case PORT_PWM4: + /* select 4-pin PWM mode */ + servo_mode = PX4FMU::MODE_4PWM; + break; +#endif + /* mixed modes supported on v1 board only */ #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) @@ -1667,6 +1703,24 @@ sensor_reset(int ms) } void +peripheral_reset(int ms) +{ + int fd; + + fd = open(PX4FMU_DEVICE_PATH, O_RDWR); + + if (fd < 0) { + errx(1, "open fail"); + } + + if (ioctl(fd, GPIO_PERIPHERAL_RAIL_RESET, ms) < 0) { + warnx("peripheral rail reset failed"); + } + + close(fd); +} + +void test(void) { int fd; @@ -1836,6 +1890,10 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(verb, "mode_pwm")) { new_mode = PORT_FULL_PWM; +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + } else if (!strcmp(verb, "mode_pwm4")) { + new_mode = PORT_PWM4; +#endif #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) } else if (!strcmp(verb, "mode_serial")) { @@ -1883,6 +1941,19 @@ fmu_main(int argc, char *argv[]) exit(0); } + if (!strcmp(verb, "peripheral_reset")) { + if (argc > 2) { + int reset_time = strtol(argv[2], 0, 0); + peripheral_reset(reset_time); + + } else { + peripheral_reset(0); + warnx("resettet default time"); + } + + exit(0); + } + if (!strcmp(verb, "i2c")) { if (argc > 3) { int bus = strtol(argv[2], 0, 0); diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index bf6e3365d..d28966fca 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -116,6 +116,35 @@ public: } /** + * 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 */ void from_euler(float roll, float pitch, float yaw) { @@ -135,12 +164,38 @@ public: data[3] = static_cast<float>(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2); } - void from_dcm(const Matrix<3, 3> &m) { - // avoiding singularities by not using division equations - data[0] = 0.5f * sqrtf(1.0f + m.data[0][0] + m.data[1][1] + m.data[2][2]); - data[1] = 0.5f * sqrtf(1.0f + m.data[0][0] - m.data[1][1] - m.data[2][2]); - data[2] = 0.5f * sqrtf(1.0f - m.data[0][0] + m.data[1][1] - m.data[2][2]); - data[3] = 0.5f * sqrtf(1.0f - m.data[0][0] - m.data[1][1] + m.data[2][2]); + /** + * set quaternion to rotation by DCM + * Reference: Shoemake, Quaternions, http://www.cs.ucr.edu/~vbz/resources/quatut.pdf + */ + void from_dcm(const Matrix<3, 3> &dcm) { + float tr = dcm.data[0][0] + dcm.data[1][1] + dcm.data[2][2]; + if (tr > 0.0f) { + float s = sqrtf(tr + 1.0f); + data[0] = s * 0.5f; + s = 0.5f / s; + data[1] = (dcm.data[2][1] - dcm.data[1][2]) * s; + data[2] = (dcm.data[0][2] - dcm.data[2][0]) * s; + data[3] = (dcm.data[1][0] - dcm.data[0][1]) * s; + } else { + /* Find maximum diagonal element in dcm + * store index in dcm_i */ + int dcm_i = 0; + for (int i = 1; i < 3; i++) { + if (dcm.data[i][i] > dcm.data[dcm_i][dcm_i]) { + dcm_i = i; + } + } + int dcm_j = (dcm_i + 1) % 3; + int dcm_k = (dcm_i + 2) % 3; + float s = sqrtf((dcm.data[dcm_i][dcm_i] - dcm.data[dcm_j][dcm_j] - + dcm.data[dcm_k][dcm_k]) + 1.0f); + data[dcm_i + 1] = s * 0.5f; + s = 0.5f / s; + data[dcm_j + 1] = (dcm.data[dcm_i][dcm_j] + dcm.data[dcm_j][dcm_i]) * s; + data[dcm_k + 1] = (dcm.data[dcm_k][dcm_i] + dcm.data[dcm_i][dcm_k]) * s; + data[0] = (dcm.data[dcm_k][dcm_j] - dcm.data[dcm_j][dcm_k]) * s; + } } /** diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 87065b56f..d70e05000 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -358,7 +358,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se if (orient < 0) { mavlink_and_console_log_info(mavlink_fd, "invalid motion, hold still..."); - sleep(3); + sleep(2); continue; } @@ -372,6 +372,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se mavlink_and_console_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]); sleep(1); read_accelerometer_avg(subs, accel_ref, orient, samples_num); + mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]); + usleep(100000); mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[0][orient][0], (double)accel_ref[0][orient][1], diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 242d8a486..f832f761e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -65,7 +65,7 @@ #include <uORB/topics/sensor_combined.h> #include <uORB/topics/battery_status.h> #include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/offboard_control_setpoint.h> +#include <uORB/topics/offboard_control_mode.h> #include <uORB/topics/home_position.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_local_position.h> @@ -180,7 +180,7 @@ static struct vehicle_status_s status; static struct actuator_armed_s armed; static struct safety_s safety; static struct vehicle_control_mode_s control_mode; -static struct offboard_control_setpoint_s sp_offboard; +static struct offboard_control_mode_s offboard_control_mode; /* tasks waiting for low prio thread */ typedef enum { @@ -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, 2900); + pthread_attr_setstacksize(&commander_low_prio_attr, 2400); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); @@ -1016,8 +1016,8 @@ int commander_thread_main(int argc, char *argv[]) memset(&sp_man, 0, sizeof(sp_man)); /* Subscribe to offboard control data */ - int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - memset(&sp_offboard, 0, sizeof(sp_offboard)); + int offboard_control_mode_sub = orb_subscribe(ORB_ID(offboard_control_mode)); + memset(&offboard_control_mode, 0, sizeof(offboard_control_mode)); /* Subscribe to telemetry status topics */ int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; @@ -1227,14 +1227,14 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); } - orb_check(sp_offboard_sub, &updated); + orb_check(offboard_control_mode_sub, &updated); if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + orb_copy(ORB_ID(offboard_control_mode), offboard_control_mode_sub, &offboard_control_mode); } - if (sp_offboard.timestamp != 0 && - sp_offboard.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) { + if (offboard_control_mode.timestamp != 0 && + offboard_control_mode.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) { if (status.offboard_control_signal_lost) { status.offboard_control_signal_lost = false; status_changed = true; @@ -2051,7 +2051,7 @@ int commander_thread_main(int argc, char *argv[]) led_deinit(); buzzer_deinit(); close(sp_man_sub); - close(sp_offboard_sub); + close(offboard_control_mode_sub); close(local_position_sub); close(global_position_sub); close(gps_sub); @@ -2426,56 +2426,30 @@ set_control_mode() control_mode.flag_control_auto_enabled = false; control_mode.flag_control_offboard_enabled = true; - switch (sp_offboard.mode) { - case OFFBOARD_CONTROL_MODE_DIRECT_RATES: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; + /* + * The control flags depend on what is ignored according to the offboard control mode topic + * Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position) + */ + control_mode.flag_control_rates_enabled = !offboard_control_mode.ignore_bodyrate || + !offboard_control_mode.ignore_attitude || + !offboard_control_mode.ignore_position || + !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_acceleration_force; - case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; + control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude || + !offboard_control_mode.ignore_position || + !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_acceleration_force; - case OFFBOARD_CONTROL_MODE_DIRECT_FORCE: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_force_enabled = true; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; + control_mode.flag_control_velocity_enabled = !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_position; - case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED: - case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: - case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: - case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; - //XXX: the flags could depend on sp_offboard.ignore - break; + control_mode.flag_control_climb_rate_enabled = !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_position; - default: - control_mode.flag_control_rates_enabled = false; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - } + control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position; + + control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position; break; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index a94b478c4..e0786db79 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -137,6 +137,9 @@ int do_mag_calibration(int mavlink_fd) } if (calibrated_ok) { + + mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); + usleep(100000); mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); /* auto-save to EEPROM */ @@ -155,7 +158,7 @@ int do_mag_calibration(int mavlink_fd) int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) { /* 45 seconds */ - uint64_t calibration_interval = 45 * 1000 * 1000; + uint64_t calibration_interval = 25 * 1000 * 1000; /* maximum 500 values */ const unsigned int calibration_maxcount = 240; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 40aa4a2f0..0154f235f 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 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 @@ -35,7 +35,7 @@ * @file state_machine_helper.cpp * State machine helper functions implementations * - * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Thomas Gubler <thomas@px4.io> * @author Julian Oes <julian@oes.ch> */ diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 1c79cb61d..903158129 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -457,9 +457,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() rep.states[i] = ekf_report.states[i]; } - for (size_t i = 0; i < rep.n_states; i++) { - rep.states[i] = ekf_report.states[i]; - } + if (_estimator_status_pub > 0) { orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); @@ -774,14 +772,14 @@ void AttitudePositionEstimatorEKF::publishAttitude() _att.pitch = euler(1); _att.yaw = euler(2); - _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; - _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; - _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; + _att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMU; + _att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMU; + _att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMU; // gyro offsets - _att.rate_offsets[0] = _ekf->states[10]; - _att.rate_offsets[1] = _ekf->states[11]; - _att.rate_offsets[2] = _ekf->states[12]; + _att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMU; + _att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMU; + _att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMU; /* lazily publish the attitude only once available */ if (_att_pub > 0) { @@ -1056,7 +1054,7 @@ void AttitudePositionEstimatorEKF::print_status() // 4-6: Velocity - m/sec (North, East, Down) // 7-9: Position - m (North, East, Down) // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13: Accelerometer offset + // 13: Delta Velocity Bias - m/s (Z) // 14-15: Wind Vector - m/sec (North,East) // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 6e49bd1d1..5daeae477 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -520,7 +520,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll() orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); if (vcontrol_mode_updated) { - + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); } } @@ -795,10 +795,10 @@ FixedwingAttitudeControl::task_main() /* Simple handling of failsafe: deploy parachute if failsafe is on */ if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[7] = 1.0f; -// warnx("_actuators_airframe.control[1] = 1.0f;"); + //warnx("_actuators_airframe.control[1] = 1.0f;"); } else { _actuators_airframe.control[7] = 0.0f; -// warnx("_actuators_airframe.control[1] = -1.0f;"); + //warnx("_actuators_airframe.control[1] = -1.0f;"); } /* decide if in stabilized or full manual control */ @@ -1077,20 +1077,25 @@ FixedwingAttitudeControl::task_main() _actuators_airframe.timestamp = hrt_absolute_time(); _actuators_airframe.timestamp_sample = _att.timestamp; - /* publish the actuator controls */ - if (_actuators_0_pub > 0) { - orb_publish(_actuators_id, _actuators_0_pub, &_actuators); - } else if (_actuators_id) { - _actuators_0_pub= orb_advertise(_actuators_id, &_actuators); - } + /* Only publish if any of the proper modes are enabled */ + if(_vcontrol_mode.flag_control_rates_enabled || + _vcontrol_mode.flag_control_attitude_enabled) + { + /* publish the actuator controls */ + if (_actuators_0_pub > 0) { + orb_publish(_actuators_id, _actuators_0_pub, &_actuators); + } else if (_actuators_id) { + _actuators_0_pub= orb_advertise(_actuators_id, &_actuators); + } - if (_actuators_2_pub > 0) { - /* publish the actuator controls*/ - orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe); + if (_actuators_2_pub > 0) { + /* publish the actuator controls*/ + orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe); - } else { - /* advertise and publish */ - _actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe); + } else { + /* advertise and publish */ + _actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe); + } } } diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index a61108c4c..6ab3ddbfc 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -302,10 +302,10 @@ PARAM_DEFINE_INT32(FW_YCO_METHOD, 0); * * @unit m/s * @min 0.0 - * @max 30.0 + * @max 40 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f); +PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f); /** * Trim Airspeed @@ -314,10 +314,10 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f); * * @unit m/s * @min 0.0 - * @max 30.0 + * @max 40 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f); +PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f); /** * Maximum Airspeed @@ -327,10 +327,10 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f); * * @unit m/s * @min 0.0 - * @max 30.0 + * @max 40 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f); +PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f); /** * Roll Setpoint Offset diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 8e5bcf7ba..e26954d1a 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -66,6 +66,8 @@ void FixedwingLandDetector::initialize() // Subscribe to local position and airspeed data _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + + updateParameterCache(true); } void FixedwingLandDetector::updateSubscriptions() diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 1e43e7ad5..011567e57 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -139,7 +139,7 @@ static int land_detector_start(const char *mode) _landDetectorTaskID = task_spawn_cmd("land_detector", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1200, + 1000, (main_t)&land_detector_deamon_thread, nullptr); @@ -179,8 +179,7 @@ int land_detector_main(int argc, char *argv[]) { if (argc < 1) { - warnx("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'"); - exit(0); + goto exiterr; } if (argc >= 2 && !strcmp(argv[1], "start")) { @@ -209,6 +208,8 @@ int land_detector_main(int argc, char *argv[]) } } - warn("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'"); +exiterr: + warnx("usage: land_detector {start|stop|status} [mode]"); + warnx("mode can either be 'fixedwing' or 'multicopter'"); return 1; } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 024dfd6fb..f8e819ce7 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, - 2800, + 2400, (main_t)&Mavlink::start_helper, (char * const *)argv); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 9711d8fc3..7d6b60e22 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -51,7 +51,6 @@ #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/home_position.h> #include <uORB/topics/vehicle_status.h> -#include <uORB/topics/offboard_control_setpoint.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_vicon_position.h> diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 90c3cb47f..0c34fc58a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -108,7 +108,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _cmd_pub(-1), _flow_pub(-1), _range_pub(-1), - _offboard_control_sp_pub(-1), + _offboard_control_mode_pub(-1), + _actuator_controls_pub(-1), _global_vel_sp_pub(-1), _att_sp_pub(-1), _rates_sp_pub(-1), @@ -170,6 +171,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_set_attitude_target(msg); break; + case MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET: + handle_message_set_actuator_control_target(msg); + break; + case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: handle_message_vision_position_estimate(msg); break; @@ -517,8 +522,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t mavlink_set_position_target_local_ned_t set_position_target_local_ned; mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned); - struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints + struct offboard_control_mode_s offboard_control_mode; + memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == set_position_target_local_ned.target_system || @@ -527,64 +532,22 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t set_position_target_local_ned.target_component == 0)) { /* convert mavlink type (local, NED) to uORB offboard control struct */ - switch (set_position_target_local_ned.coordinate_frame) { - case MAV_FRAME_LOCAL_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED; - break; - case MAV_FRAME_LOCAL_OFFSET_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED; - break; - case MAV_FRAME_BODY_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED; - break; - case MAV_FRAME_BODY_OFFSET_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED; - break; - default: - /* invalid setpoint, avoid publishing */ - return; - } - offboard_control_sp.position[0] = set_position_target_local_ned.x; - offboard_control_sp.position[1] = set_position_target_local_ned.y; - offboard_control_sp.position[2] = set_position_target_local_ned.z; - offboard_control_sp.velocity[0] = set_position_target_local_ned.vx; - offboard_control_sp.velocity[1] = set_position_target_local_ned.vy; - offboard_control_sp.velocity[2] = set_position_target_local_ned.vz; - offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx; - offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy; - offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz; - offboard_control_sp.yaw = set_position_target_local_ned.yaw; - offboard_control_sp.yaw_rate = set_position_target_local_ned.yaw_rate; - offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); - - /* If we are in force control mode, for now set offboard mode to force control */ - if (offboard_control_sp.isForceSetpoint) { - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE; - } - - /* set ignore flags */ - for (int i = 0; i < 9; i++) { - offboard_control_sp.ignore &= ~(1 << i); - offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); - } - - offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW); - if (set_position_target_local_ned.type_mask & (1 << 10)) { - offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAW); - } - - offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); - if (set_position_target_local_ned.type_mask & (1 << 11)) { - offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE); - } + offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7); + offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38); + offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0); + bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); + /* yaw ignore flag mapps to ignore_attitude */ + offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400); + /* yawrate ignore flag mapps to ignore_bodyrate */ + offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800); - offboard_control_sp.timestamp = hrt_absolute_time(); + offboard_control_mode.timestamp = hrt_absolute_time(); - if (_offboard_control_sp_pub < 0) { - _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + if (_offboard_control_mode_pub < 0) { + _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); } else { - orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); } /* If we are in offboard control mode and offboard control loop through is enabled @@ -596,15 +559,14 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } if (_control_mode.flag_control_offboard_enabled) { - if (offboard_control_sp.isForceSetpoint && - offboard_control_sp_ignore_position_all(offboard_control_sp) && - offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { + if (is_force_sp && offboard_control_mode.ignore_position && + offboard_control_mode.ignore_velocity) { /* The offboard setpoint is a force setpoint only, directly writing to the force * setpoint topic and not publishing the setpoint triplet topic */ struct vehicle_force_setpoint_s force_sp; - force_sp.x = offboard_control_sp.acceleration[0]; - force_sp.y = offboard_control_sp.acceleration[1]; - force_sp.z = offboard_control_sp.acceleration[2]; + force_sp.x = set_position_target_local_ned.afx; + force_sp.y = set_position_target_local_ned.afy; + force_sp.z = set_position_target_local_ned.afz; //XXX: yaw if (_force_sp_pub < 0) { _force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp); @@ -619,62 +581,53 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t pos_sp_triplet.current.valid = true; pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; //XXX support others - /* set the local pos values if the setpoint type is 'local pos' and none - * of the local pos fields is set to 'ignore' */ - if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_position_some(offboard_control_sp)) { - pos_sp_triplet.current.position_valid = true; - pos_sp_triplet.current.x = offboard_control_sp.position[0]; - pos_sp_triplet.current.y = offboard_control_sp.position[1]; - pos_sp_triplet.current.z = offboard_control_sp.position[2]; + /* set the local pos values */ + if (!offboard_control_mode.ignore_position) { + pos_sp_triplet.current.position_valid = true; + pos_sp_triplet.current.x = set_position_target_local_ned.x; + pos_sp_triplet.current.y = set_position_target_local_ned.y; + pos_sp_triplet.current.z = set_position_target_local_ned.z; } else { pos_sp_triplet.current.position_valid = false; } - /* set the local vel values if the setpoint type is 'local pos' and none - * of the local vel fields is set to 'ignore' */ - if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_velocity_some(offboard_control_sp)) { + /* set the local vel values */ + if (!offboard_control_mode.ignore_velocity) { pos_sp_triplet.current.velocity_valid = true; - pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; - pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; - pos_sp_triplet.current.vz = offboard_control_sp.velocity[2]; + pos_sp_triplet.current.vx = set_position_target_local_ned.vx; + pos_sp_triplet.current.vy = set_position_target_local_ned.vy; + pos_sp_triplet.current.vz = set_position_target_local_ned.vz; } else { pos_sp_triplet.current.velocity_valid = false; } /* set the local acceleration values if the setpoint type is 'local pos' and none * of the accelerations fields is set to 'ignore' */ - if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) { + if (!offboard_control_mode.ignore_acceleration_force) { pos_sp_triplet.current.acceleration_valid = true; - pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0]; - pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1]; - pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2]; + pos_sp_triplet.current.a_x = set_position_target_local_ned.afx; + pos_sp_triplet.current.a_y = set_position_target_local_ned.afy; + pos_sp_triplet.current.a_z = set_position_target_local_ned.afz; pos_sp_triplet.current.acceleration_is_force = - offboard_control_sp.isForceSetpoint; + is_force_sp; } else { pos_sp_triplet.current.acceleration_valid = false; } - /* set the yaw sp value if the setpoint type is 'local pos' and the yaw - * field is not set to 'ignore' */ - if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_yaw(offboard_control_sp)) { + /* set the yaw sp value */ + if (!offboard_control_mode.ignore_attitude) { pos_sp_triplet.current.yaw_valid = true; - pos_sp_triplet.current.yaw = offboard_control_sp.yaw; + pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw; } else { pos_sp_triplet.current.yaw_valid = false; } - /* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate - * field is not set to 'ignore' */ - if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_yawrate(offboard_control_sp)) { + /* set the yawrate sp value */ + if (!offboard_control_mode.ignore_bodyrate) { pos_sp_triplet.current.yawspeed_valid = true; - pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate; + pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate; } else { pos_sp_triplet.current.yawspeed_valid = false; @@ -699,6 +652,66 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } void +MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *msg) +{ + mavlink_set_actuator_control_target_t set_actuator_control_target; + mavlink_msg_set_actuator_control_target_decode(msg, &set_actuator_control_target); + + struct offboard_control_mode_s offboard_control_mode; + memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints + + struct actuator_controls_s actuator_controls; + memset(&actuator_controls, 0, sizeof(actuator_controls));//XXX breaks compatibility with multiple setpoints + + if ((mavlink_system.sysid == set_actuator_control_target.target_system || + set_actuator_control_target.target_system == 0) && + (mavlink_system.compid == set_actuator_control_target.target_component || + set_actuator_control_target.target_component == 0)) { + + /* ignore all since we are setting raw actuators here */ + offboard_control_mode.ignore_thrust = true; + offboard_control_mode.ignore_attitude = true; + offboard_control_mode.ignore_bodyrate = true; + offboard_control_mode.ignore_position = true; + offboard_control_mode.ignore_velocity = true; + offboard_control_mode.ignore_acceleration_force = true; + + offboard_control_mode.timestamp = hrt_absolute_time(); + + if (_offboard_control_mode_pub < 0) { + _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); + } else { + orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); + } + + + /* If we are in offboard control mode, publish the actuator controls */ + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } + + if (_control_mode.flag_control_offboard_enabled) { + + actuator_controls.timestamp = hrt_absolute_time(); + + /* Set duty cycles for the servos in actuator_controls_0 */ + for(size_t i = 0; i < 8; i++) { + actuator_controls.control[i] = set_actuator_control_target.controls[i]; + } + + if (_actuator_controls_pub < 0) { + _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); + } else { + orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls); + } + } + } + +} + +void MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) { mavlink_vision_position_estimate_t pos; @@ -743,42 +756,52 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) mavlink_set_attitude_target_t set_attitude_target; mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target); - struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints + static struct offboard_control_mode_s offboard_control_mode = {}; /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == set_attitude_target.target_system || set_attitude_target.target_system == 0) && (mavlink_system.compid == set_attitude_target.target_component || set_attitude_target.target_component == 0)) { - for (int i = 0; i < 4; i++) { - offboard_control_sp.attitude[i] = set_attitude_target.q[i]; - } - offboard_control_sp.attitude_rate[0] = set_attitude_target.body_roll_rate; - offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate; - offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate; - - /* set correct ignore flags for body rate fields: copy from mavlink message */ - for (int i = 0; i < 3; i++) { - offboard_control_sp.ignore &= ~(1 << (i + OFB_IGN_BIT_BODYRATE_X)); - offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X; - } + /* set correct ignore flags for thrust field: copy from mavlink message */ - offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST); - offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST); - /* set correct ignore flags for attitude field: copy from mavlink message */ - offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_ATT); - offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << OFB_IGN_BIT_ATT); + offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); + + /* + * The tricky part in parsing this message is that the offboard sender *can* set attitude and thrust + * using different messages. Eg.: First send set_attitude_target containing the attitude and ignore + * bits set for everything else and then send set_attitude_target containing the thrust and ignore bits + * set for everything else. + */ + + /* + * if attitude or body rate have been used (not ignored) previously and this message only sends + * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and + * body rates to keep the controllers running + */ + bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7); + bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); + + if (ignore_bodyrate && ignore_attitude && !offboard_control_mode.ignore_thrust) { + /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ + offboard_control_mode.ignore_bodyrate = ignore_bodyrate && offboard_control_mode.ignore_bodyrate; + offboard_control_mode.ignore_attitude = ignore_attitude && offboard_control_mode.ignore_attitude; + } else { + offboard_control_mode.ignore_bodyrate = ignore_bodyrate; + offboard_control_mode.ignore_attitude = ignore_attitude; + } + offboard_control_mode.ignore_position = true; + offboard_control_mode.ignore_velocity = true; + offboard_control_mode.ignore_acceleration_force = true; - offboard_control_sp.timestamp = hrt_absolute_time(); - offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode + offboard_control_mode.timestamp = hrt_absolute_time(); - if (_offboard_control_sp_pub < 0) { - _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + if (_offboard_control_mode_pub < 0) { + _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); } else { - orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); } /* If we are in offboard control mode and offboard control loop through is enabled @@ -793,15 +816,16 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if (_control_mode.flag_control_offboard_enabled) { /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ - if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) || - offboard_control_sp_ignore_thrust(offboard_control_sp))) { - struct vehicle_attitude_setpoint_s att_sp; + if (!(offboard_control_mode.ignore_attitude)) { + static struct vehicle_attitude_setpoint_s att_sp = {}; att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body); att_sp.R_valid = true; - att_sp.thrust = set_attitude_target.thrust; + if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + att_sp.thrust = set_attitude_target.thrust; + } att_sp.yaw_sp_move_rate = 0.0; memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); att_sp.q_d_valid = true; @@ -814,14 +838,15 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ ///XXX add support for ignoring individual axes - if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) || - offboard_control_sp_ignore_thrust(offboard_control_sp))) { - struct vehicle_rates_setpoint_s rates_sp; + if (!(offboard_control_mode.ignore_bodyrate)) { + static struct vehicle_rates_setpoint_s rates_sp = {}; rates_sp.timestamp = hrt_absolute_time(); rates_sp.roll = set_attitude_target.body_roll_rate; rates_sp.pitch = set_attitude_target.body_pitch_rate; rates_sp.yaw = set_attitude_target.body_yaw_rate; - rates_sp.thrust = set_attitude_target.thrust; + if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + rates_sp.thrust = set_attitude_target.thrust; + } if (_att_sp_pub < 0) { _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); @@ -1524,7 +1549,7 @@ MavlinkReceiver::receive_start(Mavlink *parent) param.sched_priority = SCHED_PRIORITY_MAX - 80; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 2900); + pthread_attr_setstacksize(&receiveloop_attr, 2100); pthread_t thread; pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 699996860..4886bbd0a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -53,7 +53,7 @@ #include <uORB/topics/vehicle_land_detected.h> #include <uORB/topics/home_position.h> #include <uORB/topics/vehicle_status.h> -#include <uORB/topics/offboard_control_setpoint.h> +#include <uORB/topics/offboard_control_mode.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> @@ -122,6 +122,7 @@ private: void handle_message_vision_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_set_position_target_local_ned(mavlink_message_t *msg); + void handle_message_set_actuator_control_target(mavlink_message_t *msg); void handle_message_set_attitude_target(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); @@ -142,7 +143,7 @@ private: /** * Exponential moving average filter to smooth time offset */ - void smooth_time_offset(uint64_t offset_ns); + void smooth_time_offset(uint64_t offset_ns); mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; @@ -162,7 +163,8 @@ private: orb_advert_t _cmd_pub; orb_advert_t _flow_pub; orb_advert_t _range_pub; - orb_advert_t _offboard_control_sp_pub; + orb_advert_t _offboard_control_mode_pub; + orb_advert_t _actuator_controls_pub; orb_advert_t _global_vel_sp_pub; orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index f9d30dcbe..e82b8bd93 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/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index a0d76b0a6..0243dc2f7 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -835,7 +835,7 @@ MulticopterAttitudeControl::start() _control_task = task_spawn_cmd("mc_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2000, + 1800, (main_t)&MulticopterAttitudeControl::task_main_trampoline, nullptr); diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp index ecbf98c9e..b5a551109 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp @@ -89,9 +89,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : .yaw_rate_d = px4::ParameterFloat("MC_YAWRATE_D", PARAM_MC_YAWRATE_D_DEFAULT), .yaw_ff = px4::ParameterFloat("MC_YAW_FF", PARAM_MC_YAW_FF_DEFAULT), .yaw_rate_max = px4::ParameterFloat("MC_YAWRATE_MAX", PARAM_MC_YAWRATE_MAX_DEFAULT), - .man_roll_max = px4::ParameterFloat("MC_MAN_R_MAX", PARAM_MC_MAN_R_MAX_DEFAULT), - .man_pitch_max = px4::ParameterFloat("MC_MAN_P_MAX", PARAM_MC_MAN_P_MAX_DEFAULT), - .man_yaw_max = px4::ParameterFloat("MC_MAN_Y_MAX", PARAM_MC_MAN_Y_MAX_DEFAULT), .acro_roll_max = px4::ParameterFloat("MC_ACRO_R_MAX", PARAM_MC_ACRO_R_MAX_DEFAULT), .acro_pitch_max = px4::ParameterFloat("MC_ACRO_P_MAX", PARAM_MC_ACRO_P_MAX_DEFAULT), .acro_yaw_max = px4::ParameterFloat("MC_ACRO_Y_MAX", PARAM_MC_ACRO_Y_MAX_DEFAULT) @@ -146,11 +143,6 @@ MulticopterAttitudeControl::parameters_update() _params.yaw_ff = _params_handles.yaw_ff.update(); _params.yaw_rate_max = math::radians(_params_handles.yaw_rate_max.update()); - /* manual control scale */ - _params.man_roll_max = math::radians(_params_handles.man_roll_max.update()); - _params.man_pitch_max = math::radians(_params_handles.man_pitch_max.update()); - _params.man_yaw_max = math::radians(_params_handles.man_yaw_max.update()); - /* acro control scale */ _params.acro_rate_max(0) = math::radians(_params_handles.acro_roll_max.update()); _params.acro_rate_max(1) = math::radians(_params_handles.acro_pitch_max.update()); @@ -186,18 +178,6 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_atti if (_v_control_mode->data().flag_control_attitude_enabled) { control_attitude(dt); - /* publish the attitude setpoint if needed */ - if (_publish_att_sp && _v_status->data().is_rotary_wing) { - _v_att_sp_mod.data().timestamp = px4::get_time_micros(); - - if (_att_sp_pub != nullptr) { - _att_sp_pub->publish(_v_att_sp_mod); - - } else { - _att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>(); - } - } - /* publish attitude rates setpoint */ _v_rates_sp_mod.data().roll = _rates_sp(0); _v_rates_sp_mod.data().pitch = _rates_sp(1); @@ -224,9 +204,6 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_atti _manual_control_sp->data().r).emult(_params.acro_rate_max); _thrust_sp = _manual_control_sp->data().z; - /* reset yaw setpoint after ACRO */ - _reset_yaw_sp = true; - /* publish attitude rates setpoint */ _v_rates_sp_mod.data().roll = _rates_sp(0); _v_rates_sp_mod.data().pitch = _rates_sp(1); diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h index e44317316..52ba369c1 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h @@ -110,9 +110,6 @@ private: px4::ParameterFloat yaw_ff; px4::ParameterFloat yaw_rate_max; - px4::ParameterFloat man_roll_max; - px4::ParameterFloat man_pitch_max; - px4::ParameterFloat man_yaw_max; px4::ParameterFloat acro_roll_max; px4::ParameterFloat acro_pitch_max; px4::ParameterFloat acro_yaw_max; diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp index e779239ba..5db8f77ac 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp @@ -55,7 +55,6 @@ using namespace std; #endif MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : - _v_att_sp_mod(), _v_rates_sp_mod(), _actuators(), _publish_att_sp(false) @@ -67,9 +66,6 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : _params.rate_d.zero(); _params.yaw_ff = 0.0f; _params.yaw_rate_max = 0.0f; - _params.man_roll_max = 0.0f; - _params.man_pitch_max = 0.0f; - _params.man_yaw_max = 0.0f; _params.acro_rate_max.zero(); _rates_prev.zero(); @@ -87,101 +83,11 @@ MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() void MulticopterAttitudeControlBase::control_attitude(float dt) { - float yaw_sp_move_rate = 0.0f; - _publish_att_sp = false; - - - if (_v_control_mode->data().flag_control_manual_enabled) { - /* manual input, set or modify attitude setpoint */ - - if (_v_control_mode->data().flag_control_velocity_enabled - || _v_control_mode->data().flag_control_climb_rate_enabled) { - /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ - //XXX get rid of memcpy - memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data())); - } - - if (!_v_control_mode->data().flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude stabilized mode */ - _v_att_sp_mod.data().thrust = _manual_control_sp->data().z; - _publish_att_sp = true; - } - - if (!_armed->data().armed) { - /* reset yaw setpoint when disarmed */ - _reset_yaw_sp = true; - } - - /* reset yaw setpoint to current position if needed */ - if (_reset_yaw_sp) { - _reset_yaw_sp = false; - _v_att_sp_mod.data().yaw_body = _v_att->data().yaw; - _v_att_sp_mod.data().R_valid = false; - // _publish_att_sp = true; - } - - if (!_v_control_mode->data().flag_control_velocity_enabled) { - - if (_v_att_sp_mod.data().thrust < 0.1f) { - // TODO - //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ - // reset_yaw_sp = true; - //} - } else { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max; - _v_att_sp_mod.data().yaw_body = _wrap_pi( - _v_att_sp_mod.data().yaw_body + yaw_sp_move_rate * dt); - float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); - float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw); - - if (yaw_offs < -yaw_offs_max) { - _v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max); - - } else if (yaw_offs > yaw_offs_max) { - _v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max); - } - - _v_att_sp_mod.data().R_valid = false; - // _publish_att_sp = true; - } - /* update attitude setpoint if not in position control mode */ - _v_att_sp_mod.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max; - _v_att_sp_mod.data().pitch_body = -_manual_control_sp->data().x - * _params.man_pitch_max; - _v_att_sp_mod.data().R_valid = false; - // _publish_att_sp = true; - } - - } else { - /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - //XXX get rid of memcpy - memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data())); - - /* reset yaw setpoint after non-manual control mode */ - _reset_yaw_sp = true; - } - - _thrust_sp = _v_att_sp_mod.data().thrust; + _thrust_sp = _v_att_sp->data().thrust; /* construct attitude setpoint rotation matrix */ math::Matrix<3, 3> R_sp; - - if (_v_att_sp_mod.data().R_valid) { - /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_v_att_sp_mod.data().R_body[0]); - - } else { - /* rotation matrix in _att_sp is not valid, use euler angles instead */ - R_sp.from_euler(_v_att_sp_mod.data().roll_body, _v_att_sp_mod.data().pitch_body, - _v_att_sp_mod.data().yaw_body); - - /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp_mod.data().R_body[0], &R_sp.data[0][0], - sizeof(_v_att_sp_mod.data().R_body)); - _v_att_sp_mod.data().R_valid = true; - } + R_sp.set(_v_att_sp->data().R_body); /* rotation matrix for current state */ math::Matrix<3, 3> R; @@ -190,11 +96,11 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) /* all input data is ready, run controller itself */ /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2)); - math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); + math::Vector <3> R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector <3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); /* axis and sin(angle) of desired rotation */ - math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z); + math::Vector <3> e_R = R.transposed() * (R_z % R_sp_z); /* calculate angle error */ float e_R_z_sin = e_R.length(); @@ -209,7 +115,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) if (e_R_z_sin > 0.0f) { /* get axis-angle representation */ float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); - math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin; + math::Vector <3> e_R_z_axis = e_R / e_R_z_sin; e_R = e_R_z_axis * e_R_z_angle; @@ -224,9 +130,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) e_R_cp(2, 1) = e_R_z_axis(0); /* rotation matrix for roll/pitch only rotation */ - R_rp = R - * (_I + e_R_cp * e_R_z_sin - + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); } else { /* zero roll/pitch rotation */ @@ -234,8 +138,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) } /* R_rp and R_sp has the same Z axis, calculate yaw error */ - math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); - math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + math::Vector <3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); + math::Vector <3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; if (e_R_z_cos < 0.0f) { @@ -243,7 +147,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) * calculate angle and axis for R -> R_sp rotation directly */ math::Quaternion q; q.from_dcm(R.transposed() * R_sp); - math::Vector < 3 > e_R_d = q.imag(); + math::Vector <3> e_R_d = q.imag(); e_R_d.normalize(); e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); @@ -260,7 +164,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) _params.yaw_rate_max); /* feed forward yaw setpoint rate */ - _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; + _rates_sp(2) += _v_att_sp->data().yaw_sp_move_rate * yaw_w * _params.yaw_ff; + } void MulticopterAttitudeControlBase::control_attitude_rates(float dt) diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h index 124147079..4e33018b4 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h @@ -97,8 +97,6 @@ protected: px4::Subscriber<px4_actuator_armed> *_armed; /**< actuator arming status */ px4::Subscriber<px4_vehicle_status> *_v_status; /**< vehicle status */ - px4_vehicle_attitude_setpoint _v_att_sp_mod; /**< modified vehicle attitude setpoint - that gets published eventually */ px4_vehicle_rates_setpoint _v_rates_sp_mod; /**< vehicle rates setpoint that gets published eventually*/ px4_actuator_controls_0 _actuators; /**< actuator controls */ @@ -121,9 +119,6 @@ protected: float yaw_ff; /**< yaw control feed-forward */ float yaw_rate_max; /**< max yaw rate */ - float man_roll_max; - float man_pitch_max; - float man_yaw_max; math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ int32_t autostart_id; diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c index a0b1706f2..395ae83b0 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c @@ -190,35 +190,6 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF); PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX); /** - * Max manual roll - * - * @unit deg - * @min 0.0 - * @max 90.0 - * @group Multicopter Attitude Control - */ -PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX); - -/** - * Max manual pitch - * - * @unit deg - * @min 0.0 - * @max 90.0 - * @group Multicopter Attitude Control - */ -PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX); - -/** - * Max manual yaw rate - * - * @unit deg/s - * @min 0.0 - * @group Multicopter Attitude Control - */ -PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX); - -/** * Max acro roll rate * * @unit deg/s diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.h b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h index 59dd5240f..ff962dbf1 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_params.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h @@ -57,9 +57,6 @@ #define PARAM_MC_YAWRATE_D_DEFAULT 0.0f #define PARAM_MC_YAW_FF_DEFAULT 0.5f #define PARAM_MC_YAWRATE_MAX_DEFAULT 120.0f -#define PARAM_MC_MAN_R_MAX_DEFAULT 35.0f -#define PARAM_MC_MAN_P_MAX_DEFAULT 35.0f -#define PARAM_MC_MAN_Y_MAX_DEFAULT 120.0f #define PARAM_MC_ACRO_R_MAX_DEFAULT 35.0f #define PARAM_MC_ACRO_P_MAX_DEFAULT 35.0f #define PARAM_MC_ACRO_Y_MAX_DEFAULT 120.0f diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp index c2b847075..40eb498b4 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp @@ -71,7 +71,7 @@ int mc_att_control_m_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("mc_att_control_m", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 3000, + 1900, main, (argv) ? (char* const*)&argv[2] : (char* const*)NULL); 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 7f87e3532..d993692ab 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -761,8 +761,8 @@ void MulticopterPositionControl::control_auto(float dt) orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); //Make sure that the position setpoint is valid - if (!isfinite(_pos_sp_triplet.current.lat) || - !isfinite(_pos_sp_triplet.current.lon) || + if (!isfinite(_pos_sp_triplet.current.lat) || + !isfinite(_pos_sp_triplet.current.lon) || !isfinite(_pos_sp_triplet.current.alt)) { _pos_sp_triplet.current.valid = false; } @@ -1367,7 +1367,7 @@ MulticopterPositionControl::task_main() } else if (yaw_offs > YAW_OFFSET_MAX) { _att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX); - } + } } //Control roll and pitch directly if we no aiding velocity controller is active @@ -1388,15 +1388,22 @@ MulticopterPositionControl::task_main() _att_sp.timestamp = hrt_absolute_time(); } else { - reset_yaw_sp = true; + reset_yaw_sp = true; } - // publish attitude setpoint - if (_att_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + /* publish attitude setpoint + * Do not publish if offboard is enabled but position/velocity control is disabled, + * in this case the attitude setpoint is published by the mavlink app + */ + if (!(_control_mode.flag_control_offboard_enabled && + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled))) { + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ @@ -1419,7 +1426,7 @@ MulticopterPositionControl::start() _control_task = task_spawn_cmd("mc_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2000, + 1800, (main_t)&MulticopterPositionControl::task_main_trampoline, nullptr); diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index 391558dcc..40268358a 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -41,6 +41,9 @@ #include "mc_pos_control.h" #include "mc_pos_control_params.h" +/* The following inclue is needed because the pos controller depens on a parameter from attitude control to set a + * reasonable yaw setpoint in manual mode */ +#include <mc_att_control_multiplatform/mc_att_control_params.h> #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f @@ -82,7 +85,11 @@ MulticopterPositionControl::MulticopterPositionControl() : .xy_ff = px4::ParameterFloat("MPC_XY_FF", PARAM_MPC_XY_FF_DEFAULT), .tilt_max_air = px4::ParameterFloat("MPC_TILTMAX_AIR", PARAM_MPC_TILTMAX_AIR_DEFAULT), .land_speed = px4::ParameterFloat("MPC_LAND_SPEED", PARAM_MPC_LAND_SPEED_DEFAULT), - .tilt_max_land = px4::ParameterFloat("MPC_TILTMAX_LND", PARAM_MPC_TILTMAX_LND_DEFAULT) + .tilt_max_land = px4::ParameterFloat("MPC_TILTMAX_LND", PARAM_MPC_TILTMAX_LND_DEFAULT), + .man_roll_max = px4::ParameterFloat("MPC_MAN_R_MAX", PARAM_MPC_MAN_R_MAX_DEFAULT), + .man_pitch_max = px4::ParameterFloat("MPC_MAN_P_MAX", PARAM_MPC_MAN_P_MAX_DEFAULT), + .man_yaw_max = px4::ParameterFloat("MPC_MAN_Y_MAX", PARAM_MPC_MAN_Y_MAX_DEFAULT), + .mc_att_yaw_p = px4::ParameterFloat("MC_YAW_P", PARAM_MC_YAW_P_DEFAULT) }), _ref_alt(0.0f), _ref_timestamp(0), @@ -99,14 +106,13 @@ MulticopterPositionControl::MulticopterPositionControl() : * Do subscriptions */ _att = _n.subscribe<px4_vehicle_attitude>(&MulticopterPositionControl::handle_vehicle_attitude, this, 0); - _v_att_sp = _n.subscribe<px4_vehicle_attitude_setpoint>(0); _control_mode = _n.subscribe<px4_vehicle_control_mode>(0); _parameter_update = _n.subscribe<px4_parameter_update>( &MulticopterPositionControl::handle_parameter_update, this, 1000); _manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0); _armed = _n.subscribe<px4_actuator_armed>(0); _local_pos = _n.subscribe<px4_vehicle_local_position>(0); - _pos_sp_triplet = _n.subscribe<px4_position_setpoint_triplet>(0); + _pos_sp_triplet = _n.subscribe<px4_position_setpoint_triplet>(&MulticopterPositionControl::handle_position_setpoint_triplet, this, 0); _local_pos_sp = _n.subscribe<px4_vehicle_local_position_setpoint>(0); _global_vel_sp = _n.subscribe<px4_vehicle_global_velocity_setpoint>(0); @@ -146,6 +152,13 @@ MulticopterPositionControl::parameters_update() _params.land_speed = _params_handles.land_speed.update(); _params.tilt_max_land = math::radians(_params_handles.tilt_max_land.update()); + /* manual control scale */ + _params.man_roll_max = math::radians(_params_handles.man_roll_max.update()); + _params.man_pitch_max = math::radians(_params_handles.man_pitch_max.update()); + _params.man_yaw_max = math::radians(_params_handles.man_yaw_max.update()); + + _params.mc_att_yaw_p = _params_handles.mc_att_yaw_p.update(); + _params.pos_p(0) = _params.pos_p(1) = _params_handles.xy_p.update(); _params.pos_p(2) = _params_handles.z_p.update(); _params.vel_p(0) = _params.vel_p(1) = _params_handles.xy_vel_p.update(); @@ -287,22 +300,6 @@ MulticopterPositionControl::control_manual(float dt) _sp_move_rate /= sp_move_norm; } - /* move yaw setpoint */ - //XXX hardcoded hack until #1741 is in/ported (the values stem - //from default param values, see how yaw setpoint is moved in the attitude controller) - float yaw_sp_move_rate = _manual_control_sp->data().r * 120.0f * M_DEG_TO_RAD_F; - _att_sp_msg.data().yaw_body = _wrap_pi( - _att_sp_msg.data().yaw_body + yaw_sp_move_rate * dt); - float yaw_offs_max = 120.0f * M_DEG_TO_RAD_F / 2.0f; - float yaw_offs = _wrap_pi(_att_sp_msg.data().yaw_body - _att->data().yaw); - - if (yaw_offs < -yaw_offs_max) { - _att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw - yaw_offs_max); - - } else if (yaw_offs > yaw_offs_max) { - _att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw + yaw_offs_max); - } - /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ math::Matrix<3, 3> R_yaw_sp; R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body); @@ -554,11 +551,22 @@ void MulticopterPositionControl::handle_parameter_update(const px4_parameter_upd parameters_update(); } +void MulticopterPositionControl::handle_position_setpoint_triplet(const px4_position_setpoint_triplet &msg) +{ + /* Make sure that the position setpoint is valid */ + if (!isfinite(_pos_sp_triplet->data().current.lat) || + !isfinite(_pos_sp_triplet->data().current.lon) || + !isfinite(_pos_sp_triplet->data().current.alt)) { + _pos_sp_triplet->data().current.valid = false; + } +} + void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) { static bool reset_int_z = true; static bool reset_int_z_manual = false; static bool reset_int_xy = true; + static bool reset_yaw_sp = true; static bool was_armed = false; static uint64_t t_prev = 0; @@ -572,8 +580,10 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti _reset_alt_sp = true; reset_int_z = true; reset_int_xy = true; + reset_yaw_sp = true; } + /* Update previous arming state */ was_armed = _control_mode->data().flag_armed; update_ref(); @@ -610,22 +620,6 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti control_auto(dt); } - /* fill local position setpoint */ - _local_pos_sp_msg.data().timestamp = get_time_micros(); - _local_pos_sp_msg.data().x = _pos_sp(0); - _local_pos_sp_msg.data().y = _pos_sp(1); - _local_pos_sp_msg.data().z = _pos_sp(2); - _local_pos_sp_msg.data().yaw = _att_sp_msg.data().yaw_body; - - /* publish local position setpoint */ - if (_local_pos_sp_pub != nullptr) { - _local_pos_sp_pub->publish(_local_pos_sp_msg); - - } else { - _local_pos_sp_pub = _n.advertise<px4_vehicle_local_position_setpoint>(); - } - - if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ _R.identity(); @@ -633,7 +627,8 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti _att_sp_msg.data().R_valid = true; _att_sp_msg.data().roll_body = 0.0f; - // _att_sp_msg.data().yaw_body = _att->data().yaw; + _att_sp_msg.data().pitch_body = 0.0f; + _att_sp_msg.data().yaw_body = _att->data().yaw; _att_sp_msg.data().thrust = 0.0f; _att_sp_msg.data().timestamp = get_time_micros(); @@ -925,6 +920,11 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti _att_sp_msg.data().thrust = thrust_abs; + /* save thrust setpoint for logging */ + _local_pos_sp_msg.data().acc_x = thrust_sp(0); + _local_pos_sp_msg.data().acc_x = thrust_sp(1); + _local_pos_sp_msg.data().acc_x = thrust_sp(2); + _att_sp_msg.data().timestamp = get_time_micros(); /* publish attitude setpoint */ @@ -940,6 +940,25 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti } } + /* fill local position setpoint */ + _local_pos_sp_msg.data().timestamp = get_time_micros(); + _local_pos_sp_msg.data().x = _pos_sp(0); + _local_pos_sp_msg.data().y = _pos_sp(1); + _local_pos_sp_msg.data().z = _pos_sp(2); + _local_pos_sp_msg.data().yaw = _att_sp_msg.data().yaw_body; + _local_pos_sp_msg.data().vx = _vel_sp(0); + _local_pos_sp_msg.data().vy = _vel_sp(1); + _local_pos_sp_msg.data().vz = _vel_sp(2); + + /* publish local position setpoint */ + if (_local_pos_sp_pub != nullptr) { + _local_pos_sp_pub->publish(_local_pos_sp_msg); + + } else { + _local_pos_sp_pub = _n.advertise<px4_vehicle_local_position_setpoint>(); + } + + } else { /* position controller disabled, reset setpoints */ _reset_alt_sp = true; @@ -949,6 +968,68 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti reset_int_xy = true; } + /* generate attitude setpoint from manual controls */ + if(_control_mode->data().flag_control_manual_enabled && _control_mode->data().flag_control_attitude_enabled) { + + /* reset yaw setpoint to current position if needed */ + if (reset_yaw_sp) { + reset_yaw_sp = false; + _att_sp_msg.data().yaw_body = _att->data().yaw; + } + + /* do not move yaw while arming */ + else if (_manual_control_sp->data().z > 0.1f) + { + const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p; + + _att_sp_msg.data().yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max; + _att_sp_msg.data().yaw_body = _wrap_pi(_att_sp_msg.data().yaw_body + _att_sp_msg.data().yaw_sp_move_rate * dt); + float yaw_offs = _wrap_pi(_att_sp_msg.data().yaw_body - _att->data().yaw); + if (yaw_offs < - YAW_OFFSET_MAX) { + _att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw - YAW_OFFSET_MAX); + + } else if (yaw_offs > YAW_OFFSET_MAX) { + _att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw + YAW_OFFSET_MAX); + } + } + + /* Control roll and pitch directly if we no aiding velocity controller is active */ + if(!_control_mode->data().flag_control_velocity_enabled) { + _att_sp_msg.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max; + _att_sp_msg.data().pitch_body = -_manual_control_sp->data().x * _params.man_pitch_max; + } + + /* Control climb rate directly if no aiding altitude controller is active */ + if(!_control_mode->data().flag_control_climb_rate_enabled) { + _att_sp_msg.data().thrust = _manual_control_sp->data().z; + } + + /* Construct attitude setpoint rotation matrix */ + math::Matrix<3,3> R_sp; + R_sp.from_euler(_att_sp_msg.data().roll_body,_att_sp_msg.data().pitch_body,_att_sp_msg.data().yaw_body); + _att_sp_msg.data().R_valid = true; + memcpy(&_att_sp_msg.data().R_body[0], R_sp.data, sizeof(_att_sp_msg.data().R_body)); + _att_sp_msg.data().timestamp = get_time_micros(); + } + else { + reset_yaw_sp = true; + } + + /* publish attitude setpoint + * Do not publish if offboard is enabled but position/velocity control is disabled, in this case the attitude setpoint + * is published by the mavlink app + */ + if (!(_control_mode->data().flag_control_offboard_enabled && + !(_control_mode->data().flag_control_position_enabled || + _control_mode->data().flag_control_velocity_enabled))) { + if (_att_sp_pub != nullptr) { + _att_sp_pub->publish(_att_sp_msg); + + } else { + _att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>(); + } + } + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ reset_int_z_manual = _control_mode->data().flag_armed && _control_mode->data().flag_control_manual_enabled && !_control_mode->data().flag_control_climb_rate_enabled; } diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h index 245b5f5a9..54c6de155 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -75,6 +75,7 @@ public: /* Callbacks for topics */ void handle_vehicle_attitude(const px4_vehicle_attitude &msg); void handle_parameter_update(const px4_parameter_update &msg); + void handle_position_setpoint_triplet(const px4_position_setpoint_triplet &msg); void spin() { _n.spin(); } @@ -87,19 +88,18 @@ protected: Publisher<px4_vehicle_attitude_setpoint> *_att_sp_pub; /**< attitude setpoint publication */ Publisher<px4_vehicle_local_position_setpoint> *_local_pos_sp_pub; /**< vehicle local position setpoint publication */ - Publisher<px4_vehicle_global_velocity_setpoint> *_global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ + Publisher<px4_vehicle_global_velocity_setpoint> *_global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ - Subscriber<px4_vehicle_attitude> *_att; /**< vehicle attitude */ - Subscriber<px4_vehicle_attitude_setpoint> *_v_att_sp; /**< vehicle attitude setpoint */ - Subscriber<px4_vehicle_control_mode> *_control_mode; /**< vehicle control mode */ - Subscriber<px4_parameter_update> *_parameter_update; /**< parameter update */ - Subscriber<px4_manual_control_setpoint> *_manual_control_sp; /**< manual control setpoint */ - Subscriber<px4_actuator_armed> *_armed; /**< actuator arming status */ - Subscriber<px4_vehicle_local_position> *_local_pos; /**< local position */ - Subscriber<px4_position_setpoint_triplet> *_pos_sp_triplet; /**< local position */ - Subscriber<px4_vehicle_local_position_setpoint> *_local_pos_sp; /**< local position */ - Subscriber<px4_vehicle_global_velocity_setpoint> *_global_vel_sp; /**< local position */ + Subscriber<px4_vehicle_attitude> *_att; /**< vehicle attitude */ + Subscriber<px4_vehicle_control_mode> *_control_mode; /**< vehicle control mode */ + Subscriber<px4_parameter_update> *_parameter_update; /**< parameter update */ + Subscriber<px4_manual_control_setpoint> *_manual_control_sp; /**< manual control setpoint */ + Subscriber<px4_actuator_armed> *_armed; /**< actuator arming status */ + Subscriber<px4_vehicle_local_position> *_local_pos; /**< local position */ + Subscriber<px4_position_setpoint_triplet> *_pos_sp_triplet; /**< local position */ + Subscriber<px4_vehicle_local_position_setpoint> *_local_pos_sp; /**< local position */ + Subscriber<px4_vehicle_global_velocity_setpoint> *_global_vel_sp; /**< local position */ px4_vehicle_attitude_setpoint _att_sp_msg; px4_vehicle_local_position_setpoint _local_pos_sp_msg; @@ -125,6 +125,10 @@ protected: px4::ParameterFloat tilt_max_air; px4::ParameterFloat land_speed; px4::ParameterFloat tilt_max_land; + px4::ParameterFloat man_roll_max; + px4::ParameterFloat man_pitch_max; + px4::ParameterFloat man_yaw_max; + px4::ParameterFloat mc_att_yaw_p; // needed for calculating reasonable attitude setpoints in manual } _params_handles; /**< handles for interesting parameters */ struct { @@ -133,6 +137,10 @@ protected: float tilt_max_air; float land_speed; float tilt_max_land; + float man_roll_max; + float man_pitch_max; + float man_yaw_max; + float mc_att_yaw_p; math::Vector<3> pos_p; math::Vector<3> vel_p; diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c index c741a7f0a..709085662 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c @@ -210,3 +210,32 @@ PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND); */ PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED); +/** + * Max manual roll + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_MAN_R_MAX); + +/** + * Max manual pitch + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX); + +/** + * Max manual yaw rate + * + * @unit deg/s + * @min 0.0 + * @group Multicopter Attitude Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX); + diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h index fec3bcb7c..8c8b707ae 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h @@ -58,4 +58,7 @@ #define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f #define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f #define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f +#define PARAM_MPC_MAN_R_MAX_DEFAULT 35.0f +#define PARAM_MPC_MAN_P_MAX_DEFAULT 35.0f +#define PARAM_MPC_MAN_Y_MAX_DEFAULT 120.0f diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp index 87996d93b..1082061f6 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp @@ -71,7 +71,7 @@ int mc_pos_control_m_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("mc_pos_control_m", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 3000, + 2500, main, (argv) ? (char* const*)&argv[2] : (char* const*)NULL); diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 1568233b0..10394fed1 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -83,7 +83,7 @@ PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60); * @max 100 * @group RTL */ -PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); +PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 30); /** * RTL delay diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 5387b7e87..91915fb53 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -288,6 +288,20 @@ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); */ PARAM_DEFINE_INT32(CBRK_NO_VISION, 0); +/** + * INAV enabled + * + * If set to 1, use INAV for position estimation + * the system uses the compined attitude / position + * filter framework. + * + * @min 0.0 + * @max 1.0 + * @unit s + * @group Position Estimator INAV + */ +PARAM_DEFINE_INT32(INAV_ENABLED, 0); + int parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 3e21ec2a9..272e4b14f 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -130,7 +130,7 @@ PARAM_DEFINE_INT32(CAL_MAG0_ID, 0); * @max 30 * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_MAG0_ROT, 0); +PARAM_DEFINE_INT32(CAL_MAG0_ROT, -1); /** * Magnetometer X-axis offset @@ -308,7 +308,7 @@ PARAM_DEFINE_INT32(CAL_MAG1_ID, 0); * @max 30 * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_MAG1_ROT, 0); +PARAM_DEFINE_INT32(CAL_MAG1_ROT, -1); /** * Magnetometer X-axis offset @@ -486,7 +486,7 @@ PARAM_DEFINE_INT32(CAL_MAG2_ID, 0); * @max 30 * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_MAG2_ROT, 0); +PARAM_DEFINE_INT32(CAL_MAG2_ROT, -1); /** * Magnetometer X-axis offset @@ -994,6 +994,36 @@ 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. * * The channel index (starting from 1 for channel 1) indicates diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9a90c84e0..527ca2210 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1511,14 +1511,21 @@ Sensors::parameter_update_poll(bool forced) if (ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0) { /* mag is internal */ _mag_rotation[s] = _board_rotation; - /* reset param to -1 to indicate external mag */ + /* reset param to -1 to indicate internal mag */ int32_t minus_one = MAG_ROT_VAL_INTERNAL; param_set_no_notification(param_find(str), &minus_one); } else { - int32_t mag_rot = 0; + int32_t mag_rot; param_get(param_find(str), &mag_rot); + /* check if this mag is still set as internal */ + if (mag_rot < 0) { + /* it was marked as internal, change to external with no rotation */ + mag_rot = 0; + param_set_no_notification(param_find(str), &mag_rot); + } + /* handling of old setups, will be removed later (noted Feb 2015) */ int32_t deprecated_mag_rot = 0; param_get(param_find("SENS_EXT_MAG_ROT"), &deprecated_mag_rot); @@ -1536,6 +1543,9 @@ Sensors::parameter_update_poll(bool forced) if ((deprecated_mag_rot != 0) && (mag_rot <= 0)) { mag_rot = deprecated_mag_rot; param_set_no_notification(param_find(str), &mag_rot); + /* clear the old param, not supported in GUI anyway */ + deprecated_mag_rot = 0; + param_set_no_notification(param_find("SENS_EXT_MAG_ROT"), &deprecated_mag_rot); } /* handling of transition from internal to external */ diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index f60aa8d86..dbed29774 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -163,8 +163,8 @@ ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); #include "topics/vehicle_control_debug.h" ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s); -#include "topics/offboard_control_setpoint.h" -ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s); +#include "topics/offboard_control_mode.h" +ORB_DEFINE(offboard_control_mode, struct offboard_control_mode_s); #include "topics/optical_flow.h" ORB_DEFINE(optical_flow, struct optical_flow_s); diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h deleted file mode 100644 index 72a28e501..000000000 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ /dev/null @@ -1,276 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * - * 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. - * - ****************************************************************************/ - -/** - * @file offboard_control_setpoint.h - * Definition of the manual_control_setpoint uORB topic. - */ - -#ifndef TOPIC_OFFBOARD_CONTROL_SETPOINT_H_ -#define TOPIC_OFFBOARD_CONTROL_SETPOINT_H_ - -#include <stdint.h> -#include "../uORB.h" - -/** - * Off-board control inputs. - * - * Typically sent by a ground control station / joystick or by - * some off-board controller via C or SIMULINK. - */ -enum OFFBOARD_CONTROL_MODE { - OFFBOARD_CONTROL_MODE_DIRECT = 0, - OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1, - OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2, - OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED = 3, - OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED = 4, - OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5, - OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6, - OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7, - OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8, - OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9, - OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10, - OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ -}; - -enum OFFBOARD_CONTROL_FRAME { - OFFBOARD_CONTROL_FRAME_LOCAL_NED = 0, - OFFBOARD_CONTROL_FRAME_LOCAL_OFFSET_NED = 1, - OFFBOARD_CONTROL_FRAME_BODY_NED = 2, - OFFBOARD_CONTROL_FRAME_BODY_OFFSET_NED = 3, - OFFBOARD_CONTROL_FRAME_GLOBAL = 4 -}; - -/* mappings for the ignore bitmask */ -enum {OFB_IGN_BIT_POS_X, - OFB_IGN_BIT_POS_Y, - OFB_IGN_BIT_POS_Z, - OFB_IGN_BIT_VEL_X, - OFB_IGN_BIT_VEL_Y, - OFB_IGN_BIT_VEL_Z, - OFB_IGN_BIT_ACC_X, - OFB_IGN_BIT_ACC_Y, - OFB_IGN_BIT_ACC_Z, - OFB_IGN_BIT_BODYRATE_X, - OFB_IGN_BIT_BODYRATE_Y, - OFB_IGN_BIT_BODYRATE_Z, - OFB_IGN_BIT_ATT, - OFB_IGN_BIT_THRUST, - OFB_IGN_BIT_YAW, - OFB_IGN_BIT_YAWRATE, -}; - -/** - * @addtogroup topics - * @{ - */ - -struct offboard_control_setpoint_s { - uint64_t timestamp; - - enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */ - - double position[3]; /**< lat, lon, alt / x, y, z */ - float velocity[3]; /**< x vel, y vel, z vel */ - float acceleration[3]; /**< x acc, y acc, z acc */ - float attitude[4]; /**< attitude of vehicle (quaternion) */ - float attitude_rate[3]; /**< body angular rates (x, y, z) */ - float thrust; /**< thrust */ - float yaw; /**< yaw: this is the yaw from the position_target message - (not from the full attitude_target message) */ - float yaw_rate; /**< yaw rate: this is the yaw from the position_target message - (not from the full attitude_target message) */ - - uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file - for mapping */ - - bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */ - - float override_mode_switch; - - float aux1_cam_pan_flaps; - float aux2_cam_tilt; - float aux3_cam_zoom; - float aux4_cam_roll; - -}; /**< offboard control inputs */ -/** - * @} - */ - -/** - * Returns true if the position setpoint at index should be ignored - */ -inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_POS_X + index))); -} - -/** - * Returns true if all position setpoints should be ignored - */ -inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) { - for (int i = 0; i < 3; i++) { - if (!offboard_control_sp_ignore_position(offboard_control_sp, i)) { - return false; - } - } - return true; -} - -/** - * Returns true if some position setpoints should be ignored - */ -inline bool offboard_control_sp_ignore_position_some(const struct offboard_control_setpoint_s &offboard_control_sp) { - for (int i = 0; i < 3; i++) { - if (offboard_control_sp_ignore_position(offboard_control_sp, i)) { - return true; - } - } - return false; -} - -/** - * Returns true if the velocity setpoint at index should be ignored - */ -inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_VEL_X + index))); -} - -/** - * Returns true if all velocity setpoints should be ignored - */ -inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) { - for (int i = 0; i < 3; i++) { - if (!offboard_control_sp_ignore_velocity(offboard_control_sp, i)) { - return false; - } - } - return true; -} - -/** - * Returns true if some velocity setpoints should be ignored - */ -inline bool offboard_control_sp_ignore_velocity_some(const struct offboard_control_setpoint_s &offboard_control_sp) { - for (int i = 0; i < 3; i++) { - if (offboard_control_sp_ignore_velocity(offboard_control_sp, i)) { - return true; - } - } - return false; -} - -/** - * Returns true if the acceleration setpoint at index should be ignored - */ -inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_ACC_X + index))); -} - -/** - * Returns true if all acceleration setpoints should be ignored - */ -inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) { - for (int i = 0; i < 3; i++) { - if (!offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) { - return false; - } - } - return true; -} - -/** - * Returns true if some acceleration setpoints should be ignored - */ -inline bool offboard_control_sp_ignore_acceleration_some(const struct offboard_control_setpoint_s &offboard_control_sp) { - for (int i = 0; i < 3; i++) { - if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) { - return true; - } - } - return false; -} - -/** - * Returns true if the bodyrate setpoint at index should be ignored - */ -inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_BODYRATE_X + index))); -} - -/** - * Returns true if some of the bodyrate setpoints should be ignored - */ -inline bool offboard_control_sp_ignore_bodyrates_some(const struct offboard_control_setpoint_s &offboard_control_sp) { - for (int i = 0; i < 3; i++) { - if (offboard_control_sp_ignore_bodyrates(offboard_control_sp, i)) { - return true; - } - } - return false; -} - -/** - * Returns true if the attitude setpoint should be ignored - */ -inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) { - return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_ATT)); -} - -/** - * Returns true if the thrust setpoint should be ignored - */ -inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setpoint_s &offboard_control_sp) { - return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_THRUST)); -} - -/** - * Returns true if the yaw setpoint should be ignored - */ -inline bool offboard_control_sp_ignore_yaw(const struct offboard_control_setpoint_s &offboard_control_sp) { - return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAW)); -} - -/** - * Returns true if the yaw rate setpoint should be ignored - */ -inline bool offboard_control_sp_ignore_yawrate(const struct offboard_control_setpoint_s &offboard_control_sp) { - return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAWRATE)); -} - - -/* register this as object request broker structure */ -ORB_DECLARE(offboard_control_setpoint); - -#endif diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 764e33179..b4f81d429 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -684,6 +684,8 @@ namespace ORBDevMaster *g_dev; bool pubsubtest_passed = false; +bool pubsubtest_print = false; +int pubsubtest_res = OK; struct orb_test { int val; @@ -693,6 +695,22 @@ struct orb_test { ORB_DEFINE(orb_test, struct orb_test); ORB_DEFINE(orb_multitest, struct orb_test); +struct orb_test_medium { + int val; + hrt_abstime time; + char junk[64]; +}; + +ORB_DEFINE(orb_test_medium, struct orb_test_medium); + +struct orb_test_large { + int val; + hrt_abstime time; + char junk[512]; +}; + +ORB_DEFINE(orb_test_large, struct orb_test_large); + int test_fail(const char *fmt, ...) { @@ -727,21 +745,44 @@ int pubsublatency_main(void) float latency_integral = 0.0f; /* wakeup source(s) */ - struct pollfd fds[1]; + struct pollfd fds[3]; - int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_multitest), 0); + int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test), 0); + int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0); + int test_multi_sub_large = orb_subscribe_multi(ORB_ID(orb_test_large), 0); + + struct orb_test_large t; + + /* clear all ready flags */ + orb_copy(ORB_ID(orb_test), test_multi_sub, &t); + orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); + orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); fds[0].fd = test_multi_sub; fds[0].events = POLLIN; + fds[1].fd = test_multi_sub_medium; + fds[1].events = POLLIN; + fds[2].fd = test_multi_sub_large; + fds[2].events = POLLIN; - struct orb_test t; + const unsigned maxruns = 1000; + unsigned timingsgroup = 0; - const unsigned maxruns = 10; + unsigned *timings = new unsigned[maxruns]; for (unsigned i = 0; i < maxruns; i++) { /* wait for up to 500ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); - orb_copy(ORB_ID(orb_multitest), test_multi_sub, &t); + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(orb_test), test_multi_sub, &t); + timingsgroup = 0; + } else if (fds[1].revents & POLLIN) { + orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); + timingsgroup = 1; + } else if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); + timingsgroup = 2; + } if (pret < 0) { warn("poll error %d, %d", pret, errno); @@ -750,17 +791,46 @@ int pubsublatency_main(void) hrt_abstime elt = hrt_elapsed_time(&t.time); latency_integral += elt; + timings[i] = elt; } orb_unsubscribe(test_multi_sub); + orb_unsubscribe(test_multi_sub_medium); + orb_unsubscribe(test_multi_sub_large); + + if (pubsubtest_print) { + char fname[32]; + sprintf(fname, "/fs/microsd/timings%u.txt", timingsgroup); + FILE *f = fopen(fname, "w"); + if (f == NULL) { + warnx("Error opening file!\n"); + return ERROR; + } + + for (unsigned i = 0; i < maxruns; i++) { + fprintf(f, "%u\n", timings[i]); + } + + fclose(f); + } + + delete[] timings; warnx("mean: %8.4f", static_cast<double>(latency_integral / maxruns)); pubsubtest_passed = true; - return OK; + if (static_cast<float>(latency_integral / maxruns) > 30.0f) { + pubsubtest_res = ERROR; + } else { + pubsubtest_res = OK; + } + + return pubsubtest_res; } +template<typename S> int latency_test(orb_id_t T, bool print); + int test() { @@ -874,6 +944,25 @@ test() if (prio != ORB_PRIO_MIN) return test_fail("prio: %d", prio); + if (OK != latency_test<struct orb_test>(ORB_ID(orb_test), false)) + return test_fail("latency test failed"); + + return test_note("PASS"); +} + +template<typename S> int +latency_test(orb_id_t T, bool print) +{ + S t; + t.val = 308; + t.time = hrt_absolute_time(); + + int pfd0 = orb_advertise(T, &t); + + pubsubtest_print = print; + + pubsubtest_passed = false; + /* test pub / sub latency */ int pubsub_task = task_spawn_cmd("uorb_latency", @@ -885,20 +974,22 @@ test() /* give the test task some data */ while (!pubsubtest_passed) { - t.val = 303; + t.val = 308; t.time = hrt_absolute_time(); - if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) + if (OK != orb_publish(T, pfd0, &t)) return test_fail("mult. pub0 timing fail"); /* simulate >800 Hz system operation */ usleep(1000); } + close(pfd0); + if (pubsub_task < 0) { return test_fail("failed launching task"); } - return test_note("PASS"); + return pubsubtest_res; } int @@ -956,12 +1047,26 @@ uorb_main(int argc, char *argv[]) return test(); /* + * Test the latency. + */ + if (!strcmp(argv[1], "latency_test")) { + + if (argc > 2 && !strcmp(argv[2], "medium")) { + return latency_test<struct orb_test_medium>(ORB_ID(orb_test_medium), true); + } else if (argc > 2 && !strcmp(argv[2], "large")) { + return latency_test<struct orb_test_large>(ORB_ID(orb_test_large), true); + } else { + return latency_test<struct orb_test>(ORB_ID(orb_test), true); + } + } + + /* * Print driver information. */ if (!strcmp(argv[1], "status")) return info(); - errx(-EINVAL, "unrecognized command, try 'start', 'test' or 'status'"); + errx(-EINVAL, "unrecognized command, try 'start', 'test', 'latency_test' or 'status'"); } /* diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 288ba2ebe..74e1efd6c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -782,18 +782,23 @@ void VtolAttitudeControl::task_main() fill_mc_att_control_output(); fill_mc_att_rates_sp(); - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); - - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } - - if (_actuators_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); - - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + /* Only publish if the proper mode(s) are enabled */ + if(_v_control_mode.flag_control_attitude_enabled || + _v_control_mode.flag_control_rates_enabled) + { + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); + } + + if (_actuators_1_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + + } else { + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + } } } } @@ -813,18 +818,23 @@ void VtolAttitudeControl::task_main() fill_fw_att_control_output(); fill_fw_att_rates_sp(); - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + /* Only publish if the proper mode(s) are enabled */ + if(_v_control_mode.flag_control_attitude_enabled || + _v_control_mode.flag_control_rates_enabled) + { + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); + } - if (_actuators_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + if (_actuators_1_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + } else { + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + } } } } diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index f8561fa3b..0e98783fd 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -66,6 +66,8 @@ #include <px4_vehicle_global_velocity_setpoint.h> #include <px4_vehicle_local_position.h> #include <px4_position_setpoint_triplet.h> +#include <px4_offboard_control_mode.h> +#include <px4_vehicle_force_setpoint.h> #endif #else @@ -93,6 +95,8 @@ #include <platforms/nuttx/px4_messages/px4_vehicle_global_velocity_setpoint.h> #include <platforms/nuttx/px4_messages/px4_vehicle_local_position.h> #include <platforms/nuttx/px4_messages/px4_position_setpoint_triplet.h> +#include <platforms/nuttx/px4_messages/px4_offboard_control_mode.h> +#include <platforms/nuttx/px4_messages/px4_vehicle_force_setpoint.h> #endif #include <systemlib/err.h> #include <systemlib/param/param.h> diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index 2673122c7..0c32026f3 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -45,18 +45,25 @@ Commander::Commander() : _n(), _man_ctrl_sp_sub(_n.subscribe("manual_control_setpoint", 10, &Commander::ManualControlInputCallback, this)), + _offboard_control_mode_sub(_n.subscribe("offboard_control_mode", 10, &Commander::OffboardControlModeCallback, this)), _vehicle_control_mode_pub(_n.advertise<px4::vehicle_control_mode>("vehicle_control_mode", 10)), _actuator_armed_pub(_n.advertise<px4::actuator_armed>("actuator_armed", 10)), _vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10)), _parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)), _msg_parameter_update(), _msg_actuator_armed(), - _msg_vehicle_control_mode() + _msg_vehicle_control_mode(), + _msg_offboard_control_mode(), + _got_manual_control(false) { + + /* Default to offboard control: when no joystick is connected offboard control should just work */ + } void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg) { + _got_manual_control = true; px4::vehicle_status msg_vehicle_status; /* fill vehicle control mode based on (faked) stick positions*/ @@ -101,12 +108,51 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon } } +void Commander::SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode, + px4::vehicle_control_mode &msg_vehicle_control_mode) +{ + msg_vehicle_control_mode.flag_control_manual_enabled = false; + msg_vehicle_control_mode.flag_control_offboard_enabled = true; + msg_vehicle_control_mode.flag_control_auto_enabled = false; + + msg_vehicle_control_mode.flag_control_rates_enabled = !msg_offboard_control_mode.ignore_bodyrate || + !msg_offboard_control_mode.ignore_attitude || + !msg_offboard_control_mode.ignore_position || + !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_acceleration_force; + + msg_vehicle_control_mode.flag_control_attitude_enabled = !msg_offboard_control_mode.ignore_attitude || + !msg_offboard_control_mode.ignore_position || + !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_acceleration_force; + + + msg_vehicle_control_mode.flag_control_velocity_enabled = !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_position; + + msg_vehicle_control_mode.flag_control_climb_rate_enabled = !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_position; + + msg_vehicle_control_mode.flag_control_position_enabled = !msg_offboard_control_mode.ignore_position; + + msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_position; +} + void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, px4::vehicle_status &msg_vehicle_status, px4::vehicle_control_mode &msg_vehicle_control_mode) { // XXX this is a minimal implementation. If more advanced functionalities are // needed consider a full port of the commander + + if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON) + { + SetOffboardControl(_msg_offboard_control_mode, msg_vehicle_control_mode); + return; + } + + msg_vehicle_control_mode.flag_control_offboard_enabled = false; + switch (msg->mode_switch) { case px4::manual_control_setpoint::SWITCH_POS_NONE: ROS_WARN("Joystick button mapping error, main mode not set"); @@ -152,6 +198,35 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, } +void Commander::OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg) +{ + _msg_offboard_control_mode = *msg; + + /* Force system into offboard control mode */ + if (!_got_manual_control) { + SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode); + + px4::vehicle_status msg_vehicle_status; + msg_vehicle_status.timestamp = px4::get_time_micros(); + msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF; + msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; + msg_vehicle_status.is_rotary_wing = true; + msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; + + + _msg_actuator_armed.armed = true; + _msg_actuator_armed.timestamp = px4::get_time_micros(); + + _msg_vehicle_control_mode.timestamp = px4::get_time_micros(); + _msg_vehicle_control_mode.flag_armed = true; + + + _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); + _actuator_armed_pub.publish(_msg_actuator_armed); + _vehicle_status_pub.publish(msg_vehicle_status); + } +} + int main(int argc, char **argv) { ros::init(argc, argv, "commander"); diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index 58b7257b7..c537c8419 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -44,6 +44,7 @@ #include <px4/vehicle_status.h> #include <px4/parameter_update.h> #include <px4/actuator_armed.h> +#include <px4/offboard_control_mode.h> class Commander { @@ -59,14 +60,26 @@ protected: void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg); /** + * Stores the offboard control mode + */ + void OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg); + + /** * Set control mode flags based on stick positions (equiv to code in px4 commander) */ void EvalSwitches(const px4::manual_control_setpointConstPtr &msg, px4::vehicle_status &msg_vehicle_status, px4::vehicle_control_mode &msg_vehicle_control_mode); + /** + * Sets offboard controll flags in msg_vehicle_control_mode + */ + void SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode, + px4::vehicle_control_mode &msg_vehicle_control_mode); + ros::NodeHandle _n; ros::Subscriber _man_ctrl_sp_sub; + ros::Subscriber _offboard_control_mode_sub; ros::Publisher _vehicle_control_mode_pub; ros::Publisher _actuator_armed_pub; ros::Publisher _vehicle_status_pub; @@ -75,5 +88,8 @@ protected: px4::parameter_update _msg_parameter_update; px4::actuator_armed _msg_actuator_armed; px4::vehicle_control_mode _msg_vehicle_control_mode; + px4::offboard_control_mode _msg_offboard_control_mode; + + bool _got_manual_control; }; diff --git a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp new file mode 100644 index 000000000..fb0b09de1 --- /dev/null +++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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. + * + ****************************************************************************/ + +/** + * @file demo_offboard_position_Setpoints.cpp + * + * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL + * + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ + +#include "demo_offboard_attitude_setpoints.h" + +#include <platforms/px4_middleware.h> +#include <geometry_msgs/PoseStamped.h> +#include <std_msgs/Float64.h> +#include <math.h> +#include <tf/transform_datatypes.h> + +DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() : + _n(), + _attitude_sp_pub(_n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint/attitude", 1)), + _thrust_sp_pub(_n.advertise<std_msgs::Float64>("mavros/setpoint/att_throttle", 1)) +{ +} + + +int DemoOffboardAttitudeSetpoints::main() +{ + px4::Rate loop_rate(10); + + while (ros::ok()) { + loop_rate.sleep(); + ros::spinOnce(); + + /* Publish example offboard attitude setpoint */ + geometry_msgs::PoseStamped pose; + tf::Quaternion q = tf::createQuaternionFromRPY(0.0, 0.1 * (sinf(0.5 * (float)px4::get_time_micros() / 1000000.0f)) , 0.0); + quaternionTFToMsg(q, pose.pose.orientation); + + _attitude_sp_pub.publish(pose); + + std_msgs::Float64 thrust; + thrust.data = 0.4f + 0.25 * (sinf((float)px4::get_time_micros() / 1000000.0f)); // just some example throttle input that makes the quad 'jump' + _thrust_sp_pub.publish(thrust); + } + return 0; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "demo_offboard_position_setpoints"); + DemoOffboardAttitudeSetpoints d; + return d.main(); +} diff --git a/src/modules/uORB/topics/vehicle_force_setpoint.h b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h index e3a7360b2..d7b7a37ba 100644 --- a/src/modules/uORB/topics/vehicle_force_setpoint.h +++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2015 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 @@ -32,34 +32,27 @@ ****************************************************************************/ /** - * @file vehicle_force_setpoint.h + * @file demo_offboard_attitude_Setpoints.h + * + * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL + * * @author Thomas Gubler <thomasgubler@gmail.com> - * Definition of force (NED) setpoint uORB topic. Typically this can be used - * by a position control app together with an attitude control app. - */ - -#ifndef TOPIC_VEHICLE_FORCE_SETPOINT_H_ -#define TOPIC_VEHICLE_FORCE_SETPOINT_H_ +*/ -#include "../uORB.h" +#include "ros/ros.h" +#include <px4/manual_control_setpoint.h> -/** - * @addtogroup topics - * @{ - */ - -struct vehicle_force_setpoint_s { - float x; /**< in N NED */ - float y; /**< in N NED */ - float z; /**< in N NED */ - float yaw; /**< right-hand rotation around downward axis (rad, equivalent to Tait-Bryan yaw) */ -}; /**< Desired force in NED frame */ +class DemoOffboardAttitudeSetpoints +{ +public: + DemoOffboardAttitudeSetpoints(); -/** - * @} - */ + ~DemoOffboardAttitudeSetpoints() {} -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_force_setpoint); + int main(); -#endif +protected: + ros::NodeHandle _n; + ros::Publisher _attitude_sp_pub; + ros::Publisher _thrust_sp_pub; +}; diff --git a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp new file mode 100644 index 000000000..7366d7fc6 --- /dev/null +++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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. + * + ****************************************************************************/ + +/** + * @file demo_offboard_position_Setpoints.cpp + * + * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL + * + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ + +#include "demo_offboard_position_setpoints.h" + +#include <platforms/px4_middleware.h> +#include <geometry_msgs/PoseStamped.h> + +DemoOffboardPositionSetpoints::DemoOffboardPositionSetpoints() : + _n(), + _local_position_sp_pub(_n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint/local_position", 1)) +{ +} + + +int DemoOffboardPositionSetpoints::main() +{ + px4::Rate loop_rate(10); + + while (ros::ok()) { + loop_rate.sleep(); + ros::spinOnce(); + + /* Publish example offboard position setpoint */ + geometry_msgs::PoseStamped pose; + pose.pose.position.x = 0; + pose.pose.position.y = 0; + pose.pose.position.z = 1; + _local_position_sp_pub.publish(pose); + } + return 0; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "demo_offboard_position_setpoints"); + DemoOffboardPositionSetpoints d; + return d.main(); +} diff --git a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h new file mode 100644 index 000000000..7d39690f4 --- /dev/null +++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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. + * + ****************************************************************************/ + +/** + * @file demo_offboard_position_Setpoints.h + * + * Demo for sending offboard position setpoints to mavros to show offboard position control in SITL + * + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ + +#include "ros/ros.h" +#include <px4/manual_control_setpoint.h> + +class DemoOffboardPositionSetpoints +{ +public: + DemoOffboardPositionSetpoints(); + + ~DemoOffboardPositionSetpoints() {} + + int main(); + +protected: + ros::NodeHandle _n; + ros::Publisher _local_position_sp_pub; +}; diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 72f6e252f..8488c518f 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -76,7 +76,8 @@ ManualInput::ManualInput() : _n.param("map_posctl", _param_buttons_map[2], 2); _n.param("map_auto_mission", _param_buttons_map[3], 3); _n.param("map_auto_loiter", _param_buttons_map[4], 4); - _n.param("map_auto_rtl", _param_buttons_map[5], 4); + _n.param("map_auto_rtl", _param_buttons_map[5], 5); + _n.param("map_offboard", _param_buttons_map[6], 6); /* Default to manual */ _msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; @@ -84,6 +85,8 @@ ManualInput::ManualInput() : _msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; _msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; _msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; } @@ -120,7 +123,6 @@ void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, do void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) { msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; - msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; if (_buttons_state[MAIN_STATE_MANUAL] != msg->buttons[_param_buttons_map[MAIN_STATE_MANUAL]] == true) { msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; @@ -128,6 +130,7 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; return; } else if (_buttons_state[MAIN_STATE_ALTCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_ALTCTL]] == true) { @@ -136,6 +139,7 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; return; } else if (_buttons_state[MAIN_STATE_POSCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_POSCTL]] == true) { @@ -144,6 +148,15 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_ON; msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + return; + } else if (_buttons_state[MAIN_STATE_OFFBOARD] != msg->buttons[_param_buttons_map[MAIN_STATE_OFFBOARD]] == true) { + msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_ON; + msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_ON; return; } diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h index bf704f675..2bafcca2e 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.h +++ b/src/platforms/ros/nodes/manual_input/manual_input.h @@ -77,6 +77,8 @@ protected: MAIN_STATE_AUTO_MISSION, MAIN_STATE_AUTO_LOITER, MAIN_STATE_AUTO_RTL, + // MAIN_STATE_ACRO, + MAIN_STATE_OFFBOARD, MAIN_STATE_MAX }; diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp new file mode 100644 index 000000000..5459fcffd --- /dev/null +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -0,0 +1,296 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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. + * + ****************************************************************************/ + +/** + * @file mavlink.cpp + * Dummy mavlink node that interfaces to a mavros node via UDP + * This simulates the onboard mavlink app to some degree. It should be possible to + * send offboard setpoints via mavros to the SITL setup the same way as on the real system + * + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ + +#include "mavlink.h" + +#include <platforms/px4_middleware.h> + +using namespace px4; + +Mavlink::Mavlink() : + _n(), + _v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)), + _v_local_pos_sub(_n.subscribe("vehicle_local_position", 1, &Mavlink::VehicleLocalPositionCallback, this)), + _v_att_sp_pub(_n.advertise<vehicle_attitude_setpoint>("vehicle_attitude_setpoint", 1)), + _pos_sp_triplet_pub(_n.advertise<position_setpoint_triplet>("position_setpoint_triplet", 1)), + _offboard_control_mode_pub(_n.advertise<offboard_control_mode>("offboard_control_mode", 1)), + _force_sp_pub(_n.advertise<vehicle_force_setpoint>("vehicle_force_setpoint", 1)) +{ + _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); + _link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3)); + +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "mavlink"); + Mavlink m; + ros::spin(); + return 0; +} + +void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg) +{ + mavlink_message_t msg_m; + mavlink_msg_attitude_quaternion_pack_chan( + _link->get_system_id(), + _link->get_component_id(), + _link->get_channel(), + &msg_m, + get_time_micros() / 1000, + msg->q[0], + msg->q[1], + msg->q[2], + msg->q[3], + msg->rollspeed, + msg->pitchspeed, + msg->yawspeed); + _link->send_message(&msg_m); +} + +void Mavlink::VehicleLocalPositionCallback(const vehicle_local_positionConstPtr &msg) +{ + mavlink_message_t msg_m; + mavlink_msg_local_position_ned_pack_chan( + _link->get_system_id(), + _link->get_component_id(), + _link->get_channel(), + &msg_m, + get_time_micros() / 1000, + msg->x, + msg->y, + msg->z, + msg->vx, + msg->vy, + msg->vz); + _link->send_message(&msg_m); +} + +void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) { + (void)sysid; + (void)compid; + + switch(mmsg->msgid) { + case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: + handle_msg_set_attitude_target(mmsg); + break; + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + handle_msg_set_position_target_local_ned(mmsg); + break; + default: + break; + } + +} + +void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) +{ + mavlink_set_attitude_target_t set_attitude_target; + mavlink_msg_set_attitude_target_decode(mmsg, &set_attitude_target); + + static offboard_control_mode offboard_control_mode; + + /* set correct ignore flags for thrust field: copy from mavlink message */ + offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); + + /* + * if attitude or body rate have been used (not ignored) previously and this message only sends + * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and + * body rates to keep the controllers running + */ + bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7); + bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); + + if (ignore_bodyrate && ignore_attitude && !offboard_control_mode.ignore_thrust) { + /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ + offboard_control_mode.ignore_bodyrate = ignore_bodyrate && offboard_control_mode.ignore_bodyrate; + offboard_control_mode.ignore_attitude = ignore_attitude && offboard_control_mode.ignore_attitude; + } else { + offboard_control_mode.ignore_bodyrate = ignore_bodyrate; + offboard_control_mode.ignore_attitude = ignore_attitude; + } + offboard_control_mode.ignore_position = true; + offboard_control_mode.ignore_velocity = true; + offboard_control_mode.ignore_acceleration_force = true; + + offboard_control_mode.timestamp = get_time_micros(); + _offboard_control_mode_pub.publish(offboard_control_mode); + + static vehicle_attitude_setpoint att_sp = {}; + + /* The real mavlink app has a ckeck at this location which makes sure that the attitude setpoint + * gets published only if in offboard mode. We leave that out for now. + */ + + att_sp.timestamp = get_time_micros(); + mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, + &att_sp.yaw_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body.data()); + att_sp.R_valid = true; + + if (!offboard_control_mode.ignore_thrust) { + att_sp.thrust = set_attitude_target.thrust; + } + + if (!offboard_control_mode.ignore_attitude) { + for (ssize_t i = 0; i < 4; i++) { + att_sp.q_d[i] = set_attitude_target.q[i]; + } + att_sp.q_d_valid = true; + } + + _v_att_sp_pub.publish(att_sp); + + + //XXX real mavlink publishes rate sp here + +} + +void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t *mmsg) +{ + + mavlink_set_position_target_local_ned_t set_position_target_local_ned; + mavlink_msg_set_position_target_local_ned_decode(mmsg, &set_position_target_local_ned); + + offboard_control_mode offboard_control_mode; + // memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints + + /* Only accept messages which are intended for this system */ + // XXX removed for sitl, makes maybe sense to re-introduce at some point + // if ((mavlink_system.sysid == set_position_target_local_ned.target_system || + // set_position_target_local_ned.target_system == 0) && + // (mavlink_system.compid == set_position_target_local_ned.target_component || + // set_position_target_local_ned.target_component == 0)) { + + /* convert mavlink type (local, NED) to uORB offboard control struct */ + offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7); + offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38); + offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0); + bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); + /* yaw ignore flag mapps to ignore_attitude */ + offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400); + /* yawrate ignore flag mapps to ignore_bodyrate */ + offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800); + + + + offboard_control_mode.timestamp = get_time_micros(); + _offboard_control_mode_pub.publish(offboard_control_mode); + + /* The real mavlink app has a ckeck at this location which makes sure that the position setpoint triplet + * gets published only if in offboard mode. We leave that out for now. + */ + if (is_force_sp && offboard_control_mode.ignore_position && + offboard_control_mode.ignore_velocity) { + /* The offboard setpoint is a force setpoint only, directly writing to the force + * setpoint topic and not publishing the setpoint triplet topic */ + vehicle_force_setpoint force_sp; + force_sp.x = set_position_target_local_ned.afx; + force_sp.y = set_position_target_local_ned.afy; + force_sp.z = set_position_target_local_ned.afz; + //XXX: yaw + + _force_sp_pub.publish(force_sp); + } else { + /* It's not a pure force setpoint: publish to setpoint triplet topic */ + position_setpoint_triplet pos_sp_triplet; + pos_sp_triplet.previous.valid = false; + pos_sp_triplet.next.valid = false; + pos_sp_triplet.current.valid = true; + pos_sp_triplet.current.type = position_setpoint::SETPOINT_TYPE_POSITION; //XXX support others + + /* set the local pos values */ + if (!offboard_control_mode.ignore_position) { + pos_sp_triplet.current.position_valid = true; + pos_sp_triplet.current.x = set_position_target_local_ned.x; + pos_sp_triplet.current.y = set_position_target_local_ned.y; + pos_sp_triplet.current.z = set_position_target_local_ned.z; + } else { + pos_sp_triplet.current.position_valid = false; + } + + /* set the local vel values */ + if (!offboard_control_mode.ignore_velocity) { + pos_sp_triplet.current.velocity_valid = true; + pos_sp_triplet.current.vx = set_position_target_local_ned.vx; + pos_sp_triplet.current.vy = set_position_target_local_ned.vy; + pos_sp_triplet.current.vz = set_position_target_local_ned.vz; + } else { + pos_sp_triplet.current.velocity_valid = false; + } + + /* set the local acceleration values if the setpoint type is 'local pos' and none + * of the accelerations fields is set to 'ignore' */ + if (!offboard_control_mode.ignore_acceleration_force) { + pos_sp_triplet.current.acceleration_valid = true; + pos_sp_triplet.current.a_x = set_position_target_local_ned.afx; + pos_sp_triplet.current.a_y = set_position_target_local_ned.afy; + pos_sp_triplet.current.a_z = set_position_target_local_ned.afz; + pos_sp_triplet.current.acceleration_is_force = + is_force_sp; + + } else { + pos_sp_triplet.current.acceleration_valid = false; + } + + /* set the yaw sp value */ + if (!offboard_control_mode.ignore_attitude) { + pos_sp_triplet.current.yaw_valid = true; + pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw; + + } else { + pos_sp_triplet.current.yaw_valid = false; + } + + /* set the yawrate sp value */ + if (!offboard_control_mode.ignore_bodyrate) { + pos_sp_triplet.current.yawspeed_valid = true; + pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate; + + } else { + pos_sp_triplet.current.yawspeed_valid = false; + } + //XXX handle global pos setpoints (different MAV frames) + + _pos_sp_triplet_pub.publish(pos_sp_triplet); + } +} diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h new file mode 100644 index 000000000..acb2408f3 --- /dev/null +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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. + * + ****************************************************************************/ + +/** + * @file mavlink.h + * Dummy mavlink node that interfaces to a mavros node via UDP + * This simulates the onboard mavlink app to some degree. It should be possible to + * send offboard setpoints via mavros to the SITL setup the same way as on the real system + * + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ + +#include "ros/ros.h" +#include <mavconn/interface.h> +#include <px4/vehicle_attitude.h> +#include <px4/vehicle_local_position.h> +#include <px4/vehicle_attitude_setpoint.h> +#include <px4/position_setpoint_triplet.h> +#include <px4/vehicle_force_setpoint.h> +#include <px4/offboard_control_mode.h> + +namespace px4 +{ + +class Mavlink +{ +public: + Mavlink(); + + ~Mavlink() {} + +protected: + + ros::NodeHandle _n; + mavconn::MAVConnInterface::Ptr _link; + ros::Subscriber _v_att_sub; + ros::Subscriber _v_local_pos_sub; + ros::Publisher _v_att_sp_pub; + ros::Publisher _pos_sp_triplet_pub; + ros::Publisher _offboard_control_mode_pub; + ros::Publisher _force_sp_pub; + + /** + * + * Simulates output of attitude data from the FCU + * Equivalent to the mavlink stream ATTITUDE_QUATERNION + * + * */ + void VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg); + + /** + * + * Simulates output of local position data from the FCU + * Equivalent to the mavlink stream LOCAL_POSITION_NED + * + * */ + void VehicleLocalPositionCallback(const vehicle_local_positionConstPtr &msg); + + + /** + * + * Handle incoming mavlink messages ant publish them to ROS ("Mavlink Receiver") + * + * */ + void handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid); + + /** + * + * Handle SET_ATTITUDE_TARGET mavlink messages + * + * */ + void handle_msg_set_attitude_target(const mavlink_message_t *mmsg); + + /** + * + * Handle SET_POSITION_TARGET_LOCAL_NED mavlink messages + * + * */ + void handle_msg_set_position_target_local_ned(const mavlink_message_t *mmsg); + +}; + +} diff --git a/src/systemcmds/bl_update/bl_update.c b/src/systemcmds/bl_update/bl_update.c index ec9269d39..02d2d6f3c 100644 --- a/src/systemcmds/bl_update/bl_update.c +++ b/src/systemcmds/bl_update/bl_update.c @@ -74,7 +74,7 @@ bl_update_main(int argc, char *argv[]) struct stat s; - if (!stat(argv[1], &s)) + if (stat(argv[1], &s) != 0) err(1, "stat %s", argv[1]); /* sanity-check file size */ @@ -214,4 +214,4 @@ setopt(void) errx(1, "option bits setting failed; readback 0x%04x", *optcr); -}
\ No newline at end of file +} diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 0c984d69f..620b0a6f6 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -279,7 +279,14 @@ pwm_main(int argc, char *argv[]) if (pwm_value == 0) usage("no PWM value provided"); - struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + pwm_values.channel_count = servo_count; + /* first get current state before modifying it */ + ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (long unsigned int)&pwm_values); + if (ret != OK) { + errx(ret, "failed get min values"); + } for (unsigned i = 0; i < servo_count; i++) { if (set_mask & 1<<i) { @@ -287,7 +294,6 @@ pwm_main(int argc, char *argv[]) if (print_verbose) warnx("Channel %d: min PWM: %d", i+1, pwm_value); } - pwm_values.channel_count++; } if (pwm_values.channel_count == 0) { @@ -308,7 +314,14 @@ pwm_main(int argc, char *argv[]) if (pwm_value == 0) usage("no PWM value provided"); - struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + pwm_values.channel_count = servo_count; + /* first get current state before modifying it */ + ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&pwm_values); + if (ret != OK) { + errx(ret, "failed get max values"); + } for (unsigned i = 0; i < servo_count; i++) { if (set_mask & 1<<i) { @@ -316,7 +329,6 @@ pwm_main(int argc, char *argv[]) if (print_verbose) warnx("Channel %d: max PWM: %d", i+1, pwm_value); } - pwm_values.channel_count++; } if (pwm_values.channel_count == 0) { @@ -337,15 +349,22 @@ pwm_main(int argc, char *argv[]) if (pwm_value == 0) warnx("reading disarmed value of zero, disabling disarmed PWM"); - struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + pwm_values.channel_count = servo_count; + /* first get current state before modifying it */ + ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&pwm_values); + if (ret != OK) { + errx(ret, "failed get disarmed values"); + } for (unsigned i = 0; i < servo_count; i++) { if (set_mask & 1<<i) { pwm_values.values[i] = pwm_value; - if (print_verbose) - warnx("channel %d: disarmed PWM: %d", i+1, pwm_value); + if (print_verbose) { + warnx("chan %d: disarmed PWM: %d", i+1, pwm_value); + } } - pwm_values.channel_count++; } if (pwm_values.channel_count == 0) { @@ -353,8 +372,9 @@ pwm_main(int argc, char *argv[]) } else { ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values); - if (ret != OK) + if (ret != OK) { errx(ret, "failed setting disarmed values"); + } } exit(0); @@ -366,7 +386,14 @@ pwm_main(int argc, char *argv[]) if (pwm_value == 0) usage("no PWM provided"); - struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + pwm_values.channel_count = servo_count; + /* first get current state before modifying it */ + ret = ioctl(fd, PWM_SERVO_GET_FAILSAFE_PWM, (long unsigned int)&pwm_values); + if (ret != OK) { + errx(ret, "failed get failsafe values"); + } for (unsigned i = 0; i < servo_count; i++) { if (set_mask & 1<<i) { @@ -374,7 +401,6 @@ pwm_main(int argc, char *argv[]) if (print_verbose) warnx("Channel %d: failsafe PWM: %d", i+1, pwm_value); } - pwm_values.channel_count++; } if (pwm_values.channel_count == 0) { diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 9fa52aaaa..682514cb7 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 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 @@ -158,12 +158,13 @@ int test_mathlib(int argc, char *argv[]) } { + warnx("Nonsymmetric matrix operations test"); // test nonsymmetric +, -, +=, -= - float data1[2][3] = {{1,2,3},{4,5,6}}; - float data2[2][3] = {{2,4,6},{8,10,12}}; - float data3[2][3] = {{3,6,9},{12,15,18}}; - + float data1[2][3] = {{1, 2, 3}, {4, 5, 6}}; + float data2[2][3] = {{2, 4, 6}, {8, 10, 12}}; + float data3[2][3] = {{3, 6, 9}, {12, 15, 18}}; + Matrix<2, 3> m1(data1); Matrix<2, 3> m2(data2); Matrix<2, 3> m3(data3); @@ -185,6 +186,7 @@ int test_mathlib(int argc, char *argv[]) } m1 += m2; + if (m1 != m3) { warnx("Matrix<2, 3> += Matrix<2, 3> failed!"); m1.print(); @@ -195,6 +197,7 @@ int test_mathlib(int argc, char *argv[]) m1 -= m2; Matrix<2, 3> m1_orig(data1); + if (m1 != m1_orig) { warnx("Matrix<2, 3> -= Matrix<2, 3> failed!"); m1.print(); @@ -202,7 +205,81 @@ int test_mathlib(int argc, char *argv[]) m1_orig.print(); rc = 1; } - + + } + + { + // test conversion rotation matrix to quaternion and back + math::Matrix<3, 3> R_orig; + math::Matrix<3, 3> R; + math::Quaternion q; + float diff = 0.1f; + float tol = 0.00001f; + + warnx("Quaternion transformation methods test."); + + for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { + for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { + for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { + R_orig.from_euler(roll, pitch, yaw); + q.from_dcm(R_orig); + R = q.to_dcm(); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + if (fabsf(R_orig.data[i][j] - R.data[i][j]) > 0.00001f) { + warnx("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!"); + rc = 1; + } + } + } + } + } + } + + // test against some known values + tol = 0.0001f; + math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f}; + R_orig.identity(); + q.from_dcm(R_orig); + + for (unsigned i = 0; i < 4; i++) { + if (fabsf(q.data[i] - q_true.data[i]) > tol) { + warnx("Quaternion method 'from_dcm()' outside tolerance!"); + rc = 1; + } + } + + q_true.from_euler(0.3f, 0.2f, 0.1f); + q = {0.9833f, 0.1436f, 0.1060f, 0.0343f}; + + for (unsigned i = 0; i < 4; i++) { + if (fabsf(q.data[i] - q_true.data[i]) > tol) { + warnx("Quaternion method 'from_euler()' outside tolerance!"); + rc = 1; + } + } + + q_true.from_euler(-1.5f, -0.2f, 0.5f); + q = {0.7222f, -0.6391f, -0.2386f, 0.1142f}; + + for (unsigned i = 0; i < 4; i++) { + if (fabsf(q.data[i] - q_true.data[i]) > tol) { + warnx("Quaternion method 'from_euler()' outside tolerance!"); + rc = 1; + } + } + + q_true.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3); + q = {0.6830f, 0.1830f, -0.6830f, 0.1830f}; + + for (unsigned i = 0; i < 4; i++) { + if (fabsf(q.data[i] - q_true.data[i]) > tol) { + warnx("Quaternion method 'from_euler()' outside tolerance!"); + rc = 1; + } + } + } return rc; |