mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
[rotwing] Fly back to standby in fixed wing. Gih add setters for max bank, climb, and descend speed (#3452)
This commit is contained in:
NoahWe
GitHub
@@ -196,6 +196,7 @@
|
||||
<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_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_P_GAIN" value="0.001"/> <!-- Skewing reference model proportional gain -->
|
||||
|
||||
@@ -23,6 +23,16 @@
|
||||
<waypoint lat="52.164861" lon="4.427268" name="C7"/>
|
||||
<waypoint lat="52.170422" lon="4.427511" name="C8"/>
|
||||
<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>
|
||||
<sectors>
|
||||
<sector color="red" name="Flyzone">
|
||||
@@ -36,11 +46,27 @@
|
||||
<corner name="C8"/>
|
||||
<corner name="C9"/>
|
||||
</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>
|
||||
<variables>
|
||||
<variable var="liftoff_pitch"/>
|
||||
<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="hybrid_height" init="HYBRID_HEIGHT"/>
|
||||
<variable var="transition_height" init="TRANSITION_HEIGHT"/>
|
||||
<variable var="flare_height" init="FLARE_HEIGHT"/>
|
||||
</variables>
|
||||
<modules>
|
||||
<module name="follow_me">
|
||||
@@ -48,8 +74,16 @@
|
||||
</module>
|
||||
</modules>
|
||||
<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"/>
|
||||
<!-- 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"/>
|
||||
<!-- 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>
|
||||
<blocks>
|
||||
<block name="Wait GPS">
|
||||
@@ -94,16 +128,15 @@
|
||||
<attitude pitch="0" roll="0" throttle="0.75" until="stage_time>3" vmode="throttle"/>
|
||||
</block>
|
||||
<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="nav_set_heading_current()"/>
|
||||
<stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
|
||||
</block>
|
||||
<block name="Standby" strip_button="Standby" strip_icon="home.png">
|
||||
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
|
||||
<block name="Standby" strip_button="Standby" strip_icon="home.png" pre_call="rotwing_state_choose_state_by_dist(WP_STDBY)">
|
||||
<stay wp="STDBY"/>
|
||||
</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)"/>
|
||||
<stay wp="STDBY"/>
|
||||
</block>
|
||||
@@ -111,17 +144,17 @@
|
||||
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
|
||||
<stay wp="p1"/>
|
||||
</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)"/>
|
||||
<stay wp="p2"/>
|
||||
</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)"/>
|
||||
<go wp="p1"/>
|
||||
<stay until="stage_time>5" wp="p1"/>
|
||||
<go wp="p2"/>
|
||||
<stay until="stage_time>5" wp="p2"/>
|
||||
<deroute block="p1_p2"/>
|
||||
<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)"/>
|
||||
@@ -139,37 +172,39 @@
|
||||
<go from="p4" hmode="route" wp="p1"/>
|
||||
<deroute block="route"/>
|
||||
</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)"/>
|
||||
<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"/>
|
||||
</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="p1"/>
|
||||
<deroute block="small_route"/>
|
||||
</block> -->
|
||||
<block name="Circle_CW_fwd" strip_button="CircleRight" strip_icon="circle-right.png">
|
||||
<deroute block="route_p1"/>
|
||||
</block>
|
||||
<block name="Circle_CW_fwd" strip_button="Circle CW" strip_icon="circle-right.png">
|
||||
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
|
||||
<circle radius="nav.radius" wp="circ"/>
|
||||
</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)"/>
|
||||
<circle radius="-nav.radius" wp="circ"/>
|
||||
</block>
|
||||
<block name="Circle_Auto_fwd" strip_button="CircleAuto" strip_icon="circle-left.png">
|
||||
<!-- <call_once var="circle_direction" fun="rotwing_state_choose_circle_direction(WP_circ)"/> -->
|
||||
<!-- <circle radius="-nav.radius" wp="circ"/> -->
|
||||
<block name="Circle_Auto_fwd" strip_button="Circle Auto" strip_icon="circle-left.png"> <!-- Comment out later -->
|
||||
<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"/>
|
||||
</block>
|
||||
<block name="Approach APP">
|
||||
<!-- <block name="Approach APP">
|
||||
<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"/>
|
||||
</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> -->
|
||||
<block name="land here">
|
||||
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
|
||||
<call_once fun="NavSetWaypointHere(WP_TD)"/>
|
||||
@@ -180,7 +215,7 @@
|
||||
</block>
|
||||
<block name="descend" strip_button="Descend" strip_icon="descend.png">
|
||||
<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"/>
|
||||
</block>
|
||||
<block name="flare">
|
||||
|
||||
@@ -19,6 +19,12 @@
|
||||
<waypoint lat="52.4054122" lon="5.9581765" name="C3"/>
|
||||
<waypoint lat="52.4179727" lon="6.0083051" name="C4"/>
|
||||
<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>
|
||||
<sectors>
|
||||
<sector color="red" name="Flyzone">
|
||||
@@ -28,15 +34,35 @@
|
||||
<corner name="C4"/>
|
||||
<corner name="C5"/>
|
||||
</sector>
|
||||
<sector color="orange" name="SoftGeofence">
|
||||
<corner name="S1"/>
|
||||
<corner name="S2"/>
|
||||
<corner name="S3"/>
|
||||
<corner name="S4"/>
|
||||
<corner name="S5"/>
|
||||
</sector>
|
||||
</sectors>
|
||||
<modules>
|
||||
<!--<module name="follow_me">
|
||||
<define name="FOLLOW_ME_MOVING_WPS" value="WP_p1,WP_p2,WP_p3,WP_p4,WP_STDBY,WP_circ,WP_APP"/>
|
||||
</module> -->
|
||||
</modules>
|
||||
<variables>
|
||||
<variable var="liftoff_pitch"/>
|
||||
<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="hybrid_height" init="HYBRID_HEIGHT"/>
|
||||
<variable var="transition_height" init="TRANSITION_HEIGHT"/>
|
||||
<variable var="flare_height" init="FLARE_HEIGHT"/>
|
||||
</variables>
|
||||
<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"/>
|
||||
<!-- 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"/>
|
||||
<!-- 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>
|
||||
<blocks>
|
||||
<block name="Wait GPS">
|
||||
@@ -56,84 +82,119 @@
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
<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="nav_set_heading_current()"/>
|
||||
<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 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)"/>
|
||||
<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="NavSetWaypointHere(WP_CLIMB)"/>
|
||||
<attitude pitch="0" roll="0" throttle="0.75" until="stage_time>3" vmode="throttle"/>
|
||||
</block>
|
||||
<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="nav_set_heading_current()"/>
|
||||
<stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
|
||||
</block>
|
||||
<block name="Standby" strip_button="Standby" strip_icon="home.png">
|
||||
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
|
||||
<block name="Standby" strip_button="Standby" strip_icon="home.png" pre_call="rotwing_state_choose_state_by_dist(WP_STDBY)">
|
||||
<stay wp="STDBY"/>
|
||||
</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)"/>
|
||||
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
|
||||
</block>
|
||||
</block> -->
|
||||
<!-- <block name="follow_module">
|
||||
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
|
||||
<stay pre_call="follow_me_set_wp(WP_p1, 0)" wp="p1"/>
|
||||
</block> -->
|
||||
<block name="route">
|
||||
<!-- <block name="route">
|
||||
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
|
||||
<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="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="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)"/>
|
||||
<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"/>
|
||||
</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="p1"/>
|
||||
<deroute block="small_route"/>
|
||||
<deroute block="route_p1"/>
|
||||
</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)"/>
|
||||
<circle radius="100" wp="circ"/>
|
||||
<circle radius="nav.radius" wp="circ"/>
|
||||
</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)"/>
|
||||
<circle radius="-100" wp="circ"/>
|
||||
<circle radius="-nav.radius" wp="circ"/>
|
||||
</block>
|
||||
<block name="big_Circle_CW_fwd">
|
||||
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
|
||||
<circle radius="150" wp="circ"/>
|
||||
<block name="Circle_Auto_fwd" strip_button="Circle Auto" strip_icon="circle-left.png"> <!-- Comment out later -->
|
||||
<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"/>
|
||||
</block>
|
||||
<block name="big_Circle_CCW_fwd">
|
||||
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
|
||||
<circle radius="-150" wp="circ"/>
|
||||
</block>
|
||||
<block name="Transition_quad">
|
||||
<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">
|
||||
<!-- <block name="Approach APP">
|
||||
<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"/>
|
||||
</block> -->
|
||||
<block name="land here">
|
||||
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
|
||||
<call_once fun="NavSetWaypointHere(WP_TD)"/>
|
||||
</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)"/>
|
||||
<go wp="TD"/>
|
||||
</block>
|
||||
<block name="descend">
|
||||
<block name="descend" strip_button="Descend" strip_icon="descend.png">
|
||||
<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"/>
|
||||
</block>
|
||||
<block name="flare">
|
||||
|
||||
@@ -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"
|
||||
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>
|
||||
|
||||
@@ -416,6 +416,20 @@ void guidance_set_min_max_airspeed(float min_airspeed, float 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 heading_sp the desired heading [rad]
|
||||
|
||||
@@ -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_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_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 {
|
||||
float pos_gain;
|
||||
|
||||
@@ -60,6 +60,26 @@
|
||||
#define ROTWING_FW_SKEW_ANGLE 85.0
|
||||
#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.... */
|
||||
#ifndef ROTWING_SKEW_BACK_MARGIN
|
||||
#define ROTWING_SKEW_BACK_MARGIN 5.0
|
||||
@@ -80,6 +100,10 @@
|
||||
#define ROTWING_FW_STALL_TIMEOUT 0.5
|
||||
#endif
|
||||
|
||||
#ifndef ROTWING_STATE_MIN_FW_DIST
|
||||
#define ROTWING_STATE_MIN_FW_DIST 200.0
|
||||
#endif
|
||||
|
||||
/* Fix for not having double can busses */
|
||||
#ifndef SERVO_BMOTOR_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;
|
||||
}
|
||||
|
||||
/* 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
|
||||
/*if(meas_skew_angle > 70 && rotwing_state_.skewing_failing) {
|
||||
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);
|
||||
|
||||
/* 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?
|
||||
|
||||
@@ -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
|
||||
struct EnuCoor_f target_enu = {.x = stateGetPositionNed_f()->y,
|
||||
.y = stateGetPositionNed_f()->x,
|
||||
.z = -stateGetPositionNed_f()->z};
|
||||
|
||||
static struct FloatVect3 x_axis = {.x = 1, .y = 0, .z = 0};
|
||||
struct FloatVect3 x_att_NED;
|
||||
|
||||
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);
|
||||
});
|
||||
if (autopilot.mode == AP_MODE_NAV) {
|
||||
if (dist_to_wp > ROTWING_STATE_MIN_FW_DIST) {
|
||||
rotwing_state_set(ROTWING_STATE_REQUEST_FW);
|
||||
return true; // Necessary for flight plan
|
||||
} else {
|
||||
rotwing_state_set(ROTWING_STATE_REQUEST_HOVER);
|
||||
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);
|
||||
return false; // Necessary for flight plan
|
||||
}
|
||||
|
||||
@@ -98,7 +98,6 @@ bool rotwing_state_hover_motors_idling(void);
|
||||
|
||||
void rotwing_state_set(enum rotwing_states_t state);
|
||||
bool rotwing_state_choose_circle_direction(uint8_t wp_id);
|
||||
void rotwing_state_set_transition_wp(uint8_t wp_id);
|
||||
void rotwing_state_update_WP_height(uint8_t wp_id, float height);
|
||||
bool rotwing_state_choose_state_by_dist(uint8_t wp_id);
|
||||
|
||||
#endif // ROTWING_STATE_H
|
||||
|
||||
Reference in New Issue
Block a user