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

[generator] allow multiple state machines for autopilot generator

Browse Source
This commit is contained in:
Gautier Hattenberger
2014-09-04 11:18:54 +02:00
parent 41177146cb
commit f30047d852
9 changed files with 305 additions and 265 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/rotorcraft_autopilot.xml
+67 -63
View File
@@ -20,73 +20,77 @@
<call fun="SaturateThrottle(rc_values)"/>
</control_block>
<exceptions>
<exception cond="too_far_from_home" deroute="HOME"/>
</exceptions>
<state_machine name="Main">
<mode name="ATTITUDE" start="stabilization_attitude_enter()">
<select cond="$DEFAULT_MODE"/>
<select cond="RCMode0()"/>
<control freq="512">
<call_block name="attitude_loop"/>
<call fun="SetThrottleFromRC(rc_values)"/>
<call_block name="set_actuators"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<exceptions>
<exception cond="too_far_from_home" deroute="HOME"/>
</exceptions>
<mode name="VERTICAL" start="stabilization_attitude_enter()|guidance_v_enter()">
<select cond="RCMode1()"/>
<control freq="512">
<call_block name="attitude_loop"/>
<call_block name="altitude_loop"/>
<call_block name="set_actuators"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<mode name="ATTITUDE" start="stabilization_attitude_enter()">
<select cond="$DEFAULT_MODE"/>
<select cond="RCMode0()"/>
<control freq="512">
<call_block name="attitude_loop"/>
<call fun="SetThrottleFromRC(rc_values)"/>
<call_block name="set_actuators"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<mode name="NAV" start="guidance_h_nav_enter()|guidance_v_enter()">
<select cond="RCMode2()" exception="HOME"/>
<select cond="RCMode2() && DLModeNav()"/>
<control freq="32">
<call fun="nav_periodic_task()"/>
</control>
<control freq="512">
<call fun="SetCommandFromAP()"/>
<call fun="GuidanceNavHorizontal()"/>
<call fun="GuidanceNavVertical()"/>
<call fun="AddAttitudeFromRC(rc_values)" cond="!RCLost()"/>
<call fun="stabilization_attitude_run(autopilot_in_flight)"/>
<call fun="SaturateThrottle(rc_values)" cond="!RCLost()"/>
<call_block name="set_actuators"/>
</control>
<exception cond="GPSLost()" deroute="FAILSAFE"/>
</mode>
<mode name="VERTICAL" start="stabilization_attitude_enter()|guidance_v_enter()">
<select cond="RCMode1()"/>
<control freq="512">
<call_block name="attitude_loop"/>
<call_block name="altitude_loop"/>
<call_block name="set_actuators"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<mode name="HOME" start="guidance_h_nav_enter()|guidance_v_enter()">
<control freq="32">
<call fun="nav_home()"/>
</control>
<control freq="512">
<call fun="SetCommandFromAP()"/>
<call fun="GuidanceNavHorizontal()"/>
<call fun="GuidanceNavVertical()"/>
<call fun="stabilization_attitude_run(autopilot_in_flight)"/>
<call_block name="set_actuators"/>
</control>
<exception cond="GPSLost()" deroute="FAILSAFE"/>
</mode>
<mode name="NAV" start="guidance_h_nav_enter()|guidance_v_enter()">
<select cond="RCMode2()" exception="HOME"/>
<select cond="RCMode2() && DLModeNav()"/>
<control freq="32">
<call fun="nav_periodic_task()"/>
</control>
<control freq="512">
<call fun="SetCommandFromAP()"/>
<call fun="GuidanceNavHorizontal()"/>
<call fun="GuidanceNavVertical()"/>
<call fun="AddAttitudeFromRC(rc_values)" cond="!RCLost()"/>
<call fun="stabilization_attitude_run(autopilot_in_flight)"/>
<call fun="SaturateThrottle(rc_values)" cond="!RCLost()"/>
<call_block name="set_actuators"/>
</control>
<exception cond="GPSLost()" deroute="FAILSAFE"/>
</mode>
<!-- Safe landing -->
<mode name="FAILSAFE" start="stabilization_attitude_set_failsafe_setpoint()|guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)|guidance_v_zd_sp = SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED)">
<control freq="512">
<call fun="SetFailsafeCommand()"/>
<call fun="stabilization_attitude_run(autopilot_in_flight)"/>
<call fun="gv_update_ref_from_zd_sp(guidance_v_zd_sp)"/>
<call fun="run_hover_loop(autopilot_in_flight)"/>
<call fun="actuators_set(autopilot_motors_on)"/>
</control>
<exception cond="!GPSLost()" deroute="$LAST_MODE"/>
</mode>
<mode name="HOME" start="guidance_h_nav_enter()|guidance_v_enter()">
<control freq="32">
<call fun="nav_home()"/>
</control>
<control freq="512">
<call fun="SetCommandFromAP()"/>
<call fun="GuidanceNavHorizontal()"/>
<call fun="GuidanceNavVertical()"/>
<call fun="stabilization_attitude_run(autopilot_in_flight)"/>
<call_block name="set_actuators"/>
</control>
<exception cond="GPSLost()" deroute="FAILSAFE"/>
</mode>
<!-- Safe landing -->
<mode name="FAILSAFE" start="stabilization_attitude_set_failsafe_setpoint()|guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)|guidance_v_zd_sp = SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED)">
<control freq="512">
<call fun="SetFailsafeCommand()"/>
<call fun="stabilization_attitude_run(autopilot_in_flight)"/>
<call fun="gv_update_ref_from_zd_sp(guidance_v_zd_sp)"/>
<call fun="run_hover_loop(autopilot_in_flight)"/>
<call fun="actuators_set(autopilot_motors_on)"/>
</control>
<exception cond="!GPSLost()" deroute="$LAST_MODE"/>
</mode>
</state_machine>
</autopilot>