Subversion Repositories f9daq

Rev

Blame | Last modification | View Log | RSS feed

#include        <math.h>
#include        <stdio.h>
#include        <string.h>
#include        <fcntl.h>      /* open */
#include        <unistd.h>     /* exit */
#include        <sys/ioctl.h>  /* ioctl */
#include        "libusmc.h"
#include        "usmctypes.h"


#define         USB_ERROR(errCode)              (errCode|0x00000000L)
// Linux kernel errors or'ed with this value
// to distinguish them from usb errors.
#define         LINKRN_ERROR(errCode)   (errCode|0x10000000L)
#define         HIBYTE(w)                       ((w&0xff00)>>8)
#define         LOBYTE(w)                       (w&0x00ff)
#define         HIWORD(dw)                      ((dw&0xffff0000)>>16)
#define         LOWORD(dw)                      (dw&0x0000ffff)
#define         PACK_WORD(w)                    (HIBYTE(w)|(LOBYTE(w)<<8))
//#define               PACK_WORD(w)                    (LOBYTE(w)|(HIBYTE(w)<<8))
#define         PACK_DWORD(w)                   (HIBYTE(HIWORD(w))| \
                                                                         (LOBYTE(HIWORD(w))<<8)| \
                                                                         (HIBYTE(LOWORD(w))<<16)| \
                                                                         (LOBYTE(LOWORD(w))<<24))



// DLL data:
BOOL                 g_IsInitialized = FALSE;
USMC_Devices         g_devices;
USMC_Mode            g_deviceMode            [32];
USMC_Parameters      g_deviceParameters      [32];
USMC_StartParameters g_deviceStartParameters [32];
DWORD                g_devicesVersions [32];
char                 g_lastErrDesc     [256];   // Null-terminated ASCII error description string.


// Error string descriptions:
static char * errDesc [] = {
        "0x???????? ( Unknown )",
        "0x00000000 ( Success )",
/*      "0x00000001 ( USB CRC )",
        "0x00000002 ( USB Bit stuffing )",
        "0x00000003 ( USB Data toggle mismatch )",

        "0x00000004 ( USB Stall )",
        "0x00000005 ( USB Device not responding )",
        "0x00000006 ( USB PID check failure )",
        "0x00000007 ( USB Unexpected PID )",
        "0x00000008 ( USB Data overrun )",

        "0x00000009 ( USB Data underrun )",
        "0x0000000C ( USB Buffer overrun )",
        "0x0000000D ( USB Buffer underrun )",
        "0x0000000E ( USB Not accessed )",
        "0x0000000F ( USB Not accessed alt )",

        "0x00000100 ( USB Isochronous )",
        "0x00000101 ( USB Canceled )",
        "0x00000103 ( USB Not complete )",
        "0x00000104 ( USB Client buffer )",*/

        "0x14141414 ( USMC Not initialized )",
        //"0x00000006 ( USMC Reffered Device not connected )",
        //"0x00000012 ( DeviceManager error - driver function called with invalid arguments )",
        "0x14141415 ( USMC No devices connected )"
};


// Internal-use functions prototypes:
DWORD  SetLastErr  ( const char * str, DWORD dwErrCode );
char * ErrCode2Str ( DWORD dwErrCode );
int    clamp       ( int val, int min, int max );
float  clampf      ( float val, float min, float max );
DWORD  IOCTRL       ( DWORD Device,
                                         DWORD dwCode,
                                         PVOID pInBuf,
                                         DWORD dwInBufSize,
                                         PVOID pOutBuf,
                                         DWORD dwOutBufSize );




//      Function: SetLastErr ()
//      Argument list:
//              const char * str √ in √ Null-terminated ASCII error description.
//      DWORD dwErrCode - in - optional - error code supplied with description.
//      Return value:
//              DWORD - modified error code.
//      Remarks:
//              The SetLastErr () sets description of last occured error.
DWORD SetLastErr ( const char * str, DWORD dwErrCode )
{
        if ( dwErrCode == -USMC_ERROR_REF_DEV_DISCONNECTED )
                sprintf ( ( char * ) g_lastErrDesc, "Error: %s. Code: %s", str, "Reffered device disconnected" );
        else if ( dwErrCode != 1 )
                sprintf ( ( char * ) g_lastErrDesc, "Error: %s. Code: %d", str, ( signed int ) dwErrCode );

        //if ( dwErrCode != 1 )
        //      sprintf ( ( char * ) g_lastErrDesc, "Error: %s. Code: %s", str, ErrCode2Str ( dwErrCode ) );
        //else
        //      sprintf ( ( char * ) g_lastErrDesc, "Error: %s.", str );


        return  dwErrCode;
}




