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 <sys/stat.h>
  31 #include "libinternal.h"
  32
  33 static int set_calibration( comedi_t *dev, const comedi_calibration_t *parsed_file,
  34         unsigned int cal_index );
  35
  36 static int check_cal_file( comedi_t *dev, const comedi_calibration_t *parsed_file )
  37 {
  38         if( strcmp( comedi_get_driver_name( dev ), parsed_file->driver_name ) )
  39         {
  40                 COMEDILIB_DEBUG( 3, "driver name does not match '%s' from calibration file\n",
  41                         parsed_file->driver_name );
  42                 return -1;
  43         }
  44
  45         if( strcmp( comedi_get_board_name( dev ), parsed_file->board_name ) )
  46         {
  47                 COMEDILIB_DEBUG( 3, "board name does not match '%s' from calibration file\n",
  48                         parsed_file->board_name );
  49                 return -1;
  50         }
  51
  52         return 0;
  53 }
  54
  55 static inline int valid_channel( const comedi_calibration_t *parsed_file,
  56         unsigned int cal_index, unsigned int channel )
  57 {
  58         int num_channels, i;
  59
  60         num_channels = parsed_file->settings[ cal_index ].num_channels;
  61         if( num_channels == 0 ) return 1;
  62         for( i = 0; i < num_channels; i++ )
  63         {
  64                 if( parsed_file->settings[ cal_index ].channels[ i ] == channel )
  65                         return 1;
  66         }
  67
  68         return 0;
  69 }
  70
  71 static inline int valid_range( const comedi_calibration_t *parsed_file,
  72         unsigned int cal_index, unsigned int range )
  73 {
  74         int num_ranges, i;
  75
  76         num_ranges = parsed_file->settings[ cal_index ].num_ranges;
  77         if( num_ranges == 0 ) return 1;
  78         for( i = 0; i < num_ranges; i++ )
  79         {
  80                 if( parsed_file->settings[ cal_index ].ranges[ i ] == range )
  81                         return 1;
  82         }
  83
  84         return 0;
  85 }
  86
  87 static inline int valid_aref( const comedi_calibration_t *parsed_file,
  88         unsigned int cal_index, unsigned int aref )
  89 {
  90         int num_arefs, i;
  91
  92         num_arefs = parsed_file->settings[ cal_index ].num_arefs;
  93         if( num_arefs == 0 ) return 1;
  94         for( i = 0; i < num_arefs; i++ )
  95         {
  96                 if( parsed_file->settings[ cal_index ].arefs[ i ] == aref )
  97                         return 1;
  98         }
  99
 100         return 0;
 101 }
 102
 103 static int apply_calibration( comedi_t *dev, const comedi_calibration_t *parsed_file,
 104         unsigned int subdev, unsigned int channel, unsigned int range, unsigned int aref )
 105 {
 106         int num_cals, i, retval;
 107         int found_cal = 0;
 108
 109         num_cals = parsed_file->num_settings;
 110
 111         for( i = 0; i < num_cals; i++ )
 112         {
 113                 if( parsed_file->settings[ i ].subdevice != subdev ) continue;
 114                 if( valid_range( parsed_file, i, range ) == 0 ) continue;
 115                 if( valid_channel( parsed_file, i, channel ) == 0 ) continue;
 116                 if( valid_aref( parsed_file, i, aref ) == 0 ) continue;
 117
 118                 retval = set_calibration( dev, parsed_file, i );
 119                 if( retval < 0 ) return retval;
 120                 found_cal = 1;
 121         }
 122         if( found_cal == 0 )
 123         {
 124                 COMEDILIB_DEBUG( 3, "failed to find matching calibration\n" );
 125                 return -1;
 126         }
 127
 128         return 0;
 129 }
 130
 131 static int set_calibration( comedi_t *dev, const comedi_calibration_t *parsed_file,
 132         unsigned int cal_index )
 133 {
 134         int i, retval, num_caldacs;
 135
 136         num_caldacs = parsed_file->settings[ cal_index ].num_caldacs;
 137         COMEDILIB_DEBUG( 4, "num_caldacs %i\n", num_caldacs );
 138
 139         for( i = 0; i < num_caldacs; i++ )
 140         {
 141                 comedi_caldac_t caldac;
 142
 143                 caldac = parsed_file->settings[ cal_index ].caldacs[ i ];
 144                 COMEDILIB_DEBUG( 4, "subdev %i, ch %i, val %i\n", caldac.subdevice,
 145                         caldac.channel,caldac.value);
 146                 retval = comedi_data_write( dev, caldac.subdevice, caldac.channel,
 147                         0, 0, caldac.value );
 148                 if( retval < 0 ) return retval;
 149         }
 150
 151         return 0;
 152 }
 153
 154 EXPORT_ALIAS_DEFAULT(_comedi_apply_parsed_calibration,comedi_apply_parsed_calibration,0.7.20);
 155 int _comedi_apply_parsed_calibration( comedi_t *dev, unsigned int subdev, unsigned int channel,
 156         unsigned int range, unsigned int aref, const comedi_calibration_t *calibration )
 157 {
 158         int retval;
 159
 160         if(!valid_dev(dev)) return -1;
 161         retval = check_cal_file( dev, calibration );
 162         if( retval < 0 ) return retval;
 163
 164         retval = apply_calibration( dev, calibration, subdev, channel, range, aref );
 165         return retval;
 166 }
 167
 168 /* munge characters in board name that will cause problems with file paths */
 169 static void fixup_board_name( char *name )
 170 {
 171         while( ( name = strchr( name, '/' ) ) )
 172         {
 173                 if( name )
 174                 {
 175                         *name = '-';
 176                         name++;
 177                 }
 178         }
 179 }
 180
 181 EXPORT_ALIAS_DEFAULT(_comedi_get_default_calibration_path,comedi_get_default_calibration_path,0.7.20);
 182 char* _comedi_get_default_calibration_path( comedi_t *dev )
 183 {
 184         struct stat file_stats;
 185         char *file_path;
 186         const char *temp;
 187         char *board_name;
 188         const char *driver_name;
 189         int err;
 190
 191         if(!valid_dev(dev)) return NULL;
 192         if( fstat( comedi_fileno( dev ), &file_stats ) < 0 )
 193         {
 194                 COMEDILIB_DEBUG( 3, "failed to get file stats of comedi device file\n" );
 195                 return NULL;
 196         }
 197
 198         driver_name = comedi_get_driver_name( dev );
 199         if( driver_name == NULL )
 200         {
 201                 return NULL;
 202         }
 203         temp = comedi_get_board_name( dev );
 204         if( temp == NULL )
 205         {
 206                 return NULL;
 207         }
 208         board_name = strdup( temp );
 209
 210         fixup_board_name( board_name );
 211         err = asprintf( &file_path, LOCALSTATEDIR "/lib/comedi/calibrations/%s_%s_comedi%li",
 212                 driver_name, board_name, ( unsigned long ) minor( file_stats.st_rdev ) );
 213
 214         free( board_name );
 215         if( err < 0 )
 216         {
 217                 return NULL;
 218         }
 219         return file_path;
 220 }
 221
 222 EXPORT_ALIAS_DEFAULT(_comedi_apply_calibration,comedi_apply_calibration,0.7.20);
 223 int _comedi_apply_calibration( comedi_t *dev, unsigned int subdev, unsigned int channel,
 224         unsigned int range, unsigned int aref, const char *cal_file_path )
 225 {
 226         char file_path[ 1024 ];
 227         int retval;
 228         comedi_calibration_t *parsed_file;
 229
 230         if( cal_file_path )
 231         {
 232                 strncpy( file_path, cal_file_path, sizeof( file_path ) );
 233         }else
 234         {
 235                 char *temp;
 236
 237                 temp = comedi_get_default_calibration_path( dev );
 238                 if( temp == NULL ) return -1;
 239                 strncpy( file_path, temp, sizeof( file_path ) );
 240                 free( temp );
 241         }
 242
 243         parsed_file = comedi_parse_calibration_file( file_path );
 244         if( parsed_file == NULL )
 245         {
 246                 COMEDILIB_DEBUG( 3, "failed to parse calibration file\n" );
 247                 return -1;
 248         }
 249
 250         retval = comedi_apply_parsed_calibration( dev, subdev, channel, range, aref, parsed_file );
 251
 252         comedi_cleanup_calibration( parsed_file );
 253
 254         return retval;
 255 }
 256
 257 EXPORT_ALIAS_DEFAULT(_comedi_get_hardcal_converter, comedi_get_hardcal_converter, 0.8.0);
 258 int _comedi_get_hardcal_converter(
 259         comedi_t *dev, unsigned subdevice, unsigned channel, unsigned range,
 260         enum comedi_conversion_direction direction,
 261         comedi_polynomial_t* polynomial)
 262 {
 263         comedi_range *range_ptr = comedi_get_range(dev, subdevice, channel, range);
 264         lsampl_t maxdata;
 265
 266         if(range_ptr == NULL)
 267         {
 268                 return -1;
 269         }
 270         maxdata = comedi_get_maxdata(dev, subdevice, channel);
 271         if(maxdata == 0)
 272         {
 273                 return -1;
 274         }
 275         polynomial->order = 1;
 276         switch(direction)
 277         {
 278         case COMEDI_TO_PHYSICAL:
 279                 polynomial->expansion_origin = 0.;
 280                 polynomial->coefficients[0] = range_ptr->min;
 281                 polynomial->coefficients[1] = (range_ptr->max - range_ptr->min) / maxdata;
 282                 break;
 283         case COMEDI_FROM_PHYSICAL:
 284                 polynomial->expansion_origin = range_ptr->min;
 285                 polynomial->coefficients[0] = 0.;
 286                 polynomial->coefficients[1] =  maxdata / (range_ptr->max - range_ptr->min);
 287                 break;
 288         }
 289         return 0;
 290 }
 291
 292 EXPORT_ALIAS_DEFAULT(_comedi_get_softcal_converter, comedi_get_softcal_converter, 0.8.0);
 293 int _comedi_get_softcal_converter(
 294         unsigned subdevice, unsigned channel, unsigned range,
 295         enum comedi_conversion_direction direction,
 296         const comedi_calibration_t *calibration, comedi_polynomial_t* polynomial)
 297 {
 298         unsigned i;
 299
 300         for(i = 0; i < calibration->num_settings; ++i)
 301         {
 302                 if(calibration->settings[i].subdevice != subdevice) continue;
 303                 if(valid_channel(calibration, i, channel) == 0) continue;
 304                 if(valid_range(calibration, i, range) == 0) continue;
 305                 switch(direction)
 306                 {
 307                 case COMEDI_TO_PHYSICAL:
 308                         if(calibration->settings[i].soft_calibration.to_phys == NULL)
 309                         {
 310                                 continue;
 311                         }
 312                         *polynomial = *calibration->settings[i].soft_calibration.to_phys;
 313                         break;
 314                 case COMEDI_FROM_PHYSICAL:
 315                         if(calibration->settings[i].soft_calibration.from_phys == NULL)
 316                         {
 317                                 continue;
 318                         }
 319                         *polynomial = *calibration->settings[i].soft_calibration.from_phys;
 320                         break;
 321                 }
 322                 return 0;
 323         }
 324         return -1;
 325 }
 326
 327 static double apply_polynomial(const comedi_polynomial_t *polynomial, double input)
 328 {
 329         double value = 0.;
 330         double term = 1.;
 331         unsigned i;
 332         assert(polynomial->order < COMEDI_MAX_NUM_POLYNOMIAL_COEFFICIENTS);
 333         for(i = 0; i <= polynomial->order; ++i)
 334         {
 335                 value += polynomial->coefficients[i] * term;
 336                 term *= input - polynomial->expansion_origin;
 337         }
 338         return value;
 339 }
 340
 341 EXPORT_ALIAS_DEFAULT(_comedi_to_physical, comedi_to_physical, 0.8.0);
 342 double _comedi_to_physical(lsampl_t data,
 343         const comedi_polynomial_t *conversion_polynomial)
 344 {
 345         return apply_polynomial(conversion_polynomial, data);
 346 }
 347
 348 EXPORT_ALIAS_DEFAULT(_comedi_from_physical, comedi_from_physical, 0.8.0);
 349 lsampl_t _comedi_from_physical(double data,
 350         const comedi_polynomial_t *conversion_polynomial)
 351 {
 352         return nearbyint(apply_polynomial(conversion_polynomial, data));
 353 }