Module: xenomai-3
Branch: next
Commit: 3a4768dc4f3370f989a3ebdbd08b8580aa2cb1ba
URL:    
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=3a4768dc4f3370f989a3ebdbd08b8580aa2cb1ba

Author: Jorge Ramirez-Ortiz <j...@xenomai.org>
Date:   Fri Oct 24 08:59:31 2014 -0400

lib/analogy: calibration - a4l_rawtodcal & a4l_dcaltoraw

---

 include/rtdm/analogy.h      |    9 ++
 include/rtdm/uapi/analogy.h |    6 ++
 lib/analogy/calibration.c   |  237 ++++++++++++++++++++++++++++++++++++++++++-
 lib/analogy/math.c          |    8 +-
 utils/analogy/Makefile.am   |    2 +-
 5 files changed, 252 insertions(+), 10 deletions(-)

diff --git a/include/rtdm/analogy.h b/include/rtdm/analogy.h
index a87ce54..066d05a 100644
--- a/include/rtdm/analogy.h
+++ b/include/rtdm/analogy.h
@@ -232,6 +232,15 @@ int a4l_dtoraw(a4l_chinfo_t *chan,
 
 int a4l_read_calibration_file(char *name, struct a4l_calibration_data *data);
 
+int a4l_get_softcal_converter(struct a4l_polynomial *converter,
+                             int subd, int chan, int range,
+                             struct a4l_calibration_data *data );
+
+int a4l_rawtodcal(a4l_chinfo_t *chan, double *dst, void *src,
+                 int cnt, struct a4l_polynomial *converter);
+int a4l_dcaltoraw(a4l_chinfo_t * chan, void *dst, double *src, int cnt,
+                 struct a4l_polynomial *converter);
+
 int a4l_math_polyfit(unsigned order, double *r,double orig,
                     const unsigned dim, double *x, double *y);
 
diff --git a/include/rtdm/uapi/analogy.h b/include/rtdm/uapi/analogy.h
index a0a1e59..2d53168 100644
--- a/include/rtdm/uapi/analogy.h
+++ b/include/rtdm/uapi/analogy.h
@@ -732,6 +732,12 @@ struct a4l_calibration_data {
        struct a4l_calibration_subdev_data *ao;
 };
 
+struct a4l_polynomial {
+       int expansion;
+       int order;
+       int nb_coeff;
+       double *coeff;
+};
 
 
 #endif /* _RTDM_UAPI_ANALOGY_H */
diff --git a/lib/analogy/calibration.c b/lib/analogy/calibration.c
index dcec16d..9fd944c 100644
--- a/lib/analogy/calibration.c
+++ b/lib/analogy/calibration.c
@@ -20,6 +20,7 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA.
  */
 
+#include <math.h>
 #include <rtdm/analogy.h>
 #include <stdio.h>
 #include <errno.h>
@@ -28,8 +29,39 @@
 #include "boilerplate/list.h"
 #include "calibration.h"
 
+
 #define ARRAY_LEN(a)  (sizeof(a) / sizeof((a)[0]))
 
+static lsampl_t data32_get(void *src)
+{
+       return (lsampl_t) * ((lsampl_t *) (src));
+}
+
+static lsampl_t data16_get(void *src)
+{
+       return (lsampl_t) * ((sampl_t *) (src));
+}
+
+static lsampl_t data8_get(void *src)
+{
+       return (lsampl_t) * ((unsigned char *)(src));
+}
+
+static void data32_set(void *dst, lsampl_t val)
+{
+       *((lsampl_t *) (dst)) = val;
+}
+
+static void data16_set(void *dst, lsampl_t val)
+{
+       *((sampl_t *) (dst)) = (sampl_t) (0xffff & val);
+}
+
+static void data8_set(void *dst, lsampl_t val)
+{
+       *((unsigned char *)(dst)) = (unsigned char)(0xff & val);
+}
+
 static inline int read_dbl(double *d, struct _dictionary_ *f,const char *subd,
                           int subd_idx, char *type, int type_idx)
 {
@@ -73,7 +105,7 @@ static inline int read_int(int *val, struct _dictionary_ *f, 
const char *subd,
 }
 
 static inline int read_str(char **val, struct _dictionary_ *f, const char 
*subd,
-                          const char *type)
+                          const char *type)
 {
        char *str;
        int ret;
@@ -105,7 +137,7 @@ static inline void write_calibration(FILE *file, char *fmt, 
...)
 
 void
 write_calibration_file(FILE *dst, struct list *l,
-                      struct a4l_calibration_subdev *subd, a4l_desc_t *desc)
+                      struct a4l_calibration_subdev *subd, a4l_desc_t *desc)
 {
        struct subdevice_calibration_node *e, *t;
        int i, j = 0;
@@ -205,7 +237,7 @@ int a4l_read_calibration_file(char *name, struct 
a4l_calibration_data *data)
                if (strncmp(subdevice[k], AI_SUBD_STR,
                            strlen(AI_SUBD_STR)) == 0) {
                        data->ai = malloc(nb_elements *
-                                  sizeof(struct a4l_calibration_subdev_data));
+                                         sizeof(struct 
a4l_calibration_subdev_data));
                        data->nb_ai = nb_elements;
                        p  = data->ai;
                }
@@ -213,7 +245,7 @@ int a4l_read_calibration_file(char *name, struct 
a4l_calibration_data *data)
                if (strncmp(subdevice[k], AO_SUBD_STR,
                            strlen(AO_SUBD_STR)) == 0) {
                        data->ao = malloc(nb_elements *
-                                  sizeof(struct a4l_calibration_subdev_data));
+                                         sizeof(struct 
a4l_calibration_subdev_data));
                        data->nb_ao = nb_elements;
                        p = data->ao;
                }
