Tag: python

Introduction

Sorry it has taken me so long to write this post! The last post on the blog was October 19th – almost 6 weeks ago. Life happens. We have a 15 month old running around and she is a handful!

Anyways, back to the next topic – coding a pitch/roll (2 axis) autopilot in X-Plane with Python with altitude and heading hold. Today we will be adding the following:

• Real-time graphing for 6 parameters
• Additional method to grab data out of X-Plane
• A normalize function to limit outputs to reasonable values
• Altitude preselect and hold function

The full code will be at the end of this post.

Video Link

coming soon

Contents

1. Adding PyQtGraph
2. Developing a normalize function
3. Initializing the data structures to hold the graph values
4. Defining the PyQtGraph window and parameters
5. Getting more data points out of X-Plane
6. Feeding the graph data structures with data
7. Adding altitude preselect and hold

1 – Adding PyQtGraph

Pip is my preferred tool to manage Python packages. It is easy and works well. I initially tried graphing with MatPlotLib but it took 200-300ms to update, which really dragged down the control loop to the point of being unusable. Instead, we will be using PyQtGraph. Install it with Pip:

pip install pyqtgraph

2 – Developing a normalize function

This task is pretty straightforward. There are a couple places where we want to pass values that need to be within a certain range. The first example is to the client.sendCTRL() method to set the control surfaces in X-Plane. The documentation states values are expected to be from -1 to 1. I have got some really weird results sending values outside that range (specifically for throttle, if you send something like 4, you can end up with 400% throttle which is wayyy more than the engines can actually output).

# this function takes an input and either passes it through or adjusts
# the input to fit within the specified max/min values
def normalize(value, min=-1, max=1):
	# if value = 700, and max = 20, return 20
	# if value = -200, and min = -20, return -20
	if (value > max):
		return max
	elif (value < min):
		return min
	else:
		return value

3 – Initializing the graphing data structures

We need a couple of arrays to store the data for our graphs. We need (desired data to plot) + 1 arrays. The +1 is the x-axis, which will just store values like 0,1,2,3,etc. The others will be the y-values. We haven’t added the altitude stuff yet, so you can add them but they won’t be used yet.

x_axis_counters = [] #0, 1, 2, 3, etc. just basic x-axis values used for plotting
roll_history = []
pitch_history = []
#altitude_history = []
roll_setpoint_history = []
pitch_setpoint_history = []
#altitude_setpoint_history = []
plot_array_max_length = 100 # how many data points to hold in our arrays and graph
i = 1 # initialize x_axis_counter

4 – Defining the PyQtGraph window and parameters

Working with PyQtGraph more or less means we’ll be working with a full blown GUI (just stripped down).

# first the base app needs to be instantiated
app = pg.mkQApp("python xplane autopilot monitor")

# now the window itself is defined and sized
win = pg.GraphicsLayoutWidget(show=True)
win.resize(1000,600) #pixels
win.setWindowTitle("XPlane autopilot system control")

# we have 3 subplots
p1 = win.addPlot(title="roll",row=0,col=0)
p2 = win.addPlot(title="pitch",row=1,col=0)
p3 = win.addPlot(title="altitude", row=2, col=0)

# show the y grid lines to make it easier to interpret the graphs
p1.showGrid(y=True)
p2.showGrid(y=True)
p3.showGrid(y=True)

5 – Getting more data points out of X-Plane

The initial .getPOSI() method that came in the example has worked well for us so far. But at this point we need more data that isn’t available in the .getPOSI() method. We will be utilizing a different method called .getDREFs() which is short for ‘get data references’. We will need to construct a list of data references we want to retrieve, pass that list to the method, and then parse the output. It is more granular than .getPOSI(). I haven’t evaluated the performance but I don’t think it is a problem.

The DREFs we want are for indicated airspeed, magnetic heading (.getPOSI() has true heading, not magnetic), an indicator to show if we are on the ground or not, and height as understood by the flight model. Thus, we can define our DREFs as follows:

