mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
Gautier Hattenberger
0a65e14903
* [autopilot] compiling generated AP for fixedwing * update some old variable names for autopilot mode * prevent oscillation on the ground with real aircraft * add guidance loop to 'new' control all this guidance code should really be better factorized * fix rotorcraft autopilot for new API * read guidance RC if needed * use generated autopilot as soon as an autopilot file is defined * autopilot node at airframe, firmware or target level The autopilot can be specific to one of the firmware, or one of the target. At the moment, it will fail at runtime if more than one autopilot in the same airframe since server don't know which target is in use.
185 lines
7.0 KiB
XML
185 lines
7.0 KiB
XML
<!DOCTYPE autopilot SYSTEM "autopilot.dtd">
|
|
|
|
<autopilot name="Quadrotor Autopilot (Basic version)">
|
|
|
|
<state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true" settings_handler="autopilot_generated|SetModeHandler">
|
|
|
|
<includes>
|
|
<include name="generated/airframe.h"/>
|
|
<include name="autopilot.h"/>
|
|
<include name="autopilot_rc_helpers.h"/>
|
|
<include name="navigation.h"/>
|
|
<include name="guidance.h"/>
|
|
<include name="stabilization/stabilization_attitude.h"/>
|
|
<include name="subsystems/radio_control.h"/>
|
|
<include name="subsystems/gps.h"/>
|
|
<include name="subsystems/actuators.h"/>
|
|
<include name="subsystems/actuators/motor_mixing.h"/>
|
|
<!--define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT" cond="ifndef MODE_MANUAL"/>
|
|
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD" cond="ifndef MODE_AUTO1"/-->
|
|
<define name="MODE_AUTO2" value="AP_MODE_NAV" cond="ifndef MODE_AUTO2"/>
|
|
<define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/>
|
|
<define name="DLModeNav()" value="(autopilot_mode_auto2 == AP_MODE_NAV)"/>
|
|
<define name="DLModeGuided()" value="(autopilot_mode_auto2 == AP_MODE_GUIDED)"/>
|
|
</includes>
|
|
|
|
<settings>
|
|
<dl_setting var="autopilot.kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
|
|
<dl_setting var="autopilot_mode_auto2" min="2" step="1" max="3" module="autopilot" values="NAV|GUIDED"/>
|
|
</settings>
|
|
|
|
<control_block name="set_actuators">
|
|
<call fun="SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
|
|
<call fun="SetActuatorsFromCommands(commands, autopilot_mode)"/>
|
|
</control_block>
|
|
|
|
<control_block name="attitude_loop">
|
|
<call fun="stabilization_attitude_read_rc(autopilot_in_flight(), FALSE, FALSE)"/>
|
|
<call fun="stabilization_attitude_run(autopilot_in_flight())"/>
|
|
</control_block>
|
|
|
|
<control_block name="throttle_direct">
|
|
<call fun="guidance_v_read_rc()"/>
|
|
<call fun="guidance_v_z_sp = stateGetPositionNed_i()->z"/>
|
|
<call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t"/>
|
|
</control_block>
|
|
|
|
<control_block name="altitude_loop">
|
|
<call fun="gv_update_ref_from_z_sp(guidance_v_z_sp)"/>
|
|
<call fun="run_hover_loop(autopilot_in_flight())"/>
|
|
<!--call fun="SaturateThrottle(rc_values)"/-->
|
|
</control_block>
|
|
|
|
<exceptions>
|
|
<exception cond="too_far_from_home" deroute="HOME"/>
|
|
<exception cond="kill_switch_is_on()" deroute="KILL"/>
|
|
</exceptions>
|
|
|
|
<mode name="ATTITUDE_DIRECT" shortname="ATT">
|
|
<select cond="RCMode0()"/>
|
|
<on_enter>
|
|
<call fun="stabilization_attitude_enter()"/>
|
|
</on_enter>
|
|
<control freq="NAV_FREQ">
|
|
<call fun="nav_periodic_task()"/>
|
|
</control>
|
|
<control>
|
|
<call_block name="attitude_loop"/>
|
|
<call_block name="throttle_direct"/>
|
|
<call_block name="set_actuators"/>
|
|
</control>
|
|
<exception cond="RCLost()" deroute="FAILSAFE"/>
|
|
</mode>
|
|
|
|
<mode name="ATTITUDE_Z_HOLD" shortname="A_ZH">
|
|
<select cond="RCMode1()"/>
|
|
<on_enter>
|
|
<call fun="stabilization_attitude_enter()"/>
|
|
<call fun="guidance_v_z_enter()"/>
|
|
</on_enter>
|
|
<control freq="NAV_FREQ">
|
|
<call fun="nav_periodic_task()"/>
|
|
</control>
|
|
<control>
|
|
<call_block name="attitude_loop"/>
|
|
<call_block name="altitude_loop"/>
|
|
<call_block name="set_actuators"/>
|
|
</control>
|
|
<exception cond="RCLost()" deroute="FAILSAFE"/>
|
|
</mode>
|
|
|
|
<mode name="NAV">
|
|
<select cond="RCMode2() && DLModeNav()" exception="HOME"/>
|
|
<on_enter>
|
|
<call fun="guidance_h_nav_enter()"/>
|
|
<call fun="stabilization_attitude_enter()"/>
|
|
<call fun="guidance_v_z_enter()"/>
|
|
</on_enter>
|
|
<control freq="NAV_FREQ">
|
|
<call fun="nav_periodic_task()"/>
|
|
</control>
|
|
<control>
|
|
<call fun="guidance_v_read_rc()"/>
|
|
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
|
|
<call fun="guidance_v_from_nav(autopilot_in_flight())"/>
|
|
<call fun="guidance_h_from_nav(autopilot_in_flight())"/>
|
|
<call_block name="set_actuators"/>
|
|
</control>
|
|
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
|
|
</mode>
|
|
|
|
<mode name="GUIDED">
|
|
<select cond="RCMode2() && DLModeGuided()" exception="HOME"/>
|
|
<on_enter>
|
|
<call fun="guidance_h_hover_enter()"/>
|
|
<call fun="stabilization_attitude_enter()"/>
|
|
<call fun="guidance_v_mode = GUIDANCE_V_MODE_GUIDED"/>
|
|
<call fun="guidance_v_guided_enter()"/>
|
|
</on_enter>
|
|
<control freq="NAV_FREQ">
|
|
<call fun="nav_periodic_task()"/>
|
|
</control>
|
|
<control>
|
|
<call fun="guidance_v_read_rc()"/>
|
|
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
|
|
<call fun="guidance_v_guided_run(autopilot_in_flight())"/>
|
|
<call fun="guidance_h_guided_run(autopilot_in_flight())"/>
|
|
<call_block name="set_actuators"/>
|
|
</control>
|
|
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
|
|
</mode>
|
|
|
|
<mode name="HOME">
|
|
<on_enter>
|
|
<call fun="guidance_h_nav_enter()"/>
|
|
<call fun="stabilization_attitude_enter()"/>
|
|
<call fun="guidance_v_z_enter()"/>
|
|
</on_enter>
|
|
<control freq="NAV_FREQ">
|
|
<call fun="nav_home()"/>
|
|
</control>
|
|
<control>
|
|
<call fun="guidance_v_read_rc()"/>
|
|
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
|
|
<call fun="guidance_v_from_nav(autopilot_in_flight())"/>
|
|
<call fun="guidance_h_from_nav(autopilot_in_flight())"/>
|
|
<call_block name="set_actuators"/>
|
|
</control>
|
|
<exception cond="GpsIsLost()" deroute="FAILSAFE"/>
|
|
</mode>
|
|
|
|
<!-- Safe landing -->
|
|
<mode name="FAILSAFE" shortname="FAIL">
|
|
<on_enter>
|
|
<call fun="stabilization_attitude_set_failsafe_setpoint()"/>
|
|
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
|
|
<call fun="guidance_v_zd_sp = SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED)"/>
|
|
</on_enter>
|
|
<control>
|
|
<call fun="stabilization_attitude_set_failsafe_setpoint()"/>
|
|
<call fun="stabilization_attitude_run(autopilot_in_flight())"/>
|
|
<call fun="gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z)"/>
|
|
<call fun="run_hover_loop(autopilot_in_flight())"/>
|
|
<call_block name="set_actuators"/>
|
|
</control>
|
|
<exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
|
|
</mode>
|
|
|
|
<!-- Kill throttle mode -->
|
|
<mode name="KILL">
|
|
<select cond="$DEFAULT_MODE"/>
|
|
<select cond="kill_switch_is_on()"/>
|
|
<on_enter>
|
|
<call fun="autopilot_set_in_flight(false)"/>
|
|
<call fun="autopilot_set_motors_on(false)"/>
|
|
<call fun="stabilization_cmd[COMMAND_THRUST] = 0"/>
|
|
</on_enter>
|
|
<control>
|
|
<call fun="SetCommands(commands_failsafe)"/>
|
|
</control>
|
|
</mode>
|
|
|
|
</state_machine>
|
|
|
|
</autopilot>
|