Accurate position measurement using low-cost cameras and OpenCV

There are many ways to sense the position of an object, and they’re generally either expensive or low-resolution. Laser interferometers are incredibly accurate, but the complex optics & electronics make the price very high. Hand-held laser measures are quite cheap, but they use a time-of-flight measurement method which limits their resolution, as light travels at roughly 1 foot (300 mm) per nanosecond, and making sub-nanosecond measurements isn’t easy. Lidar (light-based radar) is currently quite expensive, and has similar constraints. Ultrasonic methods benefit from the fact that sound waves travel at a much slower speed; they work well in constrained environments, such as measuring the height of liquid in a tank, but multipath reflections are a problem if there is more than one object in view.

Thanks to the smartphone boom, high-resolution camera modules are quite cheap, and I’ve been wondering whether they could be used to sense the position of an object to a reasonable accuracy for everyday measurements (at least 0.5 mm or 0.02 inches).

To test the idea I’ve set up 2 low-cost webcams at right-angles, to sense the X and Y position of an LED. To give a reproducible setup, I’ve engraved a baseboard with 1 cm squares, and laser-cut a LED support, so I can accurately position the LED and see the result.

The webcams are Logitech C270, that can provide an HD video resolution of 720p (i.e. 1280 x 720 pixels). For image analysis I’ll be using Python OpenCV; it has a wide range of sophisticated software tools, that allow you to experiment with some highly advanced methods, but for now I’ll only be using a few basic functions.

The techniques I’m using are equally applicable to single-camera measurements, e.g. tracking the position of the sun in the sky.

Camera input

My camera display application uses PyQt and OpenCV to display camera images, and it is strongly recommended that you start with this, to prove that your cameras will work with the OpenCV drivers. It contains code that can be re-used for this application, so is imported as a module.

Since we’re dealing with multiple cameras and displays, we need a storage class to house the data.

import sys, time, threading, cv2, numpy as np
import cam_display as camdisp

IMG_SIZE    = 1280,720          # 640,480 or 1280,720 or 1920,1080
DISP_SCALE  = 2                 # Scaling factor for display image
DISP_MSEC   = 50                # Delay between display cycles
CAP_API     = cv2.CAP_ANY       # API: CAP_ANY or CAP_DSHOW etc...

# Class to hold capture & display data for a camera
class CamCap(object):
    def __init__(self, cam_num, label, disp):
        self.cam_num, self.label, self.display = cam_num, label, disp
        self.imageq = camdisp.Queue.Queue()
        self.pos = 0
        self.cap = cv2.VideoCapture(self.cam_num-1 + CAP_API)
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, IMG_SIZE[0])
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, IMG_SIZE[1])

The main window of the GUI is subclassed from cam_display, with the addition of a second display area, and storage for the camera capture data:

# Main window
class MyWindow(camdisp.MyWindow):
    def __init__(self, parent=None):
        camdisp.MyWindow.__init__(self, parent)
        self.camcaps = []
        self.disp2 = camdisp.ImageWidget(self)
        self.capturing = True

On startup, 2 cameras are added to the window:

if __name__ == '__main__':
    app = camdisp.QApplication(sys.argv)
    win = MyWindow()
    win.camcaps.append(CamCap(2, 'x', win.disp))
    win.camcaps.append(CamCap(1, 'y', win.disp2))

As with cam_display, a separate thread is used to fetch data from the cameras:

    # Grab camera images (separate thread)
    def grab_images(self):
        while self.capturing:
            for cam in self.camcaps:
                if cam.cap.grab():
                    retval, image = cam.cap.retrieve(0)
                    if image is not None and cam.imageq.qsize() < 2:
                        time.sleep(DISP_MSEC / 1000.0)
                    print("Error: can't grab camera image")
                    self.capturing = False
        for cam in self.camcaps:

Image display

A timer event is used to fetch the image from the queue, convert it to RGB, do the image processing, and display the result.

    # Fetch & display camera images
    def show_images(self):
        for cam in self.camcaps:
            if not cam.imageq.empty():
                image = cam.imageq.get()
                if image is not None and len(image) > 0:
                    img = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
                    cam.pos = colour_detect(img)
                    self.display_image(img, cam.display, DISP_SCALE)
    # Show position values given by cameras
    def show_positions(self, s=""):
        for cam in self.camcaps:
            s += "%s=%-5.1f " % (cam.label, cam.pos)

