diff --git a/integrationtests/demo_tests/direct_manual_input_test.py b/integrationtests/demo_tests/direct_manual_input_test.py deleted file mode 100755 index e5d2887671..0000000000 --- a/integrationtests/demo_tests/direct_manual_input_test.py +++ /dev/null @@ -1,90 +0,0 @@ -#!/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 -# -PKG = 'px4' - -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): - - 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.actuator_status = data - - def vehicle_control_mode_callback(self, data): - self.control_mode = data - - # - # Test arming - # - def test_manual_input(self): - rospy.init_node('test_node', anonymous=True) - rospy.Subscriber('actuator_armed', actuator_armed, self.actuator_armed_callback) - rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - - man_in = ManualInput() - - # Test arming - man_in.arm() - self.assertEquals(self.actuator_status.armed, True, "did not arm") - - # Test posctl - man_in.posctl() - self.assertTrue(self.control_mode.flag_control_position_enabled, "flag_control_position_enabled is not set") - - # Test offboard - man_in.offboard() - self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") - - -if __name__ == '__main__': - import rostest - rostest.rosrun(PKG, 'direct_manual_input_test', ManualInputTest) diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py deleted file mode 100755 index 9a6c4af783..0000000000 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ /dev/null @@ -1,175 +0,0 @@ -#!/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 -# -PKG = 'px4' - -import unittest -import rospy -import rosbag - -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 position_setpoint_triplet -from px4.msg import position_setpoint -from px4.msg import vehicle_local_position_setpoint - -from manual_input import ManualInput -from flight_path_assertion import FlightPathAssertion -from px4_test_helper import PX4TestHelper - -# -# 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) - self.helper = PX4TestHelper("direct_offboard_posctl_test") - self.helper.setUp() - - rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - self.sub_vlp = rospy.Subscriber("vehicle_local_position", vehicle_local_position, self.position_callback) - self.pub_spt = rospy.Publisher('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: - self.fpa.stop() - - self.helper.tearDown() - - # - # General callback functions used in tests - # - - def position_callback(self, data): - self.has_pos = True - self.local_position = data - - def vehicle_control_mode_callback(self, data): - self.control_mode = data - - - # - # Helper methods - # - def is_at_position(self, x, y, z, offset): - 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.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): - # 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.pub_spt.publish(stp) - self.helper.bag_write('px4/position_setpoint_triplet', 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): - man_in = ManualInput() - - # arm and go into offboard - 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), - (0, 0, -2), - (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? - count = 0 - timeout = 50 - while count < timeout: - if not self.is_at_position(2, 2, -2, 0.5): - break - count = count + 1 - self.rate.sleep() - - self.assertTrue(count == timeout, "position could not be held") - self.fpa.stop() - - -if __name__ == '__main__': - import rostest - rostest.rosrun(PKG, 'direct_offboard_posctl_test', DirectOffboardPosctlTest) - #unittest.main() diff --git a/integrationtests/demo_tests/direct_tests.launch b/integrationtests/demo_tests/direct_tests.launch deleted file mode 100644 index 04ba8a54d3..0000000000 --- a/integrationtests/demo_tests/direct_tests.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/integrationtests/demo_tests/mavros_tests.launch b/integrationtests/demo_tests/mavros_tests.launch deleted file mode 100644 index cdafc967c5..0000000000 --- a/integrationtests/demo_tests/mavros_tests.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/integrationtests/demo_tests/mavros_tests_posix.launch b/integrationtests/demo_tests/mavros_tests_posix.launch deleted file mode 100644 index 029ef81590..0000000000 --- a/integrationtests/demo_tests/mavros_tests_posix.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py similarity index 90% rename from integrationtests/demo_tests/mavros_offboard_attctl_test.py rename to integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py index 36077f32dd..8668a34d2a 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py @@ -46,6 +46,7 @@ from std_msgs.msg import Float64 from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler from mavros_msgs.srv import CommandLong +from sensor_msgs.msg import NavSatFix #from px4_test_helper import PX4TestHelper class MavrosOffboardAttctlTest(unittest.TestCase): @@ -63,12 +64,13 @@ class MavrosOffboardAttctlTest(unittest.TestCase): #self.helper.setUp() rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback) + rospy.Subscriber("mavros/global_position/global", NavSatFix, self.global_position_callback) self.pub_att = rospy.Publisher('mavros/setpoint_attitude/attitude', PoseStamped, queue_size=10) self.pub_thr = rospy.Publisher('mavros/setpoint_attitude/att_throttle', Float64, queue_size=10) rospy.wait_for_service('mavros/cmd/command', 30) self._srv_cmd_long = rospy.ServiceProxy('mavros/cmd/command', CommandLong, persistent=True) self.rate = rospy.Rate(10) # 10hz - self.has_pos = False + self.has_global_pos = False self.local_position = PoseStamped() def tearDown(self): @@ -79,22 +81,28 @@ class MavrosOffboardAttctlTest(unittest.TestCase): # General callback functions used in tests # def position_callback(self, data): - self.has_pos = True self.local_position = data + def global_position_callback(self, data): + self.has_global_pos = True + def test_attctl(self): """Test offboard attitude control""" + # FIXME: hack to wait for simulation to be ready + while not self.has_global_pos: + self.rate.sleep() + # 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) + quaternion = quaternion_from_euler(0.25, 0.25, 0) att.pose.orientation = Quaternion(*quaternion) throttle = Float64() - throttle.data = 0.8 + throttle.data = 0.7 armed = False # does it cross expected boundaries in X seconds? @@ -110,7 +118,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase): #self.helper.bag_write('mavros/setpoint_attitude/att_throttle', throttle) self.rate.sleep() - # arm and switch to offboard + # FIXME: arm and switch to offboard # (need to wait the first few rounds until PX4 has the offboard stream) if not armed and count > 5: self._srv_cmd_long(False, 176, False, diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py similarity index 93% rename from integrationtests/demo_tests/mavros_offboard_posctl_test.py rename to integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py index 7747d904d8..bd106d672b 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py @@ -49,6 +49,7 @@ from std_msgs.msg import Header from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler from mavros_msgs.srv import CommandLong +from sensor_msgs.msg import NavSatFix #from px4_test_helper import PX4TestHelper class MavrosOffboardPosctlTest(unittest.TestCase): @@ -66,11 +67,12 @@ class MavrosOffboardPosctlTest(unittest.TestCase): #self.helper.setUp() rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback) + rospy.Subscriber("mavros/global_position/global", NavSatFix, self.global_position_callback) self.pub_spt = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) rospy.wait_for_service('mavros/cmd/command', 30) self._srv_cmd_long = rospy.ServiceProxy('mavros/cmd/command', CommandLong, persistent=True) self.rate = rospy.Rate(10) # 10hz - self.has_pos = False + self.has_global_pos = False self.local_position = PoseStamped() self.armed = False @@ -82,17 +84,15 @@ class MavrosOffboardPosctlTest(unittest.TestCase): # General callback functions used in tests # def position_callback(self, data): - self.has_pos = True self.local_position = data + def global_position_callback(self, data): + self.has_global_pos = True # # Helper methods # def is_at_position(self, x, y, z, offset): - if not self.has_pos: - return False - rospy.logdebug("current position %f, %f, %f" % (self.local_position.pose.position.x, self.local_position.pose.position.y, @@ -127,7 +127,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase): self.pub_spt.publish(pos) #self.helper.bag_write('mavros/setpoint_position/local', pos) - # arm and switch to offboard + # FIXME: arm and switch to offboard # (need to wait the first few rounds until PX4 has the offboard stream) if not self.armed and count > 5: self._srv_cmd_long(False, 176, False, @@ -144,6 +144,10 @@ class MavrosOffboardPosctlTest(unittest.TestCase): def test_posctl(self): """Test offboard position control""" + # FIXME: hack to wait for simulation to be ready + while not self.has_global_pos: + self.rate.sleep() + positions = ( (0, 0, 0), (2, 2, 2), diff --git a/integrationtests/python_src/px4_it/util/TODO.md b/integrationtests/python_src/px4_it/util/TODO.md new file mode 100644 index 0000000000..b57e40a46e --- /dev/null +++ b/integrationtests/python_src/px4_it/util/TODO.md @@ -0,0 +1 @@ +TODO: Adopt to new SITL diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/python_src/px4_it/util/flight_path_assertion.py similarity index 100% rename from integrationtests/demo_tests/flight_path_assertion.py rename to integrationtests/python_src/px4_it/util/flight_path_assertion.py diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/python_src/px4_it/util/manual_input.py similarity index 100% rename from integrationtests/demo_tests/manual_input.py rename to integrationtests/python_src/px4_it/util/manual_input.py diff --git a/integrationtests/demo_tests/px4_test_helper.py b/integrationtests/python_src/px4_it/util/px4_test_helper.py similarity index 100% rename from integrationtests/demo_tests/px4_test_helper.py rename to integrationtests/python_src/px4_it/util/px4_test_helper.py diff --git a/integrationtests/run-container.bash b/integrationtests/run_container.bash similarity index 91% rename from integrationtests/run-container.bash rename to integrationtests/run_container.bash index 73475ffe14..73272d13a4 100755 --- a/integrationtests/run-container.bash +++ b/integrationtests/run_container.bash @@ -21,5 +21,5 @@ fi # Assuming that necessary source projects, including this one, are cloned in the build server workspace of this job. # echo "===> run container" -docker run --rm -v "$WORKSPACE:/job:rw" px4io/px4-dev-ros bash "/job/Firmware/integrationtests/run-tests.bash" +docker run --rm -v "$WORKSPACE:/job:rw" px4io/px4-dev-ros bash "/job/Firmware/integrationtests/run_tests.bash" /job/Firmware echo "<===" diff --git a/integrationtests/run-tests.bash b/integrationtests/run_tests.bash similarity index 71% rename from integrationtests/run-tests.bash rename to integrationtests/run_tests.bash index 139b292150..caaf31f9d9 100755 --- a/integrationtests/run-tests.bash +++ b/integrationtests/run_tests.bash @@ -5,48 +5,55 @@ # License: according to LICENSE.md in the root directory of the PX4 Firmware repository set -e -SRC_DIR=/root/Firmware +if [ "$#" -lt 1 ] +then + echo usage: run_tests.bash firmware_src_dir + echo "" + exit 1 +fi + +SRC_DIR=$1 +JOB_DIR=SRC_DIR/.. BUILD=posix_sitl_default # TODO ROS_TEST_RESULT_DIR=/root/.ros/test_results/px4 ROS_LOG_DIR=/root/.ros/log PX4_LOG_DIR=${SRC_DIR}/build_${BUILD}/src/firmware/posix/rootfs/fs/microsd/log -TEST_RESULT_TARGET_DIR=/job/test_results +TEST_RESULT_TARGET_DIR=$JOB_DIR/test_results # BAGS=/root/.ros # CHARTS=/root/.ros/charts # EXPORT_CHARTS=/sitl/testing/export_charts.py -# source ROS env, setup Gazebo env +# source ROS env source /opt/ros/indigo/setup.bash -export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:${SRC_DIR}/Tools/sitl_gazebo/models -export GAZEBO_PLUGIN_PATH=${SRC_DIR}/Tools/sitl_gazebo/Build/:${GAZEBO_PLUGIN_PATH} -export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:${SRC_DIR}/Tools/sitl_gazebo/Build/msgs/ -export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:${SRC_DIR} +source $SRC_DIR/integrationtests/setup_gazebo_ros.bash $SRC_DIR echo "deleting previous test results" if [ -d ${TEST_RESULT_TARGET_DIR} ]; then rm -r ${TEST_RESULT_TARGET_DIR} fi -echo "linking source to test" -if [ -d "${SRC_DIR}" ]; then - rm -r ${SRC_DIR} +# FIXME: Firmware compilation seems to CD into this directory (/root/Firmware) +# when run from "run_container.bash" +if [ -d /root/Firmware ]; then + rm /root/Firmware fi -ln -s /job/Firmware ${SRC_DIR} +ln -s ${SRC_DIR} /root/Firmware -echo "=====> compile" +echo "=====> compile ($SRC_DIR)" cd $SRC_DIR make ${BUILD} mkdir -p Tools/sitl_gazebo/Build cd Tools/sitl_gazebo/Build cmake -Wno-dev .. make -j4 +make sdf echo "<=====" # don't exit on error anymore from here on (because single tests or exports might fail) set +e echo "=====> run tests" -rostest px4 mavros_tests_posix.launch +rostest px4 mavros_posix_tests_iris.launch TEST_RESULT=$? echo "<=====" diff --git a/integrationtests/setup_gazebo_ros.bash b/integrationtests/setup_gazebo_ros.bash new file mode 100755 index 0000000000..7655fec9a7 --- /dev/null +++ b/integrationtests/setup_gazebo_ros.bash @@ -0,0 +1,20 @@ +#!/bin/bash +# +# Setup environment to make PX4 visible to ROS/Gazebo. +# +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository + +if [ "$#" -lt 1 ] +then + echo usage: source setup_gazebo_ros.bash firmware_src_dir + echo "" + return 1 +fi + +SRC_DIR=$1 + +# setup Gazebo env and update package path +export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:${SRC_DIR}/Tools/sitl_gazebo/models +export GAZEBO_PLUGIN_PATH=${SRC_DIR}/Tools/sitl_gazebo/Build/:${GAZEBO_PLUGIN_PATH} +export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:${SRC_DIR}/Tools/sitl_gazebo/Build/msgs/ +export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:${SRC_DIR} diff --git a/launch/mavros.launch b/launch/mavros.launch new file mode 100644 index 0000000000..e61a913714 --- /dev/null +++ b/launch/mavros.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/mavros_posix_sitl.launch b/launch/mavros_posix_sitl.launch new file mode 100644 index 0000000000..905e0a2ff1 --- /dev/null +++ b/launch/mavros_posix_sitl.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/mavros_posix_tests_iris.launch b/launch/mavros_posix_tests_iris.launch new file mode 100644 index 0000000000..235acda6f2 --- /dev/null +++ b/launch/mavros_posix_tests_iris.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch index 7997a69d8a..50ede33f9c 100644 --- a/launch/mavros_sitl.launch +++ b/launch/mavros_sitl.launch @@ -1,6 +1,5 @@ - - + diff --git a/launch/posix_sitl.launch b/launch/posix_sitl.launch new file mode 100644 index 0000000000..4e3b71f70c --- /dev/null +++ b/launch/posix_sitl.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris index f95c7da6d8..674c2e6d28 100644 --- a/posix-configs/SITL/init/rcS_gazebo_iris +++ b/posix-configs/SITL/init/rcS_gazebo_iris @@ -18,11 +18,6 @@ param set CAL_ACC0_YSCALE 1.01 param set CAL_ACC0_ZSCALE 1.01 param set CAL_ACC1_XOFF 0.01 param set CAL_MAG0_XOFF 0.01 -param set MPC_XY_P 0.25 -param set MPC_XY_VEL_P 0.05 -param set MPC_XY_VEL_D 0.005 -param set MPC_XY_FF 0.1 -param set MPC_Z_P 1.3 param set SENS_BOARD_ROT 8 param set SENS_BOARD_X_OFF 0.000001 param set COM_RC_IN_MODE 1