schunk_libm5api: schunk_libm5api: ProtocolDevice.cpp Source File

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