From 90b65247eb399c3574495f065c42d8031d8dd3ac Mon Sep 17 00:00:00 2001
From: Stuart Buchanan <stuart_d_buchanan@yahoo.co.uk>
Date: Thu, 14 Feb 2019 22:29:01 +0000
Subject: [PATCH] FG1000: GFC700 ALTS/ALT vertical autopilot modes

GFC700 now supports the Selected Altitude Capture (ALTS) and Altitude
Hold (ALT) modes.

ALTS is automatically armed in more vertical modes, and allows capture
of whatever altitude is selected.
---
 .../FG1000/GFC700-vertical-FSM.xml            | 89 +++++++++++++++++--
 Aircraft/Instruments-3d/FG1000/GFC700.xml     | 16 ++--
 .../Instruments-3d/FG1000/Nasal/Commands.nas  | 11 ++-
 3 files changed, 96 insertions(+), 20 deletions(-)

diff --git a/Aircraft/Instruments-3d/FG1000/GFC700-vertical-FSM.xml b/Aircraft/Instruments-3d/FG1000/GFC700-vertical-FSM.xml
index af471c65a..e23eb7f03 100644
--- a/Aircraft/Instruments-3d/FG1000/GFC700-vertical-FSM.xml
+++ b/Aircraft/Instruments-3d/FG1000/GFC700-vertical-FSM.xml
@@ -157,17 +157,17 @@
 
 <transition>
   <name>ALTS Capture</name>
-  <!-- Altitude Capture Mode enabled when we get within 500ft of the selected altitude -->
+  <!-- Altitude Capture Mode enabled when we get within 200ft of the selected altitude -->
   <source>ALTS-ARM</source>
   <target>ALTS</target>
   <condition>
     <less-than>
-      <property>/autopilot/internal/target-altitude-delta-ft</property>
-      <value>500</value>
+      <property>/autopilot/internal/ALTS-target-altitude-delta-ft</property>
+      <value>200</value>
     </less-than>
     <greater-than>
-      <property>/autopilot/internal/target-altitude-delta-ft</property>
-      <value>-500</value>
+      <property>/autopilot/internal/ALTS-target-altitude-delta-ft</property>
+      <value>-200</value>
     </greater-than>
   </condition>
 </transition>
@@ -180,7 +180,7 @@
       <!-- Set up the 2nd target, so that it is independent of the Selected Altitude,
        then annunciator to the reference value current target value, then enable
       the altitude capture function  -->
-      setprop("/autopilot/settings/target-altitude-ft-2", getprop("/autopilot/settings/target-altitude-ft"));
+      setprop("/autopilot/settings/target-altitude-ft-2", getprop("/autopilot/settings/target-alt-ft"));
       setprop("/autopilot/annunciator/vertical-mode-target",
         sprintf("%ift", getprop("/autopilot/settings/target-altitude-ft-2"))
       );
@@ -218,10 +218,10 @@
   <target>ALTS-ARM</target>
   <!-- Transitions must have a condition element.  So set it to True -->
   <condition>
-    <not>
+    <greater-than>
       <property>/sim/time/elapsed-sec</property>
       <value>0</value>
-    </not>
+    </greater-than>
   </condition>
 </transition>
 
@@ -275,6 +275,18 @@
   </enter>
 </state>
 
+<state>
+  <name>VNV</name>
+</state>
+
+<state>
+  <name>BC</name>
+</state>
+
+<state>
+  <name>FLC</name>
+</state>
+
 <!-- From the OFF state, pressing any of FD, AP, CWS, NAV, BC, APR, HDG changes to PIT mode -->
 <transition>
   <name>OFF-PIT</name>
@@ -335,7 +347,7 @@
 
 <!-- From any state pressing VS changes to VS mode -->
 <transition>
-  <name>VS</name>
+  <name>VS-ON</name>
   <target>VS</target>
   <exclude-target>true</exclude-target>
   <condition>
@@ -343,6 +355,9 @@
   </condition>
 </transition>
 
+
+<!-- Transitions to switch off various AP modes, which return to PIT mode -->
+
 <!-- From any state, pressing GA goes to GA mode -->
 <transition>
   <name>GA</name>
@@ -371,4 +386,60 @@
   </binding>
 </transition>
 
