[Jsbsim-cvslogs] CVS: JSBSim/check_cases/piston_takeoff/systems Autopilot.xml, NONE, 1.1 GNCUtilities.xml, NONE, 1.1 Makefile.am, NONE, 1.1

[Jon S. B. <jb...@us...>]

Update of /cvsroot/jsbsim/JSBSim/check_cases/piston_takeoff/systems
In directory vz-cvs-3.sog:/tmp/cvs-serv16347/piston_takeoff/systems

Added Files:
	Autopilot.xml GNCUtilities.xml Makefile.am 
Log Message:
Adding new and modified tests

--- NEW FILE ---
<?xml version="1.0"?>
<system name="Autopilot"
  xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
  xsi:noNamespaceSchemaLocation="http://jsbsim.sourceforge.net/JSBSimSystem.xsd">

<!-- #######################################################################

  Author:   Jon Berndt
  Date:     January 2009
  Function: Autopilot

  Description:

  This file contains general purpose autopilot functions for JSBSim aircraft.
  
  Usage
  =====
  
  This file contains algorithms that depend on parameters calculated in the
  file, GNCUtilities.xml, which should be included prior to the inclusion of
  this file, e.g.,
  
  <system file="GNCUtilities"/>

  <system file="Autopilot"/>
   
  Inputs: 

    [Declared Interface Parameters]
    ap/roll-attitude-mode - This should be set to 0 for wings level or to 1
                            for roll angle hold.
    ap/autopilot-roll-on - this is either 0 (off) or 1. This turns the roll
                           autopilot on or off.

    [Overridable constants]
    guidance/roll-angle-limit = 0.524 (default value in radians)
    guidance/roll-rate-limit = 0.174 (default value in radians)

  Introduced parameters (temporary terms):

  Outputs:
    ap/roll-cmd-norm-output - roll command to the flight control roll channel
                              in a normalized range from -1 to +1.

######################################################################## -->

  <property> ap/roll-attitude-mode </property>
  <property> ap/autopilot-roll-on  </property>
  
  <!-- Initial constants (can be overridden in the aircraft file) -->

  <property value="0.524"> guidance/roll-angle-limit </property>
  <property value="0.174"> guidance/roll-rate-limit </property>

  <property value="0"> ap/roll-angle-saturation </property>