//      Function: ErrCode2Str ()
//      Argument list:
//              DWORD dwErrCode √ in √ error code returned by any operation.
//      Return value:
//      char * - string equivalent for specified numerical error code.
//      Remarks:
//      There is no need to free error string obtained with this function:
//      return value points to static dll variable.
char * ErrCode2Str ( DWORD dwErrCode )
{
        switch ( dwErrCode )
        {
/*      case USB_NO_ERROR:
                return  errDesc [1]; break;
        case USB_CRC_ERROR:
                return  errDesc [2]; break;
        case USB_BIT_STUFFING_ERROR:
                return  errDesc [3]; break;
        case USB_DATA_TOGGLE_MISMATCH_ERROR:
                return  errDesc [4]; break;
        case USB_STALL_ERROR:
                return  errDesc [5]; break;
        case USB_DEVICE_NOT_RESPONDING_ERROR:
                return  errDesc [6]; break;
        case USB_PID_CHECK_FAILURE_ERROR:
                return  errDesc [7]; break;
        case USB_UNEXPECTED_PID_ERROR:
                return  errDesc [8]; break;
        case USB_DATA_OVERRUN_ERROR:
                return  errDesc [9]; break;
        case USB_DATA_UNDERRUN_ERROR:
                return  errDesc [10]; break;
        case USB_BUFFER_OVERRUN_ERROR:
                return  errDesc [11]; break;
        case USB_BUFFER_UNDERRUN_ERROR:
                return  errDesc [12]; break;
        case USB_NOT_ACCESSED_ERROR:
                return  errDesc [13]; break;
        case USB_NOT_ACCESSED_ALT:
                return  errDesc [14]; break;
        case USB_ISOCH_ERROR:
                return  errDesc [15]; break;
        case USB_CANCELED_ERROR:
                return  errDesc [16]; break;
        case USB_NOT_COMPLETE_ERROR:
                return  errDesc [17]; break;
        case USB_CLIENT_BUFFER_ERROR:
                return  errDesc [18]; break;
        case USMC_ERROR_NOT_INITIALIZED:
                return  errDesc [19]; break;
        case USMC_ERROR_REF_DEV_DISCONNECTED:
                return  errDesc [20]; break;
        case LINKRN_ERROR ( ERROR_BAD_ARGUMENTS ):
                return  errDesc [21]; break;
        case USMC_ERROR_NO_DEVICES_CONNECTED:
                return  errDesc [22]; break;*/

        default:
                return  errDesc [0]; break;
        }
}




int clamp ( int val, int min, int max )
{
        return  val > max ? max : ( val < min ? min : val );
}




float clampf ( float val, float min, float max )
{
        return  val > max ? max : ( val < min ? min : val );
}



//      Function: InitDefaultValues ()
//              Initializes structures with default values.
//      Return Value:
//              The InitDefaultValues function has no return values.
void InitDefaultValues ()
{
        int i;

        for ( i = 0 ; i < 32 ; i++ )
        {
                memset ( &g_deviceMode [i], 0, sizeof ( USMC_Mode ) );
                memset ( &g_deviceParameters [i], 0, sizeof ( USMC_Parameters ) );
                memset ( &g_deviceStartParameters [i], 0, sizeof ( USMC_StartParameters ) );

                // USMC_Mode defaults:
                g_deviceMode [i].PReg      = TRUE;
                g_deviceMode [i].Tr1En     = TRUE;
                g_deviceMode [i].Tr2En     = TRUE;
                g_deviceMode [i].RotTrOp   = TRUE;
                g_deviceMode [i].SyncOUTEn = TRUE;
                g_deviceMode [i].SyncINOp  = TRUE;
                g_deviceMode [i].SyncCount = 4;

                // USMC_Parameters defaults:
                g_deviceParameters [i].MaxTemp    = 70.0f;
                g_deviceParameters [i].AccelT     = 200.0f;
                g_deviceParameters [i].DecelT     = 200.0f;
                g_deviceParameters [i].BTimeout1  = 500.0f;
                g_deviceParameters [i].BTimeout2  = 500.0f;
                g_deviceParameters [i].BTimeout3  = 500.0f;
                g_deviceParameters [i].BTimeout4  = 500.0f;
                g_deviceParameters [i].BTO1P      = 200.0f;
                g_deviceParameters [i].BTO2P      = 300.0f;
                g_deviceParameters [i].BTO3P      = 400.0f;
                g_deviceParameters [i].BTO4P      = 500.0f;
                g_deviceParameters [i].MinP       = 500.0f;
                g_deviceParameters [i].BTimeoutR  = 500.0f;
                g_deviceParameters [i].LoftPeriod = 500.0f;
                g_deviceParameters [i].RTDelta    = 200;
                g_deviceParameters [i].RTMinError = 15;
                g_deviceParameters [i].EncMult    = 2.5f;
                g_deviceParameters [i].MaxLoft    = 32;
                g_deviceParameters [i].PTimeout   = 100.0f;
                g_deviceParameters [i].SynOUTP    = 1;
                g_deviceParameters [i].StartPos   = 0;

                // USMC_StartParameters defaults:
                g_deviceStartParameters [i].SDivisor = 8;
                g_deviceStartParameters [i].LoftEn   = TRUE;
                g_deviceStartParameters [i].SlStart  = TRUE;
        }
}




