diff options
| -rwxr-xr-x | Tools/boot_now.py | 59 | ||||
| -rwxr-xr-x | Tools/px_uploader.py | 11 | ||||
| -rwxr-xr-x | integrationtests/demo_tests/direct_manual_input_test.py | 23 | ||||
| -rwxr-xr-x | integrationtests/demo_tests/direct_offboard_posctl_test.py | 56 | ||||
| -rw-r--r-- | integrationtests/demo_tests/direct_tests.launch | 2 | ||||
| -rw-r--r-- | integrationtests/demo_tests/flight_path_assertion.py | 110 | ||||
| -rwxr-xr-x | integrationtests/demo_tests/manual_input.py | 24 | ||||
| -rwxr-xr-x | integrationtests/demo_tests/mavros_offboard_attctl_test.py | 65 | ||||
| -rwxr-xr-x | integrationtests/demo_tests/mavros_offboard_posctl_test.py | 71 | ||||
| -rw-r--r-- | integrationtests/demo_tests/mavros_tests.launch | 2 | ||||
| -rw-r--r-- | launch/gazebo_ardrone_empty_world.launch | 2 | ||||
| -rw-r--r-- | nuttx-configs/px4fmu-v2/scripts/ld.script | 8 | ||||
| -rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 16 | ||||
| -rw-r--r-- | src/modules/mc_att_control/mc_att_control_params.c | 30 | ||||
| -rw-r--r-- | src/platforms/ros/nodes/mavlink/mavlink.cpp | 54 | ||||
| -rw-r--r-- | src/platforms/ros/nodes/mavlink/mavlink.h | 2 |
16 files changed, 321 insertions, 214 deletions
diff --git a/Tools/boot_now.py b/Tools/boot_now.py new file mode 100755 index 000000000..5a9e608da --- /dev/null +++ b/Tools/boot_now.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2012-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. +# +############################################################################ + +# send BOOT command to a device + +import argparse +import serial, sys + +from sys import platform as _platform + +# Parse commandline arguments +parser = argparse.ArgumentParser(description="Send boot command to a device") +parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port") +parser.add_argument('port', action="store", help="Serial port(s) to which the FMU may be attached") +args = parser.parse_args() + +REBOOT = b'\x30' +EOC = b'\x20' + +print("Sending reboot to %s" % args.port) +try: + port = serial.Serial(args.port, args.baud, timeout=0.5) +except Exception: + print("Unable to open %s" % args.port) + sys.exit(1) +port.write(REBOOT + EOC) +port.close() +sys.exit(0) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 43a116745..3cdd9d1a6 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -161,6 +161,7 @@ class uploader(object): GET_OTP = b'\x2a' # rev4+ , get a word from OTP area GET_SN = b'\x2b' # rev4+ , get a word from SN area GET_CHIP = b'\x2c' # rev5+ , get chip version + SET_BOOT_DELAY = b'\x2d' # rev5+ , set boot delay REBOOT = b'\x30' INFO_BL_REV = b'\x01' # bootloader protocol revision @@ -405,6 +406,12 @@ class uploader(object): raise RuntimeError("Program CRC failed") self.__drawProgressBar(label, 100, 100) + def __set_boot_delay(self, boot_delay): + self.__send(uploader.SET_BOOT_DELAY + + struct.pack("b", boot_delay) + + uploader.EOC) + self.__getSync() + # get basic data about the board def identify(self): # make sure we are in sync before starting @@ -472,6 +479,9 @@ class uploader(object): else: self.__verify_v3("Verify ", fw) + if args.boot_delay is not None: + self.__set_boot_delay(args.boot_delay) + print("\nRebooting.\n") self.__reboot() self.port.close() @@ -501,6 +511,7 @@ parser = argparse.ArgumentParser(description="Firmware uploader for the PX autop parser.add_argument('--port', action="store", required=True, help="Serial port(s) to which the FMU may be attached") parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200), only required for true serial ports.") parser.add_argument('--force', action='store_true', default=False, help='Override board type check and continue loading') +parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash') parser.add_argument('firmware', action="store", help="Firmware file to be uploaded") args = parser.parse_args() diff --git a/integrationtests/demo_tests/direct_manual_input_test.py b/integrationtests/demo_tests/direct_manual_input_test.py index b037d842f..394500e94 100755 --- a/integrationtests/demo_tests/direct_manual_input_test.py +++ b/integrationtests/demo_tests/direct_manual_input_test.py @@ -37,7 +37,6 @@ # PKG = 'px4' -import sys import unittest import rospy @@ -50,14 +49,18 @@ from manual_input import ManualInput # class ManualInputTest(unittest.TestCase): + def setUp(self): + self.actuator_status = actuator_armed() + self.control_mode = vehicle_control_mode() + # # General callback functions used in tests # def actuator_armed_callback(self, data): - self.actuatorStatus = data + self.actuator_status = data def vehicle_control_mode_callback(self, data): - self.controlMode = data + self.control_mode = data # # Test arming @@ -67,19 +70,19 @@ class ManualInputTest(unittest.TestCase): rospy.Subscriber('iris/actuator_armed', actuator_armed, self.actuator_armed_callback) rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - man = ManualInput() + man_in = ManualInput() # Test arming - man.arm() - self.assertEquals(self.actuatorStatus.armed, True, "did not arm") + man_in.arm() + self.assertEquals(self.actuator_status.armed, True, "did not arm") # Test posctl - man.posctl() - self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") + man_in.posctl() + self.assertTrue(self.control_mode.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") + man_in.offboard() + self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") if __name__ == '__main__': diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index 18239e926..d61eec063 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -37,7 +37,6 @@ # PKG = 'px4' -import sys import unittest import rospy @@ -46,11 +45,8 @@ 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 @@ -68,31 +64,34 @@ class DirectOffboardPosctlTest(unittest.TestCase): rospy.init_node('test_node', anonymous=True) rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback) - self.pubSpt = rospy.Publisher('iris/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) + self.pub_spt = rospy.Publisher('iris/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) self.rate = rospy.Rate(10) # 10hz + self.has_pos = False + self.local_position = vehicle_local_position() + self.control_mode = vehicle_control_mode() def tearDown(self): - if (self.fpa): + if self.fpa: self.fpa.stop() # # General callback functions used in tests # def position_callback(self, data): - self.hasPos = True - self.localPosition = data + self.has_pos = True + self.local_position = data def vehicle_control_mode_callback(self, data): - self.controlMode = data + self.control_mode = 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)) + rospy.logdebug("current position %f, %f, %f" % (self.local_position.x, self.local_position.y, self.local_position.z)) desired = np.array((x, y, z)) - pos = np.array((self.localPosition.x, self.localPosition.y, self.localPosition.z)) + pos = np.array((self.local_position.x, self.local_position.y, self.local_position.z)) return linalg.norm(desired - pos) < offset def reach_position(self, x, y, z, timeout): @@ -105,12 +104,12 @@ class DirectOffboardPosctlTest(unittest.TestCase): pos.position_valid = True stp = position_setpoint_triplet() stp.current = pos - self.pubSpt.publish(stp) + self.pub_spt.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)): + while count < timeout: + if self.is_at_position(pos.x, pos.y, pos.z, 0.5): break count = count + 1 self.rate.sleep() @@ -121,22 +120,23 @@ class DirectOffboardPosctlTest(unittest.TestCase): # Test offboard position control # def test_posctl(self): - manIn = ManualInput() + man_in = 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") + man_in.arm() + man_in.offboard() + self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set") + self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") + self.assertTrue(self.control_mode.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)) + (0, 0, 0), + (0, 0, -2), + (2, 2, -2), + (2, -2, -2), + (-2, -2, -2), + (2, 2, -2)) # flight path assertion self.fpa = FlightPathAssertion(positions, 1, 0) @@ -147,12 +147,10 @@ class DirectOffboardPosctlTest(unittest.TestCase): 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 + while count < timeout: + if not self.is_at_position(2, 2, -2, 0.5): break count = count + 1 self.rate.sleep() diff --git a/integrationtests/demo_tests/direct_tests.launch b/integrationtests/demo_tests/direct_tests.launch index 1a7d843fd..f332332cf 100644 --- a/integrationtests/demo_tests/direct_tests.launch +++ b/integrationtests/demo_tests/direct_tests.launch @@ -2,7 +2,7 @@ <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="enable_ground_truth" default="true"/> <arg name="log_file" default="iris"/> <include file="$(find px4)/launch/gazebo_iris_empty_world.launch"> diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/demo_tests/flight_path_assertion.py index 485de8c41..244324cd0 100644 --- a/integrationtests/demo_tests/flight_path_assertion.py +++ b/integrationtests/demo_tests/flight_path_assertion.py @@ -35,7 +35,6 @@ # # @author Andreas Antener <andreas@uaventure.com> # -import sys import rospy import threading @@ -48,7 +47,6 @@ 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. @@ -62,30 +60,52 @@ class FlightPathAssertion(threading.Thread): # TODO: yaw validation # TODO: fail main test thread # - def __init__(self, positions, tunnelRadius = 1, yawOffset = 0.2): + def __init__(self, positions, tunnelRadius=1, yaw_offset=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) + rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback) + self.spawn_model = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) + self.set_model_state = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) + self.delete_model = rospy.ServiceProxy('gazebo/delete_model', DeleteModel) self.positions = positions - self.tunnelRadius = tunnelRadius - self.yawOffset = yawOffset - self.hasPos = False - self.shouldStop = False + self.tunnel_radius = tunnelRadius + self.yaw_offset = yaw_offset + self.has_pos = False + self.should_stop = False self.center = positions[0] - self.endOfSegment = False + self.end_of_segment = False self.failed = False + self.local_position = vehicle_local_position def position_callback(self, data): - self.hasPos = True - self.localPosition = data + self.has_pos = True + self.local_position = 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(), "") + self.delete_model("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.tunnel_radius + + self.spawn_model("indicator", xml, "", Pose(), "") def position_indicator(self): state = SetModelState() @@ -97,7 +117,7 @@ class FlightPathAssertion(threading.Thread): state.pose = pose state.twist = Twist() state.reference_frame = "" - self.setModelState(state) + self.set_model_state(state) def distance_to_line(self, a, b, pos): v = b - a @@ -111,7 +131,7 @@ class FlightPathAssertion(threading.Thread): c2 = np.dot(v, v) if c2 <= c1: # after b self.center = b - self.endOfSegment = True + self.end_of_segment = True return linalg.norm(pos - b) x = c1 / c2 @@ -120,52 +140,58 @@ class FlightPathAssertion(threading.Thread): return linalg.norm(pos - l) def stop(self): - self.shouldStop = True + self.should_stop = True def run(self): rate = rospy.Rate(10) # 10hz self.spawn_indicator() current = 0 - - while not self.shouldStop: - if (self.hasPos): + count = 0 + while not self.should_stop: + if self.has_pos: # calculate distance to line segment between first two points - # if distances > tunnelRadius + # if distances > tunnel_radius # exit with error - # advance current pos if not on the line anymore or distance to next point < tunnelRadius + # advance current pos if not on the line anymore or distance to next point < tunnel_radius # 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])) + pos = np.array((self.local_position.x, + self.local_position.y, + self.local_position.z)) + a_pos = np.array((self.positions[current][0], + self.positions[current][1], + self.positions[current][2])) + b_pos = 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) + dist = self.distance_to_line(a_pos, b_pos, pos) + b_dist = linalg.norm(pos - b_pos) - rospy.logdebug("distance to line: %f, distance to end: %f" % (dist, bDist)) + rospy.logdebug("distance to line: %f, distance to end: %f" % (dist, b_dist)) - if (dist > self.tunnelRadius): - msg = "left tunnel at position (%f, %f, %f)" % (self.localPosition.x, self.localPosition.y, self.localPosition.z) + if dist > self.tunnel_radius: + msg = "left tunnel at position (%f, %f, %f)" % (self.local_position.x, self.local_position.y, self.local_position.z) rospy.logerr(msg) self.failed = True break - if (self.endOfSegment or bDist < self.tunnelRadius): + if self.end_of_segment or b_dist < self.tunnel_radius: rospy.loginfo("next segment") - self.endOfSegment = False + self.end_of_segment = False current = current + 1 - if (current == len(self.positions) - 1): + if current == len(self.positions) - 1: rospy.loginfo("no more positions") break rate.sleep() + count = count + 1 + + if count > 10 and not self.has_pos: # no position after 1 sec + rospy.logerr("no position") + self.failed = True + break diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index 7072464de..167221285 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/integrationtests/demo_tests/manual_input.py @@ -35,7 +35,6 @@ # # @author Andreas Antener <andreas@uaventure.com> # -import sys import rospy from px4.msg import manual_control_setpoint @@ -46,17 +45,12 @@ 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: +class ManualInput(object): def __init__(self): rospy.init_node('test_node', anonymous=True) - self.pubMcsp = rospy.Publisher('iris/manual_control_setpoint', manual_control_setpoint, queue_size=10) - self.pubOff = rospy.Publisher('iris/offboard_control_mode', offboard_control_mode, queue_size=10) - self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) + self.pub_mcsp = rospy.Publisher('iris/manual_control_setpoint', manual_control_setpoint, queue_size=10) + self.pub_off = rospy.Publisher('iris/offboard_control_mode', offboard_control_mode, queue_size=10) def arm(self): rate = rospy.Rate(10) # 10hz @@ -81,9 +75,7 @@ class ManualInput: 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) + self.pub_mcsp.publish(pos) rate.sleep() count = count + 1 @@ -93,7 +85,7 @@ class ManualInput: rospy.loginfo("arming") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) + self.pub_mcsp.publish(pos) rate.sleep() count = count + 1 @@ -118,7 +110,7 @@ class ManualInput: rospy.loginfo("triggering posctl") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) + self.pub_mcsp.publish(pos) rate.sleep() count = count + 1 @@ -147,7 +139,7 @@ class ManualInput: rospy.loginfo("setting offboard mode") time = rospy.get_rostime().now() mode.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubOff.publish(mode) + self.pub_off.publish(mode) rate.sleep() count = count + 1 @@ -169,7 +161,7 @@ class ManualInput: rospy.loginfo("triggering offboard") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) + self.pub_mcsp.publish(pos) rate.sleep() count = count + 1 diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index 92cdf1e0a..30e9fe9ba 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -37,29 +37,20 @@ # 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 +# Tests flying a path in offboard control by sending attitude and thrust 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) +# For the test to be successful it needs to cross a certain boundary in time. # class MavrosOffboardAttctlTest(unittest.TestCase): @@ -68,45 +59,27 @@ class MavrosOffboardAttctlTest(unittest.TestCase): rospy.wait_for_service('iris/mavros/cmd/arming', 30) rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) - self.pubAtt = rospy.Publisher('iris/mavros/setpoint/attitude', PoseStamped, queue_size=10) - self.pubThr = rospy.Publisher('iris/mavros/setpoint/att_throttle', Float64, queue_size=10) - self.cmdArm = rospy.ServiceProxy("iris/mavros/cmd/arming", CommandBool) + self.pub_att = rospy.Publisher('iris/mavros/setpoint/attitude', PoseStamped, queue_size=10) + self.pub_thr = rospy.Publisher('iris/mavros/setpoint/att_throttle', Float64, queue_size=10) self.rate = rospy.Rate(10) # 10hz - self.rateSec = rospy.Rate(1) - self.hasPos = False - self.controlMode = vehicle_control_mode() + self.has_pos = False + self.control_mode = vehicle_control_mode() + self.local_position = PoseStamped() # # General callback functions used in tests # def position_callback(self, data): - self.hasPos = True - self.localPosition = data + self.has_pos = True + self.local_position = data def vehicle_control_mode_callback(self, data): - self.controlMode = data - - - # - # Helper methods - # - def arm(self): - return self.cmdArm(value=True) + self.control_mode = data # # 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() @@ -121,19 +94,23 @@ class MavrosOffboardAttctlTest(unittest.TestCase): # does it cross expected boundaries in X seconds? count = 0 timeout = 120 - while(count < timeout): + 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): + self.pub_att.publish(att) + self.pub_thr.publish(throttle) + self.rate.sleep() + + if (self.local_position.pose.position.x > 5 + and self.local_position.pose.position.z > 5 + and self.local_position.pose.position.y < -5): break count = count + 1 - self.rate.sleep() + self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set") + self.assertTrue(self.control_mode.flag_control_attitude_enabled, "flag_control_attitude_enabled is not set") + self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") self.assertTrue(count < timeout, "took too long to cross boundaries") diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index 6d26015e7..568c2cbd8 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -37,7 +37,6 @@ # PKG = 'px4' -import sys import unittest import rospy import math @@ -49,9 +48,6 @@ 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 @@ -64,37 +60,41 @@ class MavrosOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - rospy.wait_for_service('iris/mavros/cmd/arming', 30) rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) - self.pubSpt = rospy.Publisher('iris/mavros/setpoint/local_position', PoseStamped, queue_size=10) - self.cmdArm = rospy.ServiceProxy("iris/mavros/cmd/arming", CommandBool) + self.pub_spt = rospy.Publisher('iris/mavros/setpoint/local_position', PoseStamped, queue_size=10) self.rate = rospy.Rate(10) # 10hz - self.rateSec = rospy.Rate(1) - self.hasPos = False - self.controlMode = vehicle_control_mode() + self.has_pos = False + self.local_position = PoseStamped() + self.control_mode = vehicle_control_mode() # # General callback functions used in tests # def position_callback(self, data): - self.hasPos = True - self.localPosition = data + self.has_pos = True + self.local_position = data def vehicle_control_mode_callback(self, data): - self.controlMode = data + self.control_mode = data # # Helper methods # def is_at_position(self, x, y, z, offset): - if(not self.hasPos): + if not self.has_pos: return False - rospy.logdebug("current position %f, %f, %f" % (self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z)) + rospy.logdebug("current position %f, %f, %f" % + (self.local_position.pose.position.x, + self.local_position.pose.position.y, + self.local_position.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)) + pos = np.array((self.local_position.pose.position.x, + self.local_position.pose.position.y, + self.local_position.pose.position.z)) return linalg.norm(desired - pos) < offset def reach_position(self, x, y, z, timeout): @@ -114,57 +114,44 @@ class MavrosOffboardPosctlTest(unittest.TestCase): # does it reach the position in X seconds? count = 0 - while(count < timeout): + while count < timeout: # update timestamp for each published SP pos.header.stamp = rospy.Time.now() - self.pubSpt.publish(pos) + self.pub_spt.publish(pos) - if(self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5)): + 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)) + (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 + while count < timeout: + if not self.is_at_position(2, 2, 2, 0.5): break count = count + 1 self.rate.sleep() + self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set") + self.assertTrue(self.control_mode.flag_control_position_enabled, "flag_control_position_enabled is not set") + self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") self.assertTrue(count == timeout, "position could not be held") diff --git a/integrationtests/demo_tests/mavros_tests.launch b/integrationtests/demo_tests/mavros_tests.launch index e42db6043..cc4918307 100644 --- a/integrationtests/demo_tests/mavros_tests.launch +++ b/integrationtests/demo_tests/mavros_tests.launch @@ -2,7 +2,7 @@ <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="enable_ground_truth" default="true"/> <arg name="ns" default="iris"/> <arg name="log_file" default="$(arg ns)"/> diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch index 122a27867..7243e7476 100644 --- a/launch/gazebo_ardrone_empty_world.launch +++ b/launch/gazebo_ardrone_empty_world.launch @@ -3,7 +3,7 @@ <arg name="headless" default="false"/> <arg name="gui" default="true"/> <arg name="enable_logging" default="false"/> - <arg name="enable_ground_truth" default="false"/> + <arg name="enable_ground_truth" default="true"/> <arg name="ns" default="ardrone"/> <arg name="log_file" default="$(arg ns)"/> diff --git a/nuttx-configs/px4fmu-v2/scripts/ld.script b/nuttx-configs/px4fmu-v2/scripts/ld.script index bec896d1c..b04ad89a6 100644 --- a/nuttx-configs/px4fmu-v2/scripts/ld.script +++ b/nuttx-configs/px4fmu-v2/scripts/ld.script @@ -66,12 +66,20 @@ EXTERN(_vectors) /* force the vectors to be included in the output */ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). */ EXTERN(abort) +EXTERN(_bootdelay_signature) SECTIONS { .text : { _stext = ABSOLUTE(.); *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; *(.text .text.*) *(.fixup) *(.gnu.warning) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 401cf05f1..85a0dda9e 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -166,14 +166,17 @@ private: param_t roll_rate_p; param_t roll_rate_i; param_t roll_rate_d; + param_t roll_rate_ff; param_t pitch_p; param_t pitch_rate_p; param_t pitch_rate_i; param_t pitch_rate_d; + param_t pitch_rate_ff; param_t yaw_p; param_t yaw_rate_p; param_t yaw_rate_i; param_t yaw_rate_d; + param_t yaw_rate_ff; param_t yaw_ff; param_t yaw_rate_max; @@ -191,6 +194,7 @@ private: math::Vector<3> rate_p; /**< P gain for angular rate error */ math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ + math::Vector<3> rate_ff; /**< Feedforward gain for desired rates */ float yaw_ff; /**< yaw control feed-forward */ float yaw_rate_max; /**< max yaw rate */ @@ -316,6 +320,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); + _params.rate_ff.zero(); _params.yaw_ff = 0.0f; _params.yaw_rate_max = 0.0f; _params.man_roll_max = 0.0f; @@ -335,14 +340,17 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); + _params_handles.roll_rate_ff = param_find("MC_ROLLRATE_FF"); _params_handles.pitch_p = param_find("MC_PITCH_P"); _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.pitch_rate_ff = param_find("MC_PITCHRATE_FF"); _params_handles.yaw_p = param_find("MC_YAW_P"); _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_rate_ff = param_find("MC_YAWRATE_FF"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); @@ -394,6 +402,8 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(0) = v; param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v; + param_get(_params_handles.roll_rate_ff, &v); + _params.rate_ff(0) = v; /* pitch gains */ param_get(_params_handles.pitch_p, &v); @@ -404,6 +414,8 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(1) = v; param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v; + param_get(_params_handles.pitch_rate_ff, &v); + _params.rate_ff(1) = v; /* yaw gains */ param_get(_params_handles.yaw_p, &v); @@ -414,6 +426,8 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(2) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; + param_get(_params_handles.yaw_rate_ff, &v); + _params.rate_ff(2) = v; param_get(_params_handles.yaw_ff, &_params.yaw_ff); param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); @@ -653,7 +667,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; - _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp); _rates_prev = rates; /* update integral only if not saturated on low limit */ diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 302551959..6a21f9046 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -83,6 +83,16 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); /** + * Roll rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); + +/** * Pitch P gain * * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad. @@ -124,6 +134,16 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); /** + * Pitch rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f); + +/** * Yaw P gain * * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad. @@ -165,6 +185,16 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); /** + * Yaw rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f); + +/** * Yaw feed forward * * Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot. diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 5459fcffd..2758979a2 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -57,7 +57,8 @@ Mavlink::Mavlink() : { _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); _link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3)); - + _att_sp = {}; + _offboard_control_mode = {}; } int main(int argc, char **argv) @@ -127,10 +128,8 @@ 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)); + _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 @@ -140,45 +139,46 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) 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) { + 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; + _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_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); + _offboard_control_mode.ignore_position = true; + _offboard_control_mode.ignore_velocity = true; + _offboard_control_mode.ignore_acceleration_force = true; - static vehicle_attitude_setpoint att_sp = {}; + _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 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; + _att_sp.timestamp = get_time_micros(); + if (!ignore_attitude) { + 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_thrust) { + _att_sp.thrust = set_attitude_target.thrust; } - if (!offboard_control_mode.ignore_attitude) { + if (!ignore_attitude) { for (ssize_t i = 0; i < 4; i++) { - att_sp.q_d[i] = set_attitude_target.q[i]; + _att_sp.q_d[i] = set_attitude_target.q[i]; } - att_sp.q_d_valid = true; + _att_sp.q_d_valid = true; } - _v_att_sp_pub.publish(att_sp); + _v_att_sp_pub.publish(_att_sp); //XXX real mavlink publishes rate sp here diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index acb2408f3..8b7d08d24 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -69,6 +69,8 @@ protected: ros::Publisher _pos_sp_triplet_pub; ros::Publisher _offboard_control_mode_pub; ros::Publisher _force_sp_pub; + vehicle_attitude_setpoint _att_sp; + offboard_control_mode _offboard_control_mode; /** * |