+<!-- From VS state pressing VS changes back to to PIT mode -->
+<transition>
+  <name>VS-OFF</name>
+  <source>VS</source>
+  <target>PIT</target>
+  <exclude-target>true</exclude-target>
+  <condition>
+    <equals><property>/autopilot/vertical-mode-button</property><value>VS</value></equals>
+  </condition>
+</transition>
+
+<!-- From ALT state pressing ALT changes back to to PIT mode -->
+<transition>
+  <name>ALT-OFF</name>
+  <source>ALT</source>
+  <target>PIT</target>
+  <exclude-target>true</exclude-target>
+  <condition>
+    <equals><property>/autopilot/vertical-mode-button</property><value>ALT</value></equals>
+  </condition>
+</transition>
+
+<!-- From VNV state pressing ALT changes back to to PIT mode -->
+<transition>
+  <name>VNV-OFF</name>
+  <source>VNV</source>
+  <target>PIT</target>
+  <exclude-target>true</exclude-target>
+  <condition>
+    <equals><property>/autopilot/vertical-mode-button</property><value>VNV</value></equals>
+  </condition>
+</transition>
+
+<!-- From BC state pressing BC changes back to to PIT mode -->
+<transition>
+  <name>BC-OFF</name>
+  <source>BC</source>
+  <target>PIT</target>
+  <exclude-target>true</exclude-target>
+  <condition>
+    <equals><property>/autopilot/vertical-mode-button</property><value>BC</value></equals>
+  </condition>
+</transition>
+
+<!-- From BC state pressing BC changes back to to PIT mode -->
+<transition>
+  <name>FLC-OFF</name>
+  <source>FLC</source>
+  <target>PIT</target>
+  <exclude-target>true</exclude-target>
+  <condition>
+    <equals><property>/autopilot/vertical-mode-button</property><value>FLC</value></equals>
+  </condition>
+</transition>
+
+
 </PropertyList>
diff --git a/Aircraft/Instruments-3d/FG1000/GFC700.xml b/Aircraft/Instruments-3d/FG1000/GFC700.xml
index 251cfe0f0..b266e77e4 100644
--- a/Aircraft/Instruments-3d/FG1000/GFC700.xml
+++ b/Aircraft/Instruments-3d/FG1000/GFC700.xml
@@ -174,9 +174,9 @@
     <debug>false</debug>
     <type>gain</type>
     <input>
-      <property>/autopilot/settings/target-altitude-ft</property>
+      <property>/autopilot/settings/target-alt-ft</property>
       <offset>
-        <property>/instrumentation/altimeter/indicated-altitude-ft-2</property>
+        <property>/instrumentation/altimeter/indicated-altitude-ft</property>
         <scale>-1.0</scale>
       </offset>
     </input>
@@ -467,15 +467,15 @@
        <prop>/autopilot/settings/target-pitch-deg</prop>
      </output>
      <config>
-       <Kp>-0.01</Kp>      <!-- proportional gain -->
+       <Kp>0.1</Kp>       <!-- proportional gain -->
        <beta>1.0</beta>    <!-- input value weighing factor -->
        <alpha>0.1</alpha>  <!-- low pass filter weighing factor -->
        <gamma>0.0</gamma>  <!-- input value weighing factor for -->
                            <!-- unfiltered derivative error -->
-       <Ti>10.0</Ti>       <!-- integrator time -->
+       <Ti>1.0</Ti>        <!-- integrator time -->
        <Td>0.00001</Td>    <!-- derivator time -->
-       <u_min>-1.0</u_min> <!-- minimum output clamp -->
-       <u_max>1.0</u_max>  <!-- maximum output clamp -->
+       <u_min>-15.0</u_min><!-- minimum output clamp -->
+       <u_max>15.0</u_max> <!-- maximum output clamp -->
      </config>
    </pid-controller>
 
@@ -496,7 +496,7 @@
       </condition>
     </enable>
     <input>
-      <prop>/position/altitude-ft</prop>
+      <prop>/instrumentation/altimeter/indicated-altitude-ft</prop>
     </input>
     <reference>
       <prop>/autopilot/settings/target-altitude-ft</prop>
@@ -528,7 +528,7 @@
       </condition>
     </enable>
     <input>
-      <prop>/position/altitude-ft</prop>
+      <prop>/instrumentation/altimeter/indicated-altitude-ft</prop>
     </input>
     <reference>
       <prop>/autopilot/settings/target-altitude-ft-2</prop>
diff --git a/Aircraft/Instruments-3d/FG1000/Nasal/Commands.nas b/Aircraft/Instruments-3d/FG1000/Nasal/Commands.nas
index a1f79b6db..f454e1a1a 100644
--- a/Aircraft/Instruments-3d/FG1000/Nasal/Commands.nas
+++ b/Aircraft/Instruments-3d/FG1000/Nasal/Commands.nas
@@ -20,19 +20,24 @@ io.include("Constants.nas");
 removecommand("FG1000HardKeyPushed");
 addcommand("FG1000HardKeyPushed",
   func(node) {
-    var device = int(node.getNode("device", 1).getValue());
+    var device = node.getNode("device", 1).getValue();
     var name = node.getNode("notification",1).getValue();
 
     # The knob animation stores the value as an offset property
     var value = node.getNode("offset", 1).getValue();
 
+    if (name == nil) {
+      print("FG1000HardKeyPushed: No <name> argument passed to fgcommand");
+      return;
+    }
+
     if (value == nil) {
       print("FG1000HardKeyPushed: No <offset> argument passed to fgcommand");
       return;
     }
 
     if (device == nil) {
-      print("FG1000HardKeyPushed: Unknown device" ~ node.getNode("device").getValue());
+      print("FG1000HardKeyPushed: No <device> argument passed to fgcommand for " ~ name);
       return;
     }
 
@@ -49,7 +54,7 @@ addcommand("FG1000HardKeyPushed",
 
     var notification = notifications.PFDEventNotification.new(
       "MFD",
-      device,
+      int(device),
       notifications.PFDEventNotification.HardKeyPushed,
       { Id: name, Value: value }
     );