@@ -232,7 +264,7 @@ int a4l_read_calibration_file(char *name, struct 
a4l_calibration_data *data)
 
                        for (j = 0; j < p->nb_coeff; j++) {
                                read_dbl(&p->coeff[j], d, subdevice[k], i,
-                                       COEFF_STR, j);
+                                        COEFF_STR, j);
                        }
 
                        p->index = index;
@@ -244,5 +276,200 @@ int a4l_read_calibration_file(char *name, struct 
a4l_calibration_data *data)
        return 0;
 }
 
+/**
+ * @brief Get the polynomial that will be use for the software calibration
+ *
+ * @param[out] converter Polynomial to be used on the software calibration
+ * @param[in] subd Subdevice index
+ * @param[in] chan Channel
+ * @param[in] range Range
+ * @param[in] data Calibration data read from the calibration file
+ *
+ * @return -1 on error
+ *
+ */
+
+int a4l_get_softcal_converter(struct a4l_polynomial *converter,
+                             int subd, int chan, int range,
+                             struct a4l_calibration_data *data )
+{
+       int i;
+
+       for (i = 0; i < data->nb_ai; i++) {
+               if (data->ai[i].index != subd)
+                       break;
+               if ((data->ai[i].channel == chan || data->ai[i].channel == -1)
+                   &&
+                   (data->ai[i].range == range || data->ai[i].range == -1)) {
+                       converter->expansion = data->ai[i].expansion;
+                       converter->nb_coeff = data->ai[i].nb_coeff;
+                       converter->coeff = data->ai[i].coeff;
+                       converter->order = data->ai[i].nb_coeff - 1;
+                       return 0;
+               }
+       }
+
+       for (i = 0; i < data->nb_ao; i++) {
+               if (data->ao[i].index != subd)
+                       break;
+               if ((data->ao[i].channel == chan || data->ao[i].channel == -1)
+                   &&
+                   (data->ao[i].range == range || data->ao[i].range == -1)) {
+                       converter->expansion = data->ao[i].expansion;
+                       converter->nb_coeff = data->ao[i].nb_coeff;
+                       converter->coeff = data->ao[i].coeff;
+                       converter->order = data->ao[i].nb_coeff - 1;
+                       return 0;
+               }
+       }
+
+       return -1;
+}
+
+/**
+ * @brief Convert raw data (from the driver) to calibrated double units
+ * @param[in] chan Channel descriptor
+ * @param[out] dst Ouput buffer
+ * @param[in] src Input buffer
+ * @param[in] cnt Count of conversion to perform
+ * @param[in] converter Conversion polynomial
+ *
+ *
+ * @return the count of conversion performed, otherwise a negative
+ * error code:
+ *
+ * - -EINVAL is returned if some argument is missing or wrong;
+ *    chan, rng and the pointers should be checked; check also the
+ *    kernel log ("dmesg"); WARNING: a4l_fill_desc() should be called
+ *    before using a4l_rawtodcal()
+ *
+ *
+ *
+ */
+
+int a4l_rawtodcal(a4l_chinfo_t *chan, double *dst, void *src,
+                 int cnt, struct a4l_polynomial *converter)
+{
+       int i = 0, j = 0, k = 0;
+       double term = 1.0;
+       lsampl_t tmp;
+       int size;
+
+       /* Temporary data accessor */
+       lsampl_t(*datax_get) (void *);
+
+       /* Basic checking */
+       if (chan == NULL)
+               return -EINVAL;
+
+       /* Find out the size in memory */
+       size = a4l_sizeof_chan(chan);
+
+       /* Get the suitable accessor */
+       switch (a4l_sizeof_chan(chan)) {
+       case 4:
+               datax_get = data32_get;
+               break;
+       case 2:
+               datax_get = data16_get;
+               break;
+       case 1:
+               datax_get = data8_get;
+               break;
+       default:
+               return -EINVAL;
+       };
+
+       while (j < cnt) {
+               /* Properly retrieve the data */
+               tmp = datax_get(src + i);
+
+               /* Perform the conversion */
+               dst[j] = 0.0;
+               term = 1.0;
+               for (k = 0; k < converter->nb_coeff; k++) {
+                       dst[j] += converter->coeff[k] * term;
+                       term *= tmp - converter->expansion;
+               }
+
+               /* Update the counters */
+               i += size;
+               j++;
+       }
+
+       return j;
+}
+
+/**
+ * @brief Convert double values to raw calibrated data using polynomials
+ *
+ * @param[in] chan Channel descriptor
+ * @param[out] dst Ouput buffer
+ * @param[in] src Input buffer
+ * @param[in] cnt Count of conversion to perform
+ * @param[in] converter Conversion polynomial
+ *
+ * @return the count of conversion performed, otherwise a negative
+ * error code:
+ *
+ * - -EINVAL is returned if some argument is missing or wrong;
+ *    chan, rng and the pointers should be checked; check also the
+ *    kernel log ("dmesg"); WARNING: a4l_fill_desc() should be called
+ *    before using a4l_dcaltoraw()
+ *
+ */
+
+int a4l_dcaltoraw( a4l_chinfo_t * chan, void *dst, double *src, int cnt,
+                  struct a4l_polynomial *converter)
+{
+       int size, i = 0, j = 0, k = 0;
+       double value, term;
+
+       /* Temporary data accessor */
+       void (*datax_set) (void *, lsampl_t);
+
+       /* Basic checking */
+       if (chan == NULL)
+               return -EINVAL;
+
+       /* Find out the size in memory */
+       size = a4l_sizeof_chan(chan);
+
+       /* Select the suitable accessor */
+       switch (size) {
+       case 4:
+               datax_set = data32_set;
+               break;
+       case 2:
+               datax_set = data16_set;
+               break;
+       case 1:
+               datax_set = data8_set;
+               break;
+       default:
+               return -EINVAL;
+       };
+
+       while (j < cnt) {
+
+               /* Performs the conversion */
+               value = 0.0;
+               term = 1.0;
+               for (k = 0; k < converter->nb_coeff; k++) {
+                       value += converter->coeff[k] * term;
+                       term *= src[j] - converter->expansion;
+               }
+               value = nearbyint(value);
+
+               datax_set(dst + i, (lsampl_t) value);
+
+               /* Updates the counters */
+               i += size;
+               j++;
+       }
+
+       return j;
+}
+
 /** @} Calibration API */
 
