Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

[nps] load aircraft into gazebo at run time + actuator handling (#2135)

Browse Source 
* load aircraft into gazebo at run time + actuator handling

* Remove environment from airframe
This commit is contained in:
Kirk Scheper
2017-10-26 14:36:38 +02:00
committed by GitHub
parent 66821d37ac
commit 3bf4b8731d
18 changed files with 750 additions and 191 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/KS/kirk_conf.xml
+1 -1
View File
@@ -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"
/>
<aircraft
conf/airframes/KS/ks_bebop2_stereo.xml
+40 -37
View File
@@ -3,7 +3,13 @@
<airframe name="bebop2_opticflow">
<firmware name="rotorcraft">
<target name="ap" board="bebop2"/>
<target name="ap" board="bebop2">
<module name="stabilization" type="indi_simple"/>
</target>
<target name="nps" board="pc">
<module name="fdm" type="gazebo"/>
<module name="stabilization" type="int_quat"/>
</target>
<module name="telemetry" type="transparent_udp"/>
<module name="radio_control" type="datalink"/>
@@ -11,19 +17,15 @@
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="datalink"/>
<module name="stabilization" type="indi_simple"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="FALSE"/>
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
<define name="AHRS_HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
</module>
<module name="ins" type="extended"/>
<module name="ins" type="hff_extended"/>
<module name="stereocam">
<configure name="STEREO_UART" value="UART2"/>
<define name="STEREO_BODY_TO_STEREO_PHI" value="90."/>
<define name="STEREO_BODY_TO_STEREO_THETA" value="0."/>
<define name="STEREO_BODY_TO_STEREO_PSI" value="90."/>
</module>
</firmware>
@@ -57,11 +59,11 @@
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
</command_laws>
<section name="AIR_DATA" prefix="AIR_DATA_">
<define name="CALC_AIRSPEED" value="FALSE"/>
<define name="CALC_TAS_FACTOR" value="FALSE"/>
<define name="CALC_AMSL_BARO" value="FALSE"/>
<section name="stereocam" prefix="STEREO_">
<define name="BODY_TO_STEREO_PHI" value="90." unit="deg"/>
<define name="BODY_TO_STEREO_THETA" value="0." unit="deg"/>
<define name="BODY_TO_STEREO_PSI" value="90." unit="deg"/>
</section>
<!-- Magnetometer still needs to be calibrated -->
@@ -78,8 +80,7 @@
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
<section name="AHRS" prefix="AHRS_">
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
<define name="HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
<define name="GRAVITY_HEURISTIC_FACTOR" value="0"/>
<!--define name="GRAVITY_HEURISTIC_FACTOR" value="0"/-->
<!-- Delft -->
<define name="H_X" value="0.3892503"/>
<define name="H_Y" value="0.0017972"/>
@@ -91,10 +92,6 @@
<!-- trust GPS a lot: -->
<define name="VFF_R_GPS" value="0.01"/>
<define name="VFF_VZ_R_GPS" value="0.01"/>
<define name="SONAR_MIN_RANGE" value="0.0"/>
<define name="SONAR_MAX_RANGE" value="2.2"/>
<define name="SONAR_UPDATE_ON_AGL" value="FALSE"/>
</section>
@@ -124,6 +121,24 @@
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="600." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="850"/>
<define name="PHI_DGAIN" value="425"/>
<define name="PHI_IGAIN" value="0"/>
<define name="THETA_PGAIN" value="850"/>
<define name="THETA_DGAIN" value="425"/>
<define name="THETA_IGAIN" value="0"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="700"/>
<define name="PSI_IGAIN" value="0"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="0"/>
<define name="THETA_DDGAIN" value="0"/>
<define name="PSI_DDGAIN" value="100"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
@@ -149,14 +164,7 @@
<define name="REF_RATE_R" value="28.0"/>
<!-- second order filter parameters -->
<!-- old?
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.7"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.7"/>
-->
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF" value="3.2"/>
<define name="FILT_CUTOFF_R" value="3.2"/>
<!-- first order actuator dynamics -->
@@ -175,12 +183,6 @@
<define name="HOVER_KI" value="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.52"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
<!-- old?
<define name="ADAPT_INITIAL_HOVER_THROTTLE" value="0.55"/>
<define name="ADAPT_MIN_HOVER_THROTTLE" value="0.48"/>
<define name="ADAPT_MAX_HOVER_THROTTLE" value="0.62"/>
<define name="ADAPT_NOISE_FACTOR" value="0.8"/>
-->
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
@@ -200,6 +202,13 @@
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="GAZEBO_AC_NAME" value="simple_x_quad" type="string"/>
<define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="double[]"/>
<define name="ACTUATOR_TORQUES" value="0.155, 0.155, 0.155, 0.155" type="double[]"/>
<!--define name="DEBUG_STEREOCAM" value="1"/-->
<!--define name="NO_GPS" value="1"/-->
</section>
<section name="AUTOPILOT">
@@ -208,12 +217,6 @@
<define name="MODE_AUTO1" value="AP_MODE_MODULE"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<!--
<define name="MODE_STARTUP" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_MANUAL" value="AP_MODE_MODULE"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_ATTITUDE_DIRECT"/>
-->
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
</section>
conf/airframes/TUDELFT/tudelft_conf.xml
+2 -2
View File
@@ -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"
/>
<aircraft
@@ -360,7 +360,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="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/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/radio_control_superbitrf_rc.xml modules/gps_ubx_ucenter.xml"
gui_color="blue"
/>
<aircraft
conf/airframes/TUDELFT/tudelft_ladybird_lisa_mxs.xml
+79 -89
View File
@@ -1,6 +1,5 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!--
The motor and rotor configuration is the following:
@@ -24,7 +23,48 @@
<airframe name="quadrotor_lisa_mxs">
<description>LadyBird quadrotor frame equiped with Lisa/MXS 1.0 with four brushed motors in an X configuration.
</description>
<firmware name="rotorcraft">
<target name="ap" board="lisa_mxs_1.0">
<define name="REMAP_UART3" value="TRUE" />
<define name="USE_PERSISTENT_SETTINGS" value="TRUE"/>
</target>
<target name="nps" board="pc">
<module name="fdm" type="gazebo"/>
</target>
<module name="motor_mixing"/>
<module name="actuators" type="pwm">
<!-- This airframe is using the servo PWM outputs directly to drive mosfets, which in turn directly drive brushed motors.
With the following settings we will be driving the motors with a 36KHz signal that should be well above anyone's hearing range.
If we went with the 500Hz that woul be the default we would be hearing a very annoying high pitched sound coming from the motors.
-->
<!-- Setting the PWM timer base frequency to 36MHz -->
<define name="PWM_BASE_FREQ" value="36000000"/>
<!-- Setting the PWM interval to 36KHz -->
<define name="SERVO_HZ" value="36000"/>
</module>
<module name="telemetry" type="transparent">
<configure name="MODEM_PORT" value="UART3"/>
<configure name="MODEM_BAUD" value="B115200"/>
</module>
<module name="radio_control" type="datalink"/>
<module name="imu" type="lisa_mx_v2.1"/>
<module name="gps" type="ublox">
<configure name="GPS_PORT" value="UART2"/>
<configure name="GPS_BAUD" value="B38400"/>
</module>
<module name="gps" type="ubx_ucenter"/>
<module name="stabilization" type="int_quat"/>
<module name="ahrs" type="int_cmpl_quat"/>
<module name="ins"/>
<module name="air_data">
<define name="AIR_DATA_CALC_AMSL_BARO" value="TRUE"/>
</module>
<module name="send_imu_mag_current"/>
</firmware>
<servos driver="Pwm">
<servo name="NE" no="0" min="0" neutral="50" max="1000"/>
<servo name="SE" no="5" min="0" neutral="50" max="1000"/>
@@ -39,25 +79,20 @@
<axis name="THRUST" failsafe_value="0"/>
</commands>
<command_laws>
<call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
<set servo="NE" value="motor_mixing.commands[0]"/>
<set servo="SE" value="motor_mixing.commands[1]"/>
<set servo="SW" value="motor_mixing.commands[2]"/>
<set servo="NW" value="motor_mixing.commands[3]"/>
</command_laws>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>
<define name="NB_MOTOR" value="4"/>
<define name="SCALE" value="256"/>
<define name="ROLL_COEF" value="{ -256, -256, 256, 256 }"/>
<define name="PITCH_COEF" value="{ 256, -256, -256, 256 }"/>
<define name="YAW_COEF" value="{ 256, -256, 256, -256 }"/>
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
<define name="TYPE" value="QUAD_X"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
<set servo="NE" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
<set servo="SE" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
<set servo="SW" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
<set servo="NW" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
</command_laws>
<section name="IMU" prefix="IMU_">
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
@@ -82,24 +117,7 @@
<define name="MAG_Z_CURRENT_COEF" value="-0.01935617335"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_HOVER_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="14000"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="3.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="3.3" unit="V"/>
<define name="LOW_BAT_LEVEL" value="3.5" unit="V"/>
<define name="MAX_BAT_LEVEL" value="4.1" unit="V"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45." unit="deg"/>
<define name="SP_MAX_THETA" value="45." unit="deg"/>
@@ -162,69 +180,41 @@
<define name="H_X" value=" 0.47577"/>
<define name="H_Y" value=" 0.11811"/>
<define name="H_Z" value=" 0.87161"/>
<define name="PROPAGATE_FREQUENCY" value="500"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="39"/>
<define name="DGAIN" value="50"/>
<define name="IGAIN" value="19"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="39"/>
<define name="DGAIN" value="50"/>
<define name="IGAIN" value="19"/>
</section>
<section name="MISC">
<define name="VoltageOfAdc(adc)" value="(adc)*0.00162f" />
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="GAZEBO_AC_NAME" value="simple_x_quad" type="string"/>
<define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="double[]"/>
<define name="ACTUATOR_TORQUES" value="0.155, 0.155, 0.155, 0.155" type="double[]"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_HOVER_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>
<modules main_freq="512">
<module name="gps" type="ubx_ucenter"/>
<module name="send_imu_mag_current"/>
</modules>
<firmware name="rotorcraft">
<target name="ap" board="lisa_mxs_1.0">
<define name="REMAP_UART3" value="TRUE" />
<module name="radio_control" type="datalink"/>
<!--module name="radio_control" type="spektrum">
<configure name="RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT" value="UART5" />
</module-->
<configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
<define name="USE_PERSISTENT_SETTINGS" value="TRUE"/>
</target>
<module name="motor_mixing"/>
<module name="actuators" type="pwm">
<!-- This airframe is using the servo PWM outputs directly to drive mosfets, which in turn directly drive brushed motors.
With the following settings we will be driving the motors with a 36KHz signal that should be well above anyone's hearing range.
If we went with the 500Hz that woul be the default we would be hearing a very annoying high pitched sound coming from the motors.
-->
<!-- Setting the PWM timer base frequency to 36MHz -->
<define name="PWM_BASE_FREQ" value="36000000"/>
<!-- Setting the PWM interval to 36KHz -->
<define name="SERVO_HZ" value="36000"/>
</module>
<module name="telemetry" type="transparent">
<configure name="MODEM_PORT" value="UART3"/>
<configure name="MODEM_BAUD" value="B115200"/>
</module>
<module name="imu" type="lisa_mx_v2.1"/>
<module name="gps" type="ublox">
<configure name="GPS_PORT" value="UART2"/>
<configure name="GPS_BAUD" value="B38400"/>
</module>
<module name="stabilization" type="int_quat"/>
<module name="ahrs" type="int_cmpl_quat"/>
<module name="ins"/>
<module name="air_data">
<define name="AIR_DATA_CALC_AMSL_BARO" value="TRUE"/>
</module>
</firmware>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="14000"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="3.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="3.3" unit="V"/>
<define name="LOW_BAT_LEVEL" value="3.5" unit="V"/>
<define name="MAX_BAT_LEVEL" value="4.1" unit="V"/>
</section>
</airframe>
conf/modules/video_thread.xml
+2
View File
@@ -43,5 +43,7 @@
<file name="image.c" dir="modules/computer_vision/lib/vision"/>
<file name="jpeg.c" dir="modules/computer_vision/lib/encoding"/>
<flag name="LDFLAGS" value="lpthread"/>
<define name="NPS_SIMULATE_VIDEO" value="1"/>
</makefile>
</module>
conf/simulator/gazebo/models/ardrone/ardrone.sdf
+30 -31
View File
@@ -1,6 +1,6 @@
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="paparazzi_uav">
<model name="ardrone">
<pose>0 0 .1 0 0 0</pose>
<link name="chassis">
@@ -9,12 +9,12 @@
</velocity_decay>
<inertial><!-- Taken from Paparazzi's ARDrone model for JSBsim -->
<inertial><!-- Converted to SI from Paparazzi's ARDrone model for JSBsim -->
<mass>0.4</mass>
<inertia>
<ixx> 0.005 </ixx>
<iyy> 0.005 </iyy>
<izz> 0.010 </izz>
<ixx> 0.00678 </ixx>
<iyy> 0.00678 </iyy>
<izz> 0.01356 </izz>
<ixy> 0. </ixy>
<ixz> 0. </ixz>
<iyz> 0. </iyz>
@@ -24,7 +24,7 @@
<collision name="collision">
<geometry>
<box>
<size>.4 .4 .05</size>
<size>0.4 0.4 0.05</size>
</box>
</geometry>
</collision>
@@ -32,7 +32,7 @@
<visual name="visual">
<geometry>
<box>
<size>.2 .2 .05</size>
<size>0.2 0.2 0.05</size>
</box>
</geometry>
</visual>
@@ -44,16 +44,15 @@
</sensor>
</link>
<!-- MOTORS -->
<!-- MOTORS -->
<link name="nw_motor">
<pose>0.12 0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>.0001</ixx>
<iyy>.0001</iyy>
<izz>.0001</izz>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
@@ -63,7 +62,7 @@
<geometry>
<cylinder>
<radius>0.10</radius>
<length>.07</length>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
@@ -79,9 +78,9 @@
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>.0001</ixx>
<iyy>.0001</iyy>
<izz>.0001</izz>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
@@ -91,7 +90,7 @@
<geometry>
<cylinder>
<radius>0.10</radius>
<length>.07</length>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
@@ -107,9 +106,9 @@
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>.0001</ixx>
<iyy>.0001</iyy>
<izz>.0001</izz>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
@@ -119,7 +118,7 @@
<geometry>
<cylinder>
<radius>0.10</radius>
<length>.07</length>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
@@ -135,9 +134,9 @@
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>.0001</ixx>
<iyy>.0001</iyy>
<izz>.0001</izz>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
@@ -147,7 +146,7 @@
<geometry>
<cylinder>
<radius>0.10</radius>
<length>.07</length>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
@@ -166,9 +165,9 @@
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>.0001</ixx>
<iyy>.0001</iyy>
<izz>.0001</izz>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
@@ -209,9 +208,9 @@
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>.0001</ixx>
<iyy>.0001</iyy>
<izz>.0001</izz>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
conf/simulator/gazebo/models/simple_quad/model.config
+15
View File
@@ -0,0 +1,15 @@
<?xml version="1.0"?>
<model>
<name>Simple quad (Paparazzi)</name>
<version>1.0</version>
<sdf version='1.4'>simple_quad.sdf</sdf>
<author>
<name>Kirk Scheper</name>
<email>kirkscheper@users.noreply.github.com</email>
</author>
<description>
Simple quadrotor model in a plus coniguration for use with Paparazzi's NPS (http://wiki.paparazziuav.org).
</description>
</model>
conf/simulator/gazebo/models/simple_quad/simple_quad.sdf
+249
View File
@@ -0,0 +1,249 @@
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="simple_quad">
<pose>0 0 .1 0 0 0</pose>
<link name="chassis">
<velocity_decay>
<linear>0.001</linear>
</velocity_decay>
<inertial><!-- Converted to SI from Paparazzi's ARDrone model for JSBsim -->
<mass>0.4</mass>
<inertia>
<ixx> 0.00678 </ixx>
<iyy> 0.00678 </iyy>
<izz> 0.01356 </izz>
<ixy> 0. </ixy>
<ixz> 0. </ixz>
<iyz> 0. </iyz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.4 0.4 0.05</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.2 0.2 0.05</size>
</box>
</geometry>
</visual>
<sensor name="contactsensor" type="contact">
<contact>
<collision>collision</collision>
</contact>
</sensor>
</link>
<!-- MOTORS -->
<link name="front_motor">
<pose>0.12 0 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="front_motor_joint">
<parent>chassis</parent>
<child>front_motor</child>
</joint>
<link name="back_motor">
<pose>-0.12 0 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="back_motor_joint">
<parent>chassis</parent>
<child>back_motor</child>
</joint>
<link name="right_motor">
<pose>0 -0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="right_motor_joint">
<parent>chassis</parent>
<child>right_motor</child>
</joint>
<link name="left_motor">
<pose>0 0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="left_motor_joint">
<parent>chassis</parent>
<child>left_motor</child>
</joint>
<!-- CAMERAS -->
<link name="front_camera">
<pose>0.15 0 0 0 0 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="camera" name="front_camera">
<update_rate>30.0</update_rate>
<camera name="front_camera">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="front_camera_joint" type="fixed">
<parent>chassis</parent>
<child>front_camera</child>
</joint>
<link name="bottom_camera">
<pose>0 0 -.03 0 1.57 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="camera" name="bottom_camera">
<update_rate>30.0</update_rate>
<camera name="bottom_camera">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="bottom_camera_joint" type="fixed">
<parent>chassis</parent>
<child>bottom_camera</child>
</joint>
</model>
</sdf>
conf/simulator/gazebo/models/simple_x_quad/model.config
+15
View File
@@ -0,0 +1,15 @@
<?xml version="1.0"?>
<model>
<name>Simple quad x(Paparazzi)</name>
<version>1.0</version>
<sdf version='1.4'>simple_x_quad.sdf</sdf>
<author>
<name>Kirk Scheper</name>
<email>kirkscheper@users.noreply.github.com</email>
</author>
<description>
Simple quadrotor model in a cross coniguration for use with Paparazzi's NPS (http://wiki.paparazziuav.org).
</description>
</model>
conf/simulator/gazebo/models/simple_x_quad/simple_x_quad.sdf
+249
View File
@@ -0,0 +1,249 @@
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="simple_x_quad">
<pose>0 0 .1 0 0 0</pose>
<link name="chassis">
<velocity_decay>
<linear>0.001</linear>
</velocity_decay>
<inertial><!-- Converted to SI from Paparazzi's ARDrone model for JSBsim -->
<mass>0.4</mass>
<inertia>
<ixx> 0.00678 </ixx>
<iyy> 0.00678 </iyy>
<izz> 0.01356 </izz>
<ixy> 0. </ixy>
<ixz> 0. </ixz>
<iyz> 0. </iyz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.4 0.4 0.05</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.2 0.2 0.05</size>
</box>
</geometry>
</visual>
<sensor name="contactsensor" type="contact">
<contact>
<collision>collision</collision>
</contact>
</sensor>
</link>
<!-- MOTORS -->
<link name="nw_motor">
<pose>0.12 0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="nw_motor_joint">
<parent>chassis</parent>
<child>nw_motor</child>
</joint>
<link name="se_motor">
<pose>-0.12 -0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="se_motor_joint">
<parent>chassis</parent>
<child>se_motor</child>
</joint>
<link name="ne_motor">
<pose>0.12 -0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="ne_motor_joint">
<parent>chassis</parent>
<child>ne_motor</child>
</joint>
<link name="sw_motor">
<pose>-0.12 0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="sw_motor_joint">
<parent>chassis</parent>
<child>sw_motor</child>
</joint>
<!-- CAMERAS -->
<link name="front_camera">
<pose>0.15 0 0 0 0 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="camera" name="front_camera">
<update_rate>30.0</update_rate>
<camera name="front_camera">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="front_camera_joint" type="fixed">
<parent>chassis</parent>
<child>front_camera</child>
</joint>
<link name="bottom_camera">
<pose>0 0 -.03 0 1.57 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="camera" name="bottom_camera">
<update_rate>30.0</update_rate>
<camera name="bottom_camera">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="bottom_camera_joint" type="fixed">
<parent>chassis</parent>
<child>bottom_camera</child>
</joint>
</model>
</sdf>
conf/simulator/gazebo/world/ardrone.world → conf/simulator/gazebo/world/empty.world
+5 -8
View File
@@ -1,6 +1,6 @@
<?xml version='1.0'?>
<sdf version="1.6">
<world name="default">
<world name="empty">
<physics type="ode">
<max_step_size>0.001</max_step_size>
<real_time_update_rate>0</real_time_update_rate><!-- Handled by Paparazzi! -->
@@ -25,17 +25,14 @@
</light>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>51.9906</latitude_deg>
<longitude_deg>4.37679</longitude_deg>
<elevation>0</elevation>
<latitude_deg>51.9906340</latitude_deg>
<longitude_deg>4.3767889</longitude_deg>
<elevation>45.110</elevation>
<heading_deg>180</heading_deg><!-- Temporary fix for issue https://bitbucket.org/osrf/gazebo/issues/2022/default-sphericalcoordinates-frame-should -->
</spherical_coordinates>
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://ardrone</uri>
</include>
</world>
</sdf>
</sdf>
sw/airborne/arch/sim/subsystems/settings_arch.c
+5
View File
@@ -37,3 +37,8 @@ int32_t persistent_read(void *ptr UNUSED, uint32_t size UNUSED)
{
return -1;
}
int32_t persistent_clear(void)
{
return -1;
}
sw/airborne/subsystems/actuators/motor_mixing_types.h
+2
View File
@@ -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
sw/airborne/subsystems/gps/gps_sim_nps.c
+5
View File
@@ -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) {
sw/simulator/nps/nps_autopilot_fixedwing.c
-2
View File
@@ -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()) {
sw/simulator/nps/nps_autopilot_rotorcraft.c
+1 -1
View File
@@ -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();
sw/simulator/nps/nps_fdm_gazebo.cpp
+48 -19
View File
@@ -32,13 +32,6 @@
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#ifdef NPS_DEBUG_VIDEO
// Opencv tools
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#endif
#include <cstdio>
#include <cstdlib>
#include <string>
@@ -47,27 +40,39 @@
#include <gazebo/gazebo.hh>
#include <gazebo/common/common.hh>
#include <gazebo/math/gzmath.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/sensors/sensors.hh>
#include <gazebo/gazebo_config.h>
#include <sdf/sdf.hh>
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 <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#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));
}
sw/simulator/nps/nps_sensor_gps.c
+2 -1
View File
@@ -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
}