//      Function: IOCTRL ()
//              Encapsulates DeviceIoControl call ( internal use ).
//      Parameters:
//              [in]  Device: device number to call DeviceIoControl on.
//              [in]  dwCode: IO Control code for operation.
//              [out] pInBuf: buffer pointer to store returned data in.
//              [in]  dwInBufSize: size of in buffer.
//              [in]  pOutBuf: buffer pointer to send data from.
//              [in]  dwOutBufSize: size of out buffer.
//      Return Value:
//              DWORD; zero means succes, other value - error code.
//      Remarks:
//              Call this function with Device == -1 when devices wasn't enumerated yet.
//      The IOCTRL () function DOES NOT sets error description. This routine must
//      be completed by caller function to specify most appropriate information: where
//      error has occured and with which operation it associated.
DWORD IOCTRL ( DWORD Device,
                          DWORD dwCode,
                          PVOID pInBuf,
                          DWORD dwInBufSize,
                          PVOID pOutBuf,
                          DWORD dwOutBufSize )
{
        DWORD  deviceNumber;
        char   deviceName [128];        // Device name in form Prefix + DeviceNumber + ":\0".
        DWORD  dwErr;
        int    hFile;
        char * buf;

        dwErr = 0;

        deviceNumber = Device == -1 ? 0 : Device;
        sprintf ( deviceName, "/dev/usmc%d", deviceNumber );

        hFile = open ( deviceName, 0 );

        if ( hFile == -1 )
                return  -USMC_ERROR_REF_DEV_DISCONNECTED;


        if ( pInBuf && dwInBufSize ) {
                buf = pInBuf;
        } else if ( pOutBuf && dwOutBufSize ) {
                buf = pOutBuf;
        }


        dwErr = ioctl ( hFile,
                        dwCode,
                        buf );

        close ( hFile );

        return  dwErr;
}




DWORD USMC_Init ( USMC_Devices * Str )
{
        int nod;
        DWORD i;

        nod = 0;
        Str -> NOD = 0;
        Str -> Serial = Str -> Version = NULL;

        if ( !g_IsInitialized )
        {
                //InitializeCriticalSection ( &g_Lock );
                g_IsInitialized = TRUE;
        }

        //EnterCriticalSection ( &g_Lock );
        DWORD dwRes = IOCTRL ( -1,
                                                  IOCTL_GET_NOD,
                                                  ( PVOID ) &nod,
                                                  1, NULL, 0 );

        if ( ( int ) dwRes < 0 )
                return  USMC_SUCCESS;   // No devices connected, so return NOD = 0.
        else
                Str -> NOD = nod;


        // Enumerate connected devices:
        Str -> Serial  = ( char ** ) malloc ( ( ssize_t ) ( sizeof ( char * ) * 32 ) );
        Str -> Version = ( char ** ) malloc ( ( ssize_t ) ( sizeof ( char * ) * 32 ) );

        for ( i = 0 ; i < Str -> NOD; i++ )
        {
                char * serial  = ( char * ) malloc ( ( ssize_t ) 16 );
                char * version = ( char * ) malloc ( ( ssize_t ) 6 );

                dwRes = IOCTRL ( i,
                                                IOCTL_GET_SERIAL,
                                                ( PVOID ) serial,
                                                16, NULL, 0 );

                if ( ( int ) dwRes < 0 )
                {
                        dwRes = SetLastErr ( "USMC_Init () failed during IOCTRL ( IOCTL_GET_SERIAL ) call", dwRes );
                        free ( serial  );
                        free ( version );
                        Str -> NOD = 0;
                        Str -> Serial = Str -> Version = NULL;

                        return  dwRes;
                }
 

                dwRes = IOCTRL ( i,
                                                IOCTL_GET_VERSION,
                                                ( PVOID ) version,
                                                6, NULL, 0 );

                if ( ( int ) dwRes < 0 )
                {
                        dwRes = SetLastErr ( "USMC_Init () failed during IOCTRL ( IOCTL_GET_VERSION ) call", dwRes );
                        free ( serial  );
                        free ( version );
                        Str -> NOD = 0;
                        Str -> Serial = Str -> Version = NULL;

                        return  dwRes;
                }


                Str -> Serial [i] = serial;

                ( Str -> Version [i] ) = ( char * ) malloc ( ( ssize_t ) 4 );
                memcpy ( Str -> Version [i], version + 2, ( ssize_t ) 4 );

                //strupr ( Str -> Version [i] );
                sscanf ( Str -> Version [i], "%X", &g_devicesVersions [i] );
        }


        g_devices = *Str;
        InitDefaultValues ();

        //LeaveCriticalSection ( &g_Lock );
       

        return  USMC_SUCCESS;
}




