mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-29 19:17:28 +08:00
Add PyBullet FDM for nps. (#3024)
This commit is contained in:
Fabien-B
GitHub
@@ -10,28 +10,6 @@
|
|||||||
settings_modules="modules/ahrs_float_dcm.xml modules/gps.xml modules/guidance_basic_fw.xml modules/imu_common.xml modules/nav_basic_fw.xml modules/power_switch.xml modules/stabilization_attitude_fw.xml"
|
settings_modules="modules/ahrs_float_dcm.xml modules/gps.xml modules/guidance_basic_fw.xml modules/imu_common.xml modules/nav_basic_fw.xml modules/power_switch.xml modules/stabilization_attitude_fw.xml"
|
||||||
gui_color="blue"
|
gui_color="blue"
|
||||||
/>
|
/>
|
||||||
<aircraft
|
|
||||||
name="ARD2_101"
|
|
||||||
ac_id="101"
|
|
||||||
airframe="airframes/ENAC/quadrotor/ard2_basic.xml"
|
|
||||||
radio="radios/cockpitSX.xml"
|
|
||||||
telemetry="telemetry/default_rotorcraft.xml"
|
|
||||||
flight_plan="flight_plans/rotorcraft_basic_safety.xml"
|
|
||||||
settings="settings/rotorcraft_basic.xml"
|
|
||||||
settings_modules="modules/ahrs_float_invariant.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_quat.xml"
|
|
||||||
gui_color="red"
|
|
||||||
/>
|
|
||||||
<aircraft
|
|
||||||
name="ARD2_103"
|
|
||||||
ac_id="103"
|
|
||||||
airframe="airframes/ENAC/quadrotor/ard2_basic_adhoc.xml"
|
|
||||||
radio="radios/cockpitSX.xml"
|
|
||||||
telemetry="telemetry/default_rotorcraft.xml"
|
|
||||||
flight_plan="flight_plans/rotorcraft_basic_safety.xml"
|
|
||||||
settings="settings/rotorcraft_basic.xml"
|
|
||||||
settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_float_invariant.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_quat.xml"
|
|
||||||
gui_color="yellow"
|
|
||||||
/>
|
|
||||||
<aircraft
|
<aircraft
|
||||||
name="CHIMERA"
|
name="CHIMERA"
|
||||||
ac_id="23"
|
ac_id="23"
|
||||||
@@ -73,7 +51,7 @@
|
|||||||
telemetry="telemetry/default_fixedwing_imu.xml"
|
telemetry="telemetry/default_fixedwing_imu.xml"
|
||||||
flight_plan="flight_plans/basic.xml"
|
flight_plan="flight_plans/basic.xml"
|
||||||
settings="settings/fixedwing_basic.xml [settings/nps.xml]"
|
settings="settings/fixedwing_basic.xml [settings/nps.xml]"
|
||||||
settings_modules="modules/ahrs_float_dcm.xml modules/airspeed_adc.xml modules/gps.xml [modules/gps_ubx_ucenter.xml] modules/guidance_full_pid_fw.xml modules/imu_common.xml modules/ins_float_invariant.xml modules/nav_basic_fw.xml modules/power_switch.xml modules/stabilization_adaptive_fw.xml"
|
settings_modules="modules/ahrs_float_dcm.xml modules/airspeed_adc.xml modules/gps.xml modules/gps_ublox.xml [modules/gps_ubx_ucenter.xml] modules/guidance_full_pid_fw.xml modules/imu_common.xml modules/ins_float_invariant.xml modules/nav_basic_fw.xml modules/power_switch.xml modules/stabilization_adaptive_fw.xml"
|
||||||
gui_color="#ffff7d7d0000"
|
gui_color="#ffff7d7d0000"
|
||||||
/>
|
/>
|
||||||
<aircraft
|
<aircraft
|
||||||
@@ -131,4 +109,37 @@
|
|||||||
settings_modules="modules/ahrs_float_cmpl_quat.xml modules/ahrs_float_dcm.xml modules/air_data.xml modules/gps.xml modules/guidance_full_pid_fw.xml modules/imu_common.xml modules/ins_float_invariant.xml modules/nav_basic_fw.xml modules/stabilization_adaptive_fw.xml"
|
settings_modules="modules/ahrs_float_cmpl_quat.xml modules/ahrs_float_dcm.xml modules/air_data.xml modules/gps.xml modules/guidance_full_pid_fw.xml modules/imu_common.xml modules/ins_float_invariant.xml modules/nav_basic_fw.xml modules/stabilization_adaptive_fw.xml"
|
||||||
gui_color="blue"
|
gui_color="blue"
|
||||||
/>
|
/>
|
||||||
|
<aircraft
|
||||||
|
name="RoBoBee"
|
||||||
|
ac_id="2"
|
||||||
|
airframe="airframes/ENAC/quadrotor/robobee.xml"
|
||||||
|
radio="radios/FrSky_X-Lite.xml"
|
||||||
|
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/air_data.xml modules/filter_1euro_imu.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/jevois.xml modules/logger_sd_chibios.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml modules/tag_tracking.xml"
|
||||||
|
gui_color="#3ce8d771db45"
|
||||||
|
/>
|
||||||
|
<aircraft
|
||||||
|
name="ANTON"
|
||||||
|
ac_id="217"
|
||||||
|
airframe="airframes/ENAC/quadrotor/anton_indi_aruco.xml"
|
||||||
|
radio="radios/FrSky_X-Lite.xml"
|
||||||
|
telemetry="telemetry/rotorcraft_imav2022.xml"
|
||||||
|
flight_plan="flight_plans/competitions/IMAV2022_drop.xml"
|
||||||
|
settings="settings/rotorcraft_basic.xml"
|
||||||
|
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/filter_1euro_imu.xml modules/gps.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/jevois.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_poly_rotorcraft.xml modules/stabilization_indi.xml modules/switch_servo.xml modules/tag_tracking.xml"
|
||||||
|
gui_color="#fdd12f992f99"
|
||||||
|
/>
|
||||||
|
<aircraft
|
||||||
|
name="ULYSSE"
|
||||||
|
ac_id="218"
|
||||||
|
airframe="airframes/ENAC/quadrotor/ulysse_indi.xml"
|
||||||
|
radio="radios/FrSky_X-Lite.xml"
|
||||||
|
telemetry="telemetry/rotorcraft_imav2022.xml"
|
||||||
|
flight_plan="flight_plans/competitions/IMAV2022_drop.xml"
|
||||||
|
settings="settings/rotorcraft_basic.xml"
|
||||||
|
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/gps.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/jevois.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_poly_rotorcraft.xml modules/stabilization_indi.xml modules/switch_servo.xml modules/tag_tracking.xml"
|
||||||
|
gui_color="#c6d33e3ef958"
|
||||||
|
/>
|
||||||
</conf>
|
</conf>
|
||||||
|
|||||||
@@ -0,0 +1,272 @@
|
|||||||
|
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
|
||||||
|
|
||||||
|
<airframe name="Quadricopter ANTON Tawaki">
|
||||||
|
|
||||||
|
<description>
|
||||||
|
* Autopilot: Tawaki
|
||||||
|
* Actuators: 4 in 4 Holybro BLHELI ESC
|
||||||
|
* Telemetry: XBee
|
||||||
|
* GPS: ublox
|
||||||
|
* RC: FrSky XM+
|
||||||
|
</description>
|
||||||
|
|
||||||
|
<firmware name="rotorcraft">
|
||||||
|
<configure name="PERIODIC_FREQUENCY" value="1000"/>
|
||||||
|
<configure name="AHRS_PROPAGATE_FREQUENCY" value="$(PERIODIC_FREQUENCY)"/>
|
||||||
|
|
||||||
|
<target name="ap" board="tawaki_1.0">
|
||||||
|
<configure name="BARO_PERIODIC_FREQUENCY" value="50"/>
|
||||||
|
<module name="radio_control" type="sbus"/>
|
||||||
|
<define name="RADIO_KILL_SWITCH" value="RADIO_GAIN1"/>
|
||||||
|
<define name="LOW_NOISE_TIME" value="10"/>
|
||||||
|
</target>
|
||||||
|
|
||||||
|
<target name="nps" board="pc">
|
||||||
|
<module name="fdm" type="jsbsim"/>
|
||||||
|
<module name="radio_control" type="ppm"/>
|
||||||
|
</target>
|
||||||
|
|
||||||
|
<module name="telemetry" type="xbee_api"/>
|
||||||
|
|
||||||
|
<module name="motor_mixing"/>
|
||||||
|
|
||||||
|
<module name="actuators" type="dshot">
|
||||||
|
<!--define name="DSHOT_SPEED" value="300"/-->
|
||||||
|
</module>
|
||||||
|
|
||||||
|
<module name="board" type="tawaki">
|
||||||
|
<define name="IMU_MPU_LOWPASS_FILTER" value="MPU60X0_DLPF_256HZ"/>
|
||||||
|
<define name="IMU_MPU_ACCEL_LOWPASS_FILTER" value="MPU60X0_DLPF_ACC_218HZ"/>
|
||||||
|
<define name="IMU_MPU_SMPLRT_DIV" value="0"/>
|
||||||
|
<!--configure name="BOARD_TAWAKI_ROTATED" value="TRUE"/-->
|
||||||
|
<configure name="MAG_LIS3MDL_I2C_DEV" value="i2c2"/>
|
||||||
|
</module>
|
||||||
|
<module name="gps" type="ublox">
|
||||||
|
<configure name="GPS_BAUD" value="B115200"/>
|
||||||
|
<define name="USE_GPS_UBX_RTCM" value="TRUE"/>
|
||||||
|
</module>
|
||||||
|
|
||||||
|
<module name="stabilization" type="indi"/>
|
||||||
|
<module name="guidance" type="indi">
|
||||||
|
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
|
||||||
|
<define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.03"/>
|
||||||
|
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
|
||||||
|
</module>
|
||||||
|
|
||||||
|
<module name="ins"/>
|
||||||
|
<module name="ahrs" type="int_cmpl_quat"/>
|
||||||
|
<module name="air_data"/>
|
||||||
|
|
||||||
|
<module name="actuators" type="pwm"/>
|
||||||
|
<module name="switch" type="servo"/>
|
||||||
|
|
||||||
|
<module name="filter" type="1euro_imu">
|
||||||
|
<define name="AHRS_ICQ_IMU_ID" value="IMU_F1E_ID"/>
|
||||||
|
<define name="AHRS_ALIGNER_IMU_ID" value="IMU_F1E_ID"/>
|
||||||
|
</module>
|
||||||
|
|
||||||
|
<!--module name="sonar_adc">
|
||||||
|
<configure name="ADC_SONAR" value="ADC_1"/>
|
||||||
|
<define name="USE_SONAR"/>
|
||||||
|
</module>
|
||||||
|
<module name="agl_dist"/-->
|
||||||
|
|
||||||
|
<module name="tag_tracking">
|
||||||
|
<configure name="JEVOIS_UART" value="UART3" description="UART on which Jevois camera is connected"/>
|
||||||
|
</module>
|
||||||
|
|
||||||
|
<module name="flight_recorder"/>
|
||||||
|
<!--module name="logger" type="tune_indi"/-->
|
||||||
|
</firmware>
|
||||||
|
|
||||||
|
<servos driver="DShot">
|
||||||
|
<servo name="FR" no="3" min="0" neutral="100" max="2000"/>
|
||||||
|
<servo name="BR" no="4" min="0" neutral="100" max="2000"/>
|
||||||
|
<servo name="BL" no="2" min="0" neutral="100" max="2000"/>
|
||||||
|
<servo name="FL" no="1" min="0" neutral="100" max="2000"/>
|
||||||
|
</servos>
|
||||||
|
|
||||||
|
<servos driver="Pwm">
|
||||||
|
<servo name="SWITCH" no="1" min="1000" neutral="1000" max="2000"/>
|
||||||
|
</servos>
|
||||||
|
|
||||||
|
<commands>
|
||||||
|
<axis name="ROLL" failsafe_value="0"/>
|
||||||
|
<axis name="PITCH" failsafe_value="0"/>
|
||||||
|
<axis name="YAW" failsafe_value="0"/>
|
||||||
|
<axis name="THRUST" failsafe_value="0"/>
|
||||||
|
</commands>
|
||||||
|
|
||||||
|
<section name="MIXING" prefix="MOTOR_MIXING_">
|
||||||
|
<define name="TYPE" value="QUAD_X"/>
|
||||||
|
<define name="TRIM_ROLL" value="0"/>
|
||||||
|
<define name="TRIM_PITCH" value="0"/>
|
||||||
|
<define name="TRIM_YAW" value="0"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<command_laws>
|
||||||
|
<set servo="FR" value="autopilot_get_motors_on() ? actuators_pprz[0] : -MAX_PPRZ"/>
|
||||||
|
<set servo="BR" value="autopilot_get_motors_on() ? actuators_pprz[1] : -MAX_PPRZ"/>
|
||||||
|
<set servo="BL" value="autopilot_get_motors_on() ? actuators_pprz[2] : -MAX_PPRZ"/>
|
||||||
|
<set servo="FL" value="autopilot_get_motors_on() ? actuators_pprz[3] : -MAX_PPRZ"/>
|
||||||
|
</command_laws>
|
||||||
|
|
||||||
|
<section name="IMU" prefix="IMU_">
|
||||||
|
<define name="GYRO_P_SIGN" value="-1"/>
|
||||||
|
<define name="GYRO_Q_SIGN" value="-1"/>
|
||||||
|
<define name="GYRO_R_SIGN" value="1"/>
|
||||||
|
|
||||||
|
<define name="ACCEL_X_SIGN" value="-1"/>
|
||||||
|
<define name="ACCEL_Y_SIGN" value="-1"/>
|
||||||
|
<define name="ACCEL_Z_SIGN" value="1"/>
|
||||||
|
<define name="ACCEL_X_NEUTRAL" value="-90"/>
|
||||||
|
<define name="ACCEL_Y_NEUTRAL" value="40"/>
|
||||||
|
<define name="ACCEL_Z_NEUTRAL" value="12"/>
|
||||||
|
<define name="ACCEL_X_SENS" value="2.321864827044042" integer="16"/>
|
||||||
|
<define name="ACCEL_Y_SENS" value="2.450207588413862" integer="16"/>
|
||||||
|
<define name="ACCEL_Z_SENS" value="2.4560049628471914" integer="16"/>
|
||||||
|
|
||||||
|
<define name="MAG_X_SIGN" value="1"/>
|
||||||
|
<define name="MAG_Y_SIGN" value="1"/>
|
||||||
|
<define name="MAG_Z_SIGN" value="1"/>
|
||||||
|
<define name="MAG_X_NEUTRAL" value="-3868"/>
|
||||||
|
<define name="MAG_Y_NEUTRAL" value="2013"/>
|
||||||
|
<define name="MAG_Z_NEUTRAL" value="-101"/>
|
||||||
|
<define name="MAG_X_SENS" value="0.6497766229092939" integer="16"/>
|
||||||
|
<define name="MAG_Y_SENS" value="0.6352026516080006" integer="16"/>
|
||||||
|
<define name="MAG_Z_SENS" value="0.6627284899394246" integer="16"/>
|
||||||
|
|
||||||
|
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
|
||||||
|
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
|
||||||
|
<define name="BODY_TO_IMU_PSI" value="-45." unit="deg"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="MAG">
|
||||||
|
<define name="LIS3MDL_MAG_TO_IMU_PHI" value="0." unit="deg"/>
|
||||||
|
<define name="LIS3MDL_MAG_TO_IMU_THETA" value="0." unit="deg"/>
|
||||||
|
<define name="LIS3MDL_MAG_TO_IMU_PSI" value="90." unit="deg"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<include href="conf/mag/delft_valkenburg.xml"/>
|
||||||
|
|
||||||
|
<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"/>
|
||||||
|
<define name="SP_MAX_R" value="60." unit="deg/s"/>
|
||||||
|
<define name="DEADBAND_R" value="250"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||||
|
|
||||||
|
<!-- reference acceleration for attitude control -->
|
||||||
|
<define name="REF_ERR_P" value="101"/>
|
||||||
|
<define name="REF_ERR_Q" value="101"/>
|
||||||
|
<define name="REF_ERR_R" value="124"/>
|
||||||
|
<define name="REF_RATE_P" value="12.6"/>
|
||||||
|
<define name="REF_RATE_Q" value="14.0"/>
|
||||||
|
<define name="REF_RATE_R" value="14.0"/>
|
||||||
|
|
||||||
|
<define name="MAX_R" value="60" unit="deg/s"/>
|
||||||
|
|
||||||
|
<!-- second order filter parameters -->
|
||||||
|
<define name="FILT_CUTOFF" value="4.0"/>
|
||||||
|
<define name="FILT_CUTOFF_R" value="4.0"/>
|
||||||
|
|
||||||
|
<!-- Adaptive Learning Rate -->
|
||||||
|
<define name="USE_ADAPTIVE" value="FALSE"/>
|
||||||
|
<define name="ADAPTIVE_MU" value="0.0001"/>
|
||||||
|
|
||||||
|
<!-- Full INDI -->
|
||||||
|
<!-- control effectiveness -->
|
||||||
|
<define name="G1_ROLL" value="{-40 , -40, 40 , 40 }"/>
|
||||||
|
<define name="G1_PITCH" value="{40 , -40, -40 , 40 }"/>
|
||||||
|
<define name="G1_YAW" value="{5, -5, 5, -5}"/>
|
||||||
|
<define name="G1_THRUST" value="{-1.5, -1.5, -1.5, -1.5}"/>
|
||||||
|
<!--Counter torque effect of spinning up a rotor-->
|
||||||
|
<define name="G2" value="{150.0, -150.0, 150.0, -150.0 }"/>
|
||||||
|
<!-- first order actuator dynamics -->
|
||||||
|
<define name="ACT_DYN" value="{0.03, 0.03, 0.03, 0.03}"/>
|
||||||
|
<define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
|
||||||
|
|
||||||
|
<!--Priority for each axis (roll, pitch, yaw and thrust)-->
|
||||||
|
<define name="WLS_PRIORITIES" value="{1000, 1000, 1, 100}"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
|
||||||
|
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||||
|
<define name="REF_MIN_ZDD" value="-0.4*9.81"/>
|
||||||
|
<define name="REF_MAX_ZDD" value=" 0.4*9.81"/>
|
||||||
|
<define name="REF_MIN_ZD" value="-1.5"/>
|
||||||
|
<define name="REF_MAX_ZD" value=" 1."/>
|
||||||
|
<define name="HOVER_KP" value="87"/>
|
||||||
|
<define name="HOVER_KD" value="120"/>
|
||||||
|
<define name="HOVER_KI" value="11"/>
|
||||||
|
<define name="NOMINAL_HOVER_THROTTLE" value="0.30"/>
|
||||||
|
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
|
||||||
|
<define name="ADAPT_NOISE_FACTOR" value="1."/>
|
||||||
|
<define name="ADAPT_INITIAL_HOVER_THROTTLE" value="0.25"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||||
|
<define name="MAX_BANK" value="20" unit="deg"/>
|
||||||
|
<define name="PGAIN" value="40"/>
|
||||||
|
<define name="DGAIN" value="108"/>
|
||||||
|
<define name="IGAIN" value="20"/>
|
||||||
|
<define name="NGAIN" value="0"/>
|
||||||
|
<!-- feedforward -->
|
||||||
|
<define name="AGAIN" value="0"/>
|
||||||
|
<define name="REF_MAX_SPEED" value="2.5"/>
|
||||||
|
<define name="REF_MAX_ACCEL" value="2.5"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="NAV">
|
||||||
|
<define name="ARRIVED_AT_WAYPOINT" value="2.0"/>
|
||||||
|
<define name="NAV_CLIMB_VSPEED" value="1.0"/>
|
||||||
|
<define name="NAV_DESCEND_VSPEED" value="-0.5"/>
|
||||||
|
<define name="RECTANGLE_SURVEY_HEADING_NS" value="0."/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="BAT">
|
||||||
|
<define name="CRITIC_BAT_LEVEL" value="9.3" unit="V"/>
|
||||||
|
</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_ATTITUDE_Z_HOLD"/>
|
||||||
|
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="SONAR">
|
||||||
|
<!--define name="SENSOR_SYNC_SEND_SONAR"/-->
|
||||||
|
<define name="SONAR_SCALE" value="0.0025"/> <!-- Vcc/1024 per cm => (3.3/2^12)*(0.01/(3.3/2^10) = 0.01*2^10/2^12 -->
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="AGL" prefix="AGL_DIST_SONAR_">
|
||||||
|
<define name="ID" value="ABI_BROADCAST"/>
|
||||||
|
<define name="MAX_RANGE" value="6." unit="m"/>
|
||||||
|
<define name="MIN_RANGE" value="0.01" unit="m"/>
|
||||||
|
<define name="FILTER" value="0.15"/> <!--Low pass filter time constant-->
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="TAG_TRACKING" prefix="TAG_TRACKING_">
|
||||||
|
<define name="BODY_TO_CAM_PSI" value="0"/>
|
||||||
|
<define name="CAM_POS_Y" value="-0.12"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="GCS">
|
||||||
|
<define name="ALT_SHIFT_PLUS_PLUS" value="3"/>
|
||||||
|
<define name="ALT_SHIFT_PLUS" value="1"/>
|
||||||
|
<define name="ALT_SHIFT_MINUS" value="-0.5"/>
|
||||||
|
</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="NO_MOTOR_MIXING" value="TRUE"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
</airframe>
|
||||||
|
|
||||||
@@ -0,0 +1,277 @@
|
|||||||
|
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
|
||||||
|
|
||||||
|
<airframe name="Robo_Bee_3">
|
||||||
|
|
||||||
|
<description>
|
||||||
|
* Autopilot: Tawaki
|
||||||
|
* Actuators: 4 in 1
|
||||||
|
* Telemetry: XBee
|
||||||
|
* GPS: datalink
|
||||||
|
* RC: SBUS
|
||||||
|
</description>
|
||||||
|
|
||||||
|
<firmware name="rotorcraft">
|
||||||
|
<configure name="PERIODIC_FREQUENCY" value="512"/>
|
||||||
|
<configure name="AHRS_PROPAGATE_FREQUENCY" value="$(PERIODIC_FREQUENCY)"/>
|
||||||
|
|
||||||
|
<target name="ap" board="tawaki_1.1">
|
||||||
|
<module name="radio_control" type="sbus"/>
|
||||||
|
<configure name="BARO_PERIODIC_FREQUENCY" value="50"/>
|
||||||
|
<define name="RADIO_KILL_SWITCH" value="RADIO_GAIN1"/>
|
||||||
|
</target>
|
||||||
|
|
||||||
|
<target name="nps" board="pc">
|
||||||
|
<!--module name="fdm" type="jsbsim"/-->
|
||||||
|
<module name="fdm" type="pybullet">
|
||||||
|
<define name="PYBULLET_GUI" value="TRUE"/>
|
||||||
|
<define name="NPS_BYPASS_AHRS" value="TRUE"/>
|
||||||
|
<define name="NPS_BYPASS_INS" value="TRUE"/>
|
||||||
|
</module>
|
||||||
|
<module name="radio_control" type="datalink"/>
|
||||||
|
</target>
|
||||||
|
|
||||||
|
<module name="telemetry" type="xbee_api"/>
|
||||||
|
|
||||||
|
<module name="actuators" type="dshot">
|
||||||
|
<!--define name="DSHOT_SPEED" value="300"/-->
|
||||||
|
</module>
|
||||||
|
<module name="board" type="tawaki">
|
||||||
|
<define name="IMU_MPU_LOWPASS_FILTER" value="MPU60X0_DLPF_256HZ"/>
|
||||||
|
<define name="IMU_MPU_ACCEL_LOWPASS_FILTER" value="MPU60X0_DLPF_ACC_218HZ"/>
|
||||||
|
<define name="IMU_MPU_SMPLRT_DIV" value="0"/>
|
||||||
|
</module>
|
||||||
|
<module name="gps" type="ublox">
|
||||||
|
<configure name="GPS_BAUD" value="B115200"/>
|
||||||
|
</module>
|
||||||
|
|
||||||
|
<module name="stabilization" type="indi"/>
|
||||||
|
<module name="guidance" type="indi">
|
||||||
|
<define name="GUIDANCE_INDI_SPEED_GAIN" value="3.0"/>
|
||||||
|
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
|
||||||
|
<define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.03"/>
|
||||||
|
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
|
||||||
|
</module>
|
||||||
|
|
||||||
|
<module name="ins"/>
|
||||||
|
<module name="ahrs" type="int_cmpl_quat">
|
||||||
|
<configure name="USE_MAGNETOMETER" value="TRUE"/>
|
||||||
|
<!-- <define name="AHRS_USE_GPS_HEADING" value="FALSE"/> -->
|
||||||
|
</module>
|
||||||
|
<module name="air_data"/>
|
||||||
|
|
||||||
|
<module name="filter" type="1euro_imu">
|
||||||
|
<define name="AHRS_ICQ_IMU_ID" value="IMU_F1E_ID"/>
|
||||||
|
<define name="AHRS_ALIGNER_IMU_ID" value="IMU_F1E_ID"/>
|
||||||
|
</module>
|
||||||
|
|
||||||
|
<module name="tag_tracking">
|
||||||
|
<configure name="JEVOIS_UART" value="UART4" description="dummy uart in this config"/>
|
||||||
|
</module>
|
||||||
|
<module name="extra_dl">
|
||||||
|
<configure name="EXTRA_DL_PORT" value="UART3"/>
|
||||||
|
<configure name="EXTRA_DL_BAUD" value="B115200"/>
|
||||||
|
</module>
|
||||||
|
|
||||||
|
<module name="motor_mixing"/>
|
||||||
|
|
||||||
|
<module name="flight_recorder"/>
|
||||||
|
<!--module name="logger" type="tune_indi"/-->
|
||||||
|
</firmware>
|
||||||
|
|
||||||
|
<servos driver="DShot">
|
||||||
|
<servo name="FR" no="1" min="0" neutral="100" max="2000"/>
|
||||||
|
<servo name="BR" no="2" min="0" neutral="100" max="2000"/>
|
||||||
|
<servo name="BL" no="3" min="0" neutral="100" max="2000"/>
|
||||||
|
<servo name="FL" no="4" min="0" neutral="100" max="2000"/>
|
||||||
|
</servos>
|
||||||
|
|
||||||
|
<commands>
|
||||||
|
<axis name="ROLL" failsafe_value="0"/>
|
||||||
|
<axis name="PITCH" failsafe_value="0"/>
|
||||||
|
<axis name="YAW" failsafe_value="0"/>
|
||||||
|
<axis name="THRUST" failsafe_value="0"/>
|
||||||
|
</commands>
|
||||||
|
|
||||||
|
<section name="MIXING" prefix="MOTOR_MIXING_">
|
||||||
|
<define name="TYPE" value="QUAD_X"/>
|
||||||
|
<define name="REVERSE" value="TRUE"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<!--command_laws>
|
||||||
|
<call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
|
||||||
|
<set servo="FR" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
|
||||||
|
<set servo="BR" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
|
||||||
|
<set servo="BL" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
|
||||||
|
<set servo="FL" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
|
||||||
|
</command_laws-->
|
||||||
|
<command_laws>
|
||||||
|
<set servo="FR" value="autopilot_get_motors_on() ? actuators_pprz[0] : -MAX_PPRZ"/>
|
||||||
|
<set servo="BR" value="autopilot_get_motors_on() ? actuators_pprz[1] : -MAX_PPRZ"/>
|
||||||
|
<set servo="BL" value="autopilot_get_motors_on() ? actuators_pprz[2] : -MAX_PPRZ"/>
|
||||||
|
<set servo="FL" value="autopilot_get_motors_on() ? actuators_pprz[3] : -MAX_PPRZ"/>
|
||||||
|
</command_laws>
|
||||||
|
|
||||||
|
|
||||||
|
<section name="IMU" prefix="IMU_">
|
||||||
|
<define name="GYRO_P_SIGN" value="-1"/>
|
||||||
|
<define name="GYRO_Q_SIGN" value="-1"/>
|
||||||
|
<define name="GYRO_R_SIGN" value="1"/>
|
||||||
|
|
||||||
|
|
||||||
|
<define name="ACCEL_X_SIGN" value="-1"/>
|
||||||
|
<define name="ACCEL_Y_SIGN" value="-1"/>
|
||||||
|
<define name="ACCEL_Z_SIGN" value="1"/>
|
||||||
|
|
||||||
|
<define name="ACCEL_X_NEUTRAL" value="-52"/>
|
||||||
|
<define name="ACCEL_Y_NEUTRAL" value="63"/>
|
||||||
|
<define name="ACCEL_Z_NEUTRAL" value="78"/>
|
||||||
|
<define name="ACCEL_X_SENS" value="2.5604087853414104" integer="16"/>
|
||||||
|
<define name="ACCEL_Y_SENS" value="2.448144955015108" integer="16"/>
|
||||||
|
<define name="ACCEL_Z_SENS" value="2.4642689651242438" integer="16"/>
|
||||||
|
|
||||||
|
<define name="MAG_X_SIGN" value="-1"/>
|
||||||
|
<define name="MAG_Y_SIGN" value="-1"/>
|
||||||
|
<define name="MAG_Z_SIGN" value="1"/>
|
||||||
|
|
||||||
|
<define name="MAG_X_NEUTRAL" value="-3389"/>
|
||||||
|
<define name="MAG_Y_NEUTRAL" value="-9655"/>
|
||||||
|
<define name="MAG_Z_NEUTRAL" value="1277"/>
|
||||||
|
<define name="MAG_X_SENS" value="0.725111506132357" integer="16"/>
|
||||||
|
<define name="MAG_Y_SENS" value="0.7213630968776462" integer="16"/>
|
||||||
|
<define name="MAG_Z_SENS" value="0.6528914510154457" integer="16"/>
|
||||||
|
|
||||||
|
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
|
||||||
|
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
|
||||||
|
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<include href="conf/mag/delft_valkenburg.xml"/>
|
||||||
|
|
||||||
|
<section name="INS" prefix="INS_">
|
||||||
|
<define name="INV_NXZ" value="0.25"/>
|
||||||
|
<define name="INV_NH" value="2.0"/>
|
||||||
|
<define name="INV_MVZ" value="8."/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="AHRS" prefix="AHRS_">
|
||||||
|
<!-- Use GPS heading instead of magneto -->
|
||||||
|
<define name="USE_GPS_HEADING" value="0"/>
|
||||||
|
<!-- <define name="HEADING_UPDATE_GPS_MIN_SPEED" value="0"/> -->
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
|
||||||
|
<!-- setpoint limits for attitude stabilization rc flight -->
|
||||||
|
<define name="SP_MAX_PHI" value="45" unit="deg"/>
|
||||||
|
<define name="SP_MAX_THETA" value="45" unit="deg"/>
|
||||||
|
<define name="SP_MAX_R" value="120" unit="deg/s"/>
|
||||||
|
<define name="DEADBAND_A" value="0"/>
|
||||||
|
<define name="DEADBAND_E" value="0"/>
|
||||||
|
<define name="DEADBAND_R" value="50"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||||
|
<define name="NUM_ACT" value="4"/>
|
||||||
|
<!-- reference acceleration for attitude control -->
|
||||||
|
<define name="REF_ERR_P" value="111"/>
|
||||||
|
<define name="REF_ERR_Q" value="111"/>
|
||||||
|
<define name="REF_ERR_R" value="136"/>
|
||||||
|
<define name="REF_RATE_P" value="20.0"/>
|
||||||
|
<define name="REF_RATE_Q" value="20.0"/>
|
||||||
|
<define name="REF_RATE_R" value="11.0"/>
|
||||||
|
|
||||||
|
<define name="MAX_R" value="120" unit="deg/s"/>
|
||||||
|
|
||||||
|
<!-- second order filter parameters -->
|
||||||
|
<define name="FILT_CUTOFF" value="4.0"/>
|
||||||
|
<define name="FILT_CUTOFF_R" value="4.0"/>
|
||||||
|
|
||||||
|
<!-- Adaptive Learning Rate -->
|
||||||
|
<define name="USE_ADAPTIVE" value="FALSE"/>
|
||||||
|
<define name="ADAPTIVE_MU" value="0.0001"/>
|
||||||
|
|
||||||
|
|
||||||
|
<!-- Full INDI -->
|
||||||
|
<!-- control effectiveness -->
|
||||||
|
<define name="G1_ROLL" value="{ -20.0, -20.0, 20.0, 20.0 }"/>
|
||||||
|
<define name="G1_PITCH" value="{ 20.0, -20.0, -20.0, 20.0 }"/>
|
||||||
|
<define name="G1_YAW" value="{ 3, -3, 3, -3 }"/>
|
||||||
|
<define name="G1_THRUST" value="{ -0.5, -0.5, -0.5, -0.5}"/>
|
||||||
|
<!--Counter torque effect of spinning up a rotor-->
|
||||||
|
<define name="G2" value="{100, -100, 100, -100.0 }"/>
|
||||||
|
<!-- first order actuator dynamics -->
|
||||||
|
<define name="ACT_DYN" value="{0.02, 0.02, 0.02, 0.02}"/>
|
||||||
|
<define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
|
||||||
|
|
||||||
|
<!--Priority for each axis (roll, pitch, yaw and thrust)-->
|
||||||
|
<define name="WLS_PRIORITIES" value="{1000, 1000, 1, 100}"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||||
|
<define name="REF_MIN_ZDD" value="-0.4*9.81"/>
|
||||||
|
<define name="REF_MAX_ZDD" value=" 0.4*9.81"/>
|
||||||
|
<define name="REF_MIN_ZD" value="-1.5"/>
|
||||||
|
<define name="REF_MAX_ZD" value=" 1."/>
|
||||||
|
<define name="HOVER_KP" value="90"/>
|
||||||
|
<define name="HOVER_KD" value="110"/>
|
||||||
|
<define name="HOVER_KI" value="10"/>
|
||||||
|
<define name="NOMINAL_HOVER_THROTTLE" value="0.35"/>
|
||||||
|
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
|
||||||
|
<define name="ADAPT_NOISE_FACTOR" value="1."/>
|
||||||
|
<define name="ADAPT_INITIAL_HOVER_THROTTLE" value="0.3"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
|
||||||
|
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||||
|
<define name="MAX_BANK" value="20" unit="deg"/>
|
||||||
|
<define name="PGAIN" value="41"/>
|
||||||
|
<define name="DGAIN" value="108"/>
|
||||||
|
<define name="IGAIN" value="20"/>
|
||||||
|
<define name="NGAIN" value="0"/>
|
||||||
|
<!-- feedforward -->
|
||||||
|
<define name="AGAIN" value="0"/>
|
||||||
|
<define name="REF_MAX_SPEED" value="4."/>
|
||||||
|
<define name="REF_MAX_ACCEL" value="2."/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="NAV">
|
||||||
|
<define name="ARRIVED_AT_WAYPOINT" value="2.0"/>
|
||||||
|
<define name="NAV_DESCEND_VSPEED" value="-0.5"/>
|
||||||
|
<define name="NAV_CLIMB_VSPEED" value="0.5"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="BAT">
|
||||||
|
<define name="LOW_BAT_LEVEL" value="10.1" unit="V"/>
|
||||||
|
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
|
||||||
|
</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_ATTITUDE_Z_HOLD"/>
|
||||||
|
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="TAG_TRACKING" prefix="TAG_TRACKING_">
|
||||||
|
<define name="BODY_TO_CAM_PSI" value="M_PI"/>
|
||||||
|
<define name="CAM_POS_X" value="0.1"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="GCS">
|
||||||
|
<define name="ALT_SHIFT_PLUS_PLUS" value="5"/>
|
||||||
|
<define name="ALT_SHIFT_PLUS" value="1"/>
|
||||||
|
<define name="ALT_SHIFT_MINUS" value="-1"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="SIMULATOR" prefix="NPS_">
|
||||||
|
<define name="ACTUATORS_ORDER" value="{3, 2, 1, 0}"/>
|
||||||
|
<define name="PYBULLET_MODULE" value="simple_quad_sim" type="string"/>
|
||||||
|
<define name="PYBULLET_URDF" value="robobee.urdf" type="string"/>
|
||||||
|
<!--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="NO_MOTOR_MIXING" value="TRUE"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
|
||||||
|
</airframe>
|
||||||
@@ -0,0 +1,261 @@
|
|||||||
|
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
|
||||||
|
|
||||||
|
<airframe name="Quadricopter ULYSSE Tawaki">
|
||||||
|
|
||||||
|
<description>
|
||||||
|
* Autopilot: Tawaki
|
||||||
|
* Actuators: 4 in 4 Holybro BLHELI ESC
|
||||||
|
* Telemetry: XBee
|
||||||
|
* GPS: ublox
|
||||||
|
* RC: FrSky XM+
|
||||||
|
</description>
|
||||||
|
|
||||||
|
<firmware name="rotorcraft">
|
||||||
|
<!--configure name="RTOS_DEBUG" value="TRUE"/-->
|
||||||
|
<configure name="PERIODIC_FREQUENCY" value="1000"/>
|
||||||
|
<configure name="AHRS_PROPAGATE_FREQUENCY" value="$(PERIODIC_FREQUENCY)"/>
|
||||||
|
|
||||||
|
<target name="ap" board="tawaki_1.0">
|
||||||
|
<configure name="BARO_PERIODIC_FREQUENCY" value="50"/>
|
||||||
|
<module name="radio_control" type="sbus"/>
|
||||||
|
<define name="RADIO_KILL_SWITCH" value="RADIO_GAIN1"/>
|
||||||
|
</target>
|
||||||
|
|
||||||
|
<target name="nps" board="pc">
|
||||||
|
<module name="fdm" type="jsbsim"/>
|
||||||
|
<module name="radio_control" type="ppm"/>
|
||||||
|
</target>
|
||||||
|
|
||||||
|
<module name="telemetry" type="xbee_api"/>
|
||||||
|
|
||||||
|
<module name="motor_mixing"/>
|
||||||
|
|
||||||
|
<module name="actuators" type="dshot">
|
||||||
|
<!--define name="DSHOT_SPEED" value="300"/-->
|
||||||
|
</module>
|
||||||
|
|
||||||
|
<module name="board" type="tawaki">
|
||||||
|
<!--define name="IMU_MPU_LOWPASS_FILTER" value="MPU60X0_DLPF_256HZ"/-->
|
||||||
|
<!--define name="IMU_MPU_ACCEL_LOWPASS_FILTER" value="MPU60X0_DLPF_ACC_218HZ"/-->
|
||||||
|
<define name="IMU_MPU_LOWPASS_FILTER" value="MPU60X0_DLPF_98HZ"/>
|
||||||
|
<define name="IMU_MPU_ACCEL_LOWPASS_FILTER" value="MPU60X0_DLPF_ACC_99HZ"/>
|
||||||
|
<define name="IMU_MPU_SMPLRT_DIV" value="0"/>
|
||||||
|
<!--configure name="BOARD_TAWAKI_ROTATED" value="TRUE"/-->
|
||||||
|
<configure name="MAG_LIS3MDL_I2C_DEV" value="i2c2"/>
|
||||||
|
</module>
|
||||||
|
<module name="gps" type="ublox">
|
||||||
|
<configure name="GPS_BAUD" value="B115200"/>
|
||||||
|
<define name="USE_GPS_UBX_RTCM" value="TRUE"/>
|
||||||
|
</module>
|
||||||
|
|
||||||
|
<module name="stabilization" type="indi"/>
|
||||||
|
<module name="guidance" type="indi">
|
||||||
|
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
|
||||||
|
<define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.03"/>
|
||||||
|
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
|
||||||
|
</module>
|
||||||
|
|
||||||
|
<module name="ins"/>
|
||||||
|
<module name="ahrs" type="int_cmpl_quat"/>
|
||||||
|
<module name="air_data"/>
|
||||||
|
|
||||||
|
<module name="actuators" type="pwm"/>
|
||||||
|
<module name="switch" type="servo"/>
|
||||||
|
|
||||||
|
<!--module name="filter" type="1euro_imu">
|
||||||
|
<define name="AHRS_ICQ_IMU_ID" value="IMU_F1E_ID"/>
|
||||||
|
<define name="AHRS_ALIGNER_IMU_ID" value="IMU_F1E_ID"/>
|
||||||
|
</module-->
|
||||||
|
|
||||||
|
<module name="tag_tracking">
|
||||||
|
<configure name="JEVOIS_UART" value="UART3" description="UART on which Jevois camera is connected"/>
|
||||||
|
</module>
|
||||||
|
|
||||||
|
<module name="flight_recorder"/>
|
||||||
|
<!--module name="logger" type="tune_indi"/-->
|
||||||
|
</firmware>
|
||||||
|
|
||||||
|
<servos driver="DShot">
|
||||||
|
<servo name="FR" no="4" min="0" neutral="100" max="2000"/>
|
||||||
|
<servo name="BR" no="2" min="0" neutral="100" max="2000"/>
|
||||||
|
<servo name="BL" no="1" min="0" neutral="100" max="2000"/>
|
||||||
|
<servo name="FL" no="3" min="0" neutral="100" max="2000"/>
|
||||||
|
</servos>
|
||||||
|
|
||||||
|
<servos driver="Pwm">
|
||||||
|
<servo name="SWITCH" no="4" min="1000" neutral="1000" max="2000"/>
|
||||||
|
</servos>
|
||||||
|
|
||||||
|
<commands>
|
||||||
|
<axis name="ROLL" failsafe_value="0"/>
|
||||||
|
<axis name="PITCH" failsafe_value="0"/>
|
||||||
|
<axis name="YAW" failsafe_value="0"/>
|
||||||
|
<axis name="THRUST" failsafe_value="0"/>
|
||||||
|
</commands>
|
||||||
|
|
||||||
|
<section name="MIXING" prefix="MOTOR_MIXING_">
|
||||||
|
<define name="TYPE" value="QUAD_X"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<command_laws>
|
||||||
|
<set servo="FR" value="autopilot_get_motors_on() ? actuators_pprz[0] : -MAX_PPRZ"/>
|
||||||
|
<set servo="BR" value="autopilot_get_motors_on() ? actuators_pprz[1] : -MAX_PPRZ"/>
|
||||||
|
<set servo="BL" value="autopilot_get_motors_on() ? actuators_pprz[2] : -MAX_PPRZ"/>
|
||||||
|
<set servo="FL" value="autopilot_get_motors_on() ? actuators_pprz[3] : -MAX_PPRZ"/>
|
||||||
|
</command_laws>
|
||||||
|
|
||||||
|
<section name="IMU" prefix="IMU_">
|
||||||
|
<define name="GYRO_P_SIGN" value="1"/>
|
||||||
|
<define name="GYRO_Q_SIGN" value="1"/>
|
||||||
|
<define name="GYRO_R_SIGN" value="1"/>
|
||||||
|
|
||||||
|
<define name="ACCEL_X_SIGN" value="1"/>
|
||||||
|
<define name="ACCEL_Y_SIGN" value="1"/>
|
||||||
|
<define name="ACCEL_Z_SIGN" value="1"/>
|
||||||
|
<define name="ACCEL_X_NEUTRAL" value="-27"/>
|
||||||
|
<define name="ACCEL_Y_NEUTRAL" value="75"/>
|
||||||
|
<define name="ACCEL_Z_NEUTRAL" value="-109"/>
|
||||||
|
<define name="ACCEL_X_SENS" value="2.27584739975" integer="16"/>
|
||||||
|
<define name="ACCEL_Y_SENS" value="2.45270163528" integer="16"/>
|
||||||
|
<define name="ACCEL_Z_SENS" value="2.44302118661" integer="16"/>
|
||||||
|
|
||||||
|
<define name="MAG_X_SIGN" value="1"/>
|
||||||
|
<define name="MAG_Y_SIGN" value="1"/>
|
||||||
|
<define name="MAG_Z_SIGN" value="1"/>
|
||||||
|
<define name="MAG_X_NEUTRAL" value="-525"/>
|
||||||
|
<define name="MAG_Y_NEUTRAL" value="3252"/>
|
||||||
|
<define name="MAG_Z_NEUTRAL" value="1029"/>
|
||||||
|
<define name="MAG_X_SENS" value="0.6621839327083922" integer="16"/>
|
||||||
|
<define name="MAG_Y_SENS" value="0.635352946158572" integer="16"/>
|
||||||
|
<define name="MAG_Z_SENS" value="0.6546061419369537" integer="16"/>
|
||||||
|
|
||||||
|
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
|
||||||
|
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
|
||||||
|
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="MAG">
|
||||||
|
<define name="LIS3MDL_MAG_TO_IMU_PHI" value="0." unit="deg"/>
|
||||||
|
<define name="LIS3MDL_MAG_TO_IMU_THETA" value="0." unit="deg"/>
|
||||||
|
<define name="LIS3MDL_MAG_TO_IMU_PSI" value="135." unit="deg"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<include href="conf/mag/delft_valkenburg.xml"/>
|
||||||
|
|
||||||
|
<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"/>
|
||||||
|
<define name="SP_MAX_R" value="60." unit="deg/s"/>
|
||||||
|
<define name="DEADBAND_R" value="250"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||||
|
|
||||||
|
<!-- reference acceleration for attitude control -->
|
||||||
|
<define name="REF_ERR_P" value="101"/>
|
||||||
|
<define name="REF_ERR_Q" value="101"/>
|
||||||
|
<define name="REF_ERR_R" value="124"/>
|
||||||
|
<define name="REF_RATE_P" value="18.0"/>
|
||||||
|
<define name="REF_RATE_Q" value="18.0"/>
|
||||||
|
<define name="REF_RATE_R" value="14.0"/>
|
||||||
|
|
||||||
|
<define name="MAX_R" value="60" unit="deg/s"/>
|
||||||
|
|
||||||
|
<!-- second order filter parameters -->
|
||||||
|
<define name="FILT_CUTOFF" value="4.0"/>
|
||||||
|
<define name="FILT_CUTOFF_R" value="4.0"/>
|
||||||
|
|
||||||
|
<!-- Adaptive Learning Rate -->
|
||||||
|
<define name="USE_ADAPTIVE" value="FALSE"/>
|
||||||
|
<define name="ADAPTIVE_MU" value="0.0001"/>
|
||||||
|
|
||||||
|
|
||||||
|
<!-- Full INDI -->
|
||||||
|
<!-- control effectiveness -->
|
||||||
|
<define name="G1_ROLL" value="{-40 , -40, 40 , 40 }"/>
|
||||||
|
<define name="G1_PITCH" value="{40 , -40, -40 , 40 }"/>
|
||||||
|
<define name="G1_YAW" value="{5, -5, 5, -5}"/>
|
||||||
|
<define name="G1_THRUST" value="{-1.5, -1.5, -1.5, -1.5}"/>
|
||||||
|
<!--Counter torque effect of spinning up a rotor-->
|
||||||
|
<define name="G2" value="{150.0, -150.0, 150.0, -150.0 }"/>
|
||||||
|
<!-- first order actuator dynamics -->
|
||||||
|
<define name="ACT_DYN" value="{0.03, 0.03, 0.03, 0.03}"/>
|
||||||
|
<define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
|
||||||
|
|
||||||
|
<!--Priority for each axis (roll, pitch, yaw and thrust)-->
|
||||||
|
<define name="WLS_PRIORITIES" value="{1000, 1000, 1, 100}"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
|
||||||
|
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||||
|
<define name="REF_MIN_ZDD" value="-0.4*9.81"/>
|
||||||
|
<define name="REF_MAX_ZDD" value=" 0.4*9.81"/>
|
||||||
|
<define name="REF_MIN_ZD" value="-1.5"/>
|
||||||
|
<define name="REF_MAX_ZD" value=" 1."/>
|
||||||
|
<define name="HOVER_KP" value="48"/>
|
||||||
|
<define name="HOVER_KD" value="46"/>
|
||||||
|
<define name="HOVER_KI" value="11"/>
|
||||||
|
<define name="NOMINAL_HOVER_THROTTLE" value="0.48"/>
|
||||||
|
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
|
||||||
|
<define name="ADAPT_NOISE_FACTOR" value="1."/>
|
||||||
|
<define name="ADAPT_INITIAL_HOVER_THROTTLE" value="0.4"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||||
|
<define name="MAX_BANK" value="20" unit="deg"/>
|
||||||
|
<define name="PGAIN" value="41"/>
|
||||||
|
<define name="DGAIN" value="108"/>
|
||||||
|
<define name="IGAIN" value="20"/>
|
||||||
|
<define name="NGAIN" value="0"/>
|
||||||
|
<!-- feedforward -->
|
||||||
|
<define name="AGAIN" value="0"/>
|
||||||
|
<define name="REF_MAX_SPEED" value="2.5"/>
|
||||||
|
<define name="REF_MAX_ACCEL" value="2.5"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="NAV">
|
||||||
|
<define name="ARRIVED_AT_WAYPOINT" value="2.0"/>
|
||||||
|
<define name="NAV_CLIMB_VSPEED" value="1.0"/>
|
||||||
|
<define name="NAV_DESCEND_VSPEED" value="-0.5"/>
|
||||||
|
<define name="RECTANGLE_SURVEY_HEADING_NS" value="0."/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="BAT">
|
||||||
|
<define name="CRITIC_BAT_LEVEL" value="9.3" unit="V"/>
|
||||||
|
</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_ATTITUDE_Z_HOLD"/>
|
||||||
|
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="AGL" prefix="AGL_DIST_SONAR_">
|
||||||
|
<define name="ID" value="ABI_BROADCAST"/>
|
||||||
|
<define name="MAX_RANGE" value="6." unit="m"/>
|
||||||
|
<define name="MIN_RANGE" value="0.01" unit="m"/>
|
||||||
|
<define name="FILTER" value="0.15"/> <!--Low pass filter time constant-->
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="TAG_TRACKING" prefix="TAG_TRACKING_">
|
||||||
|
<define name="BODY_TO_CAM_PSI" value="180."/>
|
||||||
|
<define name="CAM_POS_Y" value="0.12"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="GCS">
|
||||||
|
<define name="ALT_SHIFT_PLUS_PLUS" value="3"/>
|
||||||
|
<define name="ALT_SHIFT_PLUS" value="1"/>
|
||||||
|
<define name="ALT_SHIFT_MINUS" value="-0.5"/>
|
||||||
|
</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="NO_MOTOR_MIXING" value="TRUE"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
</airframe>
|
||||||
|
|
||||||
@@ -0,0 +1,24 @@
|
|||||||
|
<!DOCTYPE module SYSTEM "module.dtd">
|
||||||
|
|
||||||
|
<module name="fdm_pybullet" dir="fdm">
|
||||||
|
<doc>
|
||||||
|
<description>
|
||||||
|
PyBullet based FDM for NPS simulator
|
||||||
|
</description>
|
||||||
|
<define name="PYTHON_EXEC" value="python3" type="string" description="python executable to use. Can be a /path/to/venv/python3"/>
|
||||||
|
<define name="PYBULLET_GUI" value="TRUE" description="start pybullet Gui"/>
|
||||||
|
<define name="NPS_ACTUATORS_ORDER" value="{3, 2, 1, 0}" description="set actuators URDF indexes"/>
|
||||||
|
<define name="NPS_PYBULLET_MODULE" value="simple_quad_sim" type="string" description="python module for your pybullet model"/>
|
||||||
|
<define name="NPS_PYBULLET_URDF" value="robobee.urdf" type="string" description="URDF vehicle model"/>
|
||||||
|
</doc>
|
||||||
|
<header/>
|
||||||
|
<makefile target="nps|hitl">
|
||||||
|
<raw>
|
||||||
|
nps.CFLAGS += $(shell python3-config --cflags) -fPIE
|
||||||
|
nps.LDFLAGS += $(shell python3-config --ldflags) -lpython3.8
|
||||||
|
nps.CFLAGS += -DPAPARAZZI_SRC=\"$(PAPARAZZI_SRC)\"
|
||||||
|
</raw>
|
||||||
|
<file name="nps_fdm_pybullet.c" dir="nps"/>
|
||||||
|
</makefile>
|
||||||
|
</module>
|
||||||
|
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,170 @@
|
|||||||
|
<?xml version="1.0" ?>
|
||||||
|
|
||||||
|
<robot name="RoboBee">
|
||||||
|
|
||||||
|
<material name="white">
|
||||||
|
<color rgba="1. 1. 1. 1.0"/>
|
||||||
|
</material>
|
||||||
|
|
||||||
|
<material name="black">
|
||||||
|
<color rgba="0. 0. 0. 1.0"/>
|
||||||
|
</material>
|
||||||
|
|
||||||
|
<material name="blue0">
|
||||||
|
<color rgba="15./255. 141./255. 238./255. 1.0"/>
|
||||||
|
</material>
|
||||||
|
|
||||||
|
<material name="darkgrey">
|
||||||
|
<color rgba="0.4 0.4 0.4 1.0"/>
|
||||||
|
</material>
|
||||||
|
|
||||||
|
<material name="blue1">
|
||||||
|
<color rgba="0.06 0.46 0.9 0.6"/>
|
||||||
|
</material>
|
||||||
|
|
||||||
|
<material name="orange_red">
|
||||||
|
<color rgba="1 0.25 0 0.4"/>
|
||||||
|
</material>
|
||||||
|
|
||||||
|
<configuration type="quad"/>
|
||||||
|
|
||||||
|
<properties arm="0.0635" kf="2.0e-8" km="2.74e-10" thrust2weight="2" max_speed_kmh="30" gnd_eff_coeff="11.36859" prop_radius="3.31348e-2" drag_coeff_xy="9.1785e-7" drag_coeff_z="10.311e-7" dw_coeff_1="2267.18" dw_coeff_2=".16" dw_coeff_3="-.11"/>
|
||||||
|
|
||||||
|
<control>
|
||||||
|
<indi actuator_nr="4" output_nr="4" />
|
||||||
|
<indi_1 roll=" 50.0 50.0 -50.0 -50.0" />
|
||||||
|
<indi_2 pitch="-50.0 50.0 50.0 -50.0" />
|
||||||
|
<indi_3 yaw=" -7.0 7.0 -7.0 7.0" />
|
||||||
|
<indi_4 thrust=" 1.7 1.7 1.7 1.7" />
|
||||||
|
|
||||||
|
<indi_guidance_gains>
|
||||||
|
<pos kp="1.0" kd="2.2"/>
|
||||||
|
</indi_guidance_gains>
|
||||||
|
|
||||||
|
<indi_att_gains>
|
||||||
|
<att p="7." q="7." r="5." />
|
||||||
|
<rate p="18." q="18." r="10." />
|
||||||
|
</indi_att_gains>
|
||||||
|
|
||||||
|
<pwm>
|
||||||
|
<pwm2rpm scale="20000. 20000. 20000. 20000." const="0. 0. 0. 0." />
|
||||||
|
<limit min="0 0 0 0" max="1 1 1 1" />
|
||||||
|
</pwm>
|
||||||
|
</control>
|
||||||
|
|
||||||
|
|
||||||
|
<link name="base_link">
|
||||||
|
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<mass value="1"/>
|
||||||
|
<inertia ixx="6.2e-4" ixy="0.0" ixz="0.0" iyy="6.2e-4" iyz="0.0" izz="1.1e-3"/>
|
||||||
|
</inertial>
|
||||||
|
|
||||||
|
<visual>
|
||||||
|
<origin rpy="1.5708 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="./robobee.obj" scale="1. 1. 1."/>
|
||||||
|
</geometry>
|
||||||
|
<material name="darkgrey"/>
|
||||||
|
</visual>
|
||||||
|
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder radius="0.15" length=".1"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="prop0_link">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.11 0.11 0"/>
|
||||||
|
<mass value="0"/>
|
||||||
|
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0.11 0.11 0.01"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder radius="0.0635" length=".001"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="orange_red"/>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint name="prop0_joint" type="fixed">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="prop0_link"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="prop1_link">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.11 0.11 0"/>
|
||||||
|
<mass value="0"/>
|
||||||
|
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.11 0.11 0.02"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder radius="0.0635" length=".001"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="blue1"/>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint name="prop1_joint" type="fixed">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="prop1_link"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="prop2_link">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.11 -0.11 0.03"/>
|
||||||
|
<mass value="0"/>
|
||||||
|
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.11 -0.11 0.01"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder radius="0.0635" length=".001"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="blue1"/>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint name="prop2_joint" type="fixed">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="prop2_link"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="prop3_link">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.11 -0.11 0.04"/>
|
||||||
|
<mass value="0"/>
|
||||||
|
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0.11 -0.11 0.01"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder radius="0.0635" length=".001"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="orange_red"/>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
<joint name="prop3_joint" type="fixed">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="prop3_link"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="center_of_mass_link">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<mass value="0"/>
|
||||||
|
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint name="center_of_mass_joint" type="fixed">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="center_of_mass_link"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
</robot>
|
||||||
|
|
||||||
@@ -111,6 +111,15 @@ void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct D
|
|||||||
v_out->z = 2 * (m20 * v_in->x + m21 * v_in->y + m22 * v_in->z);
|
v_out->z = 2 * (m20 * v_in->x + m21 * v_in->y + m22 * v_in->z);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void double_quat_comp(struct DoubleQuat *a2c, struct DoubleQuat *a2b, struct DoubleQuat *b2c)
|
||||||
|
{
|
||||||
|
a2c->qi = a2b->qi * b2c->qi - a2b->qx * b2c->qx - a2b->qy * b2c->qy - a2b->qz * b2c->qz;
|
||||||
|
a2c->qx = a2b->qi * b2c->qx + a2b->qx * b2c->qi + a2b->qy * b2c->qz - a2b->qz * b2c->qy;
|
||||||
|
a2c->qy = a2b->qi * b2c->qy - a2b->qx * b2c->qz + a2b->qy * b2c->qi + a2b->qz * b2c->qx;
|
||||||
|
a2c->qz = a2b->qi * b2c->qz + a2b->qx * b2c->qy - a2b->qy * b2c->qx + a2b->qz * b2c->qi;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
void double_rmat_inv(struct DoubleRMat *m_b2a, struct DoubleRMat *m_a2b)
|
void double_rmat_inv(struct DoubleRMat *m_b2a, struct DoubleRMat *m_a2b)
|
||||||
{
|
{
|
||||||
/*RMAT_ELMT(*m_b2a, 0, 0) = RMAT_ELMT(*m_a2b, 0, 0);*/
|
/*RMAT_ELMT(*m_b2a, 0, 0) = RMAT_ELMT(*m_a2b, 0, 0);*/
|
||||||
|
|||||||
@@ -157,6 +157,11 @@ extern void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e);
|
|||||||
extern void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQuat *q);
|
extern void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQuat *q);
|
||||||
extern void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in);
|
extern void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in);
|
||||||
|
|
||||||
|
/** Composition (multiplication) of two quaternions.
|
||||||
|
* a2c = a2b comp b2c , aka a2c = a2b * b2c
|
||||||
|
*/
|
||||||
|
extern void double_quat_comp(struct DoubleQuat *a2c, struct DoubleQuat *a2b, struct DoubleQuat *b2c);
|
||||||
|
|
||||||
/** initialises a rotation matrix to identity */
|
/** initialises a rotation matrix to identity */
|
||||||
static inline void double_rmat_identity(struct DoubleRMat *rm)
|
static inline void double_rmat_identity(struct DoubleRMat *rm)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -0,0 +1,387 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2021 Fabien-B <fabien-b@github.com>
|
||||||
|
*
|
||||||
|
* This file is part of paparazzi. See LICENCE file.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "nps_fdm.h"
|
||||||
|
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <Python.h>
|
||||||
|
|
||||||
|
#include "math/pprz_geodetic.h"
|
||||||
|
#include "math/pprz_geodetic_double.h"
|
||||||
|
#include "math/pprz_geodetic_float.h"
|
||||||
|
#include "math/pprz_algebra.h"
|
||||||
|
#include "math/pprz_algebra_float.h"
|
||||||
|
#include "math/pprz_isa.h"
|
||||||
|
|
||||||
|
#include "generated/airframe.h"
|
||||||
|
#include "generated/flight_plan.h"
|
||||||
|
|
||||||
|
#include "state.h"
|
||||||
|
|
||||||
|
#define _WIDEN(x) L ## x
|
||||||
|
#define WIDEN(x) _WIDEN(x)
|
||||||
|
|
||||||
|
#ifndef PYTHON_EXEC
|
||||||
|
#define PYTHON_EXEC "python3"
|
||||||
|
#endif
|
||||||
|
MESSAGE("PyBullet using" VALUE(PYTHON_EXEC))
|
||||||
|
|
||||||
|
#ifndef PYBULLET_GUI
|
||||||
|
#define PYBULLET_GUI TRUE
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef NPS_ACTUATORS_ORDER
|
||||||
|
int actuators_order[ACTUATORS_NB] = NPS_ACTUATORS_ORDER;
|
||||||
|
#else
|
||||||
|
#error "[PyBullet] missing NPS_ACTUATORS_ORDER define!"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef NPS_PYBULLET_MODULE
|
||||||
|
#define NPS_PYBULLET_MODULE "simple_quad_sim"
|
||||||
|
MESSAGE("NPS_PYBULLET_MODULE not defined, take 'simple_quad_sim' as default value.")
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef NPS_PYBULLET_URDF
|
||||||
|
#define NPS_PYBULLET_URDF "robobee.urdf"
|
||||||
|
MESSAGE("NPS_PYBULLET_URDF not defined, take 'robobee.urdf' as default value.")
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
// NpsFdm structure
|
||||||
|
struct NpsFdm fdm;
|
||||||
|
|
||||||
|
// Reference point
|
||||||
|
static struct LtpDef_d ltpdef;
|
||||||
|
|
||||||
|
//rotation from pybullet to pprz
|
||||||
|
struct DoubleQuat quat_to_pprz = {
|
||||||
|
1/sqrt(2),
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
1/sqrt(2),
|
||||||
|
};
|
||||||
|
|
||||||
|
// python object declarations
|
||||||
|
PyObject *fdm_module = NULL;
|
||||||
|
PyObject *bullet_fdm = NULL;
|
||||||
|
|
||||||
|
// static python related function declaration
|
||||||
|
static void py_check_status(PyConfig* config, PyStatus* status);
|
||||||
|
static void py_check(bool exit_on_error, int line_nb);
|
||||||
|
static void python_init(double dt);
|
||||||
|
|
||||||
|
// Static functions declaration
|
||||||
|
static void init_ltp(void);
|
||||||
|
|
||||||
|
static void get_pos(PyObject* ppos);
|
||||||
|
static void get_vel(PyObject* pvel);
|
||||||
|
static void get_acc(PyObject* pacc);
|
||||||
|
|
||||||
|
static void get_orient(PyObject* porient);
|
||||||
|
static void get_ang_vel(PyObject* pang_vel);
|
||||||
|
static void get_ang_acc(PyObject* pang_acc);
|
||||||
|
|
||||||
|
void nps_fdm_init(double dt)
|
||||||
|
{
|
||||||
|
python_init(dt);
|
||||||
|
|
||||||
|
fdm.init_dt = dt; // (1 / simulation freq)
|
||||||
|
fdm.curr_dt = dt;
|
||||||
|
fdm.time = dt;
|
||||||
|
|
||||||
|
fdm.on_ground = TRUE;
|
||||||
|
|
||||||
|
fdm.nan_count = 0;
|
||||||
|
fdm.pressure = -1;
|
||||||
|
fdm.pressure_sl = PPRZ_ISA_SEA_LEVEL_PRESSURE;
|
||||||
|
fdm.total_pressure = -1;
|
||||||
|
fdm.dynamic_pressure = -1;
|
||||||
|
fdm.temperature = -1;
|
||||||
|
|
||||||
|
fdm.ltpprz_to_body_eulers.psi = 0.0;
|
||||||
|
init_ltp();
|
||||||
|
|
||||||
|
// run a first step to initialize all fdm fields
|
||||||
|
double dummy_commands[] = {1, 2, 3, 4};
|
||||||
|
nps_fdm_run_step(false, dummy_commands, 4);
|
||||||
|
}
|
||||||
|
|
||||||
|
void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int commands_nb __attribute__((unused)))
|
||||||
|
{
|
||||||
|
// TODO create a np.array instead ?
|
||||||
|
PyObject* pcmd = PyList_New(commands_nb);
|
||||||
|
|
||||||
|
for(int i=0; i<commands_nb; i++) {
|
||||||
|
int j = actuators_order[i];
|
||||||
|
PyList_SetItem(pcmd, i, PyFloat_FromDouble(commands[j]));
|
||||||
|
}
|
||||||
|
|
||||||
|
if(commands[0] < 0) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
PyObject* ret = PyObject_CallMethod(bullet_fdm, "step", "(O)", pcmd);
|
||||||
|
py_check(true, __LINE__);
|
||||||
|
|
||||||
|
// borrowed references
|
||||||
|
PyObject* ppos = PyDict_GetItemString(ret, "pos");
|
||||||
|
PyObject* pvel = PyDict_GetItemString(ret, "vel");
|
||||||
|
PyObject* pacc = PyDict_GetItemString(ret, "accel");
|
||||||
|
|
||||||
|
PyObject* pquat = PyDict_GetItemString(ret, "quat");
|
||||||
|
PyObject* pang_v = PyDict_GetItemString(ret, "ang_v");
|
||||||
|
PyObject* pang_acc = PyDict_GetItemString(ret, "ang_accel");
|
||||||
|
//PyObject* prpy = PyDict_GetItemString(ret, "rpy");
|
||||||
|
py_check(true, __LINE__);
|
||||||
|
|
||||||
|
|
||||||
|
get_pos(ppos);
|
||||||
|
get_orient(pquat);
|
||||||
|
get_vel(pvel);
|
||||||
|
get_acc(pacc);
|
||||||
|
get_ang_vel(pang_v);
|
||||||
|
get_ang_acc(pang_acc);
|
||||||
|
|
||||||
|
Py_XDECREF(pcmd);
|
||||||
|
Py_XDECREF(ret);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void get_pos(PyObject* ppos) {
|
||||||
|
// get position from pybullet, in ENU, in the local frame
|
||||||
|
struct EnuCoor_d enu_pos;
|
||||||
|
enu_pos.x = PyFloat_AsDouble(PyTuple_GetItem(ppos, 0));
|
||||||
|
enu_pos.y = PyFloat_AsDouble(PyTuple_GetItem(ppos, 1));
|
||||||
|
enu_pos.z = PyFloat_AsDouble(PyTuple_GetItem(ppos, 2));
|
||||||
|
py_check(true, __LINE__);
|
||||||
|
|
||||||
|
/*********** positions *************/
|
||||||
|
VECT3_NED_OF_ENU(fdm.ltpprz_pos, enu_pos)
|
||||||
|
ecef_of_enu_point_d(&fdm.ecef_pos, <pdef, &enu_pos);
|
||||||
|
lla_of_ecef_d(&fdm.lla_pos, &fdm.ecef_pos);
|
||||||
|
fdm.hmsl = -fdm.ltpprz_pos.z; //FIXME
|
||||||
|
fdm.agl = fdm.hmsl; //FIXME
|
||||||
|
}
|
||||||
|
|
||||||
|
static void get_vel(PyObject* pvel) {
|
||||||
|
// get velocity from pybullet, in ENU wrt local frame (fixed/world)
|
||||||
|
struct EnuCoor_d enu_vel;
|
||||||
|
enu_vel.x = PyFloat_AsDouble(PyTuple_GetItem(pvel, 0));
|
||||||
|
enu_vel.y = PyFloat_AsDouble(PyTuple_GetItem(pvel, 1));
|
||||||
|
enu_vel.z = PyFloat_AsDouble(PyTuple_GetItem(pvel, 2));
|
||||||
|
py_check(true, __LINE__);
|
||||||
|
|
||||||
|
/*********** velocities *************/
|
||||||
|
/** velocity in LTP frame, wrt ECEF frame (NedCoor_d) */
|
||||||
|
VECT3_NED_OF_ENU(fdm.ltp_ecef_vel, enu_vel)
|
||||||
|
/** velocity in ECEF frame, wrt ECEF frame */
|
||||||
|
ecef_of_enu_vect_d(&fdm.ecef_ecef_vel, <pdef, &enu_vel);
|
||||||
|
// /** velocity in body frame, wrt ECEF frame */
|
||||||
|
// struct DoubleVect3 body_ecef_vel; /* aka UVW */
|
||||||
|
// /** velocity in ltppprz frame, wrt ECEF frame */
|
||||||
|
fdm.ltpprz_ecef_vel = fdm.ltp_ecef_vel;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void get_acc(PyObject* pacc) {
|
||||||
|
|
||||||
|
struct EnuCoor_d enu_accel = {
|
||||||
|
PyFloat_AsDouble(PyTuple_GetItem(pacc, 0)),
|
||||||
|
PyFloat_AsDouble(PyTuple_GetItem(pacc, 1)),
|
||||||
|
PyFloat_AsDouble(PyTuple_GetItem(pacc, 2)),
|
||||||
|
};
|
||||||
|
|
||||||
|
/** accel in ltppprz frame, wrt ECEF frame */
|
||||||
|
VECT3_NED_OF_ENU(fdm.ltpprz_ecef_accel, enu_accel)
|
||||||
|
/** acceleration in LTP frame, wrt ECEF frame */
|
||||||
|
VECT3_COPY(fdm.ltp_ecef_accel, fdm.ltpprz_ecef_accel)
|
||||||
|
|
||||||
|
/** acceleration in ECEF frame, wrt ECEF frame */
|
||||||
|
ecef_of_enu_vect_d(&fdm.ecef_ecef_accel, <pdef, &enu_accel);
|
||||||
|
|
||||||
|
/** acceleration in body frame as measured by an accelerometer (incl. gravity) */
|
||||||
|
struct DoubleVect3 tmp = {
|
||||||
|
fdm.ltp_ecef_accel.x,
|
||||||
|
fdm.ltp_ecef_accel.y,
|
||||||
|
fdm.ltp_ecef_accel.z - 10,
|
||||||
|
};
|
||||||
|
double_quat_vmult(&fdm.body_accel, &fdm.ltp_to_body_quat, &tmp);
|
||||||
|
|
||||||
|
// /** acceleration in body frame, wrt ECEF frame */
|
||||||
|
// fdm.body_ecef_accel;
|
||||||
|
// /** acceleration in body frame, wrt ECI inertial frame */
|
||||||
|
// fdm.body_inertial_accel;
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
static void get_orient(PyObject* pquat) {
|
||||||
|
// /* attitude */
|
||||||
|
|
||||||
|
struct DoubleQuat enu_quat = {
|
||||||
|
PyFloat_AsDouble(PyTuple_GetItem(pquat, 3)),
|
||||||
|
PyFloat_AsDouble(PyTuple_GetItem(pquat, 1)),
|
||||||
|
PyFloat_AsDouble(PyTuple_GetItem(pquat, 0)),
|
||||||
|
-PyFloat_AsDouble(PyTuple_GetItem(pquat, 2)),
|
||||||
|
};
|
||||||
|
|
||||||
|
double_quat_comp(&fdm.ltpprz_to_body_quat, &enu_quat, &quat_to_pprz);
|
||||||
|
double_eulers_of_quat(&fdm.ltpprz_to_body_eulers, &fdm.ltpprz_to_body_quat);
|
||||||
|
QUAT_COPY(fdm.ltp_to_body_quat, fdm.ltpprz_to_body_quat);
|
||||||
|
EULERS_COPY(fdm.ltp_to_body_eulers, fdm.ltpprz_to_body_eulers);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void get_ang_vel(PyObject* pang_vel) {
|
||||||
|
|
||||||
|
|
||||||
|
struct DoubleVect3 sim_rates, pprz_rates;
|
||||||
|
sim_rates.x = PyFloat_AsDouble(PyTuple_GetItem(pang_vel, 1));
|
||||||
|
sim_rates.y = PyFloat_AsDouble(PyTuple_GetItem(pang_vel, 0));
|
||||||
|
sim_rates.z = -PyFloat_AsDouble(PyTuple_GetItem(pang_vel, 2));
|
||||||
|
|
||||||
|
double_quat_vmult(&pprz_rates, &fdm.ltp_to_body_quat, &sim_rates);
|
||||||
|
|
||||||
|
fdm.body_inertial_rotvel.p = pprz_rates.x;
|
||||||
|
fdm.body_inertial_rotvel.q = pprz_rates.y;
|
||||||
|
fdm.body_inertial_rotvel.r = pprz_rates.z;
|
||||||
|
fdm.body_ecef_rotvel = fdm.body_inertial_rotvel;
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
static void get_ang_acc(PyObject* pang_acc) {
|
||||||
|
|
||||||
|
struct DoubleVect3 sim_rotaccel, pprz_rotaccel;
|
||||||
|
sim_rotaccel.x = PyFloat_AsDouble(PyTuple_GetItem(pang_acc, 1));
|
||||||
|
sim_rotaccel.y = PyFloat_AsDouble(PyTuple_GetItem(pang_acc, 0));
|
||||||
|
sim_rotaccel.z = -PyFloat_AsDouble(PyTuple_GetItem(pang_acc, 2));
|
||||||
|
|
||||||
|
double_quat_vmult(&pprz_rotaccel, &fdm.ltp_to_body_quat, &sim_rotaccel);
|
||||||
|
|
||||||
|
fdm.body_inertial_rotaccel.p = pprz_rotaccel.x;
|
||||||
|
fdm.body_inertial_rotaccel.q = pprz_rotaccel.y;
|
||||||
|
fdm.body_inertial_rotaccel.r = pprz_rotaccel.z;
|
||||||
|
fdm.body_ecef_rotaccel = fdm.body_inertial_rotaccel;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/**************************
|
||||||
|
** Generating LTP plane **
|
||||||
|
**************************/
|
||||||
|
|
||||||
|
static void init_ltp(void)
|
||||||
|
{
|
||||||
|
|
||||||
|
struct LlaCoor_d llh_nav0; /* Height above the ellipsoid */
|
||||||
|
llh_nav0.lat = RadOfDeg((double)NAV_LAT0 / 1e7);
|
||||||
|
llh_nav0.lon = RadOfDeg((double)NAV_LON0 / 1e7);
|
||||||
|
llh_nav0.alt = NAV_ALT0 / 1000.0;
|
||||||
|
|
||||||
|
struct EcefCoor_d ecef_nav0;
|
||||||
|
|
||||||
|
ecef_of_lla_d(&ecef_nav0, &llh_nav0);
|
||||||
|
|
||||||
|
ltp_def_from_ecef_d(<pdef, &ecef_nav0);
|
||||||
|
fdm.ecef_pos = ecef_nav0;
|
||||||
|
|
||||||
|
|
||||||
|
// TODO vérifier tout ça
|
||||||
|
fdm.ltp_g.x = 0.;
|
||||||
|
fdm.ltp_g.y = 0.;
|
||||||
|
fdm.ltp_g.z = 0.; // accel data are already with the correct format
|
||||||
|
|
||||||
|
#ifdef AHRS_H_X
|
||||||
|
fdm.ltp_h.x = AHRS_H_X;
|
||||||
|
fdm.ltp_h.y = AHRS_H_Y;
|
||||||
|
fdm.ltp_h.z = AHRS_H_Z;
|
||||||
|
PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file.")
|
||||||
|
#else
|
||||||
|
fdm.ltp_h.x = 0.4912;
|
||||||
|
fdm.ltp_h.y = 0.1225;
|
||||||
|
fdm.ltp_h.z = 0.8624;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/*****************************************************/
|
||||||
|
// Atmosphere function (we don't need that features) //
|
||||||
|
void nps_fdm_set_wind(double speed __attribute__((unused)),
|
||||||
|
double dir __attribute__((unused)))
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
void nps_fdm_set_wind_ned(double wind_north __attribute__((unused)),
|
||||||
|
double wind_east __attribute__((unused)),
|
||||||
|
double wind_down __attribute__((unused)))
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
void nps_fdm_set_turbulence(double wind_speed __attribute__((unused)),
|
||||||
|
int turbulence_severity __attribute__((unused)))
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
void nps_fdm_set_temperature(double temp __attribute__((unused)),
|
||||||
|
double h __attribute__((unused)))
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
static void python_init(double dt) {
|
||||||
|
PyStatus status;
|
||||||
|
PyConfig config;
|
||||||
|
PyConfig_InitPythonConfig(&config);
|
||||||
|
|
||||||
|
// needed for the prints
|
||||||
|
config.configure_c_stdio = 1;
|
||||||
|
config.buffered_stdio = 0;
|
||||||
|
|
||||||
|
status = PyConfig_SetString(&config, &config.program_name, WIDEN(PYTHON_EXEC));
|
||||||
|
py_check_status(&config, &status);
|
||||||
|
status = Py_InitializeFromConfig(&config);
|
||||||
|
py_check_status(&config, &status);
|
||||||
|
|
||||||
|
// add path to sys.path
|
||||||
|
PyObject *sys_path = PySys_GetObject("path");
|
||||||
|
PyObject *module_path = PyUnicode_FromString( PAPARAZZI_SRC "/sw/simulator/nps/pybullet");
|
||||||
|
PyList_Append(sys_path, module_path);
|
||||||
|
Py_DECREF(module_path);
|
||||||
|
|
||||||
|
fdm_module = PyImport_ImportModule(NPS_PYBULLET_MODULE);
|
||||||
|
py_check(true, __LINE__);
|
||||||
|
|
||||||
|
PyObject* bullet_fdm_class = PyObject_GetAttrString(fdm_module, "BulletFDM");
|
||||||
|
py_check(true, __LINE__);
|
||||||
|
|
||||||
|
PyObject* fdm_ctor_args = Py_BuildValue("(d)", dt);
|
||||||
|
PyObject* fdm_ctor_kwargs = Py_BuildValue("{siss}", "GUI", PYBULLET_GUI, "urdf", NPS_PYBULLET_URDF);
|
||||||
|
bullet_fdm = PyObject_Call(bullet_fdm_class, fdm_ctor_args, fdm_ctor_kwargs);
|
||||||
|
|
||||||
|
py_check(true, __LINE__);
|
||||||
|
Py_DECREF(fdm_ctor_args);
|
||||||
|
Py_DECREF(bullet_fdm_class);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void py_check_status(PyConfig* config, PyStatus* status) {
|
||||||
|
if (PyStatus_Exception(*status)) {
|
||||||
|
printf("error\n");
|
||||||
|
PyConfig_Clear(config);
|
||||||
|
Py_ExitStatusException(*status);
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
static void py_check(bool exit_on_error, int line_nb) {
|
||||||
|
if(PyErr_Occurred()) {
|
||||||
|
printf("Error at line %d\n", line_nb);
|
||||||
|
PyErr_Print();
|
||||||
|
if(exit_on_error) {
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,2 @@
|
|||||||
|
pybullet
|
||||||
|
|
||||||
@@ -0,0 +1,185 @@
|
|||||||
|
import pybullet as p
|
||||||
|
import pybullet_data
|
||||||
|
import numpy as np
|
||||||
|
import time
|
||||||
|
import os
|
||||||
|
|
||||||
|
|
||||||
|
PPRZ_HOME = os.getenv("PAPARAZZI_HOME", os.getcwd())
|
||||||
|
PYBULLET_CONF_PATH = os.path.join(PPRZ_HOME, "conf/simulator/pybullet")
|
||||||
|
|
||||||
|
|
||||||
|
class BulletFDM():
|
||||||
|
def __init__(self, dt=0.02, GUI=True, debug=False, urdf="robobee.urdf"):
|
||||||
|
print(f"Hello from PyBullet ! dt={dt}, GUI={GUI} debug={debug}")
|
||||||
|
|
||||||
|
pprz_src = os.getenv("PAPARAZZI_SRC")
|
||||||
|
print(f"pprz_src = {pprz_src}")
|
||||||
|
|
||||||
|
# self.last_time_print = 0
|
||||||
|
|
||||||
|
# dt : Time step
|
||||||
|
self.dt=dt
|
||||||
|
if GUI:
|
||||||
|
self.physicsClient = p.connect(p.GUI)
|
||||||
|
else:
|
||||||
|
self.physicsClient = p.connect(p.DIRECT)
|
||||||
|
|
||||||
|
# Add path for pybullet assests e.g : plane and grid...
|
||||||
|
p.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||||
|
|
||||||
|
self.planeId = p.loadURDF("plane.urdf")
|
||||||
|
self.textureId = p.loadTexture("checker_grid.jpg")
|
||||||
|
|
||||||
|
self.vehicle_start_pos = [0, 0, 0.2]
|
||||||
|
self.vehicle_start_orientation = p.getQuaternionFromEuler([0, 0, 0])
|
||||||
|
|
||||||
|
vehicule_urdf = os.path.join(PYBULLET_CONF_PATH, urdf)
|
||||||
|
self.vehicle = p.loadURDF(vehicule_urdf, self.vehicle_start_pos, self.vehicle_start_orientation)
|
||||||
|
|
||||||
|
# Bullet physics uses ENU frame
|
||||||
|
p.setGravity(0, 0, -9.81, physicsClientId=self.physicsClient)
|
||||||
|
# Real time simulation should be off in order to apply external force and moments !
|
||||||
|
p.setRealTimeSimulation(0, physicsClientId=self.physicsClient)
|
||||||
|
# dt : Time step
|
||||||
|
p.setTimeStep(self.dt, physicsClientId=self.physicsClient)
|
||||||
|
# Orient the camera if needed
|
||||||
|
p.resetDebugVisualizerCamera( cameraDistance=3.5, cameraYaw=-80, cameraPitch=-40, cameraTargetPosition=[0.0, 0.0, 0.0])
|
||||||
|
# Vehicle properties
|
||||||
|
self.KF = 0.01 ; self.KM = 0.001
|
||||||
|
# Initialte velocity and acceleration vectors
|
||||||
|
self.vel = np.zeros(3)
|
||||||
|
self.accel = np.zeros(3)
|
||||||
|
# Initialte angular velocity and acceleration vectors
|
||||||
|
self.ang_vel = np.zeros(3)
|
||||||
|
self.ang_accel = np.zeros(3)
|
||||||
|
|
||||||
|
# State
|
||||||
|
self.observation = {}
|
||||||
|
|
||||||
|
# For debug purposes
|
||||||
|
if debug:
|
||||||
|
p.setGravity(0, 0, 0, physicsClientId=self.physicsClient)
|
||||||
|
self.roll_Id = p.addUserDebugParameter("roll_cmd", -0.01, 0.01, 0)
|
||||||
|
self.pitch_Id = p.addUserDebugParameter("pitch_cmd", -0.01, 0.01, 0)
|
||||||
|
self.yaw_Id = p.addUserDebugParameter("yaw_cmd", -0.01, 0.01, 0)
|
||||||
|
|
||||||
|
def apply_force_and_moments(self,rpm):
|
||||||
|
''' rpm = [ quadrotor prop rpms ]'''
|
||||||
|
forces = np.array(rpm**2)*self.KF
|
||||||
|
torques = np.array(rpm**2)*self.KM
|
||||||
|
|
||||||
|
f_noise = np.random.normal(0, 0.01, len(rpm))
|
||||||
|
m_noise = np.random.normal(0, 0.001,len(rpm))
|
||||||
|
|
||||||
|
# f_noise = np.zeros(4)
|
||||||
|
# m_noise = np.zeros(4)
|
||||||
|
|
||||||
|
forces += f_noise
|
||||||
|
torques += m_noise
|
||||||
|
|
||||||
|
z_torque = (torques[0] - torques[1] + torques[2] - torques[3])
|
||||||
|
for i in range(4): #FIXME : decide according to commands length
|
||||||
|
p.applyExternalForce(self.vehicle,
|
||||||
|
i,
|
||||||
|
forceObj=[f_noise[0], f_noise[1], forces[i]],
|
||||||
|
posObj=[0, 0, 0],
|
||||||
|
flags=p.LINK_FRAME,
|
||||||
|
physicsClientId=self.physicsClient
|
||||||
|
)
|
||||||
|
p.applyExternalTorque(self.vehicle,
|
||||||
|
-1, # FIXME : this is not correct , use center of mass !!!
|
||||||
|
torqueObj=[m_noise[0], m_noise[1], z_torque],
|
||||||
|
flags=p.LINK_FRAME,
|
||||||
|
physicsClientId=self.physicsClient
|
||||||
|
)
|
||||||
|
|
||||||
|
def step(self,commands):
|
||||||
|
|
||||||
|
if not isinstance(commands, np.ndarray):
|
||||||
|
commands = np.array(commands)
|
||||||
|
|
||||||
|
# little hack to scale commands. It should probably be in the URDF file.
|
||||||
|
commands *= 30
|
||||||
|
|
||||||
|
# t = time.time()
|
||||||
|
# if (t - self.last_time_print) > 0.5:
|
||||||
|
# self.last_time_print = t
|
||||||
|
# print(f"{commands[0]:.2f} {commands[1]:.2f} {commands[2]:.2f} {commands[3]:.2f}")
|
||||||
|
|
||||||
|
self.apply_force_and_moments(commands)
|
||||||
|
p.stepSimulation(physicsClientId=self.physicsClient)
|
||||||
|
return self.get_observation()
|
||||||
|
|
||||||
|
def step_debug(self,commands):
|
||||||
|
# commands are not used...
|
||||||
|
roll_cmd = p.readUserDebugParameter(self.roll_Id)
|
||||||
|
pitch_cmd = p.readUserDebugParameter(self.pitch_Id)
|
||||||
|
yaw_cmd = p.readUserDebugParameter(self.yaw_Id)
|
||||||
|
|
||||||
|
# p.applyExternalForce(self.vehicle,
|
||||||
|
# -1,
|
||||||
|
# forceObj=[0, 0, 9 + yaw_cmd*100],
|
||||||
|
# posObj=[0, 0, 0],
|
||||||
|
# flags=p.LINK_FRAME,
|
||||||
|
# physicsClientId=self.physicsClient
|
||||||
|
# )
|
||||||
|
|
||||||
|
p.applyExternalTorque(self.vehicle,
|
||||||
|
-1, # FIXME : this is not correct , use center of mass !!!
|
||||||
|
torqueObj=[roll_cmd, pitch_cmd, yaw_cmd],
|
||||||
|
flags=p.LINK_FRAME,
|
||||||
|
physicsClientId=self.physicsClient
|
||||||
|
)
|
||||||
|
|
||||||
|
p.stepSimulation(physicsClientId=self.physicsClient)
|
||||||
|
return self.get_observation()
|
||||||
|
|
||||||
|
def get_observation(self):
|
||||||
|
# Get observation
|
||||||
|
v_pos, v_quat = p.getBasePositionAndOrientation(self.vehicle)
|
||||||
|
# print(v_Pos, type(v_Pos))
|
||||||
|
v_rpy = p.getEulerFromQuaternion(v_quat)
|
||||||
|
v_vel, v_ang_v = p.getBaseVelocity(self.vehicle)
|
||||||
|
|
||||||
|
self.accel = (np.array(v_vel) - self.vel)/self.dt
|
||||||
|
self.vel = np.array(v_vel)
|
||||||
|
self.ang_accel = (np.array(v_ang_v) - self.ang_vel)/self.dt
|
||||||
|
self.ang_vel = np.array(v_ang_v)
|
||||||
|
|
||||||
|
self.observation = {'pos':v_pos,
|
||||||
|
'quat':v_quat,
|
||||||
|
'rpy':v_rpy,
|
||||||
|
'vel':tuple(self.vel),
|
||||||
|
'ang_v':tuple(self.ang_vel),
|
||||||
|
'accel':tuple(self.accel),
|
||||||
|
'ang_accel':tuple(self.ang_accel)
|
||||||
|
}
|
||||||
|
return self.observation
|
||||||
|
|
||||||
|
def reset(self):
|
||||||
|
# Reset the simulation and the state
|
||||||
|
# p.resetSimulation(physicsClientId=self.physicsClient) # FIXME : this is not the correct way to reset the simulation
|
||||||
|
p.resetBasePositionAndOrientation(self.vehicle, self.vehicle_start_pos, self.vehicle_start_orientation)
|
||||||
|
p.resetBaseVelocity(self.vehicle, [0, 0, 0], [0, 0, 0])
|
||||||
|
|
||||||
|
return self.get_observation()
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
from time import sleep
|
||||||
|
debug = True
|
||||||
|
m = BulletFDM(GUI=True, debug=debug)
|
||||||
|
|
||||||
|
# An example simulation loop with random commands generation
|
||||||
|
while 1:
|
||||||
|
for i in range(200):
|
||||||
|
# commands = np.array([10., 10., 10., 10.]) # fixed rpms
|
||||||
|
commands = np.random.normal(13., 0.5, 4) # random rpms
|
||||||
|
if debug:
|
||||||
|
m.step_debug(commands)
|
||||||
|
else:
|
||||||
|
m.step(commands)
|
||||||
|
sleep(0.05) # FIXME : checck the computation time and sleep to sync realtime...
|
||||||
|
m.reset()
|
||||||
Reference in New Issue
Block a user