Control: New speed on pitch for generic autopilot
This commit is contained in:
Josh Davidson
Stuart Buchanan
parent
b9b496d33b
commit
6e33987a91
1 changed files with 225 additions and 97 deletions
|
|
@ -34,6 +34,9 @@ pitch-limit-max
|
|||
|
||||
pitch-limit-min
|
||||
- Set the minimum pitch angle in degrees
|
||||
|
||||
speed-by-pitch-accel
|
||||
- Set the maximum acceleration rate for speed on pitch
|
||||
-->
|
||||
|
||||
<PropertyList>
|
||||
|
|
@ -423,10 +426,26 @@ pitch-limit-min
|
|||
<output>/autopilot/internal/mach</output>
|
||||
</filter>
|
||||
|
||||
<filter> <!-- Filter knots -->
|
||||
<name>Knots Lag</name>
|
||||
<type>exponential</type>
|
||||
<input>/velocities/airspeed-kt</input> <!-- This should NOT change no matter what, do not use the current knots prop -->
|
||||
<output>/autopilot/internal/kts-input-lag</output>
|
||||
<filter-time>0.04</filter-time>
|
||||
</filter>
|
||||
|
||||
<filter> <!-- Average knots -->
|
||||
<name>Knots Average</name>
|
||||
<type>moving-average</type>
|
||||
<input>/autopilot/internal/kts-input-lag</input>
|
||||
<output>/autopilot/internal/kts-input-avg</output>
|
||||
<samples>60</samples>
|
||||
</filter>
|
||||
|
||||
<filter> <!-- Compute knots rate -->
|
||||
<name>Knots Rate</name>
|
||||
<type>derivative</type>
|
||||
<input>/velocities/airspeed-kt</input> <!-- This should NOT change no matter what, do not use the current knots prop -->
|
||||
<input>/autopilot/internal/kts-input-avg</input>
|
||||
<output>/autopilot/internal/kts-rate-raw</output>
|
||||
<filter-time>1.0</filter-time>
|
||||
</filter>
|
||||
|
|
@ -436,7 +455,64 @@ pitch-limit-min
|
|||
<type>exponential</type>
|
||||
<input>/autopilot/internal/kts-rate-raw</input>
|
||||
<output>/autopilot/internal/kts-rate</output>
|
||||
<filter-time>0.08</filter-time>
|
||||
<filter-time>0.04</filter-time>
|
||||
</filter>
|
||||
|
||||
<filter> <!-- Filter mach -->
|
||||
<name>Mach Lag</name>
|
||||
<type>exponential</type>
|
||||
<input>/velocities/mach</input> <!-- This should NOT change no matter what, do not use the current mach prop -->
|
||||
<output>/autopilot/internal/mach-input-lag</output>
|
||||
<filter-time>0.04</filter-time>
|
||||
</filter>
|
||||
|
||||
<filter> <!-- Average mach -->
|
||||
<name>Mach Average</name>
|
||||
<type>moving-average</type>
|
||||
<input>/autopilot/internal/mach-input-lag</input>
|
||||
<output>/autopilot/internal/mach-input-avg</output>
|
||||
<samples>60</samples>
|
||||
</filter>
|
||||
|
||||
<filter> <!-- Compute mach rate -->
|
||||
<name>Mach Rate</name>
|
||||
<type>derivative</type>
|
||||
<input>/autopilot/internal/mach-input-avg</input>
|
||||
<output>/autopilot/internal/mach-rate-raw</output>
|
||||
<filter-time>1.0</filter-time>
|
||||
</filter>
|
||||
|
||||
<filter> <!-- Multiply mach rate -->
|
||||
<name>Mach Rate Gain</name>
|
||||
<type>gain</type>
|
||||
<gain>650</gain>
|
||||
<input>/autopilot/internal/mach-rate-raw</input>
|
||||
<output>/autopilot/internal/mach-rate-gain</output>
|
||||
</filter>
|
||||
|
||||
<filter> <!-- Filter mach rate -->
|
||||
<name>Mach Rate Lag</name>
|
||||
<type>exponential</type>
|
||||
<input>/autopilot/internal/mach-rate-raw</input>
|
||||
<output>/autopilot/internal/mach-rate</output>
|
||||
<filter-time>0.04</filter-time>
|
||||
</filter>
|
||||
|
||||
<filter> <!-- Acceleration limit for speed on pitch -->
|
||||
<name>Flch Acceleration Limit</name>
|
||||
<type>gain</type>
|
||||
<gain>1.0</gain>
|
||||
<input>
|
||||
<condition>
|
||||
<greater-than>
|
||||
<property>/autopilot/config/speed-by-pitch-accel</property>
|
||||
<value>0</value>
|
||||
</greater-than>
|
||||
</condition>
|
||||
<property>/autopilot/config/speed-by-pitch-accel</property>
|
||||
</input>
|
||||
<input>1.5</input>
|
||||
<output>/autopilot/internal/flch-accel</output>
|
||||
</filter>
|
||||
|
||||
<filter> <!-- Knots rate computer for speed on pitch -->
|
||||
|
|
@ -456,7 +532,7 @@ pitch-limit-min
|
|||
<property>/autopilot/settings/target-speed-mach</property>
|
||||
<property>/autopilot/internal/mach</property>
|
||||
</difference>
|
||||
<value>80</value>
|
||||
<value>100</value>
|
||||
</product>
|
||||
</expression>
|
||||
</input>
|
||||
|
|
@ -467,13 +543,16 @@ pitch-limit-min
|
|||
<property>/autopilot/settings/target-speed-kt</property>
|
||||
<property>/autopilot/internal/airspeed-kt</property>
|
||||
</difference>
|
||||
<value>0.08</value>
|
||||
<value>0.1</value>
|
||||
</product>
|
||||
</expression>
|
||||
</input>
|
||||
<output>/autopilot/internal/flch-kts-error-cmd</output>
|
||||
<min>-1.5</min>
|
||||
<max>1.5</max>
|
||||
<min>
|
||||
<property>/autopilot/internal/flch-accel</property>
|
||||
<scale>-1</scale>
|
||||
</min>
|
||||
<max>/autopilot/internal/flch-accel</max>
|
||||
</filter>
|
||||
|
||||
<filter> <!-- Knots rate command filter -->
|
||||
|
|
@ -493,7 +572,7 @@ pitch-limit-min
|
|||
</not-equals>
|
||||
</and>
|
||||
</condition>
|
||||
<value>2.5</value>
|
||||
<value>1.5</value>
|
||||
</max-rate-of-change>
|
||||
<max-rate-of-change>100</max-rate-of-change>
|
||||
</filter>
|
||||
|
|
@ -610,7 +689,7 @@ pitch-limit-min
|
|||
<property>/velocities/airspeed-kt</property>
|
||||
<entry><ind> 80</ind><dep>0.016</dep></entry>
|
||||
<entry><ind>360</ind><dep>0.007</dep></entry>
|
||||
<entry><ind>600</ind><dep>0.002</dep></entry>
|
||||
<entry><ind>600</ind><dep>0.003</dep></entry>
|
||||
</table>
|
||||
<property>/autopilot/internal/gain-factor</property>
|
||||
</product>
|
||||
|
|
@ -788,59 +867,150 @@ pitch-limit-min
|
|||
<max-rate-of-change>10000</max-rate-of-change>
|
||||
</filter>
|
||||
|
||||
<pid-controller> <!-- Speed on pitch target fpm controller -->
|
||||
<name>IT-CONTROLLER: Speed By Pitch</name>
|
||||
<enable>
|
||||
<filter> <!-- Speed on pitch target fpm delta controller -->
|
||||
<name>IT-CONTROLLER: Speed By Pitch Delta</name>
|
||||
<type>gain</type>
|
||||
<gain>
|
||||
<expression>
|
||||
<table>
|
||||
<property>/velocities/airspeed-kt</property>
|
||||
<entry><ind>140</ind><dep> 700</dep></entry>
|
||||
<entry><ind>360</ind><dep>1400</dep></entry>
|
||||
<entry><ind>600</ind><dep>2100</dep></entry>
|
||||
</table>
|
||||
</expression>
|
||||
</gain>
|
||||
<input>
|
||||
<condition>
|
||||
<equals>
|
||||
<property>/autopilot/locks/speed</property>
|
||||
<value>mach-with-pitch-trim</value>
|
||||
</equals>
|
||||
</condition>
|
||||
<property>/autopilot/internal/mach-rate</property>
|
||||
</input>
|
||||
<input>/autopilot/internal/kts-rate</input>
|
||||
<reference>/autopilot/internal/flch-kts-error</reference>
|
||||
<output>/autopilot/internal/flch-raw</output>
|
||||
<min>
|
||||
<condition>
|
||||
<or>
|
||||
<equals>
|
||||
<property>/gear/gear[1]/wow</property>
|
||||
<value>1</value>
|
||||
</equals>
|
||||
<equals>
|
||||
<property>/gear/gear[2]/wow</property>
|
||||
<value>1</value>
|
||||
</equals>
|
||||
</or>
|
||||
</condition>
|
||||
<value>0</value>
|
||||
</min>
|
||||
<min>-2000</min>
|
||||
<max>2000</max>
|
||||
</filter>
|
||||
|
||||
<filter> <!-- Filter speed on pitch target fpm delta -->
|
||||
<name>Speed By Pitch Filter</name>
|
||||
<type>noise-spike</type>
|
||||
<input>/autopilot/internal/flch-raw</input>
|
||||
<output>/autopilot/internal/flch-filtered</output>
|
||||
<max-rate-of-change>
|
||||
<condition>
|
||||
<property>/autopilot/internal/speed-on-pitch-active</property>
|
||||
</condition>
|
||||
</enable>
|
||||
<input>/autopilot/internal/kts-rate</input>
|
||||
<reference>/autopilot/internal/flch-kts-error</reference>
|
||||
<output>/autopilot/internal/target-fpm-flch</output>
|
||||
<config>
|
||||
<Kp>
|
||||
<expression>
|
||||
<table>
|
||||
<property>/velocities/airspeed-kt</property>
|
||||
<entry><ind> 80</ind><dep>-380</dep></entry>
|
||||
<entry><ind>360</ind><dep>-430</dep></entry>
|
||||
<entry><ind>600</ind><dep>-470</dep></entry>
|
||||
</table>
|
||||
</expression>
|
||||
</Kp>
|
||||
<Ti>1.3</Ti>
|
||||
<Td>0.0</Td>
|
||||
<u_min>
|
||||
<expression>
|
||||
<difference>
|
||||
<property>/autopilot/internal/vertical-speed-fpm</property>
|
||||
<value>2000</value>
|
||||
</difference>
|
||||
</expression>
|
||||
</u_min>
|
||||
<u_max>
|
||||
<expression>
|
||||
<sum>
|
||||
<property>/autopilot/internal/vertical-speed-fpm</property>
|
||||
<value>2000</value>
|
||||
</sum>
|
||||
</expression>
|
||||
</u_max>
|
||||
</config>
|
||||
</pid-controller>
|
||||
<value>1500</value>
|
||||
</max-rate-of-change>
|
||||
<max-rate-of-change>10000</max-rate-of-change>
|
||||
</filter>
|
||||
|
||||
<filter> <!-- Smooth engagement by syncing when disabled -->
|
||||
<name>Speed By Pitch Inactive Sync</name>
|
||||
<filter> <!-- Compute speed on pitch target fpm -->
|
||||
<name>Speed By Pitch Final</name>
|
||||
<type>gain</type>
|
||||
<gain>1.0</gain>
|
||||
<enable>
|
||||
<condition>
|
||||
<not><property>/autopilot/internal/speed-on-pitch-active</property></not>
|
||||
</condition>
|
||||
</enable>
|
||||
<input>/autopilot/internal/vertical-speed-fpm</input>
|
||||
<input>
|
||||
<expression>
|
||||
<sum>
|
||||
<property>/autopilot/internal/vert-speed-fpm</property>
|
||||
<property>/autopilot/internal/flch-filtered</property>
|
||||
</sum>
|
||||
</expression>
|
||||
</input>
|
||||
<output>/autopilot/internal/target-fpm-flch</output>
|
||||
<min>
|
||||
<condition>
|
||||
<or>
|
||||
<equals>
|
||||
<property>/gear/gear[1]/wow</property>
|
||||
<value>1</value>
|
||||
</equals>
|
||||
<equals>
|
||||
<property>/gear/gear[2]/wow</property>
|
||||
<value>1</value>
|
||||
</equals>
|
||||
</or>
|
||||
</condition>
|
||||
<value>0</value>
|
||||
</min>
|
||||
<min>-100000</min>
|
||||
<max>100000</max>
|
||||
</filter>
|
||||
|
||||
<filter> <!-- Filter target fpm -->
|
||||
<name>Target FPM Filter</name>
|
||||
<type>noise-spike</type>
|
||||
<input> <!-- Must be on top of the others -->
|
||||
<condition>
|
||||
<property>/autopilot/internal/speed-on-pitch-active</property>
|
||||
</condition>
|
||||
<property>/autopilot/internal/target-fpm-flch</property>
|
||||
</input>
|
||||
<input>
|
||||
<condition>
|
||||
<equals>
|
||||
<property>/autopilot/locks/altitude</property>
|
||||
<value>altitude-hold</value>
|
||||
</equals>
|
||||
</condition>
|
||||
<property>/autopilot/internal/target-fpm-alt</property>
|
||||
</input>
|
||||
<input>
|
||||
<condition>
|
||||
<equals>
|
||||
<property>/autopilot/locks/altitude</property>
|
||||
<value>agl-hold</value>
|
||||
</equals>
|
||||
</condition>
|
||||
<property>/autopilot/internal/target-fpm-agl</property>
|
||||
</input>
|
||||
<input>
|
||||
<condition>
|
||||
<equals>
|
||||
<property>/autopilot/locks/altitude</property>
|
||||
<value>fpa-hold</value>
|
||||
</equals>
|
||||
</condition>
|
||||
<property>/autopilot/internal/target-fpm-fpa</property>
|
||||
</input>
|
||||
<input>
|
||||
<condition>
|
||||
<equals>
|
||||
<property>/autopilot/locks/altitude</property>
|
||||
<value>gs1-hold</value>
|
||||
</equals>
|
||||
</condition>
|
||||
<property>/autopilot/internal/nav1-rate-of-climb-fpm</property>
|
||||
</input>
|
||||
<input>/autopilot/internal/target-fpm-vs</input>
|
||||
<output>/autopilot/internal/target-fpm</output>
|
||||
<max-rate-of-change>
|
||||
<condition>
|
||||
<property>/autopilot/internal/vs-active</property>
|
||||
</condition>
|
||||
<value>700</value>
|
||||
</max-rate-of-change>
|
||||
<max-rate-of-change>10000</max-rate-of-change>
|
||||
</filter>
|
||||
|
||||
<pid-controller> <!-- Main fpm controller -->
|
||||
|
|
@ -851,49 +1021,7 @@ pitch-limit-min
|
|||
</condition>
|
||||
</enable>
|
||||
<input>/autopilot/internal/vertical-speed-fpm</input>
|
||||
<reference> <!-- Must be on top of the others -->
|
||||
<condition>
|
||||
<property>/autopilot/internal/speed-on-pitch-active</property>
|
||||
</condition>
|
||||
<property>/autopilot/internal/target-fpm-flch</property>
|
||||
</reference>
|
||||
<reference>
|
||||
<condition>
|
||||
<equals>
|
||||
<property>/autopilot/locks/altitude</property>
|
||||
<value>altitude-hold</value>
|
||||
</equals>
|
||||
</condition>
|
||||
<property>/autopilot/internal/target-fpm-alt</property>
|
||||
</reference>
|
||||
<reference>
|
||||
<condition>
|
||||
<equals>
|
||||
<property>/autopilot/locks/altitude</property>
|
||||
<value>agl-hold</value>
|
||||
</equals>
|
||||
</condition>
|
||||
<property>/autopilot/internal/target-fpm-agl</property>
|
||||
</reference>
|
||||
<reference>
|
||||
<condition>
|
||||
<equals>
|
||||
<property>/autopilot/locks/altitude</property>
|
||||
<value>fpa-hold</value>
|
||||
</equals>
|
||||
</condition>
|
||||
<property>/autopilot/internal/target-fpm-fpa</property>
|
||||
</reference>
|
||||
<reference>
|
||||
<condition>
|
||||
<equals>
|
||||
<property>/autopilot/locks/altitude</property>
|
||||
<value>gs1-hold</value>
|
||||
</equals>
|
||||
</condition>
|
||||
<property>/autopilot/internal/nav1-rate-of-climb-fpm</property>
|
||||
</reference>
|
||||
<reference>/autopilot/internal/target-fpm-vs</reference>
|
||||
<reference>/autopilot/internal/target-fpm</reference>
|
||||
<output>/autopilot/internal/target-pitch-deg</output>
|
||||
<config>
|
||||
<Kp>
|
||||
|
|
|
|||
Loading…
Reference in a new issue