DWORD USMC_GetState ( DWORD Device, USMC_State * Str )
{
        if ( !g_IsInitialized )
                return  SetLastErr ( "USMC_GetState () failed", -USMC_ERROR_NOT_INITIALIZED );



        STATE_PACKET getStateData;

        //EnterCriticalSection ( &g_Lock );
        DWORD dwRes = IOCTRL ( Device,
                                                  IOCTL_GET_STATE,
                                                  ( PVOID ) &getStateData,
                                                  11, NULL, 0 );
        //LeaveCriticalSection ( &g_Lock );

        if ( ( int ) dwRes < 0 )
                return  SetLastErr ( "USMC_GetState () failed during IOCTRL ( IOCTL_GET_STATE ) call", dwRes );


        Str -> AReset    = getStateData.AFTRESET;
        Str -> CurPos    = ( ( signed int ) getStateData.CurPos ) / 8;
        Str -> CW_CCW    = getStateData.CW_CCW;
        Str -> EmReset   = getStateData.EMRESET;
        Str -> FullPower = getStateData.REFIN;
        Str -> FullSpeed = getStateData.FULLSPEED;
        Str -> Loft      = getStateData.LOFT;
        Str -> Power     = getStateData.RESET;
        Str -> RotTr     = getStateData.ROTTR;
        Str -> RotTrErr  = getStateData.ROTTRERR;
        Str -> RUN       = getStateData.RUN;
        /*Str -> SDivisor= See below;*/
        Str -> SyncIN    = getStateData.SYNCIN;
        Str -> SyncOUT   = getStateData.SYNCOUT;
        /*Str -> Temp    = See below;*/
        Str -> Trailer1  = getStateData.TRAILER1;
        Str -> Trailer2  = getStateData.TRAILER2;
        /*Str -> Voltage = See below;*/

        Str -> SDivisor  = ( BYTE ) ( 1 << ( getStateData.M2 << 1 | getStateData.M1 ) );
        double t      = ( double ) getStateData.Temp;

        if ( g_devicesVersions [Device] < 0x2400 )
        {
                t = t * 3.3 / 65536.0;
                t = t * 10.0 / ( 5.0 - t );
                t = ( 1.0 / 298.0 ) + ( 1.0 / 3950.0 ) * log ( t / 10.0 );
                t = 1.0 / t - 273.0;
        }
        else
        {
                t = ( ( t * 3.3 * 100.0 / 65536.0 ) - 50.0 );
        }


        Str -> Temp    = ( float ) t;
        Str -> Voltage = ( float ) ( ( ( double ) getStateData.Voltage ) / 65536.0 * 3.3 * 20.0 );
        Str -> Voltage = Str -> Voltage < 5.0f ? 0.0f : Str -> Voltage;


        return  USMC_SUCCESS;
}




DWORD USMC_SaveParametersToFlash ( DWORD Device )
{
        if ( !g_IsInitialized )
                return  SetLastErr ( "USMC_SaveParametersToFlash () failed", -USMC_ERROR_NOT_INITIALIZED );


        //EnterCriticalSection ( &g_Lock );
        DWORD dwRes = IOCTRL ( Device,
                                                  IOCTL_SAVE_PARAMETERS,
                                                  NULL, 0, NULL, 0 );
        //LeaveCriticalSection ( &g_Lock );


        if ( ( int ) dwRes < 0 )
                return  SetLastErr ( "USMC_SaveParametersToFlash () failed during IOCTRL ( IOCTL_SAVE_PARAMETERS ) call", dwRes );



        return  USMC_SUCCESS;
}




DWORD USMC_GetMode ( DWORD Device, USMC_Mode * Str )
{
        if ( !g_IsInitialized )
                return  SetLastErr ( "USMC_GetMode () failed", -USMC_ERROR_NOT_INITIALIZED );


        *Str = g_deviceMode [Device];


        return  USMC_SUCCESS;
}