Image processing

We need to measure the horizontal (left-to-right) position of the LED for each camera. If the LED is brighter than the surroundings, this isn’t difficult; first we create a mask that isolates the LED from the background, then extract the ‘contour’ of the object with the background masked off. The contour is a continuous curve that marks the boundary between the object and the background; for the illuminated LED this will approximate to a circle. To find an exact position, the contour is converted to a true circle, which is drawn in yellow, and the horizontal position of the circle centre is returned.

LOWER_DET   = np.array([240,  0,  0])       # Colour limits for detection
UPPER_DET   = np.array([255,200,200])

# Do colour detection on image
def colour_detect(img):
    mask = cv2.inRange(img, LOWER_DET, UPPER_DET)
    ctrs = cv2.findContours(mask, cv2.RETR_TREE,
    if len(ctrs) > 0:
        (x,y),radius = cv2.minEnclosingCircle(ctrs[0])
        radius = int(radius), (int(x),int(y)), radius, (255,255,0), 2)
        return x
    return 0

This code is remarkably brief, and if you’re thinking that I may have taken a few short-cuts, you’d be right:

Colour detection: I’ve specified the upper and lower RGB values that are acceptable; because this is a red LED, the red value is higher than the rest, being between 240 and 255 (the maximum is 255). I don’t want to trigger on a pure white background so I’ve set the green and blue values between 0 and 200, so a pure white (255,255,255) will be rejected. This approach is a bit too simplistic; if the LED is too bright it can saturate the sensor and appear completely white, and conversely another bright light source can cause the camera’s auto-exposure to automatically reduce the image intensity, such that the LED falls below the required level. The normal defence against this is to use manual camera exposure, which can be adjusted to your specific environment. Also it might be worth changing the RGB colourspace to HSV for image matching; I haven’t yet tried this.

Multiple contours: the findContours function returns a list of contours, and I’m always taking the first of these. In a real application, there may be several contours in the list, and it will be necessary to check them all, to find the most likely – for example, the size of the circle to see if it is within an acceptable range.

However, the measurement method does show some very positive aspects:

Complex background: as you can see from the image at the top of this blog, it works well in a normal office environment – no need for a special plain-colour background.

No focussing: most optical applications require the camera to be focussed, but in this case there is no need. I’ve deliberately chosen a target distance of approximately 4 inches (100 mm) that results in a blurred image, but OpenCV is still able to produce an accurate position indication.

Sub-pixel accuracy: with regard to measurement accuracy, the main rule for the camera is obviously “the more pixels, the better”, but also OpenCV can compute the position to within a fraction of a pixel. My application displays the position (in pixels) to one decimal place; at 4 inches (100 mm) distance, the Logitech cameras’ field of view is about 3.6 inches (90 mm), so if the position can be measured within, say, 0.2 of a pixel, this would be a resolution of 0.0006 inch (0.015 mm).

Of course these figures are purely theoretical, and the resolution will be much reduced in a real-world application, but all the same, it does suggest the technique may be capable of achieving quite good accuracy, at relatively low cost.

Single camera

With minor modifications, the code can be used in a single-camera application, e.g. tracking the position of the sun in the sky.

The code scans all the cameras in the ‘camcaps’ list, so will automatically adapt if there is only one.

The colour_detect function currently returns the horizontal position only; this can be changed to return the vertical as well. The show_positions method can be changed to display both of the returned values from the single camera.

Then you just need a wide-angle lens, and a suitable filter to stop the image sensor being overloaded. Sundial, anyone?

Source code

The ‘campos’ source code is available here, and is compatible with Windows and Linux, Python 2.7 and 3.x, PyQt v4 and v5. It imports my cam_display application, and I strongly recommended that you start by running that on its own, to check compatibility. If it fails, read the Image Capture section of that blog, which contains some pointers that might be of help.

Copyright (c) Jeremy P Bentham 2019. Please credit this blog if you use the information or software in it.

PC / RPi camera display using PyQt and OpenCV

