Module: xenomai-3
Branch: master
Commit: 029424c470f6697dca77b9e693a8669d66474d54
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=029424c470f6697dca77b9e693a8669d66474d54
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