DWORD USMC_SetMode ( DWORD Device, USMC_Mode * Str )
{
        if ( !g_IsInitialized )
                return  SetLastErr ( "USMC_SetMode () failed", -USMC_ERROR_NOT_INITIALIZED );


        MODE_PACKET setModeData;
        // Byte 0:
        setModeData.PMODE     = Str -> PMode;
        setModeData.REFINEN   = Str -> PReg;
        setModeData.RESETD    = Str -> ResetD;
        setModeData.EMRESET   = Str -> EMReset;
        setModeData.TR1T      = Str -> Tr1T;
        setModeData.TR2T      = Str -> Tr2T;
        setModeData.ROTTRT    = Str -> RotTrT;
        setModeData.TRSWAP    = Str -> TrSwap; 
        // Byte 1:
        setModeData.TR1EN     = Str -> Tr1En;
        setModeData.TR2EN     = Str -> Tr2En;
        setModeData.ROTTREN   = Str -> RotTeEn;
        setModeData.ROTTROP   = Str -> RotTrOp;
        setModeData.BUTT1T    = Str -> Butt1T;
        setModeData.BUTT2T    = Str -> Butt2T;
        /*setModeData.BUTSWAP = ...;*/
        setModeData.RESETRT   = Str -> ResetRT;
        // Byte 2:
        setModeData.SNCOUTEN  = Str -> SyncOUTEn;
        setModeData.SYNCOUTR  = Str -> SyncOUTR;
        setModeData.SYNCINOP  = Str -> SyncINOp;
        setModeData.SYNCOPOL  = Str -> SyncInvert;
        setModeData.ENCODER   = Str -> EncoderEn;
        setModeData.INVENC    = Str -> EncoderInv;
        setModeData.RESBENC   = Str -> ResBEnc;
        setModeData.RESENC    = Str -> ResEnc;

        setModeData.SYNCCOUNT = PACK_DWORD ( Str -> SyncCount );

        // Save parameters within DLL for further USMC_GetMode () calls:
        //EnterCriticalSection ( &g_Lock );
        g_deviceMode [Device] = *Str;

        DWORD dwRes = IOCTRL ( Device,
                                                  IOCTL_SET_MODE,
                                                  NULL, 0,
                                                  ( PVOID ) &setModeData, 7 );
        //LeaveCriticalSection ( &g_Lock );


        if ( ( int ) dwRes < 0 )
                return  SetLastErr ( "USMC_SetMode () failed during IOCTRL ( IOCTL_SET_MODE ) call", dwRes );


        return  USMC_SUCCESS;
}




DWORD USMC_GetParameters ( DWORD Device, USMC_Parameters * Str )
{
        if ( !g_IsInitialized )
                return  SetLastErr ( "USMC_GetParameters () failed", -USMC_ERROR_NOT_INITIALIZED );


        *Str = g_deviceParameters [Device];


        return  USMC_SUCCESS;
}




