Adding velocity estimation in hal_pru_generic encoder

I needed this feature for my lathe control panel project. The goal is to drive the jog steps size by the rotation speed of the jog wheels.

The algorithm has been validated from 5 Hz to 100 kHz.
Maximum measurement error is 1%. See curves below.

The patch is available here:


HAL configuration

loadusr -w ../

loadrt threads name1=servo-thread period1=1000000
loadrt hal_pru_generic prucode=$(HAL_RTMOD_DIR)/xenomai/pru_generic.bin pru=0 num_encoders=1 num_pwmgens=1 pru_period=2500 halname=hpg
addf hpg.update servo-thread
addf hpg.capture-position servo-thread

setp hpg.encoder.00.chan.00.A-pin 7 #925 ENCxI
setp hpg.encoder.00.chan.00.counter-mode 2 #Up Counter (counts rising edges on A, always counts up, B ignored)

setp 921 #PWM1
setp hpg.pwmgen.00.out.00.enable 1
setp hpg.pwmgen.00.pwm_period 1000000
setp hpg.pwmgen.00.out.00.value 0.5

loadusr -w sleep 1
loadusr -w python


import subprocess
import time
import os

def readpin(p):
 r = subprocess.check_output('halcmd -s show pin ' + p + '|head -1',shell=True) 
 lst = r.split()
 return lst[3]

def setpin(p, v):
 subprocess.check_output('halcmd -s setp %s %d' % (p, v), shell=True)

freq = 5.0
prev_freq = 0.0
maxerror = 0
f = open('result.csv', 'w')
line = 'in(Hz), out(Hz), err(%), latency(ms)'
print line
f.write(line + '\n')

while freq < 100e3:
 period = int(1e9/freq/2500) * 2500 # because pru_period=2500
 real_freq = 1e9/period
 if (real_freq <> prev_freq):
 prev_freq = real_freq
 setpin('hpg.pwmgen.00.pwm_period', period)
 measured_freq = float(readpin('hpg.encoder.00.chan.00.velocity'))
 latency = float(readpin('hpg.encoder.00.chan.00.latency')) * 1e-6
 error = abs(real_freq / measured_freq - 1.0)
 maxerror = max(maxerror, error)
 line = "%8.1f, %8.1f, %4.2f, %3.0f" % (real_freq, measured_freq, error*100.0, latency)
 print line
 f.write(line + '\n')
 freq = 1.1*freq

print "Max error %.1f%s" % (maxerror*100.0, '%')