diff --git a/conf/airframes/KS/kirk_conf.xml b/conf/airframes/KS/kirk_conf.xml index 0ae1c93aae..646d0cbe40 100644 --- a/conf/airframes/KS/kirk_conf.xml +++ b/conf/airframes/KS/kirk_conf.xml @@ -7,7 +7,7 @@ telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/rotorcraft_optitrack.xml" settings="settings/rotorcraft_basic.xml" - settings_modules="modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml" + settings_modules="modules/ins_hff_extended.xml modules/ahrs_int_cmpl_quat.xml modules/gps.xml modules/imu_common.xml modules/stabilization_int_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml" gui_color="#ffff00000000" /> - + + + + + + + @@ -11,19 +17,15 @@ - - + - - - @@ -57,11 +59,11 @@ - -
- - - + +
+ + +
@@ -78,8 +80,7 @@
- - + @@ -91,10 +92,6 @@ - - - -
@@ -124,6 +121,24 @@ + + + + + + + + + + + + + + + + + +
@@ -149,14 +164,7 @@ - - - + @@ -175,12 +183,6 @@ -
@@ -200,6 +202,13 @@ + + + + + + +
@@ -208,12 +217,6 @@ -
diff --git a/conf/airframes/TUDELFT/tudelft_conf.xml b/conf/airframes/TUDELFT/tudelft_conf.xml index 158e52825f..7651cfd76c 100644 --- a/conf/airframes/TUDELFT/tudelft_conf.xml +++ b/conf/airframes/TUDELFT/tudelft_conf.xml @@ -249,7 +249,7 @@ telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/rotorcraft_basic.xml" settings="settings/rotorcraft_basic.xml" - settings_modules="modules/air_data.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml modules/gps_ubx_ucenter.xml" + settings_modules="modules/air_data.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps_ubx_ucenter.xml modules/gps.xml modules/imu_common.xml" gui_color="blue" /> - + + + + + + + + + + + + + + + + + + + + + + + + + @@ -39,25 +79,20 @@ - - - - - - - -
- - - - - - +
+ + + + + + + +
@@ -82,24 +117,7 @@
-
- - - - -
- -
- - - - - -
- -
- @@ -162,69 +180,41 @@ +
-
- - - -
+
+ + + +
+ +
+ + + + + + +
+ +
+ + + + +
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +
+ + + + + +
diff --git a/conf/modules/video_thread.xml b/conf/modules/video_thread.xml index c57ec52298..7f16776bc9 100644 --- a/conf/modules/video_thread.xml +++ b/conf/modules/video_thread.xml @@ -43,5 +43,7 @@ + + diff --git a/conf/simulator/gazebo/models/ardrone/ardrone.sdf b/conf/simulator/gazebo/models/ardrone/ardrone.sdf index 67065e483b..cca03ce3b3 100644 --- a/conf/simulator/gazebo/models/ardrone/ardrone.sdf +++ b/conf/simulator/gazebo/models/ardrone/ardrone.sdf @@ -1,6 +1,6 @@ - + 0 0 .1 0 0 0 @@ -9,12 +9,12 @@ - + 0.4 - 0.005 - 0.005 - 0.010 + 0.00678 + 0.00678 + 0.01356 0. 0. 0. @@ -24,7 +24,7 @@ - .4 .4 .05 + 0.4 0.4 0.05 @@ -32,7 +32,7 @@ - .2 .2 .05 + 0.2 0.2 0.05 @@ -44,16 +44,15 @@ - - + 0.12 0.12 0 0 0 0 0.01 - .0001 - .0001 - .0001 + 0.0001 + 0.0001 + 0.0001 0 0 0 @@ -63,7 +62,7 @@ 0.10 - .07 + 0.07 @@ -79,9 +78,9 @@ 0.01 - .0001 - .0001 - .0001 + 0.0001 + 0.0001 + 0.0001 0 0 0 @@ -91,7 +90,7 @@ 0.10 - .07 + 0.07 @@ -107,9 +106,9 @@ 0.01 - .0001 - .0001 - .0001 + 0.0001 + 0.0001 + 0.0001 0 0 0 @@ -119,7 +118,7 @@ 0.10 - .07 + 0.07 @@ -135,9 +134,9 @@ 0.01 - .0001 - .0001 - .0001 + 0.0001 + 0.0001 + 0.0001 0 0 0 @@ -147,7 +146,7 @@ 0.10 - .07 + 0.07 @@ -166,9 +165,9 @@ 0.001 - .0001 - .0001 - .0001 + 0.0001 + 0.0001 + 0.0001 0 0 0 @@ -209,9 +208,9 @@ 0.001 - .0001 - .0001 - .0001 + 0.0001 + 0.0001 + 0.0001 0 0 0 diff --git a/conf/simulator/gazebo/models/simple_quad/model.config b/conf/simulator/gazebo/models/simple_quad/model.config new file mode 100644 index 0000000000..f02c496866 --- /dev/null +++ b/conf/simulator/gazebo/models/simple_quad/model.config @@ -0,0 +1,15 @@ + + + Simple quad (Paparazzi) + 1.0 + simple_quad.sdf + + + Kirk Scheper + kirkscheper@users.noreply.github.com + + + + Simple quadrotor model in a plus coniguration for use with Paparazzi's NPS (http://wiki.paparazziuav.org). + + diff --git a/conf/simulator/gazebo/models/simple_quad/simple_quad.sdf b/conf/simulator/gazebo/models/simple_quad/simple_quad.sdf new file mode 100644 index 0000000000..d26a7ab199 --- /dev/null +++ b/conf/simulator/gazebo/models/simple_quad/simple_quad.sdf @@ -0,0 +1,249 @@ + + + + 0 0 .1 0 0 0 + + + + 0.001 + + + + + 0.4 + + 0.00678 + 0.00678 + 0.01356 + 0. + 0. + 0. + + + + + + + 0.4 0.4 0.05 + + + + + + + + 0.2 0.2 0.05 + + + + + + + collision + + + + + + + 0.12 0 0 0 0 0 + + 0.01 + + 0.0001 + 0.0001 + 0.0001 + 0 + 0 + 0 + + + + + + 0.10 + 0.07 + + + + + + + chassis + front_motor + + + + -0.12 0 0 0 0 0 + + 0.01 + + 0.0001 + 0.0001 + 0.0001 + 0 + 0 + 0 + + + + + + 0.10 + 0.07 + + + + + + + chassis + back_motor + + + + 0 -0.12 0 0 0 0 + + 0.01 + + 0.0001 + 0.0001 + 0.0001 + 0 + 0 + 0 + + + + + + 0.10 + 0.07 + + + + + + + chassis + right_motor + + + + 0 0.12 0 0 0 0 + + 0.01 + + 0.0001 + 0.0001 + 0.0001 + 0 + 0 + 0 + + + + + + 0.10 + 0.07 + + + + + + + chassis + left_motor + + + + + + + 0.15 0 0 0 0 0 + + 0.001 + + 0.0001 + 0.0001 + 0.0001 + 0 + 0 + 0 + + + + 30.0 + + 1.3962634 + + 1280 + 720 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + + + + chassis + front_camera + + + + 0 0 -.03 0 1.57 0 + + 0.001 + + 0.0001 + 0.0001 + 0.0001 + 0 + 0 + 0 + + + + 30.0 + + 1.3962634 + + 320 + 240 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + + + + chassis + bottom_camera + + + diff --git a/conf/simulator/gazebo/models/simple_x_quad/model.config b/conf/simulator/gazebo/models/simple_x_quad/model.config new file mode 100644 index 0000000000..f981ebe08b --- /dev/null +++ b/conf/simulator/gazebo/models/simple_x_quad/model.config @@ -0,0 +1,15 @@ + + + Simple quad x(Paparazzi) + 1.0 + simple_x_quad.sdf + + + Kirk Scheper + kirkscheper@users.noreply.github.com + + + + Simple quadrotor model in a cross coniguration for use with Paparazzi's NPS (http://wiki.paparazziuav.org). + + diff --git a/conf/simulator/gazebo/models/simple_x_quad/simple_x_quad.sdf b/conf/simulator/gazebo/models/simple_x_quad/simple_x_quad.sdf new file mode 100644 index 0000000000..019ce6ca81 --- /dev/null +++ b/conf/simulator/gazebo/models/simple_x_quad/simple_x_quad.sdf @@ -0,0 +1,249 @@ + + + + 0 0 .1 0 0 0 + + + + 0.001 + + + + + 0.4 + + 0.00678 + 0.00678 + 0.01356 + 0. + 0. + 0. + + + + + + + 0.4 0.4 0.05 + + + + + + + + 0.2 0.2 0.05 + + + + + + + collision + + + + + + + 0.12 0.12 0 0 0 0 + + 0.01 + + 0.0001 + 0.0001 + 0.0001 + 0 + 0 + 0 + + + + + + 0.10 + 0.07 + + + + + + + chassis + nw_motor + + + + -0.12 -0.12 0 0 0 0 + + 0.01 + + 0.0001 + 0.0001 + 0.0001 + 0 + 0 + 0 + + + + + + 0.10 + 0.07 + + + + + + + chassis + se_motor + + + + 0.12 -0.12 0 0 0 0 + + 0.01 + + 0.0001 + 0.0001 + 0.0001 + 0 + 0 + 0 + + + + + + 0.10 + 0.07 + + + + + + + chassis + ne_motor + + + + -0.12 0.12 0 0 0 0 + + 0.01 + + 0.0001 + 0.0001 + 0.0001 + 0 + 0 + 0 + + + + + + 0.10 + 0.07 + + + + + + + chassis + sw_motor + + + + + + + 0.15 0 0 0 0 0 + + 0.001 + + 0.0001 + 0.0001 + 0.0001 + 0 + 0 + 0 + + + + 30.0 + + 1.3962634 + + 1280 + 720 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + + + + chassis + front_camera + + + + 0 0 -.03 0 1.57 0 + + 0.001 + + 0.0001 + 0.0001 + 0.0001 + 0 + 0 + 0 + + + + 30.0 + + 1.3962634 + + 320 + 240 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + + + + chassis + bottom_camera + + + diff --git a/conf/simulator/gazebo/world/ardrone.world b/conf/simulator/gazebo/world/empty.world similarity index 84% rename from conf/simulator/gazebo/world/ardrone.world rename to conf/simulator/gazebo/world/empty.world index 4a0d782fcd..bc1383cd83 100644 --- a/conf/simulator/gazebo/world/ardrone.world +++ b/conf/simulator/gazebo/world/empty.world @@ -1,6 +1,6 @@ - + 0.001 0 @@ -25,17 +25,14 @@ EARTH_WGS84 - 51.9906 - 4.37679 - 0 + 51.9906340 + 4.3767889 + 45.110 180 model://ground_plane - - model://ardrone - - \ No newline at end of file + diff --git a/sw/airborne/arch/sim/subsystems/settings_arch.c b/sw/airborne/arch/sim/subsystems/settings_arch.c index 2ce2cfd57d..d6658e2afd 100644 --- a/sw/airborne/arch/sim/subsystems/settings_arch.c +++ b/sw/airborne/arch/sim/subsystems/settings_arch.c @@ -37,3 +37,8 @@ int32_t persistent_read(void *ptr UNUSED, uint32_t size UNUSED) { return -1; } + +int32_t persistent_clear(void) +{ + return -1; +} diff --git a/sw/airborne/subsystems/actuators/motor_mixing_types.h b/sw/airborne/subsystems/actuators/motor_mixing_types.h index 2b6468b4b5..81a3ca1b7b 100644 --- a/sw/airborne/subsystems/actuators/motor_mixing_types.h +++ b/sw/airborne/subsystems/actuators/motor_mixing_types.h @@ -26,6 +26,8 @@ #ifndef MOTOR_MIXING_TYPES_H #define MOTOR_MIXING_TYPES_H +#include "generated/airframe.h" + /* already defined common configurations*/ #define QUAD_PLUS 1 #define QUAD_X 2 diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.c b/sw/airborne/subsystems/gps/gps_sim_nps.c index 1959ca41f8..280ad16202 100644 --- a/sw/airborne/subsystems/gps/gps_sim_nps.c +++ b/sw/airborne/subsystems/gps/gps_sim_nps.c @@ -66,8 +66,13 @@ void gps_feed_value(void) gps_nps.gspeed = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y) * 100; gps_nps.speed_3d = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y + ned_vel_d.z * ned_vel_d.z) * 100; +#if PRIMARY_GPS == GPS_DATALINK + /* vehicle heading in radians * 1e7 */ + gps_nps.course = fdm.ltp_to_body_eulers.psi * 1e7; +#else /* ground course in radians * 1e7 */ gps_nps.course = atan2(ned_vel_d.y, ned_vel_d.x) * 1e7; +#endif SetBit(gps_nps.valid_fields, GPS_VALID_COURSE_BIT); if (gps_has_fix) { diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c index e6822182f0..f02a2d0175 100644 --- a/sw/simulator/nps/nps_autopilot_fixedwing.c +++ b/sw/simulator/nps/nps_autopilot_fixedwing.c @@ -146,13 +146,11 @@ 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 f8de57863d..01161c60a0 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 && !defined(NPS_NO_GPS) +#if USE_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 73009b6630..4d5b75601e 100644 --- a/sw/simulator/nps/nps_fdm_gazebo.cpp +++ b/sw/simulator/nps/nps_fdm_gazebo.cpp @@ -32,13 +32,6 @@ #pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#ifdef NPS_DEBUG_VIDEO -// Opencv tools -#include -#include -#include -#endif - #include #include #include @@ -47,27 +40,39 @@ #include #include +#include #include #include #include +#include extern "C" { #include "nps_fdm.h" +#include "nps_autopilot.h" #include "generated/airframe.h" #include "autopilot.h" #include "math/pprz_isa.h" #include "math/pprz_algebra_double.h" + +#include "subsystems/actuators/motor_mixing_types.h" } +#if defined(NPS_DEBUG_VIDEO) || defined(NPS_DEBUG_STEREOCAM) +// Opencv tools +#include +#include +#include +#endif + using namespace std; #ifndef NPS_GAZEBO_WORLD -#define NPS_GAZEBO_WORLD "ardrone.world" +#define NPS_GAZEBO_WORLD "empty.world" #endif #ifndef NPS_GAZEBO_AC_NAME -#define NPS_GAZEBO_AC_NAME "paparazzi_uav" +#define NPS_GAZEBO_AC_NAME "ardrone" #endif // Add video handling functions if req'd. @@ -92,6 +97,15 @@ static struct gazebocam_t gazebo_cams[VIDEO_THREAD_MAX_CAMERAS] = { { NULL, 0 } }; #endif +struct gazebo_actuators_t +{ + string names[NPS_COMMANDS_NB]; + double thrusts[NPS_COMMANDS_NB]; + double torques[NPS_COMMANDS_NB]; +}; + +struct gazebo_actuators_t gazebo_actuators = {NPS_ACTUATOR_NAMES, {NPS_ACTUATOR_THRUSTS}, {NPS_ACTUATOR_THRUSTS}}; + /// Holds all necessary NPS FDM state information struct NpsFdm fdm; @@ -277,8 +291,17 @@ static void init_gazebo(void) gazebodir + "models/"); cout << "Load world: " << gazebodir + "world/" + NPS_GAZEBO_WORLD << endl; - gazebo::physics::WorldPtr world = gazebo::loadWorld( - gazebodir + "world/" + NPS_GAZEBO_WORLD); + + sdf::SDFPtr world_sdf(new sdf::SDF()); + sdf::init(world_sdf); + sdf::readFile(gazebodir + "world/" + NPS_GAZEBO_WORLD, world_sdf); + + world_sdf->Root()->GetFirstElement()->AddElement("include")->GetElement("uri")->Set((string)"model://" + NPS_GAZEBO_AC_NAME); + world_sdf->Write(pprz_home + "/var/gazebo.world"); + + // TODO add sensors to the gazebo model in the same way as the vehicle. + + gazebo::physics::WorldPtr world = gazebo::loadWorld(pprz_home + "/var/gazebo.world"); if (!world) { cout << "Failed to open world, exiting." << endl; std::exit(-1); @@ -303,6 +326,15 @@ static void init_gazebo(void) ct = static_pointer_cast < gazebo::sensors::ContactSensor > (mgr->GetSensor("contactsensor")); ct->SetActive(true); + // Overwrite motor directions as defined by motor_mixing +#ifdef MOTOR_MIXING_YAW_COEF + const double yaw_coef[] = MOTOR_MIXING_YAW_COEF; + + for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) + { + gazebo_actuators.torques[i] = -fabs(gazebo_actuators.torques[i]) * yaw_coef[i] / fabs(yaw_coef[i]); + } +#endif cout << "Gazebo initialized successfully!" << endl; } @@ -428,7 +460,7 @@ static void gazebo_read(void) #else // Gazebo versions < 8 do not have atmosphere or wind support // Use placeholder values. Valid for low altitude, low speed flights. - fdm.wind = {.x = 0, .y = 0, .z = 0}; + fdm.wind = (struct DoubleVect3){0, 0, 0}; fdm.pressure_sl = 101325; // Pa fdm.airspeed = 0; @@ -467,14 +499,11 @@ static void gazebo_read(void) */ 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 }; - + // TODO simulte actuator dynamics so indi can work for (int i = 0; i < commands_nb; ++i) { - 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]); + double thrust = autopilot.motors_on ? gazebo_actuators.thrusts[i] * act_commands[i] : 0.0; + double torque = autopilot.motors_on ? gazebo_actuators.torques[i] * act_commands[i] : 0.0; + gazebo::physics::LinkPtr link = model->GetLink(gazebo_actuators.names[i]); link->AddRelativeForce(gazebo::math::Vector3(0, 0, thrust)); link->AddRelativeTorque(gazebo::math::Vector3(0, 0, torque)); } diff --git a/sw/simulator/nps/nps_sensor_gps.c b/sw/simulator/nps/nps_sensor_gps.c index 8a2ed0e6f8..9465c09df4 100644 --- a/sw/simulator/nps/nps_sensor_gps.c +++ b/sw/simulator/nps/nps_sensor_gps.c @@ -89,7 +89,8 @@ void nps_sensor_gps_run_step(struct NpsSensorGps *gps, double time) UpdateSensorLatency_Single(time, &cur_hmsl_reading, &gps->hmsl_history, gps->pos_latency, &gps->hmsl); gps->next_update += NPS_GPS_DT; +#ifndef NPS_NO_GPS gps->data_available = TRUE; - +#endif }