Paparazzi UAS  v6.2_unstable
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 "modules/core/abi.h"
53 
54 #include "math/pprz_isa.h"
58 
60 
63 #include "state.h"
64 }
65 
66 #if defined(NPS_DEBUG_VIDEO)
67 // Opencv tools
68 #include <opencv2/core/core.hpp>
69 #include <opencv2/highgui/highgui.hpp>
70 #include <opencv2/imgproc/imgproc.hpp>
71 #endif
72 
73 using namespace std;
74 
75 #ifndef NPS_GAZEBO_WORLD
76 #define NPS_GAZEBO_WORLD "empty.world"
77 #endif
78 #ifndef NPS_GAZEBO_AC_NAME
79 #define NPS_GAZEBO_AC_NAME "ardrone"
80 #endif
81 
82 // Add video handling functions if req'd.
83 #if NPS_SIMULATE_VIDEO
84 extern "C" {
88 #include "mcu_periph/sys_time.h"
89 #include "boards/bebop/mt9f002.h"
90 #include "boards/bebop/mt9v117.h"
91 struct mt9f002_t mt9f002 __attribute__((weak)); // Prevent undefined reference errors when Bebop code is not linked.
92 }
93 
94 static void init_gazebo_video(void);
95 static void gazebo_read_video(void);
96 static void read_image(
97  struct image_t *img,
98  gazebo::sensors::CameraSensorPtr cam);
99 struct gazebocam_t {
100  gazebo::sensors::CameraSensorPtr cam;
101  gazebo::common::Time last_measurement_time;
102 };
103 static struct gazebocam_t gazebo_cams[VIDEO_THREAD_MAX_CAMERAS] =
104 { { NULL, 0 } };
105 
106 // Reduce resolution of the simulated MT9F002 sensor (Bebop) to improve runtime
107 // performance at the cost of image resolution.
108 // Recommended values: 1 (realistic), 2, 4 (fast but slightly blurry)
109 #ifndef NPS_MT9F002_SENSOR_RES_DIVIDER
110 #define NPS_MT9F002_SENSOR_RES_DIVIDER 1
111 #endif
112 #endif // NPS_SIMULATE_VIDEO
113 
115  string names[NPS_COMMANDS_NB];
116  double thrusts[NPS_COMMANDS_NB];
117  double torques[NPS_COMMANDS_NB];
120  double max_ang_momentum[NPS_COMMANDS_NB];
121 };
122 
123 struct gazebo_actuators_t gazebo_actuators = { NPS_ACTUATOR_NAMES, NPS_ACTUATOR_THRUSTS, NPS_ACTUATOR_TORQUES, { }, { }, { }};
124 
125 #if NPS_SIMULATE_LASER_RANGE_ARRAY
126 extern "C" {
127 #include "modules/core/abi.h"
128 }
129 
130 static void gazebo_init_range_sensors(void);
131 static void gazebo_read_range_sensors(void);
132 
133 #endif
134 
135 std::shared_ptr<gazebo::sensors::SonarSensor> sonar = NULL;
136 
138 struct NpsFdm fdm;
139 
140 // Pointer to Gazebo data
141 static bool gazebo_initialized = false;
142 static gazebo::physics::ModelPtr model = NULL;
143 
144 // Get contact sensor
145 static gazebo::sensors::ContactSensorPtr ct;
146 
147 // Helper functions
148 static void init_gazebo(void);
149 static void gazebo_read(void);
150 static void gazebo_write(double act_commands[], int commands_nb);
151 
152 // Conversion routines
153 inline struct EcefCoor_d to_pprz_ecef(ignition::math::Vector3d ecef_i)
154 {
155  struct EcefCoor_d ecef_p;
156  ecef_p.x = ecef_i.X();
157  ecef_p.y = ecef_i.Y();
158  ecef_p.z = ecef_i.Z();
159  return ecef_p;
160 }
161 
162 inline struct NedCoor_d to_pprz_ned(ignition::math::Vector3d global)
163 {
164  struct NedCoor_d ned;
165  ned.x = global.Y();
166  ned.y = global.X();
167  ned.z = -global.Z();
168  return ned;
169 }
170 
171 inline struct LlaCoor_d to_pprz_lla(ignition::math::Vector3d lla_i)
172 {
173  struct LlaCoor_d lla_p;
174  lla_p.lat = lla_i.X();
175  lla_p.lon = lla_i.Y();
176  lla_p.alt = lla_i.Z();
177  return lla_p;
178 }
179 
180 inline struct DoubleVect3 to_pprz_body(ignition::math::Vector3d body_g)
181 {
182  struct DoubleVect3 body_p;
183  body_p.x = body_g.X();
184  body_p.y = -body_g.Y();
185  body_p.z = -body_g.Z();
186  return body_p;
187 }
188 
189 inline struct DoubleRates to_pprz_rates(ignition::math::Vector3d body_g)
190 {
191  struct DoubleRates body_p;
192  body_p.p = body_g.X();
193  body_p.q = -body_g.Y();
194  body_p.r = -body_g.Z();
195  return body_p;
196 }
197 
198 inline struct DoubleEulers to_pprz_eulers(ignition::math::Quaterniond quat)
199 {
200  struct DoubleEulers eulers;
201  eulers.psi = -quat.Yaw();
202  eulers.theta = -quat.Pitch();
203  eulers.phi = quat.Roll();
204  return eulers;
205 }
206 
207 inline struct DoubleEulers to_global_pprz_eulers(ignition::math::Quaterniond quat)
208 {
209  struct DoubleEulers eulers;
210  eulers.psi = -quat.Yaw() - M_PI / 2;
211  eulers.theta = -quat.Pitch();
212  eulers.phi = quat.Roll();
213  return eulers;
214 }
215 
216 inline struct DoubleVect3 to_pprz_ltp(ignition::math::Vector3d xyz)
217 {
218  struct DoubleVect3 ltp;
219  ltp.x = xyz.Y();
220  ltp.y = xyz.X();
221  ltp.z = -xyz.Z();
222  return ltp;
223 }
224 
225 // External functions, interface with Paparazzi's NPS as declared in nps_fdm.h
226 
231 void nps_fdm_init(double dt)
232 {
233  fdm.init_dt = dt; // JSBsim specific
234  fdm.curr_dt = dt; // JSBsim specific
235  fdm.nan_count = 0; // JSBsim specific
236 
237 #ifdef NPS_ACTUATOR_TIME_CONSTANTS
238  // Set up low-pass filter to simulate delayed actuator response
239  const float tau[NPS_COMMANDS_NB] = NPS_ACTUATOR_TIME_CONSTANTS;
240  for (int i = 0; i < NPS_COMMANDS_NB; i++) {
241  init_first_order_low_pass(&gazebo_actuators.lowpass[i], tau[i], dt, 0.f);
242  }
243 #ifdef NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM
244  // Set up high-pass filter to simulate spinup torque
245  const float Iwmax[NPS_COMMANDS_NB] = NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM;
246  for (int i = 0; i < NPS_COMMANDS_NB; i++) {
248  gazebo_actuators.max_ang_momentum[i] = Iwmax[i];
249  }
250 #endif
251 #endif
252 }
253 
261  bool launch __attribute__((unused)),
262  double *act_commands,
263  int commands_nb)
264 {
265  // Initialize Gazebo if req'd.
266  // Initialization is peformed here instead of in nps_fdm_init because:
267  // - Video devices need to added at this point. Video devices have not been
268  // added yet when nps_fdm_init is called.
269  // - nps_fdm_init runs on a different thread then nps_fdm_run_step, which
270  // causes problems with Gazebo.
271  if (!gazebo_initialized) {
272  init_gazebo();
273  gazebo_read();
274 #if NPS_SIMULATE_VIDEO
275  init_gazebo_video();
276 #endif
277 #if NPS_SIMULATE_LASER_RANGE_ARRAY
278  gazebo_init_range_sensors();
279 #endif
280  gazebo_initialized = true;
281  }
282 
283  // Update the simulation for a single timestep.
284  gazebo::runWorld(model->GetWorld(), 1);
285  gazebo::sensors::run_once();
286  gazebo_write(act_commands, commands_nb);
287  gazebo_read();
288 #if NPS_SIMULATE_VIDEO
289  gazebo_read_video();
290 #endif
291 #if NPS_SIMULATE_LASER_RANGE_ARRAY
292  gazebo_read_range_sensors();
293 #endif
294 
295 }
296 
297 // TODO Atmosphere functions have not been implemented yet.
298 // Starting at version 8, Gazebo has its own atmosphere and wind model.
300  double speed __attribute__((unused)),
301  double dir __attribute__((unused)))
302 {
303 }
304 
306  double wind_north __attribute__((unused)),
307  double wind_east __attribute__((unused)),
308  double wind_down __attribute__((unused)))
309 {
310 }
311 
313  double wind_speed __attribute__((unused)),
314  int turbulence_severity __attribute__((unused)))
315 {
316 }
317 
320  double temp __attribute__((unused)),
321  double h __attribute__((unused)))
322 {
323 }
324 
325 // Internal functions
337 static void init_gazebo(void)
338 {
339  string gazebo_home = "/conf/simulator/gazebo/";
340  string pprz_home(getenv("PAPARAZZI_HOME"));
341  string gazebodir = pprz_home + gazebo_home;
342  cout << "Gazebo directory: " << gazebodir << endl;
343 
344  if (getenv("ROS_MASTER_URI")) {
345  // Launch with ROS support
346  cout << "Add ROS plugins... ";
347  gazebo::addPlugin("libgazebo_ros_paths_plugin.so");
348  gazebo::addPlugin("libgazebo_ros_api_plugin.so");
349  cout << "ok" << endl;
350  }
351 
352  if (!gazebo::setupServer(0, NULL)) {
353  cout << "Failed to start Gazebo, exiting." << endl;
354  std::exit(-1);
355  }
356 
357  cout << "Add Paparazzi paths: " << gazebodir << endl;
358  gazebo::common::SystemPaths::Instance()->AddModelPaths(gazebodir + "models/");
359  sdf::addURIPath("model://", gazebodir + "models/");
360  sdf::addURIPath("world://", gazebodir + "world/");
361 
362  cout << "Add TU Delft paths: " << pprz_home + "/sw/ext/tudelft_gazebo_models/" << endl;
363  gazebo::common::SystemPaths::Instance()->AddModelPaths(pprz_home + "/sw/ext/tudelft_gazebo_models/models/");
364  sdf::addURIPath("model://", pprz_home + "/sw/ext/tudelft_gazebo_models/models/");
365  sdf::addURIPath("world://", pprz_home + "/sw/ext/tudelft_gazebo_models/world/");
366 
367  // get vehicles
368  string vehicle_uri = "model://" + string(NPS_GAZEBO_AC_NAME) + "/" + string(NPS_GAZEBO_AC_NAME) + ".sdf";
369  string vehicle_filename = sdf::findFile(vehicle_uri, false);
370  if (vehicle_filename.empty()) {
371  cout << "ERROR, could not find vehicle " + vehicle_uri << endl;
372  std::exit(-1);
373  }
374  cout << "Load vehicle: " << vehicle_filename << endl;
375  sdf::SDFPtr vehicle_sdf(new sdf::SDF());
376  sdf::init(vehicle_sdf);
377  if (!sdf::readFile(vehicle_filename, vehicle_sdf)) {
378  cout << "ERROR, could not read vehicle " + vehicle_filename << endl;
379  std::exit(-1);
380  }
381 
382  // add or set up sensors before the vehicle gets loaded
383 #if NPS_SIMULATE_VIDEO
384  // Cameras
385  sdf::ElementPtr link = vehicle_sdf->Root()->GetFirstElement()->GetElement("link");
386  while (link) {
387  if (link->Get<string>("name") == "front_camera" && link->GetElement("sensor")->Get<string>("name") == "mt9f002") {
388  if (NPS_MT9F002_SENSOR_RES_DIVIDER != 1) {
389  int w = link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("width")->Get<int>();
390  int h = link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("height")->Get<int>();
391  int env = link->GetElement("sensor")->GetElement("camera")->GetElement("lens")->GetElement("env_texture_size")->Get<int>();
392  link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("width")->Set(w / NPS_MT9F002_SENSOR_RES_DIVIDER);
393  link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("height")->Set(h / NPS_MT9F002_SENSOR_RES_DIVIDER);
394  link->GetElement("sensor")->GetElement("camera")->GetElement("lens")->GetElement("env_texture_size")->Set(env / NPS_MT9F002_SENSOR_RES_DIVIDER);
395  }
396  if (MT9F002_TARGET_FPS){
397  int fps = Min(MT9F002_TARGET_FPS, link->GetElement("sensor")->GetElement("update_rate")->Get<int>());
398  link->GetElement("sensor")->GetElement("update_rate")->Set(fps);
399  }
400  } else if (link->Get<string>("name") == "bottom_camera" && link->GetElement("sensor")->Get<string>("name") == "mt9v117") {
401  if (MT9V117_TARGET_FPS){
402  int fps = Min(MT9V117_TARGET_FPS, link->GetElement("sensor")->GetElement("update_rate")->Get<int>());
403  link->GetElement("sensor")->GetElement("update_rate")->Set(fps);
404  }
405  }
406  link = link->GetNextElement("link");
407  }
408 #endif
409 
410  // laser range array
411 #if NPS_SIMULATE_LASER_RANGE_ARRAY
412  vehicle_sdf->Root()->GetFirstElement()->AddElement("include")->GetElement("uri")->Set("model://range_sensors");
413  sdf::ElementPtr range_joint = vehicle_sdf->Root()->GetFirstElement()->AddElement("joint");
414  range_joint->GetAttribute("name")->Set("range_sensor_joint");
415  range_joint->GetAttribute("type")->Set("fixed");
416  range_joint->GetElement("parent")->Set("chassis");
417  range_joint->GetElement("child")->Set("range_sensors::base");
418 #endif
419 
420  // get world
421  string world_uri = "world://" + string(NPS_GAZEBO_WORLD);
422  string world_filename = sdf::findFile(world_uri, false);
423  if (world_filename.empty()) {
424  cout << "ERROR, could not find world " + world_uri << endl;
425  std::exit(-1);
426  }
427  cout << "Load world: " << world_filename << endl;
428  sdf::SDFPtr world_sdf(new sdf::SDF());
429  sdf::init(world_sdf);
430  if (!sdf::readFile(world_filename, world_sdf)) {
431  cout << "ERROR, could not read world " + world_filename << endl;
432  std::exit(-1);
433  }
434 
435  // add vehicles
436  world_sdf->Root()->GetFirstElement()->InsertElement(vehicle_sdf->Root()->GetFirstElement());
437 
438  world_sdf->Write(pprz_home + "/var/gazebo.world");
439 
440  gazebo::physics::WorldPtr world = gazebo::loadWorld(pprz_home + "/var/gazebo.world");
441  if (!world) {
442  cout << "Failed to open world, exiting." << endl;
443  std::exit(-1);
444  }
445 
446  cout << "Get pointer to aircraft: " << NPS_GAZEBO_AC_NAME << endl;
447  model = world->ModelByName(NPS_GAZEBO_AC_NAME);
448  if (!model) {
449  cout << "Failed to find '" << NPS_GAZEBO_AC_NAME << "', exiting."
450  << endl;
451  std::exit(-1);
452  }
453 
454  // Initialize sensors
455  gazebo::sensors::run_once(true);
456  gazebo::sensors::run_threads();
457  gazebo::runWorld(world, 1);
458  cout << "Sensors initialized..." << endl;
459 
460  // Find sensors
461  // Contact sensor
462  gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance();
463  ct = static_pointer_cast < gazebo::sensors::ContactSensor > (mgr->GetSensor("contactsensor"));
464  ct->SetActive(true);
465  // Sonar
466  sonar = static_pointer_cast<gazebo::sensors::SonarSensor>(mgr->GetSensor("sonarsensor"));
467  if(sonar) {
468  cout << "Found sonar" << endl;
469  }
470 
471  gazebo::physics::LinkPtr sonar_link = model->GetLink("sonar");
472  if (sonar_link) {
473  // Get a pointer to the sensor using its full name
474  if (sonar_link->GetSensorCount() != 1) {
475  cout << "ERROR: Link '" << sonar_link->GetName()
476  << "' should only contain 1 sensor!" << endl;
477  } else {
478  string name = sonar_link->GetSensorName(0);
479  sonar = static_pointer_cast< gazebo::sensors::SonarSensor > (mgr->GetSensor(name));
480  if (!sonar) {
481  cout << "ERROR: Could not get pointer to '" << name << "'!" << endl;
482  } else {
483  // Activate sensor
484  sonar->SetActive(true);
485  }
486  }
487  }
488 
489  // Overwrite motor directions as defined by motor_mixing
490 #ifdef MOTOR_MIXING_YAW_COEF
491  const double yaw_coef[] = MOTOR_MIXING_YAW_COEF;
492 
493  for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
494  gazebo_actuators.torques[i] = -fabs(gazebo_actuators.torques[i]) * yaw_coef[i] / fabs(yaw_coef[i]);
496  }
497 #endif
498  cout << "Gazebo initialized successfully!" << endl;
499 }
500 
509 static void gazebo_read(void)
510 {
511  static ignition::math::Vector3d vel_prev;
512  static double time_prev;
513 
514  gazebo::physics::WorldPtr world = model->GetWorld();
515  ignition::math::Pose3d pose = model->WorldPose(); // In LOCAL xyz frame
516  ignition::math::Vector3d vel = model->WorldLinearVel();
517  ignition::math::Vector3d ang_vel = model->WorldAngularVel();
518  gazebo::common::SphericalCoordinatesPtr sphere = world->SphericalCoords();
519  ignition::math::Quaterniond local_to_global_quat(0, 0, -sphere->HeadingOffset().Radian());
520 
521  /* Fill FDM struct */
522  fdm.time = world->SimTime().Double();
523 
524  // Find world acceleration by differentiating velocity
525  // model->GetWorldLinearAccel() does not seem to take the velocity_decay into account!
526  // Derivation of the velocity also follows the IMU implementation of Gazebo itself:
527  // https://bitbucket.org/osrf/gazebo/src/e26144434b932b4b6a760ddaa19cfcf9f1734748/gazebo/sensors/ImuSensor.cc?at=default&fileviewer=file-view-default#ImuSensor.cc-370
528  double dt = fdm.time - time_prev;
529  ignition::math::Vector3d accel = (vel - vel_prev) / dt;
530  vel_prev = vel;
531  time_prev = fdm.time;
532 
533  // init_dt: unused
534  // curr_dt: unused
535  // on_ground: unused
536  // nan_count: unused
537 
538  // Transform ltp definition to double for accuracy
539  struct LtpDef_d ltpdef_d;
540  ltpdef_d.ecef.x = state.ned_origin_f.ecef.x;
541  ltpdef_d.ecef.y = state.ned_origin_f.ecef.y;
542  ltpdef_d.ecef.z = state.ned_origin_f.ecef.z;
543  ltpdef_d.lla.lat = state.ned_origin_f.lla.lat;
544  ltpdef_d.lla.lon = state.ned_origin_f.lla.lon;
545  ltpdef_d.lla.alt = state.ned_origin_f.lla.alt;
546  for (int i = 0; i < 3 * 3; i++) {
547  ltpdef_d.ltp_of_ecef.m[i] = state.ned_origin_f.ltp_of_ecef.m[i];
548  }
549  ltpdef_d.hmsl = state.ned_origin_f.hmsl;
550 
551  /* position */
552  fdm.ltpprz_pos = to_pprz_ned(sphere->PositionTransform(pose.Pos(), gazebo::common::SphericalCoordinates::LOCAL,
553  gazebo::common::SphericalCoordinates::GLOBAL)); // Allows Gazebo worlds rotated wrt north.
554  fdm.hmsl = -fdm.ltpprz_pos.z;
557 
558  /* debug positions */
559  fdm.lla_pos_pprz = fdm.lla_pos; // Don't really care...
563 
564  if(sonar) {
565  double agl = sonar->Range();
566  if (agl > sonar->RangeMax()) agl = -1.0;
567  fdm.agl = agl;
568  } else {
569  fdm.agl = pose.Pos().Z(); // TODO Measure with sensor
570  }
571 
572  /* velocity */
573  fdm.ltp_ecef_vel = to_pprz_ned(sphere->VelocityTransform(vel, gazebo::common::SphericalCoordinates::LOCAL,
574  gazebo::common::SphericalCoordinates::GLOBAL));
576  fdm.body_ecef_vel = to_pprz_body(pose.Rot().RotateVectorReverse(vel)); // Note: unused
578 
579  /* acceleration */
580  fdm.ltp_ecef_accel = to_pprz_ned(sphere->VelocityTransform(accel, gazebo::common::SphericalCoordinates::LOCAL,
581  gazebo::common::SphericalCoordinates::GLOBAL)); // Note: unused
583  fdm.body_ecef_accel = to_pprz_body(pose.Rot().RotateVectorReverse(accel));
584  fdm.body_inertial_accel = fdm.body_ecef_accel; // Approximate, unused.
585  fdm.body_accel = to_pprz_body(pose.Rot().RotateVectorReverse(accel - world->Gravity()));
587 
588  /* attitude */
589  // ecef_to_body_quat: unused
590  fdm.ltp_to_body_eulers = to_global_pprz_eulers(local_to_global_quat * pose.Rot());
594 
595  /* angular velocity */
596  fdm.body_ecef_rotvel = to_pprz_rates(pose.Rot().RotateVectorReverse(ang_vel));
597  fdm.body_inertial_rotvel = fdm.body_ecef_rotvel; // Approximate
598 
599  /* angular acceleration */
600  // body_ecef_rotaccel: unused
601  // body_inertial_rotaccel: unused
602  /* misc */
603  fdm.ltp_g = to_pprz_ltp(sphere->VelocityTransform(-1 * world->Gravity(), gazebo::common::SphericalCoordinates::LOCAL,
604  gazebo::common::SphericalCoordinates::GLOBAL)); // unused
605  fdm.ltp_h = to_pprz_ltp(sphere->VelocityTransform(world->MagneticField(), gazebo::common::SphericalCoordinates::LOCAL,
606  gazebo::common::SphericalCoordinates::GLOBAL));
607 
608  /* atmosphere */
609 #if GAZEBO_MAJOR_VERSION >= 8 && 0 // TODO implement
610 
611 #else
612  // Gazebo versions < 8 do not have atmosphere or wind support
613  // Use placeholder values. Valid for low altitude, low speed flights.
614  fdm.wind = (struct DoubleVect3) {0, 0, 0};
615  fdm.pressure_sl = 101325; // Pa
616 
617  fdm.airspeed = 0;
620  fdm.temperature = 20.0; // C
621  fdm.aoa = 0; // rad
622  fdm.sideslip = 0; // rad
623 #endif
624  /* flight controls: unused */
625  fdm.elevator = 0;
626  fdm.flap = 0;
627  fdm.left_aileron = 0;
628  fdm.right_aileron = 0;
629  fdm.rudder = 0;
630  /* engine: unused */
631  fdm.num_engines = 0;
632 }
633 
651 static void gazebo_write(double act_commands[], int commands_nb)
652 {
653  for (int i = 0; i < commands_nb; ++i) {
654  // Thrust setpoint
655  double sp = autopilot.motors_on ? act_commands[i] : 0.0; // Normalized thrust setpoint
656 
657  // Actuator dynamics, forces and torques
658 #ifdef NPS_ACTUATOR_TIME_CONSTANTS
659  // Delayed response from actuator
660  double u = update_first_order_low_pass(&gazebo_actuators.lowpass[i], sp);// Normalized actual thrust
661 #else
662  double u = sp;
663 #endif
664  double thrust = gazebo_actuators.thrusts[i] * u;
665  double torque = gazebo_actuators.torques[i] * u;
666 
667 #ifdef NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM
668  // Spinup torque
670  double spinup_torque = gazebo_actuators.max_ang_momentum[i] / (2.0 * sqrt(u > 0.05 ? u : 0.05)) * udot;
671  torque += spinup_torque;
672 #endif
673 
674  // Apply force and torque to gazebo model
675  gazebo::physics::LinkPtr link = model->GetLink(gazebo_actuators.names[i]);
676  link->AddRelativeForce(ignition::math::Vector3d(0, 0, thrust));
677  link->AddRelativeTorque(ignition::math::Vector3d(0, 0, torque));
678  }
679 }
680 
681 #if NPS_SIMULATE_VIDEO
682 
696 static void init_gazebo_video(void)
697 {
698  gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance();
699 
700  cout << "Initializing cameras..." << endl;
701  // Loop over cameras registered in video_thread_nps
702  for (int i = 0; i < VIDEO_THREAD_MAX_CAMERAS && cameras[i] != NULL; ++i) {
703  // Find link in gazebo model
704  cout << "Setting up '" << cameras[i]->dev_name << "'... ";
705  gazebo::physics::LinkPtr link = model->GetLink(cameras[i]->dev_name);
706  if (!link) {
707  cout << "ERROR: Link '" << cameras[i]->dev_name << "' not found!"
708  << endl;
709  ;
710  continue;
711  }
712  // Get a pointer to the sensor using its full name
713  if (link->GetSensorCount() != 1) {
714  cout << "ERROR: Link '" << link->GetName()
715  << "' should only contain 1 sensor!" << endl;
716  continue;
717  }
718  string name = link->GetSensorName(0);
719  gazebo::sensors::CameraSensorPtr cam = static_pointer_cast
720  < gazebo::sensors::CameraSensor > (mgr->GetSensor(name));
721  if (!cam) {
722  cout << "ERROR: Could not get pointer to '" << name << "'!" << endl;
723  continue;
724  }
725  // Activate sensor
726  cam->SetActive(true);
727 
728  // Add to list of cameras
729  gazebo_cams[i].cam = cam;
730  gazebo_cams[i].last_measurement_time = cam->LastMeasurementTime();
731 
732  // set default camera settings
733  // Copy video_config settings from Gazebo's camera
734  cameras[i]->output_size.w = cam->ImageWidth();
735  cameras[i]->output_size.h = cam->ImageHeight();
736  cameras[i]->sensor_size.w = cam->ImageWidth();
737  cameras[i]->sensor_size.h = cam->ImageHeight();
738  cameras[i]->crop.w = cam->ImageWidth();
739  cameras[i]->crop.h = cam->ImageHeight();
740  cameras[i]->fps = 0;
745 
746  if (cam->Name() == "mt9f002") {
747  // See boards/bebop/mt9f002.c
755  cameras[i]->camera_intrinsics = {
757  .focal_y = MT9F002_FOCAL_Y,
758  .center_x = MT9F002_CENTER_X,
759  .center_y = MT9F002_CENTER_Y,
760  .Dhane_k = MT9F002_DHANE_K
761  };
762  } else if (cam->Name() == "mt9v117") {
763  // See boards/bebop/mt9v117.h
765  cameras[i]->camera_intrinsics = {
767  .focal_y = MT9V117_FOCAL_Y,
768  .center_x = MT9V117_CENTER_X,
769  .center_y = MT9V117_CENTER_Y,
770  .Dhane_k = MT9V117_DHANE_K
771  };
772  }
773  cout << "ok" << endl;
774  }
775 }
776 
785 static void gazebo_read_video(void)
786 {
787  for (int i = 0; i < VIDEO_THREAD_MAX_CAMERAS; ++i) {
788  gazebo::sensors::CameraSensorPtr &cam = gazebo_cams[i].cam;
789  // Skip unregistered or unfound cameras
790  if (cam == NULL) { continue; }
791  // Skip if not updated
792  // Also skip when LastMeasurementTime() is zero (workaround)
793  if ((cam->LastMeasurementTime() - gazebo_cams[i].last_measurement_time).Float() < 0.005
794  || cam->LastMeasurementTime() == 0) { continue; }
795  // Grab image, convert and send to video thread
796  struct image_t img;
797  read_image(&img, cam);
798 
799 #if NPS_DEBUG_VIDEO
800  cv::Mat RGB_cam(cam->ImageHeight(), cam->ImageWidth(), CV_8UC3, (uint8_t *)cam->ImageData());
801  cv::cvtColor(RGB_cam, RGB_cam, cv::COLOR_RGB2BGR);
802  cv::namedWindow(cameras[i]->dev_name, cv::WINDOW_AUTOSIZE); // Create a window for display.
803  cv::imshow(cameras[i]->dev_name, RGB_cam);
804  cv::waitKey(1);
805 #endif
806 
807  cv_run_device(cameras[i], &img);
808  // Free image buffer after use.
809  image_free(&img);
810  // Keep track of last update time.
811  gazebo_cams[i].last_measurement_time = cam->LastMeasurementTime();
812  }
813 }
814 
825 static void read_image(struct image_t *img, gazebo::sensors::CameraSensorPtr cam)
826 {
827  bool is_mt9f002 = false;
828  if (cam->Name() == "mt9f002") {
830  is_mt9f002 = true;
831  } else {
832  image_create(img, cam->ImageWidth(), cam->ImageHeight(), IMAGE_YUV422);
833  }
834 
835  // Convert Gazebo's *RGB888* image to Paparazzi's YUV422
836  const uint8_t *data_rgb = cam->ImageData();
837  uint8_t *data_yuv = (uint8_t *)(img->buf);
838  for (int x_yuv = 0; x_yuv < img->w; ++x_yuv) {
839  for (int y_yuv = 0; y_yuv < img->h; ++y_yuv) {
840  int x_rgb = x_yuv;
841  int y_rgb = y_yuv;
842  if (is_mt9f002) {
843  // Change sampling points for zoomed and/or cropped image.
844  // Use nearest-neighbour sampling for now.
845  x_rgb = (mt9f002.offset_x + ((float)x_yuv / img->w) * mt9f002.sensor_width)
846  / CFG_MT9F002_PIXEL_ARRAY_WIDTH * cam->ImageWidth();
847  y_rgb = (mt9f002.offset_y + ((float)y_yuv / img->h) * mt9f002.sensor_height)
848  / CFG_MT9F002_PIXEL_ARRAY_HEIGHT * cam->ImageHeight();
849  }
850  int idx_rgb = 3 * (cam->ImageWidth() * y_rgb + x_rgb);
851  int idx_yuv = 2 * (img->w * y_yuv + x_yuv);
852  int idx_px = img->w * y_yuv + x_yuv;
853  if (idx_px % 2 == 0) { // Pick U or V
854  data_yuv[idx_yuv] = - 0.148 * data_rgb[idx_rgb]
855  - 0.291 * data_rgb[idx_rgb + 1]
856  + 0.439 * data_rgb[idx_rgb + 2] + 128; // U
857  } else {
858  data_yuv[idx_yuv] = 0.439 * data_rgb[idx_rgb]
859  - 0.368 * data_rgb[idx_rgb + 1]
860  - 0.071 * data_rgb[idx_rgb + 2] + 128; // V
861  }
862  data_yuv[idx_yuv + 1] = 0.257 * data_rgb[idx_rgb]
863  + 0.504 * data_rgb[idx_rgb + 1]
864  + 0.098 * data_rgb[idx_rgb + 2] + 16; // Y
865  }
866  }
867  // Fill miscellaneous fields
868  gazebo::common::Time ts = cam->LastMeasurementTime();
869  img->ts.tv_sec = ts.sec;
870  img->ts.tv_usec = ts.nsec / 1000.0;
871  img->pprz_ts = ts.Double() * 1e6;
872  img->buf_idx = 0; // unused
873 }
874 #endif
875 
876 #if NPS_SIMULATE_LASER_RANGE_ARRAY
877 /*
878  * Simulate range sensors:
879  *
880  * In the airframe file, set NPS_SIMULATE_LASER_RANGE_ARRAY if you want to make use of the integrated Ray sensors.
881  * These are defined in their own model which is added to the ardrone.sdf (called range_sensors). Here you can add
882  * single ray point sensors with a specified position and orientation. It is also possible add noise.
883  *
884  * Within the airframe file (see e.g ardrone2_rangesensors_gazebo.xml), the amount of sensors
885  * (LASER_RANGE_ARRAY_NUM_SENSORS) and their orientations
886  * (LASER_RANGE_ARRAY_ORIENTATIONS={phi_1,theta_1,psi_1...phi_n,theta_n,psi_n} n = number of sensors) need to be
887  * specified as well. This is to keep it generic since this need to be done on the real platform with an external
888  * ray sensor. The function will compare the orientations from the ray sensors of gazebo, with the ones specified
889  * in the airframe, and will fill up an array to send and abi message to be used by other modules.
890  *
891  * NPS_GAZEBO_RANGE_SEND_AGL defines if the sensor that is defined as down should be used to send an AGL message.
892  * to send an OBSTACLE_DETECTION message.
893  *
894  * Functions:
895  * gazebo_init_range_sensors() -> Finds and initializes all ray sensors in gazebo
896  * gazebo_read_range_sensors() -> Reads and evaluates the ray sensors values, and sending it to other pprz modules
897  */
898 
899 struct gazebo_ray_t {
900  gazebo::sensors::RaySensorPtr sensor;
901  gazebo::common::Time last_measurement_time;
902 };
903 
904 static struct gazebo_ray_t rays[LASER_RANGE_ARRAY_NUM_SENSORS] = {{NULL, 0}};
905 static float range_orientation[] = LASER_RANGE_ARRAY_ORIENTATIONS;
906 static uint8_t ray_sensor_agl_index = 255;
907 
908 #define VL53L0_MAX_VAL 8.191f
909 
910 static void gazebo_init_range_sensors(void)
911 {
912  gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance();
913  gazebo::sensors::Sensor_V sensors = mgr->GetSensors(); // list of all sensors
914 
915  uint16_t sensor_count = model->GetSensorCount();
916 
917  gazebo::sensors::RaySensorPtr ray_sensor;
918  uint8_t ray_sensor_count_selected = 0;
919 
920  // Loop though all sensors and only select ray sensors, which are saved within a struct
921  for (uint16_t i = 0; i < sensor_count; i++) {
922  if (ray_sensor_count_selected > LASER_RANGE_ARRAY_NUM_SENSORS) {
923  break;
924  }
925 
926  if (sensors.at(i)->Type() == "ray") {
927  ray_sensor = static_pointer_cast<gazebo::sensors::RaySensor>(sensors.at(i));
928 
929  if (!ray_sensor) {
930  cout << "ERROR: Could not get pointer to raysensor " << i << "!" << endl;
931  continue;
932  }
933 
934  // Read out the pose from per ray sensors in gazebo relative to body
935  struct DoubleEulers pose_sensor = to_pprz_eulers(ray_sensor->Pose().Rot());
936 
937  struct DoubleQuat q_ray, q_def;
938  double_quat_of_eulers(&q_ray, &pose_sensor);
939 
940  /* Check the orientations of the ray sensors found in gazebo, if they are similar (within 5 deg) to the orientations
941  * given in the airframe file in LASER_RANGE_ARRAY_RANGE_ORIENTATION
942  */
943  for (int j = 0; j < LASER_RANGE_ARRAY_NUM_SENSORS; j++) {
944  struct DoubleEulers def = {0, range_orientation[j * 2], range_orientation[j * 2 + 1]};
945  double_quat_of_eulers(&q_def, &def);
946  // get angle between required angle and ray angle
947  double angle = acos(QUAT_DOT_PRODUCT(q_ray, q_def));
948 
949  if (fabs(angle) < RadOfDeg(5)) {
950  ray_sensor_count_selected++;
951  rays[j].sensor = ray_sensor;
952  rays[j].sensor->SetActive(true);
953 
954 #if LASER_RANGE_ARRAY_SEND_AGL
955  // find the sensor pointing down
956  if (fabs(range_orientation[j * 2] + M_PI_2) < RadOfDeg(5)) {
957  ray_sensor_agl_index = j;
958  }
959 #endif
960  break;
961  }
962  }
963  }
964  }
965 
966  if (ray_sensor_count_selected != LASER_RANGE_ARRAY_NUM_SENSORS) {
967  cout << "ERROR: you have defined " << LASER_RANGE_ARRAY_NUM_SENSORS << " sensors in your airframe file, but only "
968  << ray_sensor_count_selected << " sensors have been found in the gazebo simulator, "
969  "with the same orientation as in the airframe file " << endl;
970  exit(0);
971  }
972  cout << "Initialized laser range array" << endl;
973 }
974 
975 static void gazebo_read_range_sensors(void)
976 {
977  static float range;
978 
979  // Loop through all ray sensors found in gazebo
980  for (int i = 0; i < LASER_RANGE_ARRAY_NUM_SENSORS; i++) {
981  if ((rays[i].sensor->LastMeasurementTime() - rays[i].last_measurement_time).Float() < 0.005
982  || rays[i].sensor->LastMeasurementTime() == 0) { continue; }
983 
984  if (rays[i].sensor->Range(0) < 1e-5 || rays[i].sensor->Range(0) > VL53L0_MAX_VAL) {
985  range = VL53L0_MAX_VAL;
986  } else {
987  range = rays[i].sensor->Range(0);
988  }
989  AbiSendMsgOBSTACLE_DETECTION(OBS_DETECTION_RANGE_ARRAY_NPS_ID, range, range_orientation[i * 2],
990  range_orientation[i * 2 + 1]);
991 
992  if (i == ray_sensor_agl_index) {
993  uint32_t now_ts = get_sys_time_usec();
994  float agl = rays[i].sensor->Range(0);
995  // Down range sensor as agl
996  if (agl > 1e-5 && !isinf(agl)) {
997  AbiSendMsgAGL(AGL_RAY_SENSOR_GAZEBO_ID, now_ts, agl);
998  }
999  }
1000  rays[i].last_measurement_time = rays[i].sensor->LastMeasurementTime();
1001  }
1002 }
1003 #endif // NPS_SIMULATE_LASER_RANGE_ARRAY
1004 
1005 #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:153
NPS_GAZEBO_WORLD
#define NPS_GAZEBO_WORLD
Definition: nps_fdm_gazebo.cpp:76
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
EcefCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:47
NedCoor_d::y
double y
in meters
Definition: pprz_geodetic_double.h:69
fdm
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
Definition: nps_fdm_gazebo.cpp:138
NpsFdm::ltp_h
struct DoubleVect3 ltp_h
Definition: nps_fdm.h:105
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
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
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
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:135
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
DoubleRMat::m
double m[3 *3]
Definition: pprz_algebra_double.h:70
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:319
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:123
gazebo_actuators_t::thrusts
double thrusts[NPS_COMMANDS_NB]
Definition: nps_fdm_gazebo.cpp:116
NedCoor_d::x
double x
in meters
Definition: pprz_geodetic_double.h:68
LtpDef_d::hmsl
double hmsl
height in meters above mean sea level
Definition: pprz_geodetic_double.h:101
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
LtpDef_d::ltp_of_ecef
struct DoubleRMat ltp_of_ecef
rotation from ECEF to local frame
Definition: pprz_geodetic_double.h:100
EcefCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:46
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
LlaCoor_f::lon
float lon
in radians
Definition: pprz_geodetic_float.h:56
to_pprz_lla
struct LlaCoor_d to_pprz_lla(ignition::math::Vector3d lla_i)
Definition: nps_fdm_gazebo.cpp:171
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
EcefCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:48
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:312
LtpDef_d::ecef
struct EcefCoor_d ecef
origin of local frame in ECEF
Definition: pprz_geodetic_double.h:98
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:305
LtpDef_f::lla
struct LlaCoor_f lla
origin of local frame in LLA
Definition: pprz_geodetic_float.h:95
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
ecef_of_ned_vect_d
void ecef_of_ned_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
Definition: pprz_geodetic_double.c:186
image_t::h
uint16_t h
Image height.
Definition: image.h:47
MT9F002_CENTER_Y
#define MT9F002_CENTER_Y
Definition: mt9f002.h:119
ecef_of_ned_point_d
void ecef_of_ned_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
Definition: pprz_geodetic_double.c:174
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:75
NpsFdm::agl
double agl
Definition: nps_fdm.h:61
gazebo_actuators_t::names
string names[NPS_COMMANDS_NB]
Definition: nps_fdm_gazebo.cpp:115
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
pprz_geodetic_float.h
Paparazzi floating point math for geodetic calculations.
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:470
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:48
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:260
LtpDef_f::hmsl
float hmsl
Height above mean sea level in meters.
Definition: pprz_geodetic_float.h:97
nps_fdm_init
void nps_fdm_init(double dt)
Initialize actuator dynamics, set unused fields in fdm.
Definition: nps_fdm_gazebo.cpp:231
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
LtpDef_f::ltp_of_ecef
struct FloatRMat ltp_of_ecef
rotation from ECEF to local frame
Definition: pprz_geodetic_float.h:96
mt9f002.h
to_pprz_body
struct DoubleVect3 to_pprz_body(ignition::math::Vector3d body_g)
Definition: nps_fdm_gazebo.cpp:180
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
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
State::ned_origin_f
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
gazebo_actuators_t::lowpass
struct FirstOrderLowPass lowpass[NPS_COMMANDS_NB]
Definition: nps_fdm_gazebo.cpp:118
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
pprz_geodetic_double.h
Paparazzi double-precision floating point math for geodetic calculations.
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
LtpDef_d
definition of the local (flat earth) coordinate system
Definition: pprz_geodetic_double.h:97
LlaCoor_f::alt
float alt
in meters (normally above WGS84 reference ellipsoid)
Definition: pprz_geodetic_float.h:57
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:337
gazebo_actuators_t::max_ang_momentum
double max_ang_momentum[NPS_COMMANDS_NB]
Definition: nps_fdm_gazebo.cpp:120
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:216
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:207
gazebo_actuators_t::torques
double torques[NPS_COMMANDS_NB]
Definition: nps_fdm_gazebo.cpp:117
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:198
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:189
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:145
lla_of_ecef_d
void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef)
Definition: pprz_geodetic_double.c:83
MT9F002_OUTPUT_WIDTH
#define MT9F002_OUTPUT_WIDTH
Definition: mt9f002.h:82
mt9v117.h
gc_of_gd_lat_d
double gc_of_gd_lat_d(double gd_lat, double hmsl)
Definition: pprz_geodetic_double.c:210
NPS_GAZEBO_AC_NAME
#define NPS_GAZEBO_AC_NAME
Definition: nps_fdm_gazebo.cpp:79
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
LlaCoor_f::lat
float lat
in radians
Definition: pprz_geodetic_float.h:55
gazebo_actuators_t::highpass
struct FirstOrderHighPass highpass[NPS_COMMANDS_NB]
Definition: nps_fdm_gazebo.cpp:119
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:141
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:173
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:162
state.h
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
FloatRMat::m
float m[3 *3]
Definition: pprz_algebra_float.h:78
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
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
video_config_t::crop
struct crop_t crop
Cropped area definition.
Definition: video_device.h:58
state
struct State state
Definition: state.c:36
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
LtpDef_d::lla
struct LlaCoor_d lla
origin of local frame in LLA
Definition: pprz_geodetic_double.h:99
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:651
gazebo_actuators_t
Definition: nps_fdm_gazebo.cpp:114
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:142
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
LtpDef_f::ecef
struct EcefCoor_f ecef
origin of local frame in ECEF
Definition: pprz_geodetic_float.h:94
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:509
nps_fdm_set_wind
void nps_fdm_set_wind(double speed, double dir)
Definition: nps_fdm_gazebo.cpp:299
Min
#define Min(x, y)
Definition: esc_dshot.c:85
low_pass_filter.h
Simple first order low pass filter with bilinear transform.