Subversion Repositories f9daq

#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;