WebSVN - f9daq - Rev 296 - /8SMC1-USBhF/libusmc/libusmc.c

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

Subversion Repositories f9daq

(root)/8SMC1-USBhF/libusmc/libusmc.c - Rev 296