SPI-FbDrvMonarco
5-production-stableFB Driver for SPI based Monarco HAT
Brought to you by:
aliazzz
Unlicense
Diff of /trunk/2.0.0.1/+CTRLBYTE/Monarco HAT library for codesys 2.0.0.1.library.md [000000] .. [r4] Maximize Restore
Switch to unified view
| a | b/trunk/2.0.0.1/+CTRLBYTE/Monarco HAT library for codesys 2.0.0.1.library.md | ||
|---|---|---|---|
| 1 | <?xml version="1.0" encoding="utf-8"?>--- |
||
| 2 | |||
| 3 | ~~~ST |
||
| 4 | FUNCTION_BLOCK IoDrvMonarco EXTENDS IoDrvBase |
||
| 5 | VAR |
||
| 6 | _IIoDrv: ICmpIoDrv; |
||
| 7 | _IIoDrvParameter: ICmpIoDrvParameter; |
||
| 8 | Monarco: MonarcoItf; |
||
| 9 | _dwInUse: DWORD; |
||
| 10 | _xBackGroundDiagStarted: BOOL; |
||
| 11 | _bDeactivated: BOOL; |
||
| 12 | END_VAR |
||
| 13 | |||
| 14 | ~~~ |
||
| 15 | ~~~ST |
||
| 16 | |||
| 17 | ~~~ |
||
| 18 | --- |
||
| 19 | ~~~ST |
||
| 20 | METHOD FB_Init: BOOL |
||
| 21 | VAR_INPUT |
||
| 22 | bInitRetains: BOOL; |
||
| 23 | bInCopyCode: BOOL; |
||
| 24 | END_VAR |
||
| 25 | VAR |
||
| 26 | Result: UDINT; |
||
| 27 | CLASSID_CCmpIoDrvTemplate: DWORD; |
||
| 28 | END_VAR |
||
| 29 | |||
| 30 | ~~~ |
||
| 31 | ~~~ST |
||
| 32 | FB_Init_Count := FB_Init_Count + 1; |
||
| 33 | |||
| 34 | m_Info.szDriverName := 'IoDrvMonarco'; |
||
| 35 | m_Info.szVendorName := 'Monarcio.IO'; |
||
| 36 | m_Info.szDeviceName := 'Monarco HAT'; |
||
| 37 | m_Info.wModuleType := 501; |
||
| 38 | |||
| 39 | _IIoDrv := THIS^; |
||
| 40 | _IIoDrvParameter := THIS^; |
||
| 41 | |||
| 42 | m_IBaseItf := THIS^; |
||
| 43 | m_hInterface := IoMgrRegisterInstance2(dwClassId:=CLASSID_CCmpIoDrvTemplate, pItf:=m_IBaseItf, pResult:=ADR(Result)); |
||
| 44 | FB_Init := TRUE; |
||
| 45 | ~~~ |
||
| 46 | --- |
||
| 47 | ~~~ST |
||
| 48 | METHOD FB_Exit: BOOL |
||
| 49 | VAR_INPUT |
||
| 50 | bInCopyCode: BOOL; |
||
| 51 | END_VAR |
||
| 52 | |||
| 53 | ~~~ |
||
| 54 | ~~~ST |
||
| 55 | FB_Exit_Count := FB_Exit_Count + 1; |
||
| 56 | FB_Exit := TRUE; |
||
| 57 | |||
| 58 | ~~~ |
||
| 59 | --- |
||
| 60 | ~~~ST |
||
| 61 | METHOD FB_Reinit: BOOL |
||
| 62 | |||
| 63 | ~~~ |
||
| 64 | ~~~ST |
||
| 65 | FB_Reinit_Count := FB_Reinit_Count + 1; |
||
| 66 | FB_Reinit := TRUE; |
||
| 67 | ~~~ |
||
| 68 | --- |
||
| 69 | ~~~ST |
||
| 70 | METHOD QueryInterface: pointer |
||
| 71 | VAR_INPUT |
||
| 72 | iid: DWORD; |
||
| 73 | pResult: pointer; |
||
| 74 | END_VAR |
||
| 75 | |||
| 76 | ~~~ |
||
| 77 | ~~~ST |
||
| 78 | QueryInterface_Count := QueryInterface_Count + 1; |
||
| 79 | IF iid = ITFID_ICmpIoDrv THEN |
||
| 80 | QueryInterface := ADR(_IIoDrv); |
||
| 81 | AddRef(); |
||
| 82 | IF (pResult <> 0) THEN |
||
| 83 | pResult^ := Errors.ERR_OK; |
||
| 84 | END_IF |
||
| 85 | RETURN; |
||
| 86 | ELSIF iid = ITFID_ICmpIoDrvParameter THEN |
||
| 87 | QueryInterface := ADR(_IIoDrvParameter); |
||
| 88 | AddRef(); |
||
| 89 | IF (pResult <> 0) THEN |
||
| 90 | pResult^ := Errors.ERR_OK; |
||
| 91 | END_IF |
||
| 92 | RETURN; |
||
| 93 | ELSE |
||
| 94 | QueryInterface := SUPER^.QueryInterface(iid, pResult); |
||
| 95 | END_IF |
||
| 96 | |||
| 97 | ~~~ |
||
| 98 | --- |
||
| 99 | ~~~ST |
||
| 100 | METHOD Initialize: UDINT |
||
| 101 | VAR_INPUT |
||
| 102 | wModuleType: UINT; |
||
| 103 | dwInstance: UDINT; |
||
| 104 | pConnector: pointer; |
||
| 105 | END_VAR |
||
| 106 | |||
| 107 | ~~~ |
||
| 108 | ~~~ST |
||
| 109 | Initialize_Count := Initialize_Count + 1; |
||
| 110 | |||
| 111 | m_wModuleType := wModuleType; |
||
| 112 | m_dwInstance := dwInstance; |
||
| 113 | m_Info.wModuleType := wModuleType; |
||
| 114 | |||
| 115 | Monarco.Initialize(wModuleType, dwInstance, pConnector); |
||
| 116 | |||
| 117 | Initialize := Errors.ERR_OK; |
||
| 118 | ~~~ |
||
| 119 | --- |
||
| 120 | ~~~ST |
||
| 121 | METHOD IoDrvReadParameter: UDINT |
||
| 122 | VAR_INPUT |
||
| 123 | pConnector: pointer; |
||
| 124 | pParameter: pointer; |
||
| 125 | pData: pointer; |
||
| 126 | dwBitSize: DWORD; |
||
| 127 | dwBitOffset: DWORD; |
||
| 128 | END_VAR |
||
| 129 | |||
| 130 | ~~~ |
||
| 131 | ~~~ST |
||
| 132 | IF pConnector = 0 OR pParameter = 0 OR pData = 0 THEN |
||
| 133 | IoDrvReadParameter := Errors.ERR_PARAMETER; |
||
| 134 | RETURN; |
||
| 135 | END_IF |
||
| 136 | |||
| 137 | // All standard parameters of our device are handled by the IO-manager! |
||
| 138 | IoDrvReadParameter := Errors.ERR_FAILED; |
||
| 139 | |||
| 140 | ~~~ |
||
| 141 | --- |
||
| 142 | ~~~ST |
||
| 143 | METHOD IoDrvWriteParameter: UDINT |
||
| 144 | VAR_INPUT |
||
| 145 | pConnector: pointer; |
||
| 146 | pParameter: pointer; |
||
| 147 | pData: pointer; |
||
| 148 | dwBitSize: DWORD; |
||
| 149 | dwBitOffset: DWORD; |
||
| 150 | END_VAR |
||
| 151 | VAR |
||
| 152 | pParam: pointer; |
||
| 153 | pstDiagString: pointer; |
||
| 154 | END_VAR |
||
| 155 | |||
| 156 | ~~~ |
||
| 157 | ~~~ST |
||
| 158 | IF pConnector = 0 OR pParameter = 0 OR pData = 0 THEN |
||
| 159 | IoDrvWriteParameter := Errors.ERR_PARAMETER; |
||
| 160 | RETURN; |
||
| 161 | END_IF |
||
| 162 | |||
| 163 | IoDrvWriteParameter := Errors.ERR_FAILED; |
||
| 164 | ~~~ |
||
| 165 | --- |
||
| 166 | ~~~ST |
||
| 167 | METHOD IoDrvGetConnector: pointer |
||
| 168 | VAR_INPUT |
||
| 169 | pResult: pointer; |
||
| 170 | END_VAR |
||
| 171 | |||
| 172 | ~~~ |
||
| 173 | ~~~ST |
||
| 174 | IF m_pConnector = 0 THEN |
||
| 175 | IF pResult <> 0 THEN |
||
| 176 | pResult^ := Errors.ERR_FAILED; |
||
| 177 | END_IF |
||
| 178 | IoDrvGetConnector := 0; |
||
| 179 | RETURN; |
||
| 180 | END_IF |
||
| 181 | IF pResult <> 0 THEN |
||
| 182 | pResult^ := Errors.ERR_OK; |
||
| 183 | END_IF |
||
| 184 | IoDrvGetConnector := m_pConnector; |
||
| 185 | |||
| 186 | |||
| 187 | ~~~ |
||
| 188 | --- |
||
| 189 | ~~~ST |
||
| 190 | METHOD IoDrvIdentify: UDINT |
||
| 191 | VAR_INPUT |
||
| 192 | pConnector: pointer; |
||
| 193 | END_VAR |
||
| 194 | |||
| 195 | ~~~ |
||
| 196 | ~~~ST |
||
| 197 | IoDrvIdentify_Count := IoDrvIdentify_Count + 1; |
||
| 198 | IoDrvIdentify := Errors.ERR_NOTIMPLEMENTED; |
||
| 199 | ~~~ |
||
| 200 | --- |
||
| 201 | ~~~ST |
||
| 202 | METHOD IoDrvWatchdogTrigger: UDINT |
||
| 203 | VAR_INPUT |
||
| 204 | pConnector: pointer; |
||
| 205 | END_VAR |
||
| 206 | |||
| 207 | ~~~ |
||
| 208 | ~~~ST |
||
| 209 | IoDrvWatchdogTrigger_Count := IoDrvWatchdogTrigger_Count + 1; |
||
| 210 | IoDrvWatchdogTrigger := Errors.ERR_OK; |
||
| 211 | ~~~ |
||
| 212 | --- |
||
| 213 | ~~~ST |
||
| 214 | METHOD IoDrvGetModuleDiagnosis: UDINT |
||
| 215 | VAR_INPUT |
||
| 216 | pConnector: pointer; |
||
| 217 | END_VAR |
||
| 218 | |||
| 219 | ~~~ |
||
| 220 | ~~~ST |
||
| 221 | IF pConnector = 0 THEN |
||
| 222 | //we are called from the background task |
||
| 223 | _xBackGroundDiagStarted := TRUE; |
||
| 224 | END_IF |
||
| 225 | |||
| 226 | //Todo: update connectorflags if something changed |
||
| 227 | |||
| 228 | IoDrvGetModuleDiagnosis_Count := IoDrvGetModuleDiagnosis_Count + 1; |
||
| 229 | IoDrvGetModuleDiagnosis := Errors.ERR_NOTIMPLEMENTED; |
||
| 230 | ~~~ |
||
| 231 | --- |
||
| 232 | ~~~ST |
||
| 233 | METHOD IoDrvWriteOutputs: UDINT |
||
| 234 | VAR_INPUT |
||
| 235 | pConnectorMapList: pointer; |
||
| 236 | nCount: DINT; |
||
| 237 | END_VAR |
||
| 238 | VAR |
||
| 239 | i: DINT; |
||
| 240 | j: DINT; |
||
| 241 | wSize: WORD; |
||
| 242 | pbyIecAddress: pointer; |
||
| 243 | bySrcValue: BYTE; |
||
| 244 | wSrcIndex: WORD; |
||
| 245 | bySrcMask: BYTE; |
||
| 246 | wDestIndex: WORD; |
||
| 247 | ptr: pointer; |
||
| 248 | ptrW: pointer; |
||
| 249 | ptrB: pointer; |
||
| 250 | END_VAR |
||
| 251 | |||
| 252 | ~~~ |
||
| 253 | ~~~ST |
||
| 254 | IoDrvWriteOutputs_Count := IoDrvWriteOutputs_Count + 1; |
||
| 255 | |||
| 256 | IF pConnectorMapList = 0 OR nCount = 0 THEN |
||
| 257 | IoDrvWriteOutputs := Errors.ERR_PARAMETER; |
||
| 258 | RETURN; |
||
| 259 | END_IF |
||
| 260 | |||
| 261 | FOR i:=0 TO nCount - 1 DO |
||
| 262 | IF (pConnectorMapList[i].dwNumOfChannels = 0) THEN |
||
| 263 | CONTINUE; |
||
| 264 | END_IF |
||
| 265 | FOR j:= 0 TO UDINT_TO_UINT(pConnectorMapList[i].dwNumOfChannels) - 1 DO |
||
| 266 | wSrcIndex := pConnectorMapList[i].pChannelMapList[j].wIecAddressBitOffset / 8; |
||
| 267 | pbyIecAddress := pConnectorMapList[i].pChannelMapList[j].pbyIecAddress; |
||
| 268 | |||
| 269 | IF (pConnectorMapList[i].pChannelMapList[j].pParameter^.dwParameterId >= 2000 AND pConnectorMapList[i].pChannelMapList[j].pParameter^.dwParameterId<=2002) |
||
| 270 | OR (pConnectorMapList[i].pChannelMapList[j].pParameter^.dwParameterId >= 9010 AND pConnectorMapList[i].pChannelMapList[j].pParameter^.dwParameterId<=9016) |
||
| 271 | OR (pConnectorMapList[i].pChannelMapList[j].pParameter^.dwParameterId >= 9030 AND pConnectorMapList[i].pChannelMapList[j].pParameter^.dwParameterId<=9031) THEN |
||
| 272 | IF pConnectorMapList[i].pChannelMapList[j].wSize = 1 THEN |
||
| 273 | bySrcValue := pbyIecAddress[wSrcIndex]; |
||
| 274 | |||
| 275 | IF (pbyIecAddress[wSrcIndex] AND SHL(BYTE#1, pConnectorMapList[i].pChannelMapList[j].wIecAddressBitOffset MOD 8)) <> 0 THEN |
||
| 276 | {IF defined (pou:SysCpuSetBit2)} |
||
| 277 | SysCpuSetBit2(pConnectorMapList[i].pChannelMapList[j].pParameter^.dwDriverSpecific, pConnectorMapList[i].pChannelMapList[j].wParameterBitOffset MOD 8); |
||
| 278 | {ELSE} |
||
| 279 | SysCpuSetBit(pConnectorMapList[i].pChannelMapList[j].pParameter^.dwDriverSpecific, pConnectorMapList[i].pChannelMapList[j].wParameterBitOffset MOD 8); |
||
| 280 | {END_IF} |
||
| 281 | ELSE |
||
| 282 | {IF defined (pou:SysCpuResetBit2)} |
||
| 283 | SysCpuResetBit2(pConnectorMapList[i].pChannelMapList[j].pParameter^.dwDriverSpecific, pConnectorMapList[i].pChannelMapList[j].wParameterBitOffset MOD 8); |
||
| 284 | {ELSE} |
||
| 285 | SysCpuResetBit(pConnectorMapList[i].pChannelMapList[j].pParameter^.dwDriverSpecific, pConnectorMapList[i].pChannelMapList[j].wParameterBitOffset MOD 8); |
||
| 286 | {END_IF} |
||
| 287 | END_IF |
||
| 288 | ELSIF pConnectorMapList[i].pChannelMapList[j].wSize = 8 THEN |
||
| 289 | ptr:= pConnectorMapList[i].pChannelMapList[j].pParameter^.dwDriverSpecific; |
||
| 290 | ptr^ := pbyIecAddress[wSrcIndex]; |
||
| 291 | END_IF |
||
| 292 | ELSIF pConnectorMapList[i].pChannelMapList[j].pParameter^.dwParameterId = 2100 THEN |
||
| 293 | IF pConnectorMapList[i].pChannelMapList[j].wSize = 16 THEN |
||
| 294 | ptrW:= pbyIecAddress; |
||
| 295 | Monarco.wAOut1 := ptrW^; |
||
| 296 | END_IF |
||
| 297 | ELSIF pConnectorMapList[i].pChannelMapList[j].pParameter^.dwParameterId = 2101 THEN |
||
| 298 | IF pConnectorMapList[i].pChannelMapList[j].wSize = 16 THEN |
||
| 299 | ptrW:= pbyIecAddress; |
||
| 300 | Monarco.wAOut2 := ptrW^; |
||
| 301 | END_IF |
||
| 302 | END_IF |
||
| 303 | END_FOR |
||
| 304 | END_FOR |
||
| 305 | |||
| 306 | IoDrvWriteOutputs := Errors.ERR_OK; |
||
| 307 | ~~~ |
||
| 308 | --- |
||
| 309 | ~~~ST |
||
| 310 | METHOD IoDrvUpdateMapping: UDINT |
||
| 311 | VAR_INPUT |
||
| 312 | pTaskMapList: pointer; |
||
| 313 | nCount: DINT; |
||
| 314 | END_VAR |
||
| 315 | |||
| 316 | ~~~ |
||
| 317 | ~~~ST |
||
| 318 | IoDrvUpdateMapping_Count := IoDrvUpdateMapping_Count + 1; |
||
| 319 | |||
| 320 | IF (pTaskMapList = 0) THEN |
||
| 321 | IoDrvUpdateMapping := Errors.ERR_PARAMETER; |
||
| 322 | RETURN; |
||
| 323 | END_IF |
||
| 324 | |||
| 325 | IoDrvUpdateMapping := Errors.ERR_OK; |
||
| 326 | ~~~ |
||
| 327 | --- |
||
| 328 | ~~~ST |
||
| 329 | METHOD IoDrvUpdateConfiguration: UDINT |
||
| 330 | VAR_INPUT |
||
| 331 | pConnectorList: pointer; |
||
| 332 | nCount: DINT; |
||
| 333 | END_VAR |
||
| 334 | VAR |
||
| 335 | pParameter: pointer; |
||
| 336 | pChild: pointer; |
||
| 337 | pstConnectorVendorName: pointer; |
||
| 338 | pstConnectorDeviceName: pointer; |
||
| 339 | dwTest: pointer; |
||
| 340 | Result: RTS_IEC_RESULT; |
||
| 341 | END_VAR |
||
| 342 | |||
| 343 | ~~~ |
||
| 344 | ~~~ST |
||
| 345 | IoDrvUpdateConfiguration_Count := IoDrvUpdateConfiguration_Count + 1; |
||
| 346 | IoDrvUpdateConfiguration := Errors.ERR_OK; |
||
| 347 | |||
| 348 | IF (pConnectorList = 0) THEN |
||
| 349 | // Reset application |
||
| 350 | // TODO: Free resources |
||
| 351 | RETURN; |
||
| 352 | END_IF |
||
| 353 | |||
| 354 | m_pConnector := IoMgrConfigGetConnector(pConnectorList, ADR(nCount), m_wModuleType, m_dwInstance); |
||
| 355 | |||
| 356 | IF m_pConnector = 0 THEN |
||
| 357 | IoDrvUpdateConfiguration := ERRORS.ERR_PARAMETER; |
||
| 358 | RETURN; |
||
| 359 | END_IF |
||
| 360 | |||
| 361 | //check if device is enabled in the device tree |
||
| 362 | IF (m_pConnector^.dwFlags AND ConnectorFlags.CF_ENABLE) = 0 THEN |
||
| 363 | _bDeactivated := TRUE; |
||
| 364 | IoDrvUpdateConfiguration := Errors.ERR_OK; |
||
| 365 | RETURN; |
||
| 366 | END_IF |
||
| 367 | |||
| 368 | IF m_pConnector^.hIoDrv = 0 THEN |
||
| 369 | m_pConnector^.hIoDrv := m_hInterface; |
||
| 370 | m_pConnector^.pFather^.hIoDrv := m_hInterface; |
||
| 371 | |||
| 372 | {IF defined (pou:IoMgrConfigSetDiagnosis)} |
||
| 373 | IoMgrConfigSetDiagnosis(m_pConnector, |
||
| 374 | ConnectorFlags.CF_DRIVER_AVAILABLE OR |
||
| 375 | ConnectorFlags.CF_CONNECTOR_FOUND OR |
||
| 376 | ConnectorFlags.CF_CONNECTOR_CONFIGURED OR |
||
| 377 | ConnectorFlags.CF_CONNECTOR_ACTIVE); |
||
| 378 | {END_IF} |
||
| 379 | |||
| 380 | {IF defined (pou:IoMgrConfigSetDiagnosis)} |
||
| 381 | IoMgrConfigSetDiagnosis(m_pConnector^.pFather, |
||
| 382 | ConnectorFlags.CF_DRIVER_AVAILABLE OR |
||
| 383 | ConnectorFlags.CF_CONNECTOR_FOUND OR |
||
| 384 | ConnectorFlags.CF_CONNECTOR_CONFIGURED OR |
||
| 385 | ConnectorFlags.CF_CONNECTOR_ACTIVE); |
||
| 386 | {END_IF} |
||
| 387 | |||
| 388 | {IF defined (pou:IoMgrSetDriverProperties)} |
||
| 389 | IoMgrSetDriverProperties(m_hInterface, DRVPROP_CONSISTENCY OR DRVPROP_BACKGROUND_GETDIAG); |
||
| 390 | //Note: background diagnosis property flag is optional (supported with 3.5.1.0 runtime) |
||
| 391 | {END_IF} |
||
| 392 | |||
| 393 | //Setup I/O area |
||
| 394 | pParameter := IoMgrConfigGetParameter(m_pConnector, 2000); (* outputs *) |
||
| 395 | IF (pParameter <> 0) THEN |
||
| 396 | pParameter^.dwDriverSpecific := ADR(Monarco.byDOut); |
||
| 397 | END_IF |
||
| 398 | |||
| 399 | |||
| 400 | //Go through all childs of the device |
||
| 401 | pChild := IoMgrConfigGetFirstChild(m_pConnector, ADR(nCount), m_pConnector); |
||
| 402 | WHILE (pChild <> 0) DO |
||
| 403 | IF (pChild^.dwFlags AND ConnectorFlags.CF_ENABLE) <> 0 THEN |
||
| 404 | pChild^.hIoDrv := m_hInterface; |
||
| 405 | {IF defined (pou:IoMgrConfigSetDiagnosis)} |
||
| 406 | IoMgrConfigSetDiagnosis(pChild, |
||
| 407 | ConnectorFlags.CF_DRIVER_AVAILABLE OR |
||
| 408 | ConnectorFlags.CF_CONNECTOR_FOUND OR |
||
| 409 | ConnectorFlags.CF_CONNECTOR_CONFIGURED OR |
||
| 410 | ConnectorFlags.CF_CONNECTOR_ACTIVE); |
||
| 411 | {END_IF} |
||
| 412 | END_IF |
||
| 413 | pChild := IoMgrConfigGetNextChild(pChild, ADR(nCount), m_pConnector); |
||
| 414 | END_WHILE |
||
| 415 | |||
| 416 | END_IF |
||
| 417 | ~~~ |
||
| 418 | --- |
||
| 419 | ~~~ST |
||
| 420 | METHOD IoDrvStartBusCycle: UDINT |
||
| 421 | VAR_INPUT |
||
| 422 | pConnector: pointer; |
||
| 423 | END_VAR |
||
| 424 | |||
| 425 | ~~~ |
||
| 426 | ~~~ST |
||
| 427 | IoDrvStartBusCycle_Count := IoDrvStartBusCycle_Count + 1; |
||
| 428 | |||
| 429 | //if background diagnosis is not active call IoDrvGetModuleDiagnosis; normally it will be called by the runtime if DRVPROP_BACKGROUND_GETDIAG is set |
||
| 430 | IF NOT _xBackGroundDiagStarted THEN |
||
| 431 | IoDrvGetModuleDiagnosis(m_pConnector); //DRVPROP_BACKGROUND_GETDIAG not set or runtime version < V3.5.1.0 |
||
| 432 | END_IF |
||
| 433 | |||
| 434 | //optional: call IoDrvWatchdogTrigger or set driver property DRVPROP_WATCHDOG in UpdateConfiguration |
||
| 435 | IoDrvWatchdogTrigger(pConnector); |
||
| 436 | |||
| 437 | IF NOT _bDeactivated THEN |
||
| 438 | Monarco.AfterReadInputs(); |
||
| 439 | Monarco.BeforeWriteOutputs(); |
||
| 440 | END_IF |
||
| 441 | |||
| 442 | IoDrvStartBusCycle := Errors.ERR_OK; |
||
| 443 | ~~~ |
||
| 444 | --- |
||
| 445 | ~~~ST |
||
| 446 | METHOD IoDrvScanModules: UDINT |
||
| 447 | VAR_INPUT |
||
| 448 | pConnector: pointer; |
||
| 449 | ppConnectorList: pointer; |
||
| 450 | pnCount: pointer; |
||
| 451 | END_VAR |
||
| 452 | |||
| 453 | ~~~ |
||
| 454 | ~~~ST |
||
| 455 | IoDrvScanModules_Count := IoDrvScanModules_Count + 1; |
||
| 456 | IoDrvScanModules := Errors.ERR_NOTIMPLEMENTED; |
||
| 457 | ~~~ |
||
| 458 | --- |
||
| 459 | ~~~ST |
||
| 460 | METHOD IoDrvReadInputs: UDINT |
||
| 461 | VAR_INPUT |
||
| 462 | pConnectorMapList: pointer; |
||
| 463 | nCount: DINT; |
||
| 464 | END_VAR |
||
| 465 | VAR |
||
| 466 | i: DINT; |
||
| 467 | j: DINT; |
||
| 468 | wSize: WORD; |
||
| 469 | pbyIecAddress: pointer; |
||
| 470 | pwIecAddress: pointer; |
||
| 471 | prIecAddress: pointer; |
||
| 472 | prPiXtendSrc: pointer; |
||
| 473 | bySrcValue: BYTE; |
||
| 474 | bySrcMask: BYTE; |
||
| 475 | wSrcIndex: WORD; |
||
| 476 | wDestIndex: WORD; |
||
| 477 | END_VAR |
||
| 478 | |||
| 479 | ~~~ |
||
| 480 | ~~~ST |
||
| 481 | IoDrvReadInputs_Count := IoDrvReadInputs_Count + 1; |
||
| 482 | |||
| 483 | IF pConnectorMapList = 0 OR nCount = 0 THEN |
||
| 484 | IoDrvReadInputs := Errors.ERR_PARAMETER; |
||
| 485 | RETURN; |
||
| 486 | END_IF |
||
| 487 | |||
| 488 | FOR i:=0 TO nCount - 1 DO |
||
| 489 | IF (pConnectorMapList[i].dwNumOfChannels = 0) THEN |
||
| 490 | CONTINUE; |
||
| 491 | END_IF |
||
| 492 | |||
| 493 | FOR j:= 0 TO UDINT_TO_UINT(pConnectorMapList[i].dwNumOfChannels) - 1 DO |
||
| 494 | IF pConnectorMapList[i].pChannelMapList[j].pParameter^.dwParameterId =1000 THEN |
||
| 495 | |||
| 496 | pbyIecAddress := pConnectorMapList[i].pChannelMapList[j].pbyIecAddress; |
||
| 497 | wDestIndex := pConnectorMapList[i].pChannelMapList[j].wIecAddressBitOffset / 8; |
||
| 498 | |||
| 499 | IF (pConnectorMapList[i].pChannelMapList[j].wSize = 1) THEN |
||
| 500 | IF (Monarco.byDIn AND SHL(BYTE#1, pConnectorMapList[i].pChannelMapList[j].wParameterBitOffset MOD 8)) <> 0 THEN |
||
| 501 | {IF defined (pou:SysCpuSetBit2)} |
||
| 502 | SysCpuSetBit2(ADR(pbyIecAddress[wDestIndex]), pConnectorMapList[i].pChannelMapList[j].wIecAddressBitOffset MOD 8); |
||
| 503 | {ELSE} |
||
| 504 | SysCpuSetBit(ADR(pbyIecAddress[wDestIndex]), pConnectorMapList[i].pChannelMapList[j].wIecAddressBitOffset MOD 8); |
||
| 505 | {END_IF} |
||
| 506 | ELSE |
||
| 507 | {IF defined (pou:SysCpuResetBit2)} |
||
| 508 | SysCpuResetBit2(ADR(pbyIecAddress[wDestIndex]), pConnectorMapList[i].pChannelMapList[j].wIecAddressBitOffset MOD 8); |
||
| 509 | {ELSE} |
||
| 510 | SysCpuResetBit(ADR(pbyIecAddress[wDestIndex]), pConnectorMapList[i].pChannelMapList[j].wIecAddressBitOffset MOD 8); |
||
| 511 | {END_IF} |
||
| 512 | END_IF |
||
| 513 | ELSIF pConnectorMapList[i].pChannelMapList[j].wSize = 8 THEN |
||
| 514 | pbyIecAddress[wDestIndex] := Monarco.byDIn; |
||
| 515 | END_IF |
||
| 516 | |||
| 517 | ELSIF pConnectorMapList[i].pChannelMapList[j].pParameter^.dwParameterId = 1110 THEN |
||
| 518 | pwIecAddress := pConnectorMapList[i].pChannelMapList[j].pbyIecAddress; |
||
| 519 | pwIecAddress^:= Monarco.wAIn1; |
||
| 520 | ELSIF pConnectorMapList[i].pChannelMapList[j].pParameter^.dwParameterId = 1111 THEN |
||
| 521 | pwIecAddress := pConnectorMapList[i].pChannelMapList[j].pbyIecAddress; |
||
| 522 | pwIecAddress^:= Monarco.wAIn2; |
||
| 523 | END_IF |
||
| 524 | END_FOR |
||
| 525 | END_FOR |
||
| 526 | |||
| 527 | IoDrvReadInputs := Errors.ERR_OK; |
||
| 528 | ~~~ |
||
| 529 | --- |
||
| 530 | |||
| 531 | ~~~ST |
||
| 532 | FUNCTION_BLOCK MonarcoItf EXTENDS SPI |
||
| 533 | VAR_INPUT |
||
| 534 | byDOut: BYTE; |
||
| 535 | wAOut1: WORD; |
||
| 536 | wAOut2: WORD; |
||
| 537 | END_VAR |
||
| 538 | VAR_OUTPUT |
||
| 539 | byDIn: BYTE; |
||
| 540 | wAIn1: WORD; |
||
| 541 | wAIn2: WORD; |
||
| 542 | END_VAR |
||
| 543 | VAR |
||
| 544 | _xInit: BOOL; |
||
| 545 | stHwConfig: stHWConfig1; |
||
| 546 | stAOut: stAOut; |
||
| 547 | stDOut: stDOut; |
||
| 548 | byLeds: BYTE; |
||
| 549 | udiPWM1freq: UDINT; |
||
| 550 | rDutyCycleDOut1: REAL; |
||
| 551 | rDutyCycleDOut2: REAL; |
||
| 552 | rDutyCycleDOut3: REAL; |
||
| 553 | udiPWM2freq: UDINT; |
||
| 554 | rDutyCycleDOut4: REAL; |
||
| 555 | xOperational: BOOL; |
||
| 556 | stDIn: stDIn; |
||
| 557 | stAIn: stAIn; |
||
| 558 | stCounter1Config: stCounter1Config; |
||
| 559 | stCounter2Config: stCounter2Config; |
||
| 560 | stPWM1: stPWM1; |
||
| 561 | stPWM2: stPWM2; |
||
| 562 | stHATinfo: stHATinfo; |
||
| 563 | eComStatus: eMonarcoComStatus; |
||
| 564 | _uiNumberOfDevices: INT; |
||
| 565 | _stControlByte: stControlByte; |
||
| 566 | _ControlByte8: BYTE; |
||
| 567 | _uiWatchDog: UINT; |
||
| 568 | _uiLastWatchDog: UINT; |
||
| 569 | _stRS485Mode: stRS485Mode; |
||
| 570 | _stHWConfig1: stHWConfig1; |
||
| 571 | _stLastCounter1Cfg: stCounter1Config; |
||
| 572 | _stLastCounter2Cfg: stCounter2Config; |
||
| 573 | _AfterReadInputs: UINT; |
||
| 574 | _BeforeWriteOutputs: UINT; |
||
| 575 | _abyIntTxBuf: ARRAY [..] OF ; |
||
| 576 | _abyIntRxBuf: ARRAY [..] OF ; |
||
| 577 | END_VAR |
||
| 578 | |||
| 579 | ~~~ |
||
| 580 | ~~~ST |
||
| 581 | //******************************** |
||
| 582 | // base SPI implementation |
||
| 583 | //******************************** |
||
| 584 | SUPER^(); |
||
| 585 | |||
| 586 | //******************************** |
||
| 587 | // get SPI State |
||
| 588 | //******************************** |
||
| 589 | CASE _iState OF |
||
| 590 | 0: |
||
| 591 | IF SUPER^.init() THEN |
||
| 592 | // get only once |
||
| 593 | THIS^.getHATinfo(); |
||
| 594 | _iState := 1; |
||
| 595 | END_IF |
||
| 596 | |||
| 597 | 1: _iState := 10; |
||
| 598 | END_CASE |
||
| 599 | |||
| 600 | //******************************** |
||
| 601 | // is device operational |
||
| 602 | //******************************** |
||
| 603 | IF _iState = 10 THEN |
||
| 604 | xOperational := TRUE; |
||
| 605 | ELSE |
||
| 606 | xOperational := FALSE; |
||
| 607 | END_IF |
||
| 608 | |||
| 609 | |||
| 610 | |||
| 611 | ~~~ |
||
| 612 | --- |
||
| 613 | ~~~ST |
||
| 614 | METHOD getHATinfo: |
||
| 615 | VAR |
||
| 616 | wValue: WORD; |
||
| 617 | eCmd: eServiceCommand; |
||
| 618 | eComStatus: eMonarcoComStatus; |
||
| 619 | J: INT; |
||
| 620 | wReqAdr: WORD; |
||
| 621 | awSrvAnwser: ARRAY [..] OF ; |
||
| 622 | cpu_l: DWORD; |
||
| 623 | cpu_h: DWORD; |
||
| 624 | lwcpu: LWORD; |
||
| 625 | END_VAR |
||
| 626 | |||
| 627 | ~~~ |
||
| 628 | ~~~ST |
||
| 629 | //******************************** |
||
| 630 | //Read FW, HW revision and CPU-ID |
||
| 631 | //******************************** |
||
| 632 | FOR J := 1 TO 9 DO |
||
| 633 | CASE J OF |
||
| 634 | 1: eCmd := eServiceCommand.SDC_FIXED_STATUSWORD; //bogus question (ABCD) |
||
| 635 | 2: eCmd := eServiceCommand.SDC_FIXED_FWVERL; |
||
| 636 | 3: eCmd := eServiceCommand.SDC_FIXED_FWVERH; |
||
| 637 | 4: eCmd := eServiceCommand.SDC_FIXED_HWVERL; |
||
| 638 | 5: eCmd := eServiceCommand.SDC_FIXED_HWVERH; |
||
| 639 | 6: eCmd := eServiceCommand.SDC_FIXED_CPUID1; |
||
| 640 | 7: eCmd := eServiceCommand.SDC_FIXED_CPUID2; |
||
| 641 | 8: eCmd := eServiceCommand.SDC_FIXED_CPUID3; |
||
| 642 | 9: eCmd := eServiceCommand.SDC_FIXED_CPUID4; |
||
| 643 | END_CASE |
||
| 644 | wValue := TO_WORD(eServiceCommand.SDC_FIXED_STATUSWORD); |
||
| 645 | eComStatus := THIS^.ServiceRequest( wValue := wValue, eCommand := eCmd, xWrite := FALSE ); |
||
| 646 | // Get result |
||
| 647 | eComStatus := THIS^.dataTransfer(); |
||
| 648 | awSrvAnwser[J] := MEM.PackBytesToWord(_abyIntRxBuf[1], _abyIntRxBuf[0]); |
||
| 649 | END_FOR |
||
| 650 | |||
| 651 | // Store anwsers in status |
||
| 652 | THIS^.stHATinfo.dwFWVersion := MEM.PackWordsToDword(awSrvAnwser[3],awSrvAnwser[2]); |
||
| 653 | THIS^.stHATinfo.dwHWversion := MEM.PackWordsToDword(awSrvAnwser[5],awSrvAnwser[4]); |
||
| 654 | cpu_l := MEM.PackWordsToDword(awSrvAnwser[7],awSrvAnwser[6]); |
||
| 655 | cpu_h := MEM.PackWordsToDword(awSrvAnwser[9],awSrvAnwser[8]); |
||
| 656 | lwcpu := cpu_h; |
||
| 657 | lwcpu := SHL(lwcpu,32); |
||
| 658 | THIS^.stHATinfo.lwCpuID := lwcpu OR cpu_l; |
||
| 659 | ~~~ |
||
| 660 | --- |
||
| 661 | ~~~ST |
||
| 662 | METHOD getCRC: WORD |
||
| 663 | VAR_INPUT |
||
| 664 | abyBuf: ARRAY [..] OF ; |
||
| 665 | END_VAR |
||
| 666 | VAR |
||
| 667 | uiSizeTx: UINT; |
||
| 668 | udiCRCLen: UDINT; |
||
| 669 | pByteTx: pointer; |
||
| 670 | CheckSum: WORD; |
||
| 671 | END_VAR |
||
| 672 | |||
| 673 | ~~~ |
||
| 674 | ~~~ST |
||
| 675 | // startadress of bytes |
||
| 676 | pByteTx := ADR(abyBuf); |
||
| 677 | |||
| 678 | // calculate CRC-16 checksum, beware that for the checksum calculation, we need to skip the checksum itself |
||
| 679 | uiSizeTx := SIZEOF(abyBuf); |
||
| 680 | |||
| 681 | // should always be 24 / 16#18 |
||
| 682 | udiCRCLen := (uiSizeTx -16#02); |
||
| 683 | |||
| 684 | CheckSum := MEM.CRC16_Modbus( pByteTx, TO_UINT(udiCRCLen) ); |
||
| 685 | |||
| 686 | // return checksum |
||
| 687 | getCRC := CheckSum; |
||
| 688 | ~~~ |
||
| 689 | --- |
||
| 690 | ~~~ST |
||
| 691 | METHOD dataTransfer: eMonarcoComStatus |
||
| 692 | VAR |
||
| 693 | tmpRxBuf: ARRAY [..] OF ; |
||
| 694 | udiBufSize: UDINT; |
||
| 695 | xDone: BOOL; |
||
| 696 | CRC: WORD; |
||
| 697 | END_VAR |
||
| 698 | |||
| 699 | ~~~ |
||
| 700 | ~~~ST |
||
| 701 | //init temporay buffer before filling; |
||
| 702 | //flush(tmpRxBuf); |
||
| 703 | |||
| 704 | // determine size of buffer |
||
| 705 | udiBufSize := SIZEOF( THIS^._abyIntTxBuf ); |
||
| 706 | |||
| 707 | (* Sets CRC16 checksum, for Tx byte array [24] and [25]*) |
||
| 708 | CRC := THIS^.getCRC( THIS^._abyIntTxBuf ); |
||
| 709 | THIS^._abyIntTxBuf[24] := WORD_TO_BYTE(CRC); |
||
| 710 | THIS^._abyIntTxBuf[25] := WORD_TO_BYTE(SHR(CRC,8)); |
||
| 711 | |||
| 712 | // Send, Recv |
||
| 713 | xDone := THIS^.transferExt( pabyTxBuffer := ADR( THIS^._abyIntTxBuf ), |
||
| 714 | pabyRxBuffer := ADR( tmpRxBuf ), // write result into a temporary buffer for post-analysis. Beware that the received answser is from the last question, not the current question. |
||
| 715 | udiLen := udiBufSize, |
||
| 716 | uiDelayus := 5, |
||
| 717 | udispeedHz := 0); |
||
| 718 | |||
| 719 | IF NOT xDone THEN |
||
| 720 | // Send, Recv failed |
||
| 721 | DataTransfer := eMonarcoComStatus.ERROR; |
||
| 722 | ELSE |
||
| 723 | // OK? |
||
| 724 | IF THIS^.checkCRC(tmpRxBuf) THEN |
||
| 725 | //CRC okay, return the recv buffer |
||
| 726 | THIS^._abyIntRxBuf := tmpRxBuf; |
||
| 727 | DataTransfer := eMonarcoComStatus.SUCCES; |
||
| 728 | ELSE |
||
| 729 | //CRC not okay, return an empty buffer |
||
| 730 | flush(tmpRxBuf); |
||
| 731 | THIS^._abyIntRxBuf := tmpRxBuf; |
||
| 732 | DataTransfer := eMonarcoComStatus.CrcFail; |
||
| 733 | END_IF |
||
| 734 | END_IF; |
||
| 735 | ~~~ |
||
| 736 | --- |
||
| 737 | ~~~ST |
||
| 738 | METHOD flush: BOOL |
||
| 739 | VAR_INPUT |
||
| 740 | abyRxBuf: ARRAY [..] OF ; |
||
| 741 | END_VAR |
||
| 742 | VAR |
||
| 743 | i: UINT; |
||
| 744 | END_VAR |
||
| 745 | |||
| 746 | ~~~ |
||
| 747 | ~~~ST |
||
| 748 | //flush the temporary receive buffer before filling; |
||
| 749 | FOR i :=0 TO (ParamList.MONARCO_STRUCT_SIZE-1) DO |
||
| 750 | abyRxBuf[i] := 16#00; |
||
| 751 | END_FOR |
||
| 752 | flush := TRUE; |
||
| 753 | ~~~ |
||
| 754 | --- |
||
| 755 | ~~~ST |
||
| 756 | METHOD checkCRC: BOOL |
||
| 757 | VAR_INPUT |
||
| 758 | abyRxBuf: ARRAY [..] OF ; |
||
| 759 | END_VAR |
||
| 760 | VAR |
||
| 761 | CRC: WORD; |
||
| 762 | CRCLo: BYTE; |
||
| 763 | CRCHi: BYTE; |
||
| 764 | END_VAR |
||
| 765 | |||
| 766 | ~~~ |
||
| 767 | ~~~ST |
||
| 768 | CRC := THIS^.GetCRC(abyRxBuf); |
||
| 769 | CRCHi := WORD_TO_BYTE(CRC); |
||
| 770 | CRCLo := WORD_TO_BYTE(SHR(CRC,8)); |
||
| 771 | |||
| 772 | // check to self calculated CRC against the sent CRC, Equal = OK |
||
| 773 | IF (CRCHi = abyRxBuf[ParamList.MONARCO_STRUCT_SIZE - 2]) AND (CRCLo = abyRxBuf[ParamList.MONARCO_STRUCT_SIZE - 1]) THEN |
||
| 774 | checkCRC := TRUE; |
||
| 775 | ELSE |
||
| 776 | checkCRC := FALSE; |
||
| 777 | END_IF; |
||
| 778 | ~~~ |
||
| 779 | --- |
||
| 780 | ~~~ST |
||
| 781 | METHOD setDutyCycleDOut4: eMonarcoComStatus |
||
| 782 | VAR_INPUT |
||
| 783 | rDutyCycle: REAL; |
||
| 784 | END_VAR |
||
| 785 | VAR |
||
| 786 | wDC: WORD; |
||
| 787 | END_VAR |
||
| 788 | |||
| 789 | ~~~ |
||
| 790 | ~~~ST |
||
| 791 | wDC := getDutyCycle( rDutyCycle := rDutyCycle ); |
||
| 792 | |||
| 793 | // 18.0 2.0 PWM2 Dutycycle ch A |
||
| 794 | THIS^._abyIntTxBuf[18] := WORD_TO_BYTE(wDC); // bitmask to protect prescaler |
||
| 795 | THIS^._abyIntTxBuf[19] := WORD_TO_BYTE(SHR(wDC,8)); |
||
| 796 | ~~~ |
||
| 797 | --- |
||
| 798 | ~~~ST |
||
| 799 | METHOD setDutyCycleDOut2: eMonarcoComStatus |
||
| 800 | VAR_INPUT |
||
| 801 | rDutyCycle: REAL; |
||
| 802 | END_VAR |
||
| 803 | VAR |
||
| 804 | wDC: WORD; |
||
| 805 | END_VAR |
||
| 806 | |||
| 807 | ~~~ |
||
| 808 | ~~~ST |
||
| 809 | wDC := getDutyCycle( rDutyCycle := rDutyCycle ); |
||
| 810 | |||
| 811 | // 12.0 2.0 PWM1 Dutycycle ch B |
||
| 812 | THIS^._abyIntTxBuf[12] := WORD_TO_BYTE(wDC); // bitmask to protect prescaler |
||
| 813 | THIS^._abyIntTxBuf[13] := WORD_TO_BYTE(SHR(wDC,8)); |
||
| 814 | ~~~ |
||
| 815 | --- |
||
| 816 | ~~~ST |
||
| 817 | METHOD setPWM1freq: eMonarcoComStatus |
||
| 818 | VAR_INPUT |
||
| 819 | udiHz: UDINT; |
||
| 820 | END_VAR |
||
| 821 | VAR |
||
| 822 | _uidiHz: UDINT; |
||
| 823 | ePrescale: ePrescaler; |
||
| 824 | uiRangeLow: UINT; |
||
| 825 | uiRangeHigh: UINT; |
||
| 826 | uiTOP: UINT; |
||
| 827 | fPWM: WORD; |
||
| 828 | END_VAR |
||
| 829 | |||
| 830 | ~~~ |
||
| 831 | ~~~ST |
||
| 832 | /// clip the input Hz on lower and upper bound, so; |
||
| 833 | _uidiHz := LIMIT( 1, udiHz, 100000); |
||
| 834 | |||
| 835 | /// Determine prescaler and TOP according to table |
||
| 836 | CASE _uidiHz OF |
||
| 837 | 1..9 : ePrescale := ePrescaler.ScaleFactor512; |
||
| 838 | uiTOP := TO_UINT( (32000000/ 512/ _uidiHz));// interpolate between 1 and 9Hz |
||
| 839 | |||
| 840 | 10..99 : ePrescale := ePrescaler.ScaleFactor64; |
||
| 841 | uiTOP := TO_UINT( (32000000/ 64/ _uidiHz)); // interpolate between 10 and 100Hz |
||
| 842 | |||
| 843 | 100..999 : ePrescale := ePrescaler.ScaleFactor8; |
||
| 844 | uiTOP := TO_UINT( (32000000/ 8/ _uidiHz));// interpolate between 100 and 1000Hz |
||
| 845 | |||
| 846 | 1000..100000 : ePrescale := ePrescaler.ScaleFactor1; |
||
| 847 | uiTOP := TO_UINT( (32000000/ 1/ _uidiHz));// interpolate between 1000 and 100000Hz |
||
| 848 | END_CASE |
||
| 849 | |||
| 850 | /// Now set prescaler |
||
| 851 | CASE ePrescale OF |
||
| 852 | /// 00 0 : prescaler = 1 |
||
| 853 | ePrescaler.ScaleFactor1 : fPWM.0 := FALSE; |
||
| 854 | fPWM.1 := FALSE; |
||
| 855 | /// 01 1 : prescaler = 8 |
||
| 856 | ePrescaler.ScaleFactor8 : fPWM.0 := TRUE; |
||
| 857 | fPWM.1 := FALSE; |
||
| 858 | /// 10 2 : prescaler = 64 |
||
| 859 | ePrescaler.ScaleFactor64 : fPWM.0 := FALSE; |
||
| 860 | fPWM.1 := TRUE; |
||
| 861 | /// 11 3 : prescaler = 512 |
||
| 862 | ePrescaler.ScaleFactor512 : fPWM.0 := TRUE; |
||
| 863 | fPWM.1 := TRUE; |
||
| 864 | END_CASE |
||
| 865 | |||
| 866 | // 8.0 2.0 PWM1 frequency |
||
| 867 | THIS^._abyIntTxBuf[8] := WORD_TO_BYTE(fPWM) AND 16#FC; // bitmask to protect prescaler |
||
| 868 | THIS^._abyIntTxBuf[9] := WORD_TO_BYTE(SHR(fPWM,8)); |
||
| 869 | ~~~ |
||
| 870 | --- |
||
| 871 | ~~~ST |
||
| 872 | METHOD setDutyCycleDOut3: eMonarcoComStatus |
||
| 873 | VAR_INPUT |
||
| 874 | rDutyCycle: REAL; |
||
| 875 | END_VAR |
||
| 876 | VAR |
||
| 877 | wDC: WORD; |
||
| 878 | END_VAR |
||
| 879 | |||
| 880 | ~~~ |
||
| 881 | ~~~ST |
||
| 882 | wDC := getDutyCycle( rDutyCycle := rDutyCycle ); |
||
| 883 | |||
| 884 | // 12.0 2.0 PWM1 Dutycycle ch C |
||
| 885 | THIS^._abyIntTxBuf[14] := WORD_TO_BYTE(wDC); // bitmask to protect prescaler |
||
| 886 | THIS^._abyIntTxBuf[15] := WORD_TO_BYTE(SHR(wDC,8)); |
||
| 887 | ~~~ |
||
| 888 | --- |
||
| 889 | ~~~ST |
||
| 890 | METHOD setDutyCycleDOut1: eMonarcoComStatus |
||
| 891 | VAR_INPUT |
||
| 892 | rDutyCycle: REAL; |
||
| 893 | END_VAR |
||
| 894 | VAR |
||
| 895 | wDC: WORD; |
||
| 896 | END_VAR |
||
| 897 | |||
| 898 | ~~~ |
||
| 899 | ~~~ST |
||
| 900 | wDC := getDutyCycle( rDutyCycle := rDutyCycle ); |
||
| 901 | |||
| 902 | // 10.0 2.0 PWM1 Dutycycle ch A |
||
| 903 | THIS^._abyIntTxBuf[10] := WORD_TO_BYTE(wDC); // bitmask to protect prescaler |
||
| 904 | THIS^._abyIntTxBuf[11] := WORD_TO_BYTE(SHR(wDC,8)); |
||
| 905 | ~~~ |
||
| 906 | --- |
||
| 907 | ~~~ST |
||
| 908 | METHOD setAOutWord: eMonarcoComStatus |
||
| 909 | VAR_INPUT |
||
| 910 | wAOut1: WORD; |
||
| 911 | wAOut2: WORD; |
||
| 912 | END_VAR |
||
| 913 | VAR |
||
| 914 | v1: WORD; |
||
| 915 | v2: WORD; |
||
| 916 | END_VAR |
||
| 917 | |||
| 918 | ~~~ |
||
| 919 | ~~~ST |
||
| 920 | // limit the value's |
||
| 921 | v1 := LIMIT(0, wAOut1, Paramlist.MONARCO_ADC_RANGE_MAX ); |
||
| 922 | v2 := LIMIT(0, wAOut2, Paramlist.MONARCO_ADC_RANGE_MAX ); |
||
| 923 | |||
| 924 | THIS^._abyIntTxBuf[20] := WORD_TO_BYTE(v1); |
||
| 925 | THIS^._abyIntTxBuf[21] := WORD_TO_BYTE(SHR(v1,8)); |
||
| 926 | THIS^._abyIntTxBuf[22] := WORD_TO_BYTE(v2); |
||
| 927 | THIS^._abyIntTxBuf[23] := WORD_TO_BYTE(SHR(v2,8)); |
||
| 928 | ~~~ |
||
| 929 | --- |
||
| 930 | ~~~ST |
||
| 931 | METHOD setAOut: eMonarcoComStatus |
||
| 932 | VAR_INPUT |
||
| 933 | stAOut: stAOut; |
||
| 934 | END_VAR |
||
| 935 | VAR |
||
| 936 | v1: WORD; |
||
| 937 | v2: WORD; |
||
| 938 | END_VAR |
||
| 939 | |||
| 940 | ~~~ |
||
| 941 | ~~~ST |
||
| 942 | // limit the value's |
||
| 943 | v1 := LIMIT(0, stAOut.wAOut1, Paramlist.MONARCO_ADC_RANGE_MAX ); |
||
| 944 | v2 := LIMIT(0, stAOut.wAOut2, Paramlist.MONARCO_ADC_RANGE_MAX ); |
||
| 945 | |||
| 946 | THIS^._abyIntTxBuf[20] := WORD_TO_BYTE(v1); |
||
| 947 | THIS^._abyIntTxBuf[21] := WORD_TO_BYTE(SHR(v1,8)); |
||
| 948 | THIS^._abyIntTxBuf[22] := WORD_TO_BYTE(v2); |
||
| 949 | THIS^._abyIntTxBuf[23] := WORD_TO_BYTE(SHR(v2,8)); |
||
| 950 | ~~~ |
||
| 951 | --- |
||
| 952 | ~~~ST |
||
| 953 | METHOD setPWM2freq: eMonarcoComStatus |
||
| 954 | VAR_INPUT |
||
| 955 | udiHz: UDINT; |
||
| 956 | END_VAR |
||
| 957 | VAR |
||
| 958 | _uidiHz: UDINT; |
||
| 959 | ePrescale: ePrescaler; |
||
| 960 | uiRangeLow: UINT; |
||
| 961 | uiRangeHigh: UINT; |
||
| 962 | uiTOP: UINT; |
||
| 963 | fPWM: WORD; |
||
| 964 | END_VAR |
||
| 965 | |||
| 966 | ~~~ |
||
| 967 | ~~~ST |
||
| 968 | /// clip the input Hz on lower and upper bound; |
||
| 969 | _uidiHz := LIMIT( 1, udiHz, 100000); |
||
| 970 | |||
| 971 | //// Determine prescaler and TOP according to table |
||
| 972 | CASE _uidiHz OF |
||
| 973 | 1..9 : ePrescale := ePrescaler.ScaleFactor512; |
||
| 974 | uiTOP := TO_UINT( (32000000/ 512/ _uidiHz));// interpolate between 1 and 9Hz |
||
| 975 | |||
| 976 | 10..99 : ePrescale := ePrescaler.ScaleFactor64; |
||
| 977 | uiTOP := TO_UINT( (32000000/ 64/ _uidiHz)); // interpolate between 10 and 100Hz |
||
| 978 | |||
| 979 | 100..999 : ePrescale := ePrescaler.ScaleFactor8; |
||
| 980 | uiTOP := TO_UINT( (32000000/ 8/ _uidiHz));// interpolate between 100 and 1000Hz |
||
| 981 | |||
| 982 | 1000..100000 : ePrescale := ePrescaler.ScaleFactor1; |
||
| 983 | uiTOP := TO_UINT( (32000000/ 1/ _uidiHz));// interpolate between 1000 and 100000Hz |
||
| 984 | END_CASE |
||
| 985 | |||
| 986 | /// Now set prescaler |
||
| 987 | /// bit 15 .. 2 : TOP / 4 |
||
| 988 | CASE ePrescale OF |
||
| 989 | /// 00 0 : prescaler = 1 |
||
| 990 | ePrescaler.ScaleFactor1 : fPWM.0 := FALSE; |
||
| 991 | fPWM.1 := FALSE; |
||
| 992 | /// 01 1 : prescaler = 8 |
||
| 993 | ePrescaler.ScaleFactor8 : fPWM.0 := TRUE; |
||
| 994 | fPWM.1 := FALSE; |
||
| 995 | /// 10 2 : prescaler = 64 |
||
| 996 | ePrescaler.ScaleFactor64 : fPWM.0 := FALSE; |
||
| 997 | fPWM.1 := TRUE; |
||
| 998 | /// 11 3 : prescaler = 512 |
||
| 999 | ePrescaler.ScaleFactor512 : fPWM.0 := TRUE; |
||
| 1000 | fPWM.1 := TRUE; |
||
| 1001 | END_CASE |
||
| 1002 | |||
| 1003 | // 8.0 2.0 PWM1 frequency |
||
| 1004 | THIS^._abyIntTxBuf[16] := WORD_TO_BYTE(fPWM); |
||
| 1005 | THIS^._abyIntTxBuf[17] := WORD_TO_BYTE(SHR(fPWM,8)); |
||
| 1006 | ~~~ |
||
| 1007 | --- |
||
| 1008 | ~~~ST |
||
| 1009 | METHOD getServiceResponse: eMonarcoComStatus |
||
| 1010 | VAR_OUTPUT |
||
| 1011 | Response: stServiceResponse; |
||
| 1012 | END_VAR |
||
| 1013 | VAR |
||
| 1014 | rsp: stServiceResponse; |
||
| 1015 | END_VAR |
||
| 1016 | |||
| 1017 | ~~~ |
||
| 1018 | ~~~ST |
||
| 1019 | //send/receive |
||
| 1020 | getServiceResponse := THIS^.dataTransfer(); |
||
| 1021 | |||
| 1022 | rsp.wServiceValue := MEM.PackBytesToWord(_abyIntRxBuf[1],_abyIntRxBuf[0]); |
||
| 1023 | rsp.wServiceAddress := MEM.PackBytesToWord(_abyIntRxBuf[3],_abyIntRxBuf[2]); |
||
| 1024 | |||
| 1025 | CASE rsp.wServiceValue OF |
||
| 1026 | /// Set the error bit |
||
| 1027 | Paramlist.MONARCO_SERVICE_STATUS_OK : Response.xError := FALSE; |
||
| 1028 | Paramlist.MONARCO_SERVICE_ERROR_UNKNOWN_REG : Response.xError := TRUE; |
||
| 1029 | Paramlist.MONARCO_SERVICE_ERROR_CRC : Response.xError := TRUE; |
||
| 1030 | END_CASE |
||
| 1031 | |||
| 1032 | /// just give the response back |
||
| 1033 | Response.wServiceValue := MEM.PackBytesToWord(_abyIntRxBuf[1],_abyIntRxBuf[0]); |
||
| 1034 | Response.wServiceAddress := MEM.PackBytesToWord(_abyIntRxBuf[3],_abyIntRxBuf[2]); |
||
| 1035 | ~~~ |
||
| 1036 | --- |
||
| 1037 | ~~~ST |
||
| 1038 | METHOD setDOut: |
||
| 1039 | VAR_INPUT |
||
| 1040 | stDOut: stDOut; |
||
| 1041 | END_VAR |
||
| 1042 | |||
| 1043 | ~~~ |
||
| 1044 | ~~~ST |
||
| 1045 | THIS^._abyIntTxBuf[7].0 := stDOut.xDOut1; |
||
| 1046 | THIS^._abyIntTxBuf[7].1 := stDOut.xDOut2; |
||
| 1047 | THIS^._abyIntTxBuf[7].2 := stDOut.xDOut3; |
||
| 1048 | THIS^._abyIntTxBuf[7].3 := stDOut.xDOut4; |
||
| 1049 | ~~~ |
||
| 1050 | --- |
||
| 1051 | ~~~ST |
||
| 1052 | METHOD setDOutBool: |
||
| 1053 | VAR_INPUT |
||
| 1054 | xDOut0: BOOL; |
||
| 1055 | xDOut1: BOOL; |
||
| 1056 | xDOut2: BOOL; |
||
| 1057 | xDOut3: BOOL; |
||
| 1058 | END_VAR |
||
| 1059 | |||
| 1060 | ~~~ |
||
| 1061 | ~~~ST |
||
| 1062 | THIS^._abyIntTxBuf[7].0 := xDOut0; |
||
| 1063 | THIS^._abyIntTxBuf[7].1 := xDOut1; |
||
| 1064 | THIS^._abyIntTxBuf[7].2 := xDOut2; |
||
| 1065 | THIS^._abyIntTxBuf[7].3 := xDOut3; |
||
| 1066 | ~~~ |
||
| 1067 | --- |
||
| 1068 | ~~~ST |
||
| 1069 | METHOD setDOutByte: |
||
| 1070 | VAR_INPUT |
||
| 1071 | byDOut: BYTE; |
||
| 1072 | END_VAR |
||
| 1073 | |||
| 1074 | ~~~ |
||
| 1075 | ~~~ST |
||
| 1076 | THIS^._abyIntTxBuf[7].0 := byDOut.0; |
||
| 1077 | THIS^._abyIntTxBuf[7].1 := byDOut.1; |
||
| 1078 | THIS^._abyIntTxBuf[7].2 := byDOut.2; |
||
| 1079 | THIS^._abyIntTxBuf[7].3 := byDOut.3; |
||
| 1080 | ~~~ |
||
| 1081 | --- |
||
| 1082 | ~~~ST |
||
| 1083 | METHOD getDInByte: eMonarcoComStatus |
||
| 1084 | VAR_OUTPUT |
||
| 1085 | byDIn: BYTE; |
||
| 1086 | END_VAR |
||
| 1087 | |||
| 1088 | ~~~ |
||
| 1089 | ~~~ST |
||
| 1090 | byDIn.0 := THIS^._abyIntRxBuf[7].0; |
||
| 1091 | byDIn.1 := THIS^._abyIntRxBuf[7].1; |
||
| 1092 | byDIn.2 := THIS^._abyIntRxBuf[7].2; |
||
| 1093 | byDIn.3 := THIS^._abyIntRxBuf[7].3; |
||
| 1094 | ~~~ |
||
| 1095 | --- |
||
| 1096 | ~~~ST |
||
| 1097 | METHOD getDInBool: eMonarcoComStatus |
||
| 1098 | VAR_OUTPUT |
||
| 1099 | xDIn0: BOOL; |
||
| 1100 | xDIn1: BOOL; |
||
| 1101 | xDIn2: BOOL; |
||
| 1102 | xDIn3: BOOL; |
||
| 1103 | END_VAR |
||
| 1104 | |||
| 1105 | ~~~ |
||
| 1106 | ~~~ST |
||
| 1107 | xDIn0 := THIS^._abyIntRxBuf[7].0; |
||
| 1108 | xDIn1 := THIS^._abyIntRxBuf[7].1; |
||
| 1109 | xDIn2 := THIS^._abyIntRxBuf[7].2; |
||
| 1110 | xDIn3 := THIS^._abyIntRxBuf[7].3; |
||
| 1111 | ~~~ |
||
| 1112 | --- |
||
| 1113 | ~~~ST |
||
| 1114 | METHOD getDIn: eMonarcoComStatus |
||
| 1115 | VAR_OUTPUT |
||
| 1116 | stDIn: stDIn; |
||
| 1117 | END_VAR |
||
| 1118 | |||
| 1119 | ~~~ |
||
| 1120 | ~~~ST |
||
| 1121 | stDIn.xDIn1 := THIS^._abyIntRxBuf[7].0; |
||
| 1122 | stDIn.xDIn2 := THIS^._abyIntRxBuf[7].1; |
||
| 1123 | stDIn.xDIn3 := THIS^._abyIntRxBuf[7].2; |
||
| 1124 | stDIn.xDIn4 := THIS^._abyIntRxBuf[7].3; |
||
| 1125 | ~~~ |
||
| 1126 | --- |
||
| 1127 | ~~~ST |
||
| 1128 | METHOD getAInWord: eMonarcoComStatus |
||
| 1129 | VAR_OUTPUT |
||
| 1130 | wAIn1: WORD; |
||
| 1131 | wAIn2: WORD; |
||
| 1132 | END_VAR |
||
| 1133 | |||
| 1134 | ~~~ |
||
| 1135 | ~~~ST |
||
| 1136 | wAIn1 := MEM.PackBytesToWord(THIS^._abyIntRxBuf[21],THIS^._abyIntRxBuf[20]); |
||
| 1137 | wAIn2 := MEM.PackBytesToWord(THIS^._abyIntRxBuf[23],THIS^._abyIntRxBuf[22]); |
||
| 1138 | ~~~ |
||
| 1139 | --- |
||
| 1140 | ~~~ST |
||
| 1141 | METHOD ServiceRequest: eMonarcoComStatus |
||
| 1142 | VAR_INPUT |
||
| 1143 | wValue: WORD; |
||
| 1144 | eCommand: eServiceCommand; |
||
| 1145 | xWrite: BOOL; |
||
| 1146 | END_VAR |
||
| 1147 | VAR |
||
| 1148 | stRequest: stServiceRequest; |
||
| 1149 | END_VAR |
||
| 1150 | |||
| 1151 | ~~~ |
||
| 1152 | ~~~ST |
||
| 1153 | stRequest.wServiceValue := wValue; |
||
| 1154 | stRequest.wServiceAddress := Paramlist.SDC_ADDRESS_MASK AND TO_WORD(eCommand); |
||
| 1155 | stRequest.wServiceAddress.12 := xWrite; |
||
| 1156 | |||
| 1157 | // Service Data Request |
||
| 1158 | THIS^._abyIntTxBuf[0] := WORD_TO_BYTE(stRequest.wServiceValue); // ServiceValueHi; |
||
| 1159 | THIS^._abyIntTxBuf[1] := WORD_TO_BYTE(SHR(stRequest.wServiceValue,8)); // ServiceValueLo; |
||
| 1160 | THIS^._abyIntTxBuf[2] := WORD_TO_BYTE(stRequest.wServiceAddress);//ServiceRegisterHi; |
||
| 1161 | THIS^._abyIntTxBuf[3] := WORD_TO_BYTE(SHR(stRequest.wServiceAddress,8));//ServiceRegisterLo; |
||
| 1162 | |||
| 1163 | //send/receive |
||
| 1164 | ServiceRequest := THIS^.dataTransfer(); |
||
| 1165 | ~~~ |
||
| 1166 | --- |
||
| 1167 | ~~~ST |
||
| 1168 | METHOD getAIn: eMonarcoComStatus |
||
| 1169 | VAR_OUTPUT |
||
| 1170 | stAIn: stAIn; |
||
| 1171 | END_VAR |
||
| 1172 | |||
| 1173 | ~~~ |
||
| 1174 | ~~~ST |
||
| 1175 | stAIn.wAIn1 := MEM.PackBytesToWord(THIS^._abyIntRxBuf[21],THIS^._abyIntRxBuf[20]); |
||
| 1176 | stAIn.wAIn2 := MEM.PackBytesToWord(THIS^._abyIntRxBuf[23],THIS^._abyIntRxBuf[22]); |
||
| 1177 | ~~~ |
||
| 1178 | --- |
||
| 1179 | ~~~ST |
||
| 1180 | METHOD SetUserLED: eMonarcoComStatus |
||
| 1181 | VAR_INPUT |
||
| 1182 | byLeds: BYTE; |
||
| 1183 | END_VAR |
||
| 1184 | |||
| 1185 | ~~~ |
||
| 1186 | ~~~ST |
||
| 1187 | // 10.0 2.0 PWM1 Dutycycle ch A |
||
| 1188 | THIS^._abyIntTxBuf[5] := byLeds; // set userled mask |
||
| 1189 | THIS^._abyIntTxBuf[6] := byLeds; // set userled |
||
| 1190 | ~~~ |
||
| 1191 | --- |
||
| 1192 | ~~~ST |
||
| 1193 | METHOD AfterReadInputs: INT |
||
| 1194 | |||
| 1195 | ~~~ |
||
| 1196 | ~~~ST |
||
| 1197 | ///******************************** |
||
| 1198 | /// Continous intputs |
||
| 1199 | ///******************************** |
||
| 1200 | SUPER^.AfterReadInputs(); |
||
| 1201 | |||
| 1202 | IF _iState = 10 THEN |
||
| 1203 | |||
| 1204 | // SPI reads/writes outputs in a single read/write action |
||
| 1205 | THIS^.getDInByte(byDin => byDin); |
||
| 1206 | THIS^.getAInWord(wAIn1 => wAIn1, wAIn2 => wAIn2); |
||
| 1207 | THIS^.setDoutByte(byDout := byDOut); |
||
| 1208 | THIS^.setAOutWord(wAout1 := wAOut1, wAOut2 := wAOut2); |
||
| 1209 | |||
| 1210 | _BeforeWriteOutputs := _BeforeWriteOutputs +1; |
||
| 1211 | eComStatus := THIS^.dataTransfer(); |
||
| 1212 | END_IF |
||
| 1213 | |||
| 1214 | ~~~ |
||
| 1215 | --- |
||
| 1216 | ~~~ST |
||
| 1217 | METHOD BeforeWriteOutputs: INT |
||
| 1218 | |||
| 1219 | ~~~ |
||
| 1220 | ~~~ST |
||
| 1221 | ///******************************** |
||
| 1222 | /// Continous Outputs |
||
| 1223 | ///******************************** |
||
| 1224 | SUPER^.BeforeWriteOutputs(); |
||
| 1225 | |||
| 1226 | IF _iState = 10 THEN |
||
| 1227 | _BeforeWriteOutputs := _BeforeWriteOutputs +1; |
||
| 1228 | END_IF |
||
| 1229 | ~~~ |
||
| 1230 | --- |
||
| 1231 | ~~~ST |
||
| 1232 | METHOD Initialize: UDINT |
||
| 1233 | VAR_INPUT |
||
| 1234 | wModuleType: UINT; |
||
| 1235 | dwInstance: UDINT; |
||
| 1236 | pConnector: pointer; |
||
| 1237 | END_VAR |
||
| 1238 | VAR |
||
| 1239 | pParam1: pointer; |
||
| 1240 | pParam10: pointer; |
||
| 1241 | udiResult: UDINT; |
||
| 1242 | END_VAR |
||
| 1243 | |||
| 1244 | ~~~ |
||
| 1245 | ~~~ST |
||
| 1246 | SUPER^.Initialize(wModuleType, dwInstance, pConnector); |
||
| 1247 | |||
| 1248 | pParam1 := ConfigGetParameter(_pConnector, 1); |
||
| 1249 | IF pParam1 <> 0 THEN |
||
| 1250 | THIS^.Watchdog := IoStandard.ConfigGetParameterValueByte(pParam1, ADR(udiResult)); |
||
| 1251 | END_IF |
||
| 1252 | |||
| 1253 | pParam10 := ConfigGetParameter(_pConnector, 10); |
||
| 1254 | IF pParam10 <> 0 THEN |
||
| 1255 | THIS^.ControlByte8 := IoStandard.ConfigGetParameterValueByte(pParam10, ADR(udiResult)); |
||
| 1256 | END_IF |
||
| 1257 | ~~~ |
||
| 1258 | VAR_GLOBAL |
||
| 1259 | MONARCO_STRUCT_SIZE: UINT; |
||
| 1260 | MONARCO_SERVICE_STATUS_OK: WORD; |
||
| 1261 | MONARCO_SERVICE_ERROR_UNKNOWN_REG: WORD; |
||
| 1262 | MONARCO_SERVICE_ERROR_CRC: WORD; |
||
| 1263 | SDC_ADDRESS_MASK: WORD; |
||
| 1264 | MONARCO_ADC_RANGE_MAX: WORD; |
||
| 1265 | END_VAR |