Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
28
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
45extern "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
73using 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 "bebop"
80#endif
81
82// Add video handling functions if req'd.
83#if NPS_SIMULATE_VIDEO
84extern "C" {
88#include "mcu_periph/sys_time.h"
91struct mt9f002_t mt9f002 __attribute__((weak)); // Prevent undefined reference errors when Bebop code is not linked.
92}
93
94static void init_gazebo_video(void);
95static void gazebo_read_video(void);
96static void read_image(
97 struct image_t *img,
98 gazebo::sensors::CameraSensorPtr cam);
99struct gazebocam_t {
100 gazebo::sensors::CameraSensorPtr cam;
101 gazebo::common::Time last_measurement_time;
102};
103static 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
122
123struct gazebo_actuators_t gazebo_actuators = { NPS_ACTUATOR_NAMES, NPS_ACTUATOR_THRUSTS, NPS_ACTUATOR_TORQUES, { }, { }, { }};
124
125#if NPS_SIMULATE_LASER_RANGE_ARRAY
126extern "C" {
127#include "modules/core/abi.h"
128}
129
130static void gazebo_init_range_sensors(void);
131static void gazebo_read_range_sensors(void);
132
133#endif
134
135std::shared_ptr<gazebo::sensors::SonarSensor> sonar = NULL;
136
138struct NpsFdm fdm;
139
140// Pointer to Gazebo data
141static bool gazebo_initialized = false;
142static gazebo::physics::ModelPtr model = NULL;
143
144// Get contact sensor
145static gazebo::sensors::ContactSensorPtr ct;
146
147// Helper functions
148static void init_gazebo(void);
149static void gazebo_read(void);
150static void gazebo_write(double act_commands[], int commands_nb);
151
152// Conversion routines
153inline 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
162inline 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
171inline 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
180inline 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
189inline 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
198inline 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
207inline 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
216inline 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
228{
229 // nothing to do
230}
231
236void nps_fdm_init(double dt)
237{
238 fdm.init_dt = dt; // JSBsim specific
239 fdm.curr_dt = dt; // JSBsim specific
240 fdm.nan_count = 0; // JSBsim specific
241
242#ifdef NPS_ACTUATOR_TIME_CONSTANTS
243 // Set up low-pass filter to simulate delayed actuator response
244 const float tau[NPS_COMMANDS_NB] = NPS_ACTUATOR_TIME_CONSTANTS;
245 for (int i = 0; i < NPS_COMMANDS_NB; i++) {
246 init_first_order_low_pass(&gazebo_actuators.lowpass[i], tau[i], dt, 0.f);
247 }
248#ifdef NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM
249 // Set up high-pass filter to simulate spinup torque
250 const float Iwmax[NPS_COMMANDS_NB] = NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM;
251 for (int i = 0; i < NPS_COMMANDS_NB; i++) {
252 init_first_order_high_pass(&gazebo_actuators.highpass[i], tau[i], dt, 0.f);
253 gazebo_actuators.max_ang_momentum[i] = Iwmax[i];
254 }
255#endif
256#endif
257}
258
266 bool launch __attribute__((unused)),
267 double *act_commands,
268 int commands_nb)
269{
270 // Initialize Gazebo if req'd.
271 // Initialization is peformed here instead of in nps_fdm_init because:
272 // - Video devices need to added at this point. Video devices have not been
273 // added yet when nps_fdm_init is called.
274 // - nps_fdm_init runs on a different thread then nps_fdm_run_step, which
275 // causes problems with Gazebo.
276 if (!gazebo_initialized) {
277 init_gazebo();
278 gazebo_read();
279#if NPS_SIMULATE_VIDEO
280 init_gazebo_video();
281#endif
282#if NPS_SIMULATE_LASER_RANGE_ARRAY
283 gazebo_init_range_sensors();
284#endif
285 gazebo_initialized = true;
286 }
287
288 // Update the simulation for a single timestep.
289 gazebo::runWorld(model->GetWorld(), 1);
290 gazebo::sensors::run_once();
291 gazebo_write(act_commands, commands_nb);
292 gazebo_read();
293#if NPS_SIMULATE_VIDEO
294 gazebo_read_video();
295#endif
296#if NPS_SIMULATE_LASER_RANGE_ARRAY
297 gazebo_read_range_sensors();
298#endif
299
300}
301
302// TODO Atmosphere functions have not been implemented yet.
303// Starting at version 8, Gazebo has its own atmosphere and wind model.
305 double speed __attribute__((unused)),
306 double dir __attribute__((unused)))
307{
308}
309
311 double wind_north __attribute__((unused)),
312 double wind_east __attribute__((unused)),
313 double wind_down __attribute__((unused)))
314{
315}
316
318 double wind_speed __attribute__((unused)),
319 int turbulence_severity __attribute__((unused)))
320{
321}
322
325 double temp __attribute__((unused)),
326 double h __attribute__((unused)))
327{
328}
329
330// Internal functions
342static void init_gazebo(void)
343{
344 string gazebo_home = "/conf/simulator/gazebo/";
345 string pprz_home(getenv("PAPARAZZI_HOME"));
346 string gazebodir = pprz_home + gazebo_home;
347 cout << "Gazebo directory: " << gazebodir << endl;
348
349 if (getenv("ROS_MASTER_URI")) {
350 // Launch with ROS support
351 cout << "Add ROS plugins... ";
352 gazebo::addPlugin("libgazebo_ros_paths_plugin.so");
353 gazebo::addPlugin("libgazebo_ros_api_plugin.so");
354 cout << "ok" << endl;
355 }
356
357 if (!gazebo::setupServer(0, NULL)) {
358 cout << "Failed to start Gazebo, exiting." << endl;
359 std::exit(-1);
360 }
361
362 cout << "Add Paparazzi paths: " << gazebodir << endl;
363 gazebo::common::SystemPaths::Instance()->AddModelPaths(gazebodir + "models/");
364 sdf::addURIPath("model://", gazebodir + "models/");
365 sdf::addURIPath("world://", gazebodir + "world/");
366
367 cout << "Add TU Delft paths: " << pprz_home + "/sw/ext/tudelft_gazebo_models/" << endl;
368 gazebo::common::SystemPaths::Instance()->AddModelPaths(pprz_home + "/sw/ext/tudelft_gazebo_models/models/");
369 sdf::addURIPath("model://", pprz_home + "/sw/ext/tudelft_gazebo_models/models/");
370 sdf::addURIPath("world://", pprz_home + "/sw/ext/tudelft_gazebo_models/world/");
371
372 // get vehicles
373 string vehicle_uri = "model://" + string(NPS_GAZEBO_AC_NAME) + "/" + string(NPS_GAZEBO_AC_NAME) + ".sdf";
374 string vehicle_filename = sdf::findFile(vehicle_uri, false);
375 if (vehicle_filename.empty()) {
376 cout << "ERROR, could not find vehicle " + vehicle_uri << endl;
377 std::exit(-1);
378 }
379 cout << "Load vehicle: " << vehicle_filename << endl;
380 sdf::SDFPtr vehicle_sdf(new sdf::SDF());
381 sdf::init(vehicle_sdf);
382 if (!sdf::readFile(vehicle_filename, vehicle_sdf)) {
383 cout << "ERROR, could not read vehicle " + vehicle_filename << endl;
384 std::exit(-1);
385 }
386
387 // add or set up sensors before the vehicle gets loaded
388#if NPS_SIMULATE_VIDEO
389 // Cameras
390 sdf::ElementPtr link = vehicle_sdf->Root()->GetFirstElement()->GetElement("link");
391 while (link) {
392 if (link->Get<string>("name") == "front_camera" && link->GetElement("sensor")->Get<string>("name") == "mt9f002") {
393 if (NPS_MT9F002_SENSOR_RES_DIVIDER != 1) {
394 int w = link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("width")->Get<int>();
395 int h = link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("height")->Get<int>();
396 int env = link->GetElement("sensor")->GetElement("camera")->GetElement("lens")->GetElement("env_texture_size")->Get<int>();
397 link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("width")->Set(w / NPS_MT9F002_SENSOR_RES_DIVIDER);
398 link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("height")->Set(h / NPS_MT9F002_SENSOR_RES_DIVIDER);
399 link->GetElement("sensor")->GetElement("camera")->GetElement("lens")->GetElement("env_texture_size")->Set(env / NPS_MT9F002_SENSOR_RES_DIVIDER);
400 }
402 int fps = Min(MT9F002_TARGET_FPS, link->GetElement("sensor")->GetElement("update_rate")->Get<int>());
403 link->GetElement("sensor")->GetElement("update_rate")->Set(fps);
404 }
405 } else if (link->Get<string>("name") == "bottom_camera" && link->GetElement("sensor")->Get<string>("name") == "mt9v117") {
407 int fps = Min(MT9V117_TARGET_FPS, link->GetElement("sensor")->GetElement("update_rate")->Get<int>());
408 link->GetElement("sensor")->GetElement("update_rate")->Set(fps);
409 }
410 }
411 link = link->GetNextElement("link");
412 }
413#endif
414
415 // laser range array
416#if NPS_SIMULATE_LASER_RANGE_ARRAY
417 vehicle_sdf->Root()->GetFirstElement()->AddElement("include")->GetElement("uri")->Set("model://range_sensors");
418 sdf::ElementPtr range_joint = vehicle_sdf->Root()->GetFirstElement()->AddElement("joint");
419 range_joint->GetAttribute("name")->Set("range_sensor_joint");
420 range_joint->GetAttribute("type")->Set("fixed");
421 range_joint->GetElement("parent")->Set("chassis");
422 range_joint->GetElement("child")->Set("range_sensors::base");
423#endif
424
425 // get world
426 string world_uri = "world://" + string(NPS_GAZEBO_WORLD);
427 string world_filename = sdf::findFile(world_uri, false);
428 if (world_filename.empty()) {
429 cout << "ERROR, could not find world " + world_uri << endl;
430 std::exit(-1);
431 }
432 cout << "Load world: " << world_filename << endl;
433 sdf::SDFPtr world_sdf(new sdf::SDF());
434 sdf::init(world_sdf);
435 if (!sdf::readFile(world_filename, world_sdf)) {
436 cout << "ERROR, could not read world " + world_filename << endl;
437 std::exit(-1);
438 }
439
440 // add vehicles
441 world_sdf->Root()->GetFirstElement()->InsertElement(vehicle_sdf->Root()->GetFirstElement());
442
443 world_sdf->Write(pprz_home + "/var/gazebo.world");
444
445 gazebo::physics::WorldPtr world = gazebo::loadWorld(pprz_home + "/var/gazebo.world");
446 if (!world) {
447 cout << "Failed to open world, exiting." << endl;
448 std::exit(-1);
449 }
450
451 cout << "Get pointer to aircraft: " << NPS_GAZEBO_AC_NAME << endl;
452 model = world->ModelByName(NPS_GAZEBO_AC_NAME);
453 if (!model) {
454 cout << "Failed to find '" << NPS_GAZEBO_AC_NAME << "', exiting."
455 << endl;
456 std::exit(-1);
457 }
458
459 // Initialize sensors
460 gazebo::sensors::run_once(true);
461 gazebo::sensors::run_threads();
462 gazebo::runWorld(world, 1);
463 cout << "Sensors initialized..." << endl;
464
465 // Find sensors
466 // Contact sensor
467 gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance();
468 ct = static_pointer_cast < gazebo::sensors::ContactSensor > (mgr->GetSensor("contactsensor"));
469 ct->SetActive(true);
470 // Sonar
471 sonar = static_pointer_cast<gazebo::sensors::SonarSensor>(mgr->GetSensor("sonarsensor"));
472 if(sonar) {
473 cout << "Found sonar" << endl;
474 }
475
476 gazebo::physics::LinkPtr sonar_link = model->GetLink("sonar");
477 if (sonar_link) {
478 // Get a pointer to the sensor using its full name
479 if (sonar_link->GetSensorCount() != 1) {
480 cout << "ERROR: Link '" << sonar_link->GetName()
481 << "' should only contain 1 sensor!" << endl;
482 } else {
483 string name = sonar_link->GetSensorName(0);
484 sonar = static_pointer_cast< gazebo::sensors::SonarSensor > (mgr->GetSensor(name));
485 if (!sonar) {
486 cout << "ERROR: Could not get pointer to '" << name << "'!" << endl;
487 } else {
488 // Activate sensor
489 sonar->SetActive(true);
490 }
491 }
492 }
493
494 // Overwrite motor directions as defined by motor_mixing
495#ifdef MOTOR_MIXING_YAW_COEF
496 const double yaw_coef[] = MOTOR_MIXING_YAW_COEF;
497
498 for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
499 gazebo_actuators.torques[i] = -fabs(gazebo_actuators.torques[i]) * yaw_coef[i] / fabs(yaw_coef[i]);
500 gazebo_actuators.max_ang_momentum[i] = -fabs(gazebo_actuators.max_ang_momentum[i]) * yaw_coef[i] / fabs(yaw_coef[i]);
501 }
502#endif
503 cout << "Gazebo initialized successfully!" << endl;
504}
505
514static void gazebo_read(void)
515{
516 static ignition::math::Vector3d vel_prev;
517 static double time_prev;
518
519 gazebo::physics::WorldPtr world = model->GetWorld();
520 ignition::math::Pose3d pose = model->WorldPose(); // In LOCAL xyz frame
521 ignition::math::Vector3d vel = model->WorldLinearVel();
522 ignition::math::Vector3d ang_vel = model->WorldAngularVel();
523 gazebo::common::SphericalCoordinatesPtr sphere = world->SphericalCoords();
524 ignition::math::Quaterniond local_to_global_quat(0, 0, -sphere->HeadingOffset().Radian());
525
526 /* Fill FDM struct */
527 fdm.time = world->SimTime().Double();
528
529 // Find world acceleration by differentiating velocity
530 // model->GetWorldLinearAccel() does not seem to take the velocity_decay into account!
531 // Derivation of the velocity also follows the IMU implementation of Gazebo itself:
532 // https://bitbucket.org/osrf/gazebo/src/e26144434b932b4b6a760ddaa19cfcf9f1734748/gazebo/sensors/ImuSensor.cc?at=default&fileviewer=file-view-default#ImuSensor.cc-370
533 double dt = fdm.time - time_prev;
534 ignition::math::Vector3d accel = (vel - vel_prev) / dt;
535 vel_prev = vel;
536 time_prev = fdm.time;
537
538 // init_dt: unused
539 // curr_dt: unused
540 // on_ground: unused
541 // nan_count: unused
542
543 // Transform ltp definition to double for accuracy
544 struct LtpDef_d ltpdef_d;
545 ltpdef_d.ecef.x = state.ned_origin_f.ecef.x;
546 ltpdef_d.ecef.y = state.ned_origin_f.ecef.y;
547 ltpdef_d.ecef.z = state.ned_origin_f.ecef.z;
548 ltpdef_d.lla.lat = state.ned_origin_f.lla.lat;
549 ltpdef_d.lla.lon = state.ned_origin_f.lla.lon;
550 ltpdef_d.lla.alt = state.ned_origin_f.lla.alt;
551 for (int i = 0; i < 3 * 3; i++) {
552 ltpdef_d.ltp_of_ecef.m[i] = state.ned_origin_f.ltp_of_ecef.m[i];
553 }
554 ltpdef_d.hmsl = state.ned_origin_f.hmsl;
555
556 /* position */
557 fdm.ltpprz_pos = to_pprz_ned(sphere->PositionTransform(pose.Pos(), gazebo::common::SphericalCoordinates::LOCAL,
558 gazebo::common::SphericalCoordinates::GLOBAL)); // Allows Gazebo worlds rotated wrt north.
559 fdm.hmsl = -fdm.ltpprz_pos.z;
560 ecef_of_ned_point_d(&fdm.ecef_pos, &ltpdef_d, &fdm.ltpprz_pos);
561 lla_of_ecef_d(&fdm.lla_pos, &fdm.ecef_pos);
562
563 /* debug positions */
564 fdm.lla_pos_pprz = fdm.lla_pos; // Don't really care...
565 fdm.lla_pos_geod = fdm.lla_pos;
566 fdm.lla_pos_geoc = fdm.lla_pos;
567 fdm.lla_pos_geoc.lat = gc_of_gd_lat_d(fdm.lla_pos.lat, fdm.hmsl);
568
569 if(sonar) {
570 double agl = sonar->Range();
571 if (agl > sonar->RangeMax()) agl = -1.0;
572 fdm.agl = agl;
573 } else {
574 fdm.agl = pose.Pos().Z(); // TODO Measure with sensor
575 }
576
577 /* velocity */
578 fdm.ltp_ecef_vel = to_pprz_ned(sphere->VelocityTransform(vel, gazebo::common::SphericalCoordinates::LOCAL,
579 gazebo::common::SphericalCoordinates::GLOBAL));
580 fdm.ltpprz_ecef_vel = fdm.ltp_ecef_vel; // ???
581 fdm.body_ecef_vel = to_pprz_body(pose.Rot().RotateVectorReverse(vel)); // Note: unused
582 ecef_of_ned_vect_d(&fdm.ecef_ecef_vel, &ltpdef_d, &fdm.ltp_ecef_vel);
583
584 /* acceleration */
585 fdm.ltp_ecef_accel = to_pprz_ned(sphere->VelocityTransform(accel, gazebo::common::SphericalCoordinates::LOCAL,
586 gazebo::common::SphericalCoordinates::GLOBAL)); // Note: unused
587 fdm.ltpprz_ecef_accel = fdm.ltp_ecef_accel; // ???
588 fdm.body_ecef_accel = to_pprz_body(pose.Rot().RotateVectorReverse(accel));
589 fdm.body_inertial_accel = fdm.body_ecef_accel; // Approximate, unused.
590 fdm.body_accel = to_pprz_body(pose.Rot().RotateVectorReverse(accel - world->Gravity()));
591 ecef_of_ned_vect_d(&fdm.ecef_ecef_accel, &ltpdef_d, &fdm.ltp_ecef_accel);
592
593 /* attitude */
594 // ecef_to_body_quat: unused
595 fdm.ltp_to_body_eulers = to_global_pprz_eulers(local_to_global_quat * pose.Rot());
596 double_quat_of_eulers(&(fdm.ltp_to_body_quat), &(fdm.ltp_to_body_eulers));
597 fdm.ltpprz_to_body_quat = fdm.ltp_to_body_quat; // unused
598 fdm.ltpprz_to_body_eulers = fdm.ltp_to_body_eulers; // unused
599
600 /* angular velocity */
601 fdm.body_ecef_rotvel = to_pprz_rates(pose.Rot().RotateVectorReverse(ang_vel));
602 fdm.body_inertial_rotvel = fdm.body_ecef_rotvel; // Approximate
603
604 /* angular acceleration */
605 // body_ecef_rotaccel: unused
606 // body_inertial_rotaccel: unused
607 /* misc */
608 fdm.ltp_g = to_pprz_ltp(sphere->VelocityTransform(-1 * world->Gravity(), gazebo::common::SphericalCoordinates::LOCAL,
609 gazebo::common::SphericalCoordinates::GLOBAL)); // unused
610 fdm.ltp_h = to_pprz_ltp(sphere->VelocityTransform(world->MagneticField(), gazebo::common::SphericalCoordinates::LOCAL,
611 gazebo::common::SphericalCoordinates::GLOBAL));
612
613 /* atmosphere */
614#if GAZEBO_MAJOR_VERSION >= 8 && 0 // TODO implement
615
616#else
617 // Gazebo versions < 8 do not have atmosphere or wind support
618 // Use placeholder values. Valid for low altitude, low speed flights.
619 fdm.wind = (struct DoubleVect3) {0, 0, 0};
620 fdm.pressure_sl = 101325; // Pa
621
622 fdm.airspeed = 0;
623 fdm.pressure = pprz_isa_pressure_of_height(fdm.hmsl, fdm.pressure_sl);
624 fdm.dynamic_pressure = fdm.pressure_sl; // Pa
625 fdm.temperature = 20.0; // C
626 fdm.aoa = 0; // rad
627 fdm.sideslip = 0; // rad
628#endif
629 /* flight controls: unused */
630 fdm.elevator = 0;
631 fdm.flap = 0;
632 fdm.left_aileron = 0;
633 fdm.right_aileron = 0;
634 fdm.rudder = 0;
635 /* engine: unused */
636 fdm.num_engines = 0;
637}
638
656static void gazebo_write(double act_commands[], int commands_nb)
657{
658 for (int i = 0; i < commands_nb; ++i) {
659 // Thrust setpoint
660 double sp = autopilot.motors_on ? act_commands[i] : 0.0; // Normalized thrust setpoint
661
662 // Actuator dynamics, forces and torques
663#ifdef NPS_ACTUATOR_TIME_CONSTANTS
664 // Delayed response from actuator
665 double u = update_first_order_low_pass(&gazebo_actuators.lowpass[i], sp);// Normalized actual thrust
666#else
667 double u = sp;
668#endif
669 double thrust = gazebo_actuators.thrusts[i] * u;
670 double torque = gazebo_actuators.torques[i] * u;
671
672#ifdef NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM
673 // Spinup torque
674 double udot = update_first_order_high_pass(&gazebo_actuators.highpass[i], sp);
675 double spinup_torque = gazebo_actuators.max_ang_momentum[i] / (2.0 * sqrt(u > 0.05 ? u : 0.05)) * udot;
676 torque += spinup_torque;
677#endif
678
679 // Apply force and torque to gazebo model
680 gazebo::physics::LinkPtr link = model->GetLink(gazebo_actuators.names[i]);
681 link->AddRelativeForce(ignition::math::Vector3d(0, 0, thrust));
682 link->AddRelativeTorque(ignition::math::Vector3d(0, 0, torque));
683 }
684}
685
686#if NPS_SIMULATE_VIDEO
701static void init_gazebo_video(void)
702{
703 gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance();
704
705 cout << "Initializing cameras..." << endl;
706 // Loop over cameras registered in video_thread_nps
707 for (int i = 0; i < VIDEO_THREAD_MAX_CAMERAS && cameras[i] != NULL; ++i) {
708 // Find link in gazebo model
709 cout << "Setting up '" << cameras[i]->dev_name << "'... ";
710 gazebo::physics::LinkPtr link = model->GetLink(cameras[i]->dev_name);
711 if (!link) {
712 cout << "ERROR: Link '" << cameras[i]->dev_name << "' not found!"
713 << endl;
714 ;
715 continue;
716 }
717 // Get a pointer to the sensor using its full name
718 if (link->GetSensorCount() != 1) {
719 cout << "ERROR: Link '" << link->GetName()
720 << "' should only contain 1 sensor!" << endl;
721 continue;
722 }
723 string name = link->GetSensorName(0);
724 gazebo::sensors::CameraSensorPtr cam = static_pointer_cast
725 < gazebo::sensors::CameraSensor > (mgr->GetSensor(name));
726 if (!cam) {
727 cout << "ERROR: Could not get pointer to '" << name << "'!" << endl;
728 continue;
729 }
730 // Activate sensor
731 cam->SetActive(true);
732
733 // Add to list of cameras
734 gazebo_cams[i].cam = cam;
735 gazebo_cams[i].last_measurement_time = cam->LastMeasurementTime();
736
737 // set default camera settings
738 // Copy video_config settings from Gazebo's camera
739 cameras[i]->output_size.w = cam->ImageWidth();
740 cameras[i]->output_size.h = cam->ImageHeight();
741 cameras[i]->sensor_size.w = cam->ImageWidth();
742 cameras[i]->sensor_size.h = cam->ImageHeight();
743 cameras[i]->crop.w = cam->ImageWidth();
744 cameras[i]->crop.h = cam->ImageHeight();
745 cameras[i]->fps = 0;
746 cameras[i]->camera_intrinsics.focal_x = cameras[i]->output_size.w / 2.0f;
747 cameras[i]->camera_intrinsics.center_x = cameras[i]->output_size.w / 2.0f;
748 cameras[i]->camera_intrinsics.focal_y = cameras[i]->output_size.h / 2.0f;
749 cameras[i]->camera_intrinsics.center_y = cameras[i]->output_size.h / 2.0f;
750
751 if (cam->Name() == "mt9f002") {
752 // See boards/bebop/mt9f002.c
753 cameras[i]->output_size.w = MT9F002_OUTPUT_WIDTH;
754 cameras[i]->output_size.h = MT9F002_OUTPUT_HEIGHT;
755 cameras[i]->sensor_size.w = MT9F002_OUTPUT_WIDTH;
756 cameras[i]->sensor_size.h = MT9F002_OUTPUT_HEIGHT;
757 cameras[i]->crop.w = MT9F002_OUTPUT_WIDTH;
758 cameras[i]->crop.h = MT9F002_OUTPUT_HEIGHT;
759 cameras[i]->fps = MT9F002_TARGET_FPS;
760 cameras[i]->camera_intrinsics = {
761 .focal_x = MT9F002_FOCAL_X,
762 .focal_y = MT9F002_FOCAL_Y,
763 .center_x = MT9F002_CENTER_X,
764 .center_y = MT9F002_CENTER_Y,
765 .Dhane_k = MT9F002_DHANE_K
766 };
767 } else if (cam->Name() == "mt9v117") {
768 // See boards/bebop/mt9v117.h
769 cameras[i]->fps = MT9V117_TARGET_FPS;
770 cameras[i]->camera_intrinsics = {
771 .focal_x = MT9V117_FOCAL_X,
772 .focal_y = MT9V117_FOCAL_Y,
773 .center_x = MT9V117_CENTER_X,
774 .center_y = MT9V117_CENTER_Y,
775 .Dhane_k = MT9V117_DHANE_K
776 };
777 }
778 cout << "ok" << endl;
779 }
780}
781
790static void gazebo_read_video(void)
791{
792 for (int i = 0; i < VIDEO_THREAD_MAX_CAMERAS; ++i) {
793 gazebo::sensors::CameraSensorPtr &cam = gazebo_cams[i].cam;
794 // Skip unregistered or unfound cameras
795 if (cam == NULL) { continue; }
796 // Skip if not updated
797 // Also skip when LastMeasurementTime() is zero (workaround)
798 if ((cam->LastMeasurementTime() - gazebo_cams[i].last_measurement_time).Float() < 0.005
799 || cam->LastMeasurementTime() == 0) { continue; }
800 // Grab image, convert and send to video thread
801 struct image_t img;
802 read_image(&img, cam);
803
804#if NPS_DEBUG_VIDEO
805 cv::Mat RGB_cam(cam->ImageHeight(), cam->ImageWidth(), CV_8UC3, (uint8_t *)cam->ImageData());
806 cv::cvtColor(RGB_cam, RGB_cam, cv::COLOR_RGB2BGR);
807 cv::namedWindow(cameras[i]->dev_name, cv::WINDOW_AUTOSIZE); // Create a window for display.
808 cv::imshow(cameras[i]->dev_name, RGB_cam);
809 cv::waitKey(1);
810#endif
811
812 cv_run_device(cameras[i], &img);
813 // Free image buffer after use.
814 image_free(&img);
815 // Keep track of last update time.
816 gazebo_cams[i].last_measurement_time = cam->LastMeasurementTime();
817 }
818}
819
830static void read_image(struct image_t *img, gazebo::sensors::CameraSensorPtr cam)
831{
832 bool is_mt9f002 = false;
833 if (cam->Name() == "mt9f002") {
835 is_mt9f002 = true;
836 } else {
837 image_create(img, cam->ImageWidth(), cam->ImageHeight(), IMAGE_YUV422);
838 }
839
840 // Convert Gazebo's *RGB888* image to Paparazzi's YUV422
841 const uint8_t *data_rgb = cam->ImageData();
842 uint8_t *data_yuv = (uint8_t *)(img->buf);
843 for (int x_yuv = 0; x_yuv < img->w; ++x_yuv) {
844 for (int y_yuv = 0; y_yuv < img->h; ++y_yuv) {
845 int x_rgb = x_yuv;
846 int y_rgb = y_yuv;
847 if (is_mt9f002) {
848 // Change sampling points for zoomed and/or cropped image.
849 // Use nearest-neighbour sampling for now.
850 x_rgb = (mt9f002.offset_x + ((float)x_yuv / img->w) * mt9f002.sensor_width)
851 / CFG_MT9F002_PIXEL_ARRAY_WIDTH * cam->ImageWidth();
852 y_rgb = (mt9f002.offset_y + ((float)y_yuv / img->h) * mt9f002.sensor_height)
853 / CFG_MT9F002_PIXEL_ARRAY_HEIGHT * cam->ImageHeight();
854 }
855 int idx_rgb = 3 * (cam->ImageWidth() * y_rgb + x_rgb);
856 int idx_yuv = 2 * (img->w * y_yuv + x_yuv);
857 int idx_px = img->w * y_yuv + x_yuv;
858 if (idx_px % 2 == 0) { // Pick U or V
859 data_yuv[idx_yuv] = - 0.148 * data_rgb[idx_rgb]
860 - 0.291 * data_rgb[idx_rgb + 1]
861 + 0.439 * data_rgb[idx_rgb + 2] + 128; // U
862 } else {
863 data_yuv[idx_yuv] = 0.439 * data_rgb[idx_rgb]
864 - 0.368 * data_rgb[idx_rgb + 1]
865 - 0.071 * data_rgb[idx_rgb + 2] + 128; // V
866 }
867 data_yuv[idx_yuv + 1] = 0.257 * data_rgb[idx_rgb]
868 + 0.504 * data_rgb[idx_rgb + 1]
869 + 0.098 * data_rgb[idx_rgb + 2] + 16; // Y
870 }
871 }
872 // Fill miscellaneous fields
873 gazebo::common::Time ts = cam->LastMeasurementTime();
874 img->ts.tv_sec = ts.sec;
875 img->ts.tv_usec = ts.nsec / 1000.0;
876 img->pprz_ts = ts.Double() * 1e6;
877 img->buf_idx = 0; // unused
878}
879#endif
880
881#if NPS_SIMULATE_LASER_RANGE_ARRAY
882/*
883 * Simulate range sensors:
884 *
885 * In the airframe file, set NPS_SIMULATE_LASER_RANGE_ARRAY if you want to make use of the integrated Ray sensors.
886 * These are defined in their own model which is added to the aircraft sdf (called range_sensors). Here you can add
887 * single ray point sensors with a specified position and orientation. It is also possible add noise.
888 *
889 * Within the airframe file, the amount of sensors
890 * (LASER_RANGE_ARRAY_NUM_SENSORS) and their orientations
891 * (LASER_RANGE_ARRAY_ORIENTATIONS={phi_1,theta_1,psi_1...phi_n,theta_n,psi_n} n = number of sensors) need to be
892 * specified as well. This is to keep it generic since this need to be done on the real platform with an external
893 * ray sensor. The function will compare the orientations from the ray sensors of gazebo, with the ones specified
894 * in the airframe, and will fill up an array to send and abi message to be used by other modules.
895 *
896 * NPS_GAZEBO_RANGE_SEND_AGL defines if the sensor that is defined as down should be used to send an AGL message.
897 * to send an OBSTACLE_DETECTION message.
898 *
899 * Functions:
900 * gazebo_init_range_sensors() -> Finds and initializes all ray sensors in gazebo
901 * gazebo_read_range_sensors() -> Reads and evaluates the ray sensors values, and sending it to other pprz modules
902 */
903
904struct gazebo_ray_t {
905 gazebo::sensors::RaySensorPtr sensor;
906 gazebo::common::Time last_measurement_time;
907};
908
909static struct gazebo_ray_t rays[LASER_RANGE_ARRAY_NUM_SENSORS] = {{NULL, 0}};
910static float range_orientation[] = LASER_RANGE_ARRAY_ORIENTATIONS;
911static uint8_t ray_sensor_agl_index = 255;
912
913#define VL53L0_MAX_VAL 8.191f
914
915static void gazebo_init_range_sensors(void)
916{
917 gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance();
918 gazebo::sensors::Sensor_V sensors = mgr->GetSensors(); // list of all sensors
919
920 uint16_t sensor_count = model->GetSensorCount();
921
922 gazebo::sensors::RaySensorPtr ray_sensor;
923 uint8_t ray_sensor_count_selected = 0;
924
925 // Loop though all sensors and only select ray sensors, which are saved within a struct
926 for (uint16_t i = 0; i < sensor_count; i++) {
927 if (ray_sensor_count_selected > LASER_RANGE_ARRAY_NUM_SENSORS) {
928 break;
929 }
930
931 if (sensors.at(i)->Type() == "ray") {
932 ray_sensor = static_pointer_cast<gazebo::sensors::RaySensor>(sensors.at(i));
933
934 if (!ray_sensor) {
935 cout << "ERROR: Could not get pointer to raysensor " << i << "!" << endl;
936 continue;
937 }
938
939 // Read out the pose from per ray sensors in gazebo relative to body
940 struct DoubleEulers pose_sensor = to_pprz_eulers(ray_sensor->Pose().Rot());
941
942 struct DoubleQuat q_ray, q_def;
943 double_quat_of_eulers(&q_ray, &pose_sensor);
944
945 /* Check the orientations of the ray sensors found in gazebo, if they are similar (within 5 deg) to the orientations
946 * given in the airframe file in LASER_RANGE_ARRAY_RANGE_ORIENTATION
947 */
948 for (int j = 0; j < LASER_RANGE_ARRAY_NUM_SENSORS; j++) {
949 struct DoubleEulers def = {0, range_orientation[j * 2], range_orientation[j * 2 + 1]};
950 double_quat_of_eulers(&q_def, &def);
951 // get angle between required angle and ray angle
952 double angle = acos(QUAT_DOT_PRODUCT(q_ray, q_def));
953
954 if (fabs(angle) < RadOfDeg(5)) {
955 ray_sensor_count_selected++;
956 rays[j].sensor = ray_sensor;
957 rays[j].sensor->SetActive(true);
958
959#if LASER_RANGE_ARRAY_SEND_AGL
960 // find the sensor pointing down
961 if (fabs(range_orientation[j * 2] + M_PI_2) < RadOfDeg(5)) {
962 ray_sensor_agl_index = j;
963 }
964#endif
965 break;
966 }
967 }
968 }
969 }
970
971 if (ray_sensor_count_selected != LASER_RANGE_ARRAY_NUM_SENSORS) {
972 cout << "ERROR: you have defined " << LASER_RANGE_ARRAY_NUM_SENSORS << " sensors in your airframe file, but only "
973 << ray_sensor_count_selected << " sensors have been found in the gazebo simulator, "
974 "with the same orientation as in the airframe file " << endl;
975 exit(0);
976 }
977 cout << "Initialized laser range array" << endl;
978}
979
980static void gazebo_read_range_sensors(void)
981{
982 static float range;
983
984 // Loop through all ray sensors found in gazebo
985 for (int i = 0; i < LASER_RANGE_ARRAY_NUM_SENSORS; i++) {
986 if ((rays[i].sensor->LastMeasurementTime() - rays[i].last_measurement_time).Float() < 0.005
987 || rays[i].sensor->LastMeasurementTime() == 0) { continue; }
988
989 if (rays[i].sensor->Range(0) < 1e-5 || rays[i].sensor->Range(0) > VL53L0_MAX_VAL) {
990 range = VL53L0_MAX_VAL;
991 } else {
992 range = rays[i].sensor->Range(0);
993 }
994 AbiSendMsgOBSTACLE_DETECTION(OBS_DETECTION_RANGE_ARRAY_NPS_ID, range, range_orientation[i * 2],
995 range_orientation[i * 2 + 1]);
996
997 if (i == ray_sensor_agl_index) {
998 uint32_t now_ts = get_sys_time_usec();
999 float agl = rays[i].sensor->Range(0);
1000 // Down range sensor as agl
1001 if (agl > 1e-5 && !isinf(agl)) {
1002 AbiSendMsgAGL(AGL_RAY_SENSOR_GAZEBO_ID, now_ts, agl);
1003 }
1004 }
1005 rays[i].last_measurement_time = rays[i].sensor->LastMeasurementTime();
1006 }
1007}
1008#endif // NPS_SIMULATE_LASER_RANGE_ARRAY
1009
1010#pragma GCC diagnostic pop // Ignore -Wdeprecated-declarations
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Main include for ABI (AirBorneInterface).
#define OBS_DETECTION_RANGE_ARRAY_NPS_ID
#define AGL_RAY_SENSOR_GAZEBO_ID
struct pprz_autopilot autopilot
Global autopilot structure.
Definition autopilot.c:50
Core autopilot interface common to all firmwares.
#define sensors(...)
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
void cv_run_device(struct video_config_t *device, struct image_t *img)
Definition cv.c:178
Computer vision framework for onboard processing.
#define Min(x, y)
Definition esc_dshot.c:110
double psi
in radians
double m[3 *3]
double p
in rad/s^2
double theta
in radians
double q
in rad/s^2
double phi
in radians
double r
in rad/s^2
void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e)
Roation quaternion.
angular rates
#define QUAT_DOT_PRODUCT(_qa, _qb)
double x
in meters
struct EcefCoor_d ecef
origin of local frame in ECEF
double y
in meters
double y
in meters
struct LlaCoor_d lla
origin of local frame in LLA
double z
in meters
double z
in meters
double lat
in radians
double alt
in meters above WGS84 reference ellipsoid
struct DoubleRMat ltp_of_ecef
rotation from ECEF to local frame
double lon
in radians
double hmsl
height in meters above mean sea level
double x
in meters
double gc_of_gd_lat_d(double gd_lat, double hmsl)
void ecef_of_ned_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef)
void ecef_of_ned_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
vector in North East Down coordinates Units: meters
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
struct State state
Definition state.c:36
Simple high pass filter with double precision.
static void init_first_order_high_pass(struct FirstOrderHighPass *filter, float tau, float sample_time, float value)
Init first order high pass filter.
static float update_first_order_high_pass(struct FirstOrderHighPass *filter, float value)
Update first order high pass filter state with a new value.
First order high pass filter structure.
void image_free(struct image_t *img)
Free the image.
Definition image.c:75
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
Image helper functions like resizing, color filter, converters...
struct timeval ts
The timestamp of creation.
Definition image.h:48
void * buf
Image buffer (depending on the image_type).
Definition image.h:54
uint32_t pprz_ts
The timestamp in us since system startup.
Definition image.h:50
uint16_t h
Image height.
Definition image.h:47
uint8_t buf_idx
Buffer index for V4L2 freeing.
Definition image.h:52
uint16_t w
Image width.
Definition image.h:46
@ IMAGE_YUV422
UYVY format (uint16 per pixel).
Definition image.h:36
#define VL53L0_MAX_VAL
Simple first order low pass filter with bilinear transform.
static void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, const float sample_time, float value)
Init first order low pass filter.
static float update_first_order_low_pass(struct FirstOrderLowPass *filter, const float value)
Update first order low pass filter state with a new value.
First order low pass filter structure.
static const int32_t yaw_coef[MOTOR_MIXING_NB_MOTOR]
Common Motor Mixing configuration types.
#define MOTOR_MIXING_YAW_COEF
struct mt9f002_t mt9f002
Definition mt9f002.c:168
Initialization and configuration of the MT9F002 CMOS Chip.
#define MT9F002_CENTER_X
Definition mt9f002.h:116
#define MT9F002_DHANE_K
Definition mt9f002.h:122
#define CFG_MT9F002_PIXEL_ARRAY_WIDTH
Definition mt9f002.h:39
#define MT9F002_CENTER_Y
Definition mt9f002.h:119
#define MT9F002_FOCAL_Y
Definition mt9f002.h:113
#define MT9F002_OUTPUT_WIDTH
Definition mt9f002.h:82
#define CFG_MT9F002_PIXEL_ARRAY_HEIGHT
Definition mt9f002.h:38
#define MT9F002_TARGET_FPS
Definition mt9f002.h:105
#define MT9F002_FOCAL_X
Definition mt9f002.h:110
#define MT9F002_OUTPUT_HEIGHT
Definition mt9f002.h:85
Initialization and configuration of the MT9V117 CMOS Chip.
#define MT9V117_CENTER_Y
Definition mt9v117.h:50
#define MT9V117_TARGET_FPS
Definition mt9v117.h:36
#define MT9V117_DHANE_K
Definition mt9v117.h:53
#define MT9V117_FOCAL_X
Definition mt9v117.h:41
#define MT9V117_CENTER_X
Definition mt9v117.h:47
#define MT9V117_FOCAL_Y
Definition mt9v117.h:44
#define NPS_COMMANDS_NB
Number of commands sent to the FDM of NPS.
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
void nps_fdm_init(double dt)
Initialize actuator dynamics, set unused fields in fdm.
struct gazebo_actuators_t gazebo_actuators
void nps_fdm_run_step(bool launch, double *act_commands, int commands_nb)
Update the simulation state.
struct DoubleEulers to_global_pprz_eulers(ignition::math::Quaterniond quat)
static void gazebo_write(double act_commands[], int commands_nb)
Write actuator commands to Gazebo.
struct DoubleRates to_pprz_rates(ignition::math::Vector3d body_g)
#define NPS_GAZEBO_WORLD
void nps_fdm_set_temperature(double temp, double h)
Set temperature in degrees Celcius at given height h above MSL.
struct NedCoor_d to_pprz_ned(ignition::math::Vector3d global)
struct LlaCoor_d to_pprz_lla(ignition::math::Vector3d lla_i)
static void gazebo_read(void)
Read Gazebo's simulation state and store the results in the fdm struct used by NPS.
static gazebo::physics::ModelPtr model
static gazebo::sensors::ContactSensorPtr ct
std::shared_ptr< gazebo::sensors::SonarSensor > sonar
void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity)
struct DoubleVect3 to_pprz_ltp(ignition::math::Vector3d xyz)
static void init_gazebo(void)
Set up a Gazebo server.
struct EcefCoor_d to_pprz_ecef(ignition::math::Vector3d ecef_i)
#define NPS_GAZEBO_AC_NAME
static bool gazebo_initialized
void nps_fdm_set_wind(double speed, double dir)
void nps_fdm_run_init_step(void)
struct DoubleVect3 to_pprz_body(ignition::math::Vector3d body_g)
struct DoubleEulers to_pprz_eulers(ignition::math::Quaterniond quat)
struct FirstOrderLowPass lowpass[NPS_COMMANDS_NB]
struct FirstOrderHighPass highpass[NPS_COMMANDS_NB]
double thrusts[NPS_COMMANDS_NB]
string names[NPS_COMMANDS_NB]
double max_ang_momentum[NPS_COMMANDS_NB]
double torques[NPS_COMMANDS_NB]
Paparazzi double precision floating point algebra.
Paparazzi double-precision floating point math for geodetic calculations.
Paparazzi floating point math for geodetic calculations.
Paparazzi atmospheric pressure conversion utilities.
static const float dir[]
API to get/set the generic vehicle states.
Architecture independent timing functions.
#define VIDEO_THREAD_MAX_CAMERAS
static struct video_config_t * cameras[VIDEO_THREAD_MAX_CAMERAS]
This header gives NPS access to the list of added cameras.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.