Module: xenomai-3 Branch: next Commit: a66aefdeb317df5772f84eeba1468343a9e450c1 URL: http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=a66aefdeb317df5772f84eeba1468343a9e450c1
Author: Jorge Ramirez-Ortiz <j...@xenomai.org> Date: Tue Oct 14 21:03:05 2014 -0400 utils/analogy: calibration - use the analogy's math utils --- include/rtdm/analogy.h | 12 ++ include/rtdm/uapi/analogy.h | 2 +- lib/analogy/Makefile.am | 5 +- lib/analogy/calibration.c | 22 ++ lib/analogy/calibration.h | 21 ++ lib/analogy/math.c | 418 +++++++++++++++++++++++++++++++++++++ utils/analogy/Makefile.am | 2 +- utils/analogy/analogy_calibrate.c | 6 +- utils/analogy/calibration_ni_m.c | 106 +++------- utils/analogy/calibration_ni_m.h | 6 +- 10 files changed, 515 insertions(+), 85 deletions(-) diff --git a/include/rtdm/analogy.h b/include/rtdm/analogy.h index fd26f58..c03d2b1 100644 --- a/include/rtdm/analogy.h +++ b/include/rtdm/analogy.h @@ -236,6 +236,18 @@ void a4l_write_calibration_file(FILE *dst, struct list *l, int a4l_read_calibration_file(char *name, struct a4l_calibration_data *data); +int a4l_math_polyfit(unsigned order, double *r,double orig, + const unsigned dim, double *x, double *y); + +void a4l_math_mean(double *pmean, double *val, unsigned nr); + +void a4l_math_stddev(double *pstddev, + double mean, double *val, unsigned nr); + +void a4l_math_stddev_of_mean(double *pstddevm, + double mean, double *val, unsigned nr); + + #endif /* !DOXYGEN_CPP */ diff --git a/include/rtdm/uapi/analogy.h b/include/rtdm/uapi/analogy.h index 669ded7..a0a1e59 100644 --- a/include/rtdm/uapi/analogy.h +++ b/include/rtdm/uapi/analogy.h @@ -708,9 +708,9 @@ typedef struct a4l_instruction_list a4l_insnlst_t; struct a4l_calibration_subdev { a4l_sbinfo_t *info; + char *name; int slen; int idx; - char *name; }; struct a4l_calibration_subdev_data { diff --git a/lib/analogy/Makefile.am b/lib/analogy/Makefile.am index 500453d..b79d4cb 100644 --- a/lib/analogy/Makefile.am +++ b/lib/analogy/Makefile.am @@ -7,6 +7,7 @@ libanalogy_la_SOURCES = \ descriptor.c \ info.c \ internal.h \ + math.c \ calibration.c \ range.c \ root_leaf.h \ @@ -19,4 +20,6 @@ libanalogy_la_CPPFLAGS = \ -I$(top_srcdir)/lib/boilerplate libanalogy_la_LIBADD = \ - ../boilerplate/libboilerplate.la + ../boilerplate/libboilerplate.la \ + -lm + diff --git a/lib/analogy/calibration.c b/lib/analogy/calibration.c index 74b2789..85860d1 100644 --- a/lib/analogy/calibration.c +++ b/lib/analogy/calibration.c @@ -1,3 +1,25 @@ +/** + * @file + * Analogy for Linux, device, subdevice, etc. related features + * + * @note Copyright (C) 1997-2000 David A. Schleef <d...@schleef.org> + * @note Copyright (C) 2014 Jorge A. Ramirez-Ortiz <j...@xenomai.org> + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + #include <rtdm/analogy.h> #include <stdio.h> #include <errno.h> diff --git a/lib/analogy/calibration.h b/lib/analogy/calibration.h index 04a7e72..4080c60 100644 --- a/lib/analogy/calibration.h +++ b/lib/analogy/calibration.h @@ -1,3 +1,24 @@ +/** + * @file + * Analogy for Linux, internal calibration declarations + * + * @note Copyright (C) 2014 Jorge A Ramirez-Ortiz <j...@xenomai.org> + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + #ifndef __ANALOGY_CALIBRATION_H__ #define __ANALOGY_CALIBRATION_H__ diff --git a/lib/analogy/math.c b/lib/analogy/math.c new file mode 100644 index 0000000..e85464d --- /dev/null +++ b/lib/analogy/math.c @@ -0,0 +1,418 @@ +#include <math.h> + +/* + * Copyright (C) 2014 Gilles Chanteperdrix <gilles.chanteperd...@xenomai.org>. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <stdlib.h> +#include <errno.h> +#include <string.h> +#include <math.h> +#include <assert.h> + +#include <rtdm/analogy.h> + +struct vec { + unsigned dim; + unsigned stride; + double *val; +}; + +struct mat { + unsigned rows; + unsigned cols; + double *val; +}; + +static void vec_init(struct vec *v, unsigned dim, double *val) +{ + v->dim = dim; + v->stride = 1; + v->val = val; +} + +static int vec_alloc(struct vec *v, unsigned dim) +{ + double *val = malloc(sizeof(*val) * dim); + + if (val == NULL) + return -ENOMEM; + + v->dim = dim; + v->stride = 1; + v->val = val; + + return 0; +} + +static void vec_free(struct vec *v) +{ + free(v->val); + v->val = NULL; + v->dim = 0; + v->stride = 0; +} + +static void row_vec_init(struct vec *v, struct mat *m, unsigned row) +{ + v->dim = m->cols; + v->stride = 1; + v->val = m->val + row * m->cols; +} + +static void col_vec_init(struct vec *v, struct mat *m, unsigned col) +{ + v->dim = m->rows; + v->stride = m->cols; + v->val = m->val + col; +} + +static void vec_copy(struct vec *dst, struct vec *src) +{ + const unsigned d_stride = dst->stride, s_stride = src->stride; + double *d_val = dst->val, *s_val = src->val; + unsigned dim = dst->dim; + unsigned i; + + assert(dst->dim == src->dim); + + for (i = 0; i < dim; i++, d_val += d_stride, s_val += s_stride) + *d_val = *s_val; +} + +static inline double *vec_of(struct vec *v, unsigned k) +{ + return v->val + v->stride * k; +} + +static void vec_mult_scalar(struct vec *v, double x) +{ + const unsigned v_stride = v->stride; + const unsigned dim = v->dim; + double *v_val = v->val; + unsigned i; + + for (i = 0; i < dim; i++, v_val += v_stride) + *v_val *= x; +} + +static double vec_dot(struct vec *l, struct vec *r) +{ + const unsigned l_stride = l->stride; + const unsigned r_stride = r->stride; + const double *l_val = l->val; + const double *r_val = r->val; + const unsigned dim = l->dim; + double res = 0; + unsigned i; + + assert(dim == r->dim); + + for (i = 0; i < dim; i++, l_val += l_stride, r_val += r_stride) + res += (*l_val) * (*r_val); + + return res; +} + +static inline double vec_norm2(struct vec *v) +{ + return sqrt(vec_dot(v, v)); +} + +static void vec_vandermonde(struct vec *v, const double x) +{ + const unsigned v_stride = v->stride; + const unsigned dim = v->dim; + double *v_val = v->val; + double tmp = 1; + unsigned i; + + for (i = 0; i < dim; i++, v_val += v_stride, tmp *= x) + *v_val = tmp; +} + +static void vec_householder(struct vec *res, struct vec *v, unsigned k) +{ + const unsigned res_stride = res->stride; + const unsigned v_stride = v->stride; + const unsigned dim = res->dim; + const double *v_val = v->val; + double *x_k, *res_val = res->val; + double alpha; + unsigned j; + + assert(dim == v->dim); + assert(k < dim); + + for (j = 0; j < k; j++, res_val += res_stride, v_val += v_stride) + *res_val = 0; + x_k = res_val; + for (j = k; j < dim; j++, res_val += res_stride, v_val += v_stride) + *res_val = *v_val; + + alpha = (signbit(*x_k) ? 1 : -1) * vec_norm2(res); + *x_k -= alpha; + + vec_mult_scalar(res, 1 / vec_norm2(res)); +} + +static int mat_alloc(struct mat *m, unsigned rows, unsigned cols) +{ + double *val = malloc(sizeof(*val) * rows * cols); + + if (val == NULL) + return -ENOMEM; + + m->rows = rows; + m->cols = cols; + m->val = val; + + return 0; +} + +static void mat_free(struct mat *m) +{ + free(m->val); + m->val = NULL; + m->cols = 0; + m->rows = 0; +} + +static inline double *mat_of(struct mat *m, unsigned row, unsigned col) +{ + return m->val + row * m->cols + col; +} + +static void vec_mult_mat(struct vec *res, struct vec *v, struct mat *m) +{ + const unsigned res_stride = res->stride; + const unsigned cols = m->cols; + double *res_val = res->val; + struct vec cur_col; + unsigned i; + + assert(v->dim == m->rows); + + col_vec_init(&cur_col, m, 0); + for (i = 0; i < cols; i++, cur_col.val++, res_val += res_stride) + *res_val = vec_dot(v, &cur_col); +} + +static void mat_vandermonde(struct mat *m, struct vec *v, const double origin) +{ + const unsigned v_stride = v->stride; + const unsigned m_rows = m->rows; + const unsigned m_cols = m->cols; + const double *v_val = v->val; + struct vec m_row; + unsigned i; + + assert(m->rows == v->dim); + + row_vec_init(&m_row, m, 0); + for (i = 0; i < m_rows; i++, m_row.val += m_cols, v_val += v_stride) + vec_vandermonde(&m_row, *v_val - origin); +} + +static void +house_mult_mat(struct mat *res, struct vec *tmp, struct vec *vh, struct mat *m) +{ + const double *vh_val = vh->val, *tmp_val = tmp->val, *m_val = m->val; + const unsigned tmp_stride = tmp->stride; + const unsigned vh_stride = vh->stride; + const unsigned res_cols = res->cols; + const unsigned res_rows = res->rows; + double *res_val = res->val; + unsigned i, j; + + assert(res_cols == m->cols && + res_rows == m->rows && res->rows == vh->dim); + + vec_mult_mat(tmp, vh, m); + + for (j = 0; j < res_rows; j++, vh_val += vh_stride, tmp_val = tmp->val) + for (i = 0; i < res_cols; i++, tmp_val += tmp_stride) + *res_val++ = (*m_val++) - 2 * (*vh_val) * (*tmp_val); +} + +static void +house_mult_vec(struct vec *res, struct vec *vh, struct vec *v) +{ + const double *vh_val = vh->val, *v_val = v->val; + const unsigned res_stride = res->stride; + const unsigned vh_stride = vh->stride; + const unsigned v_stride = v->stride; + const unsigned res_dim = res->dim; + double *res_val = res->val; + double dot; + unsigned i; + + assert(res_dim == v->dim); + + dot = 2 * vec_dot(vh, v); + + for (i = 0; i < res_dim; i++, res_val += res_stride, + v_val += v_stride, vh_val += vh_stride) + *res_val = (*v_val) - dot * (*vh_val); +} + +static void +mat_upper_triangular_backsub(struct vec *res, struct mat *m, struct vec *v) +{ + unsigned dim = res->dim; + unsigned j; + int i; + + assert(dim == m->cols); + + for (i = dim - 1; i >= 0; i--) { + double sum = *vec_of(v, i); + + for (j = i + 1; j < dim; j++) + sum -= (*mat_of(m, i, j)) * (*vec_of(res, j)); + + *vec_of(res, i) = sum / (*mat_of(m, i, i)); + } +} + +/* + * A = Q.R decomposition using Householder reflections + * Input: R <- A + * Y + * Output: R + * Y <- Q^tY + */ +static int mat_qr(struct mat *r, struct vec *y) +{ + struct vec r_col, vh, tr; + unsigned i; + int rc; + + rc = vec_alloc(&vh, r->rows); + if (rc < 0) + return rc; + + rc = vec_alloc(&tr, y->dim); + if (rc < 0) + goto err_free_vh; + + col_vec_init(&r_col, r, 0); + for (i = 0; i < r->cols; i++, r_col.val++) { + vec_householder(&vh, &r_col, i); + + house_mult_vec(y, &vh, y); + house_mult_mat(r, &tr, &vh, r); + } + + rc = 0; + vec_free(&tr); + err_free_vh: + vec_free(&vh); + return rc; +} + +/* + * We are looking for Res such that A.Res = Y, with A the Vandermonde + * matrix made from the X vector. + * + * Using the least square method, this means finding Res such that: + * A^t.A.Res = A^tY + * + * If we write A = Q.R with Q^t.Q = 1, and R non singular, this can be + * reduced to: + * R.Res = Q^t.Y + * + * mat_qr() gives us R and Q^t.Y from A and Y + * + * We can then obtain Res by back substitution using + * mat_upper_triangular_backsub() with R upper triangular. + */ +int a4l_math_polyfit(unsigned order, double *r, double orig, const unsigned dim, + double *x, double *y) +{ + struct vec v_x, v_y, v_r, qty; + struct mat vdm; + int rc; + + vec_init(&v_x, dim, x); + vec_init(&v_y, dim, y); + vec_init(&v_r, order, r); + + rc = vec_alloc(&qty, dim); + if (rc < 0) + return rc; + vec_copy(&qty, &v_y); + + rc = mat_alloc(&vdm, dim, order); + if (rc < 0) + goto err_free_qty; + + mat_vandermonde(&vdm, &v_x, orig); + + rc = mat_qr(&vdm, &qty); + if (rc == 0) + mat_upper_triangular_backsub(&v_r, &vdm, &qty); + + mat_free(&vdm); + + err_free_qty: + vec_free(&qty); + + return rc; +} + +void a4l_math_mean(double *pmean, double *val, unsigned nr) +{ + double sum; + int i; + + for (sum = 0, i = 0; i < nr; i++) + sum += val[i]; + + *pmean = sum / nr; +} + +void a4l_math_stddev(double *pstddev, double mean, double *val, unsigned nr) +{ + double sum, sum_sq; + int i; + + for (sum = 0, sum_sq = 0, i = 0; i < nr; i++) { + double x = val[i] - mean; + sum_sq += x * x; + sum += x; + } + + *pstddev = sqrt((sum_sq - (sum * sum) / nr) / (nr - 1)); +} + +void a4l_math_stddev_of_mean(double *pstddevm, double mean, double *val, unsigned nr) +{ + double sum, sum_sq; + int i; + + for (sum = 0, sum_sq = 0, i = 0; i < nr; i++) { + double x = val[i] - mean; + sum_sq += x * x; + sum += x; + } + + *pstddevm = sqrt(((sum_sq - (sum * sum) / nr) / (nr - 1)) / nr); +} + + + diff --git a/utils/analogy/Makefile.am b/utils/analogy/Makefile.am index a555260..6ffd0f2 100644 --- a/utils/analogy/Makefile.am +++ b/utils/analogy/Makefile.am @@ -46,7 +46,7 @@ analogy_calibrate_LDADD = \ ../../lib/analogy/libanalogy.la \ ../../lib/cobalt/libcobalt.la \ @XENO_USER_LDADD@ \ - -lpthread -lrt -lgsl -lgslcblas -lm + -lpthread -lrt cmd_read_SOURCES = cmd_read.c cmd_read_LDADD = \ diff --git a/utils/analogy/analogy_calibrate.c b/utils/analogy/analogy_calibrate.c index 5963cc4..f9add4d 100644 --- a/utils/analogy/analogy_calibrate.c +++ b/utils/analogy/analogy_calibrate.c @@ -32,7 +32,7 @@ #include "calibration_ni_m.h" struct timespec calibration_start_time; -static const char *revision = "0.0.1"; +static const char *revision = "1.0.0"; a4l_desc_t descriptor; static const struct option options[] = { @@ -105,8 +105,8 @@ int main(int argc, char *argv[]) } } - if (!p) - error(EXIT, errno, "calibration file not present"); + if (!p || !device) + error(EXIT, errno, "missing input parameters"); fd = a4l_open(&descriptor, device); if (fd < 0) diff --git a/utils/analogy/calibration_ni_m.c b/utils/analogy/calibration_ni_m.c index 2115b6d..dc2f667 100644 --- a/utils/analogy/calibration_ni_m.c +++ b/utils/analogy/calibration_ni_m.c @@ -234,88 +234,51 @@ static int data_read_async(void *dst, struct a4l_calibration_subdev *s, /* * - * math: uses the gnu statistic library and the math library + * math: uses the a4l statistic helpers and the math library * */ -static int statistics_standard_deviation_of_mean(double *dst, double src[], - int len, double mean) +static void +statistics_standard_deviation_of_mean(double *dst, double src[], int len, + double mean) { - double a; - - a = gsl_stats_variance_m(src, 1, len, mean); - a = sqrt(a/len); - *dst = a; - - return 0; + a4l_math_stddev_of_mean(dst, mean, src, len); } -static int statistics_standard_deviation(double *dst, double src[], int len, - double mean) +static void +statistics_standard_deviation(double *dst, double src[], int len, double mean) { - double a; - - a = gsl_stats_variance_m(src, 1, len, mean); - a = sqrt(a); - *dst = a; - - return 0; + a4l_math_stddev(dst, mean, src, len); } -static int statistics_mean(double *dst, double src[], int len) +static void statistics_mean(double *dst, double src[], int len) { - *dst = gsl_stats_mean(src, 1, len); - - return 0; + a4l_math_mean(dst, src, len); } static int polynomial_fit(struct polynomial *dst, struct codes_info *src) { - gsl_multifit_linear_workspace *work; - const int nb_coeff = dst->order + 1; - gsl_matrix *covariance, *m; - gsl_vector_view b, result; - double a, *tmp, chisq; - int i, j, len; + double *measured; + double *nominal; + int i, ret; + + dst->nb_coefficients = dst->order + 1; + dst->coefficients = malloc(sizeof(double) * dst->nb_coefficients); + measured = malloc(sizeof(double) * src->nb_codes); + nominal = malloc(sizeof(double) * src->nb_codes); - work = gsl_multifit_linear_alloc(src->nb_codes, nb_coeff); - covariance = gsl_matrix_alloc(nb_coeff, nb_coeff); - m = gsl_matrix_alloc(src->nb_codes, nb_coeff); + if (!dst->coefficients || !measured || !nominal) + return -ENOMEM; for (i = 0; i < src->nb_codes; i++) { - gsl_matrix_set(m, i, 0, 1.0); - for (j = 1; j < nb_coeff; j++) { - a = gsl_matrix_get(m, i, j - 1); - a = a * (src->codes[i].nominal - dst->expansion_origin); - gsl_matrix_set(m, i, j, a); - } + measured[i] = src->codes[i].measured; + nominal[i] = src->codes[i].nominal; } - len = src->nb_codes * sizeof(double); - tmp = malloc(len); - if (!tmp) - error(EXIT, 0, "malloc (%d)", len); - - for (i = 0; i < src->nb_codes; i++) - tmp[i] = src->codes[i].measured; + ret = a4l_math_polyfit(dst->nb_coefficients, dst->coefficients, + dst->expansion_origin, + src->nb_codes, nominal, measured); - b = gsl_vector_view_array(tmp, src->nb_codes); - - dst->nb_coefficients = nb_coeff; - len = dst->nb_coefficients * sizeof(double); - dst->coefficients = malloc(len); - if (!dst->coefficients) - error(EXIT, 0, "malloc (%d)", len); - - result = gsl_vector_view_array(dst->coefficients, nb_coeff); - gsl_multifit_linear(m, &b.vector, &result.vector, covariance, &chisq, - work); - gsl_matrix_free(m); - gsl_matrix_free(covariance); - gsl_multifit_linear_free(work); - - free(tmp); - - return 0; + return ret; } static int polynomial_linearize(double *dst, struct polynomial *p, double val) @@ -568,19 +531,10 @@ static int characterize_pwm(struct pwm_info *dst, int pref, unsigned range) if (err) error(EXIT, 0, "read_doubles"); - err = math.stats.mean(&mean, p, len/sizeof(*p)); - if (err) - error(EXIT, 0, "estimate_mean"); - - err = math.stats.stddev(&stddev, p, len/sizeof(*p), mean); - if (err) - error(EXIT, 0, "estimate_stddev"); - - err = math.stats.stddev_of_mean(&stddev_of_mean, p, - len/sizeof(*p), mean); - if (err) - error(EXIT, 0, "estimate_stddev_of_mean"); - + math.stats.mean(&mean, p, len/sizeof(*p)); + math.stats.stddev(&stddev, p, len/sizeof(*p), mean); + math.stats.stddev_of_mean(&stddev_of_mean, p, + len/sizeof(*p), mean); dst->node[i].up_tick = up_ticks; dst->node[i].mean = mean; } diff --git a/utils/analogy/calibration_ni_m.h b/utils/analogy/calibration_ni_m.h index 7cbdce3..102d271 100644 --- a/utils/analogy/calibration_ni_m.h +++ b/utils/analogy/calibration_ni_m.h @@ -157,9 +157,9 @@ struct codes_info { int nb_codes; }; -typedef int (*statistics_standard_deviation_of_mean_function)(double *, double [], int, double ); -typedef int (*statistics_standard_deviation_function)(double *, double [], int, double); -typedef int (*statistics_mean_function)(double *, double [], int); +typedef void (*statistics_standard_deviation_of_mean_function)(double *, double [], int, double ); +typedef void (*statistics_standard_deviation_function)(double *, double [], int, double); +typedef void (*statistics_mean_function)(double *, double [], int); struct statistics_ops { statistics_standard_deviation_of_mean_function stddev_of_mean; _______________________________________________ Xenomai-git mailing list Xenomai-git@xenomai.org http://www.xenomai.org/mailman/listinfo/xenomai-git