Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 14:18:00 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[rotwing] Fly back to standby in fixed wing. Gih add setters for max bank, climb, and descend speed (#3452)
Doxygen / build (push) Has been cancelled
Details
Issues due date / Add labels to issues (push) Has been cancelled
Details

Browse Source
This commit is contained in:
NoahWe
2025-04-29 13:00:28 +02:00
committed by GitHub
parent 4aa83b108f
commit f19834aaa7
8 changed files with 263 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/rotwing_7kg_common.xml
+1
View File
@@ -196,6 +196,7 @@
<define name="QUAD_MAX_DECELERATION" value="0.75"/> <!-- Maximum horizontal deceleration in quad mode --> <define name="QUAD_MAX_DECELERATION" value="0.75"/> <!-- Maximum horizontal deceleration in quad mode -->
<define name="SKEW_UP_AIRSPEED" value="10.0"/> <!-- Airspeed where the skewing starts when going up in airspeed --> <define name="SKEW_UP_AIRSPEED" value="10.0"/> <!-- Airspeed where the skewing starts when going up in airspeed -->
<define name="SKEW_DOWN_AIRSPEED" value="8.0"/> <!-- Airspeed where the skewing starts when going down in airspeed --> <define name="SKEW_DOWN_AIRSPEED" value="8.0"/> <!-- Airspeed where the skewing starts when going down in airspeed -->
<define name="STATE_MIN_FW_DIST" value="150"/> <!-- Minimum distance to switch to fixed wing mode -->
<define name="SKEW_REF_MODEL" value="TRUE"/> <!-- Enable second order reference model for the skewing command --> <define name="SKEW_REF_MODEL" value="TRUE"/> <!-- Enable second order reference model for the skewing command -->
<define name="SKEW_REF_MODEL_P_GAIN" value="0.001"/> <!-- Skewing reference model proportional gain --> <define name="SKEW_REF_MODEL_P_GAIN" value="0.001"/> <!-- Skewing reference model proportional gain -->
conf/flight_plans/tudelft/rotwing_EHVB.xml
+63 -28
View File
@@ -23,6 +23,16 @@
<waypoint lat="52.164861" lon="4.427268" name="C7"/> <waypoint lat="52.164861" lon="4.427268" name="C7"/>
<waypoint lat="52.170422" lon="4.427511" name="C8"/> <waypoint lat="52.170422" lon="4.427511" name="C8"/>
<waypoint lat="52.172276" lon="4.424011" name="C9"/> <waypoint lat="52.172276" lon="4.424011" name="C9"/>
<!-- Soft Geofence EHVB -->
<waypoint lat="52.1688069" lon="4.4111199" name="S1"/>
<waypoint lat="52.1679440" lon="4.4092620" name="S2"/>
<waypoint lat="52.1663569" lon="4.4098229" name="S3"/>
<waypoint lat="52.1648729" lon="4.4110140" name="S4"/>
<waypoint lat="52.1640540" lon="4.4126998" name="S5"/>
<waypoint lat="52.1642600" lon="4.4164978" name="S6"/>
<waypoint lat="52.1656110" lon="4.4208729" name="S7"/>
<waypoint lat="52.1686450" lon="4.4200858" name="S8"/>
<waypoint lat="52.1703410" lon="4.4180238" name="S9"/>
</waypoints> </waypoints>
<sectors> <sectors>
<sector color="red" name="Flyzone"> <sector color="red" name="Flyzone">
@@ -36,11 +46,27 @@
<corner name="C8"/> <corner name="C8"/>
<corner name="C9"/> <corner name="C9"/>
</sector> </sector>
<sector color="orange" name="SoftGeofence">
<corner name="S1"/>
<corner name="S2"/>
<corner name="S3"/>
<corner name="S4"/>
<corner name="S5"/>
<corner name="S6"/>
<corner name="S7"/>
<corner name="S8"/>
<corner name="S9"/>
</sector>
</sectors> </sectors>
<variables> <variables>
<variable var="liftoff_pitch"/> <variable var="liftoff_pitch"/>
<variable var="liftoff_roll"/> <variable var="liftoff_roll"/>
<variable var="liftoff_pitch_limit" init="LIFTOFF_PITCH_LIMIT_DEG"/>
<variable var="liftoff_roll_limit" init="LIFTOFF_ROLL_LIMIT_DEG"/>
<variable var="stage_timer_msec"/> <variable var="stage_timer_msec"/>
<variable var="hybrid_height" init="HYBRID_HEIGHT"/>
<variable var="transition_height" init="TRANSITION_HEIGHT"/>
<variable var="flare_height" init="FLARE_HEIGHT"/>
</variables> </variables>
<modules> <modules>
<module name="follow_me"> <module name="follow_me">
@@ -48,8 +74,16 @@
</module> </module>
</modules> </modules>
<exceptions> <exceptions>
<!-- Outside of hard (red) geofence kill -->
<exception cond="Or(!InsideFlyzone(GetPosX(), GetPosY()), GetPosHeight() @GT 1500) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/> <exception cond="Or(!InsideFlyzone(GetPosX(), GetPosY()), GetPosHeight() @GT 1500) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/>
<!-- Outside of soft (orange) geofence go to standby free -->
<exception cond="Or(!InsideSoftGeofence(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 250.0) @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"/>
<!-- Datalink timeout go to standby free -->
<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"/> <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"/>
<!-- Bat low return to standby-->
<exception cond="(electrical.bat_low @AND !(IndexOfBlock('Holding point') @GT nav_block) @AND !(nav_block >= IndexOfBlock('land here')) @AND (autopilot_in_flight() == true) )" deroute="Standby"/>
<!-- Bat critical land here-->
<exception cond="(electrical.bat_critical @AND !(IndexOfBlock('Holding point') @GT nav_block) @AND !(nav_block >= IndexOfBlock('land here')) @AND (autopilot_in_flight() == true) )" deroute="land here"/>
</exceptions> </exceptions>
<blocks> <blocks>
<block name="Wait GPS"> <block name="Wait GPS">
@@ -94,16 +128,15 @@
<attitude pitch="0" roll="0" throttle="0.75" until="stage_time>3" vmode="throttle"/> <attitude pitch="0" roll="0" throttle="0.75" until="stage_time>3" vmode="throttle"/>
</block> </block>
<block name="Climb"> <block name="Climb">
<exception cond="GetPosHeight() @GT 20.0" deroute="Standby"/> <exception cond="GetPosHeight() @GT hybrid_height" deroute="Standby"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="nav_set_heading_current()"/> <call_once fun="nav_set_heading_current()"/>
<stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/> <stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
</block> </block>
<block name="Standby" strip_button="Standby" strip_icon="home.png"> <block name="Standby" strip_button="Standby" strip_icon="home.png" pre_call="rotwing_state_choose_state_by_dist(WP_STDBY)">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="STDBY"/> <stay wp="STDBY"/>
</block> </block>
<block name="Standby_free"> <block name="Standby_free" strip_button="Standby Free" strip_icon="home.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
<stay wp="STDBY"/> <stay wp="STDBY"/>
</block> </block>
@@ -111,17 +144,17 @@
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p1"/> <stay wp="p1"/>
</block> </block>
<block name="stay_p2"> <block name="stay_p2" strip_button="Stay P2" strip_icon="wp_quad.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p2"/> <stay wp="p2"/>
</block> </block>
<block name="p1_p2"> <block name="stay_p3" strip_button="Stay P3" strip_icon="wp_quad.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<go wp="p1"/> <stay wp="p3"/>
<stay until="stage_time>5" wp="p1"/> </block>
<go wp="p2"/> <block name="stay_p4" strip_button="Stay P4" strip_icon="wp_quad.png">
<stay until="stage_time>5" wp="p2"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<deroute block="p1_p2"/> <stay wp="p4"/>
</block> </block>
<!-- <block name="Approach APP"> <!-- <block name="Approach APP">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
@@ -139,37 +172,39 @@
<go from="p4" hmode="route" wp="p1"/> <go from="p4" hmode="route" wp="p1"/>
<deroute block="route"/> <deroute block="route"/>
</block> --> </block> -->
<!-- <block name="small_route" strip_button="Route" strip_icon="path.png"> <block name="route_p1" strip_button="Route P1" strip_icon="path.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p1"/>
</block>
<block name="route_p2" strip_button="Route P2" strip_icon="path.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p2"/> <go wp="p2"/>
</block>
<block name="route_p3" strip_button="Route P3" strip_icon="path.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p3"/> <go wp="p3"/>
</block>
<block name="route_p4" strip_button="Route P4" strip_icon="path.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p4"/> <go wp="p4"/>
<go wp="p1"/> <deroute block="route_p1"/>
<deroute block="small_route"/> </block>
</block> --> <block name="Circle_CW_fwd" strip_button="Circle CW" strip_icon="circle-right.png">
<block name="Circle_CW_fwd" strip_button="CircleRight" strip_icon="circle-right.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="nav.radius" wp="circ"/> <circle radius="nav.radius" wp="circ"/>
</block> </block>
<block name="Circle_CCW_fwd" strip_button="CircleLeft" strip_icon="circle-left.png"> <block name="Circle_CCW_fwd" strip_button="Circle CCW" strip_icon="circle-left.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="-nav.radius" wp="circ"/> <circle radius="-nav.radius" wp="circ"/>
</block> </block>
<block name="Circle_Auto_fwd" strip_button="CircleAuto" strip_icon="circle-left.png"> <block name="Circle_Auto_fwd" strip_button="Circle Auto" strip_icon="circle-left.png"> <!-- Comment out later -->
<!-- <call_once var="circle_direction" fun="rotwing_state_choose_circle_direction(WP_circ)"/> -->
<!-- <circle radius="-nav.radius" wp="circ"/> -->
<exception cond="!rotwing_state_choose_circle_direction(WP_circ)" deroute="Circle_CW_fwd"/> <exception cond="!rotwing_state_choose_circle_direction(WP_circ)" deroute="Circle_CW_fwd"/>
<exception cond="rotwing_state_choose_circle_direction(WP_circ)" deroute="Circle_CCW_fwd"/> <exception cond="rotwing_state_choose_circle_direction(WP_circ)" deroute="Circle_CCW_fwd"/>
</block> </block>
<block name="Approach APP"> <!-- <block name="Approach APP">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/> <stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block> </block> -->
<block name="Transition_quad" strip_button="Transition Quad" strip_icon="wp_quad.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
<go wp="STDBY"/>
<deroute block="Standby"/>
</block>
<block name="land here"> <block name="land here">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/> <call_once fun="NavSetWaypointHere(WP_TD)"/>
@@ -180,7 +215,7 @@
</block> </block>
<block name="descend" strip_button="Descend" strip_icon="descend.png"> <block name="descend" strip_button="Descend" strip_icon="descend.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<exception cond="GetPosHeight() @LT 12.0" deroute="flare"/> <exception cond="GetPosHeight() @LT flare_height" deroute="flare"/>
<stay climb="-1.0" vmode="climb" wp="TD"/> <stay climb="-1.0" vmode="climb" wp="TD"/>
</block> </block>
<block name="flare"> <block name="flare">
conf/flight_plans/tudelft/rotwing_tharde.xml
+97 -36
View File
@@ -19,6 +19,12 @@
<waypoint lat="52.4054122" lon="5.9581765" name="C3"/> <waypoint lat="52.4054122" lon="5.9581765" name="C3"/>
<waypoint lat="52.4179727" lon="6.0083051" name="C4"/> <waypoint lat="52.4179727" lon="6.0083051" name="C4"/>
<waypoint lat="52.4420912" lon="5.9871178" name="C5"/> <waypoint lat="52.4420912" lon="5.9871178" name="C5"/>
<waypoint lat="52.4031030" lon="5.9100930" name="S1"/>
<waypoint lat="52.3897030" lon="5.9217331" name="S2"/>
<waypoint lat="52.4069299" lon="5.9574477" name="S3"/>
<waypoint lat="52.4185648" lon="6.0038740" name="S4"/>
<waypoint lat="52.4390574" lon="5.9861463" name="S5"/>
</waypoints> </waypoints>
<sectors> <sectors>
<sector color="red" name="Flyzone"> <sector color="red" name="Flyzone">
@@ -28,15 +34,35 @@
<corner name="C4"/> <corner name="C4"/>
<corner name="C5"/> <corner name="C5"/>
</sector> </sector>
<sector color="orange" name="SoftGeofence">
<corner name="S1"/>
<corner name="S2"/>
<corner name="S3"/>
<corner name="S4"/>
<corner name="S5"/>
</sector>
</sectors> </sectors>
<modules> <variables>
<!--<module name="follow_me"> <variable var="liftoff_pitch"/>
<define name="FOLLOW_ME_MOVING_WPS" value="WP_p1,WP_p2,WP_p3,WP_p4,WP_STDBY,WP_circ,WP_APP"/> <variable var="liftoff_roll"/>
</module> --> <variable var="liftoff_pitch_limit" init="LIFTOFF_PITCH_LIMIT_DEG"/>
</modules> <variable var="liftoff_roll_limit" init="LIFTOFF_ROLL_LIMIT_DEG"/>
<variable var="stage_timer_msec"/>
<variable var="hybrid_height" init="HYBRID_HEIGHT"/>
<variable var="transition_height" init="TRANSITION_HEIGHT"/>
<variable var="flare_height" init="FLARE_HEIGHT"/>
</variables>
<exceptions> <exceptions>
<!-- Outside of hard (red) geofence kill -->
<exception cond="Or(!InsideFlyzone(GetPosX(), GetPosY()), GetPosHeight() @GT 1500) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/> <exception cond="Or(!InsideFlyzone(GetPosX(), GetPosY()), GetPosHeight() @GT 1500) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/>
<!-- Outside of soft (orange) geofence go to standby free -->
<exception cond="Or(!InsideSoftGeofence(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 250.0) @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"/>
<!-- Datalink timeout go to standby free -->
<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"/> <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"/>
<!-- Bat low return to standby-->
<exception cond="(electrical.bat_low @AND !(IndexOfBlock('Holding point') @GT nav_block) @AND !(nav_block >= IndexOfBlock('land here')) @AND (autopilot_in_flight() == true) )" deroute="Standby"/>
<!-- Bat critical land here-->
<exception cond="(electrical.bat_critical @AND !(IndexOfBlock('Holding point') @GT nav_block) @AND !(nav_block >= IndexOfBlock('land here')) @AND (autopilot_in_flight() == true) )" deroute="land here"/>
</exceptions> </exceptions>
<blocks> <blocks>
<block name="Wait GPS"> <block name="Wait GPS">
@@ -56,84 +82,119 @@
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/> <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block> </block>
<block name="Start Engine" strip_button="Start Engines" strip_icon="on.png"> <block name="Start Engine" strip_button="Start Engines" strip_icon="on.png">
<exception cond="stage_time > 10" deroute="Holding point"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="nav_set_heading_current()"/>
<call_once fun="NavResurrect()"/> <call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time>10" vmode="throttle"/> <attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0" until="rotwing_state_hover_motors_running()" vmode="throttle"/>
</block> </block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png"> <block name="Wait takeoff">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<exception cond="stage_time > 10" deroute="Holding point"/>
<attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0.2" until="(fabs(DegOfRad(stateGetNedToBodyEulers_f()->theta)) @LT liftoff_roll_limit) @AND (fabs(DegOfRad(stateGetNedToBodyEulers_f()->phi)) @LT liftoff_pitch_limit) @AND (stage_time > 2)" vmode="throttle"/>
</block>
<block name="Takeoff" strip_icon="takeoff.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<exception cond="agl_dist_valid @AND (agl_dist_value @GT 1.0)" deroute="Climb"/> <exception cond="agl_dist_valid @AND (agl_dist_value @GT 1.0)" deroute="Climb"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<set var="stage_timer_msec" value="get_sys_time_msec()"/>
<set var="liftoff_roll" value="DegOfRad(stateGetNedToBodyEulers_f()->phi)"/>
<set var="liftoff_pitch" value="DegOfRad(stateGetNedToBodyEulers_f()->theta)"/>
<attitude pitch="liftoff_pitch" roll="liftoff_roll" throttle="0.75" until="(get_sys_time_msec()-stage_timer_msec)>250" vmode="throttle"/>
<call_once fun="autopilot_set_in_flight(true)"/><!-- this is a hack to solve INDI not being active fast enough --> <call_once fun="autopilot_set_in_flight(true)"/><!-- this is a hack to solve INDI not being active fast enough -->
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/> <call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<attitude pitch="0" roll="0" throttle="0.75" until="stage_time>3" vmode="throttle"/> <attitude pitch="0" roll="0" throttle="0.75" until="stage_time>3" vmode="throttle"/>
</block> </block>
<block name="Climb"> <block name="Climb">
<exception cond="GetPosHeight() @GT 20.0" deroute="Standby"/> <exception cond="GetPosHeight() @GT hybrid_height" deroute="Standby"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="nav_set_heading_current()"/> <call_once fun="nav_set_heading_current()"/>
<stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/> <stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
</block> </block>
<block name="Standby" strip_button="Standby" strip_icon="home.png"> <block name="Standby" strip_button="Standby" strip_icon="home.png" pre_call="rotwing_state_choose_state_by_dist(WP_STDBY)">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="STDBY"/> <stay wp="STDBY"/>
</block> </block>
<block name="Approach APP"> <block name="Standby_free" strip_button="Standby Free" strip_icon="home.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
<stay wp="STDBY"/>
</block>
<block name="stay_p1" strip_button="Stay P1" strip_icon="wp_quad.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p1"/>
</block>
<block name="stay_p2" strip_button="Stay P2" strip_icon="wp_quad.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p2"/>
</block>
<block name="stay_p3" strip_button="Stay P3" strip_icon="wp_quad.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p3"/>
</block>
<block name="stay_p4" strip_button="Stay P4" strip_icon="wp_quad.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="p4"/>
</block>
<!-- <block name="Approach APP">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/> <stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block> </block> -->
<!-- <block name="follow_module"> <!-- <block name="follow_module">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay pre_call="follow_me_set_wp(WP_p1, 0)" wp="p1"/> <stay pre_call="follow_me_set_wp(WP_p1, 0)" wp="p1"/>
</block> --> </block> -->
<block name="route"> <!-- <block name="route">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go from="p1" hmode="route" wp="p2"/> <go from="p1" hmode="route" wp="p2"/>
<go from="p2" hmode="route" wp="p3"/> <go from="p2" hmode="route" wp="p3"/>
<go from="p3" hmode="route" wp="p4"/> <go from="p3" hmode="route" wp="p4"/>
<go from="p4" hmode="route" wp="p1"/> <go from="p4" hmode="route" wp="p1"/>
<deroute block="route"/> <deroute block="route"/>
</block> -->
<block name="route_p1" strip_button="Route P1" strip_icon="path.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p1"/>
</block> </block>
<block name="small_route" strip_button="Route" strip_icon="path.png"> <block name="route_p2" strip_button="Route P2" strip_icon="path.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p2"/> <go wp="p2"/>
</block>
<block name="route_p3" strip_button="Route P3" strip_icon="path.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p3"/> <go wp="p3"/>
</block>
<block name="route_p4" strip_button="Route P4" strip_icon="path.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<go wp="p4"/> <go wp="p4"/>
<go wp="p1"/> <deroute block="route_p1"/>
<deroute block="small_route"/>
</block> </block>
<block name="Circle_CW_fwd" strip_button="CircleRight" strip_icon="circle-right.png"> <block name="Circle_CW_fwd" strip_button="Circle CW" strip_icon="circle-right.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="100" wp="circ"/> <circle radius="nav.radius" wp="circ"/>
</block> </block>
<block name="Circle_CCW_fwd" strip_button="CircleLeft" strip_icon="circle-left.png"> <block name="Circle_CCW_fwd" strip_button="Circle CCW" strip_icon="circle-left.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="-100" wp="circ"/> <circle radius="-nav.radius" wp="circ"/>
</block> </block>
<block name="big_Circle_CW_fwd"> <block name="Circle_Auto_fwd" strip_button="Circle Auto" strip_icon="circle-left.png"> <!-- Comment out later -->
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/> <exception cond="!rotwing_state_choose_circle_direction(WP_circ)" deroute="Circle_CW_fwd"/>
<circle radius="150" wp="circ"/> <exception cond="rotwing_state_choose_circle_direction(WP_circ)" deroute="Circle_CCW_fwd"/>
</block> </block>
<block name="big_Circle_CCW_fwd"> <!-- <block name="Approach APP">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<circle radius="-150" wp="circ"/> <stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block> </block> -->
<block name="Transition_quad"> <block name="land here">
<call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
<go wp="STDBY"/>
<deroute block="Standby"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/> <call_once fun="NavSetWaypointHere(WP_TD)"/>
</block> </block>
<block name="land"> <block name="land" strip_button="Land" strip_icon="land-right.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<go wp="TD"/> <go wp="TD"/>
</block> </block>
<block name="descend"> <block name="descend" strip_button="Descend" strip_icon="descend.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/> <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<exception cond="GetPosHeight() @LT 12.0" deroute="flare"/> <exception cond="GetPosHeight() @LT flare_height" deroute="flare"/>
<stay climb="-1.0" vmode="climb" wp="TD"/> <stay climb="-1.0" vmode="climb" wp="TD"/>
</block> </block>
<block name="flare"> <block name="flare">
conf/userconf/tudelft/conf.xml
+33
View File
@@ -670,4 +670,37 @@
settings_modules="modules/actuators_faulhaber.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rotwing.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml" settings_modules="modules/actuators_faulhaber.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rotwing.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
gui_color="red" gui_color="red"
/> />
<aircraft
name="RotatingWingV3I"
ac_id="29"
airframe="airframes/tudelft/rotwing_v3i.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotwing_EHVB.xml"
settings=""
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rotwing.xml modules/ekf_aw.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
gui_color="red"
/>
<aircraft
name="RotatingWingV3J"
ac_id="36"
airframe="airframes/tudelft/rotwing_v3j.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotwing_EHVB.xml"
settings=""
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rotwing.xml modules/ekf_aw.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
gui_color="red"
/>
<aircraft
name="RotatingWingV3K"
ac_id="37"
airframe="airframes/tudelft/rotwing_v3k.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotwing_EHVB.xml"
settings=""
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rotwing.xml modules/ekf_aw.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
gui_color="red"
/>
</conf> </conf>
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+14
View File
@@ -416,6 +416,20 @@ void guidance_set_min_max_airspeed(float min_airspeed, float max_airspeed) {
gih_params.max_airspeed = max_airspeed; gih_params.max_airspeed = max_airspeed;
} }
void guidance_set_max_bank_angle(float max_bank) {
guidance_indi_max_bank = max_bank;
}
void guidance_set_max_climb_speed(float max_climb_speed_quad, float max_climb_speed_fwd) {
gih_params.climb_vspeed_quad = max_climb_speed_quad;
gih_params.climb_vspeed_fwd = max_climb_speed_fwd;
}
void guidance_set_max_descend_speed(float max_descend_speed_quad, float max_descend_speed_fwd) {
gih_params.descend_vspeed_quad = max_descend_speed_quad;
gih_params.descend_vspeed_fwd = max_descend_speed_fwd;
}
/** /**
* @param accel_sp accel setpoint in NED frame [m/s^2] * @param accel_sp accel setpoint in NED frame [m/s^2]
* @param heading_sp the desired heading [rad] * @param heading_sp the desired heading [rad]
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
+3
View File
@@ -71,6 +71,9 @@ enum GuidanceIndiHybrid_VMode {
extern struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accep_sp, float heading_sp); extern struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accep_sp, float heading_sp);
extern struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndiHybrid_HMode h_mode, enum GuidanceIndiHybrid_VMode v_mode); extern struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndiHybrid_HMode h_mode, enum GuidanceIndiHybrid_VMode v_mode);
extern void guidance_set_min_max_airspeed(float min_airspeed, float max_airspeed); extern void guidance_set_min_max_airspeed(float min_airspeed, float max_airspeed);
extern void guidance_set_max_bank_angle(float max_bank);
extern void guidance_set_max_climb_speed(float max_climb_speed_quad, float max_climb_speed_fwd);
extern void guidance_set_max_descend_speed(float max_descend_speed_quad, float max_descend_speed_fwd);
struct guidance_indi_hybrid_params { struct guidance_indi_hybrid_params {
float pos_gain; float pos_gain;
sw/airborne/modules/rotwing_drone/rotwing_state.c
+51 -38
View File
@@ -60,6 +60,26 @@
#define ROTWING_FW_SKEW_ANGLE 85.0 #define ROTWING_FW_SKEW_ANGLE 85.0
#endif #endif
/* Maximum measured skew angle to consider the rotating wing drone in quad (deg) */
#ifndef ROTWING_QUAD_SKEW_ANGLE
#define ROTWING_QUAD_SKEW_ANGLE 25.0
#endif
/* Maximum bank angle while the rotwing is skewing */
#ifndef ROTWING_TRANSITION_MAX_BANK
#define ROTWING_TRANSITION_MAX_BANK RadOfDeg(20)
#endif
/* Maximum climb speed while the rotwing is transitioning */
#ifndef ROTWING_TRANSITION_MAX_CLIMB_SPEED
#define ROTWING_TRANSITION_MAX_CLIMB_SPEED 0.5
#endif
/* Maximum descend speed while the rotwing is transitioning */
#ifndef ROTWING_TRANSITION_MAX_DESCEND_SPEED
#define ROTWING_TRANSITION_MAX_DESCEND_SPEED -0.5
#endif
/* TODO: Give a name.... */ /* TODO: Give a name.... */
#ifndef ROTWING_SKEW_BACK_MARGIN #ifndef ROTWING_SKEW_BACK_MARGIN
#define ROTWING_SKEW_BACK_MARGIN 5.0 #define ROTWING_SKEW_BACK_MARGIN 5.0
@@ -80,6 +100,10 @@
#define ROTWING_FW_STALL_TIMEOUT 0.5 #define ROTWING_FW_STALL_TIMEOUT 0.5
#endif #endif
#ifndef ROTWING_STATE_MIN_FW_DIST
#define ROTWING_STATE_MIN_FW_DIST 200.0
#endif
/* Fix for not having double can busses */ /* Fix for not having double can busses */
#ifndef SERVO_BMOTOR_PUSH_IDX #ifndef SERVO_BMOTOR_PUSH_IDX
#define SERVO_BMOTOR_PUSH_IDX SERVO_MOTOR_PUSH_IDX #define SERVO_BMOTOR_PUSH_IDX SERVO_MOTOR_PUSH_IDX
@@ -285,6 +309,18 @@ void rotwing_state_periodic(void)
rotwing_state.max_airspeed = skew_max_airspeed; rotwing_state.max_airspeed = skew_max_airspeed;
} }
/* Bound max bank angle, climb speed and descend speed if the rotwing drone is transitioning and not in FREE mode */
if (rotwing_state.meas_skew_angle_deg < ROTWING_FW_SKEW_ANGLE && rotwing_state.meas_skew_angle_deg > ROTWING_QUAD_SKEW_ANGLE
&& rotwing_state.state != ROTWING_STATE_FREE) {
guidance_set_max_bank_angle(ROTWING_TRANSITION_MAX_BANK);
guidance_set_max_climb_speed(ROTWING_TRANSITION_MAX_CLIMB_SPEED, ROTWING_TRANSITION_MAX_CLIMB_SPEED);
guidance_set_max_descend_speed(ROTWING_TRANSITION_MAX_DESCEND_SPEED, ROTWING_TRANSITION_MAX_DESCEND_SPEED);
} else {
guidance_set_max_bank_angle(GUIDANCE_H_MAX_BANK);
guidance_set_max_climb_speed(GUIDANCE_INDI_QUAD_CLIMB_SPEED, GUIDANCE_INDI_FWD_CLIMB_SPEED);
guidance_set_max_descend_speed(GUIDANCE_INDI_QUAD_DESCEND_SPEED, GUIDANCE_INDI_FWD_DESCEND_SPEED);
}
// Override failing skewing while fwd // Override failing skewing while fwd
/*if(meas_skew_angle > 70 && rotwing_state_.skewing_failing) { /*if(meas_skew_angle > 70 && rotwing_state_.skewing_failing) {
rotwing_state.min_airspeed = 0; // Vstall + margin rotwing_state.min_airspeed = 0; // Vstall + margin
@@ -292,6 +328,7 @@ void rotwing_state_periodic(void)
}*/ }*/
guidance_set_min_max_airspeed(rotwing_state.min_airspeed, rotwing_state.max_airspeed); guidance_set_min_max_airspeed(rotwing_state.min_airspeed, rotwing_state.max_airspeed);
/* Set navigation/guidance settings */ /* Set navigation/guidance settings */
nav_max_deceleration_sp = ROTWING_FW_MAX_DECELERATION * meas_skew_angle / 90.f + ROTWING_QUAD_MAX_DECELERATION * (90.f - meas_skew_angle) / 90.f; //TODO: Do we really want to based this on the skew? nav_max_deceleration_sp = ROTWING_FW_MAX_DECELERATION * meas_skew_angle / 90.f + ROTWING_QUAD_MAX_DECELERATION * (90.f - meas_skew_angle) / 90.f; //TODO: Do we really want to based this on the skew?
@@ -477,44 +514,20 @@ bool rotwing_state_choose_circle_direction(uint8_t wp_id) {
} }
} }
void rotwing_state_set_transition_wp(uint8_t wp_id) { bool rotwing_state_choose_state_by_dist(uint8_t wp_id) {
struct EnuCoor_f wp = waypoints[wp_id].enu_f;
float dist2_to_wp = get_dist2_to_point(&wp);
float dist_to_wp = sqrtf(dist2_to_wp);
// Get drone position coordinates in NED if (autopilot.mode == AP_MODE_NAV) {
struct EnuCoor_f target_enu = {.x = stateGetPositionNed_f()->y, if (dist_to_wp > ROTWING_STATE_MIN_FW_DIST) {
.y = stateGetPositionNed_f()->x, rotwing_state_set(ROTWING_STATE_REQUEST_FW);
.z = -stateGetPositionNed_f()->z}; return true; // Necessary for flight plan
} else {
static struct FloatVect3 x_axis = {.x = 1, .y = 0, .z = 0}; rotwing_state_set(ROTWING_STATE_REQUEST_HOVER);
struct FloatVect3 x_att_NED; return false; // Necessary for flight plan
}
float_rmat_transp_vmult(&x_att_NED, stateGetNedToBodyRMat_f(), &x_axis); }
// set the new transition WP coordinates 100m ahead of the drone
target_enu.x = 100.f * x_att_NED.y + target_enu.x;
target_enu.y = 100.f * x_att_NED.x + target_enu.y;
waypoint_set_enu(wp_id, &target_enu);
// Send waypoint update every half second
RunOnceEvery(100 / 2, {
// Send to the GCS that the waypoint has been moved
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
&waypoints[wp_id].enu_i.x,
&waypoints[wp_id].enu_i.y,
&waypoints[wp_id].enu_i.z);
});
} return false; // Necessary for flight plan
void rotwing_state_update_WP_height(uint8_t wp_id, float height) {
struct EnuCoor_f target_enu = {.x = waypoints[wp_id].enu_f.x,
.y = waypoints[wp_id].enu_f.y,
.z = height};
waypoint_set_enu(wp_id, &target_enu);
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
&waypoints[wp_id].enu_i.x,
&waypoints[wp_id].enu_i.y,
&waypoints[wp_id].enu_i.z);
} }
sw/airborne/modules/rotwing_drone/rotwing_state.h
+1 -2
View File
@@ -98,7 +98,6 @@ bool rotwing_state_hover_motors_idling(void);
void rotwing_state_set(enum rotwing_states_t state); void rotwing_state_set(enum rotwing_states_t state);
bool rotwing_state_choose_circle_direction(uint8_t wp_id); bool rotwing_state_choose_circle_direction(uint8_t wp_id);
void rotwing_state_set_transition_wp(uint8_t wp_id); bool rotwing_state_choose_state_by_dist(uint8_t wp_id);
void rotwing_state_update_WP_height(uint8_t wp_id, float height);
#endif // ROTWING_STATE_H #endif // ROTWING_STATE_H