lib/calib.c

   1 /*
   2     lib/calib.c
   3     functions for setting calibration
   4
   5     Copyright (C) 2003 Frank Mori Hess <fmhess@users.sourceforge.net
   6
   7     This library is free software; you can redistribute it and/or
   8     modify it under the terms of the GNU Lesser General Public
   9     License as published by the Free Software Foundation, version 2.1
  10     of the License.
  11
  12     This library is distributed in the hope that it will be useful,
  13     but WITHOUT ANY WARRANTY; without even the implied warranty of
  14     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15     Lesser General Public License for more details.
  16
  17     You should have received a copy of the GNU Lesser General Public
  18     License along with this library; if not, write to the Free Software
  19     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
  20     USA.
  21 */
  22
  23 #define _GNU_SOURCE
  24
  25 #include <assert.h>
  26 #include <math.h>
  27 #include <stdlib.h>
  28 #include <stdio.h>
  29 #include <string.h>
  30 #include "libinternal.h"
  31
  32 static int set_calibration( comedi_t *dev, const comedi_calibration_t *parsed_file,
  33         unsigned int cal_index );
  34
  35 static int check_cal_file( comedi_t *dev, const comedi_calibration_t *parsed_file )
  36 {
  37         if( strcmp( comedi_get_driver_name( dev ), parsed_file->driver_name ) )
  38         {
  39                 COMEDILIB_DEBUG( 3, "driver name does not match '%s' from calibration file\n",
  40                         parsed_file->driver_name );
  41                 return -1;
  42         }
  43
  44         if( strcmp( comedi_get_board_name( dev ), parsed_file->board_name ) )
  45         {
  46                 COMEDILIB_DEBUG( 3, "board name does not match '%s' from calibration file\n",
  47                         parsed_file->board_name );
  48                 return -1;
  49         }
  50
  51         return 0;
  52 }
  53
  54 static inline int valid_channel( const comedi_calibration_t *parsed_file,
  55         unsigned int cal_index, unsigned int channel )
  56 {
  57         int num_channels, i;
  58
  59         num_channels = parsed_file->settings[ cal_index ].num_channels;
  60         if( num_channels == 0 ) return 1;
  61         for( i = 0; i < num_channels; i++ )
  62         {
  63                 if( parsed_file->settings[ cal_index ].channels[ i ] == channel )
  64                         return 1;
  65         }
  66
  67         return 0;
  68 }
  69
  70 static inline int valid_range( const comedi_calibration_t *parsed_file,
  71         unsigned int cal_index, unsigned int range )
  72 {
  73         int num_ranges, i;
  74
  75         num_ranges = parsed_file->settings[ cal_index ].num_ranges;
  76         if( num_ranges == 0 ) return 1;
  77         for( i = 0; i < num_ranges; i++ )
  78         {
  79                 if( parsed_file->settings[ cal_index ].ranges[ i ] == range )
  80                         return 1;
  81         }
  82
  83         return 0;
  84 }
  85
  86 static inline int valid_aref( const comedi_calibration_t *parsed_file,
  87         unsigned int cal_index, unsigned int aref )
  88 {
  89         int num_arefs, i;
  90
  91         num_arefs = parsed_file->settings[ cal_index ].num_arefs;
  92         if( num_arefs == 0 ) return 1;
  93         for( i = 0; i < num_arefs; i++ )
  94         {
  95                 if( parsed_file->settings[ cal_index ].arefs[ i ] == aref )
  96                         return 1;
  97         }
  98
  99         return 0;
 100 }
 101
 102 static int apply_calibration( comedi_t *dev, const comedi_calibration_t *parsed_file,
 103         unsigned int subdev, unsigned int channel, unsigned int range, unsigned int aref )
 104 {
 105         int num_cals, i, retval;
 106         int found_cal = 0;
 107
 108         num_cals = parsed_file->num_settings;
 109
 110         for( i = 0; i < num_cals; i++ )
 111         {
 112                 if( parsed_file->settings[ i ].subdevice != subdev ) continue;
 113                 if( valid_range( parsed_file, i, range ) == 0 ) continue;
 114                 if( valid_channel( parsed_file, i, channel ) == 0 ) continue;
 115                 if( valid_aref( parsed_file, i, aref ) == 0 ) continue;
 116
 117                 retval = set_calibration( dev, parsed_file, i );
 118                 if( retval < 0 ) return retval;
 119                 found_cal = 1;
 120         }
 121         if( found_cal == 0 )
 122         {
 123                 COMEDILIB_DEBUG( 3, "failed to find matching calibration\n" );
 124                 return -1;
 125         }
 126
 127         return 0;
 128 }
 129
 130 static int set_calibration( comedi_t *dev, const comedi_calibration_t *parsed_file,
 131         unsigned int cal_index )
 132 {
 133         int i, retval, num_caldacs;
 134
 135         num_caldacs = parsed_file->settings[ cal_index ].num_caldacs;
 136         COMEDILIB_DEBUG( 4, "num_caldacs %i\n", num_caldacs );
 137
 138         for( i = 0; i < num_caldacs; i++ )
 139         {
 140                 comedi_caldac_t caldac;
 141
 142                 caldac = parsed_file->settings[ cal_index ].caldacs[ i ];
 143                 COMEDILIB_DEBUG( 4, "subdev %i, ch %i, val %i\n", caldac.subdevice,
 144                         caldac.channel,caldac.value);
 145                 retval = comedi_data_write( dev, caldac.subdevice, caldac.channel,
 146                         0, 0, caldac.value );
 147                 if( retval < 0 ) return retval;
 148         }
 149
 150         return 0;
 151 }
 152
 153 EXPORT_ALIAS_DEFAULT(_comedi_apply_parsed_calibration,comedi_apply_parsed_calibration,0.7.20);
 154 int _comedi_apply_parsed_calibration( comedi_t *dev, unsigned int subdev, unsigned int channel,
 155         unsigned int range, unsigned int aref, const comedi_calibration_t *calibration )
 156 {
 157         int retval;
 158
 159         retval = check_cal_file( dev, calibration );
 160         if( retval < 0 ) return retval;
 161
 162         retval = apply_calibration( dev, calibration, subdev, channel, range, aref );
 163         return retval;
 164 }
 165
 166 /* munge characters in board name that will cause problems with file paths */
 167 static void fixup_board_name( char *name )
 168 {
 169         while( ( name = strchr( name, '/' ) ) )
 170         {
 171                 if( name )
 172                 {
 173                         *name = '-';
 174                         name++;
 175                 }
 176         }
 177 }
 178
 179 EXPORT_ALIAS_DEFAULT(_comedi_get_default_calibration_path,comedi_get_default_calibration_path,0.7.20);
 180 char* _comedi_get_default_calibration_path( comedi_t *dev )
 181 {
 182         struct stat file_stats;
 183         char *file_path;
 184         const char *temp;
 185         char *board_name;
 186         const char *driver_name;
 187
 188         if( fstat( comedi_fileno( dev ), &file_stats ) < 0 )
 189         {
 190                 COMEDILIB_DEBUG( 3, "failed to get file stats of comedi device file\n" );
 191                 return NULL;
 192         }
 193
 194         driver_name = comedi_get_driver_name( dev );
 195         if( driver_name == NULL )
 196         {
 197                 return NULL;
 198         }
 199         temp = comedi_get_board_name( dev );
 200         if( temp == NULL )
 201         {
 202                 return NULL;
 203         }
 204         board_name = strdup( temp );
 205
 206         fixup_board_name( board_name );
 207         asprintf( &file_path, LOCALSTATEDIR "/lib/comedi/calibrations/%s_%s_comedi%li",
 208                 driver_name, board_name, ( unsigned long ) minor( file_stats.st_rdev ) );
 209
 210         free( board_name );
 211         return file_path;
 212 }
 213
 214 EXPORT_ALIAS_DEFAULT(_comedi_apply_calibration,comedi_apply_calibration,0.7.20);
 215 int _comedi_apply_calibration( comedi_t *dev, unsigned int subdev, unsigned int channel,
 216         unsigned int range, unsigned int aref, const char *cal_file_path )
 217 {
 218         char file_path[ 1024 ];
 219         int retval;
 220         comedi_calibration_t *parsed_file;
 221
 222         if( cal_file_path )
 223         {
 224                 strncpy( file_path, cal_file_path, sizeof( file_path ) );
 225         }else
 226         {
 227                 char *temp;
 228
 229                 temp = comedi_get_default_calibration_path( dev );
 230                 if( temp == NULL ) return -1;
 231                 strncpy( file_path, temp, sizeof( file_path ) );
 232                 free( temp );
 233         }
 234
 235         parsed_file = comedi_parse_calibration_file( file_path );
 236         if( parsed_file == NULL )
 237         {
 238                 COMEDILIB_DEBUG( 3, "failed to parse calibration file\n" );
 239                 return -1;
 240         }
 241
 242         retval = comedi_apply_parsed_calibration( dev, subdev, channel, range, aref, parsed_file );
 243
 244         comedi_cleanup_calibration( parsed_file );
 245
 246         return retval;
 247 }
 248
 249 EXPORT_ALIAS_DEFAULT(_comedi_get_hardcal_converter, comedi_get_hardcal_converter, 0.8.0);
 250 int _comedi_get_hardcal_converter(
 251         comedi_t *dev, unsigned subdevice, unsigned channel, unsigned range,
 252         enum comedi_conversion_direction direction,
 253         comedi_polynomial_t* polynomial)
 254 {
 255         comedi_range *range_ptr = comedi_get_range(dev, subdevice, channel, range);
 256         if(range_ptr == NULL)
 257         {
 258                 return -1;
 259         }
 260         lsampl_t maxdata = comedi_get_maxdata(dev, subdevice, channel);
 261         if(maxdata == 0)
 262         {
 263                 return -1;
 264         }
 265         polynomial->order = 1;
 266         switch(direction)
 267         {
 268         case COMEDI_TO_PHYSICAL:
 269                 polynomial->expansion_origin = 0.;
 270                 polynomial->coefficients[0] = range_ptr->min;
 271                 polynomial->coefficients[1] = (range_ptr->max - range_ptr->min) / maxdata;
 272                 break;
 273         case COMEDI_FROM_PHYSICAL:
 274                 polynomial->expansion_origin = range_ptr->min;
 275                 polynomial->coefficients[0] = 0.;
 276                 polynomial->coefficients[1] =  maxdata / (range_ptr->max - range_ptr->min);
 277                 break;
 278         }
 279         return 0;
 280 }
 281
 282 EXPORT_ALIAS_DEFAULT(_comedi_get_softcal_converter, comedi_get_softcal_converter, 0.8.0);
 283 int _comedi_get_softcal_converter(
 284         unsigned subdevice, unsigned channel, unsigned range,
 285         enum comedi_conversion_direction direction,
 286         const comedi_calibration_t *calibration, comedi_polynomial_t* polynomial)
 287 {
 288         unsigned i;
 289
 290         for(i = 0; i < calibration->num_settings; ++i)
 291         {
 292                 if(calibration->settings[i].subdevice != subdevice) continue;
 293                 if(valid_channel(calibration, i, channel) == 0) continue;
 294                 if(valid_range(calibration, i, range) == 0) continue;
 295                 switch(direction)
 296                 {
 297                 case COMEDI_TO_PHYSICAL:
 298                         if(calibration->settings[i].soft_calibration.to_phys == NULL)
 299                         {
 300                                 continue;
 301                         }
 302                         *polynomial = *calibration->settings[i].soft_calibration.to_phys;
 303                         break;
 304                 case COMEDI_FROM_PHYSICAL:
 305                         if(calibration->settings[i].soft_calibration.from_phys == NULL)
 306                         {
 307                                 continue;
 308                         }
 309                         *polynomial = *calibration->settings[i].soft_calibration.from_phys;
 310                         break;
 311                 }
 312                 return 0;
 313         }
 314         return -1;
 315 }
 316
 317 static double apply_polynomial(const comedi_polynomial_t *polynomial, double input)
 318 {
 319         double value = 0.;
 320         double term = 1.;
 321         unsigned i;
 322         assert(polynomial->order < COMEDI_MAX_NUM_POLYNOMIAL_COEFFICIENTS);
 323         for(i = 0; i <= polynomial->order; ++i)
 324         {
 325                 value += polynomial->coefficients[i] * term;
 326                 term *= input - polynomial->expansion_origin;
 327         }
 328         return value;
 329 }
 330
 331 EXPORT_ALIAS_DEFAULT(_comedi_to_physical, comedi_to_physical, 0.8.0);
 332 double _comedi_to_physical(lsampl_t data,
 333         const comedi_polynomial_t *conversion_polynomial)
 334 {
 335         return apply_polynomial(conversion_polynomial, data);
 336 }
 337
 338 EXPORT_ALIAS_DEFAULT(_comedi_from_physical, comedi_from_physical, 0.8.0);
 339 lsampl_t _comedi_from_physical(double data,
 340         const comedi_polynomial_t *conversion_polynomial)
 341 {
 342         return nearbyint(apply_polynomial(conversion_polynomial, data));
 343 }