Software PWM - Carleton-SRCL/SPOT GitHub Wiki

Overview

This Wiki entry will cover the Python script that is currently (as of SPOT 4.0.0-RC.4) being used to control the thrusters on all three platforms. The script is broken into distinct components:

  • Importing the requisite libraries.
  • Setting up the GPIO pins.
  • Setting up the UDP receive functionality.
  • Initializing parameters and variables for the PWM.
  • Receiving and converting the desired duty cycle into a PWM signal at the desired frequency.
  • Shutting down the GPIOs and UDP when the script is terminated or when it fails due to an error.

Importing Libraries

The Python script relies on the use of either the Jetson.GPIO or RPi.GPIO libraries (for our purposes they are equivalent). The import section of the code is as follows:

import Jetson.GPIO as GPIO
import socket
import struct
import time
import signal

The various libraries provide the following functionalities:

  • Jetson.GPIO: This library was developed by NVIDIA and can be used for GPIO control. It also has hardware PWM capabilities for boards that support it, though the NVIDIA Jetson Xavier NX only has 2 hardware PWM available - which is not enough for 8 thrusters. The library is installed by default and can be found here.
  • socket: This library handles network communication.
  • struct: This library is used for packing and unpacking binary data.
  • signal: This library is to handle signals (i.e. what to do when the code completes or fails).

Setting up the GPIO Pins

Once the necessary libraries are imported, the script defines which pins will be used to control the thrusters. In this script, 8 pins are specified for this purpose.

The following code snippet highlights the setup:

# Define the 8 pins you want to use.
PINS = [7, 12, 13, 15, 16, 18, 22, 23]

# Set up the GPIO library
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)

Pin Configuration

The pins are defined in a list called PINS. Here, the pins numbered 7, 12, 13, 15, 16, 18, 22, and 23 are chosen. The specific board numbering mode (GPIO.BOARD) is selected to reference pins by their physical location on the board.

To ensure a smooth operation without warning messages, the script sets GPIO.setwarnings(False). This prevents any warnings from being displayed that might arise if a pin has been previously configured and not cleaned up.

Pin Initialization

After defining the pins, the script initializes each pin to be an output pin and sets its initial state to low (off). This is achieved through the following loop:

for pin in PINS:
    GPIO.setup(pin, GPIO.OUT)
    GPIO.output(pin, GPIO.LOW)

In this loop, each pin in the PINS list is set as an output using GPIO.setup(pin, GPIO.OUT). Immediately after, its state is set to low with GPIO.output(pin, GPIO.LOW), ensuring that no thruster is activated inadvertently upon script initialization.

Setting up the UDP Receive Functionality

To receive control signals for the thrusters, the script employs the User Datagram Protocol (UDP). UDP is a connectionless protocol, making it suitable for real-time applications where speed is crucial.

Below is the code snippet that sets up the UDP server:

# UDP setup
IP_ADDRESS = '127.0.0.1'
PORT = 48291
NUM_DOUBLES = 10

server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
server_address = (IP_ADDRESS, PORT)
server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
server_socket.bind(server_address)
server_socket.setblocking(0)

UDP Configuration

  • IP_ADDRESS: This specifies the IP address where the server should listen for incoming data. The address '127.0.0.1' is the loopback address, which means the server is listening for data sent from the same device.
  • PORT: The port number 48291 is where the server listens for incoming packets.
  • NUM_DOUBLES: This variable specifies the number of double values expected in the incoming data. In this context, it's set to 10.

Socket Initialization

  • Socket Creation: The script first creates a new UDP socket using socket.socket(socket.AF_INET, socket.SOCK_DGRAM). Here, socket.AF_INET specifies the use of IPv4, and socket.SOCK_DGRAM indicates the use of UDP.
  • Socket Options: The setsockopt method is used to modify the socket's behavior. In this case, socket.SOL_SOCKET and socket.SO_REUSEADDR are used to allow the reuse of the socket address. This is particularly useful to prevent the "Address already in use" error if the script is restarted quickly.
  • Binding the Socket: The bind method binds the socket to the specified IP address and port. In this case, it binds to the loopback address and port 48291.
  • Non-blocking Mode: Lastly, server_socket.setblocking(0) sets the socket to non-blocking mode. This means the script won't get stuck waiting for data; instead, it will raise an exception if no data is available, allowing for more responsive behavior.

Initializing Parameters and Handling Signals

To ensure smooth and safe operation, this script initializes key parameters for thruster control and incorporates a mechanism to handle termination signals gracefully.

Here is the pertinent code:

data = b''

def signal_handler(sig, frame):
    raise KeyboardInterrupt

signal.signal(signal.SIGTERM, signal_handler)

SAFETY_BIT = 568471
duty_cycles = [0] * len(PINS)
pwm_frequency = 5  # Default to 5Hz

# Initialize timestamps and states for each pin
next_toggle_time = [0] * len(PINS)
pin_states = [GPIO.LOW] * len(PINS)