<!-- #################################################################### -->

  <channel name="Heading Hold">

    <switch name="ap/roll-control-autopilot-on">
      <default value="0"/>
      <test value="1">
        ap/autopilot-roll-on ne 0.0
      </test>
    </switch>

    <switch name="ap/roll-autopilot-windup-trigger">
      <default value="1"/>
      <test value="1">
        ap/roll-angle-saturation ne 0.0
      </test>
      <test value="0">
        ap/roll-control-autopilot-on gt 0
      </test>
    </switch>

    <pure_gain name="ap/limited-roll-angle-rad">
      <description>
        This component takes as input the angle to the target heading from
        the current heading, as calculated by the Angle to Heading channel
        in the GNC Utilities system file. The output of this component (stored)
        in the component property "ap/limited-roll-angle-rad" is the roll
        angle command, which is just the correct-sense heading error, but 
        nominally limited at 30 degrees (0.524 radians), though this can be
        overridden.
      </description>
      <input> guidance/angle-to-heading-rad </input>
      <gain> guidance/angle-to-heading-sense </gain>
      <clipto>
        <min> -guidance/roll-angle-limit </min>
        <max>  guidance/roll-angle-limit </max>
      </clipto>
    </pure_gain>

    <switch name="ap/roll-attitude-selector">
      <description>
        This component selects whether to use a roll angle of "0" - which
        is a wings-level attitude - or to use the roll angle commanded by the 
        ap/limited-roll-angle-rad component, above, which is used to acquire
        and hold a heading.
        Set ap/roll-attitude-mode to 1 for roll angle hold,
                                  to 0 for wings level (roll attitude of zero).
      </description>
      <default value="0"/>
      <test value="ap/limited-roll-angle-rad">
        ap/roll-attitude-mode eq 1
      </test>
    </switch>
    
    <summer name="ap/limited-roll-angle-error">
      <description>
        This component computes the error between the calculated target roll
        angle and the actual roll angle. The output is interpreted as a roll
        rate command.
      </description>
      <input>  ap/roll-attitude-selector </input>
      <input> -attitude/phi-rad </input>
    </summer>

    <pid name="ap/roll-rate-pid-control">
      <input> ap/limited-roll-angle-error </input>
      <kp> 30 </kp>
      <ki> 0.1  </ki>
      <kd> 0.1  </kd>
      <trigger> ap/roll-autopilot-windup-trigger </trigger>
      <clipto>
        <min> -guidance/roll-rate-limit </min>
        <max>  guidance/roll-rate-limit </max>
      </clipto>
    </pid>

    <pure_gain name="ap/total-limited-roll-rate">
      <input>guidance/roll-rate-limit</input>
      <gain>2.0</gain>
    </pure_gain>
    
    <deadband name="ap/roll-rate-saturation">
      <input> ap/roll-rate-pid-control </input>
      <width> ap/total-limited-roll-rate </width>
    </deadband>

    <summer name="ap/limited-roll-rate-error">
      <description>
        This component computes the error between the calculated target roll
        rate and the actual roll rate. The output is interpreted as an aileron
        command.
      </description>
      <input> ap/roll-rate-pid-control </input>
      <input> -velocities/p-aero-rad_sec </input>
    </summer>

    <pid name="ap/roll-command-pid-control">
      <input> ap/limited-roll-rate-error </input>
      <kp>2.5</kp>
      <ki>0.0</ki>
      <kd>0.0</kd>
      <trigger> ap/roll-autopilot-windup-trigger </trigger>
    </pid>

    <lag_filter name="ap/roll-cmd-smoother">
      <input> ap/roll-command-pid-control </input>
      <c1> 100.0 </c1>
    </lag_filter>

    <switch name="ap/roll-cmd-norm-output">
      <default value="0"/>
      <test value="ap/roll-cmd-smoother">
        ap/roll-control-autopilot-on ne 0.0
      </test>
    </switch>

    <switch name="ap/roll-command-selector-steering">
      <default value="0.0"/>
      <test value="ap/roll-cmd-norm-output">
        ap/roll-control-autopilot-on == 1
        gear/unit[0]/WOW == 1
      </test>
      <output>fcs/steer-cmd-norm</output>
    </switch>

  </channel>

</system>

--- NEW FILE ---
<?xml version="1.0"?>
<system name="GNC Utilities"
        xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
        xsi:noNamespaceSchemaLocation="http://jsbsim.sourceforge.net/JSBSimSystem.xsd">

<!-- #######################################################################

  Author:   Jon Berndt
  Date:     January 2009
  Function: Calculates useful GNC values

  Inputs: 
    position/lat-gc-rad (geocentric latitude in radians)
    position/long-gc-rad (longitude in radians)

    [Declared Interface Parameters]
    guidance/target_wp_latitude_rad
    guidance/target_wp_longitude_rad

  Introduced parameters (temporary terms):
    guidance/delta-lat-rad
    guidance/delta-lon-rad
    guidance/heading-to-waypoint-rad
    guidance/wp-distance-a
    guidance/heading-to-waypoint-positive

  Outputs
    guidance/wp-distance (feet)
    guidance/wp-heading-rad (rad)
    guidance/wp-heading-deg (deg)

  The calculations, below, implement the Haversine formulas to calculate
  heading and distance to a set of lat/long coordinates from the current
  position. The latitude and longitude are expected to be in radian units
  and are measured from the 0 meridian and the equator, with positive 
  longitude being east from there, and positive latitude being north.

  The basic equations are:

  R = earth’s radius
  Δlat = lat2 − lat1
  Δlong = long2 − long1
  a = sin²(Δlat/2) + cos(lat1)∙cos(lat2)∙sin²(Δlong/2)
  c = 2∙atan2(√a, √(1−a))
  d = R∙c
  θ = atan2(sin(Δlong)∙cos(lat2), cos(lat1)∙sin(lat2) − sin(lat1) ∙cos(lat2)∙cos(Δlong) )

  Usage
  =====

  To use this system, include it in a <system> element as follows:
  
  <system file="GNCUtilities"/>
  
  Prior to including including the above system, you will want to create
  another system in which you set the target lat/lon (in radians). The
  waypoint latitude and longitude properties are mentioned above
  (Declared Interface Parameters):
  
    guidance/target_wp_latitude_rad
    guidance/target_wp_longitude_rad

  The outputs from the AP Waypoint Heading Director channel are the heading
  to the waypoint from the current location, and the distance to the waypoint
  from the current position,
  
    fcs/wp-distance
    fcs/wp-heading-rad

