Modified version of ModbusTCP
Dependencies: EthernetNetIf mbed
main.cpp@3:d7d7c67f21fa, 2012-03-15 (annotated)
- Committer:
- paleskyjp
- Date:
- Thu Mar 15 15:28:14 2012 +0000
- Revision:
- 3:d7d7c67f21fa
- Parent:
- 0:62be54b8975d
eMBRegCoilsCB was corrected.
(Implementation of writing single coil was wrong.)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
paleskyjp | 0:62be54b8975d | 1 | /* |
paleskyjp | 0:62be54b8975d | 2 | * FreeModbus Libary: BARE Demo Application |
paleskyjp | 0:62be54b8975d | 3 | * Copyright (C) 2006 Christian Walter <wolti@sil.at> |
paleskyjp | 0:62be54b8975d | 4 | * |
paleskyjp | 0:62be54b8975d | 5 | * This program is free software; you can redistribute it and/or modify |
paleskyjp | 0:62be54b8975d | 6 | * it under the terms of the GNU General Public License as published by |
paleskyjp | 0:62be54b8975d | 7 | * the Free Software Foundation; either version 2 of the License, or |
paleskyjp | 0:62be54b8975d | 8 | * (at your option) any later version. |
paleskyjp | 0:62be54b8975d | 9 | * |
paleskyjp | 0:62be54b8975d | 10 | * This program is distributed in the hope that it will be useful, |
paleskyjp | 0:62be54b8975d | 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
paleskyjp | 0:62be54b8975d | 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
paleskyjp | 0:62be54b8975d | 13 | * GNU General Public License for more details. |
paleskyjp | 0:62be54b8975d | 14 | * |
paleskyjp | 0:62be54b8975d | 15 | * You should have received a copy of the GNU General Public License |
paleskyjp | 0:62be54b8975d | 16 | * along with this program; if not, write to the Free Software |
paleskyjp | 0:62be54b8975d | 17 | * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA |
paleskyjp | 0:62be54b8975d | 18 | * |
paleskyjp | 0:62be54b8975d | 19 | * File: $Id: demo.c,v 1.1 2006/08/22 21:35:13 wolti Exp $ |
paleskyjp | 0:62be54b8975d | 20 | |
paleskyjp | 0:62be54b8975d | 21 | * modified from: Thanassis Mavrogeorgiadis 5-12-2011 |
paleskyjp | 0:62be54b8975d | 22 | */ |
paleskyjp | 0:62be54b8975d | 23 | |
paleskyjp | 0:62be54b8975d | 24 | #include "mbed.h" |
paleskyjp | 0:62be54b8975d | 25 | |
paleskyjp | 0:62be54b8975d | 26 | /* ----------------------- Modbus includes ----------------------------------*/ |
paleskyjp | 0:62be54b8975d | 27 | #include "mb.h" |
paleskyjp | 0:62be54b8975d | 28 | #include "mbport.h" |
paleskyjp | 0:62be54b8975d | 29 | #include "mbconfig.h" |
paleskyjp | 0:62be54b8975d | 30 | |
paleskyjp | 0:62be54b8975d | 31 | |
paleskyjp | 0:62be54b8975d | 32 | #if MB_TCP_ENABLED == 1 |
paleskyjp | 0:62be54b8975d | 33 | #include "EthernetNetIf.h" |
paleskyjp | 0:62be54b8975d | 34 | EthernetNetIf eth; |
paleskyjp | 0:62be54b8975d | 35 | #endif |
paleskyjp | 0:62be54b8975d | 36 | |
paleskyjp | 0:62be54b8975d | 37 | extern Serial pc; |
paleskyjp | 0:62be54b8975d | 38 | |
paleskyjp | 0:62be54b8975d | 39 | DigitalOut led1(LED1); |
paleskyjp | 0:62be54b8975d | 40 | |
paleskyjp | 0:62be54b8975d | 41 | DigitalIn ENET_LINK(P1_25); |
paleskyjp | 0:62be54b8975d | 42 | DigitalOut LedLINK(LED4); |
paleskyjp | 0:62be54b8975d | 43 | Ticker EtherPinMonitor; |
paleskyjp | 0:62be54b8975d | 44 | |
paleskyjp | 0:62be54b8975d | 45 | #if MB_TCP_ENABLED == 1 |
paleskyjp | 0:62be54b8975d | 46 | void EtherPinMonitorFunc(void) |
paleskyjp | 0:62be54b8975d | 47 | { |
paleskyjp | 0:62be54b8975d | 48 | LedLINK = !ENET_LINK; |
paleskyjp | 0:62be54b8975d | 49 | } |
paleskyjp | 0:62be54b8975d | 50 | #endif |
paleskyjp | 0:62be54b8975d | 51 | |
paleskyjp | 0:62be54b8975d | 52 | /* ----------------------- Defines ------------------------------------------*/ |
paleskyjp | 0:62be54b8975d | 53 | #define REG_INPUT_START 1001 |
paleskyjp | 0:62be54b8975d | 54 | #define REG_INPUT_NREGS 4 |
paleskyjp | 0:62be54b8975d | 55 | |
paleskyjp | 0:62be54b8975d | 56 | #define REG_HOLDING_START 2001 |
paleskyjp | 0:62be54b8975d | 57 | #define REG_HOLDING_NREGS 130 |
paleskyjp | 0:62be54b8975d | 58 | |
paleskyjp | 0:62be54b8975d | 59 | #define REG_COIL_START (3000+1) |
paleskyjp | 0:62be54b8975d | 60 | #define REG_COIL_NREGS 8 |
paleskyjp | 0:62be54b8975d | 61 | #define REG_COIL_BYTES REG_COIL_NREGS/8 |
paleskyjp | 0:62be54b8975d | 62 | |
paleskyjp | 0:62be54b8975d | 63 | #define REG_DISC_START (4000+1) |
paleskyjp | 0:62be54b8975d | 64 | #define REG_DISC_NREGS 8 |
paleskyjp | 0:62be54b8975d | 65 | #define REG_DISC_BYTES REG_DISC_NREGS/8 |
paleskyjp | 0:62be54b8975d | 66 | |
paleskyjp | 0:62be54b8975d | 67 | #define SLAVE_ID 0x0A |
paleskyjp | 0:62be54b8975d | 68 | |
paleskyjp | 0:62be54b8975d | 69 | /* ----------------------- Static variables ---------------------------------*/ |
paleskyjp | 0:62be54b8975d | 70 | static USHORT usRegInputStart = REG_INPUT_START; |
paleskyjp | 0:62be54b8975d | 71 | static USHORT usRegInputBuf[REG_INPUT_NREGS]; |
paleskyjp | 0:62be54b8975d | 72 | static USHORT usRegHoldingStart = REG_HOLDING_START; |
paleskyjp | 0:62be54b8975d | 73 | static USHORT usRegHoldingBuf[REG_HOLDING_NREGS]={0x0123,0x4567,0x89AB,0xCDEF,0xDEAD,0xBEEF,0xDEAD,0xBEEF,0xDEAD,0xBEEF}; |
paleskyjp | 0:62be54b8975d | 74 | static USHORT usRegCoilStart = REG_COIL_START; |
paleskyjp | 0:62be54b8975d | 75 | static UCHAR usRegCoilBuf[REG_COIL_BYTES]={0xA5}; |
paleskyjp | 0:62be54b8975d | 76 | static USHORT usRegDiscStart = REG_DISC_START; |
paleskyjp | 0:62be54b8975d | 77 | static UCHAR usRegDiscBuf[REG_DISC_BYTES]={0x5A}; |
paleskyjp | 0:62be54b8975d | 78 | |
paleskyjp | 0:62be54b8975d | 79 | /* ----------------------- Start implementation -----------------------------*/ |
paleskyjp | 0:62be54b8975d | 80 | |
paleskyjp | 0:62be54b8975d | 81 | |
paleskyjp | 0:62be54b8975d | 82 | int main() { |
paleskyjp | 0:62be54b8975d | 83 | eMBErrorCode eStatus; |
paleskyjp | 0:62be54b8975d | 84 | |
paleskyjp | 0:62be54b8975d | 85 | #if MB_TCP_ENABLED == 1 |
paleskyjp | 0:62be54b8975d | 86 | pc.baud(115200); |
paleskyjp | 0:62be54b8975d | 87 | EtherPinMonitor.attach(&EtherPinMonitorFunc, 0.01); |
paleskyjp | 0:62be54b8975d | 88 | printf("Setting up...\n"); |
paleskyjp | 0:62be54b8975d | 89 | EthernetErr ethErr = eth.setup(); |
paleskyjp | 0:62be54b8975d | 90 | if(ethErr) |
paleskyjp | 0:62be54b8975d | 91 | { |
paleskyjp | 0:62be54b8975d | 92 | printf("Error %d in setup.\n", ethErr); |
paleskyjp | 0:62be54b8975d | 93 | return -1; |
paleskyjp | 0:62be54b8975d | 94 | } |
paleskyjp | 0:62be54b8975d | 95 | printf("Setup OK\n"); |
paleskyjp | 0:62be54b8975d | 96 | IpAddr ip = eth.getIp(); |
paleskyjp | 0:62be54b8975d | 97 | printf("mbed IP Address is %d.%d.%d.%d\r\n", ip[0], ip[1], ip[2], ip[3]); |
paleskyjp | 0:62be54b8975d | 98 | #endif |
paleskyjp | 0:62be54b8975d | 99 | |
paleskyjp | 0:62be54b8975d | 100 | Timer tm; |
paleskyjp | 0:62be54b8975d | 101 | tm.start(); |
paleskyjp | 0:62be54b8975d | 102 | |
paleskyjp | 0:62be54b8975d | 103 | #if MB_RTU_ENABLED == 1 |
paleskyjp | 0:62be54b8975d | 104 | eStatus = eMBInit( MB_RTU, SLAVE_ID, 0, 9600, MB_PAR_NONE ); |
paleskyjp | 0:62be54b8975d | 105 | #endif |
paleskyjp | 0:62be54b8975d | 106 | #if MB_ASCII_ENABLED == 1 |
paleskyjp | 0:62be54b8975d | 107 | eStatus = eMBInit( MB_ASCII, SLAVE_ID, 0, 9600, MB_PAR_NONE ); |
paleskyjp | 0:62be54b8975d | 108 | #endif |
paleskyjp | 0:62be54b8975d | 109 | #if MB_TCP_ENABLED == 1 |
paleskyjp | 0:62be54b8975d | 110 | eStatus = eMBTCPInit( MB_TCP_PORT_USE_DEFAULT ); |
paleskyjp | 0:62be54b8975d | 111 | #endif |
paleskyjp | 0:62be54b8975d | 112 | if (eStatus != MB_ENOERR ) |
paleskyjp | 0:62be54b8975d | 113 | printf( "can't initialize modbus stack!\r\n" ); |
paleskyjp | 0:62be54b8975d | 114 | |
paleskyjp | 0:62be54b8975d | 115 | /* Enable the Modbus Protocol Stack. */ |
paleskyjp | 0:62be54b8975d | 116 | eStatus = eMBEnable( ); |
paleskyjp | 0:62be54b8975d | 117 | if (eStatus != MB_ENOERR ) |
paleskyjp | 0:62be54b8975d | 118 | fprintf( stderr, "can't enable modbus stack!\r\n" ); |
paleskyjp | 0:62be54b8975d | 119 | |
paleskyjp | 0:62be54b8975d | 120 | // Initialise some registers |
paleskyjp | 0:62be54b8975d | 121 | usRegInputBuf[1] = 0x1234; |
paleskyjp | 0:62be54b8975d | 122 | usRegInputBuf[2] = 0x5678; |
paleskyjp | 0:62be54b8975d | 123 | usRegInputBuf[3] = 0x9abc; |
paleskyjp | 0:62be54b8975d | 124 | |
paleskyjp | 0:62be54b8975d | 125 | while(true) |
paleskyjp | 0:62be54b8975d | 126 | { |
paleskyjp | 0:62be54b8975d | 127 | #if MB_TCP_ENABLED == 1 |
paleskyjp | 0:62be54b8975d | 128 | Net::poll(); |
paleskyjp | 0:62be54b8975d | 129 | #endif |
paleskyjp | 3:d7d7c67f21fa | 130 | |
paleskyjp | 0:62be54b8975d | 131 | if(tm.read()>.5) |
paleskyjp | 0:62be54b8975d | 132 | { |
paleskyjp | 0:62be54b8975d | 133 | led1=!led1; //Show that we are alive |
paleskyjp | 0:62be54b8975d | 134 | tm.start(); |
paleskyjp | 0:62be54b8975d | 135 | } |
paleskyjp | 3:d7d7c67f21fa | 136 | |
paleskyjp | 0:62be54b8975d | 137 | eStatus = eMBPoll( ); |
paleskyjp | 0:62be54b8975d | 138 | |
paleskyjp | 0:62be54b8975d | 139 | /* Here we simply count the number of poll cycles. */ |
paleskyjp | 0:62be54b8975d | 140 | usRegInputBuf[0]++; |
paleskyjp | 0:62be54b8975d | 141 | } |
paleskyjp | 0:62be54b8975d | 142 | //return 0; |
paleskyjp | 0:62be54b8975d | 143 | } |
paleskyjp | 0:62be54b8975d | 144 | |
paleskyjp | 0:62be54b8975d | 145 | eMBErrorCode |
paleskyjp | 0:62be54b8975d | 146 | eMBRegInputCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs ) |
paleskyjp | 0:62be54b8975d | 147 | { |
paleskyjp | 0:62be54b8975d | 148 | eMBErrorCode eStatus = MB_ENOERR; |
paleskyjp | 0:62be54b8975d | 149 | int iRegIndex; |
paleskyjp | 0:62be54b8975d | 150 | |
paleskyjp | 0:62be54b8975d | 151 | if( ( usAddress >= REG_INPUT_START ) |
paleskyjp | 0:62be54b8975d | 152 | && ( usAddress + usNRegs <= REG_INPUT_START + REG_INPUT_NREGS ) ) |
paleskyjp | 0:62be54b8975d | 153 | { |
paleskyjp | 0:62be54b8975d | 154 | iRegIndex = ( int )( usAddress - usRegInputStart ); |
paleskyjp | 0:62be54b8975d | 155 | while( usNRegs > 0 ) |
paleskyjp | 0:62be54b8975d | 156 | { |
paleskyjp | 0:62be54b8975d | 157 | *pucRegBuffer++ = ( unsigned char )( usRegInputBuf[iRegIndex] >> 8 ); |
paleskyjp | 0:62be54b8975d | 158 | *pucRegBuffer++ = ( unsigned char )( usRegInputBuf[iRegIndex] & 0xFF ); |
paleskyjp | 0:62be54b8975d | 159 | iRegIndex++; |
paleskyjp | 0:62be54b8975d | 160 | usNRegs--; |
paleskyjp | 0:62be54b8975d | 161 | } |
paleskyjp | 0:62be54b8975d | 162 | } |
paleskyjp | 0:62be54b8975d | 163 | else |
paleskyjp | 0:62be54b8975d | 164 | { |
paleskyjp | 0:62be54b8975d | 165 | eStatus = MB_ENOREG; |
paleskyjp | 0:62be54b8975d | 166 | } |
paleskyjp | 0:62be54b8975d | 167 | return eStatus; |
paleskyjp | 0:62be54b8975d | 168 | } |
paleskyjp | 0:62be54b8975d | 169 | |
paleskyjp | 0:62be54b8975d | 170 | eMBErrorCode |
paleskyjp | 0:62be54b8975d | 171 | eMBRegHoldingCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs, eMBRegisterMode eMode ) |
paleskyjp | 0:62be54b8975d | 172 | { |
paleskyjp | 0:62be54b8975d | 173 | eMBErrorCode eStatus = MB_ENOERR; |
paleskyjp | 0:62be54b8975d | 174 | int iRegIndex; |
paleskyjp | 0:62be54b8975d | 175 | |
paleskyjp | 0:62be54b8975d | 176 | if( ( usAddress >= REG_HOLDING_START ) && |
paleskyjp | 0:62be54b8975d | 177 | ( usAddress + usNRegs <= REG_HOLDING_START + REG_HOLDING_NREGS ) ) |
paleskyjp | 0:62be54b8975d | 178 | { |
paleskyjp | 0:62be54b8975d | 179 | iRegIndex = ( int )( usAddress - usRegHoldingStart ); |
paleskyjp | 0:62be54b8975d | 180 | switch ( eMode ) |
paleskyjp | 0:62be54b8975d | 181 | { |
paleskyjp | 0:62be54b8975d | 182 | /* Pass current register values to the protocol stack. */ |
paleskyjp | 0:62be54b8975d | 183 | case MB_REG_READ: |
paleskyjp | 0:62be54b8975d | 184 | while( usNRegs > 0 ) |
paleskyjp | 0:62be54b8975d | 185 | { |
paleskyjp | 0:62be54b8975d | 186 | *pucRegBuffer++ = ( UCHAR ) ( usRegHoldingBuf[iRegIndex] >> 8 ); |
paleskyjp | 0:62be54b8975d | 187 | *pucRegBuffer++ = ( UCHAR ) ( usRegHoldingBuf[iRegIndex] & 0xFF ); |
paleskyjp | 0:62be54b8975d | 188 | iRegIndex++; |
paleskyjp | 0:62be54b8975d | 189 | usNRegs--; |
paleskyjp | 0:62be54b8975d | 190 | } |
paleskyjp | 0:62be54b8975d | 191 | break; |
paleskyjp | 0:62be54b8975d | 192 | |
paleskyjp | 0:62be54b8975d | 193 | /* Update current register values with new values from the |
paleskyjp | 0:62be54b8975d | 194 | * protocol stack. */ |
paleskyjp | 0:62be54b8975d | 195 | case MB_REG_WRITE: |
paleskyjp | 0:62be54b8975d | 196 | while( usNRegs > 0 ) |
paleskyjp | 0:62be54b8975d | 197 | { |
paleskyjp | 0:62be54b8975d | 198 | usRegHoldingBuf[iRegIndex] = *pucRegBuffer++ << 8; |
paleskyjp | 0:62be54b8975d | 199 | usRegHoldingBuf[iRegIndex] |= *pucRegBuffer++; |
paleskyjp | 0:62be54b8975d | 200 | iRegIndex++; |
paleskyjp | 0:62be54b8975d | 201 | usNRegs--; |
paleskyjp | 0:62be54b8975d | 202 | } |
paleskyjp | 0:62be54b8975d | 203 | } |
paleskyjp | 0:62be54b8975d | 204 | } |
paleskyjp | 0:62be54b8975d | 205 | else |
paleskyjp | 0:62be54b8975d | 206 | { |
paleskyjp | 0:62be54b8975d | 207 | eStatus = MB_ENOREG; |
paleskyjp | 0:62be54b8975d | 208 | } |
paleskyjp | 0:62be54b8975d | 209 | return eStatus; |
paleskyjp | 0:62be54b8975d | 210 | } |
paleskyjp | 0:62be54b8975d | 211 | |
paleskyjp | 0:62be54b8975d | 212 | /* |
paleskyjp | 0:62be54b8975d | 213 | * Following implementation is not actually checked. |
paleskyjp | 0:62be54b8975d | 214 | */ |
paleskyjp | 0:62be54b8975d | 215 | |
paleskyjp | 0:62be54b8975d | 216 | eMBErrorCode |
paleskyjp | 0:62be54b8975d | 217 | eMBRegCoilsCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNCoils, eMBRegisterMode eMode ) |
paleskyjp | 0:62be54b8975d | 218 | { |
paleskyjp | 0:62be54b8975d | 219 | eMBErrorCode eStatus = MB_ENOERR; |
paleskyjp | 0:62be54b8975d | 220 | int iIntRegIndex; |
paleskyjp | 0:62be54b8975d | 221 | int iIntBufNum; |
paleskyjp | 0:62be54b8975d | 222 | int iIntBitNum; |
paleskyjp | 0:62be54b8975d | 223 | int iExtRegIndex=0; |
paleskyjp | 0:62be54b8975d | 224 | int iExtBufNum; |
paleskyjp | 0:62be54b8975d | 225 | int iExtBitNum; |
paleskyjp | 0:62be54b8975d | 226 | UCHAR ucTemp; |
paleskyjp | 0:62be54b8975d | 227 | if( ( usAddress >= REG_COIL_START ) |
paleskyjp | 0:62be54b8975d | 228 | && ( usAddress + usNCoils <= REG_COIL_START + REG_COIL_NREGS ) ) |
paleskyjp | 0:62be54b8975d | 229 | { |
paleskyjp | 0:62be54b8975d | 230 | iIntRegIndex = ( int )( usAddress - usRegCoilStart ); |
paleskyjp | 0:62be54b8975d | 231 | |
paleskyjp | 0:62be54b8975d | 232 | while( usNCoils > 0 ) |
paleskyjp | 0:62be54b8975d | 233 | { |
paleskyjp | 0:62be54b8975d | 234 | iIntBufNum=iIntRegIndex/8; |
paleskyjp | 0:62be54b8975d | 235 | iIntBitNum=iIntRegIndex%8; |
paleskyjp | 0:62be54b8975d | 236 | iExtBufNum=iExtRegIndex/8; |
paleskyjp | 0:62be54b8975d | 237 | iExtBitNum=iExtRegIndex%8; |
paleskyjp | 0:62be54b8975d | 238 | |
paleskyjp | 0:62be54b8975d | 239 | switch ( eMode ) |
paleskyjp | 0:62be54b8975d | 240 | { |
paleskyjp | 0:62be54b8975d | 241 | case MB_REG_READ: |
paleskyjp | 0:62be54b8975d | 242 | // Read coils |
paleskyjp | 0:62be54b8975d | 243 | if(iExtBitNum==0){ |
paleskyjp | 0:62be54b8975d | 244 | pucRegBuffer[iExtBufNum]=0; |
paleskyjp | 0:62be54b8975d | 245 | } |
paleskyjp | 0:62be54b8975d | 246 | ucTemp=(usRegCoilBuf[iIntBufNum]>>iIntBitNum) & 1; |
paleskyjp | 0:62be54b8975d | 247 | pucRegBuffer[iExtBufNum]|=ucTemp<<iExtBitNum; |
paleskyjp | 0:62be54b8975d | 248 | break; |
paleskyjp | 0:62be54b8975d | 249 | |
paleskyjp | 0:62be54b8975d | 250 | case MB_REG_WRITE: |
paleskyjp | 0:62be54b8975d | 251 | // Write coils |
paleskyjp | 3:d7d7c67f21fa | 252 | ucTemp=usRegCoilBuf[iIntBufNum]&(~(1<<iIntBitNum)); |
paleskyjp | 3:d7d7c67f21fa | 253 | ucTemp|=((pucRegBuffer[iExtBufNum]>>iExtBitNum) & 1)<<iIntBitNum; |
paleskyjp | 3:d7d7c67f21fa | 254 | usRegCoilBuf[iIntBufNum]=ucTemp; |
paleskyjp | 0:62be54b8975d | 255 | break; |
paleskyjp | 0:62be54b8975d | 256 | } |
paleskyjp | 0:62be54b8975d | 257 | iIntRegIndex++; |
paleskyjp | 0:62be54b8975d | 258 | iExtRegIndex++; |
paleskyjp | 0:62be54b8975d | 259 | usNCoils--; |
paleskyjp | 0:62be54b8975d | 260 | |
paleskyjp | 0:62be54b8975d | 261 | } |
paleskyjp | 0:62be54b8975d | 262 | } |
paleskyjp | 0:62be54b8975d | 263 | else |
paleskyjp | 0:62be54b8975d | 264 | { |
paleskyjp | 0:62be54b8975d | 265 | eStatus = MB_ENOREG; |
paleskyjp | 0:62be54b8975d | 266 | } |
paleskyjp | 0:62be54b8975d | 267 | |
paleskyjp | 0:62be54b8975d | 268 | return eStatus; |
paleskyjp | 0:62be54b8975d | 269 | } |
paleskyjp | 0:62be54b8975d | 270 | |
paleskyjp | 0:62be54b8975d | 271 | eMBErrorCode |
paleskyjp | 0:62be54b8975d | 272 | eMBRegDiscreteCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNDiscrete ) |
paleskyjp | 0:62be54b8975d | 273 | { |
paleskyjp | 0:62be54b8975d | 274 | eMBErrorCode eStatus = MB_ENOERR; |
paleskyjp | 0:62be54b8975d | 275 | int iIntRegIndex; |
paleskyjp | 0:62be54b8975d | 276 | int iIntBufNum; |
paleskyjp | 0:62be54b8975d | 277 | int iIntBitNum; |
paleskyjp | 0:62be54b8975d | 278 | int iExtRegIndex=0; |
paleskyjp | 0:62be54b8975d | 279 | int iExtBufNum; |
paleskyjp | 0:62be54b8975d | 280 | int iExtBitNum; |
paleskyjp | 0:62be54b8975d | 281 | UCHAR ucTemp; |
paleskyjp | 0:62be54b8975d | 282 | if( ( usAddress >= REG_DISC_START ) |
paleskyjp | 0:62be54b8975d | 283 | && ( usAddress + usNDiscrete <= REG_DISC_START + REG_DISC_NREGS ) ) |
paleskyjp | 0:62be54b8975d | 284 | { |
paleskyjp | 0:62be54b8975d | 285 | iIntRegIndex = ( int )( usAddress - usRegDiscStart ); |
paleskyjp | 0:62be54b8975d | 286 | |
paleskyjp | 0:62be54b8975d | 287 | while( usNDiscrete > 0 ) |
paleskyjp | 0:62be54b8975d | 288 | { |
paleskyjp | 0:62be54b8975d | 289 | iIntBufNum=iIntRegIndex/8; |
paleskyjp | 0:62be54b8975d | 290 | iIntBitNum=iIntRegIndex%8; |
paleskyjp | 0:62be54b8975d | 291 | iExtBufNum=iExtRegIndex/8; |
paleskyjp | 0:62be54b8975d | 292 | iExtBitNum=iExtRegIndex%8; |
paleskyjp | 0:62be54b8975d | 293 | |
paleskyjp | 0:62be54b8975d | 294 | // Read discrete inputs |
paleskyjp | 0:62be54b8975d | 295 | if(iExtBitNum==0){ |
paleskyjp | 0:62be54b8975d | 296 | pucRegBuffer[iExtBufNum]=0; |
paleskyjp | 0:62be54b8975d | 297 | } |
paleskyjp | 0:62be54b8975d | 298 | ucTemp=(usRegDiscBuf[iIntBufNum]>>iIntBitNum) & 1; |
paleskyjp | 0:62be54b8975d | 299 | pucRegBuffer[iExtBufNum]|=ucTemp<<iExtBitNum; |
paleskyjp | 0:62be54b8975d | 300 | |
paleskyjp | 0:62be54b8975d | 301 | iIntRegIndex++; |
paleskyjp | 0:62be54b8975d | 302 | iExtRegIndex++; |
paleskyjp | 0:62be54b8975d | 303 | usNDiscrete--; |
paleskyjp | 0:62be54b8975d | 304 | |
paleskyjp | 0:62be54b8975d | 305 | } |
paleskyjp | 0:62be54b8975d | 306 | } |
paleskyjp | 0:62be54b8975d | 307 | else |
paleskyjp | 0:62be54b8975d | 308 | { |
paleskyjp | 0:62be54b8975d | 309 | eStatus = MB_ENOREG; |
paleskyjp | 0:62be54b8975d | 310 | } |
paleskyjp | 0:62be54b8975d | 311 | |
paleskyjp | 0:62be54b8975d | 312 | return eStatus; |
paleskyjp | 0:62be54b8975d | 313 | } |