mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-21 03:43:26 +08:00
Created a default Bebop aircraft using opticflow and full INDI (#2458)
This commit is contained in:
Daniel Willemsen
committed by
Gautier Hattenberger
Gautier Hattenberger
parent
23b159b12b
commit
ca8c70ebea
@@ -0,0 +1,199 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<airframe name="bebop_optitrack">
|
||||
<description>
|
||||
Airframe file to fly a Bebop1 drone, in stable smooth flight, through waypoints using opticflow. As this airframe does not use a magnetometer nor GPS for heading determination, all positions and angles are relative to its starting position. Therefore; the drone should be facing north when started. The values are tuned so that the flight speed is limited and the drone does not over-shoot the waypoints as it would in outdoor settings.
|
||||
</description>
|
||||
<firmware name="rotorcraft">
|
||||
<target name="ap" board="bebop"/>
|
||||
<define name="USE_SONAR"/>
|
||||
<target name="nps" board="pc">
|
||||
<module name="fdm" type="jsbsim"/>
|
||||
<module name="udp"/>
|
||||
</target>
|
||||
|
||||
<!-- Subsystem section -->
|
||||
<module name="telemetry" type="transparent_udp"/>
|
||||
<module name="radio_control" type="datalink"/>
|
||||
<module name="actuators" type="bebop"/>
|
||||
<module name="imu" type="bebop"/>
|
||||
<module name="stabilization" type="indi">
|
||||
<define name="INDI_RPM_FEEDBACK" value="TRUE"/>
|
||||
</module>
|
||||
<module name="ahrs" type="int_cmpl_quat">
|
||||
<configure name="USE_MAGNETOMETER" value="FALSE"/>
|
||||
</module>
|
||||
<module name="ins" type="hff_extended"/>
|
||||
|
||||
<module name="pose_history"/>
|
||||
<module name="bebop_cam"/>
|
||||
<module name="cv_opticflow">
|
||||
<define name="OPTICFLOW_CAMERA" value="bottom_camera"/>
|
||||
<define name="MAX_HORIZON" value="10"/>
|
||||
<define name="OPTICFLOW_MAX_TRACK_CORNERS" value="10"/>
|
||||
<define name="OPTICFLOW_SUBPIXEL_FACTOR" value = "200"/> <!-- due to high framerate of the bebop bottom camera, a high subpixel factor is needed-->
|
||||
<define name="OPTICFLOW_CORNER_METHOD" value="0"/>
|
||||
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X" value="0.8"/> <!--Obtained from a linefit-->
|
||||
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y" value="0.85"/> <!--Obtained from a linefit-->
|
||||
<define name="MAX_TRACK_CORNERS" value="10"/>
|
||||
</module>
|
||||
|
||||
<module name="video_capture">
|
||||
<define name="VIDEO_CAPTURE_CAMERA" value="bottom_camera"/>
|
||||
<define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
|
||||
</module>
|
||||
|
||||
<module name="video_rtp_stream">
|
||||
<define name="VIEWVIDEO_CAMERA" value="bottom_camera"/>
|
||||
<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
|
||||
<define name="VIEWVIDEO_QUALITY_FACTOR" value="40"/>
|
||||
</module>
|
||||
</firmware>
|
||||
|
||||
<commands>
|
||||
<axis name="PITCH" failsafe_value="0"/>
|
||||
<axis name="ROLL" failsafe_value="0"/>
|
||||
<axis name="YAW" failsafe_value="0"/>
|
||||
<axis name="THRUST" failsafe_value="6000"/>
|
||||
</commands>
|
||||
|
||||
<servos driver="Default">
|
||||
<servo name="TOP_LEFT" no="0" min="1000" neutral="1000" max="9800"/>
|
||||
<servo name="TOP_RIGHT" no="1" min="1000" neutral="1000" max="9800"/>
|
||||
<servo name="BOTTOM_RIGHT" no="2" min="1000" neutral="1000" max="9800"/>
|
||||
<servo name="BOTTOM_LEFT" no="3" min="1000" neutral="1000" max="9800"/>
|
||||
</servos>
|
||||
|
||||
<command_laws>
|
||||
<set servo="TOP_LEFT" value="autopilot_get_motors_on() ? actuators_pprz[0] : -MAX_PPRZ"/>
|
||||
<set servo="TOP_RIGHT" value="autopilot_get_motors_on() ? actuators_pprz[1] : -MAX_PPRZ"/>
|
||||
<set servo="BOTTOM_RIGHT" value="autopilot_get_motors_on() ? actuators_pprz[2] : -MAX_PPRZ"/>
|
||||
<set servo="BOTTOM_LEFT" value="autopilot_get_motors_on() ? actuators_pprz[3] : -MAX_PPRZ"/>
|
||||
</command_laws>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<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="INS" prefix="INS_">
|
||||
<define name="SONAR_MAX_RANGE" value="2.2"/>
|
||||
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="RC_SETPOINT" 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="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
|
||||
<!-- attitude reference generation model -->
|
||||
<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.)"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||
<!-- Control effectiveness, have been determined by flying test flights with adaptive turned on./-->
|
||||
<define name="G1_ROLL" value="{11.0, -11.0, -11.0 , 11.0 }"/>
|
||||
<define name="G1_PITCH" value="{8.5 , 8.5, -8.5 , -8.5}"/>
|
||||
<define name="G1_YAW" value="{-0.50, 0.50, -0.50, 0.50}"/>
|
||||
<define name="G1_THRUST" value="{-.40, -.40, -.40, -.40}"/>
|
||||
<!--Counter torque effect of spinning up a rotor-->
|
||||
<define name="G2" value="{-48.0, 48.0, -48.0, 48.0}"/>
|
||||
|
||||
<!-- reference acceleration for attitude control -->
|
||||
<define name="REF_ERR_P" value="400.0"/>
|
||||
<define name="REF_ERR_Q" value="400.0"/>
|
||||
<define name="REF_ERR_R" value="400.0"/>
|
||||
<define name="REF_RATE_P" value="20.0"/>
|
||||
<define name="REF_RATE_Q" value="20.0"/>
|
||||
<define name="REF_RATE_R" value="20.0"/>
|
||||
|
||||
<define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
|
||||
<define name="FILT_CUTOFF" value="5.0"/>
|
||||
|
||||
<!-- first order actuator dynamics -->
|
||||
<define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}"/>
|
||||
|
||||
<!-- Adaptive Learning Rate -->
|
||||
<define name="USE_ADAPTIVE" value="FALSE"/>
|
||||
<define name="ADAPTIVE_MU" value="0.00001"/>
|
||||
|
||||
<!--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="HOVER_KP" value="500"/>
|
||||
<define name="HOVER_KD" value="100"/>
|
||||
<define name="HOVER_KI" value="30"/>
|
||||
<define name="NOMINAL_HOVER_THROTTLE" value="0.68"/>
|
||||
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<define name="MAX_BANK" value="20" unit="deg"/>
|
||||
<define name="REF_MAX_SPEED" value="0.7" unit="m/s"/>
|
||||
<define name="PGAIN" value="120"/>
|
||||
<define name="DGAIN" value="100"/>
|
||||
<define name="IGAIN" value="30"/>
|
||||
<define name="REF_ZETA" value="1.0"/>
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
<define name="ARRIVED_AT_WAYPOINT" value="0.4" unit="m"/> <!-- how far away it declares that the waypoint is reached -->
|
||||
<define name="THRESHOLD_GROUND_DETECT" value="40.0"/> <!-- to prevent false ground detections-->
|
||||
</section>
|
||||
|
||||
<section name="NAVIGATION" prefix="NAV_">
|
||||
<define name="CLIMB_VSPEED" value="0.5" />
|
||||
<define name="DESCEND_VSPEED" value="-1.0" />
|
||||
</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"/>
|
||||
</section>
|
||||
<include href="conf/simulator/gazebo/airframes/bebop.xml"/>
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
<define name="NO_GPS_NEEDED_FOR_NAV" value="TRUE"/> <!-- needed to be able to fly the drone in NAV mode without a GPS fix -->
|
||||
<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="BAT">
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.6" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="9.9" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="10.4" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
|
||||
</section>
|
||||
|
||||
<section name="OPTICFLOW" prefix="OPTICFLOW_">
|
||||
<define name="CORNER_METHOD" value="0"/>
|
||||
<define name="MAX_TRACK_CORNERS" value="10"/>
|
||||
</section>
|
||||
|
||||
</airframe>
|
||||
@@ -0,0 +1,93 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="1.5" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="20" name="Rotorcraft Cyberzoo (Delft) no GPS" security_height="0.3">
|
||||
<header>
|
||||
#include "autopilot.h"
|
||||
</header>
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="0.0" y="0.0"/>
|
||||
<waypoint name="CLIMB" x="1.2" y="-0.6"/>
|
||||
<waypoint name="GOAL" x="2.0" y="2.0"/>
|
||||
<waypoint name="STDBY" x="-0.7" y="-0.8"/>
|
||||
<waypoint name="TD" x="0.8" y="-1.7"/>
|
||||
<waypoint lat="51.990630" lon="4.376823" name="p1"/>
|
||||
<waypoint lat="51.990614" lon="4.376779" name="p2"/>
|
||||
<waypoint lat="51.990636" lon="4.376759" name="p3"/>
|
||||
<waypoint lat="51.990651" lon="4.376805" name="p4"/>
|
||||
<waypoint lat="51.9906213" lon="4.3768628" name="FA1"/>
|
||||
<waypoint lat="51.9905874" lon="4.3767766" name="FA2"/>
|
||||
<waypoint lat="51.9906409" lon="4.3767226" name="FA3"/>
|
||||
<waypoint lat="51.9906737" lon="4.3768074" name="FA4"/>
|
||||
</waypoints>
|
||||
<sectors>
|
||||
<sector color="red" name="Flight_Area">
|
||||
<corner name="FA4"/>
|
||||
<corner name="FA3"/>
|
||||
<corner name="FA2"/>
|
||||
<corner name="FA1"/>
|
||||
</sector>
|
||||
</sectors>
|
||||
<blocks>
|
||||
<block name="Holding point">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="Start Engine">
|
||||
<call_once fun="NavResurrect()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
|
||||
<exception cond="stateGetPositionEnu_f()->z > 1.0" deroute="Standby"/>
|
||||
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
|
||||
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
|
||||
</block>
|
||||
<block name="Standby" strip_button="Standby" strip_icon="home.png">
|
||||
<stay wp="STDBY"/>
|
||||
</block>
|
||||
<block name="START" strip_button="Go" strip_icon="lookfore.png">
|
||||
<call_once fun="NavSetWaypointHere(WP_GOAL)"/>
|
||||
</block>
|
||||
<block name="StayGoal">
|
||||
<stay wp="GOAL"/>
|
||||
</block>
|
||||
<block name="stay_p1">
|
||||
<stay wp="p1"/>
|
||||
</block>
|
||||
<block name="go_p2">
|
||||
<call_once fun="nav_set_heading_deg(90)"/>
|
||||
<go wp="p2"/>
|
||||
<deroute block="stay_p1"/>
|
||||
</block>
|
||||
<block name="line_p1_p2">
|
||||
<go from="p1" hmode="route" wp="p2"/>
|
||||
<stay until="stage_time>10" wp="p2"/>
|
||||
<go from="p2" hmode="route" wp="p1"/>
|
||||
<deroute block="stay_p1"/>
|
||||
</block>
|
||||
<block name="route">
|
||||
<go from="p1" hmode="route" wp="p3"/>
|
||||
<go from="p3" hmode="route" wp="p4"/>
|
||||
<go from="p4" hmode="route" wp="p1"/>
|
||||
<deroute block="stay_p1"/>
|
||||
</block>
|
||||
<block name="Oval">
|
||||
<oval p1="p1" p2="p2" radius="-1"/>
|
||||
</block>
|
||||
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
|
||||
<call_once fun="NavSetWaypointHere(WP_TD)"/>
|
||||
</block>
|
||||
<block name="land">
|
||||
<go wp="TD"/>
|
||||
</block>
|
||||
<block name="flare">
|
||||
<exception cond="NavDetectGround()" deroute="Holding point"/>
|
||||
<exception cond="!nav_is_in_flight()" deroute="landed"/>
|
||||
<call_once fun="NavStartDetectGround()"/>
|
||||
<stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
|
||||
</block>
|
||||
<block name="landed">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -486,6 +486,18 @@
|
||||
settings_modules="modules/air_data.xml modules/geo_mag.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/ahrs_float_mlkf.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
|
||||
gui_color="#7d7fd7e7ffff"
|
||||
/>
|
||||
<aircraft
|
||||
name="bebop_opticflow_indoor"
|
||||
ac_id="97"
|
||||
airframe="airframes/tudelft/bebop_opticflow_indoor.xml"
|
||||
radio="radios/dummy.xml"
|
||||
telemetry="telemetry/default_rotorcraft.xml"
|
||||
flight_plan="flight_plans/tudelft/rotorcraft_cyberzoo_no_gps.xml"
|
||||
settings="settings/rotorcraft_basic.xml"
|
||||
settings_modules="modules/video_rtp_stream.xml modules/video_capture.xml modules/cv_opticflow.xml modules/bebop_cam.xml modules/ins_hff_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml"
|
||||
gui_color="blue"
|
||||
release="398071ca8f7c8dc24becbe76bc5628b6ea604248"
|
||||
/>
|
||||
<aircraft
|
||||
name="fan_demo"
|
||||
ac_id="28"
|
||||
|
||||
@@ -37,10 +37,10 @@
|
||||
|
||||
// parameters for undistortion, defaults are rough estimates
|
||||
#ifndef MT9V117_FOCAL_X
|
||||
#define MT9V117_FOCAL_X 349.f
|
||||
#define MT9V117_FOCAL_X 307.f
|
||||
#endif
|
||||
#ifndef MT9V117_FOCAL_Y
|
||||
#define MT9V117_FOCAL_Y 349.f
|
||||
#define MT9V117_FOCAL_Y 307.f
|
||||
#endif
|
||||
#ifndef MT9V117_CENTER_X
|
||||
#define MT9V117_CENTER_X 120.f
|
||||
|
||||
@@ -605,7 +605,7 @@ static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
|
||||
if (last_stamp_x > 0)
|
||||
{
|
||||
float dt = (float)(stamp - last_stamp_x) * 1e-6;
|
||||
ins_int.ltp_pos.x += POS_BFP_OF_REAL(dt * vel_ned.x);
|
||||
ins_int.ltp_pos.x += lround(POS_BFP_OF_REAL(dt * vel_ned.x));
|
||||
}
|
||||
last_stamp_x = stamp;
|
||||
}
|
||||
@@ -615,7 +615,7 @@ static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
|
||||
if (last_stamp_y > 0)
|
||||
{
|
||||
float dt = (float)(stamp - last_stamp_y) * 1e-6;
|
||||
ins_int.ltp_pos.y += POS_BFP_OF_REAL(dt * vel_ned.y);
|
||||
ins_int.ltp_pos.y += lround(POS_BFP_OF_REAL(dt * vel_ned.y));
|
||||
}
|
||||
last_stamp_y = stamp;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user