Enable Simulation of the Hippocampus (AUV from TUHH)

Adding files which enable a simulation with the autonomous underwater
vehicle (AUV) from the Technical University Hamburg-Harburg

ROMFS/px4fmu_test/mixers/uuv_quad_x.mix

+M: 4
+O:       10000   10000     0 -10000  10000
+S: 0 0  -10000  -10000     0 -10000  10000
+S: 0 1  -10000  -10000     0 -10000  10000
+S: 0 2  +10000  +10000     0 -10000  10000
+S: 0 3  +10000  +10000     0 -10000  10000
+M: 4
+O:       10000   10000     0 -10000  10000
+S: 0 0  +10000  +10000     0 -10000  10000
+S: 0 1  -10000  -10000     0 -10000  10000
+S: 0 2  -10000  -10000     0 -10000  10000
+S: 0 3  +10000  +10000     0 -10000  10000
+M: 4
+O:       10000   10000     0 -10000  10000
+S: 0 0  -10000  -10000     0 -10000  10000
+S: 0 1  +10000  +10000     0 -10000  10000
+S: 0 2  -10000  -10000     0 -10000  10000
+S: 0 3  +10000  +10000     0 -10000  10000
+M: 4
+O:       10000   10000     0 -10000  10000
+S: 0 0  +10000  +10000     0 -10000  10000
+S: 0 1  +10000  +10000     0 -10000  10000
+S: 0 2  +10000  +10000     0 -10000  10000
+S: 0 3  +10000  +10000     0 -10000  10000

cmake/configs/posix_sitl_default.cmake

@@ -144,6 +144,10 @@ set(config_module_list
 	#
 	examples/rover_steering_control
 
+	#
+	# HippoCampus example (AUV from TUHH)
+	examples/auv_hippocampus_example_app
+
 	#
 	# Segway
 	#

launch/hippocampus.launch

+<launch>
+
+    <!--
+        MAVROS posix SITL environment launch script modified for hippocam,pus from mavros_posix_sitl.
+        The HippoCampus is an autonomous underwater vehicle (AUV) designed by the Technical University Hamburg-Harburg.
+        https://www.tuhh.de/mum/forschung/forschungsgebiete-und-projekte/flow-field-estimation-with-a-swarm-of-auvs.html
+    -->
+
+    <arg name="x" default="0"/>
+    <arg name="y" default="0"/>
+    <arg name="z" default="0"/>
+    <arg name="R" default="0"/>
+    <arg name="P" default="0"/>
+    <arg name="Y" default="0"/>
+
+
+    <arg name="est" default="lpe"/>
+    <arg name="vehicle" default="hippocampus"/>
+    <arg name="world" default="$(find px4)/Tools/sitl_gazebo/worlds/underwater.world"/>
+    <arg name="sdf" default="$(find px4)/Tools/sitl_gazebo/models/$(arg vehicle)/$(arg vehicle).sdf"/>
+
+    <arg name="rcS" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)"/>
+
+    <arg name="headless" default="false"/>
+    <arg name="gui" default="true"/>
+    <arg name="ns" default="/"/>
+    <arg name="fcu_url" default="udp://:14540@localhost:14557"/>
+
+    <arg name="debug" default="false"/>
+    <arg name="verbose" default="false"/>
+    <arg name="paused" default="false"/>
+
+    <include file="$(find px4)/launch/posix_sitl.launch">
+        <arg name="x" value="$(arg x)"/>
+        <arg name="y" value="$(arg y)"/>
+        <arg name="z" value="$(arg z)"/>
+        <arg name="R" value="$(arg R)"/>
+        <arg name="P" value="$(arg P)"/>
+        <arg name="Y" value="$(arg Y)"/>
+        <arg name="world" value="$(arg world)"/>
+        <arg name="vehicle" value="$(arg vehicle)"/>
+        <arg name="sdf" value="$(arg sdf)"/>
+        <arg name="rcS" value="$(arg rcS)"/>
+        <arg name="headless" value="$(arg headless)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="ns" value="$(arg ns)"/>
+        <arg name="debug" value="$(arg debug)"/>
+        <arg name="verbose" value="$(arg verbose)"/>
+        <arg name="paused" value="$(arg paused)"/>
+    </include>
+
+    <include file="$(find px4)/launch/mavros.launch">
+        <arg name="ns" value="$(arg ns)"/>
+        <arg name="gcs_url" value=""/> <!-- GCS link is provided by SITL -->
+        <arg name="fcu_url" value="$(arg fcu_url)"/>
+    </include>
+</launch>
+
+<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->

