fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 13:28:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1 from PX4/master

Browse Source 
From original
This commit is contained in:
Marco Bauer
2015-03-06 15:32:30 +01:00
parent 3de63dee6c 9d4b4ab049
commit 4a977af870
98 changed files with 4523 additions and 931 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+2
View File
@@ -46,3 +46,5 @@ Firmware.zip
unittests/build
*.generated.h
.vagrant
*.pretty
.travis.yml
+5 -1
View File
@@ -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
CMakeLists.txt
+45 -3
View File
@@ -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()
Makefile
+6 -2
View File
@@ -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
README.md
+1 -1
View File
@@ -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 ###
ROMFS/px4fmu_common/init.d/50001_rover
+2 -2
View File
@@ -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
ROMFS/px4fmu_common/init.d/rc.mc_apps
+9 -3
View File
@@ -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
ROMFS/px4fmu_common/init.d/rc.rover_defaults
+22 -7
View File
@@ -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
ROMFS/px4fmu_common/init.d/rcS
+12 -3
View File
@@ -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
Tools/check_code_style.sh
Executable
+31
View File
@@ -0,0 +1,31 @@
#!/usr/bin/env bash
set -eu
failed=0
for fn in $(find . -path './src/lib/uavcan' -prune -o \
-path './src/lib/mathlib/CMSIS' -prune -o \
-path './src/modules/attitude_estimator_ekf/codegen/' -prune -o \
-path './NuttX' -prune -o \
-path './Build' -prune -o \
-path './mavlink' -prune -o \
-path './unittests/gtest' -prune -o \
-path './unittests/build' -prune -o \
-name '*.c' -o -name '*.cpp' -o -name '*.hpp' -o -name '*.h' -type f); do
if [ -f "$fn" ];
then
./Tools/fix_code_style.sh --quiet < $fn > $fn.pretty
diffsize=$(diff -y --suppress-common-lines $fn $fn.pretty | wc -l)
rm -f $fn.pretty
if [ $diffsize -ne 0 ]; then
failed=1
echo $diffsize $fn
fi
fi
done
if [ $failed -eq 0 ]; then
echo "Format checks passed"
exit 0
else
echo "Format checks failed; please run ./Tools/fix_code_style.sh on each file"
exit 1
fi
Tools/fix_code_style.sh
+1
View File
@@ -18,4 +18,5 @@ astyle \
--exclude=EASTL \
--add-brackets \
--max-code-length=120 \
--preserve-date \
$*
Tools/pre-commit
Executable
+26
View File
@@ -0,0 +1,26 @@
#!/bin/sh
if git rev-parse --verify HEAD >/dev/null 2>&1
then
against=HEAD
else
# Initial commit: diff against an empty tree object
against=4b825dc642cb6eb9a060e54bf8d69288fbee4904
fi
# Redirect output to stderr.
exec 1>&2
CHANGED_FILES=`git diff --cached --name-only --diff-filter=ACM $against | grep '\.c\|\.cpp\|\.h\|\.hpp'`
FAILED=0
if [ ! -z "$CHANGED_FILES" -a "$CHANGED_FILES" != " " ]; then
for FILE in $CHANGED_FILES; do
./Tools/fix_code_style.sh --quiet < $FILE > $FILE.pretty
diff -u $FILE $FILE.pretty || FAILED=1
rm -f $FILE.pretty
if [ $FAILED -ne 0 ]; then
echo "There are code formatting errors. Please fix them by running ./Tools/fix_code_style.sh $FILE"
exit $FAILED
fi
done
fi
exit 0
Tools/px_uploader.py
+74 -62
View File
@@ -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
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:
for port in portlist:
#print("Trying %s" % port)
#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)
# 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
# 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))
# 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:
except RuntimeError as ex:
# print the error
print("\nERROR: %s" % ex.args)
# print the error
print("\nERROR: %s" % ex.args)
except IOError as e:
up.close();
continue
finally:
# always close the port
up.close()
finally:
# always close the port
up.close()
# we could loop here if we wanted to wait for more boards...
sys.exit(0)
# we could loop here if we wanted to wait for more boards...
sys.exit(0)
# Delay retries to < 20 Hz to prevent spin-lock from hogging the CPU
time.sleep(0.05)
# CTRL+C aborts the upload/spin-lock by interrupt mechanics
except KeyboardInterrupt:
print("\n Upload aborted by user.")
sys.exit(0)
integrationtests/demo_tests/direct_manual_input_test.py
Executable
+87
View File
@@ -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)
integrationtests/demo_tests/direct_offboard_posctl_test.py
Executable
+167
View File
@@ -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()
integrationtests/demo_tests/direct_tests.launch
+18
View File
@@ -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>
integrationtests/demo_tests/flight_path_assertion.py
+171
View File
@@ -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()
integrationtests/demo_tests/manual_input.py
Executable
+175
View File
@@ -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
integrationtests/demo_tests/mavros_offboard_attctl_test.py
Executable
+143
View File
@@ -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()
integrationtests/demo_tests/mavros_offboard_posctl_test.py
Executable
+174
View File
@@ -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()
integrationtests/demo_tests/mavros_tests.launch
+19
View File
@@ -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>
launch/gazebo_ardrone_empty_world.launch
+1 -1
View File
@@ -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">
launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch
+9
View File
@@ -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>
launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch
+9
View File
@@ -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>
launch/mavros_sitl.launch
+21
View File
@@ -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>
launch/multicopter.launch
+1
View File
@@ -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>
makefiles/config_px4fmu-v2_default.mk
+1
View File
@@ -43,6 +43,7 @@ MODULES += drivers/frsky_telemetry
MODULES += modules/sensors
MODULES += drivers/mkblctrl
MODULES += drivers/px4flow
MODULES += drivers/oreoled
#
# System commands
mavlink/include/mavlink/v1.0
+1 -1
Submodule mavlink/include/mavlink/v1.0 updated: 68a8882474...7ae438b86e
msg/offboard_control_mode.msg
+9
View File
@@ -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
msg/pwm_input.msg
+5
View File
@@ -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
msg/vehicle_force_setpoint.msg
+8
View File
@@ -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)
nuttx-configs/aerocore/nsh/defconfig
+3 -3
View File
@@ -332,7 +332,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717
CONFIG_DRAM_START=0x20000000
CONFIG_DRAM_SIZE=262144
CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
CONFIG_ARCH_INTERRUPTSTACK=4096
CONFIG_ARCH_INTERRUPTSTACK=1500
#
# Boot options
@@ -415,8 +415,8 @@ CONFIG_PREALLOC_TIMERS=50
#
# Stack and heap information
#
CONFIG_IDLETHREAD_STACKSIZE=6000
CONFIG_USERMAIN_STACKSIZE=4096
CONFIG_IDLETHREAD_STACKSIZE=1000
CONFIG_USERMAIN_STACKSIZE=3000
CONFIG_PTHREAD_STACK_MIN=512
CONFIG_PTHREAD_STACK_DEFAULT=2048
nuttx-configs/px4fmu-v1/nsh/defconfig
+4 -3
View File
@@ -325,7 +325,8 @@ CONFIG_BOARD_LOOPSPERMSEC=16717
CONFIG_DRAM_START=0x20000000
CONFIG_DRAM_SIZE=196608
CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
CONFIG_ARCH_INTERRUPTSTACK=4096
# The actual usage is 420 bytes
CONFIG_ARCH_INTERRUPTSTACK=1500
#
# Boot options
@@ -416,8 +417,8 @@ CONFIG_PREALLOC_TIMERS=50
#
# Stack and heap information
#
CONFIG_IDLETHREAD_STACKSIZE=3500
CONFIG_USERMAIN_STACKSIZE=2600
CONFIG_IDLETHREAD_STACKSIZE=1000
CONFIG_USERMAIN_STACKSIZE=3000
CONFIG_PTHREAD_STACK_MIN=512
CONFIG_PTHREAD_STACK_DEFAULT=2048
nuttx-configs/px4fmu-v2/nsh/defconfig
+3 -3
View File
@@ -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
package.xml
+5
View File
@@ -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 -->
src/drivers/batt_smbus/batt_smbus.cpp
-3
View File
@@ -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
src/drivers/boards/px4fmu-v2/board_config.h
+5
View File
@@ -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
****************************************************************************************************/
src/drivers/drv_gpio.h
+2
View File
@@ -159,4 +159,6 @@
#define GPIO_SENSOR_RAIL_RESET GPIOC(13)
#define GPIO_PERIPHERAL_RAIL_RESET GPIOC(14)
#endif /* _DRV_GPIO_H */
src/drivers/drv_oreoled.h
+149
View File
@@ -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;
src/drivers/drv_pwm_input.h
+51
View File
@@ -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
src/drivers/gps/ubx.cpp
+1 -1
View File
@@ -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>
src/drivers/mpu6000/mpu6000.cpp
+36 -9
View File
@@ -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);
}
src/drivers/oreoled/module.mk
+8
View File
@@ -0,0 +1,8 @@
#
# Oreo LED driver
#
MODULE_COMMAND = oreoled
SRCS = oreoled.cpp
MAXOPTIMIZATION = -Os
src/drivers/oreoled/oreoled.cpp
+700
View File
@@ -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)&macro_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);
}
src/drivers/pwm_input/module.mk
+41
View File
@@ -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
src/drivers/pwm_input/pwm_input.cpp
+603
View File
@@ -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;
}
src/drivers/px4fmu/fmu.cpp
+71
View File
@@ -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)
@@ -1666,6 +1702,24 @@ sensor_reset(int ms)
close(fd);
}
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)
{
@@ -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);
src/lib/mathlib/math/Quaternion.hpp
+61 -6
View File
@@ -115,6 +115,35 @@ public:
return Vector<3>(&data[1]);
}
/**
* inverse of quaternion
*/
math::Quaternion inverse() {
Quaternion res;
memcpy(res.data,data,sizeof(res.data));
res.data[1] = -res.data[1];
res.data[2] = -res.data[2];
res.data[3] = -res.data[3];
return res;
}
/**
* rotate vector by quaternion
*/
Vector<3> rotate(const Vector<3> &w) {
Quaternion q_w; // extend vector to quaternion
Quaternion q = {data[0],data[1],data[2],data[3]};
Quaternion q_rotated; // quaternion representation of rotated vector
q_w(0) = 0;
q_w(1) = w.data[0];
q_w(2) = w.data[1];
q_w(3) = w.data[2];
q_rotated = q*q_w*q.inverse();
Vector<3> res = {q_rotated.data[1],q_rotated.data[2],q_rotated.data[3]};
return res;
}
/**
* set quaternion to rotation defined by euler angles
*/
@@ -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;
}
}
/**
src/modules/commander/accelerometer_calibration.cpp
+3 -1
View File
@@ -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],
src/modules/commander/commander.cpp
+30 -56
View File
@@ -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, &param);
@@ -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;
src/modules/commander/mag_calibration.cpp
+4 -1
View File
@@ -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;
src/modules/commander/state_machine_helper.cpp
+2 -2
View File
@@ -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>
*/
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+8 -10
View File
@@ -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)
src/modules/fw_att_control/fw_att_control_main.cpp
+20 -15
View File
@@ -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);
}
}
}
src/modules/fw_att_control/fw_att_control_params.c
+6 -6
View File
@@ -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
src/modules/land_detector/FixedwingLandDetector.cpp
+2
View File
@@ -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()
src/modules/land_detector/land_detector_main.cpp
+5 -4
View File
@@ -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;
}
src/modules/mavlink/mavlink_main.cpp
+1 -1
View File
@@ -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);
src/modules/mavlink/mavlink_messages.cpp
-1
View File
@@ -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>
src/modules/mavlink/mavlink_receiver.cpp
+151 -126
View File
@@ -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));
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);
/* 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;
}
offboard_control_mode.timestamp = hrt_absolute_time();
/* 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_sp.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;
@@ -698,6 +651,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)
{
@@ -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.
*/
offboard_control_sp.timestamp = hrt_absolute_time();
offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode
/*
* 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 (_offboard_control_sp_pub < 0) {
_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
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 = 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_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, &param);
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);
src/modules/mavlink/mavlink_receiver.h
+5 -3
View File
@@ -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;
src/modules/mavlink/module.mk
+1 -1
View File
@@ -51,7 +51,7 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MAXOPTIMIZATION = -Os
MODULE_STACKSIZE = 1024
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++ -Wno-attributes -Wno-packed
src/modules/mc_att_control/mc_att_control_main.cpp
+1 -1
View File
@@ -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);
src/modules/mc_att_control_multiplatform/mc_att_control.cpp
-23
View File
@@ -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);
src/modules/mc_att_control_multiplatform/mc_att_control.h
-3
View File
@@ -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;
src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
+12 -107
View File
@@ -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)
src/modules/mc_att_control_multiplatform/mc_att_control_base.h
-5
View File
@@ -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;
src/modules/mc_att_control_multiplatform/mc_att_control_params.c
-29
View File
@@ -189,35 +189,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
*
src/modules/mc_att_control_multiplatform/mc_att_control_params.h
-3
View File
@@ -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
src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp
+1 -1
View File
@@ -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);
src/modules/mc_pos_control/mc_pos_control_main.cpp
+17 -10
View File
@@ -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);
src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
+117 -36
View File
@@ -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;
}
src/modules/mc_pos_control_multiplatform/mc_pos_control.h
+19 -11
View File
@@ -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;
src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c
+29
View File
@@ -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);
src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h
+3
View File
@@ -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
src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp
+1 -1
View File
@@ -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);
src/modules/navigator/rtl_params.c
+1 -1
View File
@@ -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
src/modules/position_estimator_inav/position_estimator_inav_params.c
+14
View File
@@ -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");
src/modules/sensors/sensor_params.c
+33 -3
View File
@@ -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
@@ -993,6 +993,36 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */
/**
* RC channel count
*
* This parameter is used by Ground Station software to save the number
* of channels which were used during RC calibration. It is only meant
* for ground station use.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_CHAN_CNT, 0);
/**
* RC mode switch threshold automaic distribution
*
* This parameter is used by Ground Station software to specify whether
* the threshold values for flight mode switches were automatically calculated.
* 0 indicates that the threshold values were set by the user. Any other value
* indicates that the threshold value where automatically set by the ground
* station software. It is only meant for ground station use.
*
* @min 0
* @max 1
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_TH_USER, 1);
/**
* Roll control channel mapping.
*
src/modules/sensors/sensors.cpp
+12 -2
View File
@@ -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 */
src/modules/uORB/objects_common.cpp
+2 -2
View File
@@ -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);
src/modules/uORB/topics/offboard_control_setpoint.h
-276
View File
@@ -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
src/modules/uORB/uORB.cpp
+115 -10
View File
@@ -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
@@ -955,13 +1046,27 @@ uorb_main(int argc, char *argv[])
if (!strcmp(argv[1], "test"))
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'");
}
/*
src/modules/vtol_att_control/vtol_att_control_main.cpp
+28 -18
View File
@@ -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);
/* 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);
}
}
}
}
@@ -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);
}
}
}
}
src/platforms/px4_includes.h
+4
View File
@@ -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>
src/platforms/ros/nodes/commander/commander.cpp
+76 -1
View File
@@ -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");
src/platforms/ros/nodes/commander/commander.h
+16
View File
@@ -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
{
@@ -58,6 +59,11 @@ 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)
*/
@@ -65,8 +71,15 @@ protected:
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;
};
src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp
+85
View File
@@ -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();
}
src/modules/uORB/topics/vehicle_force_setpoint.h → src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h
+19 -26
View File
@@ -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 "ros/ros.h"
#include <px4/manual_control_setpoint.h>
#include "../uORB.h"
class DemoOffboardAttitudeSetpoints
{
public:
DemoOffboardAttitudeSetpoints();
/**
* @addtogroup topics
* @{
*/
~DemoOffboardAttitudeSetpoints() {}
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 */
int main();
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vehicle_force_setpoint);
#endif
protected:
ros::NodeHandle _n;
ros::Publisher _attitude_sp_pub;
ros::Publisher _thrust_sp_pub;
};
src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp
+77
View File
@@ -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();
}
src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h
+57
View File
@@ -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;
};
src/platforms/ros/nodes/manual_input/manual_input.cpp
+15 -2
View File
@@ -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;
}
src/platforms/ros/nodes/manual_input/manual_input.h
+2
View File
@@ -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
};
src/platforms/ros/nodes/mavlink/mavlink.cpp
+296
View File
@@ -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);
}
}
src/platforms/ros/nodes/mavlink/mavlink.h
+113
View File
@@ -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);
};
}
src/systemcmds/bl_update/bl_update.c
+2 -2
View File
@@ -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);
}
}
src/systemcmds/pwm/pwm.c
+37 -11
View File
@@ -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) {
src/systemcmds/tests/test_mathlib.cpp
+83 -6
View File
@@ -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;