void MgsModbus::MbmProcess()
{
MbmFC = SetFC(int (MbmByteArray[7]));
#ifdef DEBUG
for (int i=0;i<MbmByteArray[5]+6;i++) {
if(MbmByteArray[i] < 16) {Serial.print("0");}
Serial.print(MbmByteArray[i],HEX);
if (i != MbmByteArray[5]+5) {Serial.print(".");
} else {Serial.println();}
}
#endif
//****************** Read Coils (1) & Read Input discretes (2) **********************
if(MbmFC == MB_FC_READ_COILS || MbmFC == MB_FC_READ_DISCRETE_INPUT) {
word Count = MbmByteArray[8] * 8;
if (MbmBitCount < Count) {
Count = MbmBitCount;
}
for (int i=0;i<Count;i++) {
if (i + MbmPos < MbDataLen * 16) {
SetBit(i + MbmPos,bitRead(MbmByteArray[(i/8)+9],i-((i/8)*8)));
}
}
}
//****************** Read Registers (3) & Read Input registers (4) ******************
if(MbmFC == MB_FC_READ_REGISTERS || MbmFC == MB_FC_READ_INPUT_REGISTER) {
word Pos = MbmPos;
for (int i=0;i<MbmByteArray[8];i=i+2) {
if (Pos < MbDataLen) {
MbData[Pos] = (MbmByteArray[i+9] * 0x100) + MbmByteArray[i+1+9];
Pos++;
}
}
}
//****************** Write Coil (5) **********************
if(MbmFC == MB_FC_WRITE_COIL){
}
//****************** Write Register (6) ******************
if(MbmFC == MB_FC_WRITE_REGISTER){
}
//****************** Write Multiple Coils (15) **********************
if(MbmFC == MB_FC_WRITE_MULTIPLE_COILS){
}
//****************** Write Multiple Registers (16) ******************
if(MbmFC == MB_FC_WRITE_MULTIPLE_REGISTERS){
}
}
//****************** Recieve data for ModBusSlave ****************
void MgsModbus::MbsRun()
{
//****************** Read from socket ****************
WiFiClient client = MbServer.available();
if(client.available())
{
delay(10);
int i = 0;
while(client.available())
{
MbsByteArray[i] = client.read();
i++;
}
MbsFC = SetFC(MbsByteArray[7]); //Byte 7 of request is FC
}
int Start, WordDataLength, ByteDataLength, CoilDataLength, MessageLength;
//****************** Read Coils (1 & 2) **********************
if(MbsFC == MB_FC_READ_COILS || MbsFC == MB_FC_READ_DISCRETE_INPUT) {
Start = word(MbsByteArray[8],MbsByteArray[9]);
CoilDataLength = word(MbsByteArray[10],MbsByteArray[11]);
ByteDataLength = CoilDataLength / 8;
if(ByteDataLength * 8 < CoilDataLength) ByteDataLength++;
CoilDataLength = ByteDataLength * 8;
MbsByteArray[5] = ByteDataLength + 3; //Number of bytes after this one.
MbsByteArray[8] = ByteDataLength; //Number of bytes after this one (or number of bytes of data).
for(int i = 0; i < ByteDataLength ; i++)
{
MbsByteArray[9 + i] = 0; // To get all remaining not written bits zero
for(int j = 0; j < 8; j++)
{
bitWrite(MbsByteArray[9 + i], j, GetBit(Start + i * 8 + j));
}
}
MessageLength = ByteDataLength + 9;
client.write((uint8_t*)MbsByteArray, MessageLength);
MbsFC = MB_FC_NONE;
}
//****************** Read Registers (3 & 4) ******************
if(MbsFC == MB_FC_READ_REGISTERS || MbsFC == MB_FC_READ_INPUT_REGISTER) {
Start = word(MbsByteArray[8],MbsByteArray[9]);
WordDataLength = word(MbsByteArray[10],MbsByteArray[11]);
ByteDataLength = WordDataLength * 2;
MbsByteArray[5] = ByteDataLength + 3; //Number of bytes after this one.
MbsByteArray[8] = ByteDataLength; //Number of bytes after this one (or number of bytes of data).
for(int i = 0; i < WordDataLength; i++)
{
MbsByteArray[ 9 + i * 2] = highByte(MbData[Start + i]);
MbsByteArray[10 + i * 2] = lowByte(MbData[Start + i]);
}
MessageLength = ByteDataLength + 9;
client.write((uint8_t*)MbsByteArray, MessageLength);
MbsFC = MB_FC_NONE;
}
//****************** Write Coil (5) **********************
if(MbsFC == MB_FC_WRITE_COIL) {
Start = word(MbsByteArray[8],MbsByteArray[9]);
if (word(MbsByteArray[10],MbsByteArray[11]) == 0xFF00){SetBit(Start,true);}
if (word(MbsByteArray[10],MbsByteArray[11]) == 0x0000){SetBit(Start,false);}
MbsByteArray[5] = 2; //Number of bytes after this one.
MessageLength = 8;
client.write((uint8_t*)MbsByteArray, MessageLength);
MbsFC = MB_FC_NONE;
}
//****************** Write Register (6) ******************
if(MbsFC == MB_FC_WRITE_REGISTER) {
Start = word(MbsByteArray[8],MbsByteArray[9]);
MbData[Start] = word(MbsByteArray[10],MbsByteArray[11]);
MbsByteArray[5] = 6; //Number of bytes after this one.
MessageLength = 12;
client.write((uint8_t*)MbsByteArray, MessageLength);
MbsFC = MB_FC_NONE;
}
//****************** Write Multiple Coils (15) **********************
if(MbsFC == MB_FC_WRITE_MULTIPLE_COILS) {
Start = word(MbsByteArray[8],MbsByteArray[9]);
CoilDataLength = word(MbsByteArray[10],MbsByteArray[11]);
MbsByteArray[5] = 6;
for(int i = 0; i < CoilDataLength; i++)
{
SetBit(Start + i,bitRead(MbsByteArray[13 + (i/8)],i-((i/8)*8)));
}
MessageLength = 12;
client.write((uint8_t*)MbsByteArray, MessageLength);
MbsFC = MB_FC_NONE;
}
//****************** Write Multiple Registers (16) ******************
if(MbsFC == MB_FC_WRITE_MULTIPLE_REGISTERS) {
Start = word(MbsByteArray[8],MbsByteArray[9]);
WordDataLength = word(MbsByteArray[10],MbsByteArray[11]);
ByteDataLength = WordDataLength * 2;
MbsByteArray[5] = 6;
for(int i = 0; i < WordDataLength; i++)
{
MbData[Start + i] = word(MbsByteArray[ 13 + i * 2],MbsByteArray[14 + i * 2]);
}
MessageLength = 12;
client.write((uint8_t*)MbsByteArray, MessageLength);
MbsFC = MB_FC_NONE;
}
}
void MudbusYun::Run(){
Runs = 1 + Runs * (Runs < 999);
//****************** Read from socket ****************
MbServer.noListenOnLocalhost();
if (first == LOW){
MbServer.begin();
Serial.println("MbServer Begin");
first = HIGH;
}
if(millis() > (PreviousActivityTime + 60000)){
if(Active){
Active = false;
#ifdef MbDebug
Serial.println("Mb not active");
#endif
}
}
if(client.connected()){
Reads = 1 + Reads * (Reads < 999);
int i = 0;
while(client.available()){
ByteArray[i] = client.read();
i++;
}
SetFC(ByteArray[7]); //Byte 7 of request is FC
if(!Active){
Active = true;
PreviousActivityTime = millis();
#ifdef MbDebug
Serial.println("Mb active");
#endif
}
}
else {client = MbServer.accept();}
int Start, WordDataLength, ByteDataLength, CoilDataLength, MessageLength;
//****************** Read Coils **********************
if(FC == MB_FC_READ_COILS){
Start = word(ByteArray[8],ByteArray[9]);
CoilDataLength = word(ByteArray[10],ByteArray[11]);
ByteDataLength = CoilDataLength / 8;
if(ByteDataLength * 8 < CoilDataLength) ByteDataLength++;
CoilDataLength = ByteDataLength * 8;
#ifdef MbDebug
Serial.print(" MB_FC_READ_COILS S=");
Serial.print(Start);
Serial.print(" L=");
Serial.println(CoilDataLength);
#endif
//Number of bytes after this one.
ByteArray[5] = ByteDataLength + 3;
//Number of bytes after this one (or number of bytes of data).
ByteArray[8] = ByteDataLength;
for(int i = 0; i < ByteDataLength ; i++){
for(int j = 0; j < 8; j++){
bitWrite(ByteArray[9 + i], j, C[Start + i * 8 + j]);
}
}
MessageLength = ByteDataLength + 9;
client.write(ByteArray, MessageLength);
Writes = 1 + Writes * (Writes < 999);
FC = MB_FC_NONE;
}
//****************** Read Registers ******************
if(FC == MB_FC_READ_REGISTERS){
Start = word(ByteArray[8],ByteArray[9]);
WordDataLength = word(ByteArray[10],ByteArray[11]);
ByteDataLength = WordDataLength * 2;
#ifdef MbDebug
Serial.print(" MB_FC_READ_REGISTERS S=");
Serial.print(Start);
Serial.print(" L=");
Serial.println(WordDataLength);
#endif
//Number of bytes after this one.
ByteArray[5] = ByteDataLength + 3;
//Number of bytes after this one (or number of bytes of data).
ByteArray[8] = ByteDataLength;
for(int i = 0; i < WordDataLength; i++){
ByteArray[ 9 + i * 2] = highByte(R[Start + i]);
ByteArray[10 + i * 2] = lowByte(R[Start + i]);
}
MessageLength = ByteDataLength + 9;
client.write(ByteArray, MessageLength);
Writes = 1 + Writes * (Writes < 999);
FC = MB_FC_NONE;
}
//****************** Write Coil **********************
if(FC == MB_FC_WRITE_COIL){
Start = word(ByteArray[8],ByteArray[9]);
C[Start] = word(ByteArray[10],ByteArray[11]) > 0;
#ifdef MbDebug
Serial.print(" MB_FC_WRITE_COIL C");
Serial.print(Start);
Serial.print("=");
Serial.println(C[Start]);
#endif
ByteArray[5] = 2; //Number of bytes after this one.
MessageLength = 8;
client.write(ByteArray, MessageLength);
Writes = 1 + Writes * (Writes < 999);
FC = MB_FC_NONE;
}
//****************** Write Register ******************
if(FC == MB_FC_WRITE_REGISTER){
Start = word(ByteArray[8],ByteArray[9]);
R[Start] = word(ByteArray[10],ByteArray[11]);
#ifdef MbDebug
Serial.print(" MB_FC_WRITE_REGISTER R");
Serial.print(Start);
Serial.print("=");
Serial.println(R[Start]);
#endif
ByteArray[5] = 6; //Number of bytes after this one.
MessageLength = 12;
client.write(ByteArray, MessageLength);
Writes = 1 + Writes * (Writes < 999);
FC = MB_FC_NONE;
}
//****************** Write Multiple Coils **********************
//Function codes 15 & 16 by Martin Pettersson http://siamect.com
if(FC == MB_FC_WRITE_MULTIPLE_COILS){
Start = word(ByteArray[8],ByteArray[9]);
CoilDataLength = word(ByteArray[10],ByteArray[11]);
ByteDataLength = CoilDataLength / 8;
if(ByteDataLength * 8 < CoilDataLength) ByteDataLength++;
CoilDataLength = ByteDataLength * 8;
#ifdef MbDebug
Serial.print(" MB_FC_WRITE_MULTIPLE_COILS S=");
Serial.print(Start);
Serial.print(" L=");
Serial.println(CoilDataLength);
#endif
//Number of bytes after this one.
ByteArray[5] = ByteDataLength + 5;
for(int i = 0; i < ByteDataLength ; i++){
for(int j = 0; j < 8; j++){
C[Start + i * 8 + j] = bitRead( ByteArray[13 + i], j);
}
}
MessageLength = 12;
client.write(ByteArray, MessageLength);
Writes = 1 + Writes * (Writes < 999);
FC = MB_FC_NONE;
}
//****************** Write Multiple Registers ******************
//Function codes 15 & 16 by Martin Pettersson http://siamect.com
if(FC == MB_FC_WRITE_MULTIPLE_REGISTERS){
Start = word(ByteArray[8],ByteArray[9]);
WordDataLength = word(ByteArray[10],ByteArray[11]);
ByteDataLength = WordDataLength * 2;
#ifdef MbDebug
Serial.print(" MB_FC_READ_REGISTERS S=");
Serial.print(Start);
Serial.print(" L=");
Serial.println(WordDataLength);
#endif
//Number of bytes after this one.
ByteArray[5] = ByteDataLength + 3;
for(int i = 0; i < WordDataLength; i++){
R[Start + i] = word(ByteArray[ 13 + i * 2],ByteArray[14 + i * 2]);
}
MessageLength = 12;
client.write(ByteArray, MessageLength);
Writes = 1 + Writes * (Writes < 999);
FC = MB_FC_NONE;
}
#ifdef MbDebug
Serial.print("Mb runs: ");
Serial.print(Runs);
Serial.print(" reads: ");
Serial.print(Reads);
Serial.print(" writes: ");
Serial.print(Writes);
Serial.println();
#endif
}