posix-configs/SITL/init/lpe/hippocampus

+uorb start
+param load
+param set MAV_TYPE 3
+param set SYS_AUTOSTART 4010
+param set SYS_RESTART_TYPE 2
+param set COM_DISARM_LAND 0
+dataman start
+param set CAL_GYRO0_ID 2293768
+param set CAL_ACC0_ID 1376264
+param set CAL_ACC1_ID 1310728
+param set CAL_MAG0_ID 196616
+
+simulator start -s
+
+pwm_out_sim mode_pwm
+mixer load /dev/pwm_output0 ROMFS/px4fmu_test/mixers/uuv_quad_x.mix
+
+gyrosim start
+accelsim start
+barosim start
+adcsim start
+
+sleep 1
+
+sensors start
+commander start
+
+attitude_estimator_q start
+
+mavlink start -u 14556 -r 4000000
+mavlink start -u 14557 -r 4000000 -m onboard -o 14540
+mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556
+mavlink stream -r 50 -s ATT_POS_MOCAP -u 14556
+mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556
+logger start -r 100 -e
+mavlink boot_complete

src/examples/auv_hippocampus_example_app/CMakeLists.txt

+############################################################################
+#
+#   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.
+#
+############################################################################
+px4_add_module(
+	MODULE examples__auv_hippocampus_example_app
+	MAIN auv_hippocampus_example_app
+	STACK_MAIN 2000
+	SRCS
+		auv_hippocampus_example_app.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/examples/auv_hippocampus_example_app/auv_hippocampus_example_app.cpp

+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2016 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 hippocampus_example_app.cpp
+ *
+ * This file let the hippocampus drive in a circle and prints the orientation as well as the acceleration data.
+ * The HippoCampus is an autonomous underwater vehicle (AUV) designed by the Technical University Hamburg-Harburg (TUHH).
+ * https://www.tuhh.de/mum/forschung/forschungsgebiete-und-projekte/flow-field-estimation-with-a-swarm-of-auvs.html
+ *
+ * @author Nils Rottann <Nils.Rottmann@tuhh.de>
+ */
+
+#include <px4_config.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <poll.h>
+#include <string.h>
+#include <math.h>
+
+// system libraries
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/circuit_breaker.h>
+// internal libraries
+#include <lib/mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+#include <lib/tailsitter_recovery/tailsitter_recovery.h>
+
+// Include uORB and the required topics for this app
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_combined.h>                // this topics hold the acceleration data
+#include <uORB/topics/actuator_controls.h>              // this topic gives the actuators control input
+#include <uORB/topics/control_state.h>                  // this topic holds the orientation of the hippocampus
+
+extern "C" __EXPORT int auv_hippocampus_example_app_main(int argc, char *argv[]);
+
+int auv_hippocampus_example_app_main(int argc, char *argv[])
+{
+	PX4_INFO("auv_hippocampus_example_app has been started!");
+
+	/* subscribe to sensor_combined topic */
+	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
+	/* limit the update rate to 5 Hz */
+	orb_set_interval(sensor_sub_fd, 200);
+
+	/* subscribe to control_state topic */
+	int ctrl_state_sub_fd = orb_subscribe(ORB_ID(control_state));
+	/* limit the update rate to 5 Hz */
+	orb_set_interval(ctrl_state_sub_fd, 200);
+
+	/* advertise to actuator_control topic */
+	struct actuator_controls_s act;
+	memset(&act, 0, sizeof(act));
+	orb_advert_t act_pub = orb_advertise(ORB_ID(actuator_controls_0), &act);
+
+	/* one could wait for multiple topics with this technique, just using one here */
+	px4_pollfd_struct_t fds[] = {
+		{ .fd = sensor_sub_fd,   .events = POLLIN },
+		{ .fd = ctrl_state_sub_fd,   .events = POLLIN },
+	};
+
+	int error_counter = 0;
+
+	for (int i = 0; i < 10; i++) {
+		/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
+		int poll_ret = px4_poll(fds, 1, 1000);
+
+		/* handle the poll result */
+		if (poll_ret == 0) {
+			/* this means none of our providers is giving us data */
+			PX4_ERR("Got no data within a second");
+
+		} else if (poll_ret < 0) {
+			/* this is seriously bad - should be an emergency */
+			if (error_counter < 10 || error_counter % 50 == 0) {
+				/* use a counter to prevent flooding (and slowing us down) */
+				PX4_ERR("ERROR return value from poll(): %d", poll_ret);
+			}
+
+			error_counter++;
+
+		} else {
+
+			if (fds[0].revents & POLLIN) {
+				/* obtained data for the first file descriptor */
+				struct sensor_combined_s raw_sensor;
+				/* copy sensors raw data into local buffer */
+				orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw_sensor);
+				// printing the sensor data into the terminal
+				PX4_INFO("Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
+					 (double)raw_sensor.accelerometer_m_s2[0],
+					 (double)raw_sensor.accelerometer_m_s2[1],
+					 (double)raw_sensor.accelerometer_m_s2[2]);
+
+				/* obtained data for the third file descriptor */
+				struct control_state_s raw_ctrl_state;
+				/* copy sensors raw data into local buffer */
+				orb_copy(ORB_ID(control_state), ctrl_state_sub_fd, &raw_ctrl_state);
+
+				// get current rotation matrix from control state quaternions, the quaternions are generated by the
+				// attitude_estimator_q application using the sensor data
+				math::Quaternion q_att(raw_ctrl_state.q[0], raw_ctrl_state.q[1], raw_ctrl_state.q[2],
+						       raw_ctrl_state.q[3]);     // control_state is frequently updated
+				math::Matrix<3, 3> R =
+					q_att.to_dcm();     // create rotation matrix for the quaternion when post multiplying with a column vector
+
+				// orientation vectors
+				math::Vector<3> x_B(R(0, 0), R(1, 0), R(2, 0));     // orientation body x-axis (in world coordinates)
+				math::Vector<3> y_B(R(0, 1), R(1, 1), R(2, 1));     // orientation body y-axis (in world coordinates)
+				math::Vector<3> z_B(R(0, 2), R(1, 2), R(2, 2));     // orientation body z-axis (in world coordinates)
+
+				PX4_INFO("x_B:\t%8.4f\t%8.4f\t%8.4f",
+					 (double)x_B(0),
+					 (double)x_B(1),
+					 (double)x_B(2));
+
+				PX4_INFO("y_B:\t%8.4f\t%8.4f\t%8.4f",
+					 (double)y_B(0),
+					 (double)y_B(1),
+					 (double)y_B(2));
+
+				PX4_INFO("z_B:\t%8.4f\t%8.4f\t%8.4f \n",
+					 (double)z_B(0),
+					 (double)z_B(1),
+					 (double)z_B(2));
+			}
+		}
+
+		// Give actuator input to the HippoC, this will result in a circle
+		act.control[0] = 0.0f;      // roll
+		act.control[1] = 0.0f;      // pitch
+		act.control[2] = 1.0f;		// yaw
+		act.control[3] = 1.0f;		// thrust
+		orb_publish(ORB_ID(actuator_controls_0), act_pub, &act);
+
+	}
+
+
+	PX4_INFO("exiting auv_hippocampus_example_app!");
+
+
+	return 0;
+}
+
+