DREFs = ["sim/cockpit2/gauges/indicators/airspeed_kts_pilot",
		"sim/cockpit2/gauges/indicators/heading_electric_deg_mag_pilot",
		"sim/flightmodel/failures/onground_any",
		"sim/flightmodel/misc/h_ind"]

And we can get the data with client.getDREFs(DREFs). The returned object is a 2d array. We need to parse out our values of interest. The full data gathering code looks like this:

posi = client.getPOSI();
ctrl = client.getCTRL();
multi_DREFs = client.getDREFs(DREFs)

current_roll = posi[4]
current_pitch = posi[3]
current_hdg = multi_DREFs[1][0]
current_altitude = multi_DREFs[3][0]
current_asi = multi_DREFs[0][0]
onground = multi_DREFs[2][0]

With those data points, we have everything we need to start plotting the state of our aircraft and monitoring for PID tuning.

6 – Feeding the real-time graphs with data

Next up is actually adding data to be plotted. There are two scenarios to consider when adding data to the arrays: 1) the arrays have not yet reached the limit we set earlier (100 points), and 2) they have. Case 1 is easy. We just append the current values to the arrays:

x_axis_counters.append(i)
roll_history.append(current_roll)
roll_setpoint_history.append(desired_roll)
pitch_history.append(current_pitch)
pitch_setpoint_history.append(pitch_PID.SetPoint)
altitude_history.append(current_altitude)
altitude_setpoint_history.append(desired_altitude)

The above code will work perfectly fine if you want the arrays to grow infinitely large over time. Ain’t nobody got time for that so we need to check how long the arrays are and delete data. We’ll check the length of the x-axis array as a proxy for all the others and use that to determine what to do. Typing this code that looks very similar over and over again means it’s probably time to abstract it into classes or something else. The more you type something over and over again, the larger indication you have that you need to so something about it. But for now we’ll leave it like this for ease of reading and comprehension.

# if we reach our data limit set point, evict old data and add new.
# this helps keep the graph clean and prevents it from growing infinitely
if(len(x_axis_counters) > plot_array_max_length):
	x_axis_counters.pop(0)
	roll_history.pop(0)
	roll_setpoint_history.pop(0)
	pitch_history.pop(0)
	pitch_setpoint_history.pop(0)
	altitude_history.pop(0)
	altitude_setpoint_history.pop(0)

	x_axis_counters.append(i)
	roll_history.append(current_roll)
	roll_setpoint_history.append(desired_roll)
	pitch_history.append(current_pitch)
	pitch_setpoint_history.append(pitch_PID.SetPoint)
	altitude_history.append(current_altitude)
	altitude_setpoint_history.append(desired_altitude)
# else, just add new. we are not yet at limit.
else:
	x_axis_counters.append(i)
	roll_history.append(current_roll)
	roll_setpoint_history.append(desired_roll)
	pitch_history.append(current_pitch)
	pitch_setpoint_history.append(pitch_PID.SetPoint)
	altitude_history.append(current_altitude)
	altitude_setpoint_history.append(desired_altitude)
i = i + 1

You will notice that there are quite a few entries for altitude. We haven’t done anything with that yet so just set desired_altitude to an integer somewhere in the code so it doesn’t error out.

To complete the graphing portion, we need to actually plot the data. The clear=true in the below lines clears out the plot so we’re not replotting on top of the old data. We also need to process events to actually draw the graph:

# process events means draw the graphs
pg.QtGui.QApplication.processEvents()

# arguments are x values, y values, options
# pen is a different line in the plot
p1.plot(x_axis_counters, roll_history, pen=0, clear=True)
p1.plot(x_axis_counters, roll_setpoint_history, pen=1)

p2.plot(x_axis_counters, pitch_history, pen=0,clear=True)
p2.plot(x_axis_counters, pitch_setpoint_history, pen=1)

p3.plot(x_axis_counters, altitude_history, pen=0,clear=True)
p3.plot(x_axis_counters, altitude_setpoint_history, pen=1)

You can now run the code to see your graph populating with data!

7 – Adding altitude autopilot (preselect and hold)