DWORD USMC_SetParameters ( DWORD Device, USMC_Parameters * Str )
{
        if ( !g_IsInitialized )
                return  SetLastErr ( "USMC_SetParameters () failed", -USMC_ERROR_NOT_INITIALIZED );


        PARAMETERS_PACKET setParametersData;

        /*=====================*/
        /* ----Conversion:---- */
        /*=====================*/
        setParametersData.DELAY1       = ( BYTE ) clamp ( ( int ) ( Str -> AccelT / 98.0f + 0.5f ), 1, 15 );
        setParametersData.DELAY2       = ( BYTE ) clamp ( ( int ) ( Str -> DecelT / 98.0f + 0.5f ), 1, 15 );
        setParametersData.RefINTimeout = ( WORD ) ( clampf ( Str -> PTimeout , 1.0f, 9961.0f ) / 0.152f + 0.5f );
        setParametersData.BTIMEOUT1    = PACK_WORD ( ( WORD ) ( clampf ( Str -> BTimeout1, 1.0f, 9961.0f ) / 0.152f + 0.5f ) );
        setParametersData.BTIMEOUT2    = PACK_WORD ( ( WORD ) ( clampf ( Str -> BTimeout2, 1.0f, 9961.0f ) / 0.152f + 0.5f ) );
        setParametersData.BTIMEOUT3    = PACK_WORD ( ( WORD ) ( clampf ( Str -> BTimeout3, 1.0f, 9961.0f ) / 0.152f + 0.5f ) );
        setParametersData.BTIMEOUT4    = PACK_WORD ( ( WORD ) ( clampf ( Str -> BTimeout4, 1.0f, 9961.0f ) / 0.152f + 0.5f ) );
        setParametersData.BTIMEOUTR    = PACK_WORD ( ( WORD ) ( clampf ( Str -> BTimeoutR, 1.0f, 9961.0f ) / 0.152f + 0.5f ) );
        setParametersData.BTIMEOUTD    = PACK_WORD ( ( WORD ) ( clampf ( Str -> BTimeoutD, 1.0f, 9961.0f ) / 0.152f + 0.5f ) );
        setParametersData.MINPERIOD    = PACK_WORD ( ( WORD ) ( 65536.0f - ( 125000.0f / clampf ( Str -> MinP , 2.0f, 625.0f ) ) + 0.5f ) );
        setParametersData.BTO1P        = PACK_WORD ( ( WORD ) ( 65536.0f - ( 125000.0f / clampf ( Str -> BTO1P, 2.0f, 625.0f ) ) + 0.5f ) );
        setParametersData.BTO2P        = PACK_WORD ( ( WORD ) ( 65536.0f - ( 125000.0f / clampf ( Str -> BTO2P, 2.0f, 625.0f ) ) + 0.5f ) );
        setParametersData.BTO3P        = PACK_WORD ( ( WORD ) ( 65536.0f - ( 125000.0f / clampf ( Str -> BTO3P, 2.0f, 625.0f ) ) + 0.5f ) );
        setParametersData.BTO4P        = PACK_WORD ( ( WORD ) ( 65536.0f - ( 125000.0f / clampf ( Str -> BTO4P, 2.0f, 625.0f ) ) + 0.5f ) );
        setParametersData.MAX_LOFT     = PACK_WORD ( ( WORD ) ( clamp ( Str -> MaxLoft, 1, 1023 ) * 64 ) );

        if ( g_devicesVersions [Device] < 0x2407 )
        {
                setParametersData.STARTPOS = 0x00000000L;
        }
        else
        {
                setParametersData.STARTPOS = PACK_DWORD ( Str -> StartPos * 8 & 0xFFFFFF00 );
        }


        setParametersData.RTDelta    = PACK_WORD ( ( WORD ) ( clamp ( Str -> RTDelta   , 4, 1023 ) * 64 ) );
        setParametersData.RTMinError = PACK_WORD ( ( WORD ) ( clamp ( Str -> RTMinError, 4, 1023 ) * 64 ) );

        double t = ( double ) clampf ( Str -> MaxTemp, 0.0f, 100.0f );

        if ( g_devicesVersions [Device] < 0x2400 )
        {
                t = 10.0 * exp ( 3950.0 * ( 1.0 / ( t + 273.0 ) - 1.0 / 298.0 ) );
                t = ( ( 5 * t / ( 10 + t ) ) * 65536.0 / 3.3 + 0.5 );
        }
        else
        {
                t = ( t + 50.0 ) / 330.0 * 65536.0;
                t = ( t + 0.5f );
        }


        setParametersData.MaxTemp    = PACK_WORD ( ( WORD ) t );
        setParametersData.SynOUTP    = Str -> SynOUTP;
        setParametersData.LoftPeriod = Str -> LoftPeriod == 0.0f ? 0 :
                                                                         PACK_WORD ( ( WORD ) ( 65536.0f - ( 125000.0f / clampf ( Str -> LoftPeriod, 16.0f, 5000.0f ) ) + 0.5f ) );
        setParametersData.EncVSCP    = ( BYTE ) ( Str -> EncMult * 4.0f + 0.5f );
//setParametersData.EncVSCP = 1;        ///////////

        memset ( setParametersData.Reserved, 0, 15 );
        //unsigned char testBuf [sizeof ( PARAMETERS_PACKET )];
        //memcpy ( ( void * ) testBuf, ( const void * ) &setParametersData, ( size_t ) sizeof ( PARAMETERS_PACKET ) );
        /*=======================*/
        /* ----Deconversion:---- */
        /*=======================*/
        Str -> AccelT    = setParametersData.DELAY1 * 98.0f;
        Str -> DecelT    = setParametersData.DELAY2 * 98.0f;
        Str -> PTimeout  = ( float ) ( setParametersData.RefINTimeout * 0.152f );
        Str -> BTimeout1 = ( float ) ( PACK_WORD ( setParametersData.BTIMEOUT1 ) * 0.152f );
        Str -> BTimeout2 = ( float ) ( PACK_WORD ( setParametersData.BTIMEOUT2 ) * 0.152f );
        Str -> BTimeout3 = ( float ) ( PACK_WORD ( setParametersData.BTIMEOUT3 ) * 0.152f );
        Str -> BTimeout4 = ( float ) ( PACK_WORD ( setParametersData.BTIMEOUT4 ) * 0.152f );
        Str -> BTimeoutR = ( float ) ( PACK_WORD ( setParametersData.BTIMEOUTR ) * 0.152f );
        Str -> BTimeoutD = ( float ) ( PACK_WORD ( setParametersData.BTIMEOUTD ) * 0.152f );
        Str -> MinP      = 125000.0f / ( 65536.0f - ( float ) ( PACK_WORD ( setParametersData.MINPERIOD ) ) );
        Str -> BTO1P     = 125000.0f / ( 65536.0f - ( float ) ( PACK_WORD ( setParametersData.BTO1P ) ) );
        Str -> BTO2P     = 125000.0f / ( 65536.0f - ( float ) ( PACK_WORD ( setParametersData.BTO2P ) ) );
        Str -> BTO3P     = 125000.0f / ( 65536.0f - ( float ) ( PACK_WORD ( setParametersData.BTO3P ) ) );
        Str -> BTO4P     = 125000.0f / ( 65536.0f - ( float ) ( PACK_WORD ( setParametersData.BTO4P ) ) );
        Str -> MaxLoft   = PACK_WORD ( setParametersData.MAX_LOFT ) / 64;

        if( g_devicesVersions [Device] < 0x2407 )
        {
                Str -> StartPos = 0x00000000L;
        }
        else
        {
                Str -> StartPos = PACK_DWORD ( setParametersData.STARTPOS ) / 8;
        }


        Str -> RTDelta    = PACK_WORD ( setParametersData.RTDelta )    / 64;
        Str -> RTMinError = PACK_WORD ( setParametersData.RTMinError ) / 64;

        if ( g_devicesVersions [Device] < 0x2400 )
        {
                t = ( double )( PACK_WORD ( setParametersData.MaxTemp ) );
                t = t * 3.3 / 65536.0;
                t = 10.0 * t / ( 5.0 - t );
                t = ( 1.0 / 298.0 ) + ( 1.0 / 3950.0 ) * log ( t / 10.0 );
                t = 1.0 / t - 273.0;
        }
        else
        {
                t = ( double ) ( PACK_WORD ( setParametersData.MaxTemp ) );
                t = ( t * 3.3 * 100.0 / 65536.0 ) - 50.0;
        }


        Str -> MaxTemp = ( float ) t;

        Str -> SynOUTP = setParametersData.SynOUTP;
        Str -> LoftPeriod = PACK_WORD ( setParametersData.LoftPeriod ) == 0 ? 0.0f :
                                         125000.0f / ( 65536.0f - ( float ) ( PACK_WORD ( setParametersData.LoftPeriod ) ) );
        Str -> EncMult = ( ( float ) setParametersData.EncVSCP ) / 4.0f;

        // Save parameters within DLL for further USMC_GetParameters () calls:
        //EnterCriticalSection ( &g_Lock );
        g_deviceParameters [Device] = *Str;

        DWORD dwRes = IOCTRL ( Device,
                                                  IOCTL_SET_PARAMETERS,
                                                  NULL, 0,
                                                  ( PVOID ) &setParametersData, 57 );
        //LeaveCriticalSection ( &g_Lock );


        if ( ( int ) dwRes < 0 )
                return  SetLastErr ( "USMC_SetParameters () failed during IOCTRL ( IOCTL_SET_PARAMETERS ) call", dwRes );


        return  USMC_SUCCESS;
}




