Struggling with Waypoint Navigation #1299

ChewJinCheng · 2025-06-27T03:33:47Z

ChewJinCheng
Jun 27, 2025

I was following Section 5. Modeling a Waypoint Navigation System from https://jsbsim.sourceforge.net/JSBSimReferenceManual.pdf.

My script runs safely without any runtime error but according to the logs, the plane is not moving towards the set waypoints after successful takeoff.

After adding debugging logs, I found that fcs/wp-distance was always 0 despite correct ap/wp long and lat values.
An example output:
Time Notify (Event 0) executed at time: 56.008109
Latitude (geod, rad): = 0.513943
Longitude (geod, rad): = -1.660959
Altitude (AGL, ft): = 805.744635
Heading (true, rad): = 6.172515
Calibrated Airspeed (kts): = 73.290845
X-Accel (forward, ft/s2): = 11.929118
Y-Accel (side_sway, ft/s2): = 0.237707
Z-Accel (vertical, ft/s2): = -29.933293
Active Waypoint #: = 2.000000
Target WP Latitude (rad): = 0.518238
Target WP Longitude (rad): = -1.664727
Distance to WP: = 0.000000
I am unable to figure out why.

These are my main changes:

In c310ap.xml

ap/attitude_hold
ap/altitude_hold
ap/heading_hold
ap/yaw_damper
ap/active-waypoint
ap/altitude_setpoint
ap/heading_setpoint
ap/heading-setpoint-select
ap/aileron_cmd
ap/elevator_cmd
ap/rudder_cmd
ap/airspeed_setpoint
ap/airspeed_hold
ap/throttle-cmd-norm

<!-- Waypoint Navigation Properties -->

<property> ap/wp_latitude_rad </property>
<property> ap/wp_longitude_rad </property>

<property>fcs/delta-lat-rad</property>
<property>fcs/delta-lon-rad</property>
<property>fcs/heading-to-waypoint-rad</property>
<property>fcs/wp-distance-a</property>
<property>fcs/wp-distance</property>
<property>fcs/heading-to-waypoint-positive</property>
<property>fcs/wp-heading-corrector</property>
<property>fcs/wp-heading-deg</property>

<!--

=====================================================
Waypoint Navigation Functions

-->

<fcs_function name="fcs/delta-lat-rad">
	<function>
		<difference>
			<property>ap/wp_latitude_rad</property>
			<property>position/lat-gc-rad</property>
		</difference>
	</function>
	<output>fcs/delta-lat-rad</output>
</fcs_function>

<fcs_function name="fcs/delta-lon-rad">
	<function>
		<difference>
			<property>ap/wp_longitude_rad</property>
			<property>position/long-gc-rad</property>
		</difference>
	</function>
	<output>fcs/delta-lon-rad</output>
</fcs_function>

<fcs_function name="fcs/heading-to-waypoint-rad">
	<function>
		<atan2>
			<product>
				<sin>
					<property>fcs/delta-lon-rad</property>
				</sin>
				<cos>
					<property>ap/wp_latitude_rad</property>
				</cos>
			</product>
			<difference>
				<product>
					<cos>
						<property>position/lat-gc-rad</property>
					</cos>
					<sin>
						<property>ap/wp_latitude_rad</property>
					</sin>
				</product>
				<product>
					<sin>
						<property>position/lat-gc-rad</property>
					</sin>
					<cos>
						<property>ap/wp_latitude_rad</property>
					</cos>
					<cos>
						<property>fcs/delta-lon-rad</property>
					</cos>
				</product>
			</difference>
		</atan2>
	</function>
	<output>fcs/heading-to-waypoint-rad</output>
</fcs_function>

<fcs_function name="fcs/wp-distance-a">
	<function>
		<sum>
			<pow>
				<sin>
					<quotient>
						<property>fcs/delta-lat-rad</property>
						<value>2.0</value>
					</quotient>
				</sin>
				<value>2.0</value>
			</pow>
			<product>
				<cos>
					<property>position/lat-gc-rad</property>
				</cos>
				<cos>
					<property>ap/wp_latitude_rad</property>
				</cos>
				<pow>
					<sin>
						<quotient>
							<property>fcs/delta-lon-rad</property>
							<value>2.0</value>
						</quotient>
					</sin>
					<value>2.0</value>
				</pow>
			</product>
		</sum>
	</function>
	<output>fcs/wp-distance-a</output>
</fcs_function>

<fcs_function name="fcs/wp-distance">
	<function>
		<product>
			<value>2.0</value>
			<value>21144000</value>
			<!-- Earth radius in feet -->
			<atan2>
				<pow>
					<property>fcs/wp-distance-a</property>
					<value>0.5</value>
				</pow>
				<pow>
					<difference>
						<value>1.0</value>
						<property>fcs/wp-distance-a</property>
					</difference>
					<value>0.5</value>
				</pow>
			</atan2>
		</product>
	</function>
	<output>fcs/wp-distance</output>
</fcs_function>

