forked from Archive/PX4-Autopilot
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
This commit is contained in:
NRottmann
Lorenz Meier
parent
e33a2b6c8d
commit
0f8f5d29be
|
|
@ -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
|
||||
|
|
@ -144,6 +144,10 @@ set(config_module_list
|
|||
#
|
||||
examples/rover_steering_control
|
||||
|
||||
#
|
||||
# HippoCampus example (AUV from TUHH)
|
||||
examples/auv_hippocampus_example_app
|
||||
|
||||
#
|
||||
# Segway
|
||||
#
|
||||
|
|
|
|||
|
|
@ -0,0 +1,59 @@
|
|||
<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 : -->
|
||||
|
|
@ -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
|
||||
|
|
@ -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 :
|
||||
|
|
@ -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 <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;
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue