From 68255aa4547eaebb47a0418384b6e4b53d29e4ad Mon Sep 17 00:00:00 2001 From: Kirk Scheper Date: Wed, 18 Oct 2017 22:30:07 +0200 Subject: [PATCH] updates to gazebo so simulate pressure and acceleration (#2120) --- conf/Makefile.nps | 11 ++- conf/modules/fdm_gazebo.xml | 9 ++- sw/airborne/arch/sim/mcu_arch.c | 1 + sw/airborne/arch/sim/mcu_periph/i2c_arch.c | 2 +- sw/simulator/nps/nps_autopilot_fixedwing.c | 2 + sw/simulator/nps/nps_autopilot_rotorcraft.c | 2 +- sw/simulator/nps/nps_fdm_gazebo.cpp | 76 +++++++++++++------ sw/simulator/nps/nps_flightgear.c | 8 +- sw/simulator/nps/nps_flightgear.h | 6 +- sw/simulator/nps/nps_main_common.c | 2 + sw/simulator/nps/nps_radio_control_spektrum.c | 4 + 11 files changed, 86 insertions(+), 37 deletions(-) diff --git a/conf/Makefile.nps b/conf/Makefile.nps index a19edca2e6..a4e671e0d5 100644 --- a/conf/Makefile.nps +++ b/conf/Makefile.nps @@ -32,7 +32,14 @@ DEBUG_FLAGS ?= -ggdb3 # Launch with "make Q=''" to get full command display Q=@ -CFLAGS = -W -Wall +# flags for warnings (C and C++) +WARN_FLAGS += -W -Wall -Wextra +WARN_FLAGS += -Wunused -Wcast-align -Wpointer-arith -Wswitch-default -Wmissing-declarations +#WARN_FLAGS += -Wcast-qual +#WARN_FLAGS += -Wredundant-decls +#WARN_FLAGS += -Wshadow + +CFLAGS = $(WARN_FLAGS) CFLAGS += $(INCLUDES) CFLAGS += $($(TARGET).CFLAGS) CFLAGS += $(USER_CFLAGS) $(BOARD_CFLAGS) @@ -41,7 +48,7 @@ CFLAGS += $(DEBUG_FLAGS) CFLAGS += -std=gnu99 CFLAGS += $(shell pkg-config --cflags-only-I ivy-glib) -CXXFLAGS = -W -Wall +CXXFLAGS = $(WARN_FLAGS) CXXFLAGS += $(INCLUDES) CXXFLAGS += $($(TARGET).CFLAGS) CXXFLAGS += $($(TARGET).CXXFLAGS) diff --git a/conf/modules/fdm_gazebo.xml b/conf/modules/fdm_gazebo.xml index ce32db93a7..d4ec1bdade 100644 --- a/conf/modules/fdm_gazebo.xml +++ b/conf/modules/fdm_gazebo.xml @@ -83,10 +83,15 @@
- nps.CXXFLAGS += $(shell pkg-config gazebo --cflags) - nps.LDFLAGS += $(shell pkg-config gazebo --libs) + nps.CXXFLAGS += $(shell pkg-config gazebo --cflags opencv) + nps.LDFLAGS += $(shell pkg-config gazebo --libs) + + + + + diff --git a/sw/airborne/arch/sim/mcu_arch.c b/sw/airborne/arch/sim/mcu_arch.c index 1820c41208..445db2179d 100644 --- a/sw/airborne/arch/sim/mcu_arch.c +++ b/sw/airborne/arch/sim/mcu_arch.c @@ -1,3 +1,4 @@ +#include "mcu_arch.h" void mcu_arch_init(void) {} diff --git a/sw/airborne/arch/sim/mcu_periph/i2c_arch.c b/sw/airborne/arch/sim/mcu_periph/i2c_arch.c index 705e00d9f1..db4095607f 100644 --- a/sw/airborne/arch/sim/mcu_periph/i2c_arch.c +++ b/sw/airborne/arch/sim/mcu_periph/i2c_arch.c @@ -30,7 +30,7 @@ bool i2c_idle(struct i2c_periph *p __attribute__((unused))) { return true; } bool i2c_submit(struct i2c_periph *p __attribute__((unused)), struct i2c_transaction *t __attribute__((unused))) { return true;} void i2c_event(void) { } -void i2c2_setbitrate(int bitrate __attribute__((unused))) { } +void i2c_setbitrate(struct i2c_periph *p __attribute__((unused)), int bitrate __attribute__((unused))) { } #if USE_I2C0 struct i2c_errors i2c0_errors; diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c index f02a2d0175..e6822182f0 100644 --- a/sw/simulator/nps/nps_autopilot_fixedwing.c +++ b/sw/simulator/nps/nps_autopilot_fixedwing.c @@ -146,11 +146,13 @@ void nps_autopilot_run_step(double time) } #endif +#ifndef NPS_NO_GPS if (nps_sensors_gps_available()) { gps_feed_value(); Fbw(event_task); Ap(event_task); } +#endif #if USE_SONAR if (nps_sensors_sonar_available()) { diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c index 01161c60a0..f8de57863d 100644 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.c +++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c @@ -140,7 +140,7 @@ void nps_autopilot_run_step(double time) } #endif -#if USE_GPS +#if USE_GPS && !defined(NPS_NO_GPS) if (nps_sensors_gps_available()) { gps_feed_value(); main_event(); diff --git a/sw/simulator/nps/nps_fdm_gazebo.cpp b/sw/simulator/nps/nps_fdm_gazebo.cpp index 0e47bce2aa..c74ff112ac 100644 --- a/sw/simulator/nps/nps_fdm_gazebo.cpp +++ b/sw/simulator/nps/nps_fdm_gazebo.cpp @@ -31,10 +31,19 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + +#ifdef NPS_DEBUG_VIDEO +// Opencv tools +#include +#include +#include +#endif + #include #include #include #include +#include #include #include @@ -43,20 +52,13 @@ #include extern "C" { -#include - #include "nps_fdm.h" -#include "math/pprz_algebra_double.h" #include "generated/airframe.h" #include "autopilot.h" -#ifdef NPS_SIMULATE_VIDEO -#include "modules/computer_vision/cv.h" -#include "modules/computer_vision/video_thread_nps.h" -#include "modules/computer_vision/lib/vision/image.h" -#include "mcu_periph/sys_time.h" -#endif +#include "math/pprz_isa.h" +#include "math/pprz_algebra_double.h" } using namespace std; @@ -70,6 +72,13 @@ using namespace std; // Add video handling functions if req'd. #ifdef NPS_SIMULATE_VIDEO +extern "C" { +#include "modules/computer_vision/cv.h" +#include "modules/computer_vision/video_thread_nps.h" +#include "modules/computer_vision/lib/vision/image.h" +#include "mcu_periph/sys_time.h" +} + static void init_gazebo_video(void); static void gazebo_read_video(void); static void read_image( @@ -90,10 +99,13 @@ struct NpsFdm fdm; static bool gazebo_initialized = false; static gazebo::physics::ModelPtr model = NULL; +// Get contact sensor +static gazebo::sensors::ContactSensorPtr ct; + // Helper functions static void init_gazebo(void); static void gazebo_read(void); -static void gazebo_write(double commands[], int commands_nb); +static void gazebo_write(double act_commands[], int commands_nb); // Conversion routines inline struct EcefCoor_d to_pprz_ecef(ignition::math::Vector3d ecef_i) @@ -175,12 +187,12 @@ void nps_fdm_init(double dt) /** * Update the simulation state. * @param launch - * @param commands + * @param act_commands * @param commands_nb */ void nps_fdm_run_step( bool launch __attribute__((unused)), - double *commands, + double *act_commands, int commands_nb) { // Initialize Gazebo if req'd. @@ -201,7 +213,7 @@ void nps_fdm_run_step( // Update the simulation for a single timestep. gazebo::runWorld(model->GetWorld(), 1); gazebo::sensors::run_once(); - gazebo_write(commands, commands_nb); + gazebo_write(act_commands, commands_nb); gazebo_read(); #ifdef NPS_SIMULATE_VIDEO gazebo_read_video(); @@ -286,6 +298,11 @@ static void init_gazebo(void) gazebo::runWorld(world, 1); cout << "Sensors initialized..." << endl; + // activate collision sensor + gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance(); + ct = static_pointer_cast < gazebo::sensors::ContactSensor > (mgr->GetSensor("contactsensor")); + ct->SetActive(true); + cout << "Gazebo initialized successfully!" << endl; } @@ -361,9 +378,15 @@ static void gazebo_read(void) gazebo::common::SphericalCoordinates::GLOBAL)); // Note: unused fdm.ltpprz_ecef_accel = fdm.ltp_ecef_accel; // ??? fdm.body_inertial_accel = fdm.body_ecef_accel; // Approximate, unused. - fdm.body_accel = to_pprz_body( - pose.rot.RotateVectorReverse(accel.Ign() - world->Gravity())); + // only use accelerometer if no collisions or if not on ground + if (ct->Contacts().contact_size() || pose.pos.z < 0.03){ + fdm.body_accel = to_pprz_body( + pose.rot.RotateVectorReverse(-world->Gravity())); + } else { + fdm.body_accel = to_pprz_body( + pose.rot.RotateVectorReverse(accel.Ign() - world->Gravity())); + } /* attitude */ // ecef_to_body_quat: unused fdm.ltp_to_body_eulers = to_pprz_eulers(local_to_global_quat * pose.rot); @@ -398,7 +421,7 @@ static void gazebo_read(void) fdm.pressure_sl = 101325; // Pa fdm.airspeed = 0; - fdm.pressure = fdm.pressure_sl; // Pa + fdm.pressure = pprz_isa_pressure_of_height(fdm.hmsl, fdm.pressure_sl); fdm.dynamic_pressure = fdm.pressure_sl; // Pa fdm.temperature = 20.0; // C fdm.aoa = 0; // rad @@ -428,23 +451,21 @@ static void gazebo_read(void) * NPS_ACTUATOR_NAMES. See conf/simulator/gazebo/models/ardrone/ardrone.sdf * for an example. * - * @param commands + * @param act_commands * @param commands_nb */ -static void gazebo_write(double commands[], int commands_nb) +static void gazebo_write(double act_commands[], int commands_nb) { const string names[] = NPS_ACTUATOR_NAMES; const double thrusts[] = { NPS_ACTUATOR_THRUSTS }; const double torques[] = { NPS_ACTUATOR_TORQUES }; for (int i = 0; i < commands_nb; ++i) { - double thrust = autopilot.motors_on ? thrusts[i] * commands[i] : 0.0; - double torque = autopilot.motors_on ? torques[i] * commands[i] : 0.0; + double thrust = autopilot.motors_on ? thrusts[i] * act_commands[i] : 0.0; + double torque = autopilot.motors_on ? torques[i] * act_commands[i] : 0.0; gazebo::physics::LinkPtr link = model->GetLink(names[i]); link->AddRelativeForce(gazebo::math::Vector3(0, 0, thrust)); link->AddRelativeTorque(gazebo::math::Vector3(0, 0, torque)); - // cout << "Motor '" << link->GetName() << "': thrust = " << thrust - // << " N, torque = " << torque << " Nm" << endl; } } @@ -526,11 +547,20 @@ static void gazebo_read_video(void) if (cam == NULL) { continue; } // Skip if not updated // Also skip when LastMeasurementTime() is zero (workaround) - if (cam->LastMeasurementTime() == gazebo_cams[i].last_measurement_time + if ((cam->LastMeasurementTime() - gazebo_cams[i].last_measurement_time).Float() < 0.005 || cam->LastMeasurementTime() == 0) { continue; } // Grab image, convert and send to video thread struct image_t img; read_image(&img, cam); + +#ifdef NPS_DEBUG_VIDEO + cv::Mat RGB_cam(cam->ImageHeight(),cam->ImageWidth(),CV_8UC3,(uint8_t *)cam->ImageData()); + cv::cvtColor(RGB_cam, RGB_cam, cv::COLOR_RGB2BGR); + cv::namedWindow(cameras[i]->dev_name, cv::WINDOW_AUTOSIZE); // Create a window for display. + cv::imshow(cameras[i]->dev_name, RGB_cam); + cv::waitKey(1); +#endif + cv_run_device(cameras[i], &img); // Free image buffer after use. image_free(&img); diff --git a/sw/simulator/nps/nps_flightgear.c b/sw/simulator/nps/nps_flightgear.c index ac994e46e0..2e60cee3dd 100644 --- a/sw/simulator/nps/nps_flightgear.c +++ b/sw/simulator/nps/nps_flightgear.c @@ -30,7 +30,7 @@ pthread_t th_fg_rx; // fligh gear receive thread void* nps_flightgear_receive(void* data __attribute__((unused))); -double htond(double x) +static double htond(double x) { int *p = (int *)&x; int tmp = p[0]; @@ -41,7 +41,7 @@ double htond(double x) } -float htonf(float x) +static float htonf(float x) { int *p = (int *)&x; *p = htonl(*p); @@ -112,7 +112,7 @@ void nps_flightgear_init(const char *host, unsigned int port, unsigned int port * For visualization with moving surfaces (elevator, propeller etc). * start fgfs with --native-fdm=socket... option */ -void nps_flightgear_send_fdm() +void nps_flightgear_send_fdm(void) { struct FGNetFDM fgfdm; @@ -176,7 +176,7 @@ void nps_flightgear_send_fdm() * * This is the default option */ -void nps_flightgear_send() +void nps_flightgear_send(void) { struct FGNetGUI gui; diff --git a/sw/simulator/nps/nps_flightgear.h b/sw/simulator/nps/nps_flightgear.h index 183a07b73c..039a08d78f 100644 --- a/sw/simulator/nps/nps_flightgear.h +++ b/sw/simulator/nps/nps_flightgear.h @@ -1,10 +1,8 @@ #ifndef NPS_FLIGHTGEAR_H #define NPS_FLIGHTGEAR_H - extern void nps_flightgear_init(const char* host, unsigned int port, unsigned int port_in, unsigned int time_offset); -extern void nps_flightgear_send(); -extern void nps_flightgear_send_fdm(); - +extern void nps_flightgear_send(void); +extern void nps_flightgear_send_fdm(void); #endif /* NPS_FLIGHTGEAR_H */ diff --git a/sw/simulator/nps/nps_main_common.c b/sw/simulator/nps/nps_main_common.c index 9d88d70cd8..b7faa63855 100644 --- a/sw/simulator/nps/nps_main_common.c +++ b/sw/simulator/nps/nps_main_common.c @@ -226,6 +226,8 @@ bool nps_main_parse_options(int argc, char **argv) nps_main.fg_fdm = 1; break; case 10: nps_main.fg_port_in = atoi(optarg); break; + default: + break; } break; diff --git a/sw/simulator/nps/nps_radio_control_spektrum.c b/sw/simulator/nps/nps_radio_control_spektrum.c index c9faf6849f..29505ef4e4 100644 --- a/sw/simulator/nps/nps_radio_control_spektrum.c +++ b/sw/simulator/nps/nps_radio_control_spektrum.c @@ -1,3 +1,4 @@ +#include "nps_radio_control_spektrum.h" #include "nps_radio_control.h" #include @@ -130,6 +131,9 @@ static void parse_data(char *buf, int len) handle_frame(); } break; + default: + status = STA_UNINIT; + break; } } }