Skip to content

Commit

Permalink
Rotwing State Update: no hardcoded defines in flightplan
Browse files Browse the repository at this point in the history
  • Loading branch information
@dewagter
dewagter committed Dec 13, 2023
1 parent 481a53d commit 20eec22
Show file tree
Hide file tree
Showing 4 changed files with 157 additions and 63 deletions.
86 changes: 34 additions & 52 deletions conf/flight_plans/tudelft/rotating_wing_EHVB.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,6 @@
<waypoint name="end_trans" x="100." y="100."/>
<waypoint name="STDBY" lat="52.1682655" lon="4.4135103"/>
<waypoint name="begin_trans" lat="52.168412" lon="4.4149282"/>
<!--<waypoint name="p1" lat="52.1674262" lon="4.4141448"/>-->
<waypoint name="p1" lat="52.1672408" lon="4.4144528"/>
<waypoint name="p2" lat="52.1682897" lon="4.4138441"/>
<waypoint name="p3" lat="52.1687878" lon="4.4155648"/>
Expand Down Expand Up @@ -43,36 +42,38 @@
</sector>
</sectors>
<modules>
<!--<module name="follow_me"/>-->
<!-- <module name="follow_me">
<define name="FOLLOW_ME_MOVING_WPS" value="WP_p1"/>
</module> -->
</modules>
<exceptions>
<exception cond="Or(!InsideFlyzone(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 1500) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/>
<exception cond="datalink_time @GT 25 @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Standby'))" deroute="Standby"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block>
<block name="Geo init">
<call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="NavKillThrottle()"/>
<set var="stabilization_cmd[COMMAND_THRUST_X]" value="0"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine" strip_button="Start Engines" strip_icon="on.png">
<call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time>10" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<exception cond="agl_dist_valid @AND (agl_dist_value @GT 1.0)" deroute="Climb"/>
<call_once fun="autopilot_set_in_flight(true)"/><!-- this is a hack to solve INDI not being active fast enough -->

Expand All @@ -81,131 +82,112 @@
</block>
<block name="Climb">
<exception cond="GetPosAlt() @GT 20.0" deroute="Standby"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="nav_set_heading_current()"/>
<stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<set value="7" var="nav_max_speed"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay wp="STDBY" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="Approach APP">
<call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block>
<!--<block name="follow_module">
<set value="false" var="force_forward"/>
<stay pre_call="follow_me_set_wp(WP_FOLLOW, 0)" wp="FOLLOW"/>
</block>-->
<block name="stay_begin-trans">
<set value="7" var="nav_max_speed"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
<!-- <block name="follow_module">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay pre_call="follow_me_set_wp(WP_p1, 0)" wp="p1"/>
</block> -->
<block name="stay_begin-trans">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay wp="begin_trans" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="line_trans_fwd">
<set value="50" var="nav_max_speed"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_SKEWING)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
<call_once fun="NavSetWaypointHere(WP_begin_trans)"/>
<go wp="end_trans"/>
<deroute block="end_transition"/>
</block>
<block name="end_transition">
<set value="7" var="nav_max_speed"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay wp="end_trans"/>
</block>
<block name="transition_CW">
<set value="50" var="nav_max_speed"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_SKEWING)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
<stay wp="end_trans"/>
<exception cond="RotWingAutomationReadyForForward()" deroute="Circle_CW_fwd"/>
</block>
<block name="transition_CCW">
<set value="50" var="nav_max_speed"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_SKEWING)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
<stay wp="end_trans"/>
<exception cond="RotWingAutomationReadyForForward()" deroute="Circle_CCW_fwd"/>
</block>
<block name="route">
<call_once fun="request_rotwing_state(ROTWING_STATE_FW)"/>
<set value="50" var="nav_max_speed"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<go from="p1" hmode="route" wp="p2"/>
<go from="p2" hmode="route" wp="p3"/>
<go from="p3" hmode="route" wp="p4"/>
<go from="p4" hmode="route" wp="p1"/>
<deroute block="route"/>
</block>
<block name="small_route" strip_button="Route" strip_icon="path.png">
<set value="50" var="nav_max_speed"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_FW)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<go wp="p2"/>
<go wp="p3"/>
<go wp="p4"/>
<go wp="p1"/>
<deroute block="small_route"/>
</block>
<block name="Circle_CW_fwd" strip_button="CircleRight" strip_icon="circle-right.png">
<set value="50" var="nav_max_speed"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_FW_HOV_MOT_OFF)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<circle radius="100" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="Circle_CCW_fwd" strip_button="CircleLeft" strip_icon="circle-left.png">
<set value="50" var="nav_max_speed"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_FW_HOV_MOT_OFF)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<circle radius="-100" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="big_Circle_CW_fwd">
<set value="50" var="nav_max_speed"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_FW_HOV_MOT_IDLE)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<circle radius="150" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="big_Circle_CCW_fwd">
<set value="50" var="nav_max_speed"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_FW_HOV_MOT_IDLE)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<circle radius="-150" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="Transition_quad">
<set value="50" var="nav_max_speed"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_SKEWING)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
<go wp="STDBY"/>
<deroute block="Standby"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<set value="7" var="nav_max_speed"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land">
<set value="7" var="nav_max_speed"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<go wp="TD"/>
</block>
<block name="descend">
<set value="7" var="nav_max_speed"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<exception cond="GetPosAlt() @LT 12.0" deroute="flare"/>
<stay climb="-1.0" vmode="climb" wp="TD"/>
</block>
<block name="flare">
<set value="7" var="nav_max_speed"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
<!--<exception cond="!(GetPosAlt() @LT 2.0)" deroute="flare_low"/>-->
<exception cond="agl_dist_valid @AND (agl_dist_value @LT 0.28)" deroute="flare_low"/>
</block>
<block name="flare_low">
<set value="7" var="nav_max_speed"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="Holding point"/>
<exception cond="ground_detect()" deroute="Holding point"/>
<call_once fun="NavStartDetectGround()"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
</block>
<block name="landed">
<set value="7" var="nav_max_speed"/>
<call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
Expand Down
1 change: 1 addition & 0 deletions conf/modules/rotwing_state.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
<dl_settings NAME="RotWingState">
<dl_setting var="rotwing_state_skewing.wing_angle_deg_sp" min="0" step="1" max="90" shortname="skew angle"/>
<dl_setting var="rotwing_state_skewing.force_rotation_angle" min="0" step="1" max="1" values="FALSE|TRUE" shortname="force_skew"/>
<dl_setting var="rotwing_state_max_hover_speed" min="5" step="0.5" max="25" shortname="hover_speed"/>
<dl_setting var="hover_motors_active" min="0" step="1" max="1" values="FALSE|TRUE" shortname="h_motors_active"/>
<dl_setting var="bool_disable_hover_motors" min="0" step="1" max="1" values="FALSE|TRUE" shortname="h_motors_disable"/>
</dl_settings>
Expand Down

0 comments on commit 20eec22

Please sign in to comment.