Ok so now that we have eye candy with the real-time graphs, we can make our autopilot do something useful: go to a selected altitude and hold it.

We already have the roll and pitch PIDs functioning as desired. How do we couple the pitch PID to get to the desired altitude? One cannot directly control altitude. Altitude is controlled via a combination of pitch and airspeed (and time).

We will call the coupled PIDs an inner loop (pitch) and an outer loop (altitude). The outer loop runs and its output will feed the input of the inner loop. The altitude PID will be fed a desired altitude and current altitude. The output will then mostly be the error (desired altitude – current altitude) multiplied by our P setting. Of course I and D will have a say in the output but by and large it will be some proportion of the error.

Let’s start with defining the altitude PID and desired altitude:

altitude_PID = PID.PID(P, I, D)
desired_altitude = 8000
altitude_PID.SetPoint = desired_altitude

With those defined, we now move to the main loop. The outer loop needs to be updated first. From there, we will normalize the output from the altitude PID and use that to set the pitch PID. The pitch PID will also be normalized to keep values in a reasonable range:

# update outer loops first
altitude_PID.update(current_altitude)

# if alt=12000, setpoint = 10000, the error is 2000. if P=0.1, output will be 2000*0.1=200
pitch_PID.SetPoint = normalize(altitude_PID.output, min=-15, max=10)

# update PIDs
roll_PID.update(current_roll)
pitch_PID.update(current_pitch)

# update control outputs
new_ail_ctrl = normalize(roll_PID.output)
new_ele_ctrl = normalize(pitch_PID.output)

Now we just need to send those new control surface commands and we’ll be controlling the plane!

Outputting the control deflections should basically be the last part of the loop. We’ll put it right before the debug output:

# sending actual control values to XPlane
ctrl = [new_ele_ctrl, new_ail_ctrl, 0.0, -998] # ele, ail, rud, thr. -998 means don't change
client.sendCTRL(ctrl)

Full code of pitch_roll_autopilot_with_graphing.py

import sys
import xpc
import PID
from datetime import datetime, timedelta
import pyqtgraph as pg
from pyqtgraph.Qt import QtCore, QtGui
import time

def normalize(value, min=-1, max=1):
	# if value = 700, and max = 20, return 20
	# if value = -200, and min = -20, return -20
	if (value > max):
		return max
	elif (value < min):
		return min
	else:
		return value

update_interval = 0.050 # seconds, 0.05 = 20 Hz
start = datetime.now()
last_update = start

# defining the initial PID values
P = 0.1 # PID library default = 0.2
I = P/10 # default = 0
D = 0 # default = 0

# initializing PID controllers
roll_PID = PID.PID(P, I, D)
pitch_PID = PID.PID(P, I, D)
altitude_PID = PID.PID(P, I, D)

# setting the desired values
# roll = 0 means wings level
# pitch = 2 means slightly nose up, which is required for level flight
desired_roll = 0
desired_pitch = 2
desired_altitude = 8000

# setting the PID set points with our desired values
roll_PID.SetPoint = desired_roll
pitch_PID.SetPoint = desired_pitch
altitude_PID.SetPoint = desired_altitude

x_axis_counters = [] #0, 1, 2, 3, etc. just basic x-axis values used for plotting
roll_history = []
pitch_history = []
altitude_history = []
roll_setpoint_history = []
pitch_setpoint_history = []
altitude_setpoint_history = []
plot_array_max_length = 300 # how many data points to hold in our arrays and graph
i = 1 # initialize x_axis_counter

# first the base app needs to be instantiated
app = pg.mkQApp("python xplane autopilot monitor")

# now the window itself is defined and sized
win = pg.GraphicsLayoutWidget(show=True)
win.resize(1000,600) #pixels
win.setWindowTitle("XPlane autopilot system control")

# we have 3 subplots
p1 = win.addPlot(title="roll",row=0,col=0)
p2 = win.addPlot(title="pitch",row=1,col=0)
p3 = win.addPlot(title="altitude", row=2, col=0)

# show the y grid lines to make it easier to interpret the graphs
p1.showGrid(y=True)
p2.showGrid(y=True)
p3.showGrid(y=True)

