Go to most recent revision | Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 8 | f9daq | 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 | } |