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.
Leave a Reply