<?xml version="1.0"?>

<!--	KFC 250 Autopilot based on Generic Autopilot Configuration	-->
<!-- partly based on IT-AUTOFLIGHT by Joshua Davidson, thanks to them! -->

<PropertyList>
	
	<!--	Flight Director System Active	-->
	<logic>
		<input>
			<property>/autopilot/switches/fd</property>
			<greater-than>
				<property>/systems/electrical/outputs/fds-ap</property>
				<value>14</value>
			</greater-than>
		</input>
		<output>/autopilot/internal/flight-director-active</output>
	</logic>
	
	<!--	Autopilot System Active	-->
	<logic>
		<input>
			<property>/autopilot/switches/ap</property>
			<greater-than>
				<property>/systems/electrical/outputs/fds-ap</property>
				<value>14</value>
			</greater-than>
		</input>
		<output>/autopilot/internal/autopilot-active</output>
	</logic>
	
	<!--	Yaw Damper System Active	-->
	<logic>
		<input>
			<property>/autopilot/switches/yd</property>
			<greater-than>
				<property>/systems/electrical/outputs/fds-ap</property>
				<value>14</value>
			</greater-than>
		</input>
		<output>/autopilot/internal/yaw-damper-active</output>
	</logic>
	
	<!--	LATERAL MODES	-->
	<!--	Wing Level	-->
	<pi-simple-controller>
		<name>Wing Level</name>
		<debug>false</debug>
		<enable>
			<prop>/autopilot/locks/lat-mode</prop>
			<value>0</value>
		</enable>
		<input>
			<prop>/instrumentation/turn-indicator/indicated-turn-rate</prop>
		</input>
		<reference>
			<value>0.0</value>
		</reference>
		<output>
			<prop>/autopilot/fd/roll-command-deg</prop>
		</output>
		<config>
			<Kp>-1.0</Kp>
			<Ki>0.0</Ki>
			<min>-25</min>
			<max>25</max>
		</config>
	</pi-simple-controller>
	<!-- HDG SEL -->
	<filter>
		<name>heading bug error computer/normalizer</name>
		<debug>false</debug>
		<type>gain</type>
		<input>
			<property>autopilot/settings/heading-bug-deg</property>
			<offset>
				<property>orientation/heading-magnetic-deg</property>
				<scale>-1.0</scale>
			</offset>
		</input>
		<output>autopilot/internal/heading-bug-error-deg</output>
		<period>
			<min>-180</min>
			<max>180</max>
		</period>
		<gain>1.0</gain>
	</filter>
	<pi-simple-controller>
		<name>Heading Bug Hold</name>
		<debug>false</debug>
		<enable>
			<prop>/autopilot/locks/lat-mode</prop>
			<value>1</value>
		</enable>
		<input>
			<prop>/autopilot/internal/heading-bug-error-deg</prop>
		</input>
		<reference>
			<value>0.0</value>
		</reference>
		<output>
			<prop>/autopilot/fd/roll-command-deg</prop>
		</output>
		<config>
			<Kp>-1.0</Kp>
			<Ki>0.0</Ki>
			<min>-25</min>
			<max>25</max>
		</config>
	</pi-simple-controller>
	<!-- NAV -->
	<filter>
		<name>NAV Error</name>
		<debug>false</debug>
		<type>gain</type>
		<input>
			<condition>
				<less-than>
					<property>/instrumentation/nav[0]/heading-needle-deflection-norm</property>
					<value>-0.3</value>
				</less-than>
			</condition>
			<expression>
				<difference>
					<difference>
						<property>/instrumentation/nav[0]/radials/selected-deg</property>
						<value>15</value>
					</difference>
					<property>/orientation/heading-magnetic-deg</property>
				</difference>
			</expression>
		</input>
		<input>
			<condition>
				<greater-than>
					<property>/instrumentation/nav[0]/heading-needle-deflection-norm</property>
					<value>0.3</value>
				</greater-than>
			</condition>
			<expression>
				<difference>
					<sum>
						<property>/instrumentation/nav[0]/radials/selected-deg</property>
						<value>15</value>
					</sum>
					<property>/orientation/heading-magnetic-deg</property>
				</difference>
			</expression>
		</input>
		<input>
			<condition>
				<greater-than-equals>
					<property>/instrumentation/nav[0]/heading-needle-deflection-norm</property>
					<value>-0.3</value>
				</greater-than-equals>
				<less-than-equals>
					<property>/instrumentation/nav[0]/heading-needle-deflection-norm</property>
					<value>0.3</value>
				</less-than-equals>
			</condition>
			<expression>
				<sum>
					<difference>
						<property>/instrumentation/nav[0]/radials/selected-deg</property>
						<property>/orientation/heading-magnetic-deg</property>
					</difference>
					<product>
						<property>/instrumentation/nav[0]/heading-needle-deflection</property>
						<value>10</value>
					</product>
				</sum>
			</expression>
		</input>
		<output>autopilot/internal/nav-error-deg</output>
		<period>
			<min>-180</min>
			<max>180</max>
		</period>
		<gain>1.0</gain>
	</filter>
	<pi-simple-controller>
		<name>NAV</name>
		<debug>false</debug>
		<enable>
			<prop>/autopilot/locks/lat-mode</prop>
			<value>2</value>
		</enable>
		<input>
			<prop>autopilot/internal/nav-error-deg</prop>
		</input>
		<reference>
			<value>0.0</value>
		</reference>
		<output>
			<prop>/autopilot/fd/roll-command-deg</prop>
		</output>
		<config>
			<Kp>-1.0</Kp>
			<Ki>0.0</Ki>
			<min>-25</min>
			<max>25</max>
		</config>
	</pi-simple-controller>

	<!--	VERTICAL MODES	-->
	<!-- Pitch holder -->
	<filter>
		<name>Pitch hold</name>
		<type>gain</type>
		<debug>false</debug>
		<gain>1.0</gain>
		<enable>
			<prop>/autopilot/locks/vert-mode</prop>
			<value>0</value>
		</enable>
		<input>
			<prop>/autopilot/settings/target-pitch-deg</prop>
		</input>
		<output>
			<prop>/autopilot/fd/pitch-command-deg</prop>
		</output>
	</filter>
	<!-- ALT HOLD -->
	<pi-simple-controller>
		<name>Altitude Hold (Altimeter based) Stage 1</name>
		<debug>false</debug>
		<enable>
			<prop>/autopilot/locks/vert-mode</prop>
			<value>1</value>
		</enable>
		<input>
			<prop>/instrumentation/altimeter/indicated-altitude-ft</prop>
		</input>
		<reference>
			<prop>/autopilot/settings/target-altitude-ft</prop>
		</reference>
		<output>
			<prop>/autopilot/internal/target-climb-rate-fps</prop>
		</output>
		<config>
			<Kp>0.3</Kp>          <!-- proportional gain -->
			<Ki>0.0</Ki>          <!-- integral gain -->
			<min>-10</min> <!-- 600 fpm -->
			<max>10</max> <!-- 600 fpm -->
		</config>
	</pi-simple-controller>
	<pi-simple-controller>
		<name>Altitude Hold (Altimeter based) Stage 2</name>
		<debug>false</debug>
		<enable>
			<prop>/autopilot/locks/vert-mode</prop>
			<value>1</value>
		</enable>
		<input>
			<prop>/velocities/vertical-speed-fps</prop>
		</input>
		<reference>
			<prop>/autopilot/internal/target-climb-rate-fps</prop>
		</reference>
		<output>
			<prop>/autopilot/fd/pitch-command-deg</prop>
		</output>
		<config>
			<Kp>0.05</Kp>
			<Ki>0.02</Ki>
			<min>-10</min>
			<max>10</max>
		</config>
	</pi-simple-controller>

	<!--	Yaw Damper -->
	<pi-simple-controller>
		<name>yaw damper</name>
		<debug>false</debug>
		<enable>
			<prop>/autopilot/switches/yd</prop>
		</enable>
		<input>
			<prop>/instrumentation/slip-skid-ball/indicated-slip-skid</prop>
		</input>
		<reference>
			<value>0.0</value>
		</reference>
		<output>
			<prop>/autopilot/servos/yaw/command</prop>
		</output>
		<config>
			<Kp>0.125</Kp>
			<Ki>0.01</Ki>
		</config>
		<min>-0.25</min>
		<max>0.25</max>
	</pi-simple-controller>

	<!--	Servo Commands	-->
	<pi-simple-controller>
		<name>Pitch Servo Controller</name>
		<debug>false</debug>
		<enable>
			<prop>/autopilot/internal/autopilot-active</prop>
		</enable>
		<input>
			<prop>/orientation/pitch-deg</prop>
		</input>
		<reference>
			<prop>/autopilot/fd/pitch-command-deg</prop>
		</reference>
		<output>
			<prop>/autopilot/servos/pitch/command</prop>
		</output>
		<config>
			<Kp>-0.05</Kp>
			<Ki>-0.0005</Ki>
		</config>
	</pi-simple-controller>
	<pi-simple-controller>
		<name>Roll Servo Controller</name>
		<debug>false</debug>
		<enable>
			<prop>/autopilot/internal/autopilot-active</prop>
		</enable>
		<input>
			<prop>/orientation/roll-deg</prop>
		</input>
		<reference>
			<prop>/autopilot/fd/roll-command-deg</prop>
		</reference>
		<output>
			<prop>/autopilot/servos/roll/command</prop>
		</output>
		<config>
			<Kp>0.05</Kp>
			<Ki>0.001</Ki>
		</config>
	</pi-simple-controller>
</PropertyList>
