diff --git a/.gitignore b/.gitignore
index fd228d9706..4883fb4ead 100644
--- a/.gitignore
+++ b/.gitignore
@@ -65,6 +65,7 @@ paparazzi.sublime-workspace
/conf/maps_data/*
/conf/maps.xml
/conf/gps/ublox_conf
+!/conf/simulator/gazebo/**/*.config
# /doc/pprz_algebra/
/doc/pprz_algebra/headfile.log
diff --git a/conf/Makefile.nps b/conf/Makefile.nps
index d2d57ede4c..ccbda58127 100644
--- a/conf/Makefile.nps
+++ b/conf/Makefile.nps
@@ -45,6 +45,7 @@ CFLAGS += $(shell pkg-config --cflags-only-I ivy-glib)
CXXFLAGS = -W -Wall
CXXFLAGS += $(INCLUDES)
CXXFLAGS += $($(TARGET).CFLAGS)
+CXXFLAGS += $($(TARGET).CXXFLAGS)
CXXFLAGS += $(USER_CFLAGS) $(BOARD_CFLAGS)
CXXFLAGS += -O$(OPT)
CXXFLAGS += $(DEBUG_FLAGS)
diff --git a/conf/airframes/examples/ardrone2_gazebo.xml b/conf/airframes/examples/ardrone2_gazebo.xml
new file mode 100644
index 0000000000..6db02e1b3b
--- /dev/null
+++ b/conf/airframes/examples/ardrone2_gazebo.xml
@@ -0,0 +1,209 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/conf_example.xml b/conf/conf_example.xml
index 457bb9a3cc..1cc7083575 100644
--- a/conf/conf_example.xml
+++ b/conf/conf_example.xml
@@ -241,6 +241,17 @@
settings_modules="modules/video_rtp_stream.xml modules/geo_mag.xml modules/air_data.xml modules/gps_ubx_ucenter.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
gui_color="red"
/>
+
-
+
diff --git a/conf/modules/fdm_gazebo.xml b/conf/modules/fdm_gazebo.xml
new file mode 100644
index 0000000000..ce32db93a7
--- /dev/null
+++ b/conf/modules/fdm_gazebo.xml
@@ -0,0 +1,92 @@
+
+
+
+
+
+ Gazebo backend for NPS simulator
+ NPS doc: http://wiki.paparazziuav.org/wiki/NPS
+
+ Usage:
+ 1. Make sure gazebo 7 or 8 is installed. (sudo apt-get install gazebo8 libgazebo8-dev)
+ 2. Prepare the Gazebo world and model:
+ a) Prepare the UAV model (see conf/simulator/gazebo/models/ardrone/):
+ Place the aircraft model in the conf/simulator/gazebo/models/
+ folder, this folder is added to Gazebo's search path when NPS is
+ launched.
+ Gazebo uses a Front, Left, Up coordinate system for aircraft, so
+ make sure the +x axis points forwards.
+ The model should include a link for each motor with the same names
+ as those listed in NPS_ACTUATOR_NAMES (see below), e.g. 'nw_motor'.
+ Camera links should have the name specified in .dev_name in the
+ corresponding video_config_t struct, see sw/airborne/boards/pc_sim.h
+ and sw/airborne/modules/computer_vision/video_thread_nps.c.
+ b) Prepare the world (see conf/simulator/gazebo/worlds/ardrone.world).
+ Pay attention to the following:
+ The real-time update rate should be set to zero, as this is
+ already handled by Paparazzi:
+ <physics type="ode">
+ <max_step_size>0.001</max_step_size>
+ <real_time_update_rate>0</real_time_update_rate><!-- Handled by Paparazzi! -->
+ </physics>
+ Spherical coordinates should be provided for navigation.
+ At this moment, there is an issue where Gazebo incorrectly
+ uses a WSU coordinate system instead of ENU. This can be fixed
+ by setting the heading to 180 degrees as shown below:
+ <spherical_coordinates>
+ <surface_model>EARTH_WGS84</surface_model>
+ <latitude_deg>51.9906</latitude_deg>
+ <longitude_deg>4.37679</longitude_deg>
+ <elevation>0</elevation>
+ <heading_deg>180</heading_deg><!-- Temporary fix for issue https://bitbucket.org/osrf/gazebo/issues/2022/default-sphericalcoordinates-frame-should -->
+ </spherical_coordinates>
+ 3. Prepare the airframe file (see examples/ardrone2_gazebo.xml):
+ a) Select Gazebo as the FDM (Flight Dynamics Model)
+ <target name="nps" board="pc">
+ <module name="fdm" type="gazebo"/>
+ </target>
+ b) Add actuator thrusts and torques to the SIMULATOR section:
+ <section name="SIMULATOR" prefix="NPS_">
+ <define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
+ <define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="double[]"/>
+ <define name="ACTUATOR_TORQUES" value="0.155, -0.155, 0.155, -0.155" type="double[]"/>
+ ...
+ <section>
+ The thrusts and torques are expressed in SI units (N, Nm) and should
+ be in the same order as the ACTUATOR_NAMES.
+ c) In the same section, bypass the AHRS and INS as these are not
+ supported yet:
+ <section name="SIMULATOR" prefix="NPS_">
+ ...
+ <define name="BYPASS_AHRS" value="1"/>
+ <define name="BYPASS_INS" value="1"/>
+ ...
+ <section>
+ d) If required, enable video thread simulation:
+ <section name="SIMULATOR" prefix="NPS_">
+ ...
+ <define name="SIMULATE_VIDEO" value="1"/>
+ ...
+ <section>
+ e) If required, specify the Gazebo world and aircraft name:
+ <section name="SIMULATOR" prefix="NPS_">
+ ...
+ <define name="GAZEBO_WORLD" value="my_world.world"/>
+ <define name="GAZEBO_AC_NAME" value="my_uav"/>
+ <section>
+ 4. Make sure all included modules work with nps. At the moment, most of
+ the modules that depend on video_thread are only built when ap is
+ selected as the target. As a quick fix, try to remove the target
+ attribute from the makefile element in the module xml, e.g.:
+ <makefile target="ap"> ---> <makefile>
+
+
+
+
+
+ nps.CXXFLAGS += $(shell pkg-config gazebo --cflags)
+ nps.LDFLAGS += $(shell pkg-config gazebo --libs)
+
+
+
+
+
diff --git a/conf/modules/video_rtp_stream.xml b/conf/modules/video_rtp_stream.xml
index f9523dd6fa..815659bde5 100644
--- a/conf/modules/video_rtp_stream.xml
+++ b/conf/modules/video_rtp_stream.xml
@@ -34,7 +34,7 @@
-
+
diff --git a/conf/simulator/gazebo/models/ardrone/ardrone.sdf b/conf/simulator/gazebo/models/ardrone/ardrone.sdf
new file mode 100644
index 0000000000..6445c39ad8
--- /dev/null
+++ b/conf/simulator/gazebo/models/ardrone/ardrone.sdf
@@ -0,0 +1,234 @@
+
+
+
+ 0 0 .1 0 0 0
+
+
+
+ 0.4
+
+ 0.005
+ 0.005
+ 0.010
+ 0.
+ 0.
+ 0.
+
+
+
+
+
+
+ .4 .4 .05
+
+
+
+
+
+
+
+ .2 .2 .05
+
+
+
+
+
+
+ 0.15 0 0 0 0 0
+
+ 0.001
+
+ .0001
+ .0001
+ .0001
+ 0
+ 0
+ 0
+
+
+
+ 30.0
+
+ 1.3962634
+
+ 1280
+ 720
+ R8G8B8
+
+
+ 0.02
+ 300
+
+
+ gaussian
+
+ 0.0
+ 0.007
+
+
+
+
+
+
+ chassis
+ front_camera
+
+
+
+ 0 0 -.03 0 1.57 0
+
+ 0.001
+
+ .0001
+ .0001
+ .0001
+ 0
+ 0
+ 0
+
+
+
+ 30.0
+
+ 1.3962634
+
+ 320
+ 240
+ R8G8B8
+
+
+ 0.02
+ 300
+
+
+ gaussian
+
+ 0.0
+ 0.007
+
+
+
+
+
+
+ chassis
+ bottom_camera
+
+
+
+ 0.12 0.12 0 0 0 0
+
+ 0.01
+
+ .0001
+ .0001
+ .0001
+ 0
+ 0
+ 0
+
+
+
+
+
+ 0.10
+ .07
+
+
+
+
+
+
+ chassis
+ nw_motor
+
+
+
+ -0.12 -0.12 0 0 0 0
+
+ 0.01
+
+ .0001
+ .0001
+ .0001
+ 0
+ 0
+ 0
+
+
+
+
+
+ 0.10
+ .07
+
+
+
+
+
+
+ chassis
+ se_motor
+
+
+
+ 0.12 -0.12 0 0 0 0
+
+ 0.01
+
+ .0001
+ .0001
+ .0001
+ 0
+ 0
+ 0
+
+
+
+
+
+ 0.10
+ .07
+
+
+
+
+
+
+ chassis
+ ne_motor
+
+
+
+ -0.12 0.12 0 0 0 0
+
+ 0.01
+
+ .0001
+ .0001
+ .0001
+ 0
+ 0
+ 0
+
+
+
+
+
+ 0.10
+ .07
+
+
+
+
+
+
+ chassis
+ sw_motor
+
+
+
\ No newline at end of file
diff --git a/conf/simulator/gazebo/models/ardrone/model.config b/conf/simulator/gazebo/models/ardrone/model.config
new file mode 100644
index 0000000000..6a427c8df2
--- /dev/null
+++ b/conf/simulator/gazebo/models/ardrone/model.config
@@ -0,0 +1,15 @@
+
+
+ ARDrone2 (Paparazzi)
+ 1.0
+ ardrone.sdf
+
+
+ Tom van Dijk
+ tomvand@users.noreply.github.com
+
+
+
+ Simple ARDrone2 model for use with Paparazzi's NPS (http://wiki.paparazziuav.org).
+
+
\ No newline at end of file
diff --git a/conf/simulator/gazebo/world/ardrone.world b/conf/simulator/gazebo/world/ardrone.world
new file mode 100644
index 0000000000..8c1f997227
--- /dev/null
+++ b/conf/simulator/gazebo/world/ardrone.world
@@ -0,0 +1,41 @@
+
+
+
+
+ 0.001
+ 0
+
+
+ 0.4 0.4 0.4 1
+ 0.7 0.7 0.7 1
+ 1
+
+
+ 1
+ 0 0 10 0 -0 0
+ 0.8 0.8 0.8 1
+ 0.1 0.1 0.1 1
+
+ 1000
+ 0.9
+ 0.01
+ 0.001
+
+ -0.5 0.5 -1
+
+
+ EARTH_WGS84
+ 51.9906
+ 4.37679
+ 0
+ 180
+
+
+
+ model://ground_plane
+
+
+ model://ardrone
+
+
+
\ No newline at end of file
diff --git a/sw/airborne/boards/pc_sim.h b/sw/airborne/boards/pc_sim.h
index bdbc806279..11786598a5 100644
--- a/sw/airborne/boards/pc_sim.h
+++ b/sw/airborne/boards/pc_sim.h
@@ -20,4 +20,8 @@
#endif
extern struct video_config_t webcam;
+// Simulated cameras, see modules/computer_vision/video_thread_nps.c
+extern struct video_config_t front_camera;
+extern struct video_config_t bottom_camera;
+
#endif /* CONFIG_PC_SIM_H */
diff --git a/sw/airborne/modules/computer_vision/video_thread_nps.c b/sw/airborne/modules/computer_vision/video_thread_nps.c
index f1111e0d7d..438dee5e1b 100644
--- a/sw/airborne/modules/computer_vision/video_thread_nps.c
+++ b/sw/airborne/modules/computer_vision/video_thread_nps.c
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2015
+ * Copyright (C) 2017 Tom van Dijk
*
* This file is part of Paparazzi.
*
@@ -20,62 +20,95 @@
*/
/**
- * Dummy C implementation for simulation
- * The V4L2 could also work in simulation, but must be adapted a bit.
+ * Video thread dummy for simulation. *
+ *
+ * Keeps track of added devices, which can be referenced by simulation code
+ * such as in simulator/nps/fdm_gazebo.c.
*/
// Own header
+#include "video_thread_nps.h"
#include "video_thread.h"
#include "cv.h"
#include "lib/vision/image.h"
+#include "modules/computer_vision/lib/v4l/v4l2.h"
+#include "peripherals/video_device.h"
-// Initialize the video_thread structure with the defaults
-struct video_thread_t video_thread = {
- .is_running = FALSE
+#include
+
+// Camera structs for use in modules.
+// See boards/pc_sim.h
+// Default values from ARDrone can be overwritten by simulator.
+struct video_config_t front_camera = {
+ .output_size = { .w = 1280, .h = 720 },
+ .sensor_size = { .w = 1280, .h = 720 },
+ .crop = { .x = 0, .y = 0, .w = 1280, .h = 720 },
+ .dev_name = "front_camera",
+ .subdev_name = NULL,
+ .format = V4L2_PIX_FMT_UYVY,
+ .buf_cnt = 10,
+ .filters = 0,
+ .cv_listener = NULL,
+ .fps = 0
};
+struct video_config_t bottom_camera = {
+ .output_size = { .w = 320, .h = 240 },
+ .sensor_size = { .w = 320, .h = 240 },
+ .crop = { .x = 0, .y = 0, .w = 320, .h = 240 },
+ .dev_name = "bottom_camera",
+ .subdev_name = NULL,
+ .format = V4L2_PIX_FMT_UYVY,
+ .buf_cnt = 10,
+ .filters = 0,
+ .cv_listener = NULL,
+ .fps = 0
+};
+
+// Keep track of added devices.
+struct video_config_t *cameras[VIDEO_THREAD_MAX_CAMERAS] = { NULL };
+
// All dummy functions
-void video_thread_init(void) {}
+void video_thread_init(void)
+{
+}
void video_thread_periodic(void)
{
- struct image_t img;
- image_create(&img, 320, 240, IMAGE_YUV422);
- int i, j;
- uint8_t u, v;
-#ifdef SMARTUAV_SIMULATOR
- SMARTUAV_IMPORT(&img);
-#else
- if (video_thread.is_running) {
- u = 0;
- v = 255;
- } else {
- u = 255;
- v = 0;
- }
- uint8_t *p = (uint8_t *) img.buf;
- for (j = 0; j < img.h; j++) {
- for (i = 0; i < img.w; i += 2) {
- *p++ = u;
- *p++ = j;
- *p++ = v;
- *p++ = j;
- }
- }
- video_thread.is_running = ! video_thread.is_running;
-#endif
-
- // Calling this function will not work because video_config_t is NULL (?) and img
- // has no timestamp
- // Commenting out for now
- // cv_run_device(NULL,&img);
-
- image_free(&img);
}
-void video_thread_start(void) {}
-void video_thread_stop(void) {}
-void video_thread_take_shot(bool take __attribute__((unused))) {}
+void video_thread_start(void)
+{
+}
+void video_thread_stop(void)
+{
+}
-bool add_video_device(struct video_config_t *device __attribute__((unused))){ return true; }
+/**
+ * Keep track of video devices added by modules.
+ */
+bool add_video_device(struct video_config_t *device)
+{
+ // Loop over camera array
+ for (int i = 0; i < VIDEO_THREAD_MAX_CAMERAS; ++i) {
+ // If device is already registered, break
+ if (cameras[i] == device) {
+ break;
+ }
+ // If camera slot is already used, continue
+ if (cameras[i] != NULL) {
+ continue;
+ }
+ // No initialization, should be handled by simulation!
+ // Store device pointer
+ cameras[i] = device;
+ // Debug statement
+ printf("[video_thread_nps] Added %s to camera array.\n",
+ device->dev_name);
+ // Successfully added
+ return true;
+ }
+ // Camera array is full
+ return false;
+}
diff --git a/sw/airborne/modules/computer_vision/video_thread_nps.h b/sw/airborne/modules/computer_vision/video_thread_nps.h
new file mode 100644
index 0000000000..feff357c9f
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/video_thread_nps.h
@@ -0,0 +1,39 @@
+/*
+ * Copyright (C) 2017 Tom van Dijk
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ *
+ */
+
+/**
+ * @file modules/computer_vision/video_thread_nps.h
+ *
+ * This header gives NPS access to the list of added cameras.
+ */
+
+#ifndef VIDEO_THREAD_NPS_H
+#define VIDEO_THREAD_NPS_H
+
+#include "peripherals/video_device.h"
+
+#ifndef VIDEO_THREAD_MAX_CAMERAS
+#define VIDEO_THREAD_MAX_CAMERAS 4
+#endif
+
+extern struct video_config_t *cameras[VIDEO_THREAD_MAX_CAMERAS];
+
+#endif
diff --git a/sw/airborne/modules/computer_vision/viewvideo_nps.c b/sw/airborne/modules/computer_vision/viewvideo_nps.c
deleted file mode 100644
index cd1120521d..0000000000
--- a/sw/airborne/modules/computer_vision/viewvideo_nps.c
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * Copyright (C) 2012-2013
- *
- * This file is part of Paparazzi.
- *
- * Paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * Paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-/**
- * Dummy C implementation for simulation
- * The V4L2 could also work in simulation, but must be adapted a bit.
- */
-
-// Own header
-#include "viewvideo.h"
-
-// Initialize the viewvideo structure with the defaults
-struct viewvideo_t viewvideo = {
- .is_streaming = FALSE,
- .downsize_factor = 1,
- .quality_factor = 99,
-};
-
-// All dummy functions
-void viewvideo_init(void) {}
-void viewvideo_periodic(void) {}
-void viewvideo_start(void) {}
-void viewvideo_stop(void) {}
-void viewvideo_take_shot(bool take __attribute__((unused))) {}
diff --git a/sw/simulator/nps/nps_fdm_gazebo.cpp b/sw/simulator/nps/nps_fdm_gazebo.cpp
new file mode 100644
index 0000000000..0e47bce2aa
--- /dev/null
+++ b/sw/simulator/nps/nps_fdm_gazebo.cpp
@@ -0,0 +1,588 @@
+/*
+ * Copyright (C) 2017 Tom van Dijk
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file nps_fdm_gazebo.cpp
+ * Flight Dynamics Model (FDM) for NPS using Gazebo.
+ *
+ * This is an FDM for NPS that uses Gazebo as the simulation engine.
+ */
+
+// The transition from Gazebo 7 to 8 deprecates a large number of functions.
+// Ignore these errors for now...
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+extern "C" {
+#include
+
+#include "nps_fdm.h"
+#include "math/pprz_algebra_double.h"
+
+#include "generated/airframe.h"
+#include "autopilot.h"
+
+#ifdef NPS_SIMULATE_VIDEO
+#include "modules/computer_vision/cv.h"
+#include "modules/computer_vision/video_thread_nps.h"
+#include "modules/computer_vision/lib/vision/image.h"
+#include "mcu_periph/sys_time.h"
+#endif
+}
+
+using namespace std;
+
+#ifndef NPS_GAZEBO_WORLD
+#define NPS_GAZEBO_WORLD "ardrone.world"
+#endif
+#ifndef NPS_GAZEBO_AC_NAME
+#define NPS_GAZEBO_AC_NAME "paparazzi_uav"
+#endif
+
+// Add video handling functions if req'd.
+#ifdef NPS_SIMULATE_VIDEO
+static void init_gazebo_video(void);
+static void gazebo_read_video(void);
+static void read_image(
+ struct image_t *img,
+ gazebo::sensors::CameraSensorPtr cam);
+struct gazebocam_t {
+ gazebo::sensors::CameraSensorPtr cam;
+ gazebo::common::Time last_measurement_time;
+};
+static struct gazebocam_t gazebo_cams[VIDEO_THREAD_MAX_CAMERAS] =
+{ { NULL, 0 } };
+#endif
+
+/// Holds all necessary NPS FDM state information
+struct NpsFdm fdm;
+
+// Pointer to Gazebo data
+static bool gazebo_initialized = false;
+static gazebo::physics::ModelPtr model = NULL;
+
+// Helper functions
+static void init_gazebo(void);
+static void gazebo_read(void);
+static void gazebo_write(double commands[], int commands_nb);
+
+// Conversion routines
+inline struct EcefCoor_d to_pprz_ecef(ignition::math::Vector3d ecef_i)
+{
+ struct EcefCoor_d ecef_p;
+ ecef_p.x = ecef_i.X();
+ ecef_p.y = ecef_i.Y();
+ ecef_p.z = ecef_i.Z();
+ return ecef_p;
+}
+
+inline struct NedCoor_d to_pprz_ned(ignition::math::Vector3d global)
+{
+ struct NedCoor_d ned;
+ ned.x = global.Y();
+ ned.y = global.X();
+ ned.z = -global.Z();
+ return ned;
+}
+
+inline struct LlaCoor_d to_pprz_lla(ignition::math::Vector3d lla_i)
+{
+ struct LlaCoor_d lla_p;
+ lla_p.lat = lla_i.X();
+ lla_p.lon = lla_i.Y();
+ lla_p.alt = lla_i.Z();
+ return lla_p;
+}
+
+inline struct DoubleVect3 to_pprz_body(gazebo::math::Vector3 body_g)
+{
+ struct DoubleVect3 body_p;
+ body_p.x = body_g.x;
+ body_p.y = -body_g.y;
+ body_p.z = -body_g.z;
+ return body_p;
+}
+
+inline struct DoubleRates to_pprz_rates(gazebo::math::Vector3 body_g)
+{
+ struct DoubleRates body_p;
+ body_p.p = body_g.x;
+ body_p.q = -body_g.y;
+ body_p.r = -body_g.z;
+ return body_p;
+}
+
+inline struct DoubleEulers to_pprz_eulers(gazebo::math::Quaternion quat)
+{
+ struct DoubleEulers eulers;
+ eulers.psi = -quat.GetYaw() - M_PI / 2;
+ eulers.theta = -quat.GetPitch();
+ eulers.phi = quat.GetRoll();
+ return eulers;
+}
+
+inline struct DoubleVect3 to_pprz_ltp(gazebo::math::Vector3 xyz)
+{
+ struct DoubleVect3 ltp;
+ ltp.x = xyz.y;
+ ltp.y = xyz.x;
+ ltp.z = -xyz.z;
+ return ltp;
+}
+
+// External functions, interface with Paparazzi's NPS as declared in nps_fdm.h
+
+/**
+ * Set JSBsim specific fields that are not used for Gazebo.
+ * @param dt
+ */
+void nps_fdm_init(double dt)
+{
+ fdm.init_dt = dt; // JSBsim specific
+ fdm.curr_dt = dt; // JSBsim specific
+ fdm.nan_count = 0; // JSBsim specific
+}
+
+/**
+ * Update the simulation state.
+ * @param launch
+ * @param commands
+ * @param commands_nb
+ */
+void nps_fdm_run_step(
+ bool launch __attribute__((unused)),
+ double *commands,
+ int commands_nb)
+{
+ // Initialize Gazebo if req'd.
+ // Initialization is peformed here instead of in nps_fdm_init because:
+ // - Video devices need to added at this point. Video devices have not been
+ // added yet when nps_fdm_init is called.
+ // - nps_fdm_init runs on a different thread then nps_fdm_run_step, which
+ // causes problems with Gazebo.
+ if (!gazebo_initialized) {
+ init_gazebo();
+ gazebo_read();
+#ifdef NPS_SIMULATE_VIDEO
+ init_gazebo_video();
+#endif
+ gazebo_initialized = true;
+ }
+
+ // Update the simulation for a single timestep.
+ gazebo::runWorld(model->GetWorld(), 1);
+ gazebo::sensors::run_once();
+ gazebo_write(commands, commands_nb);
+ gazebo_read();
+#ifdef NPS_SIMULATE_VIDEO
+ gazebo_read_video();
+#endif
+}
+
+// TODO Atmosphere functions have not been implemented yet.
+// Starting at version 8, Gazebo has its own atmosphere and wind model.
+void nps_fdm_set_wind(
+ double speed __attribute__((unused)),
+ double dir __attribute__((unused)))
+{
+}
+
+void nps_fdm_set_wind_ned(
+ double wind_north __attribute__((unused)),
+ double wind_east __attribute__((unused)),
+ double wind_down __attribute__((unused)))
+{
+}
+
+void nps_fdm_set_turbulence(
+ double wind_speed __attribute__((unused)),
+ int turbulence_severity __attribute__((unused)))
+{
+}
+
+/** Set temperature in degrees Celcius at given height h above MSL */
+void nps_fdm_set_temperature(
+ double temp __attribute__((unused)),
+ double h __attribute__((unused)))
+{
+}
+
+// Internal functions
+/**
+ * Set up a Gazebo server.
+ *
+ * Starts a Gazebo server, adds conf/simulator/gazebo/models to its model path
+ * and loads the world specified by NPS_GAZEBO_WORLD.
+ *
+ * This function also obtaines a pointer to the aircraft model, named
+ * NPS_GAZEBO_AC_NAME ('paparazzi_uav' by default). This pointer, 'model',
+ * is used to read the state and write actuator commands in gazebo_read and
+ * _write.
+ */
+static void init_gazebo(void)
+{
+ string gazebo_home = "/conf/simulator/gazebo/";
+ string pprz_home(getenv("PAPARAZZI_HOME"));
+ string gazebodir = pprz_home + gazebo_home;
+ cout << "Gazebo directory: " << gazebodir << endl;
+
+ if (!gazebo::setupServer(0, NULL)) {
+ cout << "Failed to start Gazebo, exiting." << endl;
+ std::exit(-1);
+ }
+
+ cout << "Add Paparazzi model path: " << gazebodir + "models/" << endl;
+ gazebo::common::SystemPaths::Instance()->AddModelPaths(
+ gazebodir + "models/");
+
+ cout << "Load world: " << gazebodir + "world/" + NPS_GAZEBO_WORLD << endl;
+ gazebo::physics::WorldPtr world = gazebo::loadWorld(
+ gazebodir + "world/" + NPS_GAZEBO_WORLD);
+ if (!world) {
+ cout << "Failed to open world, exiting." << endl;
+ std::exit(-1);
+ }
+
+ cout << "Get pointer to aircraft: " << NPS_GAZEBO_AC_NAME << endl;
+ model = world->GetModel(NPS_GAZEBO_AC_NAME);
+ if (!model) {
+ cout << "Failed to find '" << NPS_GAZEBO_AC_NAME << "', exiting."
+ << endl;
+ std::exit(-1);
+ }
+
+ // Initialize sensors
+ gazebo::sensors::run_once(true);
+ gazebo::sensors::run_threads();
+ gazebo::runWorld(world, 1);
+ cout << "Sensors initialized..." << endl;
+
+ cout << "Gazebo initialized successfully!" << endl;
+}
+
+/**
+ * Read Gazebo's simulation state and store the results in the fdm struct used
+ * by NPS.
+ *
+ * Not all fields are filled at the moment, as some of them are unused by
+ * paparazzi (see comments) and others are not available in Gazebo 7
+ * (atmosphere).
+ */
+static void gazebo_read(void)
+{
+ gazebo::physics::WorldPtr world = model->GetWorld();
+ gazebo::math::Pose pose = model->GetWorldPose(); // In LOCAL xyz frame
+ gazebo::math::Vector3 vel = model->GetWorldLinearVel();
+ gazebo::math::Vector3 accel = model->GetWorldLinearAccel();
+ gazebo::math::Vector3 ang_vel = model->GetWorldAngularVel();
+ gazebo::common::SphericalCoordinatesPtr sphere =
+ world->GetSphericalCoordinates();
+ gazebo::math::Quaternion local_to_global_quat(0, 0,
+ -sphere->HeadingOffset().Radian());
+
+ /* Fill FDM struct */
+ fdm.time = world->GetSimTime().Double();
+
+ // init_dt: unused
+ // curr_dt: unused
+ // on_ground: unused
+ // nan_count: unused
+
+ /* position */
+ fdm.ecef_pos = to_pprz_ecef(
+ sphere->PositionTransform(pose.pos.Ign(),
+ gazebo::common::SphericalCoordinates::LOCAL,
+ gazebo::common::SphericalCoordinates::ECEF));
+ fdm.ltpprz_pos = to_pprz_ned(
+ sphere->PositionTransform(pose.pos.Ign(),
+ gazebo::common::SphericalCoordinates::LOCAL,
+ gazebo::common::SphericalCoordinates::GLOBAL));
+ fdm.lla_pos = to_pprz_lla(
+ sphere->PositionTransform(pose.pos.Ign(),
+ gazebo::common::SphericalCoordinates::LOCAL,
+ gazebo::common::SphericalCoordinates::SPHERICAL));
+ fdm.hmsl = pose.pos.z;
+ /* debug positions */
+ fdm.lla_pos_pprz = fdm.lla_pos; // Don't really care...
+ fdm.lla_pos_geod = fdm.lla_pos;
+ fdm.lla_pos_geoc = fdm.lla_pos;
+ fdm.agl = pose.pos.z; // TODO Measure with sensor
+
+ /* velocity */
+ fdm.ecef_ecef_vel = to_pprz_ecef(
+ sphere->VelocityTransform(vel.Ign(),
+ gazebo::common::SphericalCoordinates::LOCAL,
+ gazebo::common::SphericalCoordinates::ECEF));
+ fdm.body_ecef_vel = to_pprz_body(pose.rot.RotateVectorReverse(vel)); // Note: unused
+ fdm.ltp_ecef_vel = to_pprz_ned(
+ sphere->VelocityTransform(vel.Ign(),
+ gazebo::common::SphericalCoordinates::LOCAL,
+ gazebo::common::SphericalCoordinates::GLOBAL));
+ fdm.ltpprz_ecef_vel = fdm.ltp_ecef_vel; // ???
+
+ /* acceleration */
+ fdm.ecef_ecef_accel = to_pprz_ecef(
+ sphere->VelocityTransform(accel.Ign(),
+ gazebo::common::SphericalCoordinates::LOCAL,
+ gazebo::common::SphericalCoordinates::ECEF)); // Note: unused
+ fdm.body_ecef_accel = to_pprz_body(pose.rot.RotateVectorReverse(accel));
+ fdm.ltp_ecef_accel = to_pprz_ned(
+ sphere->VelocityTransform(accel.Ign(),
+ gazebo::common::SphericalCoordinates::LOCAL,
+ gazebo::common::SphericalCoordinates::GLOBAL)); // Note: unused
+ fdm.ltpprz_ecef_accel = fdm.ltp_ecef_accel; // ???
+ fdm.body_inertial_accel = fdm.body_ecef_accel; // Approximate, unused.
+ fdm.body_accel = to_pprz_body(
+ pose.rot.RotateVectorReverse(accel.Ign() - world->Gravity()));
+
+ /* attitude */
+ // ecef_to_body_quat: unused
+ fdm.ltp_to_body_eulers = to_pprz_eulers(local_to_global_quat * pose.rot);
+ double_quat_of_eulers(&(fdm.ltp_to_body_quat), &(fdm.ltp_to_body_eulers));
+ fdm.ltpprz_to_body_quat = fdm.ltp_to_body_quat; // unused
+ fdm.ltpprz_to_body_eulers = fdm.ltp_to_body_eulers; // unused
+
+ /* angular velocity */
+ fdm.body_ecef_rotvel = to_pprz_rates(pose.rot.RotateVectorReverse(ang_vel));
+ fdm.body_inertial_rotvel = fdm.body_ecef_rotvel; // Approximate
+
+ /* angular acceleration */
+ // body_ecef_rotaccel: unused
+ // body_inertial_rotaccel: unused
+ /* misc */
+ fdm.ltp_g = to_pprz_ltp(
+ sphere->VelocityTransform(-1 * world->Gravity(),
+ gazebo::common::SphericalCoordinates::LOCAL,
+ gazebo::common::SphericalCoordinates::GLOBAL)); // unused
+ fdm.ltp_h = to_pprz_ltp(
+ sphere->VelocityTransform(world->MagneticField(),
+ gazebo::common::SphericalCoordinates::LOCAL,
+ gazebo::common::SphericalCoordinates::GLOBAL));
+
+ /* atmosphere */
+#if GAZEBO_MAJOR_VERSION >= 8 && 0 // TODO implement
+
+#else
+ // Gazebo versions < 8 do not have atmosphere or wind support
+ // Use placeholder values. Valid for low altitude, low speed flights.
+ fdm.wind = {.x = 0, .y = 0, .z = 0};
+ fdm.pressure_sl = 101325; // Pa
+
+ fdm.airspeed = 0;
+ fdm.pressure = fdm.pressure_sl; // Pa
+ fdm.dynamic_pressure = fdm.pressure_sl; // Pa
+ fdm.temperature = 20.0; // C
+ fdm.aoa = 0; // rad
+ fdm.sideslip = 0; // rad
+#endif
+ /* flight controls: unused */
+ fdm.elevator = 0;
+ fdm.flap = 0;
+ fdm.left_aileron = 0;
+ fdm.right_aileron = 0;
+ fdm.rudder = 0;
+ /* engine: unused */
+ fdm.num_engines = 0;
+}
+
+/**
+ * Write actuator commands to Gazebo.
+ *
+ * This function takes the normalized commands and applies them as forces and
+ * torques in Gazebo. Since the commands are normalized in [0,1], their
+ * thrusts (NPS_ACTUATOR_THRUSTS) and torques (NPS_ACTUATOR_TORQUES) need to
+ * be specified in the airframe file. Their values need to be specified in the
+ * same order as the NPS_ACTUATOR_NAMES and should be provided in SI units.
+ * See conf/airframes/examples/ardrone2_gazebo.xml for an example.
+ *
+ * The forces and torques are applied to (the origin of) the links named in
+ * NPS_ACTUATOR_NAMES. See conf/simulator/gazebo/models/ardrone/ardrone.sdf
+ * for an example.
+ *
+ * @param commands
+ * @param commands_nb
+ */
+static void gazebo_write(double commands[], int commands_nb)
+{
+ const string names[] = NPS_ACTUATOR_NAMES;
+ const double thrusts[] = { NPS_ACTUATOR_THRUSTS };
+ const double torques[] = { NPS_ACTUATOR_TORQUES };
+
+ for (int i = 0; i < commands_nb; ++i) {
+ double thrust = autopilot.motors_on ? thrusts[i] * commands[i] : 0.0;
+ double torque = autopilot.motors_on ? torques[i] * commands[i] : 0.0;
+ gazebo::physics::LinkPtr link = model->GetLink(names[i]);
+ link->AddRelativeForce(gazebo::math::Vector3(0, 0, thrust));
+ link->AddRelativeTorque(gazebo::math::Vector3(0, 0, torque));
+ // cout << "Motor '" << link->GetName() << "': thrust = " << thrust
+ // << " N, torque = " << torque << " Nm" << endl;
+ }
+}
+
+#ifdef NPS_SIMULATE_VIDEO
+/**
+ * Set up cameras.
+ *
+ * This function finds the video devices added through add_video_device
+ * (sw/airborne/modules/computer_vision/cv.h). The camera links in the Gazebo AC
+ * model should have the same name as the .dev_name field in the corresponding
+ * video_config_t struct stored in 'cameras[]' (computer_vision/
+ * video_thread_nps.h). Pointers to Gazebo's cameras are stored in gazebo_cams
+ * at the same index as their 'cameras[]' counterpart.
+ *
+ * The video_config_t parameters are updated using the values provided by
+ * Gazebo. This should simplify the use of different UAVs with different camera
+ * setups.
+ */
+static void init_gazebo_video(void)
+{
+ gazebo::sensors::SensorManager *mgr =
+ gazebo::sensors::SensorManager::Instance();
+
+ cout << "Initializing cameras..." << endl;
+ // Loop over cameras registered in video_thread_nps
+ for (int i = 0; i < VIDEO_THREAD_MAX_CAMERAS && cameras[i] != NULL; ++i) {
+ // Find link in gazebo model
+ cout << "Setting up '" << cameras[i]->dev_name << "'... ";
+ gazebo::physics::LinkPtr link = model->GetLink(cameras[i]->dev_name);
+ if (!link) {
+ cout << "ERROR: Link '" << cameras[i]->dev_name << "' not found!"
+ << endl;
+ ;
+ continue;
+ }
+ // Get a pointer to the sensor using its full name
+ if (link->GetSensorCount() != 1) {
+ cout << "ERROR: Link '" << link->GetName()
+ << "' should only contain 1 sensor!" << endl;
+ continue;
+ }
+ string name = link->GetSensorName(0);
+ gazebo::sensors::CameraSensorPtr cam = static_pointer_cast
+ < gazebo::sensors::CameraSensor > (mgr->GetSensor(name));
+ if (!cam) {
+ cout << "ERROR: Could not get pointer to '" << name << "'!" << endl;
+ continue;
+ }
+ // Activate sensor
+ cam->SetActive(true);
+ // Add to list of cameras
+ gazebo_cams[i].cam = cam;
+ gazebo_cams[i].last_measurement_time = cam->LastMeasurementTime();
+ // Copy video_config settings from Gazebo's camera
+ cameras[i]->output_size.w = cam->ImageWidth();
+ cameras[i]->output_size.h = cam->ImageHeight();
+ cameras[i]->sensor_size.w = cam->ImageWidth();
+ cameras[i]->sensor_size.h = cam->ImageHeight();
+ cameras[i]->crop.w = cam->ImageWidth();
+ cameras[i]->crop.h = cam->ImageHeight();
+ cameras[i]->fps = cam->UpdateRate();
+ cout << "ok" << endl;
+ }
+}
+
+/**
+ * Read camera images.
+ *
+ * Polls gazebo cameras. If the last measurement time has been updated, a new
+ * frame is available. This frame is converted to Paparazzi's UYVY format
+ * and passed to cv_run_device which runs the callbacks registered by various
+ * modules.
+ */
+static void gazebo_read_video(void)
+{
+ for (int i = 0; i < VIDEO_THREAD_MAX_CAMERAS; ++i) {
+ gazebo::sensors::CameraSensorPtr &cam = gazebo_cams[i].cam;
+ // Skip unregistered or unfound cameras
+ if (cam == NULL) { continue; }
+ // Skip if not updated
+ // Also skip when LastMeasurementTime() is zero (workaround)
+ if (cam->LastMeasurementTime() == gazebo_cams[i].last_measurement_time
+ || cam->LastMeasurementTime() == 0) { continue; }
+ // Grab image, convert and send to video thread
+ struct image_t img;
+ read_image(&img, cam);
+ cv_run_device(cameras[i], &img);
+ // Free image buffer after use.
+ image_free(&img);
+ // Keep track of last update time.
+ gazebo_cams[i].last_measurement_time = cam->LastMeasurementTime();
+ }
+}
+
+/**
+ * Read Gazebo image and convert.
+ *
+ * Converts the current camera frame to the format used by Paparazzi. This
+ * includes conversion to UYVY. Gazebo's simulation time is used for the image
+ * timestamp.
+ *
+ * @param img
+ * @param cam
+ */
+static void read_image(
+ struct image_t *img,
+ gazebo::sensors::CameraSensorPtr cam)
+{
+ image_create(img, cam->ImageWidth(), cam->ImageHeight(), IMAGE_YUV422);
+ // Convert Gazebo's *RGB888* image to Paparazzi's YUV422
+ const uint8_t *data_rgb = cam->ImageData();
+ uint8_t *data_yuv = (uint8_t *)(img->buf);
+ for (int x = 0; x < img->w; ++x) {
+ for (int y = 0; y < img->h; ++y) {
+ int idx_rgb = 3 * (img->w * y + x);
+ int idx_yuv = 2 * (img->w * y + x);
+ int idx_px = img->w * y + x;
+ if (idx_px % 2 == 0) { // Pick U or V
+ data_yuv[idx_yuv] = -0.148 * data_rgb[idx_rgb]
+ - 0.291 * data_rgb[idx_rgb + 1]
+ + 0.439 * data_rgb[idx_rgb + 2] + 128; // U
+ } else {
+ data_yuv[idx_yuv] = 0.439 * data_rgb[idx_rgb]
+ - 0.368 * data_rgb[idx_rgb + 1]
+ - 0.071 * data_rgb[idx_rgb + 2] + 128; // V
+ }
+ data_yuv[idx_yuv + 1] = 0.257 * data_rgb[idx_rgb]
+ + 0.504 * data_rgb[idx_rgb + 1]
+ + 0.098 * data_rgb[idx_rgb + 2] + 16; // Y
+ }
+ }
+ // Fill miscellaneous fields
+ gazebo::common::Time ts = cam->LastMeasurementTime();
+ img->ts.tv_sec = ts.sec;
+ img->ts.tv_usec = ts.nsec / 1000.0;
+ img->pprz_ts = ts.Double() * 1e6;
+ img->buf_idx = 0; // unused
+}
+#endif
+
+#pragma GCC diagnostic pop // Ignore -Wdeprecated-declarations