######################################################################## -->

  <property> guidance/target_wp_latitude_rad </property>
  <property> guidance/target_wp_longitude_rad </property>

  <channel name="Guidance Waypoint Heading Director">

    <fcs_function name="guidance/delta-lat-rad">
      <!-- Delta latitude in radians -->
      <function>
        <difference>
          <property> guidance/target_wp_latitude_rad </property>
          <property> position/lat-gc-rad </property>
        </difference>
      </function>
    </fcs_function>

    <fcs_function name="guidance/delta-lon-rad">
      <!-- Delta longitude in radians -->
      <function>
        <difference>
          <property> guidance/target_wp_longitude_rad </property>
          <property> position/long-gc-rad </property>
        </difference>
      </function>
    </fcs_function>

    <fcs_function name="guidance/heading-to-waypoint-rad">
      <!--
      The heading in radians to a specified waypoint, given the
      current position.
      -->
      <function>
        <atan2> <!-- atan2 (deltaY, deltaX )-->
          <product>
            <sin><property> guidance/delta-lon-rad </property></sin>
            <cos><property> guidance/target_wp_latitude_rad </property></cos>
          </product>
          <difference>
            <product>
              <cos><property> position/lat-gc-rad </property></cos>
              <sin><property> guidance/target_wp_latitude_rad </property></sin>
            </product>
            <product>
              <sin><property> position/lat-gc-rad </property></sin>
              <cos><property> guidance/target_wp_latitude_rad </property></cos>
              <cos><property> guidance/delta-lon-rad </property></cos>
            </product>
          </difference>
        </atan2>
      </function>
    </fcs_function>

    <fcs_function name="guidance/wp-distance-a">
      <!--
      This is an intermediate distance calculation between
      the current location and the specified waypoint.
      -->
      <function>
        <sum>
          <pow>
            <sin>
              <quotient>
                <property> guidance/delta-lat-rad </property>
                <value> 2.0 </value>
              </quotient>
            </sin>
            <value>2</value>
          </pow>
          <product>
            <cos>
              <property> position/lat-gc-rad </property>
            </cos>
            <cos>
              <property> guidance/target_wp_latitude_rad </property>
            </cos>
            <pow>
              <sin>
                <quotient>
                  <property> guidance/delta-lon-rad </property>
                  <value> 2.0 </value>
                </quotient>
              </sin>
              <value>2.0</value>
            </pow>
          </product>
        </sum>
      </function>
    </fcs_function> 

    <fcs_function name="guidance/wp-distance">
      <!--
        This is the final distance calculation between the current location and
        the specified waypoint. If you want this calculation to output in units
        other than feet, changed the notated value below to the radius of the
        Earth given in your units of choice.
      -->
      <function>
        <product>
          <value> 2.0 </value>
          <atan2>
            <pow>
              <property> guidance/wp-distance-a </property>
              <value> 0.5 </value>
            </pow>
            <pow>
              <difference>
                <value> 1.0 </value>
                <property> guidance/wp-distance-a </property>
              </difference>
              <value> 0.5 </value>
            </pow>
          </atan2>
          <value> 21144000 </value> <!-- radius of the Earth in feet -->
        </product>
      </function>
    </fcs_function> 

    <summer name="guidance/heading-to-waypoint-positive">
      <input> guidance/heading-to-waypoint-rad </input>
      <bias> 6.283 </bias>
    </summer>

    <switch name="guidance/wp-heading-rad">
      <default value="guidance/heading-to-waypoint-positive"/>
      <test value="guidance/heading-to-waypoint-rad">
        guidance/heading-to-waypoint-rad gt 0.0
      </test>
    </switch>

    <pure_gain name="guidance/wp-heading-deg">
      <input> guidance/wp-heading-rad </input>
      <gain> 57.3 </gain>
    </pure_gain>

  </channel>

