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

Implement classic GVF for rotorcrafts (#3327)

Browse Source 
* [modules] implemented GVF for rotorcrafts

* Note about Crazyradio communication with Crazyflies
This commit is contained in:
Angel-HF
2024-07-12 17:44:01 +02:00
committed by GitHub
parent 72fe3fb95a
commit e089fd48f7
8 changed files with 374 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/conf_example.xml
+11
View File
@@ -109,6 +109,17 @@
settings_modules="modules/air_data.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml"
gui_color="red"
/>
<aircraft
name="bebop2_gvf"
ac_id="204"
airframe="airframes/examples/bebop2_gvf.xml"
radio="radios/dummy.xml"
telemetry="telemetry/GVF/gvf_default_rotorcraft.xml"
flight_plan="flight_plans/demo_gvf_rotorcraft.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
settings_modules="modules/air_data.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gvf_module.xml~GVF~ modules/imu_common.xml modules/ins_ekf2.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml"
gui_color="red"
/>
<aircraft
name="bebop2_opticflow"
ac_id="3"
conf/flight_plans/demo_gvf_rotorcraft.xml
+88
View File
@@ -0,0 +1,88 @@
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
<flight_plan alt="720.5" ground_alt="720" lat0="37.296981" lon0="-3.683133" max_dist_from_home="200" name="Rotorcraft GVF" security_height="0.1">
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
<waypoint name="STDBY" x="0.1" y="0.8"/>
<waypoint name="S1_START" x="0.0" y="1.0"/>
<waypoint name="S1_END" x="0.0" y="5.0"/>
<waypoint name="S2_START" x="2.0" y="1.0"/>
<waypoint name="S2_END" x="2.0" y="5.0"/>
<waypoint name="TD" x="-1.0" y="1.5"/>
</waypoints>
<variables>
<variable init="0.1" var="fp_throttle"/>
<variable var="desired_heading" init="0.0f" type="float" min="0." max="10." step="0.1"/>
<variable var="nominal_alt" init="0.5" type="float" min="0." max="10." step="0.1"/>
</variables>
<modules>
<module name="gvf_module"/>
<!-- module name="gvf_parametric"/--> <!-- Not implemented yet -->
</modules>
<blocks>
<block name="FPInit">
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
<while cond="TRUE"/>
</block>
<block name="Holding point">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine" strip_button="Start Engine" strip_icon="resurrect.png" group="home">
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Take off" strip_button="Takeoff" strip_icon="takeoff.png" group="home">
<call_once fun="NavVerticalAltitudeMode(nominal_alt, 0.0)"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
<stay wp="STDBY"/>
</block>
<block name="GVF Segment 1">
<call fun="gvf_segment_loop_wp1_wp2(WP_S1_START, WP_S1_END, gvf_segment_par.d1, gvf_segment_par.d2)"/>
</block>
<block name="GVF Segment 2">
<call fun="gvf_segment_loop_wp1_wp2(WP_S2_START, WP_S2_END, gvf_segment_par.d1, gvf_segment_par.d2)"/>
</block>
<block name="GVF 25m Circle">
<call fun="gvf_ellipse_wp(WP_TD, 25, 25, gvf_ellipse_par.alpha)"/>
</block>
<!-- Function found in ./sw/airborne/modules/guidance/gfv/gvf.h -->
<block name="GVF Ellipse">
<call fun="gvf_ellipse_wp(WP_TD, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
</block>
<block name="Land here" key="l" 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="Land Target" strip_button="Land Target" group="target">
<exception cond="!nav_is_in_flight()" deroute="Kill Engine"/>
<stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="Kill Engine" key="k">
<call_once fun="NavKillThrottle()"/>
<while cond="1"/>
</block>
</blocks>
</flight_plan>
conf/modules/gvf_module.xml
+4 -2
View File
@@ -33,12 +33,14 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
<dl_settings NAME="GVF">
<dl_settings NAME="Control">
<dl_setting MAX="1" MIN="-1" STEP="2" VAR="gvf_control.s" shortname = "direction"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="gvf_control.align" shortname = "(ROTORCRAFT) align with trajectory"/>
<dl_setting MAX="20" MIN="0.0" STEP="0.2" VAR="gvf_control.speed" shortname = "(ROTORCRAFT) speed"/>
</dl_settings>
<dl_settings NAME="Ellipse">
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_ellipse_par.ke" shortname="ell_ke" param="GVF_ELLIPSE_KE"/>
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_ellipse_par.kn" shortname="ell_kn" param="GVF_ELLIPSE_KN"/>
<dl_setting MAX="150" MIN="0.0" STEP="10" VAR="gvf_ellipse_par.a" shortname="ell_a" param="GVF_ELLIPSE_A"/>
<dl_setting MAX="150" MIN="0.0" STEP="10" VAR="gvf_ellipse_par.b" shortname="ell_b" param="GVF_ELLIPSE_B"/>
<dl_setting MAX="150" MIN="0.0" STEP="5" VAR="gvf_ellipse_par.a" shortname="ell_a" param="GVF_ELLIPSE_A"/>
<dl_setting MAX="150" MIN="0.0" STEP="5" VAR="gvf_ellipse_par.b" shortname="ell_b" param="GVF_ELLIPSE_B"/>
<dl_setting MAX="90" MIN="-90" STEP="1" VAR="gvf_ellipse_par.alpha" shortname="ell_alpha" param="GVF_ELLIPSE_ALPHA"/>
</dl_settings>
<dl_settings NAME="Line">
conf/modules/nav_rotorcraft.xml
+1 -1
View File
@@ -9,7 +9,7 @@
<settings>
<dl_settings>
<dl_settings NAME="NAV">
<dl_setting var="flight_altitude" MIN="0" STEP="0.1" MAX="400" module="navigation" unit="m" handler="SetFlightAltitude"/>
<dl_setting var="flight_altitude" MIN="0" STEP="0.1" MAX="999" module="navigation" unit="m" handler="SetFlightAltitude"/>
<dl_setting var="nav.heading" MIN="0" STEP="0.1" MAX="360" unit="rad" alt_unit="deg"/>
<dl_setting var="nav.radius" MIN="-50" STEP="0.1" MAX="50" unit="m"/>
<dl_setting var="nav.climb_vspeed" MIN="0" STEP="0.1" MAX="10.0" unit="m/s" param="NAV_CLIMB_VSPEED"/>
conf/telemetry/GVF/gvf_default_rotorcraft.xml
+216
View File
@@ -0,0 +1,216 @@
<?xml version="1.0"?>
<!DOCTYPE telemetry SYSTEM "../telemetry.dtd">
<telemetry>
<process name="Main">
<mode name="default" key_press="d">
<message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_FP" period="0.25"/>
<message name="ALIVE" period="2.1"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="ROTORCRAFT_CAM" period="1."/>
<message name="GPS_INT" period=".25"/>
<message name="INS" period=".25"/>
<message name="GVF" period="3."/>
<message name="GVF_PARAMETRIC" period="0.5"/>
<message name="I2C_ERRORS" period="4.1"/>
<message name="UART_ERRORS" period="3.1"/>
<message name="SUPERBITRF" period="3"/>
<message name="ENERGY" period="2.5"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
<message name="AIR_DATA" period="1.3"/>
<message name="SURVEY" period="2.5"/>
<message name="OPTIC_FLOW_EST" period="0.05"/>
<message name="VECTORNAV_INFO" period="0.5"/>
<message name="OPTICAL_FLOW_HOVER" period="0.05"/>
<message name="VISUALTARGET" period="0.10"/>
<message name="VISION_POSITION_ESTIMATE" period="0.1"/>
<message name="DIVERGENCE" period="0.05"/>
<message name="DRAGSPEED" period="0.02"/>
<message name="LOGGER_STATUS" period="5.1"/>
<message name="LIDAR" period="1.2"/>
<message name="INS_EKF2" period=".25"/>
<message name="WIND_INFO_RET" period="1."/>
<message name="AHRS_REF_QUAT" period="0.5"/>
<message name="STAB_ATTITUDE" period=".25"/>
<message name="EFF_MAT_G" period="2.0"/>
</mode>
<mode name="ppm">
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="ROTORCRAFT_CMD" period=".05"/>
<message name="PPM" period="0.5"/>
<message name="RC" period="0.5"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.5"/>
<message name="ROTORCRAFT_STATUS" period="1"/>
<message name="BEBOP_ACTUATORS" period="0.2"/>
</mode>
<mode name="raw_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_ACCEL_RAW" period=".05"/>
<message name="IMU_GYRO_RAW" period=".05"/>
<message name="IMU_MAG_RAW" period=".05"/>
<message name="BARO_RAW" period=".1"/>
<message name="IMU_MAG_CURRENT_CALIBRATION" period=".05"/>
<message name="ARDRONE_NAVDATA" period=".05"/>
<message name="AIRSPEED_RAW" period="0.1"/>
</mode>
<mode name="scaled_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_GYRO_SCALED" period=".075"/>
<message name="IMU_ACCEL_SCALED" period=".075"/>
<message name="IMU_MAG_SCALED" period=".1"/>
</mode>
<mode name="ahrs">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="FILTER_ALIGNER" period="2.2"/>
<message name="FILTER" period=".5"/>
<message name="GEO_MAG" period="5."/>
<message name="AHRS_GYRO_BIAS_INT" period="0.08"/>
<message name="AHRS_QUAT_INT" period=".25"/>
<message name="AHRS_EULER_INT" period=".1"/>
<message name="AHRS_EULER" period=".1"/>
<!-- <message name="AHRS_RMAT_INT" period=".5"/> -->
</mode>
<mode name="rate_loop">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="RATE_LOOP" period=".02"/>
</mode>
<mode name="attitude_setpoint_viz" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.1"/>
<message name="AHRS_REF_QUAT" period="0.05"/>
</mode>
<mode name="attitude_loop" key_press="a">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="STAB_ATTITUDE_INT" period=".03"/>
<message name="STAB_ATTITUDE_REF_INT" period=".03"/>
<message name="STAB_ATTITUDE_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_REF_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE" period=".25"/>
<message name="EFF_MAT_G" period="2.0"/>
</mode>
<mode name="vert_loop" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="VFF" period=".05"/>
<message name="VFF_EXTENDED" period=".05"/>
<message name="VERT_LOOP" period=".05"/>
<message name="INS_Z" period=".05"/>
<message name="INS" period=".11"/>
<message name="INS_REF" period="5.1"/>
</mode>
<mode name="vel_guidance" key_press="q">
<message name="ALIVE" period="0.9"/>
<message name="HYBRID_GUIDANCE" period="0.062"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.1"/>
<!--<message name="STAB_ATTITUDE_REF" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="INS_REF" period="5.1"/>
<message name="WP_MOVED" period="1.3"/>
<message name="GPS_INT" period="1.0"/>
<message name="INS" period="1.0"/>
</mode>
<mode name="h_loop" key_press="h">
<message name="ALIVE" period="0.9"/>
<message name="HOVER_LOOP" period="0.062"/>
<message name="GUIDANCE_H_REF_INT" period="0.062"/>
<message name="STAB_ATTITUDE_INT" period="0.4"/>
<message name="STAB_ATTITUDE_FLOAT" period="0.4"/>
<!--<message name="STAB_ATTITUDE_REF_INT" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="INS_REF" period="5.1"/>
<!-- HFF messages are only sent if USE_HFF -->
<message name="HFF" period=".05"/>
<message name="HFF_GPS" period=".03"/>
<message name="HFF_DBG" period=".2"/>
</mode>
<mode name="aligner">
<message name="ALIVE" period="0.9"/>
<message name="FILTER_ALIGNER" period="0.02"/>
</mode>
<mode name="tune_hover">
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ALIVE" period="2.1"/>
<message name="GUIDANCE_H_INT" period="0.05"/>
<message name="ROTORCRAFT_TUNE_HOVER" period=".1"/>
<!-- <message name="GPS_INT" period=".20"/> -->
<!--<message name="INS2" period=".05"/>
<message name="INS3" period=".20"/>-->
<message name="INS_REF" period="5.1"/>
</mode>
<mode name="RTCM3" >
<message name="GPS_RXMRTCM" period="1"/>
<message name="GPS_INT" period=".25"/>
<message name="GPS_RTK" period="1"/>
</mode>
</process>
<process name="FlightRecorder">
<mode name="default">
<message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_FP" period="0.25"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="GPS_INT" period=".1"/>
<message name="INS" period=".1"/>
<message name="I2C_ERRORS" period="4.1"/>
<message name="UART_ERRORS" period="3.1"/>
<message name="ENERGY" period="2.5"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
<message name="AIR_DATA" period="1.3"/>
<message name="SURVEY" period="2.5"/>
<message name="IMU_GYRO_SCALED" period=".075"/>
<message name="IMU_ACCEL_SCALED" period=".075"/>
<message name="IMU_MAG_SCALED" period=".2"/>
<message name="LIDAR" period="0.05"/>
<message name="ESC" period="0.05"/>
<message name="STAB_ATTITUDE" period=".1"/>
</mode>
</process>
</telemetry>