OrocosComponentLibrary: EncoderSSIapci1710.cpp Source File

00001 /***************************************************************************
00002   tag: Peter Soetens  Mon May 10 19:10:32 CEST 2004  EncoderSSIapci1710.cxx
00003 
00004                         EncoderSSIapci1710.cxx -  description
00005                            -------------------
00006     begin                : Mon May 10 2004
00007     copyright            : (C) 2004 Peter Soetens
00008     email                : peter.soetens@mech.kuleuven.ac.be
00009 
00010  ***************************************************************************
00011  *   This library is free software; you can redistribute it and/or         *
00012  *   modify it under the terms of the GNU Lesser General Public            *
00013  *   License as published by the Free Software Foundation; either          *
00014  *   version 2.1 of the License, or (at your option) any later version.    *
00015  *                                                                         *
00016  *   This library is distributed in the hope that it will be useful,       *
00017  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00018  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00019  *   Lesser General Public License for more details.                       *
00020  *                                                                         *
00021  *   You should have received a copy of the GNU Lesser General Public      *
00022  *   License along with this library; if not, write to the Free Software   *
00023  *   Foundation, Inc., 59 Temple Place,                                    *
00024  *   Suite 330, Boston, MA  02111-1307  USA                                *
00025  *                                                                         *
00026  ***************************************************************************/
00027 // Copyright (C) 2003 Johan Rutgeerts <johan.rutgeerts@mech.kuleuven.ac.be>
00028 //
00029 // This program is free software; you can redistribute it and/or modify
00030 // it under the terms of the GNU General Public License as published by
00031 // the Free Software Foundation; either version 2 of the License, or
00032 // (at your option) any later version.
00033 //
00034 // This program is distributed in the hope that it will be useful,
00035 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00036 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00037 // GNU General Public License for more details.
00038 //
00039 // You should have received a copy of the GNU General Public License
00040 // along with this program; if not, write to the Free Software
00041 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00042 //
00043 
00044 
00045 #include <rtt/RTT.hpp>
00046 #include <rtt/os/threads.hpp>
00047 #include "apci-config.h"
00048 
00049 // include lib first, if not present, include KM headers.
00050 #if defined(OROPKG_OS_LXRT)
00051 #include "../drivers/lxrt/apci_lxrt.h"
00052 #endif
00053 
00054 #include "EncoderSSIapci1710.hpp"
00055 
00056 #ifdef OROPKG_CORELIB_TIMING
00057 #include <rtt/TimeService.hpp>
00058 #endif
00059 #include <rtt/Logger.hpp>
00060 
00061 namespace RTT
00062 {
00063 
00064 
00065 
00066 // --------------------------
00067 // Constructor for 1 module
00068 // --------------------------
00069 EncoderSSI_apci1710_board::EncoderSSI_apci1710_board( unsigned int mNr1 )
00070   : PeriodicActivity(RTT::OS::HighestPriority, 1./ORONUM_DEVICE_DRIVERS_APCI1710_SSI_UPDATE )
00071   , nr_of_modules(1), moduleNr1( mNr1 )
00072 {
00073 
00074     buffer1 = new unsigned int[3];
00075     buffer2 = new unsigned int[3];
00076     readbuffer  = buffer1;
00077     writebuffer = buffer2;
00078 
00079     log() << "(EncoderSSI_apci1710) Creating EncoderSSI with module:" << moduleNr1<<", ssi-profile: "<< ORONUM_DEVICE_DRIVERS_APCI1710_SSI_PROFILE
00080           <<", positionbits: "<< ORONUM_DEVICE_DRIVERS_APCI1710_SSI_POSITION_BITS<<", turnbits: "<< ORONUM_DEVICE_DRIVERS_APCI1710_SSI_TURN_BITS
00081       <<", countingmode: ,"<< COUNTINGMODE_BINARY<<" frequency: "<<ORONUM_DEVICE_DRIVERS_APCI1710_SSI_FREQ << endlog(Info);
00082     dev = apci1710_get_device();
00083     int res = 0;
00084     if ( 0 != (res = apci1710_ssi_init( dev, moduleNr1,
00085                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_PROFILE,
00086                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_POSITION_BITS,
00087                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_TURN_BITS,
00088                                         COUNTINGMODE_BINARY,
00089                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_FREQ ) ) )      //in Hz (25kHz is ok)
00090 
00091     log() << "\n(EncoderSSI_apci1710) Error "<< res << " : Module " << moduleNr1 << " is not ready for SSI\n" << endlog(Error);
00092 
00093 #ifdef OROPKG_CORELIB_TIMING
00094     // init one buffer, display some statistics
00095     TimeService::ticks t=TimeService::Instance()->getTicks();
00096     readCard( readbuffer );
00097     t = TimeService::Instance()->ticksSince(t);
00098     TimeService::nsecs n = TimeService::ticks2nsecs(t);
00099     log() << "(EncoderSSI_apci1710) Reading SSI module takes about " << long(n) << " nanoseconds" << endlog(Info);
00100 #endif
00101 
00102     this->start();
00103 }
00104 
00105 
00106 
00107 
00108 
00109 // --------------------------
00110 // Constructor for 2 modules
00111 // --------------------------
00112 EncoderSSI_apci1710_board::EncoderSSI_apci1710_board( unsigned int mNr1, unsigned int mNr2 )
00113   : PeriodicActivity( RTT::OS::HighestPriority, 1./ORONUM_DEVICE_DRIVERS_APCI1710_SSI_UPDATE )
00114   , nr_of_modules(2), moduleNr1( mNr1 ), moduleNr2( mNr2 )
00115 {
00116 
00117     buffer1 = new unsigned int[6];
00118     buffer2 = new unsigned int[6];
00119     readbuffer  = buffer1;
00120     writebuffer = buffer2;
00121 
00122     log() << "(EncoderSSI_apci1710) Creating EncoderSSI..." << endlog(Info);
00123     dev = apci1710_get_device();
00124     int res = 0;
00125     if ( 0 != (res = apci1710_ssi_init( dev, moduleNr1,
00126                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_PROFILE,
00127                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_POSITION_BITS,
00128                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_TURN_BITS,
00129                                         COUNTINGMODE_BINARY,
00130                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_FREQ ) ) )      //in Hz (25kHz is ok)
00131         log() << "\n(EncoderSSI_apci1710) Error "<< res << " : Module " << moduleNr1 << " is not ready for SSI\n" << endlog(Error);
00132 
00133     if ( 0 != (res = apci1710_ssi_init( dev, moduleNr2,
00134                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_PROFILE,
00135                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_POSITION_BITS,
00136                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_TURN_BITS,
00137                                         COUNTINGMODE_BINARY,
00138                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_FREQ ) ) )      //in Hz (25kHz is ok)
00139         log() << "\n(EncoderSSI_apci1710) Error "<< res << " : Module " << moduleNr2 << " is not ready for SSI\n" << endlog(Error);
00140 
00141 
00142 #ifdef OROPKG_CORELIB_TIMING
00143     // init one buffer, display some statistics
00144     TimeService::ticks t=TimeService::Instance()->getTicks();
00145     readCard( readbuffer );
00146     t = TimeService::Instance()->ticksSince(t);
00147     TimeService::nsecs n = TimeService::ticks2nsecs(t);
00148     log() << "(EncoderSSI_apci1710) Reading SSI modules takes about " << long(n) << " nanoseconds" << endlog(Info);
00149 #endif
00150 
00151     this->start();
00152 }
00153 
00154 
00155 
00156 
00157 
00158 // --------------------------
00159 // Constructor for 3 modules
00160 // --------------------------
00161 EncoderSSI_apci1710_board::EncoderSSI_apci1710_board( unsigned int mNr1, unsigned int mNr2, unsigned int mNr3 )
00162   : PeriodicActivity( RTT::OS::HighestPriority, 1./ORONUM_DEVICE_DRIVERS_APCI1710_SSI_UPDATE )
00163   , nr_of_modules(3), moduleNr1( mNr1 ), moduleNr2( mNr2 ), moduleNr3( mNr3 )
00164 {
00165 
00166     buffer1 = new unsigned int[9];
00167     buffer2 = new unsigned int[9];
00168     readbuffer  = buffer1;
00169     writebuffer = buffer2;
00170 
00171     log() << "(EncoderSSI_apci1710) Creating EncoderSSI..." << endlog(Info);
00172     dev = apci1710_get_device();
00173     int res = 0;
00174     if ( 0 != (res = apci1710_ssi_init( dev, moduleNr1,
00175                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_PROFILE,
00176                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_POSITION_BITS,
00177                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_TURN_BITS,
00178                                         COUNTINGMODE_BINARY,
00179                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_FREQ ) ) )      //in Hz (25kHz is ok)
00180         log() << "\n(EncoderSSI_apci1710) Error "<< res << " : Module " << moduleNr1 << " is not ready for SSI\n" << endlog(Error);
00181 
00182     if ( 0 != (res = apci1710_ssi_init( dev, moduleNr2,
00183                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_PROFILE,
00184                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_POSITION_BITS,
00185                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_TURN_BITS,
00186                                         COUNTINGMODE_BINARY,
00187                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_FREQ ) ) )      //in Hz (25kHz is ok)
00188         log() << "\n(EncoderSSI_apci1710) Error "<< res << " : Module " << moduleNr2 << " is not ready for SSI\n" << endlog(Error);
00189 
00190     if ( 0 != (res = apci1710_ssi_init( dev, moduleNr3,
00191                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_PROFILE2,
00192                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_POSITION_BITS,
00193                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_TURN_BITS2,
00194                                         COUNTINGMODE_BINARY,
00195                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_FREQ ) ) )      //in Hz (25kHz is ok)
00196         log() << "\n(EncoderSSI_apci1710) Error "<< res << " : Module " << moduleNr3 << " is not ready for SSI\n" << endlog(Error);
00197 
00198 
00199 #ifdef OROPKG_CORELIB_TIMING
00200     // init one buffer, display some statistics
00201     TimeService::ticks t=TimeService::Instance()->getTicks();
00202     readCard( readbuffer );
00203     t = TimeService::Instance()->ticksSince(t);
00204     TimeService::nsecs n = TimeService::ticks2nsecs(t);
00205     log() << "(EncoderSSI_apci1710) Reading SSI modules takes about " << long(n) << " nanoseconds" << endlog(Info);
00206 #endif
00207 
00208     this->start();
00209 }
00210 
00211 
00212 
00213 
00214 
00215 // --------------------------
00216 // Constructor for 4 modules
00217 // --------------------------
00218 EncoderSSI_apci1710_board::EncoderSSI_apci1710_board( unsigned int mNr1, unsigned int mNr2, unsigned int mNr3, unsigned int mNr4 )
00219   : PeriodicActivity(RTT::OS::HighestPriority, 1./ORONUM_DEVICE_DRIVERS_APCI1710_SSI_UPDATE )
00220   , nr_of_modules(3), moduleNr1( mNr1 ), moduleNr2( mNr2 ), moduleNr3( mNr3 ), moduleNr4( mNr4 )
00221 {
00222 
00223     buffer1 = new unsigned int[12];
00224     buffer2 = new unsigned int[12];
00225     readbuffer  = buffer1;
00226     writebuffer = buffer2;
00227 
00228     log() << "(EncoderSSI_apci1710) Creating EncoderSSI..." << endlog(Info);
00229     dev = apci1710_get_device();
00230     int res = 0;
00231     if ( 0 != (res = apci1710_ssi_init( dev, moduleNr1,
00232                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_PROFILE,
00233                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_POSITION_BITS,
00234                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_TURN_BITS,
00235                                         COUNTINGMODE_BINARY,
00236                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_FREQ ) ) )      //in Hz (25kHz is ok)
00237         log() << "\n(EncoderSSI_apci1710) Error "<< res << " : Module " << moduleNr1 << " is not ready for SSI\n" << endlog(Error);
00238 
00239     if ( 0 != (res = apci1710_ssi_init( dev, moduleNr2,
00240                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_PROFILE,
00241                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_POSITION_BITS,
00242                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_TURN_BITS,
00243                                         COUNTINGMODE_BINARY,
00244                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_FREQ ) ) )      //in Hz (25kHz is ok)
00245         log() << "\n(EncoderSSI_apci1710) Error "<< res << " : Module " << moduleNr2 << " is not ready for SSI\n" << endlog(Error);
00246 
00247     if ( 0 != (res = apci1710_ssi_init( dev, moduleNr3,
00248                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_PROFILE,
00249                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_POSITION_BITS,
00250                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_TURN_BITS,
00251                                         COUNTINGMODE_BINARY,
00252                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_FREQ ) ) )      //in Hz (25kHz is ok)
00253         log() << "\n(EncoderSSI_apci1710) Error "<< res << " : Module " << moduleNr3 << " is not ready for SSI\n" << endlog(Error);
00254 
00255     if ( 0 != (res = apci1710_ssi_init( dev, moduleNr4,
00256                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_PROFILE,
00257                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_POSITION_BITS,
00258                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_TURN_BITS,
00259                                         COUNTINGMODE_BINARY,
00260                                         ORONUM_DEVICE_DRIVERS_APCI1710_SSI_FREQ ) ) )      //in Hz (25kHz is ok)
00261         log() << "\n(EncoderSSI_apci1710) Error "<< res << " : Module " << moduleNr4 << " is not ready for SSI\n" << endlog(Error);
00262 
00263 
00264 #ifdef OROPKG_CORELIB_TIMING
00265     // init one buffer, display some statistics
00266     TimeService::ticks t=TimeService::Instance()->getTicks();
00267     readCard( readbuffer );
00268     t = TimeService::Instance()->ticksSince(t);
00269     TimeService::nsecs n = TimeService::ticks2nsecs(t);
00270     log() << "(EncoderSSI_apci1710) Reading SSI modules takes about " << long(n) << " nanoseconds" << endlog(Info);
00271 #endif
00272 
00273     this->start();
00274 }
00275 
00276 
00277 
00278 
00279 
00280 EncoderSSI_apci1710_board::~EncoderSSI_apci1710_board()
00281 {
00282     this->stop();
00283     delete[] buffer1;
00284     delete[] buffer2;
00285 }
00286 
00287 
00288 
00289 
00290 unsigned int EncoderSSI_apci1710_board::read( unsigned int encNr )
00291 {
00292 #ifdef DEBUG
00293     if ( encNr < 1 || encNr > nr_of_modules * 3 ) log() << "(EncoderSSI_apci1710) ERROR: encNr < 1 or encNr > " << nr_of_modules * 3 << "!" << endlog(Error);
00294 #endif
00295 
00296     OS::MutexLock Locker( readLock );
00297     return readbuffer[ encNr - 1 ];
00298 }
00299 
00300 
00301 
00302 
00303 void EncoderSSI_apci1710_board::readCard( unsigned int* buffer )
00304 {
00305     switch(nr_of_modules)
00306     {
00307         case 1:
00308                 apci1710_ssi_refresh(dev, moduleNr1);                         // refresh module1
00309                 apci1710_ssi_read_latest_all( dev, moduleNr1, buffer );       // read out module1
00310 
00311                 // Somehow, there are 'dirty' bits above the 21 bits, so we clean them
00312                 for ( unsigned int i = 0; i < 3; i++) buffer[i] = buffer[i] & 0x001fffff; // only use 21 bits;
00313                 break;
00314 
00315         case 2:
00316                 apci1710_ssi_refresh(dev, moduleNr1);                         // refresh module1
00317                 apci1710_ssi_refresh(dev, moduleNr2);                         // refresh module2
00318                 apci1710_ssi_read_latest_all( dev, moduleNr1, buffer );       // read out module1
00319                 apci1710_ssi_read_latest_all( dev, moduleNr2, buffer + 3 );   // read out module2
00320 
00321                 // Somehow, there are 'dirty' bits above the 21 bits, so we clean them
00322                 for ( unsigned int i = 0; i < 6; i++) buffer[i] = buffer[i] & 0x001fffff; // only use 21 bits;
00323                 break;
00324 
00325         case 3:
00326                 apci1710_ssi_refresh(dev, moduleNr1);                         // refresh module1
00327                 apci1710_ssi_refresh(dev, moduleNr2);                         // refresh module2
00328                 apci1710_ssi_refresh(dev, moduleNr3);                         // refresh module3
00329                 apci1710_ssi_read_latest_all( dev, moduleNr1, buffer );       // read out module1
00330                 apci1710_ssi_read_latest_all( dev, moduleNr2, buffer + 3 );   // read out module2
00331                 apci1710_ssi_read_latest_all( dev, moduleNr3, buffer + 6 );   // read out module3
00332 
00333                 // Somehow, there are 'dirty' bits above the 21 bits, so we clean them
00334                 for ( unsigned int i = 0; i < 9; i++) buffer[i] = buffer[i] & 0x001fffff; // only use 21 bits;
00335                 break;
00336 
00337         case 4:
00338                 apci1710_ssi_refresh(dev, moduleNr1);                         // refresh module1
00339                 apci1710_ssi_refresh(dev, moduleNr2);                         // refresh module2
00340                 apci1710_ssi_refresh(dev, moduleNr3);                         // refresh module3
00341                 apci1710_ssi_refresh(dev, moduleNr4);                         // refresh module4
00342                 apci1710_ssi_read_latest_all( dev, moduleNr1, buffer );       // read out module1
00343                 apci1710_ssi_read_latest_all( dev, moduleNr2, buffer + 3 );   // read out module2
00344                 apci1710_ssi_read_latest_all( dev, moduleNr3, buffer + 6 );   // read out module3
00345                 apci1710_ssi_read_latest_all( dev, moduleNr4, buffer + 9 );   // read out module4
00346 
00347                 // Somehow, there are 'dirty' bits above the 21 bits, so we clean them
00348                 for ( unsigned int i = 0; i < 12; i++) buffer[i] = buffer[i] & 0x001fffff; // only use 21 bits;
00349                 break;
00350     }
00351 }
00352 
00353 
00354 
00355 
00356 void EncoderSSI_apci1710_board::step()
00357 {
00358     readCard( writebuffer );
00359     switchBuffers();
00360 }
00361 
00362 
00363 
00364 
00365 void EncoderSSI_apci1710_board::switchBuffers()
00366 {
00367     // this is called *after* each refresh, so we know that it is safe
00368     // use spinlocks or implement an 'atomic' swing(b1,b2,p1,p2) function !!!
00369 
00370     OS::MutexLock locker( readLock );
00371     if ( readbuffer == buffer1 )
00372         {
00373             readbuffer = buffer2;
00374             writebuffer = buffer1;
00375         }
00376         else
00377         {
00378             readbuffer = buffer1;
00379             writebuffer = buffer2;
00380         }
00381 }
00382 
00383 
00384 
00385 }; // Namespace RTT