OpenCV is an incredibly powerful image-processing tool, but it can be difficult to know where to start – how do you grab an image from a camera, and display it in a user-friendly GUI? This post describes such an application, that runs unmodified on a PC or Raspberry Pi, Windows or Linux, Python 2.7 or 3.x, and PyQt v4 or v5.


On Windows, the OpenCV and PyQt5 libraries can be installed using pip:

pip install numpy opencv-python PyQt5

If pip isn’t available, you should be able to run the module from the command line by invoking Python, e.g. for Python 3:

py -3 -m pip install numpy opencv-python PyQt5

Installing on a Raspberry Pi is potentially a lot more complicated; it is generally recommended to install from source, and for opencv-python, this is a bit convoluted. Fortunately there is a simpler option, if you don’t mind using versions that are a few years old, namely to load the binary image from the standard repository, e.g.

sudo apt update
sudo apt install python3-opencv python3-pyqt5 

At the time of writing, the most recent version of Raspbian Linux is ‘buster’, and that has OpenCV 3.2, which is quite usable. The previous ‘stretch’ distribution has python-opencv version 2.4, which is a bit too old: my code isn’t compatible with it.

With regard to cameras, all the USB Webcams I’ve tried have worked fine on Windows without needing to have any extra driver software installed; they also work on the Raspberry Pi, as well as the standard Pi camera with the ribbon-cable interface.

PyQt main window

Being compatible with PyQt version 4 and 5 requires some boilerplate code to handle the way some functions have been moved between libraries:

import sys, time, threading, cv2
    from PyQt5.QtCore import Qt
    pyqt5 = True
    pyqt5 = False
if pyqt5:
    from PyQt5.QtCore import QTimer, QPoint, pyqtSignal
    from PyQt5.QtWidgets import QApplication, QMainWindow, QTextEdit, QLabel
    from PyQt5.QtWidgets import QWidget, QAction, QVBoxLayout, QHBoxLayout
    from PyQt5.QtGui import QFont, QPainter, QImage, QTextCursor
    from PyQt4.QtCore import Qt, pyqtSignal, QTimer, QPoint
    from PyQt4.QtGui import QApplication, QMainWindow, QTextEdit, QLabel
    from PyQt4.QtGui import QWidget, QAction, QVBoxLayout, QHBoxLayout
    from PyQt4.QtGui import QFont, QPainter, QImage, QTextCursor
    import Queue as Queue
    import queue as Queue

The main window is subclassed from PyQt, with a simple arrangement of a menu bar, video image, and text box:

class MyWindow(QMainWindow):
    text_update = pyqtSignal(str)

    # Create main window
    def __init__(self, parent=None):
        QMainWindow.__init__(self, parent)

        self.central = QWidget(self)
        self.textbox = QTextEdit(self.central)
        self.textbox.setMinimumSize(300, 100)
        sys.stdout = self
        print("Camera number %u" % camera_num)
        print("Image size %u x %u" % IMG_SIZE)
        if DISP_SCALE > 1:
            print("Display scale %u:1" % DISP_SCALE)

        self.vlayout = QVBoxLayout()        # Window layout
        self.displays = QHBoxLayout()
        self.disp = ImageWidget(self)    
        self.label = QLabel(self)

        self.mainMenu = self.menuBar()      # Menu bar
        exitAction = QAction('&Exit', self)
        self.fileMenu = self.mainMenu.addMenu('&File')

There is a horizontal box layout called ‘displays’, that seems to be unnecessary as it only has one display widget in it. This is intentional, since much of my OpenCV experimentation requires additional displays to show the image processing in action; this can easily be done by creating more ImageWidgets, and adding them to the ‘displays’ layout.

Similarly, there is a redundant QLabel below the displays, which isn’t currently used, but is handy for displaying static text below the images.

Text display

It is convenient to redirect the ‘print’ output to the text box, rather than appearing on the Python console. This is done using the ‘text_update’ signal that was defined above:

    # Handle sys.stdout.write: update text display
    def write(self, text):
    def flush(self):

    # Append to text display
    def append_text(self, text):
        cur = self.textbox.textCursor()     # Move cursor to end of text
        s = str(text)
        while s:
            head,sep,s = s.partition("\n")  # Split line at LF
            cur.insertText(head)            # Insert text at cursor
            if sep:                         # New line if LF
        self.textbox.setTextCursor(cur)     # Update visible cursor

