Wireless Servo Control - Part 3.1: Failsafe servo control

Since my previous post in this series I have been using the servo control code fairly extensively and have come across a recurring problem. If the application hangs/crashes or is terminated (without termination handling included and working), the output channels simply maintain there previous values. If a channel is connected to a motor controller this could leave the motor running, potentially causing your car/robot/aircraft to run away from you (and in to a wall) at great speed! This is not ideal, therefore, before my final post in the series, I thought I'd include this one on implementing a failsafe in the PRU code.

Here is the complete failsafe PRU code, with the new code highlighted

.origin 0
.entrypoint START
#include "../include/pru.hp"

START:
// Preamble to set up OCP and shared RAM
	LBCO	r0, CONST_PRUCFG, 4, 4		// Enable OCP master port
	CLR 	r0, r0, 4					// Clear SYSCFG[STANDBY_INIT] to enable OCP master port
	SBCO	r0, CONST_PRUCFG, 4, 4
	MOV		r0, 0x00000120				// Configure the programmable pointer register for PRU0 by setting c28_pointer[15:0]
	MOV		r1, CTPPR_0					// field to 0x0120.  This will make C28 point to 0x00012000 (PRU shared RAM).
	ST32	r0, r1
// End of preamble

// Shared memory registers
//	Int 0: 		Total PWM period (number of PRU cycles)
//	Int 1-8:	Pulse width for each channel (number of PRU cycles)
//	Int 9:		Failsafe timeout (number of PWM cycles)
//				e.g. for a PWM frequency of 50Hz, a value of 100 would be a 2s timeout
//				Must be reset everytime channels are updated
//				Set to 0 to disable failsafe feature
//	Int 10-17:	Failsafe PWM periods (number of PRU cycles)
LOOP1:										// Outer loop repeats everything at PWM period
	LBCO	r0, CONST_PRUSHAREDRAM, 0, 40	// Load in registers from shared memory
	QBEQ	NO_FAILSAFE, r9, 0				// Check to see if failsafe is enabled
	QBEQ	FAILSAFE, r9, 1					// Check to see if failsafe timeout has occured
	SUB		r9, r9, 1						// If not timed out, decrement timeout counter
	SBCO	r9, CONST_PRUSHAREDRAM, 36, 4	// Write timeout counter back to shared RAM
	QBA		NO_FAILSAFE						// Skip failsafe action
FAILSAFE:
	LBCO	r1, CONST_PRUSHAREDRAM, 40, 32	// Overwrite commanded positions with failsafe positions

NO_FAILSAFE:
	MOV		r30.b0, 0xFF					// Turn on all output channels for start of cycle

LOOP2:										// Inner loop to handle channels
		SUB		r0, r0, 1					// Subtract one from each register
		SUB		r1, r1, 1
		SUB		r2, r2, 1
		SUB		r3, r3, 1
		SUB		r4, r4, 1
		SUB		r5, r5, 1
		SUB		r6, r6, 1
		SUB		r7, r7, 1
		SUB		r8, r8, 1
		QBNE	SKIP1, r1, 0				// Compare each register with zero
		CLR		r30.t0						// If zero then turn off the corresponding channel
SKIP1:										// Otherwise skip that channel (leaving it high)
		QBNE	SKIP2, r2, 0
		CLR		r30.t1
SKIP2:
		QBNE	SKIP3, r3, 0
		CLR		r30.t2
SKIP3:
		QBNE	SKIP4, r4, 0
		CLR		r30.t3
SKIP4:
		QBNE	SKIP5, r5, 0
		CLR		r30.t4
SKIP5:
		QBNE	SKIP6, r6, 0
		CLR		r30.t5
SKIP6:
		QBNE	SKIP7, r7, 0
		CLR		r30.t6
SKIP7:
		QBNE	SKIP8, r8, 0
		CLR		r30.t7
SKIP8:
		QBEQ	LOOP1, r0, 0			// If cycle register is empty, restart
		QBA		LOOP2					// Total of 19 statements per cycle, 95ns
	HALT								// Halt the processor (although we will never get here...)

  • Lines 37-41 are just some comments about the register values.
  • Line 42 is the start of the PWM cycle, as before.
  • Line 43 reads in one extra value than before, register 9, which corresponds to the number of PWM cycles remaining until the failsafe timeout should be triggered. For example if the PWM frequency is 50Hz, a value of 100 would correspond to a 2s timeout.
  • Line 44 checks to see if the failsafe timeout has been set to zero, indicating the host code wishes to disable the failsafe. If so, the failsafe logic is skipped.
  • Line 45 checks to see if the failsafe timeout has reached 1, indicating that the timeout has occured. If so, the timer logic is skipped and execution skips ahead to load the failsafe values.
  • Lines 46-47 decrement the failsafe counter and save the new value back in to shared RAM. The host code is able to interrogate this register to see how long until the failsafe will be triggered.
  • Line 48 skips over the failsafe as we've not timed out yet.
  • Line 49 is just a label representing the start of the failsafe behaviour.
  • Line 50 loads the failsafe values in to the local registers r1-r8, overwriting the channel values loaded previously (on line 43).
  • (Line 51 is blank)
  • Line 52 is a label for skipping the failsafe.
  • Line 53 sets all the output channels high in (start of PWM cycle) one command. In my previous code I used 8 separate SET¬†instructions to do this, but as I've now introduced additional failsafe instructions at the beginning of a PWM cycle I figured I should optimise this so I don't drift too far from the desired frequency.

I have modified the C++ class to include setFailsafeValue and setFailsafeTimeout methods. In addition, whenever a call to setChannelValue is made, the failsafe timeout register is reset. Including the failsafe functionality in the PRU itself also removes the need for a signal handling code to catch Ctrl+C. The new example code is now a lot simpler, it looks like this

#include "pruPWM.h"

// Entry point
int main() {
	// Initialise PRU PWM
	PRUPWM *myPWM = new PRUPWM();

	// Set Channel 0 failsafe value
	myPWM->setFailsafeValue(0, 1000000);
	
	// Set a 2s failsafe timeout
	myPWM->setFailsafeTimeout(2000);
	
	// Start the PRU
	myPWM->start();

	// An example loop
	int pwm0 = 1000000, pwm7 = 2000000;
	int step = 100;
	while (true) {
		myPWM->setChannelValue(0,pwm0);
		myPWM->setChannelValue(7,pwm7);
		pwm0 += step;
		pwm7 -= step;
		if (pwm0 >= 2000000 || pwm0 <= 1000000 || pwm7 >= 2000000 || pwm7 <= 1000000) {
			step *= -1;
			usleep(1000000);
		}
		usleep(100);
	}
}

This example represents a typical RC aircraft configuration, where channel 0 represents the throttle channel and 1000us is its off position. If the code is halted for any reason, 2s after the last call to setChannelValue the PRU will default all channels to 1500us with the exception of channel 0 which will be set to 1000us.

Some modern RC systems enable more complex failsafes such as keeping some channels at their current value whilst failsafing others. This behavour can be achieved by calling setFailsafeValue whenever setChannelValue is called, for those channels you wish to retain their values.

Posted in BeagleBone Black

Leave a Reply

Your email address will not be published. Required fields are marked *

*


two − = 1

You may use these HTML tags and attributes: <a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <strike> <strong>