Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
nps_fdm_gazebo.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 Tom van Dijk, Kirk Scheper
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
29 #include <cstdio>
30 #include <cstdlib>
31 #include <string>
32 #include <iostream>
33 #include <sys/time.h>
34 
35 #include <gazebo/gazebo.hh>
36 #include <gazebo/physics/physics.hh>
37 #include <gazebo/sensors/sensors.hh>
38 #include <sdf/sdf.hh>
39 
40 #include <gazebo/gazebo_config.h>
41 #if GAZEBO_MAJOR_VERSION < 8
42 #error "Paparazzi only supports gazebo versions > 7, please upgrade to a more recent version of Gazebo"
43 #endif
44 
45 extern "C" {
46 #include "nps_fdm.h"
47 #include "nps_autopilot.h"
48 
49 #include "generated/airframe.h"
50 #include "generated/flight_plan.h"
51 #include "autopilot.h"
52 #include "subsystems/abi.h"
53 
54 #include "math/pprz_isa.h"
58 
60 }
61 
62 #if defined(NPS_DEBUG_VIDEO)
63 // Opencv tools
64 #include <opencv2/core/core.hpp>
65 #include <opencv2/highgui/highgui.hpp>
66 #include <opencv2/imgproc/imgproc.hpp>
67 #endif
68 
69 using namespace std;
70 
71 #ifndef NPS_GAZEBO_WORLD
72 #define NPS_GAZEBO_WORLD "empty.world"
73 #endif
74 #ifndef NPS_GAZEBO_AC_NAME
75 #define NPS_GAZEBO_AC_NAME "ardrone"
76 #endif
77 
78 // Add video handling functions if req'd.
79 #if NPS_SIMULATE_VIDEO
80 extern "C" {
84 #include "mcu_periph/sys_time.h"
85 #include "boards/bebop/mt9f002.h"
86 #include "boards/bebop/mt9v117.h"
87 struct mt9f002_t mt9f002 __attribute__((weak)); // Prevent undefined reference errors when Bebop code is not linked.
88 }
89 
90 static void init_gazebo_video(void);
91 static void gazebo_read_video(void);
92 static void read_image(
93  struct image_t *img,
94  gazebo::sensors::CameraSensorPtr cam);
95 struct gazebocam_t {
96  gazebo::sensors::CameraSensorPtr cam;
97  gazebo::common::Time last_measurement_time;
98 };
99 static struct gazebocam_t gazebo_cams[VIDEO_THREAD_MAX_CAMERAS] =
100 { { NULL, 0 } };
101 
102 // Reduce resolution of the simulated MT9F002 sensor (Bebop) to improve runtime
103 // performance at the cost of image resolution.
104 // Recommended values: 1 (realistic), 2, 4 (fast but slightly blurry)
105 #ifndef NPS_MT9F002_SENSOR_RES_DIVIDER
106 #define NPS_MT9F002_SENSOR_RES_DIVIDER 1
107 #endif
108 #endif // NPS_SIMULATE_VIDEO
109 
111  string names[NPS_COMMANDS_NB];
112  double thrusts[NPS_COMMANDS_NB];
113  double torques[NPS_COMMANDS_NB];
116  double max_ang_momentum[NPS_COMMANDS_NB];
117 };
118 
119 struct gazebo_actuators_t gazebo_actuators = { NPS_ACTUATOR_NAMES, NPS_ACTUATOR_THRUSTS, NPS_ACTUATOR_TORQUES, { }, { }, { }};
120 
121 #if NPS_SIMULATE_LASER_RANGE_ARRAY
122 extern "C" {
123 #include "subsystems/abi.h"
124 }
125 
126 static void gazebo_init_range_sensors(void);
127 static void gazebo_read_range_sensors(void);
128 
129 #endif
130 
131 std::shared_ptr<gazebo::sensors::SonarSensor> sonar = NULL;
132 
134 struct NpsFdm fdm;
135 
136 // Pointer to Gazebo data
137 static bool gazebo_initialized = false;
138 static gazebo::physics::ModelPtr model = NULL;
139 
140 // Get contact sensor
141 static gazebo::sensors::ContactSensorPtr ct;
142 
143 // Helper functions
144 static void init_gazebo(void);
145 static void gazebo_read(void);
146 static void gazebo_write(double act_commands[], int commands_nb);
147 
148 // Conversion routines
149 inline struct EcefCoor_d to_pprz_ecef(ignition::math::Vector3d ecef_i)
150 {
151  struct EcefCoor_d ecef_p;
152  ecef_p.x = ecef_i.X();
153  ecef_p.y = ecef_i.Y();
154  ecef_p.z = ecef_i.Z();
155  return ecef_p;
156 }
157 
158 inline struct NedCoor_d to_pprz_ned(ignition::math::Vector3d global)
159 {
160  struct NedCoor_d ned;
161  ned.x = global.Y();
162  ned.y = global.X();
163  ned.z = -global.Z();
164  return ned;
165 }
166 
167 inline struct LlaCoor_d to_pprz_lla(ignition::math::Vector3d lla_i)
168 {
169  struct LlaCoor_d lla_p;
170  lla_p.lat = lla_i.X();
171  lla_p.lon = lla_i.Y();
172  lla_p.alt = lla_i.Z();
173  return lla_p;
174 }
175 
176 inline struct DoubleVect3 to_pprz_body(ignition::math::Vector3d body_g)
177 {
178  struct DoubleVect3 body_p;
179  body_p.x = body_g.X();
180  body_p.y = -body_g.Y();
181  body_p.z = -body_g.Z();
182  return body_p;
183 }
184 
185 inline struct DoubleRates to_pprz_rates(ignition::math::Vector3d body_g)
186 {
187  struct DoubleRates body_p;
188  body_p.p = body_g.X();
189  body_p.q = -body_g.Y();
190  body_p.r = -body_g.Z();
191  return body_p;
192 }
193 
194 inline struct DoubleEulers to_pprz_eulers(ignition::math::Quaterniond quat)
195 {
196  struct DoubleEulers eulers;
197  eulers.psi = -quat.Yaw();
198  eulers.theta = -quat.Pitch();
199  eulers.phi = quat.Roll();
200  return eulers;
201 }
202 
203 inline struct DoubleEulers to_global_pprz_eulers(ignition::math::Quaterniond quat)
204 {
205  struct DoubleEulers eulers;
206  eulers.psi = -quat.Yaw() - M_PI / 2;
207  eulers.theta = -quat.Pitch();
208  eulers.phi = quat.Roll();
209  return eulers;
210 }
211 
212 inline struct DoubleVect3 to_pprz_ltp(ignition::math::Vector3d xyz)
213 {
214  struct DoubleVect3 ltp;
215  ltp.x = xyz.Y();
216  ltp.y = xyz.X();
217  ltp.z = -xyz.Z();
218  return ltp;
219 }
220 
221 // External functions, interface with Paparazzi's NPS as declared in nps_fdm.h
222 
227 void nps_fdm_init(double dt)
228 {
229  fdm.init_dt = dt; // JSBsim specific
230  fdm.curr_dt = dt; // JSBsim specific
231  fdm.nan_count = 0; // JSBsim specific
232 
233 #ifdef NPS_ACTUATOR_TIME_CONSTANTS
234  // Set up low-pass filter to simulate delayed actuator response
235  const float tau[NPS_COMMANDS_NB] = NPS_ACTUATOR_TIME_CONSTANTS;
236  for (int i = 0; i < NPS_COMMANDS_NB; i++) {
237  init_first_order_low_pass(&gazebo_actuators.lowpass[i], tau[i], dt, 0.f);
238  }
239 #ifdef NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM
240  // Set up high-pass filter to simulate spinup torque
241  const float Iwmax[NPS_COMMANDS_NB] = NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM;
242  for (int i = 0; i < NPS_COMMANDS_NB; i++) {
244  gazebo_actuators.max_ang_momentum[i] = Iwmax[i];
245  }
246 #endif
247 #endif
248 }
249 
257  bool launch __attribute__((unused)),
258  double *act_commands,
259  int commands_nb)
260 {
261  // Initialize Gazebo if req'd.
262  // Initialization is peformed here instead of in nps_fdm_init because:
263  // - Video devices need to added at this point. Video devices have not been
264  // added yet when nps_fdm_init is called.
265  // - nps_fdm_init runs on a different thread then nps_fdm_run_step, which
266  // causes problems with Gazebo.
267  if (!gazebo_initialized) {
268  init_gazebo();
269  gazebo_read();
270 #if NPS_SIMULATE_VIDEO
271  init_gazebo_video();
272 #endif
273 #if NPS_SIMULATE_LASER_RANGE_ARRAY
274  gazebo_init_range_sensors();
275 #endif
276  gazebo_initialized = true;
277  }
278 
279  // Update the simulation for a single timestep.
280  gazebo::runWorld(model->GetWorld(), 1);
281  gazebo::sensors::run_once();
282  gazebo_write(act_commands, commands_nb);
283  gazebo_read();
284 #if NPS_SIMULATE_VIDEO
285  gazebo_read_video();
286 #endif
287 #if NPS_SIMULATE_LASER_RANGE_ARRAY
288  gazebo_read_range_sensors();
289 #endif
290 
291 }
292 
293 // TODO Atmosphere functions have not been implemented yet.
294 // Starting at version 8, Gazebo has its own atmosphere and wind model.
296  double speed __attribute__((unused)),
297  double dir __attribute__((unused)))
298 {
299 }
300 
302  double wind_north __attribute__((unused)),
303  double wind_east __attribute__((unused)),
304  double wind_down __attribute__((unused)))
305 {
306 }
307 
309  double wind_speed __attribute__((unused)),
310  int turbulence_severity __attribute__((unused)))
311 {
312 }
313 
316  double temp __attribute__((unused)),
317  double h __attribute__((unused)))
318 {
319 }
320 
321 // Internal functions
333 static void init_gazebo(void)
334 {
335  string gazebo_home = "/conf/simulator/gazebo/";
336  string pprz_home(getenv("PAPARAZZI_HOME"));
337  string gazebodir = pprz_home + gazebo_home;
338  cout << "Gazebo directory: " << gazebodir << endl;
339 
340  if (getenv("ROS_MASTER_URI")) {
341  // Launch with ROS support
342  cout << "Add ROS plugins... ";
343  gazebo::addPlugin("libgazebo_ros_paths_plugin.so");
344  gazebo::addPlugin("libgazebo_ros_api_plugin.so");
345  cout << "ok" << endl;
346  }
347 
348  if (!gazebo::setupServer(0, NULL)) {
349  cout << "Failed to start Gazebo, exiting." << endl;
350  std::exit(-1);
351  }
352 
353  cout << "Add Paparazzi paths: " << gazebodir << endl;
354  gazebo::common::SystemPaths::Instance()->AddModelPaths(gazebodir + "models/");
355  sdf::addURIPath("model://", gazebodir + "models/");
356  sdf::addURIPath("world://", gazebodir + "world/");
357 
358  cout << "Add TU Delft paths: " << pprz_home + "/sw/ext/tudelft_gazebo_models/" << endl;
359  gazebo::common::SystemPaths::Instance()->AddModelPaths(pprz_home + "/sw/ext/tudelft_gazebo_models/models/");
360  sdf::addURIPath("model://", pprz_home + "/sw/ext/tudelft_gazebo_models/models/");
361  sdf::addURIPath("world://", pprz_home + "/sw/ext/tudelft_gazebo_models/world/");
362 
363  // get vehicles
364  string vehicle_uri = "model://" + string(NPS_GAZEBO_AC_NAME) + "/" + string(NPS_GAZEBO_AC_NAME) + ".sdf";
365  string vehicle_filename = sdf::findFile(vehicle_uri, false);
366  if (vehicle_filename.empty()) {
367  cout << "ERROR, could not find vehicle " + vehicle_uri << endl;
368  std::exit(-1);
369  }
370  cout << "Load vehicle: " << vehicle_filename << endl;
371  sdf::SDFPtr vehicle_sdf(new sdf::SDF());
372  sdf::init(vehicle_sdf);
373  if (!sdf::readFile(vehicle_filename, vehicle_sdf)) {
374  cout << "ERROR, could not read vehicle " + vehicle_filename << endl;
375  std::exit(-1);
376  }
377 
378  // add or set up sensors before the vehicle gets loaded
379 #if NPS_SIMULATE_VIDEO
380  // Cameras
381  sdf::ElementPtr link = vehicle_sdf->Root()->GetFirstElement()->GetElement("link");
382  while (link) {
383  if (link->Get<string>("name") == "front_camera" && link->GetElement("sensor")->Get<string>("name") == "mt9f002") {
384  if (NPS_MT9F002_SENSOR_RES_DIVIDER != 1) {
385  int w = link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("width")->Get<int>();
386  int h = link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("height")->Get<int>();
387  int env = link->GetElement("sensor")->GetElement("camera")->GetElement("lens")->GetElement("env_texture_size")->Get<int>();
388  link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("width")->Set(w / NPS_MT9F002_SENSOR_RES_DIVIDER);
389  link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("height")->Set(h / NPS_MT9F002_SENSOR_RES_DIVIDER);
390  link->GetElement("sensor")->GetElement("camera")->GetElement("lens")->GetElement("env_texture_size")->Set(env / NPS_MT9F002_SENSOR_RES_DIVIDER);
391  }
392  if (MT9F002_TARGET_FPS){
393  int fps = Min(MT9F002_TARGET_FPS, link->GetElement("sensor")->GetElement("update_rate")->Get<int>());
394  link->GetElement("sensor")->GetElement("update_rate")->Set(fps);
395  }
396  } else if (link->Get<string>("name") == "bottom_camera" && link->GetElement("sensor")->Get<string>("name") == "mt9v117") {
397  if (MT9V117_TARGET_FPS){
398  int fps = Min(MT9V117_TARGET_FPS, link->GetElement("sensor")->GetElement("update_rate")->Get<int>());
399  link->GetElement("sensor")->GetElement("update_rate")->Set(fps);
400  }
401  }
402  link = link->GetNextElement("link");
403  }
404 #endif
405 
406  // laser range array
407 #if NPS_SIMULATE_LASER_RANGE_ARRAY
408  vehicle_sdf->Root()->GetFirstElement()->AddElement("include")->GetElement("uri")->Set("model://range_sensors");
409  sdf::ElementPtr range_joint = vehicle_sdf->Root()->GetFirstElement()->AddElement("joint");
410  range_joint->GetAttribute("name")->Set("range_sensor_joint");
411  range_joint->GetAttribute("type")->Set("fixed");
412  range_joint->GetElement("parent")->Set("chassis");
413  range_joint->GetElement("child")->Set("range_sensors::base");
414 #endif
415 
416  // get world
417  string world_uri = "world://" + string(NPS_GAZEBO_WORLD);
418  string world_filename = sdf::findFile(world_uri, false);
419  if (world_filename.empty()) {
420  cout << "ERROR, could not find world " + world_uri << endl;
421  std::exit(-1);
422  }
423  cout << "Load world: " << world_filename << endl;
424  sdf::SDFPtr world_sdf(new sdf::SDF());
425  sdf::init(world_sdf);
426  if (!sdf::readFile(world_filename, world_sdf)) {
427  cout << "ERROR, could not read world " + world_filename << endl;
428  std::exit(-1);
429  }
430 
431  // add vehicles
432  world_sdf->Root()->GetFirstElement()->InsertElement(vehicle_sdf->Root()->GetFirstElement());
433 
434  world_sdf->Write(pprz_home + "/var/gazebo.world");
435 
436  gazebo::physics::WorldPtr world = gazebo::loadWorld(pprz_home + "/var/gazebo.world");
437  if (!world) {
438  cout << "Failed to open world, exiting." << endl;
439  std::exit(-1);
440  }
441 
442  cout << "Get pointer to aircraft: " << NPS_GAZEBO_AC_NAME << endl;
443  model = world->ModelByName(NPS_GAZEBO_AC_NAME);
444  if (!model) {
445  cout << "Failed to find '" << NPS_GAZEBO_AC_NAME << "', exiting."
446  << endl;
447  std::exit(-1);
448  }
449 
450  // Initialize sensors
451  gazebo::sensors::run_once(true);
452  gazebo::sensors::run_threads();
453  gazebo::runWorld(world, 1);
454  cout << "Sensors initialized..." << endl;
455 
456  // Find sensors
457  // Contact sensor
458  gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance();
459  ct = static_pointer_cast < gazebo::sensors::ContactSensor > (mgr->GetSensor("contactsensor"));
460  ct->SetActive(true);
461  // Sonar
462  sonar = static_pointer_cast<gazebo::sensors::SonarSensor>(mgr->GetSensor("sonarsensor"));
463  if(sonar) {
464  cout << "Found sonar" << endl;
465  }
466 
467  gazebo::physics::LinkPtr sonar_link = model->GetLink("sonar");
468  if (sonar_link) {
469  // Get a pointer to the sensor using its full name
470  if (sonar_link->GetSensorCount() != 1) {
471  cout << "ERROR: Link '" << sonar_link->GetName()
472  << "' should only contain 1 sensor!" << endl;
473  } else {
474  string name = sonar_link->GetSensorName(0);
475  sonar = static_pointer_cast< gazebo::sensors::SonarSensor > (mgr->GetSensor(name));
476  if (!sonar) {
477  cout << "ERROR: Could not get pointer to '" << name << "'!" << endl;
478  } else {
479  // Activate sensor
480  sonar->SetActive(true);
481  }
482  }
483  }
484 
485  // Overwrite motor directions as defined by motor_mixing
486 #ifdef MOTOR_MIXING_YAW_COEF
487  const double yaw_coef[] = MOTOR_MIXING_YAW_COEF;
488 
489  for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
490  gazebo_actuators.torques[i] = -fabs(gazebo_actuators.torques[i]) * yaw_coef[i] / fabs(yaw_coef[i]);
492  }
493 #endif
494  cout << "Gazebo initialized successfully!" << endl;
495 }
496 
505 static void gazebo_read(void)
506 {
507  static ignition::math::Vector3d vel_prev;
508  static double time_prev;
509 
510  gazebo::physics::WorldPtr world = model->GetWorld();
511  ignition::math::Pose3d pose = model->WorldPose(); // In LOCAL xyz frame
512  ignition::math::Vector3d vel = model->WorldLinearVel();
513  ignition::math::Vector3d ang_vel = model->WorldAngularVel();
514  gazebo::common::SphericalCoordinatesPtr sphere = world->SphericalCoords();
515  ignition::math::Quaterniond local_to_global_quat(0, 0, -sphere->HeadingOffset().Radian());
516 
517  /* Fill FDM struct */
518  fdm.time = world->SimTime().Double();
519 
520  // Find world acceleration by differentiating velocity
521  // model->GetWorldLinearAccel() does not seem to take the velocity_decay into account!
522  // Derivation of the velocity also follows the IMU implementation of Gazebo itself:
523  // https://bitbucket.org/osrf/gazebo/src/e26144434b932b4b6a760ddaa19cfcf9f1734748/gazebo/sensors/ImuSensor.cc?at=default&fileviewer=file-view-default#ImuSensor.cc-370
524  double dt = fdm.time - time_prev;
525  ignition::math::Vector3d accel = (vel - vel_prev) / dt;
526  vel_prev = vel;
527  time_prev = fdm.time;
528 
529  // init_dt: unused
530  // curr_dt: unused
531  // on_ground: unused
532  // nan_count: unused
533 
534  /* position */
535  fdm.ecef_pos = to_pprz_ecef(sphere->PositionTransform(pose.Pos(), gazebo::common::SphericalCoordinates::LOCAL,
536  gazebo::common::SphericalCoordinates::ECEF));
537  fdm.ltpprz_pos = to_pprz_ned(sphere->PositionTransform(pose.Pos(), gazebo::common::SphericalCoordinates::LOCAL,
538  gazebo::common::SphericalCoordinates::GLOBAL));
539  fdm.lla_pos = to_pprz_lla(sphere->PositionTransform(pose.Pos(), gazebo::common::SphericalCoordinates::LOCAL,
540  gazebo::common::SphericalCoordinates::SPHERICAL));
541  fdm.hmsl = pose.Pos().Z();
542 
543  /* debug positions */
544  fdm.lla_pos_pprz = fdm.lla_pos; // Don't really care...
547 
548  if(sonar) {
549  double agl = sonar->Range();
550  if (agl > sonar->RangeMax()) agl = -1.0;
551  fdm.agl = agl;
552  } else {
553  fdm.agl = pose.Pos().Z(); // TODO Measure with sensor
554  }
555 
556  /* velocity */
557  fdm.ecef_ecef_vel = to_pprz_ecef(sphere->VelocityTransform(vel, gazebo::common::SphericalCoordinates::LOCAL,
558  gazebo::common::SphericalCoordinates::ECEF));
559  fdm.body_ecef_vel = to_pprz_body(pose.Rot().RotateVectorReverse(vel)); // Note: unused
560  fdm.ltp_ecef_vel = to_pprz_ned(sphere->VelocityTransform(vel, gazebo::common::SphericalCoordinates::LOCAL,
561  gazebo::common::SphericalCoordinates::GLOBAL));
563 
564  /* acceleration */
565  fdm.ecef_ecef_accel = to_pprz_ecef(sphere->VelocityTransform(accel, gazebo::common::SphericalCoordinates::LOCAL,
566  gazebo::common::SphericalCoordinates::ECEF)); // Note: unused
567  fdm.body_ecef_accel = to_pprz_body(pose.Rot().RotateVectorReverse(accel));
568  fdm.ltp_ecef_accel = to_pprz_ned(sphere->VelocityTransform(accel, gazebo::common::SphericalCoordinates::LOCAL,
569  gazebo::common::SphericalCoordinates::GLOBAL)); // Note: unused
571  fdm.body_inertial_accel = fdm.body_ecef_accel; // Approximate, unused.
572  fdm.body_accel = to_pprz_body(pose.Rot().RotateVectorReverse(accel - world->Gravity()));
573 
574  /* attitude */
575  // ecef_to_body_quat: unused
576  fdm.ltp_to_body_eulers = to_global_pprz_eulers(local_to_global_quat * pose.Rot());
580 
581  /* angular velocity */
582  fdm.body_ecef_rotvel = to_pprz_rates(pose.Rot().RotateVectorReverse(ang_vel));
583  fdm.body_inertial_rotvel = fdm.body_ecef_rotvel; // Approximate
584 
585  /* angular acceleration */
586  // body_ecef_rotaccel: unused
587  // body_inertial_rotaccel: unused
588  /* misc */
589  fdm.ltp_g = to_pprz_ltp(sphere->VelocityTransform(-1 * world->Gravity(), gazebo::common::SphericalCoordinates::LOCAL,
590  gazebo::common::SphericalCoordinates::GLOBAL)); // unused
591  fdm.ltp_h = to_pprz_ltp(sphere->VelocityTransform(world->MagneticField(), gazebo::common::SphericalCoordinates::LOCAL,
592  gazebo::common::SphericalCoordinates::GLOBAL));
593 
594  /* atmosphere */
595 #if GAZEBO_MAJOR_VERSION >= 8 && 0 // TODO implement
596 
597 #else
598  // Gazebo versions < 8 do not have atmosphere or wind support
599  // Use placeholder values. Valid for low altitude, low speed flights.
600  fdm.wind = (struct DoubleVect3) {0, 0, 0};
601  fdm.pressure_sl = 101325; // Pa
602 
603  fdm.airspeed = 0;
606  fdm.temperature = 20.0; // C
607  fdm.aoa = 0; // rad
608  fdm.sideslip = 0; // rad
609 #endif
610  /* flight controls: unused */
611  fdm.elevator = 0;
612  fdm.flap = 0;
613  fdm.left_aileron = 0;
614  fdm.right_aileron = 0;
615  fdm.rudder = 0;
616  /* engine: unused */
617  fdm.num_engines = 0;
618 }
619 
637 static void gazebo_write(double act_commands[], int commands_nb)
638 {
639  for (int i = 0; i < commands_nb; ++i) {
640  // Thrust setpoint
641  double sp = autopilot.motors_on ? act_commands[i] : 0.0; // Normalized thrust setpoint
642 
643  // Actuator dynamics, forces and torques
644 #ifdef NPS_ACTUATOR_TIME_CONSTANTS
645  // Delayed response from actuator
646  double u = update_first_order_low_pass(&gazebo_actuators.lowpass[i], sp);// Normalized actual thrust
647 #else
648  double u = sp;
649 #endif
650  double thrust = gazebo_actuators.thrusts[i] * u;
651  double torque = gazebo_actuators.torques[i] * u;
652 
653 #ifdef NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM
654  // Spinup torque
656  double spinup_torque = gazebo_actuators.max_ang_momentum[i] / (2.0 * sqrt(u > 0.05 ? u : 0.05)) * udot;
657  torque += spinup_torque;
658 #endif
659 
660  // Apply force and torque to gazebo model
661  gazebo::physics::LinkPtr link = model->GetLink(gazebo_actuators.names[i]);
662  link->AddRelativeForce(ignition::math::Vector3d(0, 0, thrust));
663  link->AddRelativeTorque(ignition::math::Vector3d(0, 0, torque));
664  }
665 }
666 
667 #if NPS_SIMULATE_VIDEO
668 
682 static void init_gazebo_video(void)
683 {
684  gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance();
685 
686  cout << "Initializing cameras..." << endl;
687  // Loop over cameras registered in video_thread_nps
688  for (int i = 0; i < VIDEO_THREAD_MAX_CAMERAS && cameras[i] != NULL; ++i) {
689  // Find link in gazebo model
690  cout << "Setting up '" << cameras[i]->dev_name << "'... ";
691  gazebo::physics::LinkPtr link = model->GetLink(cameras[i]->dev_name);
692  if (!link) {
693  cout << "ERROR: Link '" << cameras[i]->dev_name << "' not found!"
694  << endl;
695  ;
696  continue;
697  }
698  // Get a pointer to the sensor using its full name
699  if (link->GetSensorCount() != 1) {
700  cout << "ERROR: Link '" << link->GetName()
701  << "' should only contain 1 sensor!" << endl;
702  continue;
703  }
704  string name = link->GetSensorName(0);
705  gazebo::sensors::CameraSensorPtr cam = static_pointer_cast
706  < gazebo::sensors::CameraSensor > (mgr->GetSensor(name));
707  if (!cam) {
708  cout << "ERROR: Could not get pointer to '" << name << "'!" << endl;
709  continue;
710  }
711  // Activate sensor
712  cam->SetActive(true);
713 
714  // Add to list of cameras
715  gazebo_cams[i].cam = cam;
716  gazebo_cams[i].last_measurement_time = cam->LastMeasurementTime();
717 
718  // set default camera settings
719  // Copy video_config settings from Gazebo's camera
720  cameras[i]->output_size.w = cam->ImageWidth();
721  cameras[i]->output_size.h = cam->ImageHeight();
722  cameras[i]->sensor_size.w = cam->ImageWidth();
723  cameras[i]->sensor_size.h = cam->ImageHeight();
724  cameras[i]->crop.w = cam->ImageWidth();
725  cameras[i]->crop.h = cam->ImageHeight();
726  cameras[i]->fps = 0;
731 
732  if (cam->Name() == "mt9f002") {
733  // See boards/bebop/mt9f002.c
741  cameras[i]->camera_intrinsics = {
743  .focal_y = MT9F002_FOCAL_Y,
744  .center_x = MT9F002_CENTER_X,
745  .center_y = MT9F002_CENTER_Y,
746  .Dhane_k = MT9F002_DHANE_K
747  };
748  } else if (cam->Name() == "mt9v117") {
749  // See boards/bebop/mt9v117.h
751  cameras[i]->camera_intrinsics = {
753  .focal_y = MT9V117_FOCAL_Y,
754  .center_x = MT9V117_CENTER_X,
755  .center_y = MT9V117_CENTER_Y,
756  .Dhane_k = MT9V117_DHANE_K
757  };
758  }
759  cout << "ok" << endl;
760  }
761 }
762 
771 static void gazebo_read_video(void)
772 {
773  for (int i = 0; i < VIDEO_THREAD_MAX_CAMERAS; ++i) {
774  gazebo::sensors::CameraSensorPtr &cam = gazebo_cams[i].cam;
775  // Skip unregistered or unfound cameras
776  if (cam == NULL) { continue; }
777  // Skip if not updated
778  // Also skip when LastMeasurementTime() is zero (workaround)
779  if ((cam->LastMeasurementTime() - gazebo_cams[i].last_measurement_time).Float() < 0.005
780  || cam->LastMeasurementTime() == 0) { continue; }
781  // Grab image, convert and send to video thread
782  struct image_t img;
783  read_image(&img, cam);
784 
785 #if NPS_DEBUG_VIDEO
786  cv::Mat RGB_cam(cam->ImageHeight(), cam->ImageWidth(), CV_8UC3, (uint8_t *)cam->ImageData());
787  cv::cvtColor(RGB_cam, RGB_cam, cv::COLOR_RGB2BGR);
788  cv::namedWindow(cameras[i]->dev_name, cv::WINDOW_AUTOSIZE); // Create a window for display.
789  cv::imshow(cameras[i]->dev_name, RGB_cam);
790  cv::waitKey(1);
791 #endif
792 
793  cv_run_device(cameras[i], &img);
794  // Free image buffer after use.
795  image_free(&img);
796  // Keep track of last update time.
797  gazebo_cams[i].last_measurement_time = cam->LastMeasurementTime();
798  }
799 }
800 
811 static void read_image(struct image_t *img, gazebo::sensors::CameraSensorPtr cam)
812 {
813  bool is_mt9f002 = false;
814  if (cam->Name() == "mt9f002") {
816  is_mt9f002 = true;
817  } else {
818  image_create(img, cam->ImageWidth(), cam->ImageHeight(), IMAGE_YUV422);
819  }
820 
821  // Convert Gazebo's *RGB888* image to Paparazzi's YUV422
822  const uint8_t *data_rgb = cam->ImageData();
823  uint8_t *data_yuv = (uint8_t *)(img->buf);
824  for (int x_yuv = 0; x_yuv < img->w; ++x_yuv) {
825  for (int y_yuv = 0; y_yuv < img->h; ++y_yuv) {
826  int x_rgb = x_yuv;
827  int y_rgb = y_yuv;
828  if (is_mt9f002) {
829  // Change sampling points for zoomed and/or cropped image.
830  // Use nearest-neighbour sampling for now.
831  x_rgb = (mt9f002.offset_x + ((float)x_yuv / img->w) * mt9f002.sensor_width)
832  / CFG_MT9F002_PIXEL_ARRAY_WIDTH * cam->ImageWidth();
833  y_rgb = (mt9f002.offset_y + ((float)y_yuv / img->h) * mt9f002.sensor_height)
834  / CFG_MT9F002_PIXEL_ARRAY_HEIGHT * cam->ImageHeight();
835  }
836  int idx_rgb = 3 * (cam->ImageWidth() * y_rgb + x_rgb);
837  int idx_yuv = 2 * (img->w * y_yuv + x_yuv);
838  int idx_px = img->w * y_yuv + x_yuv;
839  if (idx_px % 2 == 0) { // Pick U or V
840  data_yuv[idx_yuv] = - 0.148 * data_rgb[idx_rgb]
841  - 0.291 * data_rgb[idx_rgb + 1]
842  + 0.439 * data_rgb[idx_rgb + 2] + 128; // U
843  } else {
844  data_yuv[idx_yuv] = 0.439 * data_rgb[idx_rgb]
845  - 0.368 * data_rgb[idx_rgb + 1]
846  - 0.071 * data_rgb[idx_rgb + 2] + 128; // V
847  }
848  data_yuv[idx_yuv + 1] = 0.257 * data_rgb[idx_rgb]
849  + 0.504 * data_rgb[idx_rgb + 1]
850  + 0.098 * data_rgb[idx_rgb + 2] + 16; // Y
851  }
852  }
853  // Fill miscellaneous fields
854  gazebo::common::Time ts = cam->LastMeasurementTime();
855  img->ts.tv_sec = ts.sec;
856  img->ts.tv_usec = ts.nsec / 1000.0;
857  img->pprz_ts = ts.Double() * 1e6;
858  img->buf_idx = 0; // unused
859 }
860 #endif
861 
862 #if NPS_SIMULATE_LASER_RANGE_ARRAY
863 /*
864  * Simulate range sensors:
865  *
866  * In the airframe file, set NPS_SIMULATE_LASER_RANGE_ARRAY if you want to make use of the integrated Ray sensors.
867  * These are defined in their own model which is added to the ardrone.sdf (called range_sensors). Here you can add
868  * single ray point sensors with a specified position and orientation. It is also possible add noise.
869  *
870  * Within the airframe file (see e.g ardrone2_rangesensors_gazebo.xml), the amount of sensors
871  * (LASER_RANGE_ARRAY_NUM_SENSORS) and their orientations
872  * (LASER_RANGE_ARRAY_ORIENTATIONS={phi_1,theta_1,psi_1...phi_n,theta_n,psi_n} n = number of sensors) need to be
873  * specified as well. This is to keep it generic since this need to be done on the real platform with an external
874  * ray sensor. The function will compare the orientations from the ray sensors of gazebo, with the ones specified
875  * in the airframe, and will fill up an array to send and abi message to be used by other modules.
876  *
877  * NPS_GAZEBO_RANGE_SEND_AGL defines if the sensor that is defined as down should be used to send an AGL message.
878  * to send an OBSTACLE_DETECTION message.
879  *
880  * Functions:
881  * gazebo_init_range_sensors() -> Finds and initializes all ray sensors in gazebo
882  * gazebo_read_range_sensors() -> Reads and evaluates the ray sensors values, and sending it to other pprz modules
883  */
884 
885 struct gazebo_ray_t {
886  gazebo::sensors::RaySensorPtr sensor;
887  gazebo::common::Time last_measurement_time;
888 };
889 
890 static struct gazebo_ray_t rays[LASER_RANGE_ARRAY_NUM_SENSORS] = {{NULL, 0}};
891 static float range_orientation[] = LASER_RANGE_ARRAY_ORIENTATIONS;
892 static uint8_t ray_sensor_agl_index = 255;
893 
894 #define VL53L0_MAX_VAL 8.191f
895 
896 static void gazebo_init_range_sensors(void)
897 {
898  gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance();
899  gazebo::sensors::Sensor_V sensors = mgr->GetSensors(); // list of all sensors
900 
901  uint16_t sensor_count = model->GetSensorCount();
902 
903  gazebo::sensors::RaySensorPtr ray_sensor;
904  uint8_t ray_sensor_count_selected = 0;
905 
906  // Loop though all sensors and only select ray sensors, which are saved within a struct
907  for (uint16_t i = 0; i < sensor_count; i++) {
908  if (ray_sensor_count_selected > LASER_RANGE_ARRAY_NUM_SENSORS) {
909  break;
910  }
911 
912  if (sensors.at(i)->Type() == "ray") {
913  ray_sensor = static_pointer_cast<gazebo::sensors::RaySensor>(sensors.at(i));
914 
915  if (!ray_sensor) {
916  cout << "ERROR: Could not get pointer to raysensor " << i << "!" << endl;
917  continue;
918  }
919 
920  // Read out the pose from per ray sensors in gazebo relative to body
921  struct DoubleEulers pose_sensor = to_pprz_eulers(ray_sensor->Pose().Rot());
922 
923  struct DoubleQuat q_ray, q_def;
924  double_quat_of_eulers(&q_ray, &pose_sensor);
925 
926  /* Check the orientations of the ray sensors found in gazebo, if they are similar (within 5 deg) to the orientations
927  * given in the airframe file in LASER_RANGE_ARRAY_RANGE_ORIENTATION
928  */
929  for (int j = 0; j < LASER_RANGE_ARRAY_NUM_SENSORS; j++) {
930  struct DoubleEulers def = {0, range_orientation[j * 2], range_orientation[j * 2 + 1]};
931  double_quat_of_eulers(&q_def, &def);
932  // get angle between required angle and ray angle
933  double angle = acos(QUAT_DOT_PRODUCT(q_ray, q_def));
934 
935  if (fabs(angle) < RadOfDeg(5)) {
936  ray_sensor_count_selected++;
937  rays[j].sensor = ray_sensor;
938  rays[j].sensor->SetActive(true);
939 
940 #if LASER_RANGE_ARRAY_SEND_AGL
941  // find the sensor pointing down
942  if (fabs(range_orientation[j * 2] + M_PI_2) < RadOfDeg(5)) {
943  ray_sensor_agl_index = j;
944  }
945 #endif
946  break;
947  }
948  }
949  }
950  }
951 
952  if (ray_sensor_count_selected != LASER_RANGE_ARRAY_NUM_SENSORS) {
953  cout << "ERROR: you have defined " << LASER_RANGE_ARRAY_NUM_SENSORS << " sensors in your airframe file, but only "
954  << ray_sensor_count_selected << " sensors have been found in the gazebo simulator, "
955  "with the same orientation as in the airframe file " << endl;
956  exit(0);
957  }
958  cout << "Initialized laser range array" << endl;
959 }
960 
961 static void gazebo_read_range_sensors(void)
962 {
963  static float range;
964 
965  // Loop through all ray sensors found in gazebo
966  for (int i = 0; i < LASER_RANGE_ARRAY_NUM_SENSORS; i++) {
967  if ((rays[i].sensor->LastMeasurementTime() - rays[i].last_measurement_time).Float() < 0.005
968  || rays[i].sensor->LastMeasurementTime() == 0) { continue; }
969 
970  if (rays[i].sensor->Range(0) < 1e-5 || rays[i].sensor->Range(0) > VL53L0_MAX_VAL) {
971  range = VL53L0_MAX_VAL;
972  } else {
973  range = rays[i].sensor->Range(0);
974  }
975  AbiSendMsgOBSTACLE_DETECTION(OBS_DETECTION_RANGE_ARRAY_NPS_ID, range, range_orientation[i * 2],
976  range_orientation[i * 2 + 1]);
977 
978  if (i == ray_sensor_agl_index) {
979  uint32_t now_ts = get_sys_time_usec();
980  float agl = rays[i].sensor->Range(0);
981  // Down range sensor as agl
982  if (agl > 1e-5 && !isinf(agl)) {
983  AbiSendMsgAGL(AGL_RAY_SENSOR_GAZEBO_ID, now_ts, agl);
984  }
985  }
986  rays[i].last_measurement_time = rays[i].sensor->LastMeasurementTime();
987  }
988 }
989 #endif // NPS_SIMULATE_LASER_RANGE_ARRAY
990 
991 #pragma GCC diagnostic pop // Ignore -Wdeprecated-declarations
to_pprz_ecef
struct EcefCoor_d to_pprz_ecef(ignition::math::Vector3d ecef_i)
Definition: nps_fdm_gazebo.cpp:149
NPS_GAZEBO_WORLD
#define NPS_GAZEBO_WORLD
Definition: nps_fdm_gazebo.cpp:72
DoubleQuat
Roation quaternion.
Definition: pprz_algebra_double.h:55
NPS_COMMANDS_NB
#define NPS_COMMANDS_NB
Number of commands sent to the FDM of NPS.
Definition: nps_autopilot.h:42
NedCoor_d::y
double y
in meters
Definition: pprz_geodetic_double.h:69
uint16_t
unsigned short uint16_t
Definition: types.h:16
fdm
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
Definition: nps_fdm_gazebo.cpp:134
NpsFdm::ltp_h
struct DoubleVect3 ltp_h
Definition: nps_fdm.h:105
crop_t::h
uint16_t h
height of the cropped area
Definition: image.h:96
update_first_order_high_pass
static float update_first_order_high_pass(struct FirstOrderHighPass *filter, float value)
Update first order high pass filter state with a new value.
Definition: high_pass_filter.h:70
image_create
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
Create a new image.
Definition: image.c:43
MT9F002_TARGET_FPS
#define MT9F002_TARGET_FPS
Definition: mt9f002.h:105
mt9f002_t::sensor_height
uint16_t sensor_height
Definition: mt9f002.h:169
video_config_t::output_size
struct img_size_t output_size
Output image size.
Definition: video_device.h:56
NpsFdm::left_aileron
float left_aileron
Definition: nps_fdm.h:121
sonar
std::shared_ptr< gazebo::sensors::SonarSensor > sonar
Definition: nps_fdm_gazebo.cpp:131
img_size_t::w
uint16_t w
The width.
Definition: image.h:87
NpsFdm::airspeed
double airspeed
equivalent airspeed in m/s
Definition: nps_fdm.h:109
FirstOrderHighPass
First order high pass filter structure.
Definition: high_pass_filter.h:37
NpsFdm::elevator
float elevator
Definition: nps_fdm.h:119
NpsFdm::num_engines
uint32_t num_engines
Definition: nps_fdm.h:126
NpsFdm::body_ecef_accel
struct DoubleVect3 body_ecef_accel
acceleration in body frame, wrt ECEF frame
Definition: nps_fdm.h:71
DoubleVect3::z
double z
Definition: pprz_algebra_double.h:49
MT9F002_OUTPUT_HEIGHT
#define MT9F002_OUTPUT_HEIGHT
Definition: mt9f002.h:85
LlaCoor_d
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_double.h:58
NpsFdm
Definition: nps_fdm.h:44
double_quat_of_eulers
void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e)
Definition: pprz_algebra_double.c:49
NpsFdm::right_aileron
float right_aileron
Definition: nps_fdm.h:122
h
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Definition: UKF_Wind_Estimator.c:821
abi.h
LlaCoor_d::alt
double alt
in meters above WGS84 reference ellipsoid
Definition: pprz_geodetic_double.h:61
QUAT_DOT_PRODUCT
#define QUAT_DOT_PRODUCT(_qa, _qb)
Definition: pprz_algebra.h:650
MT9V117_TARGET_FPS
#define MT9V117_TARGET_FPS
Definition: mt9v117.h:35
nps_fdm_set_temperature
void nps_fdm_set_temperature(double temp, double h)
Set temperature in degrees Celcius at given height h above MSL.
Definition: nps_fdm_gazebo.cpp:315
NpsFdm::ltpprz_ecef_vel
struct NedCoor_d ltpprz_ecef_vel
velocity in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:79
gazebo_actuators
struct gazebo_actuators_t gazebo_actuators
Definition: nps_fdm_gazebo.cpp:119
gazebo_actuators_t::thrusts
double thrusts[NPS_COMMANDS_NB]
Definition: nps_fdm_gazebo.cpp:112
NedCoor_d::x
double x
in meters
Definition: pprz_geodetic_double.h:68
img_size_t::h
uint16_t h
The height.
Definition: image.h:88
NpsFdm::wind
struct DoubleVect3 wind
velocity in m/s in NED
Definition: nps_fdm.h:107
DoubleEulers::phi
double phi
in radians
Definition: pprz_algebra_double.h:77
DoubleRates::q
double q
in rad/s^2
Definition: pprz_algebra_double.h:87
DoubleEulers::theta
double theta
in radians
Definition: pprz_algebra_double.h:78
VL53L0_MAX_VAL
#define VL53L0_MAX_VAL
Definition: laser_range_array.c:60
DoubleRates::r
double r
in rad/s^2
Definition: pprz_algebra_double.h:88
crop_t::w
uint16_t w
Width of the cropped area.
Definition: image.h:95
init_first_order_low_pass
static void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, float sample_time, float value)
Init first order low pass filter.
Definition: low_pass_filter.h:57
to_pprz_lla
struct LlaCoor_d to_pprz_lla(ignition::math::Vector3d lla_i)
Definition: nps_fdm_gazebo.cpp:167
NpsFdm::body_accel
struct DoubleVect3 body_accel
acceleration in body frame as measured by an accelerometer (incl.
Definition: nps_fdm.h:87
update_first_order_low_pass
static float update_first_order_low_pass(struct FirstOrderLowPass *filter, float value)
Update first order low pass filter state with a new value.
Definition: low_pass_filter.h:71
MT9F002_CENTER_X
#define MT9F002_CENTER_X
Definition: mt9f002.h:116
motor_mixing_types.h
DoubleEulers::psi
double psi
in radians
Definition: pprz_algebra_double.h:79
uint32_t
unsigned long uint32_t
Definition: types.h:18
video_thread_nps.h
image_free
void image_free(struct image_t *img)
Free the image.
Definition: image.c:75
NpsFdm::lla_pos_pprz
struct LlaCoor_d lla_pos_pprz
Definition: nps_fdm.h:58
image_t::w
uint16_t w
Image width.
Definition: image.h:46
NpsFdm::sideslip
double sideslip
sideslip angle in rad
Definition: nps_fdm.h:116
cameras
static struct video_config_t * cameras[VIDEO_THREAD_MAX_CAMERAS]
Definition: video_thread.c:73
NpsFdm::ltpprz_ecef_accel
struct NedCoor_d ltpprz_ecef_accel
accel in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:81
nps_fdm.h
NpsFdm::ltp_to_body_quat
struct DoubleQuat ltp_to_body_quat
Definition: nps_fdm.h:91
nps_autopilot.h
nps_fdm_set_turbulence
void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity)
Definition: nps_fdm_gazebo.cpp:308
NpsFdm::ltp_ecef_vel
struct NedCoor_d ltp_ecef_vel
velocity in LTP frame, wrt ECEF frame
Definition: nps_fdm.h:74
NpsFdm::ltpprz_to_body_eulers
struct DoubleEulers ltpprz_to_body_eulers
Definition: nps_fdm.h:94
video_config_t::dev_name
char * dev_name
path to device
Definition: video_device.h:59
mt9f002
struct mt9f002_t mt9f002
Definition: mt9f002.c:123
nps_fdm_set_wind_ned
void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
Definition: nps_fdm_gazebo.cpp:301
MT9F002_DHANE_K
#define MT9F002_DHANE_K
Definition: mt9f002.h:122
NpsFdm::body_inertial_accel
struct DoubleVect3 body_inertial_accel
acceleration in body frame, wrt ECI inertial frame
Definition: nps_fdm.h:84
image_t::h
uint16_t h
Image height.
Definition: image.h:47
MT9F002_CENTER_Y
#define MT9F002_CENTER_Y
Definition: mt9f002.h:119
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
NpsFdm::agl
double agl
Definition: nps_fdm.h:61
gazebo_actuators_t::names
string names[NPS_COMMANDS_NB]
Definition: nps_fdm_gazebo.cpp:111
NpsFdm::body_ecef_rotvel
struct DoubleRates body_ecef_rotvel
Definition: nps_fdm.h:97
NpsFdm::ecef_ecef_vel
struct EcefCoor_d ecef_ecef_vel
velocity in ECEF frame, wrt ECEF frame
Definition: nps_fdm.h:64
mt9f002_t
Definition: mt9f002.h:133
NpsFdm::time
double time
Definition: nps_fdm.h:46
cv.h
DoubleRates
angular rates
Definition: pprz_algebra_double.h:85
pprz_autopilot::motors_on
bool motors_on
motor status
Definition: autopilot.h:68
OBS_DETECTION_RANGE_ARRAY_NPS_ID
#define OBS_DETECTION_RANGE_ARRAY_NPS_ID
Definition: abi_sender_ids.h:454
DoubleVect3::x
double x
Definition: pprz_algebra_double.h:47
NpsFdm::ltp_g
struct DoubleVect3 ltp_g
Definition: nps_fdm.h:104
NpsFdm::body_ecef_vel
struct DoubleVect3 body_ecef_vel
velocity in body frame, wrt ECEF frame
Definition: nps_fdm.h:69
dir
static const float dir[]
Definition: shift_tracking.c:91
autopilot
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:50
NpsFdm::ltp_ecef_accel
struct NedCoor_d ltp_ecef_accel
acceleration in LTP frame, wrt ECEF frame
Definition: nps_fdm.h:76
nps_fdm_run_step
void nps_fdm_run_step(bool launch, double *act_commands, int commands_nb)
Update the simulation state.
Definition: nps_fdm_gazebo.cpp:256
nps_fdm_init
void nps_fdm_init(double dt)
Initialize actuator dynamics, set unused fields in fdm.
Definition: nps_fdm_gazebo.cpp:227
mt9f002_t::offset_x
uint16_t offset_x
Offset from left in pixels.
Definition: mt9f002.h:165
yaw_coef
static const int32_t yaw_coef[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.c:91
NpsFdm::lla_pos_geoc
struct LlaCoor_d lla_pos_geoc
Definition: nps_fdm.h:60
mt9f002.h
to_pprz_body
struct DoubleVect3 to_pprz_body(ignition::math::Vector3d body_g)
Definition: nps_fdm_gazebo.cpp:176
sys_time.h
Architecture independent timing functions.
MT9V117_DHANE_K
#define MT9V117_DHANE_K
Definition: mt9v117.h:52
mt9f002_t::offset_y
uint16_t offset_y
Offset from top in pixels.
Definition: mt9f002.h:166
uint8_t
unsigned char uint8_t
Definition: types.h:14
NedCoor_d::z
double z
in meters
Definition: pprz_geodetic_double.h:70
NedCoor_d
vector in North East Down coordinates Units: meters
Definition: pprz_geodetic_double.h:67
gazebo_actuators_t::lowpass
struct FirstOrderLowPass lowpass[NPS_COMMANDS_NB]
Definition: nps_fdm_gazebo.cpp:114
image.h
LlaCoor_d::lat
double lat
in radians
Definition: pprz_geodetic_double.h:59
video_config_t::fps
int fps
Target FPS.
Definition: video_device.h:67
autopilot.h
NpsFdm::rudder
float rudder
Definition: nps_fdm.h:123
camera_intrinsics_t::focal_y
float focal_y
focal length in the y-direction in pixels
Definition: video_device.h:48
MT9F002_FOCAL_Y
#define MT9F002_FOCAL_Y
Definition: mt9f002.h:113
video_config_t::sensor_size
struct img_size_t sensor_size
Original sensor size.
Definition: video_device.h:57
init_gazebo
static void init_gazebo(void)
Set up a Gazebo server.
Definition: nps_fdm_gazebo.cpp:333
gazebo_actuators_t::max_ang_momentum
double max_ang_momentum[NPS_COMMANDS_NB]
Definition: nps_fdm_gazebo.cpp:116
NpsFdm::ecef_ecef_accel
struct EcefCoor_d ecef_ecef_accel
acceleration in ECEF frame, wrt ECEF frame
Definition: nps_fdm.h:66
to_pprz_ltp
struct DoubleVect3 to_pprz_ltp(ignition::math::Vector3d xyz)
Definition: nps_fdm_gazebo.cpp:212
NpsFdm::flap
float flap
Definition: nps_fdm.h:120
to_global_pprz_eulers
struct DoubleEulers to_global_pprz_eulers(ignition::math::Quaterniond quat)
Definition: nps_fdm_gazebo.cpp:203
gazebo_actuators_t::torques
double torques[NPS_COMMANDS_NB]
Definition: nps_fdm_gazebo.cpp:113
NpsFdm::lla_pos
struct LlaCoor_d lla_pos
Definition: nps_fdm.h:55
to_pprz_eulers
struct DoubleEulers to_pprz_eulers(ignition::math::Quaterniond quat)
Definition: nps_fdm_gazebo.cpp:194
NpsFdm::aoa
double aoa
angle of attack in rad
Definition: nps_fdm.h:115
pprz_algebra_double.h
Paparazzi double precision floating point algebra.
NpsFdm::hmsl
double hmsl
Definition: nps_fdm.h:56
init
bool init
Definition: nav_gls.c:57
to_pprz_rates
struct DoubleRates to_pprz_rates(ignition::math::Vector3d body_g)
Definition: nps_fdm_gazebo.cpp:185
CFG_MT9F002_PIXEL_ARRAY_WIDTH
#define CFG_MT9F002_PIXEL_ARRAY_WIDTH
Definition: mt9f002.h:39
video_config_t::camera_intrinsics
struct camera_intrinsics_t camera_intrinsics
Intrinsics of the camera; camera calibration parameters and distortion parameter(s)
Definition: video_device.h:68
NpsFdm::ltp_to_body_eulers
struct DoubleEulers ltp_to_body_eulers
Definition: nps_fdm.h:92
NpsFdm::init_dt
double init_dt
Definition: nps_fdm.h:47
IMAGE_YUV422
@ IMAGE_YUV422
UYVY format (uint16 per pixel)
Definition: image.h:36
NpsFdm::ltpprz_pos
struct NedCoor_d ltpprz_pos
Definition: nps_fdm.h:54
NpsFdm::ltpprz_to_body_quat
struct DoubleQuat ltpprz_to_body_quat
Definition: nps_fdm.h:93
EcefCoor_d
vector in EarthCenteredEarthFixed coordinates
Definition: pprz_geodetic_double.h:49
NpsFdm::ecef_pos
struct EcefCoor_d ecef_pos
Definition: nps_fdm.h:53
EcefCoor_d::y
double y
in meters
Definition: pprz_geodetic_double.h:51
NpsFdm::temperature
double temperature
current temperature in degrees Celcius
Definition: nps_fdm.h:113
MT9V117_CENTER_Y
#define MT9V117_CENTER_Y
Definition: mt9v117.h:49
ct
static gazebo::sensors::ContactSensorPtr ct
Definition: nps_fdm_gazebo.cpp:141
MT9F002_OUTPUT_WIDTH
#define MT9F002_OUTPUT_WIDTH
Definition: mt9f002.h:82
mt9v117.h
NPS_GAZEBO_AC_NAME
#define NPS_GAZEBO_AC_NAME
Definition: nps_fdm_gazebo.cpp:75
image_t::pprz_ts
uint32_t pprz_ts
The timestamp in us since system startup.
Definition: image.h:50
camera_intrinsics_t::center_y
float center_y
center image coordinate in the y-direction
Definition: video_device.h:50
EcefCoor_d::x
double x
in meters
Definition: pprz_geodetic_double.h:50
NpsFdm::pressure
double pressure
current (static) atmospheric pressure in Pascal
Definition: nps_fdm.h:110
gazebo_actuators_t::highpass
struct FirstOrderHighPass highpass[NPS_COMMANDS_NB]
Definition: nps_fdm_gazebo.cpp:115
NpsFdm::dynamic_pressure
double dynamic_pressure
dynamic pressure in Pascal
Definition: nps_fdm.h:112
gazebo_initialized
static bool gazebo_initialized
Definition: nps_fdm_gazebo.cpp:137
DoubleVect3
Definition: pprz_algebra_double.h:46
MT9V117_CENTER_X
#define MT9V117_CENTER_X
Definition: mt9v117.h:46
AGL_RAY_SENSOR_GAZEBO_ID
#define AGL_RAY_SENSOR_GAZEBO_ID
Definition: abi_sender_ids.h:165
EcefCoor_d::z
double z
in meters
Definition: pprz_geodetic_double.h:52
DoubleVect3::y
double y
Definition: pprz_algebra_double.h:48
image_t::ts
struct timeval ts
The timestamp of creation.
Definition: image.h:48
image_t::buf
void * buf
Image buffer (depending on the image_type)
Definition: image.h:54
MT9V117_FOCAL_X
#define MT9V117_FOCAL_X
Definition: mt9v117.h:40
to_pprz_ned
struct NedCoor_d to_pprz_ned(ignition::math::Vector3d global)
Definition: nps_fdm_gazebo.cpp:158
NpsFdm::lla_pos_geod
struct LlaCoor_d lla_pos_geod
Definition: nps_fdm.h:59
LlaCoor_d::lon
double lon
in radians
Definition: pprz_geodetic_double.h:60
CFG_MT9F002_PIXEL_ARRAY_HEIGHT
#define CFG_MT9F002_PIXEL_ARRAY_HEIGHT
Definition: mt9f002.h:38
mt9f002_t::sensor_width
uint16_t sensor_width
Definition: mt9f002.h:168
image_t::buf_idx
uint8_t buf_idx
Buffer index for V4L2 freeing.
Definition: image.h:52
cv_run_device
void cv_run_device(struct video_config_t *device, struct image_t *img)
Definition: cv.c:178
NpsFdm::curr_dt
double curr_dt
Definition: nps_fdm.h:48
video_config_t::crop
struct crop_t crop
Cropped area definition.
Definition: video_device.h:58
NpsFdm::nan_count
int nan_count
Definition: nps_fdm.h:50
camera_intrinsics_t::center_x
float center_x
center image coordinate in the x-direction
Definition: video_device.h:49
camera_intrinsics_t::focal_x
float focal_x
focal length in the x-direction in pixels
Definition: video_device.h:47
init_first_order_high_pass
static void init_first_order_high_pass(struct FirstOrderHighPass *filter, float tau, float sample_time, float value)
Init first order high pass filter.
Definition: high_pass_filter.h:56
DoubleRates::p
double p
in rad/s^2
Definition: pprz_algebra_double.h:86
FirstOrderLowPass
First order low pass filter structure.
Definition: low_pass_filter.h:39
gazebo_write
static void gazebo_write(double act_commands[], int commands_nb)
Write actuator commands to Gazebo.
Definition: nps_fdm_gazebo.cpp:637
gazebo_actuators_t
Definition: nps_fdm_gazebo.cpp:110
sensors
#define sensors(...)
Definition: cc2500_compat.h:68
NpsFdm::body_inertial_rotvel
struct DoubleRates body_inertial_rotvel
Definition: nps_fdm.h:101
image_t
Definition: image.h:44
high_pass_filter.h
Simple high pass filter with double precision.
NpsFdm::pressure_sl
double pressure_sl
pressure at sea level in Pascal
Definition: nps_fdm.h:114
model
static gazebo::physics::ModelPtr model
Definition: nps_fdm_gazebo.cpp:138
pprz_isa_pressure_of_height
static float pprz_isa_pressure_of_height(float height, float ref_p)
Get pressure in Pa from height (using simplified equation).
Definition: pprz_isa.h:129
pprz_isa.h
Paparazzi atmospheric pressure conversion utilities.
DoubleEulers
euler angles
Definition: pprz_algebra_double.h:76
MT9V117_FOCAL_Y
#define MT9V117_FOCAL_Y
Definition: mt9v117.h:43
VIDEO_THREAD_MAX_CAMERAS
#define VIDEO_THREAD_MAX_CAMERAS
Definition: video_thread.c:63
MT9F002_FOCAL_X
#define MT9F002_FOCAL_X
Definition: mt9f002.h:110
gazebo_read
static void gazebo_read(void)
Read Gazebo's simulation state and store the results in the fdm struct used by NPS.
Definition: nps_fdm_gazebo.cpp:505
nps_fdm_set_wind
void nps_fdm_set_wind(double speed, double dir)
Definition: nps_fdm_gazebo.cpp:295
Min
#define Min(x, y)
Definition: esc_dshot.c:85
low_pass_filter.h
Simple first order low pass filter with bilinear transform.