The use of a signal means that print() calls can be scattered about the code, without having to worry about which thread they’re in.

Image capture

A separate thread is used to capture the camera images, and put them in a queue to be displayed. The camera may produce images faster than they can be displayed, so it is necessary to check how many images are already in the queue; if more than 1, the new image is discarded. This prevents a buildup of unwanted images.

IMG_SIZE    = 1280,720          # 640,480 or 1280,720 or 1920,1080
CAP_API     = cv2.CAP_ANY       # or cv2.CAP_DSHOW, etc...
EXPOSURE    = 0                 # Non-zero for fixed exposure

# Grab images from the camera (separate thread)
def grab_images(cam_num, queue):
    cap = cv2.VideoCapture(cam_num-1 + CAP_API)
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, IMG_SIZE[0])
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, IMG_SIZE[1])
    if EXPOSURE:
        cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, 0)
        cap.set(cv2.CAP_PROP_EXPOSURE, EXPOSURE)
        cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, 1)
    while capturing:
        if cap.grab():
            retval, image = cap.retrieve(0)
            if image is not None and queue.qsize() < 2:
                time.sleep(DISP_MSEC / 1000.0)
            print("Error: can't grab camera image")

The choice of image size will depend on the camera used; all cameras support VGA size (640 x 480 pixels), more modern versions the high-definition standards of 720p (1280 x 720) or 1080p (1920 x 1080).

The camera number refers to the position in the list of cameras collected by the operating system; I’ve defined the first camera as number 1, but the OpenCV call defines the first as 0, so the number has to be adjusted.

The same parameter is also used to define the capture API setting; by default this is ‘any’, which usually works well; my Windows 10 system defaults to the MSMF (Microsoft Media Foundation) backend, while the Raspberry Pi defaults to Video for Linux (V4L). Sometimes you may need to force a particular API to be used, for example, I have a Logitech C270 webcam that works fine on Windows 7, but fails on Windows 10 with an ‘MSMF grab error’. Forcing the software to use the DirectShow API (using the cv2.CAP_DSHOW option) fixes the problem.

If you want to check which backend is being used, try:

print("Backend '%s'" % cap.getBackendName())

Unfortunately this only works on the later revisions of OpenCV.

Manual exposure setting can be a bit hit-and-miss, depending on the camera and API you are using; the default is automatic operation, and setting EXPOSURE non-zero (e.g. to a value of -3) generally works, however it can be difficult to set a webcam back to automatic operation: sometimes I’ve had to use another application to do this. So it is suggested that you keep auto-exposure enabled if possible.

[Supplementary note: it seems that these parameter values aren’t standardised across the backends. For example, the CAP_PROP_AUTO_EXPOSURE value in my source code is correct for the MSMF backend; a value of 1 enables automatic exposure, 0 disables it. However, the V4L backend on the Raspberry Pi uses the opposite values: automatic is 0, and manual is 1. So it looks like my code is incorrect for Linux. I haven’t yet found any detailed documentation for this, so had to fall back on reading the source code, namely the OpenCV videoio ‘cap’ files such as cap_msmf.cpp and cap_v4l.cpp.]

Image display

The camera image is displayed in a custom widget:

# Image widget
class ImageWidget(QWidget):
    def __init__(self, parent=None):
        super(ImageWidget, self).__init__(parent)
        self.image = None

    def setImage(self, image):
        self.image = image

    def paintEvent(self, event):
        qp = QPainter()
        if self.image:
            qp.drawImage(QPoint(0, 0), self.image)

A timer event is used to trigger a scan of the image queue. This contains images in the camera format, which must be converted into the PyQt display format:

DISP_SCALE  = 2                 # Scaling factor for display image

    # Start image capture & display
    def start(self):
        self.timer = QTimer(self)           # Timer to trigger display
                    self.show_image(image_queue, self.disp, DISP_SCALE))
        self.capture_thread = threading.Thread(target=grab_images, 
                    args=(camera_num, image_queue))
        self.capture_thread.start()         # Thread to grab images

    # Fetch camera image from queue, and display it
    def show_image(self, imageq, display, scale):
        if not imageq.empty():
            image = imageq.get()
            if image is not None and len(image) > 0:
                img = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
                self.display_image(img, display, scale)

    # Display an image, reduce size if required
    def display_image(self, img, display, scale=1):
        disp_size = img.shape[1]//scale, img.shape[0]//scale
        disp_bpl = disp_size[0] * 3
        if scale > 1:
            img = cv2.resize(img, disp_size, 
        qimg = QImage(, disp_size[0], disp_size[1], 
                      disp_bpl, IMG_FORMAT)

This demonstrates the power of OpenCV; with one function call we convert the image from BGR to RGB format, then another is used to resize the image using cubic interpolation. Finally a PyQt function is used to convert from OpenCV to PyQt format.

Running the application

Make sure you’re using the Python version that has the OpenCV and PyQt installed, e.g. for the Raspberry Pi:


There is an optional argument that can be used if there are multiple cameras; the default first camera is number 1.

On Linux, some USB Webcams cause a constant stream of JPEG format errors to be printed on the console, complaining about extraneous bytes in the data. There is some discussion online as to the cause of the error, and the cure seems to involve rebuilding the libraries from source; I’m keen to avoid that, so used the simple workaround of suppressing the errors by redirecting STDERR to null:

python3 2> /dev/null

Fortunately this workaround is only needed with some USB cameras; the standard Raspberry Pi camera with the CSI ribbon-cable interface works fine.

Source code

Full source code is available here.

For a more significant OpenCV application, take a look at this post.

Copyright (c) Jeremy P Bentham 2019. Please credit this blog if you use the information or software in it.

Raspberry Pi and OpenOCD

In previous blog posts I used an FTDI module and pure Python code to access the internals of an ARM CPU using the SWD interface. I want to expand this technique to provide a more comprehensive real-time display of the CPU status, but the FTDI interface is quite limiting; what I need is an fast intelligent SWD/JTAG adaptor, with a network interface so I can do both local and remote diagnosis.

Enter the raspberry Pi: a lot of computing power at very low cost, either using the built-in HDMI display output, or running ‘headless’ over a wireless network, providing diagnostic data to a remote display.

Connecting the Pi to the target system could hardly be simpler; 3 wires (clock, data & ground) are sufficient to access data from most CPUs with an SWD interface.

Raspberry Pi 3 SWD interface to STMicro ARM CPU

Raspberry Pi ZeroW JTAG interface to Atmel ARM CPU

Software-wise, OpenOCD has all the SWD/JTAG features you’ll ever need, accessed through a network interface; installation may be a bit intimidating if you’re not an experienced Linux user, but is really quite easy, as this blog will (hopefully) demonstrate.

What you end up with is a really powerful local/remote debugger for very little money; around $10 US, in the case of the Pi Zero W.

Installing OpenOCD

You need any Raspberry Pi (RPi), versions 0 to 3. The slower boards will have longer boot times & build times but are otherwise fully functional. The OS version I used was ‘Raspbian Stretch with desktop’; the ‘recommended software’ add-ons are not necessary. The total image size on SD card is around 3 GB.

A convenient way to avoid re-typing the instructions below is to enable the Secure Shell (SSH) protocol using the ‘Raspberry Pi Configuration utility’, then run a remote ssh client (e.g. ‘putty’ on Windows) to access the RPi over the network; you can then cut and paste a command line into the ssh window without re-typing. If in doubt as to the IP address of your RPi, hover the cursor over the network icon in the top-right corner, and the address will be shown, e.g. (or run ‘ifconfig’ if in text mode).

It is best to install OpenOCD from source, as the pre-built images often lack important functionality. Installation instructions can be found on many Web sites, for example Adafruit “Programming Microcontrollers using OpenOCD on a Raspberry Pi”. In summary, the steps are:

cd ~
sudo apt-get update
sudo apt-get install git autoconf libtool make pkg-config libusb-1.0-0 libusb-1.0-0-dev
git clone
cd openocd
./configure --enable-sysfsgpio --enable-bcm2835gpio
sudo make install

The ‘make’ step takes approximately an hour on the slower boards, or 15 minutes on the faster.

Configuration files

OpenOCD has a wide variety of options, so generally needs more than one configuration file, to define:

  • Debug adaptor (in our case, the RPi)
  • Communication method (SWD or JTAG)
  • Target CPU.

There are a large number of files in /usr/local/share/openocd/scripts, most notably the ‘interface’ and ‘target’ sub-directories, however there are so many permutations that it is unlikely you’ll find everything you need, so we need to think about creating our own files.

The most important first step is to work out how the RPi will be connected to the target system…

RPi I/O connections

At the time of writing, there are 3 versions of the RPi I/O connector, and 3 different pin-numbering schemes, so it is easy to get confused. The older boards may be considered obsolete, but are still more than adequate for running OpenOCD, so mustn’t be excluded.

The numbering schemes are:

  1. Connector pin numbers: sequential 1 – 26 or 1 – 40
  2. GPIO bit numbers (also known as Broadcom or BCM numbers) 0 – 27
  3. WiringPi numbers, as used in the Python library

I’ll only be using the first 2 of these. The older boards have 26 pins, the newer 40.

RPi 26-way connector with GPIO numbers

Pins 3 & 5 were initially GPIO 0 and 1, but later became GPIO 2 and 3; they are best avoided.

RPi 40-way connector with GPIO numbers

On the 40-way connector GPIO21 has become 27, so should also be avoided. The choice of ground pin is arbitrary; any of them can be used, but I avoid pin 6, as any mis-connection to the supply pins can result in significant damage.


The SWD connections given in the OpenOCD configuration file ‘raspberrypi2-native.cfg’ are:

raspberrypi2-native SWD connections

The relevant lines in the configuration file are:

# SWD                 swclk swdio
# Header pin numbers: 22    18
bcm2835gpio_swd_nums  25    24

bcm2835gpio_srst_num  18
reset_config srst_only srst_push_pull

In many applications the reset signal is unnecessary – and undesirable, if the objective is to perform non-intrusive monitoring of a running system.


JTAG is an older (and more widely available) standard for debugging, that requires 4 wires in the place of 2 for SWD. There is a standard mapping between them (SWCLK is TCK, SWDIO is TMS), but the JTAG connections in the standard OpenOCD configuration file ‘raspberrypi-native.cfg’ use completely different pins:

raspberrypi-native JTAG connections

The relevant lines in the configuration file are:

# JTAG                tck tms tdi tdo
# Header pin numbers: 23  22  19  21
bcm2835gpio_jtag_nums 11  25  10  9

# bcm2835gpio_srst_num 24
# reset_config srst_only srst_push_pull

As standard, the reset definition is commented out.

I’m not a fan of this pinout scheme; I’d like a single setup that covers both SWD and JTAG.

Other pin functions

You might wish to use the RPi for other diagnostic functions, such as monitoring a serial link, so these pins have to be kept free. The following diagram shows the alternative pin functions.


You can use any of the blue or yellow pins for the SWD/JTAG interface, it is just a question as to which other functionality you may be needing.

Combining SWD and JTAG

The compromise I’ve adopted is to preserve the existing SWD arrangement, but move the JTAG pins so one set of connections can serve both SWD & JTAG on either the 26-way or the 40-way connectors – and I’ve also avoided using any of the predefined pins, so there are no conflicts with other functionality.

rpi_swd_jtag SWD and JTAG connections

The relevant section of the configuration file is:

# SWD                swclk swdio
# Header pin numbers 22    18
bcm2835gpio_swd_nums 25    24

# JTAG                tck tms tdi tdo
# Header pin numbers  22  18  16  15 
bcm2835gpio_jtag_nums 25  24  23  22

Target system connections

The connection points on the target system will vary from board to board; for a previous demonstration I used a ‘blue pill’ STM32F103 board that has ground, SWD clock & data conveniently on some separate header pins, but the most common standard for JTAG & SWD connections is a 20-way 2-row header, as follows:

JTAG     SWD     20-way pin
Ground   Ground  4, 6, 8, 10, 14, 16, 18, 20
TRST             3
TDI              5
TMS      SWDIO   7
TCK      SWCLK   9
TDO              13
RESET            15

There is generally a keyway on the odd-numbered side of the connector.


Two reset signals are defined: TRST is ‘tap reset’, that is intended to just reset the diagnostic port; the other signal marked RESET (which OpenOCD refers to as SRST or ‘system reset’) should reset all devices, as if a reset button has been pressed. In the experimentation I’ve done, the reset lines haven’t been needed, but this is very processor-specific; sometimes the RESET line has to be used to gain control of the target system.

It is convenient to use ribbon cable for wiring up the interface, especially if the wires follow the resistor colour code:

RPi pin  Colour  20-way pin  JTAG/SWD
9        Brown   20          Ground
12       Red     15          Reset
16       Orange  5           TDI
15       Yellow  13          TDO
18       Green   7           TMS/SWDIO
22       Blue    9           TCK/SWCLK

Or in graphical form…


Interface configuration file

The above examples show how the SWD/JTAG connections are handled, but some more data is needed to fully configure the RPi interface, most notably the I/O base address and clock scaling; this tells OpenOCD where to find the I/O interface, and how to compute its speed.

There are 2 possible values for the I/O base address: the RPi zero and v1 use 0x20000000, and v2+ use 0x3F000000. If you are unsure which value to use, the boards have an excellent feature called Device Tree that documents the current hardware configuration; enter the following command in a console window:

xxd -c 4 -g 4 /proc/device-tree/soc/ranges

The base I/O address is the second value returned, for example:

RPi zero:
00000000: 7e000000  ~...
00000004: 20000000   ...
00000008: 02000000  ....
RPi v3:
00000000: 7e000000 ~...
00000004: 3f000000 ?..
00000008: 02000000 ....
..and so on..

The clock scaling is less critical, since we’re generally aiming for around 1 MHz, which gives quite a bit of leeway in terms of being fast or slow. This is fortunate, because it is difficult to find a definitive explanation of the values that should be used for all hardware & clock settings. My understanding, from reading the source code, is that every I/O read or write instruction is followed by a loop containing NOP (CPU idle) cycles to space out the operations; this number is known as the ‘jtag_delay’, and is calculated by:

(speed_coeff / khz) - speed_offset;

..where speed_coeff & speed_offset are the two scaling parameters, and khz is the desired SWD/JTAG clock speed in kHz (all the values are integers). Obviously the delay is very CPU-dependant; the standard values in the files are:

Rpi zero and v1:
  bcm2835gpio_speed_coeffs 113714 28
RPi v2+:
  bcm2835gpio_speed_coeffs 146203 36

These do seem to give roughly the right answers, and there isn’t any great necessity for the delays to be accurate – when viewed on an oscilloscope, you can see some of the cycles being stretched by an incoming interrupt, so they never will be as accurate as a pure hardware solution.

Adaptor configuration files

Combining all the information above, here are the two adaptor configuration files: rpi1.cfg for RPi zero & v1, and rpi2.cfg for v2+

# rpi1.cfg: OpenOCD interface on RPi zero and v1

# Use RPi GPIO pins
interface bcm2835gpio

# Base address of I/O port
bcm2835gpio_peripheral_base 0x20000000

# Clock scaling
bcm2835gpio_speed_coeffs 113714 28

# SWD                swclk swdio
# Header pin numbers 22    18
bcm2835gpio_swd_nums 25    24

# JTAG                tck tms tdi tdo
# Header pin numbers  22  18  16  15 
bcm2835gpio_jtag_nums 25  24  23  22
# rpi2.cfg: OpenOCD interface on RPi v2+

# Use RPi GPIO pins
interface bcm2835gpio

# Base address of I/O port
bcm2835gpio_peripheral_base 0x3F000000

# Clock scaling
bcm2835gpio_speed_coeffs 146203 36

# SWD                swclk swdio
# Header pin numbers 22    18
bcm2835gpio_swd_nums 25    24

# JTAG                tck tms tdi tdo
# Header pin numbers  22  18  16  15 
bcm2835gpio_jtag_nums 25  24  23  22

Running OpenOCD

Finally we get to run OpenOCD, but in addition to the adaptor configuration, we need to give some details about the interface & target CPU.

The command line consists of configuration files prefixed by -f, and commands prefixed by -c. In reality, a configuration file is just a series of commands; for example you can select JTAG operation using the command-line option:

openocd -c "transport select jtag"

This is exactly the same as:

openocd -f select_jtag.cfg

where the file ‘select_jtag.cfg’ has the line:

transport select jtag

So we’ll use a mixture of commands and files on our command line. The following example is for an RPi v3 driving an SWD interface into a STM32F103 processor;  I’ve used backslash continuation characters at the end of each line to make the commands more readable:

sudo openocd -f rpi2.cfg \
             -c "transport select swd" \
             -c "adapter_khz 1000" \
             -f target/stm32f1x.cfg

Some hardware operations require superuser privileges, hence the use of ‘sudo’. The usual security warnings apply when doing this; you can try without, there will just be a ‘permission denied’ error if it fails.

For a list of supported CPUs, see the files in /usr/local/share/openocd/scripts/target

When OpenOCD runs, with a bit of luck, you’ll see something like:

BCM2835 GPIO nums: swclk = 25, swdio = 24
BCM2835 GPIO config: tck = 25, tms = 24, tdi = 23, tdo = 22
adapter speed: 1000 kHz
adapter speed: 1000 kHz
adapter_nsrst_delay: 100
none separate
cortex_m reset_config sysresetreq
Info : Listening on port 6666 for tcl connections
Info : Listening on port 4444 for telnet connections
Info : BCM2835 GPIO JTAG/SWD bitbang driver
Info : JTAG and SWD modes enabled
Info : clock speed 1001 kHz
Info : SWD DPIDR 0x1ba01477
Info : stm32f1x.cpu: hardware has 6 breakpoints, 4 watchpoints
Info : Listening on port 3333 for gdb connections

If there is a configuration or wiring error, OpenOCD usually (but not always!) returns to the command line, for example if the SWDIO line is disconnected:

BCM2835 GPIO nums: swclk = 25, swdio = 24
BCM2835 GPIO config: tck = 25, tms = 24, tdi = 23, tdo = 22
adapter speed: 1000 kHz
adapter speed: 1000 kHz
adapter_nsrst_delay: 100
none separate
cortex_m reset_config sysresetreq
Info : Listening on port 6666 for tcl connections
Info : Listening on port 4444 for telnet connections
Info : BCM2835 GPIO JTAG/SWD bitbang driver
Info : JTAG and SWD modes enabled
Info : clock speed 1001 kHz
Info : SWD DPIDR 0x02192468

..and then OpenOCD terminates back to the command line..

The clue is in the SWD Data Port ID Register (DPIDR) value. According to the datasheet for the STM32F103 CPU, this should be 1BA01477. With a data line fault, every time OpenOCD runs, a different value is returned, e.g. 0x00e65468, 0x02192468, 0x00433468 and so on; the software is just picking up noise on the data line.

A disconnected clock line is harder to diagnose, as OpenOCD just terminates after the ‘clock speed’ report, with no error indication. Try using the -d option to invoke a debug display, and you’ll see lines like

JUNK DP read reg 0 = ffffffff

which suggests that all is not well in the hardware interface.

Another thing to try in the event of a failure is adding or removing a reset line, and changing its configuration entries; if there is a reset problem you’ll probably see the DPIDR value reported correctly, but other functions may not work.

What now?

Having just written 2100 words and drawn 8 diagrams, I’m going to take a short break. However, first I ought to give some indication as to how you control this OpenOCD setup.

The sign-on text mentions a telnet interface on port 4444, so we can use that; the commands highlighted in bold:

sudo apt-get install telnet  # ..if not already installed

telnet localhost 4444
Trying ::1...
Connected to localhost.
Escape character is '^]'.
Open On-Chip Debugger

As standard, this interface only works when telnet is running locally on the Raspberry Pi. To open it up to a wider network, add the command ‘bindto’ to the configuration. However, this option comes with a major security warning; think very carefully before making the system accessible to everyone on the network.

Refer to the OpenOCD documentation for information on the large number of commands that can be used over telnet, for example displaying memory using ‘mdw’ or halting the processor using ‘halt’. When finished, close the telnet link with ‘exit’.

If that doesn’t sound very exciting, my next blog combines the  power of the Raspberry Pi with the graphics in my reporta project, to produce a non-invasive high-speed display of the CPU internals – a graphical real-time viewer for your embedded systems. Click here to find out more.

Copyright (c) Jeremy P Bentham 2019. Please credit this blog if you use the information or software in it.