DWORD USMC_GetStartParameters ( DWORD Device, USMC_StartParameters * Str )
{
        if ( !g_IsInitialized )
                return  SetLastErr ( "USMC_GetStartParameters () failed", -USMC_ERROR_NOT_INITIALIZED );


        *Str = g_deviceStartParameters [Device];


        return  USMC_SUCCESS;
}




DWORD USMC_Start ( DWORD Device, int DestPos,
                                                           float * Speed,
                                                           USMC_StartParameters * Str )
{
        if ( !g_IsInitialized )
                return  SetLastErr ( "USMC_Start () failed", -USMC_ERROR_NOT_INITIALIZED );


        GO_TO_PACKET goToData;

        /*=====================*/
        /* ----Conversion:---- */
        /*=====================*/
        goToData.DestPos     = ( DWORD ) ( DestPos * 8 );
        goToData.TimerPeriod = PACK_WORD ( ( WORD  ) ( 65536.0f - ( 1000000.0f / clampf ( *Speed, 16.0f, 5000.0f ) ) + 0.5f ) );
        switch ( Str -> SDivisor )
        {
        case 1:
                goToData.M1 = goToData.M2 = 0;
                break;
        case 2:
                goToData.M1 = 1;
                goToData.M2 = 0;
                break;
        case 4:
                goToData.M1 = 0;
                goToData.M2 = 1;
                break;
        case 8:
                goToData.M1 = 1;
                goToData.M2 = 1;
                break;
        }
        //goToData.M1          = Str -> SDivisor && 0x01;
        //goToData.M2          = Str -> SDivisor && 0x02;
        goToData.DEFDIR      = Str -> DefDir;
        goToData.LOFTEN      = Str -> LoftEn;
        goToData.SLSTRT      = Str -> SlStart;
        goToData.WSYNCIN     = Str -> WSyncIN;
        goToData.SYNCOUTR    = Str -> SyncOUTR;
        goToData.FORCELOFT   = Str -> ForceLoft;


        /*=======================*/
        /* ----Deconversion:---- */
        /*=======================*/
        *Speed        = 1000000.0f / ( 65536.0f - ( float ) PACK_WORD ( goToData.TimerPeriod ) );
        Str -> SDivisor = 1 << ( ( goToData.M2 << 1 ) | goToData.M1 );

        // Save start parameters within DLL for further USMC_GetStartParameters () calls:
        //EnterCriticalSection ( &g_Lock );
        g_deviceStartParameters [Device] = *Str;

        DWORD dwRes = IOCTRL ( Device,
                                                  IOCTL_GO_TO,
                                                  NULL, 0,
                                                  ( PVOID ) &goToData, 7 );
        //LeaveCriticalSection ( &g_Lock );


        if ( ( int ) dwRes < 0 )
                return  SetLastErr ( "USMC_Start () failed during IOCTRL ( IOCTL_GO_TO ) call", dwRes );


        return  USMC_SUCCESS;
}




