Subversion Repositories f9daq

Rev

Blame | Last modification | View Log | RSS feed

  1. #include        <math.h>
  2. #include        <stdio.h>
  3. #include        <string.h>
  4. #include        <fcntl.h>      /* open */
  5. #include        <unistd.h>     /* exit */
  6. #include        <sys/ioctl.h>  /* ioctl */
  7. #include        "libusmc.h"
  8. #include        "usmctypes.h"
  9.  
  10.  
  11. #define         USB_ERROR(errCode)              (errCode|0x00000000L)
  12. // Linux kernel errors or'ed with this value
  13. // to distinguish them from usb errors.
  14. #define         LINKRN_ERROR(errCode)   (errCode|0x10000000L)
  15. #define         HIBYTE(w)                       ((w&0xff00)>>8)
  16. #define         LOBYTE(w)                       (w&0x00ff)
  17. #define         HIWORD(dw)                      ((dw&0xffff0000)>>16)
  18. #define         LOWORD(dw)                      (dw&0x0000ffff)
  19. #define         PACK_WORD(w)                    (HIBYTE(w)|(LOBYTE(w)<<8))
  20. //#define               PACK_WORD(w)                    (LOBYTE(w)|(HIBYTE(w)<<8))
  21. #define         PACK_DWORD(w)                   (HIBYTE(HIWORD(w))| \
  22.                                                                          (LOBYTE(HIWORD(w))<<8)| \
  23.                                                                          (HIBYTE(LOWORD(w))<<16)| \
  24.                                                                          (LOBYTE(LOWORD(w))<<24))
  25.  
  26.  
  27. // DLL data:
  28. BOOL                 g_IsInitialized = FALSE;
  29. USMC_Devices         g_devices;
  30. USMC_Mode            g_deviceMode            [32];
  31. USMC_Parameters      g_deviceParameters      [32];
  32. USMC_StartParameters g_deviceStartParameters [32];
  33. DWORD                g_devicesVersions [32];
  34. char                 g_lastErrDesc     [256];   // Null-terminated ASCII error description string.
  35.  
  36.  
  37. // Error string descriptions:
  38. static char * errDesc [] = {
  39.         "0x???????? ( Unknown )",
  40.         "0x00000000 ( Success )",
  41. /*      "0x00000001 ( USB CRC )",
  42.         "0x00000002 ( USB Bit stuffing )",
  43.         "0x00000003 ( USB Data toggle mismatch )",
  44.  
  45.         "0x00000004 ( USB Stall )",
  46.         "0x00000005 ( USB Device not responding )",
  47.         "0x00000006 ( USB PID check failure )",
  48.         "0x00000007 ( USB Unexpected PID )",
  49.         "0x00000008 ( USB Data overrun )",
  50.  
  51.         "0x00000009 ( USB Data underrun )",
  52.         "0x0000000C ( USB Buffer overrun )",
  53.         "0x0000000D ( USB Buffer underrun )",
  54.         "0x0000000E ( USB Not accessed )",
  55.         "0x0000000F ( USB Not accessed alt )",
  56.  
  57.         "0x00000100 ( USB Isochronous )",
  58.         "0x00000101 ( USB Canceled )",
  59.         "0x00000103 ( USB Not complete )",
  60.         "0x00000104 ( USB Client buffer )",*/
  61.         "0x14141414 ( USMC Not initialized )",
  62.         //"0x00000006 ( USMC Reffered Device not connected )",
  63.         //"0x00000012 ( DeviceManager error - driver function called with invalid arguments )",
  64.         "0x14141415 ( USMC No devices connected )"
  65. };
  66.  
  67.  
  68. // Internal-use functions prototypes:
  69. DWORD  SetLastErr  ( const char * str, DWORD dwErrCode );
  70. char * ErrCode2Str ( DWORD dwErrCode );
  71. int    clamp       ( int val, int min, int max );
  72. float  clampf      ( float val, float min, float max );
  73. DWORD  IOCTRL       ( DWORD Device,
  74.                                          DWORD dwCode,
  75.                                          PVOID pInBuf,
  76.                                          DWORD dwInBufSize,
  77.                                          PVOID pOutBuf,
  78.                                          DWORD dwOutBufSize );
  79.  
  80.  
  81.  
  82.  
  83. //      Function: SetLastErr ()
  84. //      Argument list:
  85. //              const char * str √ in √ Null-terminated ASCII error description.
  86. //      DWORD dwErrCode - in - optional - error code supplied with description.
  87. //      Return value:
  88. //              DWORD - modified error code.
  89. //      Remarks:
  90. //              The SetLastErr () sets description of last occured error.
  91. DWORD SetLastErr ( const char * str, DWORD dwErrCode )
  92. {
  93.         if ( dwErrCode == -USMC_ERROR_REF_DEV_DISCONNECTED )
  94.                 sprintf ( ( char * ) g_lastErrDesc, "Error: %s. Code: %s", str, "Reffered device disconnected" );
  95.         else if ( dwErrCode != 1 )
  96.                 sprintf ( ( char * ) g_lastErrDesc, "Error: %s. Code: %d", str, ( signed int ) dwErrCode );
  97.  
  98.         //if ( dwErrCode != 1 )
  99.         //      sprintf ( ( char * ) g_lastErrDesc, "Error: %s. Code: %s", str, ErrCode2Str ( dwErrCode ) );
  100.         //else
  101.         //      sprintf ( ( char * ) g_lastErrDesc, "Error: %s.", str );
  102.  
  103.  
  104.         return  dwErrCode;
  105. }
  106.  
  107.  
  108.  
  109.  
  110. //      Function: ErrCode2Str ()
  111. //      Argument list:
  112. //              DWORD dwErrCode √ in √ error code returned by any operation.
  113. //      Return value:
  114. //      char * - string equivalent for specified numerical error code.
  115. //      Remarks:
  116. //      There is no need to free error string obtained with this function:
  117. //      return value points to static dll variable.
  118. char * ErrCode2Str ( DWORD dwErrCode )
  119. {
  120.         switch ( dwErrCode )
  121.         {
  122. /*      case USB_NO_ERROR:
  123.                 return  errDesc [1]; break;
  124.         case USB_CRC_ERROR:
  125.                 return  errDesc [2]; break;
  126.         case USB_BIT_STUFFING_ERROR:
  127.                 return  errDesc [3]; break;
  128.         case USB_DATA_TOGGLE_MISMATCH_ERROR:
  129.                 return  errDesc [4]; break;
  130.         case USB_STALL_ERROR:
  131.                 return  errDesc [5]; break;
  132.         case USB_DEVICE_NOT_RESPONDING_ERROR:
  133.                 return  errDesc [6]; break;
  134.         case USB_PID_CHECK_FAILURE_ERROR:
  135.                 return  errDesc [7]; break;
  136.         case USB_UNEXPECTED_PID_ERROR:
  137.                 return  errDesc [8]; break;
  138.         case USB_DATA_OVERRUN_ERROR:
  139.                 return  errDesc [9]; break;
  140.         case USB_DATA_UNDERRUN_ERROR:
  141.                 return  errDesc [10]; break;
  142.         case USB_BUFFER_OVERRUN_ERROR:
  143.                 return  errDesc [11]; break;
  144.         case USB_BUFFER_UNDERRUN_ERROR:
  145.                 return  errDesc [12]; break;
  146.         case USB_NOT_ACCESSED_ERROR:
  147.                 return  errDesc [13]; break;
  148.         case USB_NOT_ACCESSED_ALT:
  149.                 return  errDesc [14]; break;
  150.         case USB_ISOCH_ERROR:
  151.                 return  errDesc [15]; break;
  152.         case USB_CANCELED_ERROR:
  153.                 return  errDesc [16]; break;
  154.         case USB_NOT_COMPLETE_ERROR:
  155.                 return  errDesc [17]; break;
  156.         case USB_CLIENT_BUFFER_ERROR:
  157.                 return  errDesc [18]; break;
  158.         case USMC_ERROR_NOT_INITIALIZED:
  159.                 return  errDesc [19]; break;
  160.         case USMC_ERROR_REF_DEV_DISCONNECTED:
  161.                 return  errDesc [20]; break;
  162.         case LINKRN_ERROR ( ERROR_BAD_ARGUMENTS ):
  163.                 return  errDesc [21]; break;
  164.         case USMC_ERROR_NO_DEVICES_CONNECTED:
  165.                 return  errDesc [22]; break;*/
  166.         default:
  167.                 return  errDesc [0]; break;
  168.         }
  169. }
  170.  
  171.  
  172.  
  173.  
  174. int clamp ( int val, int min, int max )
  175. {
  176.         return  val > max ? max : ( val < min ? min : val );
  177. }
  178.  
  179.  
  180.  
  181.  
  182. float clampf ( float val, float min, float max )
  183. {
  184.         return  val > max ? max : ( val < min ? min : val );
  185. }
  186.  
  187.  
  188.  
  189. //      Function: InitDefaultValues ()
  190. //              Initializes structures with default values.
  191. //      Return Value:
  192. //              The InitDefaultValues function has no return values.
  193. void InitDefaultValues ()
  194. {
  195.         int i;
  196.  
  197.         for ( i = 0 ; i < 32 ; i++ )
  198.         {
  199.                 memset ( &g_deviceMode [i], 0, sizeof ( USMC_Mode ) );
  200.                 memset ( &g_deviceParameters [i], 0, sizeof ( USMC_Parameters ) );
  201.                 memset ( &g_deviceStartParameters [i], 0, sizeof ( USMC_StartParameters ) );
  202.  
  203.                 // USMC_Mode defaults:
  204.                 g_deviceMode [i].PReg      = TRUE;
  205.                 g_deviceMode [i].Tr1En     = TRUE;
  206.                 g_deviceMode [i].Tr2En     = TRUE;
  207.                 g_deviceMode [i].RotTrOp   = TRUE;
  208.                 g_deviceMode [i].SyncOUTEn = TRUE;
  209.                 g_deviceMode [i].SyncINOp  = TRUE;
  210.                 g_deviceMode [i].SyncCount = 4;
  211.  
  212.                 // USMC_Parameters defaults:
  213.                 g_deviceParameters [i].MaxTemp    = 70.0f;
  214.                 g_deviceParameters [i].AccelT     = 200.0f;
  215.                 g_deviceParameters [i].DecelT     = 200.0f;
  216.                 g_deviceParameters [i].BTimeout1  = 500.0f;
  217.                 g_deviceParameters [i].BTimeout2  = 500.0f;
  218.                 g_deviceParameters [i].BTimeout3  = 500.0f;
  219.                 g_deviceParameters [i].BTimeout4  = 500.0f;
  220.                 g_deviceParameters [i].BTO1P      = 200.0f;
  221.                 g_deviceParameters [i].BTO2P      = 300.0f;
  222.                 g_deviceParameters [i].BTO3P      = 400.0f;
  223.                 g_deviceParameters [i].BTO4P      = 500.0f;
  224.                 g_deviceParameters [i].MinP       = 500.0f;
  225.                 g_deviceParameters [i].BTimeoutR  = 500.0f;
  226.                 g_deviceParameters [i].LoftPeriod = 500.0f;
  227.                 g_deviceParameters [i].RTDelta    = 200;
  228.                 g_deviceParameters [i].RTMinError = 15;
  229.                 g_deviceParameters [i].EncMult    = 2.5f;
  230.                 g_deviceParameters [i].MaxLoft    = 32;
  231.                 g_deviceParameters [i].PTimeout   = 100.0f;
  232.                 g_deviceParameters [i].SynOUTP    = 1;
  233.                 g_deviceParameters [i].StartPos   = 0;
  234.  
  235.                 // USMC_StartParameters defaults:
  236.                 g_deviceStartParameters [i].SDivisor = 8;
  237.                 g_deviceStartParameters [i].LoftEn   = TRUE;
  238.                 g_deviceStartParameters [i].SlStart  = TRUE;
  239.         }
  240. }
  241.  
  242.  
  243.  
  244.  
  245. //      Function: IOCTRL ()
  246. //              Encapsulates DeviceIoControl call ( internal use ).
  247. //      Parameters:
  248. //              [in]  Device: device number to call DeviceIoControl on.
  249. //              [in]  dwCode: IO Control code for operation.
  250. //              [out] pInBuf: buffer pointer to store returned data in.
  251. //              [in]  dwInBufSize: size of in buffer.
  252. //              [in]  pOutBuf: buffer pointer to send data from.
  253. //              [in]  dwOutBufSize: size of out buffer.
  254. //      Return Value:
  255. //              DWORD; zero means succes, other value - error code.
  256. //      Remarks:
  257. //              Call this function with Device == -1 when devices wasn't enumerated yet.
  258. //      The IOCTRL () function DOES NOT sets error description. This routine must
  259. //      be completed by caller function to specify most appropriate information: where
  260. //      error has occured and with which operation it associated.
  261. DWORD IOCTRL ( DWORD Device,
  262.                           DWORD dwCode,
  263.                           PVOID pInBuf,
  264.                           DWORD dwInBufSize,
  265.                           PVOID pOutBuf,
  266.                           DWORD dwOutBufSize )
  267. {
  268.         DWORD  deviceNumber;
  269.         char   deviceName [128];        // Device name in form Prefix + DeviceNumber + ":\0".
  270.         DWORD  dwErr;
  271.         int    hFile;
  272.         char * buf;
  273.  
  274.         dwErr = 0;
  275.  
  276.         deviceNumber = Device == -1 ? 0 : Device;
  277.         sprintf ( deviceName, "/dev/usmc%d", deviceNumber );
  278.  
  279.         hFile = open ( deviceName, 0 );
  280.  
  281.         if ( hFile == -1 )
  282.                 return  -USMC_ERROR_REF_DEV_DISCONNECTED;
  283.  
  284.  
  285.         if ( pInBuf && dwInBufSize ) {
  286.                 buf = pInBuf;
  287.         } else if ( pOutBuf && dwOutBufSize ) {
  288.                 buf = pOutBuf;
  289.         }
  290.  
  291.  
  292.         dwErr = ioctl ( hFile,
  293.                         dwCode,
  294.                         buf );
  295.  
  296.         close ( hFile );
  297.  
  298.         return  dwErr;
  299. }
  300.  
  301.  
  302.  
  303.  
  304. DWORD USMC_Init ( USMC_Devices * Str )
  305. {
  306.         int nod;
  307.         DWORD i;
  308.  
  309.         nod = 0;
  310.         Str -> NOD = 0;
  311.         Str -> Serial = Str -> Version = NULL;
  312.  
  313.         if ( !g_IsInitialized )
  314.         {
  315.                 //InitializeCriticalSection ( &g_Lock );
  316.                 g_IsInitialized = TRUE;
  317.         }
  318.  
  319.         //EnterCriticalSection ( &g_Lock );
  320.         DWORD dwRes = IOCTRL ( -1,
  321.                                                   IOCTL_GET_NOD,
  322.                                                   ( PVOID ) &nod,
  323.                                                   1, NULL, 0 );
  324.  
  325.         if ( ( int ) dwRes < 0 )
  326.                 return  USMC_SUCCESS;   // No devices connected, so return NOD = 0.
  327.         else
  328.                 Str -> NOD = nod;
  329.  
  330.  
  331.         // Enumerate connected devices:
  332.         Str -> Serial  = ( char ** ) malloc ( ( ssize_t ) ( sizeof ( char * ) * 32 ) );
  333.         Str -> Version = ( char ** ) malloc ( ( ssize_t ) ( sizeof ( char * ) * 32 ) );
  334.  
  335.         for ( i = 0 ; i < Str -> NOD; i++ )
  336.         {
  337.                 char * serial  = ( char * ) malloc ( ( ssize_t ) 16 );
  338.                 char * version = ( char * ) malloc ( ( ssize_t ) 6 );
  339.  
  340.                 dwRes = IOCTRL ( i,
  341.                                                 IOCTL_GET_SERIAL,
  342.                                                 ( PVOID ) serial,
  343.                                                 16, NULL, 0 );
  344.  
  345.                 if ( ( int ) dwRes < 0 )
  346.                 {
  347.                         dwRes = SetLastErr ( "USMC_Init () failed during IOCTRL ( IOCTL_GET_SERIAL ) call", dwRes );
  348.                         free ( serial  );
  349.                         free ( version );
  350.                         Str -> NOD = 0;
  351.                         Str -> Serial = Str -> Version = NULL;
  352.  
  353.                         return  dwRes;
  354.                 }
  355.  
  356.  
  357.                 dwRes = IOCTRL ( i,
  358.                                                 IOCTL_GET_VERSION,
  359.                                                 ( PVOID ) version,
  360.                                                 6, NULL, 0 );
  361.  
  362.                 if ( ( int ) dwRes < 0 )
  363.                 {
  364.                         dwRes = SetLastErr ( "USMC_Init () failed during IOCTRL ( IOCTL_GET_VERSION ) call", dwRes );
  365.                         free ( serial  );
  366.                         free ( version );
  367.                         Str -> NOD = 0;
  368.                         Str -> Serial = Str -> Version = NULL;
  369.  
  370.                         return  dwRes;
  371.                 }
  372.  
  373.  
  374.                 Str -> Serial [i] = serial;
  375.  
  376.                 ( Str -> Version [i] ) = ( char * ) malloc ( ( ssize_t ) 4 );
  377.                 memcpy ( Str -> Version [i], version + 2, ( ssize_t ) 4 );
  378.  
  379.                 //strupr ( Str -> Version [i] );
  380.                 sscanf ( Str -> Version [i], "%X", &g_devicesVersions [i] );
  381.         }
  382.  
  383.  
  384.         g_devices = *Str;
  385.         InitDefaultValues ();
  386.  
  387.         //LeaveCriticalSection ( &g_Lock );
  388.        
  389.  
  390.         return  USMC_SUCCESS;
  391. }
  392.  
  393.  
  394.  
  395.  
  396. DWORD USMC_GetState ( DWORD Device, USMC_State * Str )
  397. {
  398.         if ( !g_IsInitialized )
  399.                 return  SetLastErr ( "USMC_GetState () failed", -USMC_ERROR_NOT_INITIALIZED );
  400.  
  401.  
  402.  
  403.         STATE_PACKET getStateData;
  404.  
  405.         //EnterCriticalSection ( &g_Lock );
  406.         DWORD dwRes = IOCTRL ( Device,
  407.                                                   IOCTL_GET_STATE,
  408.                                                   ( PVOID ) &getStateData,
  409.                                                   11, NULL, 0 );
  410.         //LeaveCriticalSection ( &g_Lock );
  411.  
  412.         if ( ( int ) dwRes < 0 )
  413.                 return  SetLastErr ( "USMC_GetState () failed during IOCTRL ( IOCTL_GET_STATE ) call", dwRes );
  414.  
  415.  
  416.         Str -> AReset    = getStateData.AFTRESET;
  417.         Str -> CurPos    = ( ( signed int ) getStateData.CurPos ) / 8;
  418.         Str -> CW_CCW    = getStateData.CW_CCW;
  419.         Str -> EmReset   = getStateData.EMRESET;
  420.         Str -> FullPower = getStateData.REFIN;
  421.         Str -> FullSpeed = getStateData.FULLSPEED;
  422.         Str -> Loft      = getStateData.LOFT;
  423.         Str -> Power     = getStateData.RESET;
  424.         Str -> RotTr     = getStateData.ROTTR;
  425.         Str -> RotTrErr  = getStateData.ROTTRERR;
  426.         Str -> RUN       = getStateData.RUN;
  427.         /*Str -> SDivisor= See below;*/
  428.         Str -> SyncIN    = getStateData.SYNCIN;
  429.         Str -> SyncOUT   = getStateData.SYNCOUT;
  430.         /*Str -> Temp    = See below;*/
  431.         Str -> Trailer1  = getStateData.TRAILER1;
  432.         Str -> Trailer2  = getStateData.TRAILER2;
  433.         /*Str -> Voltage = See below;*/
  434.  
  435.         Str -> SDivisor  = ( BYTE ) ( 1 << ( getStateData.M2 << 1 | getStateData.M1 ) );
  436.         double t      = ( double ) getStateData.Temp;
  437.  
  438.         if ( g_devicesVersions [Device] < 0x2400 )
  439.         {
  440.                 t = t * 3.3 / 65536.0;
  441.                 t = t * 10.0 / ( 5.0 - t );
  442.                 t = ( 1.0 / 298.0 ) + ( 1.0 / 3950.0 ) * log ( t / 10.0 );
  443.                 t = 1.0 / t - 273.0;
  444.         }
  445.         else
  446.         {
  447.                 t = ( ( t * 3.3 * 100.0 / 65536.0 ) - 50.0 );
  448.         }
  449.  
  450.  
  451.         Str -> Temp    = ( float ) t;
  452.         Str -> Voltage = ( float ) ( ( ( double ) getStateData.Voltage ) / 65536.0 * 3.3 * 20.0 );
  453.         Str -> Voltage = Str -> Voltage < 5.0f ? 0.0f : Str -> Voltage;
  454.  
  455.  
  456.         return  USMC_SUCCESS;
  457. }
  458.  
  459.  
  460.  
  461.  
  462. DWORD USMC_SaveParametersToFlash ( DWORD Device )
  463. {
  464.         if ( !g_IsInitialized )
  465.                 return  SetLastErr ( "USMC_SaveParametersToFlash () failed", -USMC_ERROR_NOT_INITIALIZED );
  466.  
  467.  
  468.         //EnterCriticalSection ( &g_Lock );
  469.         DWORD dwRes = IOCTRL ( Device,
  470.                                                   IOCTL_SAVE_PARAMETERS,
  471.                                                   NULL, 0, NULL, 0 );
  472.         //LeaveCriticalSection ( &g_Lock );
  473.  
  474.  
  475.         if ( ( int ) dwRes < 0 )
  476.                 return  SetLastErr ( "USMC_SaveParametersToFlash () failed during IOCTRL ( IOCTL_SAVE_PARAMETERS ) call", dwRes );
  477.  
  478.  
  479.  
  480.         return  USMC_SUCCESS;
  481. }
  482.  
  483.  
  484.  
  485.  
  486. DWORD USMC_GetMode ( DWORD Device, USMC_Mode * Str )
  487. {
  488.         if ( !g_IsInitialized )
  489.                 return  SetLastErr ( "USMC_GetMode () failed", -USMC_ERROR_NOT_INITIALIZED );
  490.  
  491.  
  492.         *Str = g_deviceMode [Device];
  493.  
  494.  
  495.         return  USMC_SUCCESS;
  496. }
  497.  
  498.  
  499.  
  500.  
  501. DWORD USMC_SetMode ( DWORD Device, USMC_Mode * Str )
  502. {
  503.         if ( !g_IsInitialized )
  504.                 return  SetLastErr ( "USMC_SetMode () failed", -USMC_ERROR_NOT_INITIALIZED );
  505.  
  506.  
  507.         MODE_PACKET setModeData;
  508.         // Byte 0:
  509.         setModeData.PMODE     = Str -> PMode;
  510.         setModeData.REFINEN   = Str -> PReg;
  511.         setModeData.RESETD    = Str -> ResetD;
  512.         setModeData.EMRESET   = Str -> EMReset;
  513.         setModeData.TR1T      = Str -> Tr1T;
  514.         setModeData.TR2T      = Str -> Tr2T;
  515.         setModeData.ROTTRT    = Str -> RotTrT;
  516.         setModeData.TRSWAP    = Str -> TrSwap; 
  517.         // Byte 1:
  518.         setModeData.TR1EN     = Str -> Tr1En;
  519.         setModeData.TR2EN     = Str -> Tr2En;
  520.         setModeData.ROTTREN   = Str -> RotTeEn;
  521.         setModeData.ROTTROP   = Str -> RotTrOp;
  522.         setModeData.BUTT1T    = Str -> Butt1T;
  523.         setModeData.BUTT2T    = Str -> Butt2T;
  524.         /*setModeData.BUTSWAP = ...;*/
  525.         setModeData.RESETRT   = Str -> ResetRT;
  526.         // Byte 2:
  527.         setModeData.SNCOUTEN  = Str -> SyncOUTEn;
  528.         setModeData.SYNCOUTR  = Str -> SyncOUTR;
  529.         setModeData.SYNCINOP  = Str -> SyncINOp;
  530.         setModeData.SYNCOPOL  = Str -> SyncInvert;
  531.         setModeData.ENCODER   = Str -> EncoderEn;
  532.         setModeData.INVENC    = Str -> EncoderInv;
  533.         setModeData.RESBENC   = Str -> ResBEnc;
  534.         setModeData.RESENC    = Str -> ResEnc;
  535.  
  536.         setModeData.SYNCCOUNT = PACK_DWORD ( Str -> SyncCount );
  537.  
  538.         // Save parameters within DLL for further USMC_GetMode () calls:
  539.         //EnterCriticalSection ( &g_Lock );
  540.         g_deviceMode [Device] = *Str;
  541.  
  542.         DWORD dwRes = IOCTRL ( Device,
  543.                                                   IOCTL_SET_MODE,
  544.                                                   NULL, 0,
  545.                                                   ( PVOID ) &setModeData, 7 );
  546.         //LeaveCriticalSection ( &g_Lock );
  547.  
  548.  
  549.         if ( ( int ) dwRes < 0 )
  550.                 return  SetLastErr ( "USMC_SetMode () failed during IOCTRL ( IOCTL_SET_MODE ) call", dwRes );
  551.  
  552.  
  553.         return  USMC_SUCCESS;
  554. }
  555.  
  556.  
  557.  
  558.  
  559. DWORD USMC_GetParameters ( DWORD Device, USMC_Parameters * Str )
  560. {
  561.         if ( !g_IsInitialized )
  562.                 return  SetLastErr ( "USMC_GetParameters () failed", -USMC_ERROR_NOT_INITIALIZED );
  563.  
  564.  
  565.         *Str = g_deviceParameters [Device];
  566.  
  567.  
  568.         return  USMC_SUCCESS;
  569. }
  570.  
  571.  
  572.  
  573.  
  574. DWORD USMC_SetParameters ( DWORD Device, USMC_Parameters * Str )
  575. {
  576.         if ( !g_IsInitialized )
  577.                 return  SetLastErr ( "USMC_SetParameters () failed", -USMC_ERROR_NOT_INITIALIZED );
  578.  
  579.  
  580.         PARAMETERS_PACKET setParametersData;
  581.  
  582.         /*=====================*/
  583.         /* ----Conversion:---- */
  584.         /*=====================*/
  585.         setParametersData.DELAY1       = ( BYTE ) clamp ( ( int ) ( Str -> AccelT / 98.0f + 0.5f ), 1, 15 );
  586.         setParametersData.DELAY2       = ( BYTE ) clamp ( ( int ) ( Str -> DecelT / 98.0f + 0.5f ), 1, 15 );
  587.         setParametersData.RefINTimeout = ( WORD ) ( clampf ( Str -> PTimeout , 1.0f, 9961.0f ) / 0.152f + 0.5f );
  588.         setParametersData.BTIMEOUT1    = PACK_WORD ( ( WORD ) ( clampf ( Str -> BTimeout1, 1.0f, 9961.0f ) / 0.152f + 0.5f ) );
  589.         setParametersData.BTIMEOUT2    = PACK_WORD ( ( WORD ) ( clampf ( Str -> BTimeout2, 1.0f, 9961.0f ) / 0.152f + 0.5f ) );
  590.         setParametersData.BTIMEOUT3    = PACK_WORD ( ( WORD ) ( clampf ( Str -> BTimeout3, 1.0f, 9961.0f ) / 0.152f + 0.5f ) );
  591.         setParametersData.BTIMEOUT4    = PACK_WORD ( ( WORD ) ( clampf ( Str -> BTimeout4, 1.0f, 9961.0f ) / 0.152f + 0.5f ) );
  592.         setParametersData.BTIMEOUTR    = PACK_WORD ( ( WORD ) ( clampf ( Str -> BTimeoutR, 1.0f, 9961.0f ) / 0.152f + 0.5f ) );
  593.         setParametersData.BTIMEOUTD    = PACK_WORD ( ( WORD ) ( clampf ( Str -> BTimeoutD, 1.0f, 9961.0f ) / 0.152f + 0.5f ) );
  594.         setParametersData.MINPERIOD    = PACK_WORD ( ( WORD ) ( 65536.0f - ( 125000.0f / clampf ( Str -> MinP , 2.0f, 625.0f ) ) + 0.5f ) );
  595.         setParametersData.BTO1P        = PACK_WORD ( ( WORD ) ( 65536.0f - ( 125000.0f / clampf ( Str -> BTO1P, 2.0f, 625.0f ) ) + 0.5f ) );
  596.         setParametersData.BTO2P        = PACK_WORD ( ( WORD ) ( 65536.0f - ( 125000.0f / clampf ( Str -> BTO2P, 2.0f, 625.0f ) ) + 0.5f ) );
  597.         setParametersData.BTO3P        = PACK_WORD ( ( WORD ) ( 65536.0f - ( 125000.0f / clampf ( Str -> BTO3P, 2.0f, 625.0f ) ) + 0.5f ) );
  598.         setParametersData.BTO4P        = PACK_WORD ( ( WORD ) ( 65536.0f - ( 125000.0f / clampf ( Str -> BTO4P, 2.0f, 625.0f ) ) + 0.5f ) );
  599.         setParametersData.MAX_LOFT     = PACK_WORD ( ( WORD ) ( clamp ( Str -> MaxLoft, 1, 1023 ) * 64 ) );
  600.  
  601.         if ( g_devicesVersions [Device] < 0x2407 )
  602.         {
  603.                 setParametersData.STARTPOS = 0x00000000L;
  604.         }
  605.         else
  606.         {
  607.                 setParametersData.STARTPOS = PACK_DWORD ( Str -> StartPos * 8 & 0xFFFFFF00 );
  608.         }
  609.  
  610.  
  611.         setParametersData.RTDelta    = PACK_WORD ( ( WORD ) ( clamp ( Str -> RTDelta   , 4, 1023 ) * 64 ) );
  612.         setParametersData.RTMinError = PACK_WORD ( ( WORD ) ( clamp ( Str -> RTMinError, 4, 1023 ) * 64 ) );
  613.  
  614.         double t = ( double ) clampf ( Str -> MaxTemp, 0.0f, 100.0f );
  615.  
  616.         if ( g_devicesVersions [Device] < 0x2400 )
  617.         {
  618.                 t = 10.0 * exp ( 3950.0 * ( 1.0 / ( t + 273.0 ) - 1.0 / 298.0 ) );
  619.                 t = ( ( 5 * t / ( 10 + t ) ) * 65536.0 / 3.3 + 0.5 );
  620.         }
  621.         else
  622.         {
  623.                 t = ( t + 50.0 ) / 330.0 * 65536.0;
  624.                 t = ( t + 0.5f );
  625.         }
  626.  
  627.  
  628.         setParametersData.MaxTemp    = PACK_WORD ( ( WORD ) t );
  629.         setParametersData.SynOUTP    = Str -> SynOUTP;
  630.         setParametersData.LoftPeriod = Str -> LoftPeriod == 0.0f ? 0 :
  631.                                                                          PACK_WORD ( ( WORD ) ( 65536.0f - ( 125000.0f / clampf ( Str -> LoftPeriod, 16.0f, 5000.0f ) ) + 0.5f ) );
  632.         setParametersData.EncVSCP    = ( BYTE ) ( Str -> EncMult * 4.0f + 0.5f );
  633. //setParametersData.EncVSCP = 1;        ///////////
  634.  
  635.         memset ( setParametersData.Reserved, 0, 15 );
  636.         //unsigned char testBuf [sizeof ( PARAMETERS_PACKET )];
  637.         //memcpy ( ( void * ) testBuf, ( const void * ) &setParametersData, ( size_t ) sizeof ( PARAMETERS_PACKET ) );
  638.         /*=======================*/
  639.         /* ----Deconversion:---- */
  640.         /*=======================*/
  641.         Str -> AccelT    = setParametersData.DELAY1 * 98.0f;
  642.         Str -> DecelT    = setParametersData.DELAY2 * 98.0f;
  643.         Str -> PTimeout  = ( float ) ( setParametersData.RefINTimeout * 0.152f );
  644.         Str -> BTimeout1 = ( float ) ( PACK_WORD ( setParametersData.BTIMEOUT1 ) * 0.152f );
  645.         Str -> BTimeout2 = ( float ) ( PACK_WORD ( setParametersData.BTIMEOUT2 ) * 0.152f );
  646.         Str -> BTimeout3 = ( float ) ( PACK_WORD ( setParametersData.BTIMEOUT3 ) * 0.152f );
  647.         Str -> BTimeout4 = ( float ) ( PACK_WORD ( setParametersData.BTIMEOUT4 ) * 0.152f );
  648.         Str -> BTimeoutR = ( float ) ( PACK_WORD ( setParametersData.BTIMEOUTR ) * 0.152f );
  649.         Str -> BTimeoutD = ( float ) ( PACK_WORD ( setParametersData.BTIMEOUTD ) * 0.152f );
  650.         Str -> MinP      = 125000.0f / ( 65536.0f - ( float ) ( PACK_WORD ( setParametersData.MINPERIOD ) ) );
  651.         Str -> BTO1P     = 125000.0f / ( 65536.0f - ( float ) ( PACK_WORD ( setParametersData.BTO1P ) ) );
  652.         Str -> BTO2P     = 125000.0f / ( 65536.0f - ( float ) ( PACK_WORD ( setParametersData.BTO2P ) ) );
  653.         Str -> BTO3P     = 125000.0f / ( 65536.0f - ( float ) ( PACK_WORD ( setParametersData.BTO3P ) ) );
  654.         Str -> BTO4P     = 125000.0f / ( 65536.0f - ( float ) ( PACK_WORD ( setParametersData.BTO4P ) ) );
  655.         Str -> MaxLoft   = PACK_WORD ( setParametersData.MAX_LOFT ) / 64;
  656.  
  657.         if( g_devicesVersions [Device] < 0x2407 )
  658.         {
  659.                 Str -> StartPos = 0x00000000L;
  660.         }
  661.         else
  662.         {
  663.                 Str -> StartPos = PACK_DWORD ( setParametersData.STARTPOS ) / 8;
  664.         }
  665.  
  666.  
  667.         Str -> RTDelta    = PACK_WORD ( setParametersData.RTDelta )    / 64;
  668.         Str -> RTMinError = PACK_WORD ( setParametersData.RTMinError ) / 64;
  669.  
  670.         if ( g_devicesVersions [Device] < 0x2400 )
  671.         {
  672.                 t = ( double )( PACK_WORD ( setParametersData.MaxTemp ) );
  673.                 t = t * 3.3 / 65536.0;
  674.                 t = 10.0 * t / ( 5.0 - t );
  675.                 t = ( 1.0 / 298.0 ) + ( 1.0 / 3950.0 ) * log ( t / 10.0 );
  676.                 t = 1.0 / t - 273.0;
  677.         }
  678.         else
  679.         {
  680.                 t = ( double ) ( PACK_WORD ( setParametersData.MaxTemp ) );
  681.                 t = ( t * 3.3 * 100.0 / 65536.0 ) - 50.0;
  682.         }
  683.  
  684.  
  685.         Str -> MaxTemp = ( float ) t;
  686.  
  687.         Str -> SynOUTP = setParametersData.SynOUTP;
  688.         Str -> LoftPeriod = PACK_WORD ( setParametersData.LoftPeriod ) == 0 ? 0.0f :
  689.                                          125000.0f / ( 65536.0f - ( float ) ( PACK_WORD ( setParametersData.LoftPeriod ) ) );
  690.         Str -> EncMult = ( ( float ) setParametersData.EncVSCP ) / 4.0f;
  691.  
  692.         // Save parameters within DLL for further USMC_GetParameters () calls:
  693.         //EnterCriticalSection ( &g_Lock );
  694.         g_deviceParameters [Device] = *Str;
  695.  
  696.         DWORD dwRes = IOCTRL ( Device,
  697.                                                   IOCTL_SET_PARAMETERS,
  698.                                                   NULL, 0,
  699.                                                   ( PVOID ) &setParametersData, 57 );
  700.         //LeaveCriticalSection ( &g_Lock );
  701.  
  702.  
  703.         if ( ( int ) dwRes < 0 )
  704.                 return  SetLastErr ( "USMC_SetParameters () failed during IOCTRL ( IOCTL_SET_PARAMETERS ) call", dwRes );
  705.  
  706.  
  707.         return  USMC_SUCCESS;
  708. }
  709.  
  710.  
  711.  
  712.  
  713. DWORD USMC_GetStartParameters ( DWORD Device, USMC_StartParameters * Str )
  714. {
  715.         if ( !g_IsInitialized )
  716.                 return  SetLastErr ( "USMC_GetStartParameters () failed", -USMC_ERROR_NOT_INITIALIZED );
  717.  
  718.  
  719.         *Str = g_deviceStartParameters [Device];
  720.  
  721.  
  722.         return  USMC_SUCCESS;
  723. }
  724.  
  725.  
  726.  
  727.  
  728. DWORD USMC_Start ( DWORD Device, int DestPos,
  729.                                                            float * Speed,
  730.                                                            USMC_StartParameters * Str )
  731. {
  732.         if ( !g_IsInitialized )
  733.                 return  SetLastErr ( "USMC_Start () failed", -USMC_ERROR_NOT_INITIALIZED );
  734.  
  735.  
  736.         GO_TO_PACKET goToData;
  737.  
  738.         /*=====================*/
  739.         /* ----Conversion:---- */
  740.         /*=====================*/
  741.         goToData.DestPos     = ( DWORD ) ( DestPos * 8 );
  742.         goToData.TimerPeriod = PACK_WORD ( ( WORD  ) ( 65536.0f - ( 1000000.0f / clampf ( *Speed, 16.0f, 5000.0f ) ) + 0.5f ) );
  743.         switch ( Str -> SDivisor )
  744.         {
  745.         case 1:
  746.                 goToData.M1 = goToData.M2 = 0;
  747.                 break;
  748.         case 2:
  749.                 goToData.M1 = 1;
  750.                 goToData.M2 = 0;
  751.                 break;
  752.         case 4:
  753.                 goToData.M1 = 0;
  754.                 goToData.M2 = 1;
  755.                 break;
  756.         case 8:
  757.                 goToData.M1 = 1;
  758.                 goToData.M2 = 1;
  759.                 break;
  760.         }
  761.         //goToData.M1          = Str -> SDivisor && 0x01;
  762.         //goToData.M2          = Str -> SDivisor && 0x02;
  763.         goToData.DEFDIR      = Str -> DefDir;
  764.         goToData.LOFTEN      = Str -> LoftEn;
  765.         goToData.SLSTRT      = Str -> SlStart;
  766.         goToData.WSYNCIN     = Str -> WSyncIN;
  767.         goToData.SYNCOUTR    = Str -> SyncOUTR;
  768.         goToData.FORCELOFT   = Str -> ForceLoft;
  769.  
  770.  
  771.         /*=======================*/
  772.         /* ----Deconversion:---- */
  773.         /*=======================*/
  774.         *Speed        = 1000000.0f / ( 65536.0f - ( float ) PACK_WORD ( goToData.TimerPeriod ) );
  775.         Str -> SDivisor = 1 << ( ( goToData.M2 << 1 ) | goToData.M1 );
  776.  
  777.         // Save start parameters within DLL for further USMC_GetStartParameters () calls:
  778.         //EnterCriticalSection ( &g_Lock );
  779.         g_deviceStartParameters [Device] = *Str;
  780.  
  781.         DWORD dwRes = IOCTRL ( Device,
  782.                                                   IOCTL_GO_TO,
  783.                                                   NULL, 0,
  784.                                                   ( PVOID ) &goToData, 7 );
  785.         //LeaveCriticalSection ( &g_Lock );
  786.  
  787.  
  788.         if ( ( int ) dwRes < 0 )
  789.                 return  SetLastErr ( "USMC_Start () failed during IOCTRL ( IOCTL_GO_TO ) call", dwRes );
  790.  
  791.  
  792.         return  USMC_SUCCESS;
  793. }
  794.  
  795.  
  796.  
  797.  
  798. DWORD USMC_Stop ( DWORD Device )
  799. {
  800.         if ( !g_IsInitialized )
  801.                 return  SetLastErr ( "USMC_Stop () failed", -USMC_ERROR_NOT_INITIALIZED );
  802.  
  803.  
  804.         //EnterCriticalSection ( &g_Lock );
  805.         DWORD dwRes = IOCTRL ( Device,
  806.                                                   IOCTL_STOP_STEP_MOTOR,
  807.                                                   NULL, 0, NULL, 0 );
  808.         //LeaveCriticalSection ( &g_Lock );
  809.  
  810.  
  811.         if ( ( int ) dwRes < 0 )
  812.                 return  SetLastErr ( "USMC_Stop () failed during IOCTRL ( IOCTL_STOP_STEP_MOTOR ) call", dwRes );
  813.  
  814.  
  815.         return  USMC_SUCCESS;
  816. }
  817.  
  818.  
  819.  
  820.  
  821. DWORD USMC_SetCurrentPosition ( DWORD Device, int Position )
  822. {
  823.         if ( !g_IsInitialized )
  824.                 return  SetLastErr ( "USMC_SetCurrentPosition () failed", -USMC_ERROR_NOT_INITIALIZED );
  825.  
  826.  
  827.         Position *= 8;
  828.         Position &= 0xFFFFFFE0;
  829.         //EnterCriticalSection ( &g_Lock );
  830.         DWORD dwRes = IOCTRL ( Device,
  831.                                                   IOCTL_SET_CURRENT_POSITION,
  832.                                                   NULL, 0,
  833.                                                   ( PVOID ) &Position, 4 );
  834.         //LeaveCriticalSection ( &g_Lock );
  835.  
  836.  
  837.         if ( ( int ) dwRes < 0 )
  838.                 return  SetLastErr ( "USMC_SetCurrentPosition () failed during IOCTRL ( IOCTL_SET_CURRENT_POSITION ) call", dwRes );
  839.  
  840.  
  841.         return  USMC_SUCCESS;
  842. }
  843.  
  844.  
  845.  
  846.  
  847. DWORD USMC_GetEncoderState ( DWORD Device, USMC_EncoderState * Str )
  848. {
  849.         if ( !g_IsInitialized )
  850.                 return  SetLastErr ( "USMC_GetEncoderState () failed", -USMC_ERROR_NOT_INITIALIZED );
  851.  
  852.  
  853.         if ( g_devicesVersions [Device] < 0x2410 )
  854.                 return  USMC_SUCCESS;
  855.  
  856.  
  857.         //EnterCriticalSection ( &g_Lock );
  858.         ENCODER_STATE_PACKET getEncoderStateData;
  859.  
  860.         DWORD dwRes = IOCTRL ( Device,
  861.                                                   IOCTL_GET_ENCODER_STATE,
  862.                                                   ( PVOID ) &getEncoderStateData,
  863.                                                   8, NULL, 0 );
  864.         //LeaveCriticalSection ( &g_Lock );
  865.  
  866.  
  867.         if ( ( int ) dwRes < 0 )
  868.                 return  SetLastErr ( "USMC_GetEncoderState () failed during IOCTRL ( IOCTL_GET_STATE ) call", dwRes );
  869.  
  870.  
  871.         Str -> ECurPos    = getEncoderStateData.ECurPos;
  872.         Str -> EncoderPos = getEncoderStateData.EncPos;
  873.  
  874.  
  875.         return  USMC_SUCCESS;
  876. }
  877.  
  878.  
  879.  
  880.  
  881. void USMC_GetLastErr ( char * str, ssize_t len )
  882. {
  883.         //EnterCriticalSection ( &g_Lock );
  884.         if ( strlen ( g_lastErrDesc ) > len )
  885.                 memcpy ( str, g_lastErrDesc, len );
  886.         else
  887.                 strcpy ( str, ( const char * ) g_lastErrDesc );
  888.         //LeaveCriticalSection ( &g_Lock );
  889. }
  890.  
  891.  
  892.  
  893.  
  894. DWORD USMC_Close ( void )
  895. {
  896.         if ( !g_IsInitialized )
  897.                 return  SetLastErr ( "USMC_Close () failed", -USMC_ERROR_NOT_INITIALIZED );
  898.  
  899.  
  900.         return  USMC_SUCCESS;
  901. }
  902.