<!-- Ensure heading is positive -->
<summer name="fcs/heading-to-waypoint-positive">
	<input> fcs/heading-to-waypoint-rad </input>
	<bias> 6.283 </bias>
	<!-- 2 * PI -->
</summer>

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

<!-- Convert to degrees -->
<pure_gain name="fcs/wp-heading-deg">
	<input> fcs/wp-heading-corrector </input>
	<gain> 57.2958 </gain>
</pure_gain>

My reset01.xml:

3.0 29.594656 -95.16384722

<orientation unit="DEG" frame="LOCAL">
	<yaw>   0.0  </yaw>
</orientation>

<velocity unit="FT/SEC" frame="LOCAL">
	<x> 0.0 </x>
	<y> 0.0 </y>
	<z> 0.0 </z>
</velocity>

<attitude_rate unit="DEG/SEC" frame="LOCAL">
	<x> 0.0 </x>
	<y> 0.0 </y>
	<z> 0.0 </z>
</attitude_rate>

<elevation unit="FT">   26.0  </elevation>

My script:

This run is for testing the c310 waypoint navigation and cruise performance

<run start="0.0" end="3600" dt="0.0083333">
	
	<property> simulation/notify-time-trigger </property>
	
	<property> ap/wp_latitude_rad </property>
	<property> ap/wp_longitude_rad </property>

	<property>fcs/delta-lat-rad</property>
	<property>fcs/delta-lon-rad</property>
	<property>fcs/heading-to-waypoint-rad</property>
	<property>fcs/wp-distance-a</property>
	<property>fcs/wp-distance</property>
	<property>fcs/heading-to-waypoint-positive</property>
	<property>fcs/wp-heading-corrector</property>
	<property>fcs/wp-heading-deg</property>

	<event name="Time Notify" persistent="true">
		<description>Output message at fixed time intervals</description>
		<condition> simulation/sim-time-sec >= simulation/notify-time-trigger </condition>
		<set name="simulation/notify-time-trigger" value="0.5" type="FG_DELTA"/>
		<notify>
			<property caption="Latitude (geod, rad):             "> position/lat-gc-rad </property>
			<property caption="Longitude (geod, rad):            "> position/long-gc-rad </property>
			<property caption="Altitude (AGL, ft):               "> position/h-agl-ft </property>
			<property caption="Heading (true, rad):              "> attitude/heading-true-rad </property>
			<property caption="Calibrated Airspeed (kts):        "> velocities/vc-kts </property>

			<!-- Acceleration vectors -->
			<property caption="X-Accel (forward, ft/s2):         "> accelerations/a-pilot-x-ft_sec2 </property>
			<property caption="Y-Accel (side_sway, ft/s2):       "> accelerations/a-pilot-y-ft_sec2 </property>
			<property caption="Z-Accel (vertical, ft/s2):        "> accelerations/a-pilot-z-ft_sec2 </property>

			<!-- Waypoint status -->
			<property caption="Active Waypoint #:                "> ap/active-waypoint </property>
			<property caption="Target WP Latitude (rad):         "> ap/wp_latitude_rad </property>
			<property caption="Target WP Longitude (rad):        "> ap/wp_longitude_rad </property>
			<property caption="Distance to WP:                   ">fcs/wp-distance</property>
		</notify>

	</event>

	<event name="Start engine">
		<condition>
			simulation/sim-time-sec  ge  0.01
		</condition>
		<set name="fcs/mixture-cmd-norm[0]" value="0.87"/>
		<set name="fcs/mixture-cmd-norm[1]" value="0.87"/>
		<set name="fcs/advance-cmd-norm[0]" value="1.0"/>
		<set name="fcs/advance-cmd-norm[1]" value="1.0"/>
		<set name="fcs/throttle-cmd-norm[0]" value="1.0"/>
		<set name="fcs/throttle-cmd-norm[1]" value="1.0"/>
		
		<set name="propulsion/magneto_cmd" value="3"/>
		<set name="propulsion/starter_cmd" value="1"/>

		<!-- Set initial autopilot parameters -->
		<set name="ap/altitude_setpoint" action="FG_EXP" value="1000" tc="10"/>
		<set name="ap/attitude_hold" value="0"/>
		<set name="ap/wp_latitude_rad" value="0.517238"/>
		<set name="ap/wp_longitude_rad" value="-1.662727"/>
		<set name="ap/heading_setpoint" value="355"/>
		<set name="ap/heading-setpoint-select" value="0"/>
		<set name="ap/heading_hold" value="1"/>
		<set name="ap/active-waypoint" value="0"/>
		<notify/>
	</event>

	<event name="Set altitude for 1,000 ft.">
		<condition>velocities/vc-fps ge 135.0</condition>
		<set name="ap/altitude_hold" value="1"/>
		<notify/>
	</event>

	<event name="Raise landing gear">
		<condition>
			position/h-agl-ft ge 20
			velocities/vc-kts ge 60
		</condition>
		<set name="gear/gear-cmd-norm" value="0"/>
		<notify/>
	</event>

	<event name="Head to first waypoint">
		<description>
			Set heading hold to selected waypoint (setpoint) instead of
			previously specified heading when altitude surpasses 800 feet.
		</description>
		<condition>position/h-agl-ft ge 800</condition>
		<set name="ap/heading-setpoint-select" value="1"/>
		<set name="ap/active-waypoint" value="1"/>
		<notify>
			<property>fcs/wp-distance</property>
		</notify>
	</event>

	<event name="Set second waypoint">
		<description>
			When the distance to the first waypoint (Hobby
			Airport threshold) is less than 10 feet, then
			set the second waypoint.
		</description>
		<condition>
			fcs/wp-distance lt 10
			ap/active-waypoint eq 1
		</condition>
		<set name="ap/wp_latitude_rad" value="0.518238"/>
		<set name="ap/wp_longitude_rad" value="-1.664727"/>
		<set name="ap/active-waypoint" value="2"/>
		<notify>
			<property>fcs/wp-distance</property>
		</notify>
	</event>
	
	</run>

seanmcleod70 · 2025-06-30T16:02:38Z

seanmcleod70
Jun 30, 2025
Maintainer

Can you post what you did to fix the issue? So that other JSBSim users in the future who do a search regarding JSBSim autopilot issues they're having might come across your post with your solution.

1 reply

ChewJinCheng Jul 4, 2025
Author

I think there was an error in the AP Roll Heading Hold section of c310ap.xml.

Change this line:
<test value="guidance/wp-heading-deg">

to:
<test value="fcs/wp-heading-deg">

Also, when adding the new functions for waypoint navigation, make sure they are encapsulated within a <channel> tag.

Here is the full block to add:

<property>ap/wp_latitude_rad</property>
<property>ap/wp_longitude_rad</property>
<property>fcs/delta-lat-rad</property>
<property>fcs/delta-lon-rad</property>
<property>fcs/heading-to-waypoint-rad</property>
<property>fcs/wp-distance-a</property>
<property>fcs/wp-distance</property>
<property>fcs/heading-to-waypoint-positive</property>
<property>fcs/wp-heading-corrector</property>
<property>fcs/wp-heading-deg</property>

<channel name="Waypoint Navigation Channel">
<fcs_function name="fcs/delta-lat-rad">
<function>
<difference>
<property>ap/wp_latitude_rad</property>
<property>position/lat-gc-rad</property>
</difference>
</function>
</fcs_function>

<fcs_function name="fcs/delta-lon-rad">
<function>
<difference>
<property>ap/wp_longitude_rad</property>
<property>position/long-gc-rad</property>
</difference>
</function>
</fcs_function>

<fcs_function name="fcs/heading-to-waypoint-rad">
<function>
<atan2>
<product>
<sin><property>fcs/delta-lon-rad</property></sin>
<cos><property>ap/wp_latitude_rad</property></cos>
</product>
<difference>
<product>
<cos><property>position/lat-gc-rad</property></cos>
<sin><property>ap/wp_latitude_rad</property></sin>
</product>
<product>
<sin><property>position/lat-gc-rad</property></sin>
<cos><property>ap/wp_latitude_rad</property></cos>
<cos><property>fcs/delta-lon-rad</property></cos>
</product>
</difference>
</atan2>
</function>
</fcs_function>

<fcs_function name="fcs/wp-distance-a">
<function>
<sum>
<pow>
<sin>
<quotient>
<property>fcs/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>ap/wp_latitude_rad</property></cos>
<pow>
<sin>
<quotient>
<property>fcs/delta-lon-rad</property>
<value>2.0</value>
</quotient>
</sin>
<value>2.0</value>
</pow>
</product>
</sum>
</function>
</fcs_function>

<fcs_function name="fcs/wp-distance">
<function>
<product>
<value>2.0</value>
<atan2>
<pow>
<property>fcs/wp-distance-a</property>
<value>0.5</value>
</pow>
<pow>
<difference>
<value>1.0</value>
<property>fcs/wp-distance-a</property>
</difference>
<value>0.5</value>
</pow>
</atan2>
<value>21144000</value>
</product>
</function>
</fcs_function>

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

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

<pure_gain name="fcs/wp-heading-deg">
<input>fcs/wp-heading-corrector</input>
<gain>57.3</gain>
</pure_gain>
</channel>

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Struggling with Waypoint Navigation #1299

Uh oh!

{{title}}

Uh oh!

Replies: 1 comment 1 reply

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{editor}}'s edit

{{editor}}'s edit

Uh oh!

Select a reply

Uh oh!

Struggling with Waypoint Navigation #1299

Uh oh!

ChewJinCheng Jun 27, 2025

===================================================== Waypoint Navigation Functions

Replies: 1 comment · 1 reply

Uh oh!

seanmcleod70 Jun 30, 2025 Maintainer

Uh oh!

Uh oh!

ChewJinCheng Jul 4, 2025 Author

ChewJinCheng
Jun 27, 2025

=====================================================
Waypoint Navigation Functions

Replies: 1 comment 1 reply

seanmcleod70
Jun 30, 2025
Maintainer

ChewJinCheng Jul 4, 2025
Author