DWORD USMC_Stop ( DWORD Device )
{
        if ( !g_IsInitialized )
                return  SetLastErr ( "USMC_Stop () failed", -USMC_ERROR_NOT_INITIALIZED );


        //EnterCriticalSection ( &g_Lock );
        DWORD dwRes = IOCTRL ( Device,
                                                  IOCTL_STOP_STEP_MOTOR,
                                                  NULL, 0, NULL, 0 );
        //LeaveCriticalSection ( &g_Lock );


        if ( ( int ) dwRes < 0 )
                return  SetLastErr ( "USMC_Stop () failed during IOCTRL ( IOCTL_STOP_STEP_MOTOR ) call", dwRes );


        return  USMC_SUCCESS;
}




DWORD USMC_SetCurrentPosition ( DWORD Device, int Position )
{
        if ( !g_IsInitialized )
                return  SetLastErr ( "USMC_SetCurrentPosition () failed", -USMC_ERROR_NOT_INITIALIZED );


        Position *= 8;
        Position &= 0xFFFFFFE0;
        //EnterCriticalSection ( &g_Lock );
        DWORD dwRes = IOCTRL ( Device,
                                                  IOCTL_SET_CURRENT_POSITION,
                                                  NULL, 0,
                                                  ( PVOID ) &Position, 4 );
        //LeaveCriticalSection ( &g_Lock );


        if ( ( int ) dwRes < 0 )
                return  SetLastErr ( "USMC_SetCurrentPosition () failed during IOCTRL ( IOCTL_SET_CURRENT_POSITION ) call", dwRes );


        return  USMC_SUCCESS;
}




DWORD USMC_GetEncoderState ( DWORD Device, USMC_EncoderState * Str )
{
        if ( !g_IsInitialized )
                return  SetLastErr ( "USMC_GetEncoderState () failed", -USMC_ERROR_NOT_INITIALIZED );


        if ( g_devicesVersions [Device] < 0x2410 )
                return  USMC_SUCCESS;


        //EnterCriticalSection ( &g_Lock );
        ENCODER_STATE_PACKET getEncoderStateData;

        DWORD dwRes = IOCTRL ( Device,
                                                  IOCTL_GET_ENCODER_STATE,
                                                  ( PVOID ) &getEncoderStateData,
                                                  8, NULL, 0 );
        //LeaveCriticalSection ( &g_Lock );


        if ( ( int ) dwRes < 0 )
                return  SetLastErr ( "USMC_GetEncoderState () failed during IOCTRL ( IOCTL_GET_STATE ) call", dwRes );


        Str -> ECurPos    = getEncoderStateData.ECurPos;
        Str -> EncoderPos = getEncoderStateData.EncPos;


        return  USMC_SUCCESS;
}




void USMC_GetLastErr ( char * str, ssize_t len )
{
        //EnterCriticalSection ( &g_Lock );
        if ( strlen ( g_lastErrDesc ) > len )
                memcpy ( str, g_lastErrDesc, len );
        else
                strcpy ( str, ( const char * ) g_lastErrDesc );
        //LeaveCriticalSection ( &g_Lock );
}




DWORD USMC_Close ( void )
{
        if ( !g_IsInitialized )
                return  SetLastErr ( "USMC_Close () failed", -USMC_ERROR_NOT_INITIALIZED );


        return  USMC_SUCCESS;
}