From 0f8f5d29be6860e29df5d90ee84fd2983a2371bc Mon Sep 17 00:00:00 2001 From: NRottmann Date: Fri, 11 Aug 2017 13:36:44 +0200 Subject: [PATCH] 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 | 24 +++ cmake/configs/posix_sitl_default.cmake | 4 + launch/hippocampus.launch | 59 ++++++ posix-configs/SITL/init/lpe/hippocampus | 36 ++++ .../CMakeLists.txt | 42 ++++ .../auv_hippocampus_example_app.cpp | 180 ++++++++++++++++++ 6 files changed, 345 insertions(+) create mode 100644 ROMFS/px4fmu_test/mixers/uuv_quad_x.mix create mode 100644 launch/hippocampus.launch create mode 100644 posix-configs/SITL/init/lpe/hippocampus create mode 100644 src/examples/auv_hippocampus_example_app/CMakeLists.txt create mode 100644 src/examples/auv_hippocampus_example_app/auv_hippocampus_example_app.cpp diff --git a/ROMFS/px4fmu_test/mixers/uuv_quad_x.mix b/ROMFS/px4fmu_test/mixers/uuv_quad_x.mix new file mode 100644 index 0000000000..9f808e5b6b --- /dev/null +++ b/ROMFS/px4fmu_test/mixers/uuv_quad_x.mix @@ -0,0 +1,24 @@ +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 diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index ee82489674..34fda5cce5 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/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 # diff --git a/launch/hippocampus.launch b/launch/hippocampus.launch new file mode 100644 index 0000000000..8b60e64406 --- /dev/null +++ b/launch/hippocampus.launch @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/posix-configs/SITL/init/lpe/hippocampus b/posix-configs/SITL/init/lpe/hippocampus new file mode 100644 index 0000000000..3490edbe08 --- /dev/null +++ b/posix-configs/SITL/init/lpe/hippocampus @@ -0,0 +1,36 @@ +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 diff --git a/src/examples/auv_hippocampus_example_app/CMakeLists.txt b/src/examples/auv_hippocampus_example_app/CMakeLists.txt new file mode 100644 index 0000000000..a66a0b693a --- /dev/null +++ b/src/examples/auv_hippocampus_example_app/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# 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 : diff --git a/src/examples/auv_hippocampus_example_app/auv_hippocampus_example_app.cpp b/src/examples/auv_hippocampus_example_app/auv_hippocampus_example_app.cpp new file mode 100644 index 0000000000..cb09936d3a --- /dev/null +++ b/src/examples/auv_hippocampus_example_app/auv_hippocampus_example_app.cpp @@ -0,0 +1,180 @@ +/**************************************************************************** + * + * 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 + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +// system libraries +#include +#include +#include +#include +#include +// internal libraries +#include +#include +#include + +// Include uORB and the required topics for this app +#include +#include // this topics hold the acceleration data +#include // this topic gives the actuators control input +#include // 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; +} + +