Data Initialization

  • data: This variable is initialized to an empty bytes object (b''). It's used to store incoming UDP data.

Signal Handling

To ensure the script can handle external termination requests, a signal handler is set up:

  • signal_handler: This function raises a KeyboardInterrupt exception when called. Its purpose is to allow the script to perform any necessary cleanup actions before exiting.
  • signal.signal(signal.SIGTERM, signal_handler): This line sets the signal_handler function to be called when the script receives a termination signal (SIGTERM), such as from a kill command.

Thruster Parameters

  • SAFETY_BIT: This constant, set to 568471, serves as a verification value in incoming data to ensure that control signals are legitimate. So if the software fails and the script continues, it should recognize that it is no longer receiving good data and should not accept new duty cycle values.
  • duty_cycles: An initialized list of zeros, with a length equal to the number of pins (thrusters). This list will store the duty cycle values for each thruster.
  • pwm_frequency: The frequency for Pulse Width Modulation (PWM) is set to a default value of 5Hz. This is good for the relatively long time it takes for the thruster valves to open.

Timestamps and Pin States

  • next_toggle_time: A list initialized with zeros that will be used to store timestamps or durations for when each pin (thruster) should change its state next.
  • pin_states: This list, initialized with all values set to GPIO.LOW, represents the current state (on or off) of each pin (thruster).

Main Script Execution

The main portion of the script consists of a continuous loop that updates the state of the thrusters based on the received duty cycles, listens for incoming data, and manages safe script termination.

Continuous Thruster Control

try:
    while True:
        current_time = time.time()

        for i, pin in enumerate(PINS):
            if current_time >= next_toggle_time[i]:
                if pin_states[i] == GPIO.LOW:
                    pin_states[i] = GPIO.HIGH
                    next_toggle_time[i] = current_time + (duty_cycles[i] / 100) * (1 / pwm_frequency)
                else:
                    pin_states[i] = GPIO.LOW
                    next_toggle_time[i] = current_time + (1 - duty_cycles[i] / 100) * (1 / pwm_frequency)
                
                GPIO.output(pin, pin_states[i])

Here's a breakdown of the above segment:

  • The script continuously checks the current time and compares it to the next_toggle_time for each pin.
  • If the current time surpasses the next toggle time for a pin, the state of that pin is toggled (from HIGH to LOW or vice versa).
  • The next toggle time for the pin is recalculated based on the corresponding duty cycle and the PWM frequency.

UDP Data Reception and Processing

# Check for new data
try:
    more_data, client_address = server_socket.recvfrom(NUM_DOUBLES * 8 - len(data))
    if more_data:
        data += more_data
    if len(data) == NUM_DOUBLES * 8:
        doubles = struct.unpack('d' * NUM_DOUBLES, data)
        if int(doubles[0]) == SAFETY_BIT:
            pwm_frequency = doubles[1]
            duty_cycles = doubles[2:10]
        data = b''
except BlockingIOError:
    pass
  • The script listens for incoming data using the UDP socket.
  • If new data is received, it is appended to the current data.
  • Once a full packet (determined by NUM_DOUBLES * 8) is received, the data is unpacked into double precision numbers.
  • The first number is checked against the SAFETY_BIT to validate the data.
  • If the data is valid, the PWM frequency and duty cycles for the thrusters are updated.

Safe Termination

except KeyboardInterrupt:
    GPIO.output(PINS[0], GPIO.LOW)
    GPIO.output(PINS[1], GPIO.LOW)
    GPIO.output(PINS[2], GPIO.LOW)
    GPIO.output(PINS[3], GPIO.LOW)
    GPIO.output(PINS[4], GPIO.LOW)
    GPIO.output(PINS[5], GPIO.LOW)
    GPIO.output(PINS[6], GPIO.LOW)
    GPIO.output(PINS[7], GPIO.LOW)
    GPIO.cleanup()
    server_socket.close()
    print("\nExiting...")

finally:
    GPIO.output(PINS[0], GPIO.LOW)
    GPIO.output(PINS[1], GPIO.LOW)
    GPIO.output(PINS[2], GPIO.LOW)
    GPIO.output(PINS[3], GPIO.LOW)
    GPIO.output(PINS[4], GPIO.LOW)
    GPIO.output(PINS[5], GPIO.LOW)
    GPIO.output(PINS[6], GPIO.LOW)
    GPIO.output(PINS[7], GPIO.LOW)
    GPIO.cleanup()
    server_socket.close()
  • If a KeyboardInterrupt (like Ctrl+C) or a termination signal is received, the script gracefully shuts down all thrusters by setting their states to LOW.
  • The GPIO pins are cleaned up to ensure they're returned to a safe state.
  • The UDP server socket is closed.
  • The finally block ensures that even if there's an exception or unexpected termination, the cleanup code is executed to guarantee safety.

Click here to go HOME