diff --git a/lib/analogy/math.c b/lib/analogy/math.c
index a6d3c31..c0e2f60 100644
--- a/lib/analogy/math.c
+++ b/lib/analogy/math.c
@@ -336,7 +336,7 @@ static int mat_qr(struct mat *r, struct vec *y)
 /**
  * @brief Calculate the polynomial fit
  *
- * @param[in] order Order of the resulting polynomial
+ * @param[in] r_dim Number of elements of the resulting polynomial
  * @param[out] r Polynomial
  * @param[in] orig
  * @param[in] dim Number of elements in the polynomials
@@ -362,7 +362,7 @@ static int mat_qr(struct mat *r, struct vec *y)
  * mat_upper_triangular_backsub() with R upper triangular.
  *
  */
-int a4l_math_polyfit(unsigned order, double *r, double orig, const unsigned 
dim,
+int a4l_math_polyfit(unsigned r_dim, double *r, double orig, const unsigned 
dim,
                double *x, double *y)
 {
        struct vec v_x, v_y, v_r, qty;
@@ -371,14 +371,14 @@ int a4l_math_polyfit(unsigned order, double *r, double 
orig, const unsigned dim,
 
        vec_init(&v_x, dim, x);
        vec_init(&v_y, dim, y);
-       vec_init(&v_r, order, r);
+       vec_init(&v_r, r_dim, r);
 
        rc = vec_alloc(&qty, dim);
        if (rc < 0)
                return rc;
        vec_copy(&qty, &v_y);
 
-       rc = mat_alloc(&vdm, dim, order);
+       rc = mat_alloc(&vdm, dim, r_dim);
        if (rc < 0)
                goto err_free_qty;
 
diff --git a/utils/analogy/Makefile.am b/utils/analogy/Makefile.am
index 6ffd0f2..eed1265 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 
+       -lpthread -lrt -lm
 
 cmd_read_SOURCES = cmd_read.c
 cmd_read_LDADD = \


_______________________________________________
Xenomai-git mailing list
Xenomai-git@xenomai.org
http://www.xenomai.org/mailman/listinfo/xenomai-git

Reply via email to