DREFs = ["sim/cockpit2/gauges/indicators/airspeed_kts_pilot",
		"sim/cockpit2/gauges/indicators/heading_electric_deg_mag_pilot",
		"sim/flightmodel/failures/onground_any",
		"sim/flightmodel/misc/h_ind"]

def monitor():
	global i
	global last_update
	with xpc.XPlaneConnect() as client:
		while True:
			if (datetime.now() > last_update + timedelta(milliseconds = update_interval * 1000)):
				last_update = datetime.now()
				print(f"loop start - {datetime.now()}")

				posi = client.getPOSI();
				ctrl = client.getCTRL();
				multi_DREFs = client.getDREFs(DREFs)

				current_roll = posi[4]
				current_pitch = posi[3]
				current_hdg = multi_DREFs[1][0]
				current_altitude = multi_DREFs[3][0]
				current_asi = multi_DREFs[0][0]
				onground = multi_DREFs[2][0]

				# update the display
				pg.QtGui.QApplication.processEvents()

				# update outer loops first
				altitude_PID.update(current_altitude)

				# if alt=12000, setpoint = 10000, the error is 2000. if P=0.1, output will be 2000*0.1=200
				pitch_PID.SetPoint = normalize(altitude_PID.output, min=-15, max=10)

				# update PIDs
				roll_PID.update(current_roll)
				pitch_PID.update(current_pitch)

				# update control outputs
				new_ail_ctrl = normalize(roll_PID.output)
				new_ele_ctrl = normalize(pitch_PID.output)

				# if we reach our data limit set point, evict old data and add new.
				# this helps keep the graph clean and prevents it from growing infinitely
				if(len(x_axis_counters) > plot_array_max_length):
					x_axis_counters.pop(0)
					roll_history.pop(0)
					roll_setpoint_history.pop(0)
					pitch_history.pop(0)
					pitch_setpoint_history.pop(0)
					altitude_history.pop(0)
					altitude_setpoint_history.pop(0)

					x_axis_counters.append(i)
					roll_history.append(current_roll)
					roll_setpoint_history.append(desired_roll)
					pitch_history.append(current_pitch)
					pitch_setpoint_history.append(pitch_PID.SetPoint)
					altitude_history.append(0)
					altitude_setpoint_history.append(desired_altitude)
				# else, just add new. we are not yet at limit.
				else:
					x_axis_counters.append(i)
					roll_history.append(current_roll)
					roll_setpoint_history.append(desired_roll)
					pitch_history.append(current_pitch)
					pitch_setpoint_history.append(pitch_PID.SetPoint)
					altitude_history.append(0)
					altitude_setpoint_history.append(desired_altitude)
				i = i + 1

				p1.plot(x_axis_counters, roll_history, pen=0, clear=True)
				p1.plot(x_axis_counters, roll_setpoint_history, pen=1)

				p2.plot(x_axis_counters, pitch_history, pen=0,clear=True)
				p2.plot(x_axis_counters, pitch_setpoint_history, pen=1)

				p3.plot(x_axis_counters, altitude_history, pen=0,clear=True)
				p3.plot(x_axis_counters, altitude_setpoint_history, pen=1)

				# sending actual control values to XPlane
				ctrl = [new_ele_ctrl, new_ail_ctrl, 0.0, -998] # ele, ail, rud, thr. -998 means don't change
				client.sendCTRL(ctrl)

				output = f"current values --    roll: {current_roll: 0.3f},  pitch: {current_pitch: 0.3f}"
				output = output + "\n" + f"PID outputs    --    roll: {roll_PID.output: 0.3f},  pitch: {pitch_PID.output: 0.3f}"
				output = output + "\n"
				print(output)

if __name__ == "__main__":
	monitor()

Using the autopilot / Conclusion

To use the autopilot, fire up XPlane, hop in a small-ish plane (gross weight less than 10k lb), take off, climb 1000′, then execute the code. Your plane should bring roll to 0 pretty quick and start the climb/descent to the desired altitude.