ProtocolDevice.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012 SCHUNK GmbH & Co. KG
00003  * Copyright (c) 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00004  *
00005  * Licensed under the Apache License, Version 2.0 (the "License");
00006  * you may not use this file except in compliance with the License.
00007  * You may obtain a copy of the License at
00008  *
00009  *   http://www.apache.org/licenses/LICENSE-2.0
00010 
00011  * Unless required by applicable law or agreed to in writing, software
00012  * distributed under the License is distributed on an "AS IS" BASIS,
00013  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014  * See the License for the specific language governing permissions and
00015  * limitations under the License.
00016  */
00017 
00018 #include "ProtocolDevice.h"
00019 
00020 // ========================================================================== ;
00021 //                                                                            ;
00022 // ---- private auxiliary functions ----------------------------------------- ;
00023 //                                                                            ;
00024 // ========================================================================== ;
00025 
00026 // ========================================================================== ;
00027 //                                                                            ;
00028 // ---- protected auxiliary functions --------------------------------------- ;
00029 //                                                                            ;
00030 // ========================================================================== ;
00031 
00032 int CProtocolDevice::write8Bytes(int iModuleId, bool bAck, void* pBytes ) 
00033 {
00034         EnterCriticalSection(&m_csDevice);
00035         m_iErrorState = 0;
00036         static CProtocolMessage clWrite, clRead;
00037 
00038         clWrite.m_uiMessageId = MSGID_SET + iModuleId;
00039         memcpy( clWrite.m_aucMessageData, pBytes, 8 );
00040         clWrite.m_ucMessageLength = 8;
00041         clWrite.m_iModuleId = iModuleId;
00042         clRead = clWrite;
00043 
00044 //      warning("Sending %03x  %02x %02x %02x %02x %02x %02x %02x %02x", clWrite.m_uiMessageId, 
00045 //              clWrite.m_aucMessageData[0], clWrite.m_aucMessageData[1], clWrite.m_aucMessageData[2], clWrite.m_aucMessageData[3],
00046 //              clWrite.m_aucMessageData[4], clWrite.m_aucMessageData[5], clWrite.m_aucMessageData[6], clWrite.m_aucMessageData[7] );
00047 
00048         m_iErrorState = writeDevice(clWrite);
00049         if(m_iErrorState != 0)
00050         {
00051                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
00052                 LeaveCriticalSection(&m_csDevice);
00053                 return m_iErrorState;
00054         }
00055 
00056         if( bAck )
00057         {       
00058                         m_iErrorState = readDevice(clRead);
00059                         if(m_iErrorState != 0)
00060                         {
00061                                 //warning("wrong readDevice ErrorCode %i", m_iErrorState);
00062                                 LeaveCriticalSection(&m_csDevice);
00063                                 return m_iErrorState;
00064                         }
00065                         memcpy( pBytes, clRead.m_aucMessageData, clRead.m_ucMessageLength );
00066         }
00067         LeaveCriticalSection(&m_csDevice);
00068         return m_iErrorState;
00069 }
00070 
00071 int CProtocolDevice::getUnsignedLong(int iModuleId, unsigned long* puiData)
00072 {
00073         EnterCriticalSection(&m_csDevice);
00074         m_iErrorState = 0;
00075         bool bRecieved = false;
00076         static CProtocolMessage clRead;
00077         static CProtocolData clData;
00078         *puiData = 0;
00079 
00080         do
00081         {       
00082                 m_iErrorState = readDevice(clRead);
00083                 if(m_iErrorState != 0)
00084                 {
00085                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
00086                         LeaveCriticalSection(&m_csDevice);
00087                         return m_iErrorState;
00088                 }
00089                 bRecieved = true;
00090                 if(clRead.m_uiMessageId != MSGID_STATE + iModuleId)
00091                 {       
00092                         debug(1, "getUnsignedLong: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_STATE + iModuleId );
00093                         bRecieved = false;
00094                 }
00095         }
00096         while(!bRecieved);
00097         clData.aucData[0] = clRead.m_aucMessageData[0];
00098         clData.aucData[1] = clRead.m_aucMessageData[1];
00099         clData.aucData[2] = clRead.m_aucMessageData[2];
00100         clData.aucData[3] = clRead.m_aucMessageData[3];
00101         *puiData = clData.uiData;
00102         
00103         LeaveCriticalSection(&m_csDevice);
00104         return m_iErrorState;
00105 }
00106 
00107 int CProtocolDevice::readChar(int iModuleId, int iCommandId, int iParameterId, char* pcData)
00108 {
00109         EnterCriticalSection(&m_csDevice);
00110         m_iErrorState = 0;
00111         bool bRecieved = false;
00112         static CProtocolMessage clWrite, clRead;
00113 
00114         clWrite.m_uiMessageId = MSGID_GET + iModuleId;
00115         clWrite.m_aucMessageData[0] = iCommandId; 
00116         clWrite.m_aucMessageData[1] = iParameterId;
00117         clWrite.m_ucMessageLength = 2;
00118         clWrite.m_iModuleId = iModuleId;
00119         clRead = clWrite;
00120 
00121         m_iErrorState = writeDevice(clWrite);
00122         if(m_iErrorState != 0)
00123         {
00124                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
00125                 LeaveCriticalSection(&m_csDevice);
00126                 return m_iErrorState;
00127         }
00128 
00129         do
00130         {       
00131                 m_iErrorState = readDevice(clRead);
00132                 if(m_iErrorState != 0)
00133                 {
00134                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
00135                         LeaveCriticalSection(&m_csDevice);
00136                         return m_iErrorState;
00137                 }
00138                 bRecieved = true;
00139                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
00140                 {
00141                         debug(1, "readChar: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
00142                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00143                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00144                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00145                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00146                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00147                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00148                         bRecieved = false;
00149                 }
00150                 if(clRead.m_aucMessageData[0] != iCommandId)
00151                 {
00152                         debug(1,"readChar: wrong command ID");
00153                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00154                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00155                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00156                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00157                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00158                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00159                         bRecieved = false;
00160                 }
00161                 if(clRead.m_aucMessageData[1] != iParameterId)
00162                 {
00163                         debug(1,"readChar: wrong parameter ID");
00164                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00165                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00166                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00167                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00168                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00169                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00170                         bRecieved = false;
00171                 }
00172         }
00173         while(!bRecieved);
00174 
00175         *pcData = clRead.m_aucMessageData[2];
00176         LeaveCriticalSection(&m_csDevice);
00177 
00178         return m_iErrorState;
00179 }
00180 
00181 int CProtocolDevice::readUnsignedChar(int iModuleId, int iCommandId, int iParameterId, unsigned char* pucData)
00182 {
00183         EnterCriticalSection(&m_csDevice);
00184         m_iErrorState = 0;
00185         bool bRecieved = false;
00186         static CProtocolMessage clWrite, clRead;
00187 
00188         clWrite.m_uiMessageId = MSGID_GET + iModuleId;
00189         clWrite.m_aucMessageData[0] = iCommandId; 
00190         clWrite.m_aucMessageData[1] = iParameterId;
00191         clWrite.m_ucMessageLength = 2;
00192         clWrite.m_iModuleId = iModuleId;
00193         clRead = clWrite;
00194 
00195         m_iErrorState = writeDevice(clWrite);
00196         if(m_iErrorState != 0)
00197         {
00198                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
00199                 LeaveCriticalSection(&m_csDevice);
00200                 return m_iErrorState;
00201         }
00202 
00203         do
00204         {       
00205         m_iErrorState = readDevice(clRead);
00206                 if(m_iErrorState != 0)
00207                 {
00208                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
00209                         LeaveCriticalSection(&m_csDevice);
00210                         return m_iErrorState;
00211                 }
00212                 bRecieved = true;
00213                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
00214                 {
00215                         debug(1,"readUnsignedChar: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
00216                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00217                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00218                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00219                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00220                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00221                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00222                         bRecieved = false;
00223                 }
00224                 if(clRead.m_aucMessageData[0] != iCommandId)
00225                 {
00226                         debug(1,"readUnsignedChar: wrong command ID");
00227                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00228                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00229                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00230                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00231                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00232                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00233                         bRecieved = false;
00234                 }
00235                 if(clRead.m_aucMessageData[1] != iParameterId)
00236                 {
00237                         debug(1,"readUnsignedChar: wrong parameter ID");
00238                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00239                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00240                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00241                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00242                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00243                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00244                         bRecieved = false;
00245                 }
00246         }
00247         while(!bRecieved);
00248 
00249         *pucData = clRead.m_aucMessageData[2];
00250         LeaveCriticalSection(&m_csDevice);
00251 
00252         return m_iErrorState;
00253 }
00254 
00255 int CProtocolDevice::readShort(int iModuleId, int iCommandId, int iParameterId, short* piData)
00256 {
00257         EnterCriticalSection(&m_csDevice);
00258         m_iErrorState = 0;
00259         bool bRecieved = false;
00260         static CProtocolMessage clWrite, clRead;
00261         static CProtocolData clData;
00262 
00263         clWrite.m_uiMessageId = MSGID_GET + iModuleId;
00264         clWrite.m_aucMessageData[0] = iCommandId; 
00265         clWrite.m_aucMessageData[1] = iParameterId;
00266         clWrite.m_ucMessageLength = 2;
00267         clWrite.m_iModuleId = iModuleId;
00268         clRead = clWrite;
00269 
00270         m_iErrorState = writeDevice(clWrite);
00271         if(m_iErrorState != 0)
00272         {
00273                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
00274                 LeaveCriticalSection(&m_csDevice);
00275                 return m_iErrorState;
00276         }
00277 
00278         do
00279         {       
00280                 m_iErrorState = readDevice(clRead);
00281                 if(m_iErrorState != 0)
00282                 {
00283                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
00284                         LeaveCriticalSection(&m_csDevice);
00285                         return m_iErrorState;
00286                 }
00287                 bRecieved = true;
00288                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
00289                 {
00290                         debug(1,"readShort: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
00291                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00292                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00293                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00294                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00295                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00296                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00297                         bRecieved = false;
00298                 }
00299                 if(clRead.m_aucMessageData[0] != iCommandId)
00300                 {
00301                         debug(1,"readShort: wrong command ID");
00302                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00303                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00304                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00305                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00306                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00307                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00308                         bRecieved = false;
00309                 }
00310                 if(clRead.m_aucMessageData[1] != iParameterId)
00311                 {
00312                         debug(1,"readShort: wrong parameter ID");
00313                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00314                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00315                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00316                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00317                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00318                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00319                         bRecieved = false;
00320                 }
00321         }
00322         while(!bRecieved);
00323 
00324         clData.aucData[0] = clRead.m_aucMessageData[2];
00325         clData.aucData[1] = clRead.m_aucMessageData[3];
00326         *piData = clData.aiData[0];
00327         LeaveCriticalSection(&m_csDevice);
00328 
00329         return m_iErrorState;
00330 }
00331 
00332 int CProtocolDevice::readUnsignedShort(int iModuleId, int iCommandId, int iParameterId, unsigned short* puiData)
00333 {
00334         EnterCriticalSection(&m_csDevice);
00335         m_iErrorState = 0;
00336         bool bRecieved = false;
00337         static CProtocolMessage clWrite, clRead;
00338         static CProtocolData clData;
00339 
00340         clWrite.m_uiMessageId = MSGID_GET + iModuleId;
00341         clWrite.m_aucMessageData[0] = iCommandId; 
00342         clWrite.m_aucMessageData[1] = iParameterId;
00343         clWrite.m_ucMessageLength = 2;
00344         clWrite.m_iModuleId = iModuleId;
00345         clRead = clWrite;
00346 
00347         m_iErrorState = writeDevice(clWrite);
00348         if(m_iErrorState != 0)
00349         {
00350                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
00351                 LeaveCriticalSection(&m_csDevice);
00352                 return m_iErrorState;
00353         }
00354 
00355         do
00356         {       
00357                 m_iErrorState = readDevice(clRead);
00358                 if(m_iErrorState != 0)
00359                 {
00360                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
00361                         LeaveCriticalSection(&m_csDevice);
00362                         return m_iErrorState;
00363                 }
00364                 bRecieved = true;
00365                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
00366                 {
00367                         debug(1,"readUnsignedShort: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
00368                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00369                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00370                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00371                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00372                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00373                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00374                         bRecieved = false;
00375                 }
00376                 if(clRead.m_aucMessageData[0] != iCommandId)
00377                 {
00378                         debug(1,"readUnsignedShort: wrong command ID");
00379                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00380                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00381                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00382                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00383                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00384                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00385                         bRecieved = false;
00386                 }
00387                 if(clRead.m_aucMessageData[1] != iParameterId)
00388                 {
00389                         debug(1,"readUnsignedShort: wrong parameter ID");
00390                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00391                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00392                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00393                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00394                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00395                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00396                         bRecieved = false;
00397                 }
00398         }
00399         while(!bRecieved);
00400 
00401         clData.aucData[0] = clRead.m_aucMessageData[2];
00402         clData.aucData[1] = clRead.m_aucMessageData[3];
00403         *puiData = clData.auiData[0];
00404         LeaveCriticalSection(&m_csDevice);
00405 
00406         return m_iErrorState;
00407 }
00408 
00409 int CProtocolDevice::readLong(int iModuleId, int iCommandId, int iParameterId, long* piData)
00410 {
00411         EnterCriticalSection(&m_csDevice);
00412         m_iErrorState = 0;
00413         bool bRecieved = false;
00414         static CProtocolMessage clWrite, clRead;
00415         static CProtocolData clData;
00416 
00417         clWrite.m_uiMessageId = MSGID_GET + iModuleId;
00418         clWrite.m_aucMessageData[0] = iCommandId; 
00419         clWrite.m_aucMessageData[1] = iParameterId;
00420         clWrite.m_ucMessageLength = 2;
00421         clWrite.m_iModuleId = iModuleId;
00422         clRead = clWrite;
00423 
00424         m_iErrorState = writeDevice(clWrite);
00425         if(m_iErrorState != 0)
00426         {
00427                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
00428                 LeaveCriticalSection(&m_csDevice);
00429                 return m_iErrorState;
00430         }
00431 
00432         do
00433         {       
00434                 m_iErrorState = readDevice(clRead);
00435                 if(m_iErrorState != 0)
00436                 {
00437                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
00438                         LeaveCriticalSection(&m_csDevice);
00439                         return m_iErrorState;
00440                 }
00441                 bRecieved = true;
00442                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
00443                 {
00444                         debug(1,"readLong: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
00445                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00446                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00447                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00448                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00449                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00450                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00451                         bRecieved = false;
00452                 }
00453                 if(clRead.m_aucMessageData[0] != iCommandId)
00454                 {
00455                         debug(1,"readLong: wrong command ID");
00456                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00457                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00458                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00459                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00460                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00461                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00462                         bRecieved = false;
00463                 }
00464                 if(clRead.m_aucMessageData[1] != iParameterId)
00465                 {
00466                         debug(1,"readLong: wrong parameter ID");
00467                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00468                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00469                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00470                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00471                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00472                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00473                         bRecieved = false;
00474                 }
00475         }
00476         while(!bRecieved);
00477 
00478         clData.aucData[0] = clRead.m_aucMessageData[2];
00479         clData.aucData[1] = clRead.m_aucMessageData[3];
00480         clData.aucData[2] = clRead.m_aucMessageData[4];
00481         clData.aucData[3] = clRead.m_aucMessageData[5];
00482         *piData = clData.iData;
00483         LeaveCriticalSection(&m_csDevice);
00484 
00485         return m_iErrorState;
00486 }
00487 
00488 int CProtocolDevice::readUnsignedLong(int iModuleId, int iCommandId, int iParameterId, unsigned long* puiData)
00489 {
00490         EnterCriticalSection(&m_csDevice);
00491         m_iErrorState = 0;
00492         bool bRecieved = false;
00493         static CProtocolMessage clWrite, clRead;
00494         static CProtocolData clData;
00495 
00496         clWrite.m_uiMessageId = MSGID_GET + iModuleId;
00497         clWrite.m_aucMessageData[0] = iCommandId; 
00498         clWrite.m_aucMessageData[1] = iParameterId;
00499         clWrite.m_ucMessageLength = 2;
00500         clWrite.m_iModuleId = iModuleId;
00501         clRead = clWrite;
00502 
00503         m_iErrorState = writeDevice(clWrite);
00504         if(m_iErrorState != 0)
00505         {
00506                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
00507                 LeaveCriticalSection(&m_csDevice);
00508                 return m_iErrorState;
00509         }
00510         do
00511         {
00512                 m_iErrorState = readDevice(clRead);
00513                 if(m_iErrorState != 0)
00514                 {
00515                         //warning( "wrong readDevice ErrorCode %i", m_iErrorState );
00516                         LeaveCriticalSection(&m_csDevice);
00517                         return m_iErrorState;
00518                 }
00519                 bRecieved = true;
00520                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
00521                 {
00522                         debug(1,"readUnsignedLong: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
00523                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00524                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00525                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00526                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00527                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00528                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00529                         bRecieved = false;
00530                 }
00531 
00532                 if(clRead.m_aucMessageData[0] != iCommandId)
00533                 {
00534                         debug(1,"readUnsignedLong: wrong command ID");
00535                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00536                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00537                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00538                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00539                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00540                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00541                         bRecieved = false;
00542                 }
00543 
00544                 if(clRead.m_aucMessageData[1] != iParameterId)
00545                 {
00546                         debug(1,"readUnsignedLong: wrong parameter ID");
00547                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00548                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00549                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00550                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00551                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00552                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00553                         bRecieved = false;
00554                 }
00555         }
00556         while(!bRecieved);
00557 
00558         clData.aucData[0] = clRead.m_aucMessageData[2];
00559         clData.aucData[1] = clRead.m_aucMessageData[3];
00560         clData.aucData[2] = clRead.m_aucMessageData[4];
00561         clData.aucData[3] = clRead.m_aucMessageData[5];
00562         *puiData = clData.uiData;
00563         LeaveCriticalSection(&m_csDevice);
00564 
00565         return m_iErrorState;
00566 }
00567 
00568 int CProtocolDevice::readFloat(int iModuleId, int iCommandId, int iParameterId, float* pfData)
00569 {
00570         EnterCriticalSection(&m_csDevice);
00571         m_iErrorState = 0;
00572         bool bRecieved = false;
00573         static CProtocolMessage clWrite, clRead;
00574         static CProtocolData clData;
00575 
00576         clWrite.m_uiMessageId = MSGID_GET + iModuleId;
00577         clWrite.m_aucMessageData[0] = iCommandId; 
00578         clWrite.m_aucMessageData[1] = iParameterId;
00579         clWrite.m_ucMessageLength = 2;
00580         clWrite.m_iModuleId = iModuleId;
00581         clRead = clWrite;
00582 
00583         m_iErrorState = writeDevice(clWrite);
00584         if(m_iErrorState != 0)
00585         {
00586                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
00587                 LeaveCriticalSection(&m_csDevice);
00588                 return m_iErrorState;
00589         }
00590         
00591         do
00592         {
00593                 m_iErrorState = readDevice(clRead);
00594                 if(m_iErrorState != 0)
00595                 {
00596 //                      warning("wrong readDevice ErrorCode %i", m_iErrorState);
00597                         LeaveCriticalSection(&m_csDevice);
00598                         return m_iErrorState;
00599                 }
00600                 bRecieved = true;
00601                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
00602                 {
00603                         debug(1,"readFloat: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
00604                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00605                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00606                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00607                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00608                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00609                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00610                         bRecieved = false;
00611                 }
00612                 if(clRead.m_aucMessageData[0] != iCommandId)
00613                 {
00614                         debug(1,"readFloat: wrong command ID");
00615                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00616                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00617                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00618                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00619                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00620                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00621                         bRecieved = false;
00622                 }
00623                 if(clRead.m_aucMessageData[1] != iParameterId)
00624                 {
00625                         debug(1,"readFloat: wrong parameter ID");
00626                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00627                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00628                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00629                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00630                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00631                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00632                         bRecieved = false;
00633                 }
00634         }while(!bRecieved);
00635 
00636         clData.aucData[0] = clRead.m_aucMessageData[2];
00637         clData.aucData[1] = clRead.m_aucMessageData[3];
00638         clData.aucData[2] = clRead.m_aucMessageData[4];
00639         clData.aucData[3] = clRead.m_aucMessageData[5];
00640         *pfData = clData.fData;
00641         LeaveCriticalSection(&m_csDevice);
00642 
00643         return m_iErrorState;
00644 }
00645 
00646 int CProtocolDevice::readLongUnsignedChars(int iModuleId, int iCommandId, int iParameterId, long* piData, unsigned char* pucData1, unsigned char* pucData2)
00647 {
00648         EnterCriticalSection(&m_csDevice);
00649         m_iErrorState = 0;
00650         bool bRecieved = false;
00651         static CProtocolMessage clWrite, clRead;
00652         static CProtocolData clData;
00653 
00654         clWrite.m_uiMessageId = MSGID_GET + iModuleId;
00655         clWrite.m_aucMessageData[0] = iCommandId; 
00656         clWrite.m_aucMessageData[1] = iParameterId;
00657         clWrite.m_ucMessageLength = 2;
00658         clWrite.m_iModuleId = iModuleId;
00659         clRead = clWrite;
00660 
00661         m_iErrorState = writeDevice(clWrite);
00662         if(m_iErrorState != 0)
00663         {
00664                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
00665                 LeaveCriticalSection(&m_csDevice);
00666                 return m_iErrorState;
00667         }
00668 
00669         do
00670         {       
00671                 m_iErrorState = readDevice(clRead);
00672                 if(m_iErrorState != 0)
00673                 {
00674                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
00675                         LeaveCriticalSection(&m_csDevice);
00676                         return m_iErrorState;
00677                 }
00678                 bRecieved = true;
00679                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
00680                 {
00681                         debug(1,"readLongUnsignedChars: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
00682                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00683                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00684                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00685                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00686                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00687                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00688                         bRecieved = false;
00689                 }
00690                 if(clRead.m_aucMessageData[0] != iCommandId)
00691                 {
00692                         debug(1,"readLongUnsignedChars: wrong command ID");
00693                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00694                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00695                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00696                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00697                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00698                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00699                         bRecieved = false;
00700                 }
00701                 if(clRead.m_aucMessageData[1] != iParameterId)
00702                 {
00703                         debug(1,"readLongUnsignedChars: wrong parameter ID");
00704                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00705                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00706                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00707                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00708                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00709                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00710                         bRecieved = false;
00711                 }
00712         }
00713         while(!bRecieved);
00714 
00715         clData.aucData[0] = clRead.m_aucMessageData[2];
00716         clData.aucData[1] = clRead.m_aucMessageData[3];
00717         clData.aucData[2] = clRead.m_aucMessageData[4];
00718         clData.aucData[3] = clRead.m_aucMessageData[5];
00719         *piData = clData.iData;
00720         *pucData1 = clRead.m_aucMessageData[6];
00721         *pucData2 = clRead.m_aucMessageData[7];
00722         LeaveCriticalSection(&m_csDevice);
00723 
00724         return m_iErrorState;
00725 }
00726 
00727 int CProtocolDevice::readFloatUnsignedChars(int iModuleId, int iCommandId, int iParameterId, float* pfData, unsigned char* pucData1, unsigned char* pucData2)
00728 {
00729         EnterCriticalSection(&m_csDevice);
00730         m_iErrorState = 0;
00731         bool bRecieved = false;
00732         static CProtocolMessage clWrite, clRead;
00733         static CProtocolData clData;
00734 
00735         clWrite.m_uiMessageId = MSGID_GET + iModuleId;
00736         clWrite.m_aucMessageData[0] = iCommandId; 
00737         clWrite.m_aucMessageData[1] = iParameterId;
00738         clWrite.m_ucMessageLength = 2;
00739         clWrite.m_iModuleId = iModuleId;
00740         clRead = clWrite;
00741 
00742         m_iErrorState = writeDevice(clWrite);
00743         if(m_iErrorState != 0)
00744         {
00745                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
00746                 LeaveCriticalSection(&m_csDevice);
00747                 return m_iErrorState;
00748         }
00749 
00750         do
00751         {       
00752                 m_iErrorState = readDevice(clRead);
00753                 if(m_iErrorState != 0)
00754                 {
00755                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
00756                         LeaveCriticalSection(&m_csDevice);
00757                         return m_iErrorState;
00758                 }
00759                 bRecieved = true;
00760                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
00761                 {
00762                         debug(1,"readFloatUnsignedChars: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
00763                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00764                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00765                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00766                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00767                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00768                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00769                         bRecieved = false;
00770                 }
00771                 if(clRead.m_aucMessageData[0] != iCommandId)
00772                 {
00773                         debug(1,"readFloatUnsignedChars: wrong command ID");
00774                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00775                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00776                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00777                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00778                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00779                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00780                         bRecieved = false;
00781                 }
00782                 if(clRead.m_aucMessageData[1] != iParameterId)
00783                 {
00784                         debug(1,"readFloatUnsignedChars: wrong parameter ID");
00785                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00786                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00787                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00788                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00789                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00790                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00791                         bRecieved = false;
00792                 }
00793         }
00794         while(!bRecieved);
00795 
00796         clData.aucData[0] = clRead.m_aucMessageData[2];
00797         clData.aucData[1] = clRead.m_aucMessageData[3];
00798         clData.aucData[2] = clRead.m_aucMessageData[4];
00799         clData.aucData[3] = clRead.m_aucMessageData[5];
00800         *pfData = clData.fData;
00801         *pucData1 = clRead.m_aucMessageData[6];
00802         *pucData2 = clRead.m_aucMessageData[7];
00803         LeaveCriticalSection(&m_csDevice);
00804 
00805         return m_iErrorState;
00806 }
00807 
00808 int CProtocolDevice::writeChar(int iModuleId, int iCommandId, int iParameterId, char cData)
00809 {
00810         EnterCriticalSection(&m_csDevice);
00811         m_iErrorState = 0;
00812         bool bRecieved = false;
00813         static CProtocolMessage clWrite, clRead;
00814 
00815         clWrite.m_uiMessageId = MSGID_SET + iModuleId;
00816         clWrite.m_aucMessageData[0] = iCommandId; 
00817         clWrite.m_aucMessageData[1] = iParameterId;
00818         clWrite.m_aucMessageData[2] = cData;
00819         clWrite.m_ucMessageLength = 3;
00820         clWrite.m_iModuleId = iModuleId;
00821         clRead = clWrite;
00822 
00823         m_iErrorState = writeDevice(clWrite);
00824         if(m_iErrorState != 0)
00825         {
00826                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
00827                 LeaveCriticalSection(&m_csDevice);
00828                 return m_iErrorState;
00829         }
00830 
00831         do
00832         {       
00833                 m_iErrorState = readDevice(clRead);
00834                 if(m_iErrorState != 0)
00835                 {
00836                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
00837                         LeaveCriticalSection(&m_csDevice);
00838                         return m_iErrorState;
00839                 }
00840                 bRecieved = true;
00841                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
00842                 {
00843                         debug(1,"writeChar: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
00844                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00845                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00846                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00847                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00848                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00849                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00850                         bRecieved = false;
00851                 }
00852                 if(clRead.m_aucMessageData[0] != iCommandId)
00853                 {
00854                         debug(1,"writeChar: wrong command ID");
00855                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00856                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00857                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00858                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00859                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00860                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00861                         bRecieved = false;
00862                 }
00863                 if(clRead.m_aucMessageData[1] != iParameterId)
00864                 {
00865                         debug(1,"writeChar: wrong parameter ID");
00866                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00867                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00868                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00869                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00870                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00871                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00872                         bRecieved = false;
00873                 }
00874         }
00875         while(!bRecieved);
00876         LeaveCriticalSection(&m_csDevice);
00877 
00878         return m_iErrorState;
00879 }
00880 
00881 int CProtocolDevice::writeUnsignedChar(int iModuleId, int iCommandId, int iParameterId, unsigned char ucData)
00882 {
00883         EnterCriticalSection(&m_csDevice);
00884         m_iErrorState = 0;
00885         bool bRecieved = false;
00886         static CProtocolMessage clWrite, clRead;
00887 
00888         clWrite.m_uiMessageId = MSGID_SET + iModuleId;
00889         clWrite.m_aucMessageData[0] = iCommandId; 
00890         clWrite.m_aucMessageData[1] = iParameterId;
00891         clWrite.m_aucMessageData[2] = ucData;
00892         clWrite.m_ucMessageLength = 3;
00893         clWrite.m_iModuleId = iModuleId;
00894         clRead = clWrite;
00895 
00896         m_iErrorState = writeDevice(clWrite);
00897         if(m_iErrorState != 0)
00898         {
00899                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
00900                 LeaveCriticalSection(&m_csDevice);
00901                 return m_iErrorState;
00902         }
00903 
00904         do
00905         {       
00906                 m_iErrorState = readDevice(clRead);
00907                 if(m_iErrorState != 0)
00908                 {
00909                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
00910                         LeaveCriticalSection(&m_csDevice);
00911                         return m_iErrorState;
00912                 }
00913                 bRecieved = true;
00914                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
00915                 {
00916                         debug(1,"writeUnsignedChar: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
00917                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00918                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00919                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00920                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00921                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00922                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00923                         bRecieved = false;
00924                 }
00925                 if(clRead.m_aucMessageData[0] != iCommandId)
00926                 {
00927                         debug(1,"writeUnsignedChar: wrong command ID");
00928                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00929                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00930                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00931                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00932                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00933                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00934                         bRecieved = false;
00935                 }
00936                 if(clRead.m_aucMessageData[1] != iParameterId)
00937                 {
00938                         debug(1,"writeUnsignedChar: wrong parameter ID");
00939                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00940                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00941                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00942                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00943                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00944                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00945                         bRecieved = false;
00946                 }
00947         }
00948         while(!bRecieved);
00949         LeaveCriticalSection(&m_csDevice);
00950 
00951         return m_iErrorState;
00952 }
00953 
00954 int CProtocolDevice::writeShort(int iModuleId, int iCommandId, int iParameterId, short iData)
00955 {
00956         EnterCriticalSection(&m_csDevice);
00957         m_iErrorState = 0;
00958         bool bRecieved = false;
00959         static CProtocolMessage clWrite, clRead;
00960         static CProtocolData clData;
00961 
00962         clData.aiData[0] = iData;
00963         clWrite.m_uiMessageId = MSGID_SET + iModuleId;
00964         clWrite.m_aucMessageData[0] = iCommandId; 
00965         clWrite.m_aucMessageData[1] = iParameterId;
00966         clWrite.m_aucMessageData[2] = clData.aucData[0];
00967         clWrite.m_aucMessageData[3] = clData.aucData[1];
00968         clWrite.m_ucMessageLength = 4;
00969         clWrite.m_iModuleId = iModuleId;
00970         clRead = clWrite;
00971 
00972         m_iErrorState = writeDevice(clWrite);
00973         if(m_iErrorState != 0)
00974         {
00975                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
00976                 LeaveCriticalSection(&m_csDevice);
00977                 return m_iErrorState;
00978         }
00979 
00980         do
00981         {       
00982                 m_iErrorState = readDevice(clRead);
00983                 if(m_iErrorState != 0)
00984                 {
00985                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
00986                         LeaveCriticalSection(&m_csDevice);
00987                         return m_iErrorState;
00988                 }
00989                 bRecieved = true;
00990                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
00991                 {
00992                         debug(1,"writeShort: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
00993                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
00994                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
00995                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
00996                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
00997                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
00998                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
00999                         bRecieved = false;
01000                 }
01001                 if(clRead.m_aucMessageData[0] != iCommandId)
01002                 {
01003                         debug(1,"writeShort: wrong command ID");
01004                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01005                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01006                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01007                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01008                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01009                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01010                         bRecieved = false;
01011                 }
01012                 if(clRead.m_aucMessageData[1] != iParameterId)
01013                 {
01014                         debug(1,"writeShort: wrong parameter ID");
01015                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01016                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01017                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01018                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01019                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01020                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01021                         bRecieved = false;
01022                 }
01023         }
01024         while(!bRecieved);
01025         LeaveCriticalSection(&m_csDevice);
01026 
01027         return m_iErrorState;
01028 }
01029 
01030 int CProtocolDevice::writeUnsignedShort(int iModuleId, int iCommandId, int iParameterId, unsigned short uiData)
01031 {
01032         EnterCriticalSection(&m_csDevice);
01033         m_iErrorState = 0;
01034         bool bRecieved = false;
01035         static CProtocolMessage clWrite, clRead;
01036         static CProtocolData clData;
01037 
01038         clData.auiData[0] = uiData;
01039         clWrite.m_uiMessageId = MSGID_SET + iModuleId;
01040         clWrite.m_aucMessageData[0] = iCommandId; 
01041         clWrite.m_aucMessageData[1] = iParameterId;
01042         clWrite.m_aucMessageData[2] = clData.aucData[0];
01043         clWrite.m_aucMessageData[3] = clData.aucData[1];
01044         clWrite.m_ucMessageLength = 4;
01045         clWrite.m_iModuleId = iModuleId;
01046         clRead = clWrite;
01047 
01048         m_iErrorState = writeDevice(clWrite);
01049         if(m_iErrorState != 0)
01050         {
01051                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
01052                 LeaveCriticalSection(&m_csDevice);
01053                 return m_iErrorState;
01054         }
01055 
01056         do
01057         {       
01058                 m_iErrorState = readDevice(clRead);
01059                 if(m_iErrorState != 0)
01060                 {
01061                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
01062                         LeaveCriticalSection(&m_csDevice);
01063                         return m_iErrorState;
01064                 }
01065                 bRecieved = true;
01066                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
01067                 {
01068                         debug(1,"writeUnsignedShort: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
01069                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01070                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01071                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01072                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01073                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01074                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01075                         bRecieved = false;
01076                 }
01077                 if(clRead.m_aucMessageData[0] != iCommandId)
01078                 {
01079                         debug(1,"writeUnsignedShort: wrong command ID");
01080                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01081                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01082                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01083                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01084                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01085                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01086                         bRecieved = false;
01087                 }
01088                 if(clRead.m_aucMessageData[1] != iParameterId)
01089                 {
01090                         debug(1,"writeUnsignedShort: wrong parameter ID");
01091                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01092                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01093                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01094                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01095                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01096                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01097                         bRecieved = false;
01098                 }
01099         }
01100         while(!bRecieved);
01101         LeaveCriticalSection(&m_csDevice);
01102 
01103         return m_iErrorState;
01104 }
01105 
01106 int CProtocolDevice::writeLong(int iModuleId, int iCommandId, int iParameterId, long iData)
01107 {
01108         EnterCriticalSection(&m_csDevice);
01109         m_iErrorState = 0;
01110         bool bRecieved = false;
01111         static CProtocolMessage clWrite, clRead;
01112         static CProtocolData clData;
01113 
01114         clData.iData = iData;
01115         clWrite.m_uiMessageId = MSGID_SET + iModuleId;
01116         clWrite.m_aucMessageData[0] = iCommandId; 
01117         clWrite.m_aucMessageData[1] = iParameterId;
01118         clWrite.m_aucMessageData[2] = clData.aucData[0];
01119         clWrite.m_aucMessageData[3] = clData.aucData[1];
01120         clWrite.m_aucMessageData[4] = clData.aucData[2];
01121         clWrite.m_aucMessageData[5] = clData.aucData[3];
01122         clWrite.m_ucMessageLength = 6;
01123         clWrite.m_iModuleId = iModuleId;
01124         clRead = clWrite;
01125 
01126         m_iErrorState = writeDevice(clWrite);
01127         if(m_iErrorState != 0)
01128         {
01129                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
01130                 LeaveCriticalSection(&m_csDevice);
01131                 return m_iErrorState;
01132         }
01133 
01134         do
01135         {       
01136                 m_iErrorState = readDevice(clRead);
01137                 if(m_iErrorState != 0)
01138                 {
01139                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
01140                         LeaveCriticalSection(&m_csDevice);
01141                         return m_iErrorState;
01142                 }
01143                 bRecieved = true;
01144                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
01145                 {
01146                         debug(1,"writeLong: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
01147                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01148                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01149                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01150                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01151                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01152                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01153                         bRecieved = false;
01154                 }
01155                 if(clRead.m_aucMessageData[0] != iCommandId)
01156                 {
01157                         debug(1,"writeLong: wrong command ID");
01158                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01159                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01160                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01161                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01162                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01163                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01164                         bRecieved = false;
01165                 }
01166                 if(clRead.m_aucMessageData[1] != iParameterId)
01167                 {
01168                         debug(1,"writeLong: wrong parameter ID");
01169                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01170                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01171                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01172                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01173                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01174                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01175                         bRecieved = false;
01176                 }
01177         }
01178         while(!bRecieved);
01179         LeaveCriticalSection(&m_csDevice);
01180 
01181         return m_iErrorState;
01182 }
01183 
01184 int CProtocolDevice::writeUnsignedLong(int iModuleId, int iCommandId, int iParameterId, unsigned long uiData)
01185 {
01186         EnterCriticalSection(&m_csDevice);
01187         m_iErrorState = 0;
01188         bool bRecieved = false;
01189         static CProtocolMessage clWrite, clRead;
01190         static CProtocolData clData;
01191 
01192         clData.uiData = uiData;
01193         clWrite.m_uiMessageId = MSGID_SET + iModuleId;
01194         clWrite.m_aucMessageData[0] = iCommandId; 
01195         clWrite.m_aucMessageData[1] = iParameterId;
01196         clWrite.m_aucMessageData[2] = clData.aucData[0];
01197         clWrite.m_aucMessageData[3] = clData.aucData[1];
01198         clWrite.m_aucMessageData[4] = clData.aucData[2];
01199         clWrite.m_aucMessageData[5] = clData.aucData[3];
01200         clWrite.m_ucMessageLength = 6;
01201         clWrite.m_iModuleId = iModuleId;
01202         clRead = clWrite;
01203 
01204         m_iErrorState = writeDevice(clWrite);
01205         if(m_iErrorState != 0)
01206         {
01207                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
01208                 LeaveCriticalSection(&m_csDevice);
01209                 return m_iErrorState;
01210         }
01211 
01212         do
01213         {       
01214                 m_iErrorState = readDevice(clRead);
01215                 if(m_iErrorState != 0)
01216                 {
01217                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
01218                         LeaveCriticalSection(&m_csDevice);
01219                         return m_iErrorState;
01220                 }
01221                 bRecieved = true;
01222                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
01223                 {
01224                         debug(1,"writeUnsignedLong: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
01225                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01226                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01227                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01228                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01229                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01230                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01231                         bRecieved = false;
01232                 }
01233                 if(clRead.m_aucMessageData[0] != iCommandId)
01234                 {
01235                         debug(1,"writeUnsignedLong: wrong command ID");
01236                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01237                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01238                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01239                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01240                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01241                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01242                         bRecieved = false;
01243                 }
01244                 if(clRead.m_aucMessageData[1] != iParameterId)
01245                 {
01246                         debug(1,"writeUnsignedLong: wrong parameter ID");
01247                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01248                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01249                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01250                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01251                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01252                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01253                         bRecieved = false;
01254                 }
01255         }
01256         while(!bRecieved);
01257         LeaveCriticalSection(&m_csDevice);
01258 
01259         return m_iErrorState;
01260 }
01261 
01262 int CProtocolDevice::writeFloat(int iModuleId, int iCommandId, int iParameterId, float fData)
01263 {
01264         EnterCriticalSection(&m_csDevice);
01265         m_iErrorState = 0;
01266         bool bRecieved = false;
01267         static CProtocolMessage clWrite, clRead;
01268         static CProtocolData clData;
01269 
01270         clData.fData = fData;
01271         clWrite.m_uiMessageId = MSGID_SET + iModuleId;
01272         clWrite.m_aucMessageData[0] = iCommandId; 
01273         clWrite.m_aucMessageData[1] = iParameterId;
01274         clWrite.m_aucMessageData[2] = clData.aucData[0];
01275         clWrite.m_aucMessageData[3] = clData.aucData[1];
01276         clWrite.m_aucMessageData[4] = clData.aucData[2];
01277         clWrite.m_aucMessageData[5] = clData.aucData[3];
01278         clWrite.m_ucMessageLength = 6;
01279         clWrite.m_iModuleId = iModuleId;
01280         clRead = clWrite;
01281 
01282         m_iErrorState = writeDevice(clWrite);
01283         if(m_iErrorState != 0)
01284         {
01285                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
01286                 LeaveCriticalSection(&m_csDevice);
01287                 return m_iErrorState;
01288         }
01289 
01290         do
01291         {       
01292                 m_iErrorState = readDevice(clRead);
01293                 if(m_iErrorState != 0)
01294                 {
01295                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
01296                         LeaveCriticalSection(&m_csDevice);
01297                         return m_iErrorState;
01298                 }
01299                 bRecieved = true;
01300                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
01301                 {
01302                         debug(1,"writeFloat: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
01303                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01304                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01305                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01306                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01307                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01308                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01309                         bRecieved = false;
01310                 }
01311                 if(clRead.m_aucMessageData[0] != iCommandId)
01312                 {
01313                         debug(1,"writeFloat: wrong command ID");
01314                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01315                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01316                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01317                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01318                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01319                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01320                         bRecieved = false;
01321                 }
01322                 if(clRead.m_aucMessageData[1] != iParameterId)
01323                 {
01324                         debug(1,"writeFloat: wrong parameter ID");
01325                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01326                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01327                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01328                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01329                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01330                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01331                         bRecieved = false;
01332                 }
01333         }
01334         while(!bRecieved);
01335         LeaveCriticalSection(&m_csDevice);
01336 
01337         return m_iErrorState;
01338 }
01339 
01340 int CProtocolDevice::writeAll(int iCommandId, int iParameterId)
01341 {
01342         EnterCriticalSection(&m_csDevice);
01343         m_iErrorState = 0;
01344         static CProtocolMessage clWrite, clRead;
01345 
01346         clWrite.m_uiMessageId = MSGID_ALL;
01347         clWrite.m_aucMessageData[0] = iCommandId; 
01348         clWrite.m_aucMessageData[1] = iParameterId;
01349         clWrite.m_ucMessageLength = 2;
01350         clWrite.m_iModuleId = 0;
01351 
01352         m_iErrorState = writeDevice(clWrite);
01353         if(m_iErrorState != 0)
01354         {
01355                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
01356                 LeaveCriticalSection(&m_csDevice);
01357                 return m_iErrorState;
01358         }
01359 
01360         LeaveCriticalSection(&m_csDevice);
01361 
01362         return m_iErrorState;
01363 }
01364 
01365 int CProtocolDevice::writeCommand(int iModuleId, int iCommandId)
01366 {
01367         EnterCriticalSection(&m_csDevice);
01368         m_iErrorState = 0;
01369         bool bRecieved = false;
01370         static CProtocolMessage clWrite, clRead;
01371 
01372         clWrite.m_uiMessageId = MSGID_SET + iModuleId;
01373         clWrite.m_aucMessageData[0] = iCommandId; 
01374         clWrite.m_ucMessageLength = 1;
01375         clWrite.m_iModuleId = iModuleId;
01376         clRead = clWrite;
01377 
01378         m_iErrorState = writeDevice(clWrite);
01379         if(m_iErrorState != 0)
01380         {
01381                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
01382                 LeaveCriticalSection(&m_csDevice);
01383                 return m_iErrorState;
01384         }
01385 
01386         do
01387         {       
01388                 m_iErrorState = readDevice(clRead);
01389                 if(m_iErrorState != 0)
01390                 {
01391                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
01392                         LeaveCriticalSection(&m_csDevice);
01393                         return m_iErrorState;
01394                 }
01395                 bRecieved = true;
01396                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
01397                 {
01398                         debug(1,"writeCommand: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
01399                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01400                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01401                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01402                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01403                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01404                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01405                         bRecieved = false;
01406                 }
01407                 if(clRead.m_aucMessageData[0] != iCommandId)
01408                 {
01409                         debug(1,"writeCommand: wrong command ID");
01410                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01411                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01412                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01413                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01414                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01415                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01416                         bRecieved = false;
01417                 }
01418         }
01419         while(!bRecieved);
01420         LeaveCriticalSection(&m_csDevice);
01421 
01422         return m_iErrorState;
01423 }
01424 
01425 int CProtocolDevice::writeLongShort(int iModuleId, int iCommandId, int iParameterId, long iData, short iTime)
01426 {
01427         EnterCriticalSection(&m_csDevice);
01428         m_iErrorState = 0;
01429         bool bRecieved = false;
01430         static CProtocolMessage clWrite, clRead;
01431         static CProtocolData clData;
01432 
01433         clWrite.m_uiMessageId = MSGID_SET + iModuleId;
01434         clWrite.m_aucMessageData[0] = iCommandId; 
01435         clWrite.m_aucMessageData[1] = iParameterId;
01436         clData.iData = iData;
01437         clWrite.m_aucMessageData[2] = clData.aucData[0];
01438         clWrite.m_aucMessageData[3] = clData.aucData[1];
01439         clWrite.m_aucMessageData[4] = clData.aucData[2];
01440         clWrite.m_aucMessageData[5] = clData.aucData[3];
01441         clData.aiData[0] = iTime;
01442         clWrite.m_aucMessageData[6] = clData.aucData[0];
01443         clWrite.m_aucMessageData[7] = clData.aucData[1];
01444         clWrite.m_ucMessageLength = 8;
01445         clWrite.m_iModuleId = iModuleId;
01446         clRead = clWrite;
01447 
01448         m_iErrorState = writeDevice(clWrite);
01449         if(m_iErrorState != 0)
01450         {
01451                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
01452                 LeaveCriticalSection(&m_csDevice);
01453                 return m_iErrorState;
01454         }
01455 
01456         do
01457         {       
01458                 m_iErrorState = readDevice(clRead);
01459                 if(m_iErrorState != 0)
01460                 {
01461                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
01462                         LeaveCriticalSection(&m_csDevice);
01463                         return m_iErrorState;
01464                 }
01465                 bRecieved = true;
01466                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
01467                 {
01468                         debug(1,"writeLongShort: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
01469                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01470                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01471                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01472                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01473                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01474                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01475                         bRecieved = false;
01476                 }
01477                 if(clRead.m_aucMessageData[0] != iCommandId)
01478                 {
01479                         debug(1,"writeLongShort: wrong command ID");
01480                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01481                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01482                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01483                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01484                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01485                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01486                         bRecieved = false;
01487                 }
01488                 if(clRead.m_aucMessageData[1] != iParameterId)
01489                 {
01490                         debug(1,"writeLongShort: wrong parameter ID");
01491                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01492                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01493                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01494                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01495                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01496                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01497                         bRecieved = false;
01498                 }
01499         }
01500         while(!bRecieved);
01501         LeaveCriticalSection(&m_csDevice);
01502 
01503         return m_iErrorState;
01504 }
01505 
01506 int CProtocolDevice::writeFloatShort(int iModuleId, int iCommandId, int iParameterId, float fData, short iData)
01507 {
01508         EnterCriticalSection(&m_csDevice);
01509         m_iErrorState = 0;
01510         bool bRecieved = false;
01511         static CProtocolMessage clWrite, clRead;
01512         static CProtocolData clData;
01513 
01514         clWrite.m_uiMessageId = MSGID_SET + iModuleId;
01515         clWrite.m_aucMessageData[0] = iCommandId; 
01516         clWrite.m_aucMessageData[1] = iParameterId;
01517         clData.fData = fData;
01518         clWrite.m_aucMessageData[2] = clData.aucData[0];
01519         clWrite.m_aucMessageData[3] = clData.aucData[1];
01520         clWrite.m_aucMessageData[4] = clData.aucData[2];
01521         clWrite.m_aucMessageData[5] = clData.aucData[3];
01522         clData.aiData[0] = iData;
01523         clWrite.m_aucMessageData[6] = clData.aucData[0];
01524         clWrite.m_aucMessageData[7] = clData.aucData[1];
01525         clWrite.m_ucMessageLength = 8;
01526         clWrite.m_iModuleId = iModuleId;
01527         clRead = clWrite;
01528 
01529         m_iErrorState = writeDevice(clWrite);
01530         if(m_iErrorState != 0)
01531         {
01532                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
01533                 LeaveCriticalSection(&m_csDevice);
01534                 return m_iErrorState;
01535         }
01536 
01537         do
01538         {       
01539                 m_iErrorState = readDevice(clRead);
01540                 if(m_iErrorState != 0)
01541                 {
01542                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
01543                         LeaveCriticalSection(&m_csDevice);
01544                         return m_iErrorState;
01545                 }
01546                 bRecieved = true;
01547                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
01548                 {
01549                         debug(1,"writeFloatShort: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
01550                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01551                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01552                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01553                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01554                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01555                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01556                         bRecieved = false;
01557                 }
01558                 if(clRead.m_aucMessageData[0] != iCommandId)
01559                 {
01560                         debug(1,"writeFloatShort: wrong command ID");
01561                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01562                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01563                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01564                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01565                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01566                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01567                         bRecieved = false;
01568                 }
01569                 if(clRead.m_aucMessageData[1] != iParameterId)
01570                 {
01571                         debug(1,"writeFloatShort: wrong parameter ID");
01572                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01573                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01574                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01575                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01576                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01577                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01578                         bRecieved = false;
01579                 }
01580         }
01581         while(!bRecieved);
01582         LeaveCriticalSection(&m_csDevice);
01583 
01584         return m_iErrorState;
01585 }
01586 
01587 int CProtocolDevice::writeShortReadLongUnsignedChars(int iModuleId, int iCommandId, int iParameterId, short iData, long* piData, unsigned char* pucData1, unsigned char* pucData2)
01588 {
01589         EnterCriticalSection(&m_csDevice);
01590         m_iErrorState = 0;
01591         bool bRecieved = false;
01592         static CProtocolMessage clWrite, clRead;
01593         static CProtocolData clData;
01594 
01595         clData.aiData[0] = iData;
01596         clWrite.m_uiMessageId = MSGID_SET + iModuleId;
01597         clWrite.m_aucMessageData[0] = iCommandId; 
01598         clWrite.m_aucMessageData[1] = iParameterId;
01599         clWrite.m_aucMessageData[2] = clData.aucData[0];
01600         clWrite.m_aucMessageData[3] = clData.aucData[1];
01601         clWrite.m_ucMessageLength = 4;
01602         clWrite.m_iModuleId = iModuleId;
01603         clRead = clWrite;
01604 
01605         m_iErrorState = writeDevice(clWrite);
01606         if(m_iErrorState != 0)
01607         {
01608                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
01609                 LeaveCriticalSection(&m_csDevice);
01610                 return m_iErrorState;
01611         }
01612 
01613         do
01614         {       
01615                 m_iErrorState = readDevice(clRead);
01616                 if(m_iErrorState != 0)
01617                 {
01618                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
01619                         LeaveCriticalSection(&m_csDevice);
01620                         return m_iErrorState;
01621                 }
01622                 bRecieved = true;
01623                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
01624                 {
01625                         debug(1,"writeShortReadUnsignedLongChars: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
01626                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01627                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01628                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01629                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01630                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01631                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01632                         bRecieved = false;
01633                 }
01634                 if(clRead.m_aucMessageData[0] != iCommandId)
01635                 {
01636                         debug(1,"writeShortReadUnsignedLongChars: wrong command ID");
01637                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01638                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01639                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01640                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01641                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01642                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01643                         bRecieved = false;
01644                 }
01645                 if(clRead.m_aucMessageData[1] != iParameterId)
01646                 {
01647                         debug(1,"writeShortReadUnsignedLongChars: wrong parameter ID");
01648                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01649                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01650                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01651                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01652                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01653                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01654                         bRecieved = false;
01655                 }
01656         }
01657         while(!bRecieved);
01658 
01659         clData.aucData[0] = clRead.m_aucMessageData[2];
01660         clData.aucData[1] = clRead.m_aucMessageData[3];
01661         clData.aucData[2] = clRead.m_aucMessageData[4];
01662         clData.aucData[3] = clRead.m_aucMessageData[5];
01663         *piData = clData.iData;
01664         *pucData1 = clRead.m_aucMessageData[6];
01665         *pucData2 = clRead.m_aucMessageData[7];
01666         LeaveCriticalSection(&m_csDevice);
01667 
01668         return m_iErrorState;
01669 }
01670 
01671 int CProtocolDevice::writeLongReadLongUnsignedChars(int iModuleId, int iCommandId, int iParameterId, long iData, long* piData, unsigned char* pucData1, unsigned char* pucData2)
01672 {
01673         EnterCriticalSection(&m_csDevice);
01674         m_iErrorState = 0;
01675         bool bRecieved = false;
01676         static CProtocolMessage clWrite, clRead;
01677         static CProtocolData clData;
01678 
01679         clData.iData = iData;
01680         clWrite.m_uiMessageId = MSGID_SET + iModuleId;
01681         clWrite.m_aucMessageData[0] = iCommandId; 
01682         clWrite.m_aucMessageData[1] = iParameterId;
01683         clWrite.m_aucMessageData[2] = clData.aucData[0];
01684         clWrite.m_aucMessageData[3] = clData.aucData[1];
01685         clWrite.m_aucMessageData[4] = clData.aucData[2];
01686         clWrite.m_aucMessageData[5] = clData.aucData[3];
01687         clWrite.m_ucMessageLength = 6;
01688         clWrite.m_iModuleId = iModuleId;
01689         clRead = clWrite;
01690 
01691         m_iErrorState = writeDevice(clWrite);
01692         if(m_iErrorState != 0)
01693         {
01694                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
01695                 LeaveCriticalSection(&m_csDevice);
01696                 return m_iErrorState;
01697         }
01698 
01699         do
01700         {       
01701                 m_iErrorState = readDevice(clRead);
01702                 if(m_iErrorState != 0)
01703                 {
01704                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
01705                         LeaveCriticalSection(&m_csDevice);
01706                         return m_iErrorState;
01707                 }
01708                 bRecieved = true;
01709                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
01710                 {
01711                         debug(1,"writeLongReadUnsignedLongChars: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
01712                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01713                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01714                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01715                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01716                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01717                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01718                         bRecieved = false;
01719                 }
01720                 if(clRead.m_aucMessageData[0] != iCommandId)
01721                 {
01722                         debug(1,"writeLongReadUnsignedLongChars: wrong command ID");
01723                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01724                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01725                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01726                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01727                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01728                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01729                         bRecieved = false;
01730                 }
01731                 if(clRead.m_aucMessageData[1] != iParameterId)
01732                 {
01733                         debug(1,"writeLongReadUnsignedLongChars: wrong parameter ID");
01734                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01735                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01736                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01737                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01738                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01739                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01740                         bRecieved = false;
01741                 }
01742         }
01743         while(!bRecieved);
01744 
01745         clData.aucData[0] = clRead.m_aucMessageData[2];
01746         clData.aucData[1] = clRead.m_aucMessageData[3];
01747         clData.aucData[2] = clRead.m_aucMessageData[4];
01748         clData.aucData[3] = clRead.m_aucMessageData[5];
01749         *piData = clData.iData;
01750         *pucData1 = clRead.m_aucMessageData[6];
01751         *pucData2 = clRead.m_aucMessageData[7];
01752         LeaveCriticalSection(&m_csDevice);
01753 
01754         return m_iErrorState;
01755 }
01756 
01757 int CProtocolDevice::writeLongShortReadLongUnsignedChars(int iModuleId, int iCommandId, int iParameterId, long iData1, short iData2, long* piData, unsigned char* pucData1, unsigned char* pucData2)
01758 {
01759         EnterCriticalSection(&m_csDevice);
01760         m_iErrorState = 0;
01761         bool bRecieved = false;
01762         static CProtocolMessage clWrite, clRead;
01763         static CProtocolData clData;
01764 
01765         clWrite.m_uiMessageId = MSGID_SET + iModuleId;
01766         clWrite.m_aucMessageData[0] = iCommandId; 
01767         clWrite.m_aucMessageData[1] = iParameterId;
01768         clData.iData = iData1;
01769         clWrite.m_aucMessageData[2] = clData.aucData[0];
01770         clWrite.m_aucMessageData[3] = clData.aucData[1];
01771         clWrite.m_aucMessageData[4] = clData.aucData[2];
01772         clWrite.m_aucMessageData[5] = clData.aucData[3];
01773         clData.aiData[0] = iData2;
01774         clWrite.m_aucMessageData[6] = clData.aucData[0];
01775         clWrite.m_aucMessageData[7] = clData.aucData[1];
01776         clWrite.m_ucMessageLength = 8;
01777         clWrite.m_iModuleId = iModuleId;
01778         clRead = clWrite;
01779 
01780         m_iErrorState = writeDevice(clWrite);
01781         if(m_iErrorState != 0)
01782         {
01783                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
01784                 LeaveCriticalSection(&m_csDevice);
01785                 return m_iErrorState;
01786         }
01787 
01788         do
01789         {       
01790                 m_iErrorState = readDevice(clRead);
01791                 if(m_iErrorState != 0)
01792                 {
01793                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
01794                         LeaveCriticalSection(&m_csDevice);
01795                         return m_iErrorState;
01796                 }
01797                 bRecieved = true;
01798                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
01799                 {
01800                         debug(1,"writeLongShortReadUnsignedLongChars: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
01801                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01802                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01803                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01804                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01805                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01806                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01807                         bRecieved = false;
01808                 }
01809                 if(clRead.m_aucMessageData[0] != iCommandId)
01810                 {
01811                         debug(1,"writeLongShortReadUnsignedLongChars: wrong command ID");
01812                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01813                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01814                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01815                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01816                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01817                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01818                         bRecieved = false;
01819                 }
01820                 if(clRead.m_aucMessageData[1] != iParameterId)
01821                 {
01822                         debug(1,"writeLongShortReadUnsignedLongChars: wrong parameter ID");
01823                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01824                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01825                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01826                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01827                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01828                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01829                         bRecieved = false;
01830                 }
01831         }
01832         while(!bRecieved);
01833 
01834         clData.aucData[0] = clRead.m_aucMessageData[2];
01835         clData.aucData[1] = clRead.m_aucMessageData[3];
01836         clData.aucData[2] = clRead.m_aucMessageData[4];
01837         clData.aucData[3] = clRead.m_aucMessageData[5];
01838         *piData = clData.iData;
01839         *pucData1 = clRead.m_aucMessageData[6];
01840         *pucData2 = clRead.m_aucMessageData[7];
01841         LeaveCriticalSection(&m_csDevice);
01842 
01843         return m_iErrorState;
01844 }
01845 
01846 int CProtocolDevice::writeFloatReadFloatUnsignedChars(int iModuleId, int iCommandId, int iParameterId, float fData, float* pfData, unsigned char* pucData1, unsigned char* pucData2)
01847 {
01848         EnterCriticalSection(&m_csDevice);
01849         m_iErrorState = 0;
01850         bool bRecieved = false;
01851         static CProtocolMessage clWrite, clRead;
01852         static CProtocolData clData;
01853 
01854         clData.fData = fData;
01855         clWrite.m_uiMessageId = MSGID_SET + iModuleId;
01856         clWrite.m_aucMessageData[0] = iCommandId; 
01857         clWrite.m_aucMessageData[1] = iParameterId;
01858         clWrite.m_aucMessageData[2] = clData.aucData[0];
01859         clWrite.m_aucMessageData[3] = clData.aucData[1];
01860         clWrite.m_aucMessageData[4] = clData.aucData[2];
01861         clWrite.m_aucMessageData[5] = clData.aucData[3];
01862         clWrite.m_ucMessageLength = 6;
01863         clWrite.m_iModuleId = iModuleId;
01864         clRead = clWrite;
01865 
01866         m_iErrorState = writeDevice(clWrite);
01867         if(m_iErrorState != 0)
01868         {
01869                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
01870                 LeaveCriticalSection(&m_csDevice);
01871                 return m_iErrorState;
01872         }
01873 
01874         do
01875         {       
01876                 m_iErrorState = readDevice(clRead);
01877                 if(m_iErrorState != 0)
01878                 {
01879                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
01880                         LeaveCriticalSection(&m_csDevice);
01881                         return m_iErrorState;
01882                 }
01883                 bRecieved = true;
01884                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
01885                 {
01886                         debug(1,"writeFloatReadUnsignedLongChars: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
01887                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01888                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01889                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01890                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01891                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01892                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01893                         bRecieved = false;
01894                 }
01895                 if(clRead.m_aucMessageData[0] != iCommandId)
01896                 {
01897                         debug(1,"writeFloatReadUnsignedLongChars: wrong command ID");
01898                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01899                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01900                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01901                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01902                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01903                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01904                         bRecieved = false;
01905                 }
01906                 if(clRead.m_aucMessageData[1] != iParameterId)
01907                 {
01908                         debug(1,"writeFloatReadUnsignedLongChars: wrong parameter ID");
01909                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01910                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01911                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01912                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01913                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01914                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01915                         bRecieved = false;
01916                 }
01917         }
01918         while(!bRecieved);
01919 
01920         clData.aucData[0] = clRead.m_aucMessageData[2];
01921         clData.aucData[1] = clRead.m_aucMessageData[3];
01922         clData.aucData[2] = clRead.m_aucMessageData[4];
01923         clData.aucData[3] = clRead.m_aucMessageData[5];
01924         *pfData = clData.fData;
01925         *pucData1 = clRead.m_aucMessageData[6];
01926         *pucData2 = clRead.m_aucMessageData[7];
01927         LeaveCriticalSection(&m_csDevice);
01928 
01929         return m_iErrorState;
01930 }
01931 
01932 int CProtocolDevice::writeFloatShortReadFloatUnsignedChars(int iModuleId, int iCommandId, int iParameterId, float fData, short iData, float* pfData, unsigned char* pucData1, unsigned char* pucData2)
01933 {
01934         EnterCriticalSection(&m_csDevice);
01935         m_iErrorState = 0;
01936         bool bRecieved = false;
01937         static CProtocolMessage clWrite, clRead;
01938         static CProtocolData clData;
01939 
01940         clWrite.m_uiMessageId = MSGID_SET + iModuleId;
01941         clWrite.m_aucMessageData[0] = iCommandId; 
01942         clWrite.m_aucMessageData[1] = iParameterId;
01943         clData.fData = fData;
01944         clWrite.m_aucMessageData[2] = clData.aucData[0];
01945         clWrite.m_aucMessageData[3] = clData.aucData[1];
01946         clWrite.m_aucMessageData[4] = clData.aucData[2];
01947         clWrite.m_aucMessageData[5] = clData.aucData[3];
01948         clData.aiData[0] = iData;
01949         clWrite.m_aucMessageData[6] = clData.aucData[0];
01950         clWrite.m_aucMessageData[7] = clData.aucData[1];
01951         clWrite.m_ucMessageLength = 8;
01952         clWrite.m_iModuleId = iModuleId;
01953         clRead = clWrite;
01954 
01955         m_iErrorState = writeDevice(clWrite);
01956         if(m_iErrorState != 0)
01957         {
01958                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
01959                 LeaveCriticalSection(&m_csDevice);
01960                 return m_iErrorState;
01961         }
01962 
01963         do
01964         {       
01965                 m_iErrorState = readDevice(clRead);
01966                 if(m_iErrorState != 0)
01967                 {
01968                         //warning("wrong readDevice ErrorCode %i", m_iErrorState);
01969                         LeaveCriticalSection(&m_csDevice);
01970                         return m_iErrorState;
01971                 }
01972                 bRecieved = true;
01973                 if(clRead.m_uiMessageId != MSGID_ACK + iModuleId)
01974                 {
01975                         debug(1,"writeFloatShortReadUnsignedLongChars: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_ACK + iModuleId );
01976                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01977                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01978                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01979                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01980                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01981                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01982                         bRecieved = false;
01983                 }
01984                 if(clRead.m_aucMessageData[0] != iCommandId)
01985                 {
01986                         debug(1,"writeFloatShortReadUnsignedLongChars: wrong command ID");
01987                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01988                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
01989                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
01990                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
01991                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
01992                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
01993                         bRecieved = false;
01994                 }
01995                 if(clRead.m_aucMessageData[1] != iParameterId)
01996                 {
01997                         debug(1,"writeFloatShortReadUnsignedLongChars: wrong parameter ID");
01998                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
01999                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
02000                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
02001                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
02002                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
02003                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
02004                         bRecieved = false;
02005                 }
02006         }
02007         while(!bRecieved);
02008 
02009         clData.aucData[0] = clRead.m_aucMessageData[2];
02010         clData.aucData[1] = clRead.m_aucMessageData[3];
02011         clData.aucData[2] = clRead.m_aucMessageData[4];
02012         clData.aucData[3] = clRead.m_aucMessageData[5];
02013         *pfData = clData.fData;
02014         *pucData1 = clRead.m_aucMessageData[6];
02015         *pucData2 = clRead.m_aucMessageData[7];
02016         LeaveCriticalSection(&m_csDevice);
02017 
02018         return m_iErrorState;
02019 }
02020 
02021 // ========================================================================== ;
02022 //                                                                            ;
02023 // ---- constructors / destructor ------------------------------------------- ;
02024 //                                                                            ;
02025 // ========================================================================== ;
02026 
02027 CProtocolDevice::CProtocolDevice() //: m_iEMSModuleCount(0), m_iEMSModuleCountMax(31)
02028 {
02029 }
02030 
02031 CProtocolDevice::CProtocolDevice(const CProtocolDevice& rclProtocolDevice)
02032 {
02033         error(-1, "Sorry constructor is not implemented");
02034 }
02035 
02036 CProtocolDevice::~CProtocolDevice()
02037 {
02038 }
02039 
02040 // ========================================================================== ;
02041 //                                                                            ;
02042 // ---- operators ----------------------------------------------------------- ;
02043 //                                                                            ;
02044 // ========================================================================== ;
02045 
02046 CProtocolDevice& CProtocolDevice::operator=(const CProtocolDevice& rclProtocolDevice)
02047 {
02048         error(-1, "Sorry operator= is not implemented");
02049         return *this;
02050 }
02051 
02052 // ========================================================================== ;
02053 //                                                                            ;
02054 // ---- query functions ----------------------------------------------------- ;
02055 //                                                                            ;
02056 // ========================================================================== ;
02057 
02058 int CProtocolDevice::getDataDLR_FTS(std::vector<float>& rafData, long* piState)
02059 {
02060         EnterCriticalSection(&m_csDevice);
02061         m_iErrorState = 0;
02062         bool bRecieved = false;
02063         static CProtocolMessage clWrite, clRead;
02064         static CProtocolData clData;
02065         if(m_bInitFlag == false)
02066         {
02067                 warning("device not initialized");
02068                 m_iErrorState = ERRID_DEV_NOTINITIALIZED;
02069                 return m_iErrorState;
02070         }
02071 
02072         rafData.resize(6);
02073         clWrite.m_uiMessageId = MSGID_DLR_DATA_GET;
02074         clWrite.m_aucMessageData[0] = CMDID_DLR_DATA_GET;
02075         clWrite.m_aucMessageData[1] = PARID_DLR_DATA_GET;
02076         clWrite.m_ucMessageLength = 2;
02077 
02078         m_iErrorState = writeDevice(clWrite);
02079         if(m_iErrorState != 0)
02080         {
02081                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
02082                 LeaveCriticalSection(&m_csDevice);
02083                 return m_iErrorState;
02084         }
02085         do
02086         {       
02087                 m_iErrorState = readDevice(clRead);
02088                 if(m_iErrorState != 0)
02089                 {
02090                         //warning("wrong 1. readDevice ErrorCode %i", m_iErrorState);
02091                         clearReadQueue();
02092                         LeaveCriticalSection(&m_csDevice);
02093                         return m_iErrorState;
02094                 }
02095                 bRecieved = true;
02096                 if(clRead.m_uiMessageId != MSGID_DLR_DATA_ACK)
02097                 {
02098                         debug(1,"getDataDLR_FTS: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_DLR_DATA_ACK );
02099                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
02100                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
02101                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
02102                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
02103                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
02104                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
02105                         bRecieved = false;
02106                 }
02107                 if(clRead.m_aucMessageData[0] != CMDID_DLR_DATA_ACK0)
02108                 {
02109                         debug(1,"getDataDLR_FTS: wrong command ID %x, expected %x", clRead.m_aucMessageData[0], CMDID_DLR_DATA_ACK0);
02110                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
02111                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
02112                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
02113                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
02114                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
02115                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
02116                         bRecieved = false;
02117                 }
02118         }
02119         while(!bRecieved);
02120         clData.aucData[0] = clRead.m_aucMessageData[1];
02121         clData.aucData[1] = clRead.m_aucMessageData[2];
02122         clData.aucData[2] = clRead.m_aucMessageData[3];
02123         clData.aucData[3] = clRead.m_aucMessageData[4];
02124         rafData[0] = clData.fData;
02125         clData.aucData[0] = clRead.m_aucMessageData[5];
02126         clData.aucData[1] = clRead.m_aucMessageData[6];
02127         clData.aucData[2] = clRead.m_aucMessageData[7];
02128 
02129         bRecieved = false;
02130         do
02131         {       
02132                 m_iErrorState = readDevice(clRead);
02133                 if(m_iErrorState != 0)
02134                 {
02135                         //warning("wrong 2. readDevice ErrorCode %i", m_iErrorState);
02136                         clearReadQueue();
02137                         LeaveCriticalSection(&m_csDevice);
02138                         return m_iErrorState;
02139                 }
02140                 bRecieved = true;
02141                 if(clRead.m_uiMessageId != MSGID_DLR_DATA_ACK)
02142                 {
02143                         debug(1,"getDataDLR_FTS: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_DLR_DATA_ACK );
02144                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
02145                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
02146                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
02147                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
02148                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
02149                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
02150                         bRecieved = false;
02151                 }
02152                 if(clRead.m_aucMessageData[0] != CMDID_DLR_DATA_ACK1)
02153                 {
02154                         debug(1,"getDataDLR_FTS: wrong command ID %x, expected %x", clRead.m_aucMessageData[0], CMDID_DLR_DATA_ACK1);
02155                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
02156                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
02157                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
02158                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
02159                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
02160                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
02161                         bRecieved = false;
02162                 }
02163         }
02164         while(!bRecieved);
02165         clData.aucData[3] = clRead.m_aucMessageData[1];
02166         rafData[1] = clData.fData;
02167         clData.aucData[0] = clRead.m_aucMessageData[2];
02168         clData.aucData[1] = clRead.m_aucMessageData[3];
02169         clData.aucData[2] = clRead.m_aucMessageData[4];
02170         clData.aucData[3] = clRead.m_aucMessageData[5];
02171         rafData[2] = clData.fData;
02172         clData.aucData[0] = clRead.m_aucMessageData[6];
02173         clData.aucData[1] = clRead.m_aucMessageData[7];
02174 
02175         bRecieved = false;
02176         do
02177         {       
02178                 m_iErrorState = readDevice(clRead);
02179                 if(m_iErrorState != 0)
02180                 {
02181                         //warning("wrong 3. readDevice ErrorCode %i", m_iErrorState);
02182                         clearReadQueue();
02183                         LeaveCriticalSection(&m_csDevice);
02184                         return m_iErrorState;
02185                 }
02186                 bRecieved = true;
02187                 if(clRead.m_uiMessageId != MSGID_DLR_DATA_ACK)
02188                 {
02189                         debug(1,"getDataDLR_FTS: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_DLR_DATA_ACK );
02190                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
02191                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
02192                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
02193                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
02194                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
02195                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
02196                         bRecieved = false;
02197                 }
02198                 if(clRead.m_aucMessageData[0] != CMDID_DLR_DATA_ACK2)
02199                 {
02200                         debug(1,"getDataDLR_FTS: wrong command ID %x, expected %x", clRead.m_aucMessageData[0], CMDID_DLR_DATA_ACK2);
02201                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
02202                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
02203                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
02204                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
02205                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
02206                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
02207                         bRecieved = false;
02208                 }
02209         }
02210         while(!bRecieved);
02211         clData.aucData[2] = clRead.m_aucMessageData[1];
02212         clData.aucData[3] = clRead.m_aucMessageData[2];
02213         rafData[3] = clData.fData;
02214         clData.aucData[0] = clRead.m_aucMessageData[3];
02215         clData.aucData[1] = clRead.m_aucMessageData[4];
02216         clData.aucData[2] = clRead.m_aucMessageData[5];
02217         clData.aucData[3] = clRead.m_aucMessageData[6];
02218         rafData[4] = clData.fData;
02219         clData.aucData[0] = clRead.m_aucMessageData[7];
02220 
02221         bRecieved = false;
02222         do
02223         {       
02224                 m_iErrorState = readDevice(clRead);
02225                 if(m_iErrorState != 0)
02226                 {
02227                         //warning("wrong 4. readDevice ErrorCode %i", m_iErrorState);
02228                         clearReadQueue();
02229                         LeaveCriticalSection(&m_csDevice);
02230                         return m_iErrorState;
02231                 }
02232                 bRecieved = true;
02233                 if(clRead.m_uiMessageId != MSGID_DLR_DATA_ACK)
02234                 {
02235                         debug(1,"getDataDLR_FTS: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_DLR_DATA_ACK );
02236                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
02237                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
02238                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
02239                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
02240                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
02241                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
02242                         bRecieved = false;
02243                 }
02244                 if(clRead.m_aucMessageData[0] != CMDID_DLR_DATA_ACK3)
02245                 {
02246                         debug(1,"getDataDLR_FTS: wrong command ID %x, expected %x", clRead.m_aucMessageData[0], CMDID_DLR_DATA_ACK3);
02247                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
02248                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
02249                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
02250                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
02251                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
02252                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
02253                         bRecieved = false;
02254                 }
02255         }
02256         while(!bRecieved);
02257         clData.aucData[1] = clRead.m_aucMessageData[1];
02258         clData.aucData[2] = clRead.m_aucMessageData[2];
02259         clData.aucData[3] = clRead.m_aucMessageData[3];
02260         rafData[5] = clData.fData;
02261         clData.aucData[0] = clRead.m_aucMessageData[4];
02262         clData.aucData[1] = clRead.m_aucMessageData[5];
02263         clData.aucData[2] = clRead.m_aucMessageData[6];
02264         clData.aucData[3] = clRead.m_aucMessageData[7];
02265         *piState = clData.iData;
02266 
02267         LeaveCriticalSection(&m_csDevice);
02268         return m_iErrorState;
02269 }
02270 
02271 int CProtocolDevice::getDataSCHUNK_FTC(int iModuleId, int iChannelTypeId, std::vector<float>& rafData, short* piState)
02272 {
02273         EnterCriticalSection(&m_csDevice);
02274         m_iErrorState = 0;
02275         bool bRecieved = false;
02276         static CProtocolMessage clWrite, clRead;
02277         static CProtocolData clData;
02278         if(m_bInitFlag == false)
02279         {
02280                 warning("device not initialized");
02281                 m_iErrorState = ERRID_DEV_NOTINITIALIZED;
02282                 return m_iErrorState;
02283         }
02284         if(iModuleId < 1 || iModuleId > MAX_SCHUNK )
02285         {
02286                 warning("bad SCHUNK module ID %i", iModuleId);
02287                 m_iErrorState = ERRID_DEV_WRONGSCHUNKMODULEID;
02288                 LeaveCriticalSection(&m_csDevice);
02289                 return m_iErrorState;
02290         }
02291 
02292         rafData.resize(6);
02293         clWrite.m_uiMessageId = MSGID_SCHUNK_SEND + iModuleId;
02294         if(iChannelTypeId == 0)
02295                 clWrite.m_aucMessageData[0] = CMDID_SCHUNK_GET_FTI;
02296         else if(iChannelTypeId == 1)
02297                 clWrite.m_aucMessageData[0] = CMDID_SCHUNK_GET_TRI;
02298         else
02299                 clWrite.m_aucMessageData[0] = CMDID_SCHUNK_GET_TEI;
02300         clWrite.m_ucMessageLength = 1;
02301 
02302         m_iErrorState = writeDevice(clWrite);
02303         if(m_iErrorState != 0)
02304         {
02305                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
02306                 LeaveCriticalSection(&m_csDevice);
02307                 return m_iErrorState;
02308         }
02309         do
02310         {       
02311                 m_iErrorState = readDevice(clRead);
02312                 if(m_iErrorState != 0)
02313                 {
02314                         warning("wrong 1. readDevice ErrorCode %i", m_iErrorState);
02315                         LeaveCriticalSection(&m_csDevice);
02316                         return m_iErrorState;
02317                 }
02318                 bRecieved = true;
02319                 if(clRead.m_uiMessageId != MSGID_SCHUNK_RECV + iModuleId)
02320                 {
02321                         debug(1,"getDataSCHUNK_FTS: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_SCHUNK_RECV + iModuleId );
02322                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
02323                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
02324                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
02325                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
02326                         bRecieved = false;
02327                 }
02328                 if(iChannelTypeId == 0)
02329                 {
02330                         if(clRead.m_aucMessageData[0] != CMDID_SCHUNK_GET_FTI + 1)
02331                         {
02332                                 debug(1,"getDataSCHUNK_FTS: wrong command ID %x, expected %x", clRead.m_aucMessageData[0], CMDID_SCHUNK_GET_FTI + 1);
02333                                 debug(2,"write MessageId %x", clWrite.m_uiMessageId);
02334                                 debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
02335                                 debug(2,"read MessageId %x", clRead.m_uiMessageId);
02336                                 debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
02337                                 bRecieved = false;
02338                         }
02339                 }
02340                 else if(iChannelTypeId == 1)
02341                 {
02342                         if(clRead.m_aucMessageData[0] != CMDID_SCHUNK_GET_TRI + 1)
02343                         {
02344                                 debug(1,"getDataSCHUNK_FTS: wrong command ID %x, expected %x", clRead.m_aucMessageData[0], CMDID_SCHUNK_GET_TRI + 1);
02345                                 debug(2,"write MessageId %x", clWrite.m_uiMessageId);
02346                                 debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
02347                                 debug(2,"read MessageId %x", clRead.m_uiMessageId);
02348                                 debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
02349                                 bRecieved = false;
02350                         }
02351                 }
02352                 else
02353                 {
02354                         if(clRead.m_aucMessageData[0] != CMDID_SCHUNK_GET_TEI + 1)
02355                         {
02356                                 debug(1,"getDataSCHUNK_FTS: wrong command ID %x, expected %x", clRead.m_aucMessageData[0], CMDID_SCHUNK_GET_TEI + 1);
02357                                 debug(2,"write MessageId %x", clWrite.m_uiMessageId);
02358                                 debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
02359                                 debug(2,"read MessageId %x", clRead.m_uiMessageId);
02360                                 debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
02361                                 bRecieved = false;
02362                         }
02363                 }
02364         }
02365         while(!bRecieved);
02366         clData.aucData[0] = clRead.m_aucMessageData[1];
02367         clData.aucData[1] = clRead.m_aucMessageData[2];
02368         clData.aucData[2] = clRead.m_aucMessageData[3];
02369         clData.aucData[3] = clRead.m_aucMessageData[4];
02370         if(iChannelTypeId == 0)
02371         {
02372                 rafData[0] = clData.aiData[0] / 32.;
02373                 rafData[1] = clData.aiData[1] / 32.;
02374         }
02375         else if(iChannelTypeId == 1)
02376         {
02377                 rafData[0] = clData.aiData[0] / 4096000.;
02378                 rafData[1] = clData.aiData[1] / 4096000.;
02379         }
02380         else
02381         {
02382                 rafData[0] = clData.aiData[0];
02383                 rafData[1] = clData.aiData[1];
02384         }
02385         clData.aucData[0] = clRead.m_aucMessageData[5];
02386         clData.aucData[1] = clRead.m_aucMessageData[6];
02387         clData.aucData[2] = clRead.m_aucMessageData[7];
02388         if(iChannelTypeId == 0)
02389         {
02390                 rafData[2] = clData.aiData[0] / 32.;
02391         }
02392         else if(iChannelTypeId == 1)
02393         {
02394                 rafData[2] = clData.aiData[0] / 4096000.;
02395         }
02396         else
02397         {
02398                 rafData[2] = clData.aiData[0];
02399         }
02400 
02401         bRecieved = false;
02402         do
02403         {       
02404                 m_iErrorState = readDevice(clRead);
02405                 if(m_iErrorState != 0)
02406                 {
02407                         warning("wrong 2. readDevice ErrorCode %i", m_iErrorState);
02408                         LeaveCriticalSection(&m_csDevice);
02409                         return m_iErrorState;
02410                 }
02411                 bRecieved = true;
02412                 if(clRead.m_uiMessageId != MSGID_SCHUNK_RECV + iModuleId)
02413                 {
02414                         debug(1,"getDataSCHUNK_FTS: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_SCHUNK_RECV + iModuleId );
02415                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
02416                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
02417                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
02418                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
02419                         bRecieved = false;
02420                 }
02421                 if(clRead.m_aucMessageData[0] != 0x01)
02422                 {
02423                         debug(1,"getDataSCHUNK_FTS: wrong command ID %x, expected %x", clRead.m_aucMessageData[0], 0x01);
02424                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
02425                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
02426                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
02427                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
02428                         bRecieved = false;
02429                 }
02430         }
02431         while(!bRecieved);
02432         clData.aucData[3] = clRead.m_aucMessageData[1];
02433         if(iChannelTypeId == 0)
02434         {
02435                 rafData[3] = clData.aiData[1] / 1024.;
02436         }
02437         else if(iChannelTypeId == 1)
02438         {
02439                 rafData[3] = clData.aiData[1] / 65536.;
02440         }
02441         else
02442         {
02443                 rafData[3] = clData.aiData[1];
02444         }
02445         clData.aucData[0] = clRead.m_aucMessageData[2];
02446         clData.aucData[1] = clRead.m_aucMessageData[3];
02447         clData.aucData[2] = clRead.m_aucMessageData[4];
02448         clData.aucData[3] = clRead.m_aucMessageData[5];
02449         if(iChannelTypeId == 0)
02450         {
02451                 rafData[4] = clData.aiData[0] / 1024.;
02452                 rafData[5] = clData.aiData[1] / 1024.;
02453         }
02454         else if(iChannelTypeId == 1)
02455         {
02456                 rafData[4] = clData.aiData[0] / 65536.;
02457                 rafData[5] = clData.aiData[1] / 65536.;
02458         }
02459         else
02460         {
02461                 rafData[4] = clData.aiData[0];
02462                 rafData[5] = clData.aiData[1];
02463         }
02464         clData.aucData[0] = clRead.m_aucMessageData[6];
02465         clData.aucData[1] = clRead.m_aucMessageData[7];
02466 
02467         *piState = clData.aiData[0];
02468 
02469         LeaveCriticalSection(&m_csDevice);
02470         return m_iErrorState;
02471 }
02472 
02473 int CProtocolDevice::getDataMP55_IO(int iModuleId, float* pfData)
02474 {
02475         EnterCriticalSection(&m_csDevice);
02476         m_iErrorState = 0;
02477 
02478         bool bRecieved = false;
02479         static CProtocolMessage clWrite, clRead;
02480         static CProtocolData clData;
02481         if(m_bInitFlag == false)
02482         {
02483                 warning("device not initialized");
02484                 m_iErrorState = ERRID_DEV_NOTINITIALIZED;
02485                 return m_iErrorState;
02486         }
02487         if(iModuleId < 0 || iModuleId > MAX_MP55 )
02488         {
02489                 warning("bad MP55 module ID %i", iModuleId);
02490                 m_iErrorState = ERRID_DEV_WRONGMP55MODULEID;
02491                 LeaveCriticalSection(&m_csDevice);
02492                 return m_iErrorState;
02493         }
02494         *pfData = 0;
02495 
02496         clWrite.m_uiMessageId = MSGID_MP55_SEND + iModuleId;
02497         clWrite.m_bRTRFlag = false;
02498         clWrite.m_aucMessageData[0] = 0x40;
02499         clWrite.m_aucMessageData[1] = 0x01;
02500         clWrite.m_aucMessageData[2] = 0x30;
02501         clWrite.m_aucMessageData[3] = 0x01;
02502         clWrite.m_aucMessageData[4] = 0x00;
02503         clWrite.m_aucMessageData[5] = 0x00;
02504         clWrite.m_aucMessageData[6] = 0x00;
02505         clWrite.m_aucMessageData[7] = 0x00;
02506         clWrite.m_ucMessageLength = 8;
02507 
02508         m_iErrorState = writeDevice(clWrite);
02509         if(m_iErrorState != 0)
02510         {
02511                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
02512                 LeaveCriticalSection(&m_csDevice);
02513                 return m_iErrorState;
02514         }
02515         
02516         do
02517         {       
02518                 m_iErrorState = readDevice(clRead);
02519                 if(m_iErrorState != 0)
02520                 {
02521                         LeaveCriticalSection(&m_csDevice);
02522                         return m_iErrorState;
02523                 }
02524                 bRecieved = true;
02525                 if(clRead.m_uiMessageId != MSGID_MP55_RECV + iModuleId)
02526                 {
02527                         debug(1,"getDataMP55_IO: received CAN-ID %x, expected %x", clRead.m_uiMessageId, 0x580 + iModuleId);
02528                         bRecieved = false;
02529                 }
02530         }
02531         while(!bRecieved);
02532 
02533         clData.aucData[0] = clRead.m_aucMessageData[4];
02534         clData.aucData[1] = clRead.m_aucMessageData[5];
02535         clData.aucData[2] = clRead.m_aucMessageData[6];
02536         clData.aucData[3] = clRead.m_aucMessageData[7];
02537 
02538         *pfData = clData.fData;
02539 
02540         LeaveCriticalSection(&m_csDevice);
02541         return m_iErrorState;
02542 }
02543 
02544 int CProtocolDevice::getDataMP55_IO_fast(int iModuleId, float* pfData)
02545 {
02546         EnterCriticalSection(&m_csDevice);
02547         m_iErrorState = 0;
02548 
02549         bool bRecieved = false;
02550         static CProtocolMessage clWrite, clRead;
02551         static CProtocolData clData;
02552         if(m_bInitFlag == false)
02553         {
02554                 warning("device not initialized");
02555                 m_iErrorState = ERRID_DEV_NOTINITIALIZED;
02556                 return m_iErrorState;
02557         }
02558         if(iModuleId < 0 || iModuleId > MAX_MP55 )
02559         {
02560                 warning("bad MP55 module ID %i", iModuleId);
02561                 m_iErrorState = ERRID_DEV_WRONGMP55MODULEID;
02562                 LeaveCriticalSection(&m_csDevice);
02563                 return m_iErrorState;
02564         }
02565         *pfData = 0;
02566 
02567         // Send SYNC
02568         clWrite.m_uiMessageId = 0x80;
02569         clWrite.m_bRTRFlag = false;
02570         clWrite.m_aucMessageData[0] = iModuleId;
02571         clWrite.m_ucMessageLength = 1;
02572 
02573         m_iErrorState = writeDevice(clWrite);
02574 
02575         // Read PDO
02576         do
02577         {       
02578                 m_iErrorState = readDevice(clRead);
02579                 if(m_iErrorState != 0)
02580                 {
02581                         LeaveCriticalSection(&m_csDevice);
02582 //                      printf( "read PDO: %d\n", m_iErrorState );
02583                         return m_iErrorState;
02584                 }
02585                 bRecieved = true;
02586                 if(clRead.m_uiMessageId != 0x180 + iModuleId)
02587                 {
02588 //                      printf("getDataMP55_IO: received CAN-ID %x, expected %x, len= %d\n", clRead.m_uiMessageId, 0x180 + iModuleId, clRead.m_ucMessageLength);
02589 //                      for( int i = 0; i < clRead.m_ucMessageLength; i++ )
02590 //                              printf( "data[%d]=%x\n", i, clRead.m_aucMessageData[i] );
02591                         bRecieved = false;
02592                 }
02593                 Sleep( 1 );
02594         }
02595         while(!bRecieved);
02596 //      printf( "getData PDO: error state %x\n", m_iErrorState );
02597 //      printf( "getData PDO: received CAN-ID %x, expected %x\n", clRead.m_uiMessageId, 0x180 + iModuleId);
02598 
02599         clData.aucData[0] = clRead.m_aucMessageData[0];
02600         clData.aucData[1] = clRead.m_aucMessageData[1];
02601         clData.aucData[2] = clRead.m_aucMessageData[2];
02602         clData.aucData[3] = clRead.m_aucMessageData[3];
02603 
02604         //*pfData = ((float)clData.iData)/100000.0;
02605         *pfData = clData.fData;
02606 
02607         LeaveCriticalSection(&m_csDevice);
02608         return m_iErrorState;
02609 }
02610 
02611 int CProtocolDevice::getCanOpenRawAbsEnc(int iModuleId, short* piValue)
02612 {
02613         EnterCriticalSection(&m_csDevice);
02614         m_iErrorState = 0;
02615 
02616         bool bRecieved = false;
02617         static CProtocolMessage clWrite, clRead;
02618         static CProtocolData clData;
02619         if(m_bInitFlag == false)
02620         {
02621                 warning("device not initialized");
02622                 m_iErrorState = ERRID_DEV_NOTINITIALIZED;
02623                 return m_iErrorState;
02624         }
02625         if(iModuleId < 0 || iModuleId > MAX_MP55 )
02626         {
02627                 warning("bad CanOpen module ID %i", iModuleId);
02628                 m_iErrorState = ERRID_DEV_WRONGMP55MODULEID;
02629                 LeaveCriticalSection(&m_csDevice);
02630                 return m_iErrorState;
02631         }
02632         *piValue = 0;
02633 
02634         clWrite.m_uiMessageId = MSGID_MP55_SEND + iModuleId;
02635         clWrite.m_bRTRFlag = false;
02636         clWrite.m_aucMessageData[0] = 0x40;
02637         clWrite.m_aucMessageData[1] = 0x04;
02638         clWrite.m_aucMessageData[2] = 0x60;
02639         clWrite.m_aucMessageData[3] = 0x00;
02640         clWrite.m_aucMessageData[4] = 0x00;
02641         clWrite.m_aucMessageData[5] = 0x00;
02642         clWrite.m_aucMessageData[6] = 0x00;
02643         clWrite.m_aucMessageData[7] = 0x00;
02644         clWrite.m_ucMessageLength = 4;
02645 
02646         m_iErrorState = writeDevice(clWrite);
02647         if(m_iErrorState != 0)
02648         {
02649                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
02650                 LeaveCriticalSection(&m_csDevice);
02651                 return m_iErrorState;
02652         }
02653         
02654         do
02655         {       
02656                 m_iErrorState = readDevice(clRead);
02657                 if(m_iErrorState != 0)
02658                 {
02659                         printf( "wrong readDevice %d\n", m_iErrorState );
02660                         LeaveCriticalSection(&m_csDevice);
02661                         return m_iErrorState;
02662                 }
02663                 bRecieved = true;
02664                 if(clRead.m_uiMessageId != MSGID_MP55_RECV + iModuleId)
02665                 {
02666                         printf("getCanOpenRawAbsEnc: received CAN-ID %lu, expected %x \n", clRead.m_uiMessageId, 0x580 + iModuleId);
02667                         bRecieved = false;
02668                 }
02669         }
02670         while(!bRecieved);
02671 
02672         clData.aucData[0] = clRead.m_aucMessageData[4];
02673         clData.aucData[1] = clRead.m_aucMessageData[5];
02674         clData.aucData[2] = clRead.m_aucMessageData[6];
02675         clData.aucData[3] = clRead.m_aucMessageData[7];
02676 
02677         *piValue = clData.aiData[0];
02678 
02679         LeaveCriticalSection(&m_csDevice);
02680         return m_iErrorState;
02681 }
02682 
02683 
02684 // ========================================================================== ;
02685 //                                                                            ;
02686 // ---- modify functions ---------------------------------------------------- ;
02687 //                                                                            ;
02688 // ========================================================================== ;
02689 
02690 int CProtocolDevice::setNullSCHUNK_FTC(int iModuleId, short* piState)
02691 {
02692         EnterCriticalSection(&m_csDevice);
02693         m_iErrorState = 0;
02694         bool bRecieved = false;
02695         static CProtocolMessage clWrite, clRead;
02696         static CProtocolData clData;
02697         if(m_bInitFlag == false)
02698         {
02699                 warning("device not initialized");
02700                 m_iErrorState = ERRID_DEV_NOTINITIALIZED;
02701                 return m_iErrorState;
02702         }
02703         if(iModuleId < 1 || iModuleId > MAX_SCHUNK )
02704         {
02705                 warning("bad SCHUNK module ID %i", iModuleId);
02706                 m_iErrorState = ERRID_DEV_WRONGSCHUNKMODULEID;
02707                 LeaveCriticalSection(&m_csDevice);
02708                 return m_iErrorState;
02709         }
02710 
02711         clWrite.m_uiMessageId = MSGID_SCHUNK_SEND + iModuleId;
02712         clWrite.m_aucMessageData[0] = CMDID_SCHUNK_SET_NULL;
02713         clWrite.m_ucMessageLength = 1;
02714 
02715         m_iErrorState = writeDevice(clWrite);
02716         if(m_iErrorState != 0)
02717         {
02718                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
02719                 LeaveCriticalSection(&m_csDevice);
02720                 return m_iErrorState;
02721         }
02722         do
02723         {       
02724                 m_iErrorState = readDevice(clRead);
02725                 if(m_iErrorState != 0)
02726                 {
02727                         warning("wrong 1. readDevice ErrorCode %i", m_iErrorState);
02728                         LeaveCriticalSection(&m_csDevice);
02729                         return m_iErrorState;
02730                 }
02731                 bRecieved = true;
02732                 if(clRead.m_uiMessageId != MSGID_SCHUNK_RECV + iModuleId)
02733                 {
02734                         debug(1,"setNullSCHUNK_FTS: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_SCHUNK_RECV + iModuleId );
02735                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
02736                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
02737                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
02738                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
02739                         bRecieved = false;
02740                 }
02741                 if(clRead.m_aucMessageData[0] != CMDID_SCHUNK_SET_NULL + 1)
02742                 {
02743                         debug(1,"setNullSCHUNK_FTS: wrong command ID %x, expected %x", clRead.m_aucMessageData[0], CMDID_SCHUNK_SET_NULL + 1);
02744                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
02745                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
02746                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
02747                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
02748                         bRecieved = false;
02749                 }
02750         }
02751         while(!bRecieved);
02752         if(clRead.m_aucMessageData[1] != 'O' || clRead.m_aucMessageData[2] != 'K')
02753         {
02754                 debug(1,"setNullSCHUNK_FTS: wrong answer '%c%c', expected 'OK'", clRead.m_aucMessageData[1], clRead.m_aucMessageData[2]);
02755         }
02756         clData.aucData[0] = clRead.m_aucMessageData[3];
02757         clData.aucData[1] = clRead.m_aucMessageData[4];
02758 
02759         *piState = clData.aiData[0];
02760         LeaveCriticalSection(&m_csDevice);
02761         return m_iErrorState;
02762 }
02763 
02764 int CProtocolDevice::setTaraMP55_IO(int iModuleId, float fTara)
02765 {
02766         EnterCriticalSection(&m_csDevice);
02767         m_iErrorState = 0;
02768 
02769         bool bRecieved = false;
02770         static CProtocolMessage clWrite, clRead;
02771         static CProtocolData clData;
02772 
02773         if(m_bInitFlag == false)
02774         {
02775                 warning("device not initialized");
02776                 m_iErrorState = ERRID_DEV_NOTINITIALIZED;
02777                 return m_iErrorState;
02778         }
02779         if(iModuleId < 0 || iModuleId > MAX_MP55 )
02780         {
02781                 warning("bad MP55 module ID %i", iModuleId);
02782                 m_iErrorState = ERRID_DEV_WRONGMP55MODULEID;
02783                 LeaveCriticalSection(&m_csDevice);
02784                 return m_iErrorState;
02785         }
02786 
02787         clData.iData = fTara * 1000.0;
02788 
02789         clWrite.m_uiMessageId = 0x600 + iModuleId;
02790         clWrite.m_bRTRFlag = false;
02791         clWrite.m_aucMessageData[0] = 0x23;
02792         clWrite.m_aucMessageData[1] = 0x80;
02793         clWrite.m_aucMessageData[2] = 0x21;
02794         clWrite.m_aucMessageData[3] = 0x01;
02795         clWrite.m_aucMessageData[4] = clData.aucData[0];
02796         clWrite.m_aucMessageData[5] = clData.aucData[1];
02797         clWrite.m_aucMessageData[6] = clData.aucData[2];
02798         clWrite.m_aucMessageData[7] = clData.aucData[3];
02799         clWrite.m_ucMessageLength = 8;
02800 
02801         m_iErrorState = writeDevice(clWrite);
02802         if(m_iErrorState != 0)
02803         {
02804                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
02805                 LeaveCriticalSection(&m_csDevice);
02806                 return m_iErrorState;
02807         }
02808         
02809         do
02810         {       
02811                 m_iErrorState = readDevice(clRead);
02812                 if(m_iErrorState != 0)
02813                 {
02814                         warning("wrong readDevice ErrorCode %i", m_iErrorState);
02815                         LeaveCriticalSection(&m_csDevice);
02816                         return m_iErrorState;
02817                 }
02818                 bRecieved = true;
02819                 if(clRead.m_uiMessageId != 0x580 + iModuleId)
02820                 {
02821                         debug(1,"setTaraMP55_FS: received CAN-ID %x, expected %x", clRead.m_uiMessageId, 0x580 + iModuleId);
02822                         bRecieved = false;
02823                 }
02824         }
02825         while(!bRecieved);
02826 
02827         LeaveCriticalSection(&m_csDevice);
02828         return m_iErrorState;
02829 }
02830 
02831 int CProtocolDevice::setInitMP55_IO_fast( int iModuleId )
02832 {
02833         EnterCriticalSection(&m_csDevice);
02834         m_iErrorState = 0;
02835 
02836         bool bRecieved = false;
02837         static CProtocolMessage clWrite, clRead;
02838         static CProtocolData clData;
02839 
02840         if(m_bInitFlag == false)
02841         {
02842                 warning("device not initialized");
02843                 m_iErrorState = ERRID_DEV_NOTINITIALIZED;
02844                 return m_iErrorState;
02845         }
02846         if(iModuleId < 0 || iModuleId > MAX_MP55 )
02847         {
02848                 warning("bad MP55 module ID %i", iModuleId);
02849                 m_iErrorState = ERRID_DEV_WRONGMP55MODULEID;
02850                 LeaveCriticalSection(&m_csDevice);
02851                 return m_iErrorState;
02852         }
02853 
02854         // Init MP55: Stop Operational Mode
02855         clWrite.m_uiMessageId = 0x000;
02856         clWrite.m_bRTRFlag = false;
02857         clWrite.m_aucMessageData[0] = 0x80;
02858         clWrite.m_aucMessageData[1] = 0x00;
02859         clWrite.m_ucMessageLength = 2;
02860 
02861         m_iErrorState = writeDevice(clWrite);
02862 
02863 //      printf( "written Start Operational\n" );
02864         Sleep( 20 );
02865 
02866         // Init MP55: Set TPDO on SYNC (object 1800)
02867         clWrite.m_uiMessageId = 0x600 + iModuleId;
02868         clWrite.m_bRTRFlag = false;
02869         clWrite.m_aucMessageData[0] = 0x2F;
02870         clWrite.m_aucMessageData[1] = 0x00;
02871         clWrite.m_aucMessageData[2] = 0x18;
02872         clWrite.m_aucMessageData[3] = 0x02;
02873         clWrite.m_aucMessageData[4] = 0x01;
02874         clWrite.m_aucMessageData[5] = 0x00;
02875         clWrite.m_aucMessageData[6] = 0x00;
02876         clWrite.m_aucMessageData[7] = 0x00;
02877         clWrite.m_ucMessageLength = 8;
02878 
02879         m_iErrorState = writeDevice(clWrite);
02880 
02881         int end = 0;
02882         do
02883         {       
02884                 m_iErrorState = readDevice(clRead);
02885                 bRecieved = true;
02886                 if(clRead.m_uiMessageId != 0x580 + iModuleId)
02887                 {
02888 //                      printf( "setTaraMP55_FS: received CAN-ID %x, expected %x\n", clRead.m_uiMessageId, 0x580 + iModuleId);
02889                         bRecieved = false;
02890                 }
02891                 else
02892                 {       if( clRead.m_aucMessageData[0] != 0x60 )
02893                                 bRecieved = false;
02894                         else 
02895                         {       if( clRead.m_aucMessageData[1] != 0x00 )
02896                                         bRecieved = false;
02897                                 else
02898                                 { if( clRead.m_aucMessageData[1] != 0x18 )
02899                                                 bRecieved = false;
02900                                 }
02901                         }       
02902                 }
02903                 end++;
02904         }
02905         while(!bRecieved && end < 10);
02906 
02907 //      printf( "setTara obj 1800: received CAN-ID %x, expected %x\n", clRead.m_uiMessageId, 0x580 + iModuleId);
02908 //      for( int i = 0; i < clRead.m_ucMessageLength; i++ )
02909 //              printf( "data[%d]=%x\n", i, clRead.m_aucMessageData[i] );
02910 
02911 
02912         // Init MP55: Set TPDO to one message (object 1801)
02913         clWrite.m_uiMessageId = 0x600 + iModuleId;
02914         clWrite.m_bRTRFlag = false;
02915         clWrite.m_aucMessageData[0] = 0x2F;
02916         clWrite.m_aucMessageData[1] = 0x01;
02917         clWrite.m_aucMessageData[2] = 0x18;
02918         clWrite.m_aucMessageData[3] = 0x02;
02919         clWrite.m_aucMessageData[4] = 0xFF;
02920         clWrite.m_aucMessageData[5] = 0x00;
02921         clWrite.m_aucMessageData[6] = 0x00;
02922         clWrite.m_aucMessageData[7] = 0x00;
02923         clWrite.m_ucMessageLength = 8;
02924 
02925         m_iErrorState = writeDevice(clWrite);
02926 
02927 //      printf( "written obj 1801\n" );
02928         Sleep( 10 );
02929         end = 0;
02930         do
02931         {       
02932                 m_iErrorState = readDevice(clRead);
02933                 bRecieved = true;
02934                 if(clRead.m_uiMessageId != 0x580 + iModuleId)
02935                 {
02936 //                      printf( "setTaraMP55_FS: received CAN-ID %x, expected %x\n", clRead.m_uiMessageId, 0x580 + iModuleId);
02937                         bRecieved = false;
02938                 }
02939                 else
02940                 {       if( clRead.m_aucMessageData[0] != 0x60 )
02941                                 bRecieved = false;
02942                         else 
02943                         {       if( clRead.m_aucMessageData[1] != 0x01 )
02944                                         bRecieved = false;
02945                                 else
02946                                 { if( clRead.m_aucMessageData[1] != 0x18 )
02947                                                 bRecieved = false;
02948                                 }
02949                         }       
02950                 }
02951                 end++;
02952         }
02953         while(!bRecieved && end < 10);
02954 
02955 //      printf( "setTara obj 1801: received CAN-ID %x, expected %x\n", clRead.m_uiMessageId, 0x580 + iModuleId);
02956 //      for( i = 0; i < clRead.m_ucMessageLength; i++ )
02957 //              printf( "data[%d]=%x\n", i, clRead.m_aucMessageData[i] );
02958 
02959         Sleep( 10 );
02960 
02961         // Init MP55: Set to Operational Mode
02962         clWrite.m_uiMessageId = 0x000;
02963         clWrite.m_bRTRFlag = false;
02964         clWrite.m_aucMessageData[0] = 0x01;
02965         clWrite.m_aucMessageData[1] = 0x00;
02966         clWrite.m_ucMessageLength = 2;
02967 
02968         m_iErrorState = writeDevice(clWrite);
02969 
02970 //      printf( "written Start Operational\n" );
02971         Sleep( 10 );
02972 
02973         LeaveCriticalSection(&m_csDevice);
02974         return m_iErrorState;
02975 }
02976 
02977 // ========================================================================== ;
02978 //                                                                            ;
02979 // ---- I/O functions ------------------------------------------------------- ;
02980 //                                                                            ;
02981 // ========================================================================== ;
02982 
02983 // ========================================================================== ;
02984 //                                                                            ;
02985 // ---- exec functions ------------------------------------------------------ ;
02986 //                                                                            ;
02987 // ========================================================================== ;
02988 
02989 int CProtocolDevice::initDLR_FTS()
02990 {
02991         EnterCriticalSection(&m_csDevice);
02992         m_iErrorState = 0;
02993         bool bRecieved = false;
02994         static CProtocolMessage clWrite, clRead;
02995 
02996         if(m_bInitFlag == false)
02997         {
02998                 warning("device not initialized");
02999                 m_iErrorState = ERRID_DEV_NOTINITIALIZED;
03000                 return m_iErrorState;
03001         }
03002         m_iErrorState = setMessageId(MSGID_DLR_INIT_ACK);
03003         if(m_iErrorState != 0)
03004         {
03005                 warning("wrong setMessageId");
03006                 LeaveCriticalSection(&m_csDevice);
03007                 return m_iErrorState;
03008         }               
03009         m_iErrorState = setMessageId(MSGID_DLR_DATA_ACK);
03010         if(m_iErrorState != 0)
03011         {
03012                 warning("wrong setMessageId");
03013                 LeaveCriticalSection(&m_csDevice);
03014                 return m_iErrorState;
03015         }               
03016         clWrite.m_uiMessageId = MSGID_DLR_INIT_0;
03017         clWrite.m_aucMessageData[0] = 0;
03018         clWrite.m_aucMessageData[1] = 0x4b;
03019         clWrite.m_aucMessageData[2] = 0x3;
03020         clWrite.m_aucMessageData[3] = 0x1;
03021         clWrite.m_aucMessageData[4] = 0x3;
03022         clWrite.m_aucMessageData[5] = 0;
03023         clWrite.m_ucMessageLength = 6;
03024 
03025         m_iErrorState = writeDevice(clWrite);
03026         if(m_iErrorState != 0)
03027         {
03028                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
03029                 LeaveCriticalSection(&m_csDevice);
03030                 return m_iErrorState;
03031         }
03032 
03033         do
03034         {       
03035                 m_iErrorState = readDevice(clRead);
03036                 if(m_iErrorState != 0)
03037                 {
03038                         warning("wrong MSGID_DLR_INIT_0 readDevice ErrorCode %i", m_iErrorState);
03039                         LeaveCriticalSection(&m_csDevice);
03040                         return m_iErrorState;
03041                 }
03042                 bRecieved = true;
03043                 if(clRead.m_uiMessageId != MSGID_DLR_INIT_ACK)
03044                 {
03045                         debug(1,"initDLR_FTS: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_DLR_INIT_ACK );
03046                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
03047                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
03048                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
03049                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
03050                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
03051                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
03052                         bRecieved = false;
03053                 }
03054         }
03055         while(!bRecieved);
03056 
03057         clWrite.m_uiMessageId = MSGID_DLR_INIT_1;
03058         clWrite.m_aucMessageData[0] = 0x40;
03059         clWrite.m_aucMessageData[1] = 0x10;
03060         clWrite.m_aucMessageData[2] = 0x5;
03061         clWrite.m_aucMessageData[3] = 0x1;
03062         clWrite.m_aucMessageData[4] = 0x9;
03063         clWrite.m_aucMessageData[5] = 0;
03064         clWrite.m_aucMessageData[6] = 0;
03065         clWrite.m_ucMessageLength = 7;
03066 
03067         m_iErrorState = writeDevice(clWrite);
03068         if(m_iErrorState != 0)
03069         {
03070                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
03071                 LeaveCriticalSection(&m_csDevice);
03072                 return m_iErrorState;
03073         }
03074 
03075         bRecieved = false;
03076         do
03077         {       
03078                 m_iErrorState = readDevice(clRead);
03079                 if(m_iErrorState != 0)
03080                 {
03081                         warning("wrong MSGID_DLR_INIT_1 readDevice ErrorCode %i", m_iErrorState);
03082                         LeaveCriticalSection(&m_csDevice);
03083                         return m_iErrorState;
03084                 }
03085                 bRecieved = true;
03086                 if(clRead.m_uiMessageId != MSGID_DLR_INIT_ACK)
03087                 {
03088                         debug(1,"initDLR_FTS: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_DLR_INIT_ACK );
03089                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
03090                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
03091                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
03092                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
03093                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
03094                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
03095                         bRecieved = false;
03096                 }
03097         }
03098         while(!bRecieved);
03099 
03100         clWrite.m_uiMessageId = MSGID_DLR_INIT_2;
03101         clWrite.m_aucMessageData[0] = 0;
03102         clWrite.m_aucMessageData[1] = 0x10;
03103         clWrite.m_aucMessageData[2] = 0x5;
03104         clWrite.m_aucMessageData[3] = 0x2;
03105         clWrite.m_aucMessageData[4] = 0x9;
03106         clWrite.m_aucMessageData[5] = 0x10;
03107         clWrite.m_aucMessageData[6] = 0x27;
03108         clWrite.m_ucMessageLength = 7;
03109 
03110         m_iErrorState = writeDevice(clWrite);
03111         if(m_iErrorState != 0)
03112         {
03113                 warning("wrong writeDevice ErrorCode %i", m_iErrorState);
03114                 LeaveCriticalSection(&m_csDevice);
03115                 return m_iErrorState;
03116         }
03117 
03118         bRecieved = false;
03119         do
03120         {       
03121                 m_iErrorState = readDevice(clRead);
03122                 if(m_iErrorState != 0)
03123                 {
03124                         warning("wrong MSGID_DLR_INIT_2 readDevice ErrorCode %i", m_iErrorState);
03125                         LeaveCriticalSection(&m_csDevice);
03126                         return m_iErrorState;
03127                 }
03128                 bRecieved = true;
03129                 if(clRead.m_uiMessageId != MSGID_DLR_INIT_ACK)
03130                 {
03131                         debug(1,"initDLR_FTS: received CAN-ID %x, expected %x", clRead.m_uiMessageId, MSGID_DLR_INIT_ACK );
03132                         debug(2,"write MessageId %x", clWrite.m_uiMessageId);
03133                         debug(2,"write CommandId %x", clWrite.m_aucMessageData[0]);
03134                         debug(2,"write ParameterId %x", clWrite.m_aucMessageData[1]);
03135                         debug(2,"read MessageId %x", clRead.m_uiMessageId);
03136                         debug(2,"read CommandId %x", clRead.m_aucMessageData[0]);
03137                         debug(2,"read ParameterId %x", clRead.m_aucMessageData[1]);
03138                         bRecieved = false;
03139                 }
03140         }
03141         while(!bRecieved);
03142 
03143         LeaveCriticalSection(&m_csDevice);
03144         return m_iErrorState;
03145 }
03146 
03147 int CProtocolDevice::printMessage(CProtocolMessage & rclProtocolMessage, bool read)
03148 {
03149 
03150         char readWrite[10];
03151         if (read)
03152           sprintf(readWrite,"read");
03153         else
03154           sprintf(readWrite,"write");
03155 
03156         if (rclProtocolMessage.m_ucMessageLength == 8)
03157         {
03158                 debug(2,"%s CAN message Id 0x%02x, Command Id 0x%02x, ParameterId 0x%02x, Data: 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x",readWrite,
03159                                                                                 rclProtocolMessage.m_uiMessageId,
03160                                                                                 rclProtocolMessage.m_aucMessageData[0],
03161                                                                                 rclProtocolMessage.m_aucMessageData[1],
03162                                                                                 rclProtocolMessage.m_aucMessageData[2],
03163                                                                                 rclProtocolMessage.m_aucMessageData[3],
03164                                                                                 rclProtocolMessage.m_aucMessageData[4],
03165                                                                                 rclProtocolMessage.m_aucMessageData[5],
03166                                                                                 rclProtocolMessage.m_aucMessageData[6],
03167                                                                                 rclProtocolMessage.m_aucMessageData[7]); 
03168         }
03169         else if (rclProtocolMessage.m_ucMessageLength == 7)
03170         {
03171                 debug(2,"%s CAN message Id 0x%02x, Command Id 0x%02x, ParameterId 0x%02x, Data: 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x",readWrite,
03172                                                                                 rclProtocolMessage.m_uiMessageId,
03173                                                                                 rclProtocolMessage.m_aucMessageData[0],
03174                                                                                 rclProtocolMessage.m_aucMessageData[1],
03175                                                                                 rclProtocolMessage.m_aucMessageData[2],
03176                                                                                 rclProtocolMessage.m_aucMessageData[3],
03177                                                                                 rclProtocolMessage.m_aucMessageData[4],
03178                                                                                 rclProtocolMessage.m_aucMessageData[5],
03179                                                                                 rclProtocolMessage.m_aucMessageData[6]); 
03180         }
03181         else if (rclProtocolMessage.m_ucMessageLength == 6)
03182         {
03183                 debug(2,"%s CAN message Id 0x%02x, Command Id 0x%02x, ParameterId 0x%02x, Data: 0x%02x 0x%02x 0x%02x 0x%02x",readWrite,
03184                                                                                 rclProtocolMessage.m_uiMessageId,
03185                                                                                 rclProtocolMessage.m_aucMessageData[0],
03186                                                                                 rclProtocolMessage.m_aucMessageData[1],
03187                                                                                 rclProtocolMessage.m_aucMessageData[2],
03188                                                                                 rclProtocolMessage.m_aucMessageData[3],
03189                                                                                 rclProtocolMessage.m_aucMessageData[4],
03190                                                                                 rclProtocolMessage.m_aucMessageData[5]); 
03191         }
03192         else if (rclProtocolMessage.m_ucMessageLength == 5)
03193         {
03194                 debug(2,"%s CAN message Id 0x%02x, Command Id 0x%02x, ParameterId 0x%02x, Data: 0x%02x 0x%02x 0x%02x",readWrite,
03195                                                                                 rclProtocolMessage.m_uiMessageId,
03196                                                                                 rclProtocolMessage.m_aucMessageData[0],
03197                                                                                 rclProtocolMessage.m_aucMessageData[1],
03198                                                                                 rclProtocolMessage.m_aucMessageData[2],
03199                                                                                 rclProtocolMessage.m_aucMessageData[3],
03200                                                                                 rclProtocolMessage.m_aucMessageData[4]); 
03201         }
03202         else if (rclProtocolMessage.m_ucMessageLength == 4)
03203         {
03204                 debug(2,"%s CAN message Id 0x%02x, Command Id 0x%02x, ParameterId 0x%02x, Data: 0x%02x 0x%02x",readWrite,
03205                                                                                 rclProtocolMessage.m_uiMessageId,
03206                                                                                 rclProtocolMessage.m_aucMessageData[0],
03207                                                                                 rclProtocolMessage.m_aucMessageData[1],
03208                                                                                 rclProtocolMessage.m_aucMessageData[2],
03209                                                                                 rclProtocolMessage.m_aucMessageData[3]);
03210         }
03211         else if (rclProtocolMessage.m_ucMessageLength == 3)
03212         {
03213                 debug(2,"%s CAN message Id 0x%02x, Command Id 0x%02x, ParameterId 0x%02x, Data: 0x%02x",readWrite,
03214                                                                                 rclProtocolMessage.m_uiMessageId,
03215                                                                                 rclProtocolMessage.m_aucMessageData[0],
03216                                                                                 rclProtocolMessage.m_aucMessageData[1],
03217                                                                                 rclProtocolMessage.m_aucMessageData[2]);
03218         }
03219         else if (rclProtocolMessage.m_ucMessageLength == 2)
03220         {
03221                 debug(2,"%s CAN message Id 0x%02x, Command Id 0x%02x, ParameterId 0x%02x",readWrite,
03222                                                                                  rclProtocolMessage.m_uiMessageId,
03223                                                                                  rclProtocolMessage.m_aucMessageData[0],
03224                                                                                  rclProtocolMessage.m_aucMessageData[1]);
03225         }
03226 
03227         return 0;
03228 
03229 
03230 }

schunk_libm5api
Author(s): Florian Weisshardt
autogenerated on Sat Jun 8 2019 20:25:13