You are viewing an old version of this page. View the current version.
Compare with Current
View Page History
« Previous
Version 18
Next »
Reading TimingData
tst-drp_udpencoder[17524]: <I> TD: pulseId=971636136596 timeStamp=4491862046510007
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971636227596 timeStamp=4491862144510007
tst-drp_udpencoder[17524]: <I> TD: fixedRates 1 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971636318596 timeStamp=4491862242510007
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971636409596 timeStamp=4491862340510007
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971636500596 timeStamp=4491862438510007
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971636591596 timeStamp=4491865831477303
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971636682596 timeStamp=4491865929477303
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971636773596 timeStamp=4491866027477303
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971636864596 timeStamp=4491866125477303
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971636955596 timeStamp=4491866223477303
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971637046596 timeStamp=4491866321477303
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971637137596 timeStamp=4491866419477303
tst-drp_udpencoder[17524]: <I> TD: fixedRates 1 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971637228596 timeStamp=4491866517477303
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971637319596 timeStamp=4491866615477303
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971637410596 timeStamp=4491866713477303
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971637501596 timeStamp=4491870106444599
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971637592596 timeStamp=4491870204444599
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971637683596 timeStamp=4491870302444599
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971637774596 timeStamp=4491870400444599
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971637865596 timeStamp=4491870498444599
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971637956596 timeStamp=4491870596444599
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971638047596 timeStamp=4491870694444599
tst-drp_udpencoder[17524]: <I> TD: fixedRates 1 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[17524]: <I> TD: pulseId=971638138596 timeStamp=4491870792444599
tst-drp_udpencoder[17524]: <I> TD: fixedRates 0 1 1 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152713566 timeStamp=4494250361986583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152714476 timeStamp=4494250362966583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152715386 timeStamp=4494250363946583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152716296 timeStamp=4494250364926583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 1 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152717206 timeStamp=4494250365906583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152718116 timeStamp=4494250366886583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152719026 timeStamp=4494250367866583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152719936 timeStamp=4494250368846583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152720846 timeStamp=4494250369826583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152721756 timeStamp=4494250370806583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152722666 timeStamp=4494250371786583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152723576 timeStamp=4494250372766583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152724486 timeStamp=4494250373746583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152725396 timeStamp=4494250374726583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 1 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152726306 timeStamp=4494250375706583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152727216 timeStamp=4494250376686583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152728126 timeStamp=4494250377666583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152729036 timeStamp=4494250378646583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152729946 timeStamp=4494250379626583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152730856 timeStamp=4494250380606583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152731766 timeStamp=4494250381586583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152732676 timeStamp=4494250382566583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152733586 timeStamp=4494250383546583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152734496 timeStamp=4494250384526583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 1 1 1 1 1 0 0 0
tst-drp_udpencoder[18268]: <I> TD: pulseId=972152735406 timeStamp=4494250385506583
tst-drp_udpencoder[18268]: <I> TD: fixedRates 0 0 0 1 1 1 1 0 0 0
Demonstration of Polynomial Fit Using Eigen Package
Test Code
#include <Eigen/Dense>
#include <iostream>
#include <cmath>
#include <vector>
#include <Eigen/QR>
#include <unistd.h>
#include <getopt.h>
bool lVerbose = false;
void usage(const char *name)
{
printf("usage: %s [-r <ratio>] [-v] val1 val2 val3 [val4 ...]\n", name);
}
void polyfit(const std::vector<double> &t,
const std::vector<double> &v,
std::vector<double> &coeff,
int order
)
{
static bool firstCall = true;
// Create Matrix Placeholder of size n x k, n= number of datapoints, k = order of polynomial, for exame k = 3 for cubic polynomial
Eigen::MatrixXd T(t.size(), order + 1);
Eigen::VectorXd V = Eigen::VectorXd::Map(&v.front(), v.size());
Eigen::VectorXd result;
// check to make sure inputs are correct
assert(t.size() == v.size());
assert(t.size() >= order + 1);
if (firstCall && lVerbose) {
printf("t.size() = %zu\n", t.size());
printf("v.size() = %zu\n", v.size());
firstCall = false;
}
// Populate the matrix
for(size_t i = 0 ; i < t.size(); ++i)
{
for(size_t j = 0; j < order + 1; ++j)
{
T(i, j) = pow(t.at(i), j);
}
}
//std::cout<<T<<std::endl;
// Solve for linear least square fit
result = T.householderQr().solve(V);
coeff.resize(order+1);
for (int k = 0; k < order+1; k++)
{
coeff[k] = result[k];
}
}
int main(int argc, char **argv)
{
int c;
unsigned long ratio = 10;
int order = 2;
while ((c = getopt(argc, argv, "r:o:vh")) != EOF) {
switch(c) {
case 'r':
ratio = std::stoul(optarg);
break;
case 'o':
order = std::stoul(optarg);
break;
case 'v':
lVerbose = true;
break;
case 'h':
usage(argv[0]);
return 0;
default:
return 1;
}
}
int input_count = argc - optind;
if (input_count < 3) {
printf("usage: %s [-r <ratio>] [-o <order>] [-v] val1 val2 val3 [val4 ...]\n", argv[0]);
return 1;
}
if (lVerbose) {
printf("ratio: %u\n", ratio);
}
std::vector<double> time;
std::vector<double> velocity;
for (int index = optind; index < argc; index++) {
if (lVerbose) {
printf ("Non-option argument %s\n", argv[index]);
}
double increment = (1.0 / (double) (input_count - 1));
time.push_back((index - optind) * increment);
velocity.push_back(std::stoul(argv[index]));
}
// placeholder for storing polynomial coefficient
std::vector<double> coeff;
polyfit(time, velocity, coeff, order);
if (lVerbose) {
printf("coeff.size = %u\n", coeff.size());
std::cout<< "Printing fitted values (q)" << std::endl;
}
double vfitted;
unsigned fast_index;
for(unsigned fast_index = 0; fast_index < (input_count - 1) * ratio + 1; fast_index ++) {
if (fast_index > 0) {
std::cout << ",";
}
double q = ((double) fast_index / (ratio * (input_count - 1)));
vfitted = coeff[0];
for (int ord = 1; ord <= order; ord++) {
vfitted += (coeff[ord] * pow(q, ord));
}
// if fitted value is less than zero then set it to 0
if (vfitted < 0.0) {
vfitted = 0.0;
}
// if fitted value is greater than 0xffffffff then set it to 0xffffffff
if (vfitted > 0xffffffff) {
vfitted = 0xffffffff;
}
std::cout <<std::fixed << vfitted;
}
std::cout<<std::endl;
return 0;
}
ifndef GXX
$(error GXX must be set in environment)
endif
ifndef CONDA_PREFIX
$(error CONDA_PREFIX must be set in environment)
endif
eigen: eigen.cc
$(GXX) eigen.cc -o eigen -I $(CONDA_PREFIX)/include/eigen3
polyfit.py
#!/usr/bin/env python
import matplotlib.pyplot as plt
import argparse
import subprocess
import sys
def main():
parser = argparse.ArgumentParser(prog="polyfit", usage="%(prog)s [options] val val val [val...]")
parser.add_argument('val', nargs='*', help='3 or more values')
parser.add_argument('-r', type=int, default=10, help='trigger rate ratio (default=10)')
parser.add_argument('-o', type=int, default=2, help='order of polynomial (default=2)')
parser.add_argument('-v', action='store_true', help='be verbose')
args = parser.parse_args()
ratio = args.r
order = args.o
cmd = ["./eigen", "-o", f"{order}", "-r", f"{ratio}"] + args.val
if len(args.val) < 3:
parser.error("3 or more values required")
if args.v:
print(f"cmd: {cmd}")
xx = subprocess.run(cmd, capture_output=True)
result = xx.stdout.decode('UTF-8').strip()
res = [ float(x) for x in result.split(",") ]
if args.v:
print(f"ratio: {ratio}")
slow_times = range(0, ratio * len(args.val), ratio)
fast_times = range(0, len(res), 1)
if args.v:
print(f"slow_times: (len {len(slow_times)}): {slow_times}")
print(f"args.val: (len {len(args.val)}): {args.val}")
print(f"fast_times: (len {len(fast_times)}): {fast_times}")
print(f"res: (len {len(res)}): {res}")
intval = [int(v) for v in args.val]
plt.plot(slow_times, intval, 'ro', markersize=12.0)
plt.plot(fast_times, res, 'g^')
plt.title(f'Interpolated Encoder Values (order={order})')
plt.grid(True)
plt.show()
if __name__ == '__main__':
main()