35#include <gazebo/gazebo.hh>
36#include <gazebo/physics/physics.hh>
37#include <gazebo/sensors/sensors.hh>
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"
49#include "generated/airframe.h"
50#include "generated/flight_plan.h"
66#if defined(NPS_DEBUG_VIDEO)
68#include <opencv2/core/core.hpp>
69#include <opencv2/highgui/highgui.hpp>
70#include <opencv2/imgproc/imgproc.hpp>
75#ifndef NPS_GAZEBO_WORLD
76#define NPS_GAZEBO_WORLD "empty.world"
78#ifndef NPS_GAZEBO_AC_NAME
79#define NPS_GAZEBO_AC_NAME "bebop"
94static void init_gazebo_video(
void);
95static void gazebo_read_video(
void);
96static void read_image(
98 gazebo::sensors::CameraSensorPtr cam);
100 gazebo::sensors::CameraSensorPtr cam;
101 gazebo::common::Time last_measurement_time;
109#ifndef NPS_MT9F002_SENSOR_RES_DIVIDER
110#define NPS_MT9F002_SENSOR_RES_DIVIDER 1
125#if NPS_SIMULATE_LASER_RANGE_ARRAY
130static void gazebo_init_range_sensors(
void);
131static void gazebo_read_range_sensors(
void);
135std::shared_ptr<gazebo::sensors::SonarSensor>
sonar = NULL;
142static gazebo::physics::ModelPtr
model = NULL;
145static gazebo::sensors::ContactSensorPtr
ct;
150static void gazebo_write(
double act_commands[],
int commands_nb);
156 ecef_p.
x = ecef_i.X();
157 ecef_p.
y = ecef_i.Y();
158 ecef_p.
z = ecef_i.Z();
174 lla_p.
lat = lla_i.X();
175 lla_p.
lon = lla_i.Y();
176 lla_p.
alt = lla_i.Z();
183 body_p.
x = body_g.X();
184 body_p.
y = -body_g.Y();
185 body_p.
z = -body_g.Z();
192 body_p.
p = body_g.X();
193 body_p.
q = -body_g.Y();
194 body_p.
r = -body_g.Z();
201 eulers.
psi = -quat.Yaw();
202 eulers.
theta = -quat.Pitch();
203 eulers.
phi = quat.Roll();
210 eulers.
psi = -quat.Yaw() - M_PI / 2;
211 eulers.
theta = -quat.Pitch();
212 eulers.
phi = quat.Roll();
242#ifdef NPS_ACTUATOR_TIME_CONSTANTS
248#ifdef NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM
250 const float Iwmax[
NPS_COMMANDS_NB] = NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM;
266 bool launch __attribute__((unused)),
267 double *act_commands,
279#if NPS_SIMULATE_VIDEO
282#if NPS_SIMULATE_LASER_RANGE_ARRAY
283 gazebo_init_range_sensors();
289 gazebo::runWorld(
model->GetWorld(), 1);
290 gazebo::sensors::run_once();
293#if NPS_SIMULATE_VIDEO
296#if NPS_SIMULATE_LASER_RANGE_ARRAY
297 gazebo_read_range_sensors();
305 double speed __attribute__((unused)),
306 double dir __attribute__((unused)))
311 double wind_north __attribute__((unused)),
312 double wind_east __attribute__((unused)),
313 double wind_down __attribute__((unused)))
318 double wind_speed __attribute__((unused)),
319 int turbulence_severity __attribute__((unused)))
325 double temp __attribute__((unused)),
326 double h __attribute__((unused)))
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;
349 if (getenv(
"ROS_MASTER_URI")) {
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;
357 if (!gazebo::setupServer(0, NULL)) {
358 cout <<
"Failed to start Gazebo, exiting." << endl;
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/");
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/");
374 string vehicle_filename = sdf::findFile(vehicle_uri,
false);
375 if (vehicle_filename.empty()) {
376 cout <<
"ERROR, could not find vehicle " + vehicle_uri << endl;
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;
388#if NPS_SIMULATE_VIDEO
390 sdf::ElementPtr link = vehicle_sdf->Root()->GetFirstElement()->GetElement(
"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);
402 int fps =
Min(
MT9F002_TARGET_FPS, link->GetElement(
"sensor")->GetElement(
"update_rate")->Get<
int>());
403 link->GetElement(
"sensor")->GetElement(
"update_rate")->Set(fps);
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);
411 link = link->GetNextElement(
"link");
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");
427 string world_filename = sdf::findFile(world_uri,
false);
428 if (world_filename.empty()) {
429 cout <<
"ERROR, could not find world " + world_uri << endl;
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;
441 world_sdf->Root()->GetFirstElement()->InsertElement(vehicle_sdf->Root()->GetFirstElement());
443 world_sdf->Write(pprz_home +
"/var/gazebo.world");
445 gazebo::physics::WorldPtr world = gazebo::loadWorld(pprz_home +
"/var/gazebo.world");
447 cout <<
"Failed to open world, exiting." << endl;
460 gazebo::sensors::run_once(
true);
461 gazebo::sensors::run_threads();
462 gazebo::runWorld(world, 1);
463 cout <<
"Sensors initialized..." << endl;
467 gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance();
468 ct = static_pointer_cast < gazebo::sensors::ContactSensor > (mgr->GetSensor(
"contactsensor"));
471 sonar = static_pointer_cast<gazebo::sensors::SonarSensor>(mgr->GetSensor(
"sonarsensor"));
473 cout <<
"Found sonar" << endl;
476 gazebo::physics::LinkPtr sonar_link =
model->GetLink(
"sonar");
479 if (sonar_link->GetSensorCount() != 1) {
480 cout <<
"ERROR: Link '" << sonar_link->GetName()
481 <<
"' should only contain 1 sensor!" << endl;
483 string name = sonar_link->GetSensorName(0);
484 sonar = static_pointer_cast< gazebo::sensors::SonarSensor > (mgr->GetSensor(name));
486 cout <<
"ERROR: Could not get pointer to '" << name <<
"'!" << endl;
489 sonar->SetActive(
true);
495#ifdef MOTOR_MIXING_YAW_COEF
503 cout <<
"Gazebo initialized successfully!" << endl;
516 static ignition::math::Vector3d vel_prev;
517 static double time_prev;
519 gazebo::physics::WorldPtr world =
model->GetWorld();
520 ignition::math::Pose3d pose =
model->WorldPose();
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());
527 fdm.time = world->SimTime().Double();
533 double dt =
fdm.time - time_prev;
534 ignition::math::Vector3d accel = (vel - vel_prev) / dt;
536 time_prev =
fdm.time;
551 for (
int i = 0; i < 3 * 3; i++) {
557 fdm.ltpprz_pos =
to_pprz_ned(sphere->PositionTransform(pose.Pos(), gazebo::common::SphericalCoordinates::LOCAL,
558 gazebo::common::SphericalCoordinates::GLOBAL));
559 fdm.hmsl = -
fdm.ltpprz_pos.z;
564 fdm.lla_pos_pprz =
fdm.lla_pos;
565 fdm.lla_pos_geod =
fdm.lla_pos;
566 fdm.lla_pos_geoc =
fdm.lla_pos;
570 double agl =
sonar->Range();
571 if (agl >
sonar->RangeMax()) agl = -1.0;
574 fdm.agl = pose.Pos().Z();
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;
585 fdm.ltp_ecef_accel =
to_pprz_ned(sphere->VelocityTransform(accel, gazebo::common::SphericalCoordinates::LOCAL,
586 gazebo::common::SphericalCoordinates::GLOBAL));
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;
590 fdm.body_accel =
to_pprz_body(pose.Rot().RotateVectorReverse(accel - world->Gravity()));
597 fdm.ltpprz_to_body_quat =
fdm.ltp_to_body_quat;
598 fdm.ltpprz_to_body_eulers =
fdm.ltp_to_body_eulers;
601 fdm.body_ecef_rotvel =
to_pprz_rates(pose.Rot().RotateVectorReverse(ang_vel));
602 fdm.body_inertial_rotvel =
fdm.body_ecef_rotvel;
608 fdm.ltp_g =
to_pprz_ltp(sphere->VelocityTransform(-1 * world->Gravity(), gazebo::common::SphericalCoordinates::LOCAL,
609 gazebo::common::SphericalCoordinates::GLOBAL));
610 fdm.ltp_h =
to_pprz_ltp(sphere->VelocityTransform(world->MagneticField(), gazebo::common::SphericalCoordinates::LOCAL,
611 gazebo::common::SphericalCoordinates::GLOBAL));
614#if GAZEBO_MAJOR_VERSION >= 8 && 0
620 fdm.pressure_sl = 101325;
624 fdm.dynamic_pressure =
fdm.pressure_sl;
625 fdm.temperature = 20.0;
632 fdm.left_aileron = 0;
633 fdm.right_aileron = 0;
658 for (
int i = 0; i < commands_nb; ++i) {
660 double sp =
autopilot.motors_on ? act_commands[i] : 0.0;
663#ifdef NPS_ACTUATOR_TIME_CONSTANTS
672#ifdef NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM
675 double spinup_torque =
gazebo_actuators.max_ang_momentum[i] / (2.0 * sqrt(u > 0.05 ? u : 0.05)) * udot;
676 torque += spinup_torque;
681 link->AddRelativeForce(ignition::math::Vector3d(0, 0, thrust));
682 link->AddRelativeTorque(ignition::math::Vector3d(0, 0, torque));
686#if NPS_SIMULATE_VIDEO
701static void init_gazebo_video(
void)
703 gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance();
705 cout <<
"Initializing cameras..." << endl;
709 cout <<
"Setting up '" <<
cameras[i]->dev_name <<
"'... ";
710 gazebo::physics::LinkPtr link =
model->GetLink(
cameras[i]->dev_name);
712 cout <<
"ERROR: Link '" <<
cameras[i]->dev_name <<
"' not found!"
718 if (link->GetSensorCount() != 1) {
719 cout <<
"ERROR: Link '" << link->GetName()
720 <<
"' should only contain 1 sensor!" << endl;
723 string name = link->GetSensorName(0);
724 gazebo::sensors::CameraSensorPtr cam = static_pointer_cast
725 < gazebo::sensors::CameraSensor > (mgr->GetSensor(name));
727 cout <<
"ERROR: Could not get pointer to '" << name <<
"'!" << endl;
731 cam->SetActive(
true);
734 gazebo_cams[i].cam = cam;
735 gazebo_cams[i].last_measurement_time = cam->LastMeasurementTime();
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();
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;
751 if (cam->Name() ==
"mt9f002") {
760 cameras[i]->camera_intrinsics = {
767 }
else if (cam->Name() ==
"mt9v117") {
770 cameras[i]->camera_intrinsics = {
778 cout <<
"ok" << endl;
790static void gazebo_read_video(
void)
793 gazebo::sensors::CameraSensorPtr &cam = gazebo_cams[i].cam;
795 if (cam == NULL) {
continue; }
798 if ((cam->LastMeasurementTime() - gazebo_cams[i].last_measurement_time).Float() < 0.005
799 || cam->LastMeasurementTime() == 0) {
continue; }
802 read_image(&img, cam);
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);
808 cv::imshow(
cameras[i]->dev_name, RGB_cam);
816 gazebo_cams[i].last_measurement_time = cam->LastMeasurementTime();
830static void read_image(
struct image_t *img, gazebo::sensors::CameraSensorPtr cam)
832 bool is_mt9f002 =
false;
833 if (cam->Name() ==
"mt9f002") {
841 const uint8_t *data_rgb = cam->ImageData();
843 for (
int x_yuv = 0; x_yuv < img->
w; ++x_yuv) {
844 for (
int y_yuv = 0; y_yuv < img->
h; ++y_yuv) {
850 x_rgb = (
mt9f002.offset_x + ((float)x_yuv / img->
w) *
mt9f002.sensor_width)
852 y_rgb = (
mt9f002.offset_y + ((float)y_yuv / img->
h) *
mt9f002.sensor_height)
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) {
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;
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;
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;
873 gazebo::common::Time
ts = cam->LastMeasurementTime();
874 img->
ts.tv_sec =
ts.sec;
875 img->
ts.tv_usec =
ts.nsec / 1000.0;
881#if NPS_SIMULATE_LASER_RANGE_ARRAY
905 gazebo::sensors::RaySensorPtr sensor;
906 gazebo::common::Time last_measurement_time;
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;
913#define VL53L0_MAX_VAL 8.191f
915static void gazebo_init_range_sensors(
void)
917 gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance();
918 gazebo::sensors::Sensor_V
sensors = mgr->GetSensors();
922 gazebo::sensors::RaySensorPtr ray_sensor;
923 uint8_t ray_sensor_count_selected = 0;
926 for (
uint16_t i = 0; i < sensor_count; i++) {
927 if (ray_sensor_count_selected > LASER_RANGE_ARRAY_NUM_SENSORS) {
931 if (
sensors.at(i)->Type() ==
"ray") {
932 ray_sensor = static_pointer_cast<gazebo::sensors::RaySensor>(
sensors.at(i));
935 cout <<
"ERROR: Could not get pointer to raysensor " << i <<
"!" << endl;
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]};
954 if (fabs(angle) < RadOfDeg(5)) {
955 ray_sensor_count_selected++;
956 rays[j].sensor = ray_sensor;
957 rays[j].sensor->SetActive(
true);
959#if LASER_RANGE_ARRAY_SEND_AGL
961 if (fabs(range_orientation[j * 2] + M_PI_2) < RadOfDeg(5)) {
962 ray_sensor_agl_index = j;
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;
977 cout <<
"Initialized laser range array" << endl;
980static void gazebo_read_range_sensors(
void)
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; }
989 if (rays[i].sensor->Range(0) < 1e-5 || rays[i].sensor->Range(0) >
VL53L0_MAX_VAL) {
992 range = rays[i].sensor->Range(0);
995 range_orientation[i * 2 + 1]);
997 if (i == ray_sensor_agl_index) {
999 float agl = rays[i].sensor->Range(0);
1001 if (agl > 1e-5 && !isinf(agl)) {
1005 rays[i].last_measurement_time = rays[i].sensor->LastMeasurementTime();
1010#pragma GCC diagnostic pop
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.
Core autopilot interface common to all firmwares.
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)
Computer vision framework for onboard processing.
void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e)
#define QUAT_DOT_PRODUCT(_qa, _qb)
struct EcefCoor_d ecef
origin of local frame in ECEF
struct LlaCoor_d lla
origin of local frame in LLA
double alt
in meters above WGS84 reference ellipsoid
struct DoubleRMat ltp_of_ecef
rotation from ECEF to local frame
double hmsl
height in meters above mean sea level
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).
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.
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
Create a new image.
Image helper functions like resizing, color filter, converters...
struct timeval ts
The timestamp of creation.
void * buf
Image buffer (depending on the image_type).
uint32_t pprz_ts
The timestamp in us since system startup.
uint8_t buf_idx
Buffer index for V4L2 freeing.
@ IMAGE_YUV422
UYVY format (uint16 per pixel).
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
Initialization and configuration of the MT9F002 CMOS Chip.
#define CFG_MT9F002_PIXEL_ARRAY_WIDTH
#define MT9F002_OUTPUT_WIDTH
#define CFG_MT9F002_PIXEL_ARRAY_HEIGHT
#define MT9F002_TARGET_FPS
#define MT9F002_OUTPUT_HEIGHT
Initialization and configuration of the MT9V117 CMOS Chip.
#define MT9V117_TARGET_FPS
#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)
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.
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.