Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Improved: - pocketdrone avoidance flight - stereo protocol only we use - avoidance module that takes distances in meters and angles of these measure devices and outputs a velocity or attitude ref. closes #1462
- Loading branch information
Showing
16 changed files
with
2,077 additions
and
21 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -185,3 +185,5 @@ tests/results/* | |
|
||
*.dSYM | ||
|
||
# K-develop | ||
*.kate-swp |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,24 @@ | ||
<conf> | ||
<aircraft | ||
name="ardrone2" | ||
ac_id="201" | ||
airframe="airframes/TUDelft/airframes/ardrone2_oa_clint_roland.xml" | ||
radio="radios/dummy.xml" | ||
telemetry="telemetry/default_rotorcraft.xml" | ||
flight_plan="flight_plans/rotorcraft_oa_avoid.xml" | ||
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_att_indi.xml] settings/control/stabilization_rate.xml settings/control/stabilization_att_int_quat.xml" | ||
settings_modules="modules/obstacle_avoidance.xml" | ||
gui_color="red" | ||
/> | ||
<aircraft | ||
name="pocketdrone" | ||
ac_id="1" | ||
airframe="airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml" | ||
radio="radios/cockpitSX.xml" | ||
telemetry="telemetry/default_rotorcraft.xml" | ||
flight_plan="flight_plans/rotorcraft_optitrack_stereoavoid.xml" | ||
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml" | ||
settings_modules="" | ||
gui_color="blue" | ||
/> | ||
</conf> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
240 changes: 240 additions & 0 deletions
240
conf/airframes/TUDelft/airframes/ardrone2_oa_clint_roland.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,240 @@ | ||
<!DOCTYPE airframe SYSTEM "../../airframe.dtd"> | ||
|
||
<airframe name="ardrone2_oa_avoid"> | ||
|
||
<firmware name="rotorcraft"> | ||
<target name="ap" board="ardrone2"> | ||
<configure name="USE_BARO_BOARD" value="TRUE"/> | ||
</target> | ||
|
||
<define name="FAILSAFE_DESCENT_SPEED" value="0.5"/> | ||
<define name="USE_SONAR" value="TRUE"/> | ||
<configure name="USE_MAGNETOMETER" value="TRUE"/> | ||
|
||
<!-- Subsystem section --> | ||
<subsystem name="telemetry" type="transparent_udp"/> | ||
<subsystem name="radio_control" type="datalink"/> | ||
<subsystem name="motor_mixing"/> | ||
<subsystem name="actuators" type="ardrone2"/> | ||
<subsystem name="imu" type="ardrone2"/> | ||
<subsystem name="stabilization" type="int_quat"/> | ||
<subsystem name="ahrs" type="int_cmpl_quat"/> | ||
<subsystem name="ins" type="extended"/> | ||
</firmware> | ||
|
||
<modules main_freq="512"> | ||
<load name="bat_voltage_ardrone2.xml"/> | ||
<load name="stereocam.xml"> | ||
<configure name="STEREO_UART" value="UART1"/> | ||
<configure name="STEREO_BAUD" value="B1000000"/> | ||
</load> | ||
<load name="obstacle_avoidance.xml"/> | ||
</modules> | ||
|
||
<commands> | ||
<axis name="PITCH" failsafe_value="0"/> | ||
<axis name="ROLL" failsafe_value="0"/> | ||
<axis name="YAW" failsafe_value="0"/> | ||
<axis name="THRUST" failsafe_value="3000"/> | ||
</commands> | ||
|
||
<servos driver="Default"> | ||
<servo name="TOP_LEFT" no="0" min="0" neutral="1" max="500"/> | ||
<servo name="TOP_RIGHT" no="1" min="0" neutral="1" max="500"/> | ||
<servo name="BOTTOM_RIGHT" no="2" min="0" neutral="1" max="500"/> | ||
<servo name="BOTTOM_LEFT" no="3" min="0" neutral="1" max="500"/> | ||
</servos> | ||
|
||
<section name="MIXING" prefix="MOTOR_MIXING_"> | ||
<define name="TRIM_ROLL" value="0"/> | ||
<define name="TRIM_PITCH" value="0"/> | ||
<define name="TRIM_YAW" value="0"/> | ||
|
||
<!-- Time cross layout (X), with order NW (CW), NE (CCW), SE (CW), SW (CCW) --> | ||
<define name="TYPE" value="QUAD_X"/> | ||
</section> | ||
|
||
<command_laws> | ||
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/> | ||
<set servo="TOP_LEFT" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/> | ||
<set servo="TOP_RIGHT" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/> | ||
<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="IMU" prefix="IMU_"> | ||
<!-- Accelero --> | ||
<define name="ACCEL_X_NEUTRAL" value="2048"/> | ||
<define name="ACCEL_Y_NEUTRAL" value="2048"/> | ||
<define name="ACCEL_Z_NEUTRAL" value="2048"/> | ||
|
||
<!-- Magneto calibration --> | ||
<define name="MAG_X_NEUTRAL" value="0"/> | ||
<define name="MAG_Y_NEUTRAL" value="0"/> | ||
<define name="MAG_Z_NEUTRAL" value="-180"/> | ||
<define name="MAG_X_SENS" value="16." integer="16"/> | ||
<define name="MAG_Y_SENS" value="16." integer="16"/> | ||
<define name="MAG_Z_SENS" value="16." integer="16"/> | ||
|
||
<!-- Magneto current calibration --> | ||
<define name="MAG_X_CURRENT_COEF" value="0.0"/> | ||
<define name="MAG_Y_CURRENT_COEF" value="0.0"/> | ||
<define name="MAG_Z_CURRENT_COEF" value="0.0"/> | ||
|
||
<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> | ||
|
||
<!-- local magnetic field --> | ||
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field --> | ||
<section name="AHRS" prefix="AHRS_"> | ||
<!-- Delft --> | ||
<define name="H_X" value="0.3892503"/> | ||
<define name="H_Y" value="0.0017972"/> | ||
<define name="H_Z" value="0.9211303"/> | ||
</section> | ||
|
||
<section name="INS" prefix="INS_"> | ||
</section> | ||
|
||
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_"> | ||
<!-- setpoints --> | ||
<define name="SP_MAX_P" value="10000"/> | ||
<define name="SP_MAX_Q" value="10000"/> | ||
<define name="SP_MAX_R" value="10000"/> | ||
<define name="DEADBAND_P" value="20"/> | ||
<define name="DEADBAND_Q" value="20"/> | ||
<define name="DEADBAND_R" value="200"/> | ||
|
||
<!-- feedback --> | ||
<define name="GAIN_P" value="400"/> | ||
<define name="GAIN_Q" value="400"/> | ||
<define name="GAIN_R" value="350"/> | ||
|
||
<define name="IGAIN_P" value="75"/> | ||
<define name="IGAIN_Q" value="75"/> | ||
<define name="IGAIN_R" value="50"/> | ||
</section> | ||
|
||
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_"> | ||
<!-- control effectiveness --> | ||
<define name="G1_P" value="0.028551973"/> | ||
<define name="G1_Q" value="0.023775417"/> | ||
<define name="G1_R" value="0.00173069"/> | ||
<define name="G2_R" value="0.086460732"/> | ||
|
||
<!-- reference acceleration for attitude control --> | ||
<define name="REF_ERR_P" value="380.0"/> | ||
<define name="REF_ERR_Q" value="380.0"/> | ||
<define name="REF_ERR_R" value="70.0"/> | ||
<define name="REF_RATE_P" value="21.6"/> | ||
<define name="REF_RATE_Q" value="21.6"/> | ||
<define name="REF_RATE_R" value="11.0"/> | ||
|
||
<!-- second order filter parameters --> | ||
<define name="FILT_OMEGA" value="20.0"/> | ||
<define name="FILT_ZETA" value="0.55"/> | ||
<define name="FILT_OMEGA_R" value="20.0"/> | ||
<define name="FILT_ZETA_R" value="0.55"/> | ||
|
||
<!-- first order actuator dynamics --> | ||
<define name="ACT_DYN_P" value="0.06"/> | ||
<define name="ACT_DYN_Q" value="0.06"/> | ||
<define name="ACT_DYN_R" value="0.06"/> | ||
|
||
<!-- Adaptive Learning Rate --> | ||
<define name="USE_ADAPTIVE" value="FALSE"/> | ||
<define name="ADAPTIVE_MU" value="0.0001"/> | ||
</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"/> | ||
<define name="SP_MAX_R" value="180" unit="deg/s"/> | ||
<define name="DEADBAND_A" value="0"/> | ||
<define name="DEADBAND_E" value="0"/> | ||
<define name="DEADBAND_R" value="250"/> | ||
|
||
<!-- reference --> | ||
<define name="REF_OMEGA_P" value="450" unit="deg/s"/> | ||
<define name="REF_ZETA_P" value="0.9"/> | ||
<define name="REF_MAX_P" value="600." unit="deg/s"/> | ||
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/> | ||
|
||
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/> | ||
<define name="REF_ZETA_Q" value="0.9"/> | ||
<define name="REF_MAX_Q" value="600." unit="deg/s"/> | ||
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/> | ||
|
||
<define name="REF_OMEGA_R" value="450" unit="deg/s"/> | ||
<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="GUIDANCE_V" prefix="GUIDANCE_V_"> | ||
<define name="HOVER_KP" value="583"/> | ||
<define name="HOVER_KD" value="82"/> | ||
<define name="HOVER_KI" value="13"/> | ||
<define name="NOMINAL_HOVER_THROTTLE" value="0.645"/> | ||
<define name="ADAPT_THROTTLE_ENABLED" value="false"/> | ||
</section> | ||
|
||
|
||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_"> | ||
<!-- Good weather --> | ||
<define name="MAX_BANK" value="20" unit="deg"/> | ||
<!-- Bad weather --> | ||
<!-- define name="MAX_BANK" value="32" unit="deg"/ --> | ||
|
||
<define name="PGAIN" value="363"/> | ||
<define name="DGAIN" value="237"/> | ||
<define name="IGAIN" value="30"/> | ||
<define name="VGAIN" value="0"/> | ||
<define name="AGAIN" value="0"/> | ||
</section> | ||
|
||
<section name="SIMULATOR" prefix="NPS_"> | ||
<define name="ACTUATOR_NAMES" value="{"nw_motor", "ne_motor", "se_motor", "sw_motor"}"/> | ||
<define name="JSBSIM_MODEL" value=""simple_ardrone2""/> | ||
<define name="JSBSIM_INIT" value=""reset00""/> | ||
<define name="SENSORS_PARAMS" value=""nps_sensors_params_ardrone2.h""/> | ||
</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"/> | ||
<define name="ARRIVED_AT_WAYPOINT" value="0.15"/> | ||
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="1500"/> | ||
<!-- <define name="NO_RC_THRUST_LIMIT" value="TRUE"/> --> | ||
</section> | ||
|
||
<section name="BAT"> | ||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/> | ||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/> | ||
<define name="CRITIC_BAT_LEVEL" value="9.9" unit="V"/> | ||
<define name="LOW_BAT_LEVEL" value="10.1" unit="V"/> | ||
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/> | ||
</section> | ||
</airframe> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,24 @@ | ||
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd"> | ||
|
||
<flight_plan alt="152" ground_alt="147" lat0="43 33 50.83" lon0="1 28 52.61" max_dist_from_home="150" name="Rotorcraft Basic (Enac)" security_height="2"> | ||
<header> | ||
#include "autopilot.h" | ||
#include "modules/obstacle_avoidance/guidance_OA.h" | ||
</header> | ||
<waypoints> | ||
<waypoint name="HOME" x="0.0" y="0.0"/> | ||
<waypoint name="CLIMB" x="0.0" y="5.0"/> | ||
<waypoint name="STDBY" x="-2.0" y="-5.0"/> | ||
<waypoint name="p1" x="3.6" y="-13.9"/> | ||
<waypoint name="p2" x="27.5" y="-48.2"/> | ||
<waypoint name="p3" x="16.7" y="-19.6"/> | ||
<waypoint name="p4" x="13.7" y="-40.7"/> | ||
<waypoint name="CAM" x="-20" y="-50" height="2."/> | ||
<waypoint name="TD" x="5.6" y="-10.9"/> | ||
</waypoints> | ||
<blocks> | ||
<block name="avoid"> | ||
<attitude pitch="ref_pitch" roll="ref_roll" alt="2.0" vmode="alt" until="FALSE"/> | ||
</block> | ||
</blocks> | ||
</flight_plan> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.