<!-- ####################################################################### -->

<!--

  The Included Angle to Heading channel is used to find the smallest included angle
  (the angle less than or equal to 180 degrees) to a specified heading from
  the current heading. The sense of the rotation to get to that angle is also
  calculated (positive 1 for a clockwise rotation, negative 1 for counter-
  clockwise).
  
  The inputs to this channel are the interface parameters declared below:

  Inputs:
  
    navigation/actual-heading-rad
    ap/specified-heading-rad
    ap/heading-selector-switch
  
  Outputs:
  
    ap/angle-to-heading-rad
    ap/angle-to-heading-sense (+/- 1)
  
  The angle to the heading is calculated as follows:

  Given an angle phi:

  V = cos(phi)i + sin(phi)j     (this is a unit vector)

  The dot product for two, 2D vectors is written:

  V1*V2 = |V1||V2|cos(phi)

  Since the magnitude of a unit vector is 1, we can write the equation as follows:

  V1*V2 = cos(phi)

  or,

  phi  = acos(V1*V2)

  or,

  phi  = acos[ cos(phi1)cos(phi2) + sin(phi1)sin(phi2) ]

  The included angle is always positive and always less than 180 degrees.

  Usage
  =====

  To use this channel, the actual heading should be copied into the
  navigation/actual-heading-rad property. The desired target heading needs to be
  copied into the ap/specified-heading-rad property - OR, the heading calculated
  above from a given waypoint lat/lon can be used. The ap/heading-selector-switch
  property should be set according to how the target heading should be supplied.
  If the waypoint heading should be used, ap/heading-selector-switch should be
  set to anything other than "1". If the heading selector switch is set to "1",
  then whatever heading is copied into ap/specified-heading-rad will be used.
  Note that the term "heading" can refer to ground track, compass heading,
  magnetic heading, or whatever. One simply needs to make sure that the 
  spedified heading and the value copied into actual heading refer to the 
  same type of heading. Note that the waypoint heading will always be the 
  compass heading to the given waypoint lat/lon.
-->

  <property> navigation/actual-heading-rad </property>
  <property> guidance/specified-heading-rad </property>
  <property> guidance/heading-selector-switch </property>

  <channel name="Included Angle to Heading">

    <switch name="guidance/selected_target_heading">
      <default value="guidance/wp-heading-rad"/>
      <test value="guidance/specified-heading-rad">
        guidance/heading-selector-switch eq 1
      </test>
    </switch>

    <fcs_function name="guidance/x1">
      <function>
        <cos><p> navigation/actual-heading-rad </p></cos>
      </function>
    </fcs_function>
  
    <fcs_function name="guidance/y1">
      <function>
        <sin><p> navigation/actual-heading-rad </p></sin>
      </function>
    </fcs_function>

    <fcs_function name="guidance/x2">
      <function>
        <cos><p> guidance/selected_target_heading </p></cos>
      </function>
    </fcs_function>
  
    <fcs_function name="guidance/y2">
      <function>
        <sin><p> guidance/selected_target_heading </p></sin>
      </function>
    </fcs_function>

    <fcs_function name="guidance/angle-to-heading-rad">
      <function>
        <acos>
          <sum>
            <product>
              <p>guidance/x1</p>
              <p>guidance/x2</p>
            </product>
            <product>
              <p>guidance/y1</p>
              <p>guidance/y2</p>
            </product>
          </sum>
        </acos>
      </function>
    </fcs_function>

    <fcs_function name="guidance/x1y2">
      <function>
        <product>
          <p> guidance/x1 </p>
          <p> guidance/y2 </p>
        </product>
      </function>
    </fcs_function>

    <fcs_function name="guidance/x2y1">
      <function>
        <product>
          <p> guidance/x2 </p>
          <p> guidance/y1 </p>
        </product>
      </function>
    </fcs_function>

    <switch name="guidance/angle-to-heading-sense">
      <default value="-1"/>
      <test value="1">
        guidance/x1y2 gt guidance/x2y1
      </test>
    </switch>

  </channel>

<!-- ####################################################################### -->

</system>

--- NEW FILE ---
EXTRA_DIST = \
	Autopilot.xml \
	GNCUtilities.xml