forked from Archive/PX4-Autopilot
Merge branch 'master' of https://github.com/mstuettgen/Firmware
This commit is contained in:
commit
62c22582b2
|
@ -46,3 +46,5 @@ Firmware.zip
|
|||
unittests/build
|
||||
*.generated.h
|
||||
.vagrant
|
||||
*.pretty
|
||||
|
||||
|
|
|
@ -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()
|
||||
|
||||
|
||||
|
|
8
Makefile
8
Makefile
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -2,22 +2,27 @@
|
|||
|
||||
set VEHICLE_TYPE rover
|
||||
|
||||
# i dont think AUTOCNF is needed here, can it be removed?
|
||||
|
||||
# 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
|
||||
# PWM Hz - 50 Hz is the normal rate in RC cars, higher rates
|
||||
# may damage analog servos.
|
||||
set PWM_RATE 50
|
||||
|
||||
#PWW default value for "disarmed" mode
|
||||
# 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
|
||||
# PWM range
|
||||
set PWM_MIN 1200
|
||||
set PWM_MAX 1800
|
||||
|
||||
#enable servo output on pins 3 and 4 (steering and thrust)
|
||||
set PWM_OUT 34
|
||||
# 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
|
||||
|
|
|
@ -198,8 +198,12 @@ then
|
|||
|
||||
tone_alarm MLL32CP8MB
|
||||
|
||||
px4io start
|
||||
px4io safety_on
|
||||
if px4io start
|
||||
then
|
||||
if px4io safety_on
|
||||
then
|
||||
fi
|
||||
fi
|
||||
|
||||
if px4io forceupdate 14662 ${IO_FILE}
|
||||
then
|
||||
|
@ -432,7 +436,7 @@ then
|
|||
then
|
||||
if param compare SYS_COMPANION 921600
|
||||
then
|
||||
mavlink start -d /dev/ttyS2 -b 921600 -m onboard
|
||||
mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000
|
||||
fi
|
||||
fi
|
||||
|
||||
|
|
|
@ -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
|
|
@ -18,4 +18,5 @@ astyle \
|
|||
--exclude=EASTL \
|
||||
--add-brackets \
|
||||
--max-code-length=120 \
|
||||
--preserve-date \
|
||||
$*
|
||||
|
|
|
@ -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
|
|
@ -416,7 +416,7 @@ class uploader(object):
|
|||
def upload(self, fw):
|
||||
# Make sure we are doing the right thing
|
||||
if self.board_type != fw.property('board_id'):
|
||||
raise RuntimeError("Firmware not suitable for this board")
|
||||
raise IOError("Firmware not suitable for this board")
|
||||
if self.fw_maxsize < fw.property('image_size'):
|
||||
raise RuntimeError("Firmware image is too large for this board")
|
||||
|
||||
|
@ -564,10 +564,13 @@ try:
|
|||
up.upload(fw)
|
||||
|
||||
except RuntimeError as ex:
|
||||
|
||||
# print the error
|
||||
print("\nERROR: %s" % ex.args)
|
||||
|
||||
except IOError as e:
|
||||
up.close();
|
||||
continue
|
||||
|
||||
finally:
|
||||
# always close the port
|
||||
up.close()
|
||||
|
|
|
@ -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)
|
|
@ -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()
|
|
@ -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>
|
|
@ -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()
|
|
@ -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
|
||||
|
|
@ -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()
|
|
@ -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()
|
|
@ -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>
|
|
@ -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">
|
||||
|
|
|
@ -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>
|
|
@ -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>
|
|
@ -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>
|
|
@ -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>
|
||||
|
||||
|
|
|
@ -1 +1 @@
|
|||
Subproject commit 68a88824741ac26838ec0c8c38d67b51dc84b799
|
||||
Subproject commit 7ae438b86ea983e95af5f092e45a5d0f9d093c74
|
|
@ -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
|
|
@ -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)
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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 -->
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -159,4 +159,6 @@
|
|||
|
||||
#define GPIO_SENSOR_RAIL_RESET GPIOC(13)
|
||||
|
||||
#define GPIO_PERIPHERAL_RAIL_RESET GPIOC(14)
|
||||
|
||||
#endif /* _DRV_GPIO_H */
|
||||
|
|
|
@ -58,6 +58,9 @@
|
|||
/** 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
|
||||
|
||||
|
@ -65,7 +68,7 @@
|
|||
#define OREOLED_ALL_INSTANCES 0xff
|
||||
|
||||
/* maximum command length that can be sent to LEDs */
|
||||
#define OREOLED_CMD_LENGTH_MAX 10
|
||||
#define OREOLED_CMD_LENGTH_MAX 24
|
||||
|
||||
/* enum passed to OREOLED_SET_MODE ioctl()
|
||||
* defined by hardware */
|
||||
|
|
|
@ -296,7 +296,7 @@ MK::init(unsigned motors)
|
|||
_task = task_spawn_cmd("mkblctrl",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 20,
|
||||
2048,
|
||||
1500,
|
||||
(main_t)&MK::task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -368,6 +368,31 @@ OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
|
||||
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);
|
||||
|
|
|
@ -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:
|
||||
|
@ -1674,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)
|
||||
{
|
||||
|
@ -1895,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);
|
||||
|
|
|
@ -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
|
||||
*/
|
||||
|
|
|
@ -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],
|
||||
|
|
|
@ -65,7 +65,7 @@
|
|||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/offboard_control_setpoint.h>
|
||||
#include <uORB/topics/offboard_control_mode.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
|
@ -180,7 +180,7 @@ static struct vehicle_status_s status;
|
|||
static struct actuator_armed_s armed;
|
||||
static struct safety_s safety;
|
||||
static struct vehicle_control_mode_s control_mode;
|
||||
static struct offboard_control_setpoint_s sp_offboard;
|
||||
static struct offboard_control_mode_s offboard_control_mode;
|
||||
|
||||
/* tasks waiting for low prio thread */
|
||||
typedef enum {
|
||||
|
@ -966,7 +966,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
pthread_attr_t commander_low_prio_attr;
|
||||
pthread_attr_init(&commander_low_prio_attr);
|
||||
pthread_attr_setstacksize(&commander_low_prio_attr, 2900);
|
||||
pthread_attr_setstacksize(&commander_low_prio_attr, 2400);
|
||||
|
||||
struct sched_param param;
|
||||
(void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m);
|
||||
|
@ -1016,8 +1016,8 @@ int commander_thread_main(int argc, char *argv[])
|
|||
memset(&sp_man, 0, sizeof(sp_man));
|
||||
|
||||
/* Subscribe to offboard control data */
|
||||
int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
|
||||
memset(&sp_offboard, 0, sizeof(sp_offboard));
|
||||
int offboard_control_mode_sub = orb_subscribe(ORB_ID(offboard_control_mode));
|
||||
memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));
|
||||
|
||||
/* Subscribe to telemetry status topics */
|
||||
int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
|
||||
|
@ -1227,14 +1227,14 @@ int commander_thread_main(int argc, char *argv[])
|
|||
orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
|
||||
}
|
||||
|
||||
orb_check(sp_offboard_sub, &updated);
|
||||
orb_check(offboard_control_mode_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
|
||||
orb_copy(ORB_ID(offboard_control_mode), offboard_control_mode_sub, &offboard_control_mode);
|
||||
}
|
||||
|
||||
if (sp_offboard.timestamp != 0 &&
|
||||
sp_offboard.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
|
||||
if (offboard_control_mode.timestamp != 0 &&
|
||||
offboard_control_mode.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
|
||||
if (status.offboard_control_signal_lost) {
|
||||
status.offboard_control_signal_lost = false;
|
||||
status_changed = true;
|
||||
|
@ -2051,7 +2051,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
led_deinit();
|
||||
buzzer_deinit();
|
||||
close(sp_man_sub);
|
||||
close(sp_offboard_sub);
|
||||
close(offboard_control_mode_sub);
|
||||
close(local_position_sub);
|
||||
close(global_position_sub);
|
||||
close(gps_sub);
|
||||
|
@ -2426,56 +2426,30 @@ set_control_mode()
|
|||
control_mode.flag_control_auto_enabled = false;
|
||||
control_mode.flag_control_offboard_enabled = true;
|
||||
|
||||
switch (sp_offboard.mode) {
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
|
||||
control_mode.flag_control_rates_enabled = true;
|
||||
control_mode.flag_control_attitude_enabled = false;
|
||||
control_mode.flag_control_altitude_enabled = false;
|
||||
control_mode.flag_control_climb_rate_enabled = false;
|
||||
control_mode.flag_control_position_enabled = false;
|
||||
control_mode.flag_control_velocity_enabled = false;
|
||||
break;
|
||||
/*
|
||||
* The control flags depend on what is ignored according to the offboard control mode topic
|
||||
* Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position)
|
||||
*/
|
||||
control_mode.flag_control_rates_enabled = !offboard_control_mode.ignore_bodyrate ||
|
||||
!offboard_control_mode.ignore_attitude ||
|
||||
!offboard_control_mode.ignore_position ||
|
||||
!offboard_control_mode.ignore_velocity ||
|
||||
!offboard_control_mode.ignore_acceleration_force;
|
||||
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
|
||||
control_mode.flag_control_rates_enabled = true;
|
||||
control_mode.flag_control_attitude_enabled = true;
|
||||
control_mode.flag_control_altitude_enabled = false;
|
||||
control_mode.flag_control_climb_rate_enabled = false;
|
||||
control_mode.flag_control_position_enabled = false;
|
||||
control_mode.flag_control_velocity_enabled = false;
|
||||
break;
|
||||
control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude ||
|
||||
!offboard_control_mode.ignore_position ||
|
||||
!offboard_control_mode.ignore_velocity ||
|
||||
!offboard_control_mode.ignore_acceleration_force;
|
||||
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
|
||||
control_mode.flag_control_rates_enabled = true;
|
||||
control_mode.flag_control_attitude_enabled = false;
|
||||
control_mode.flag_control_force_enabled = true;
|
||||
control_mode.flag_control_altitude_enabled = false;
|
||||
control_mode.flag_control_climb_rate_enabled = false;
|
||||
control_mode.flag_control_position_enabled = false;
|
||||
control_mode.flag_control_velocity_enabled = false;
|
||||
break;
|
||||
control_mode.flag_control_velocity_enabled = !offboard_control_mode.ignore_velocity ||
|
||||
!offboard_control_mode.ignore_position;
|
||||
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
|
||||
control_mode.flag_control_rates_enabled = true;
|
||||
control_mode.flag_control_attitude_enabled = true;
|
||||
control_mode.flag_control_altitude_enabled = true;
|
||||
control_mode.flag_control_climb_rate_enabled = true;
|
||||
control_mode.flag_control_position_enabled = true;
|
||||
control_mode.flag_control_velocity_enabled = true;
|
||||
//XXX: the flags could depend on sp_offboard.ignore
|
||||
break;
|
||||
control_mode.flag_control_climb_rate_enabled = !offboard_control_mode.ignore_velocity ||
|
||||
!offboard_control_mode.ignore_position;
|
||||
|
||||
default:
|
||||
control_mode.flag_control_rates_enabled = false;
|
||||
control_mode.flag_control_attitude_enabled = false;
|
||||
control_mode.flag_control_altitude_enabled = false;
|
||||
control_mode.flag_control_climb_rate_enabled = false;
|
||||
control_mode.flag_control_position_enabled = false;
|
||||
control_mode.flag_control_velocity_enabled = false;
|
||||
}
|
||||
control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position;
|
||||
|
||||
control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position;
|
||||
|
||||
break;
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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>
|
||||
*/
|
||||
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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>
|
||||
|
|
|
@ -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, ¶m);
|
||||
|
||||
pthread_attr_setstacksize(&receiveloop_attr, 2900);
|
||||
pthread_attr_setstacksize(&receiveloop_attr, 2100);
|
||||
pthread_t thread;
|
||||
pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -51,7 +51,7 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
|
|||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
||||
MODULE_STACKSIZE = 1024
|
||||
MODULE_STACKSIZE = 1200
|
||||
|
||||
EXTRACXXFLAGS = -Weffc++ -Wno-attributes -Wno-packed
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -1015,12 +1015,19 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
|
|||
reset_yaw_sp = true;
|
||||
}
|
||||
|
||||
/* publish attitude setpoint */
|
||||
if (_att_sp_pub != nullptr) {
|
||||
_att_sp_pub->publish(_att_sp_msg);
|
||||
/* 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>();
|
||||
} else {
|
||||
_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
|
||||
}
|
||||
}
|
||||
|
||||
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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");
|
||||
|
|
|
@ -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.
|
||||
*
|
||||
|
|
|
@ -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 */
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
|
@ -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'");
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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>
|
||||
|
|
|
@ -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");
|
||||
|
|
|
@ -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;
|
||||
|
||||
};
|
||||
|
|
|
@ -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();
|
||||
}
|
|
@ -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;
|
||||
};
|
|
@ -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();
|
||||
}
|
|
@ -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;
|
||||
};
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
};
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
}
|
|
@ -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);
|
||||
|
||||
};
|
||||
|
||||
}
|
Loading…
Reference in New Issue