Khiem Tran / Mbed 2 deprecated STM32_Modbus

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Modbus.cpp@3:355a72e1d7d2, 2019-03-25 (annotated)

Committer:
KhiemTran
Date:
Mon Mar 25 14:42:49 2019 +0000
Revision:
3:355a72e1d7d2
Parent:
0:54bd111a5369 
Keyword C to C++

Who changed what in which revision?

UserRevisionLine numberNew contents of line
KhiemTran 0:54bd111a5369 1 /*
KhiemTran 0:54bd111a5369 2 Modbus.cpp - Source for Modbus Base Library
KhiemTran 3:355a72e1d7d2 3 Copyright (C) 2019 Khiem Tran
KhiemTran 0:54bd111a5369 4 */
KhiemTran 0:54bd111a5369 5 #include "Modbus.h"
KhiemTran 0:54bd111a5369 6
KhiemTran 0:54bd111a5369 7 Modbus::Modbus() {
KhiemTran 3:355a72e1d7d2 8 _regs_head = NULL;
KhiemTran 3:355a72e1d7d2 9 _regs_last = NULL;
KhiemTran 0:54bd111a5369 10 }
KhiemTran 0:54bd111a5369 11
KhiemTran 0:54bd111a5369 12 TRegister* Modbus::searchRegister(uint16_t address) {
KhiemTran 0:54bd111a5369 13 TRegister *reg = _regs_head;
KhiemTran 0:54bd111a5369 14 //if there is no register configured, bail
KhiemTran 0:54bd111a5369 15 if(reg == 0) return(0);
KhiemTran 0:54bd111a5369 16 //scan through the linked list until the end of the list or the register is found.
KhiemTran 0:54bd111a5369 17 //return the pointer.
KhiemTran 0:54bd111a5369 18 do {
KhiemTran 0:54bd111a5369 19 if (reg->address == address) return(reg);
KhiemTran 0:54bd111a5369 20 reg = reg->next;
KhiemTran 0:54bd111a5369 21 } while(reg);
KhiemTran 0:54bd111a5369 22 return(0);
KhiemTran 0:54bd111a5369 23 }
KhiemTran 0:54bd111a5369 24
KhiemTran 0:54bd111a5369 25 void Modbus::addReg(uint16_t address, uint16_t value) {
KhiemTran 0:54bd111a5369 26 TRegister *newreg;
KhiemTran 0:54bd111a5369 27
KhiemTran 3:355a72e1d7d2 28 newreg = new TRegister();
KhiemTran 3:355a72e1d7d2 29 //newreg = (TRegister *) malloc(sizeof(TRegister));
KhiemTran 0:54bd111a5369 30 newreg->address = address;
KhiemTran 0:54bd111a5369 31 newreg->value = value;
KhiemTran 0:54bd111a5369 32 newreg->next = 0;
KhiemTran 0:54bd111a5369 33
KhiemTran 0:54bd111a5369 34 if(_regs_head == 0) {
KhiemTran 0:54bd111a5369 35 _regs_head = newreg;
KhiemTran 0:54bd111a5369 36 _regs_last = _regs_head;
KhiemTran 0:54bd111a5369 37 } else {
KhiemTran 0:54bd111a5369 38 //Assign the last register's next pointer to newreg.
KhiemTran 0:54bd111a5369 39 _regs_last->next = newreg;
KhiemTran 0:54bd111a5369 40 //then make temp the last register in the list.
KhiemTran 0:54bd111a5369 41 _regs_last = newreg;
KhiemTran 0:54bd111a5369 42 }
KhiemTran 0:54bd111a5369 43 }
KhiemTran 0:54bd111a5369 44
KhiemTran 0:54bd111a5369 45 bool Modbus::Reg(uint16_t address, uint16_t value) {
KhiemTran 0:54bd111a5369 46 TRegister *reg;
KhiemTran 0:54bd111a5369 47 //search for the register address
KhiemTran 0:54bd111a5369 48 reg = this->searchRegister(address);
KhiemTran 0:54bd111a5369 49 //if found then assign the register value to the new value.
KhiemTran 0:54bd111a5369 50 if (reg) {
KhiemTran 0:54bd111a5369 51 reg->value = value;
KhiemTran 0:54bd111a5369 52 return true;
KhiemTran 0:54bd111a5369 53 } else
KhiemTran 0:54bd111a5369 54 return false;
KhiemTran 0:54bd111a5369 55 }
KhiemTran 0:54bd111a5369 56
KhiemTran 0:54bd111a5369 57 uint16_t Modbus::Reg(uint16_t address) {
KhiemTran 0:54bd111a5369 58 TRegister *reg;
KhiemTran 0:54bd111a5369 59 reg = this->searchRegister(address);
KhiemTran 0:54bd111a5369 60 if(reg)
KhiemTran 0:54bd111a5369 61 return(reg->value);
KhiemTran 0:54bd111a5369 62 else
KhiemTran 0:54bd111a5369 63 return(0);
KhiemTran 0:54bd111a5369 64 }
KhiemTran 0:54bd111a5369 65
KhiemTran 0:54bd111a5369 66 void Modbus::addHreg(uint16_t offset, uint16_t value) {
KhiemTran 0:54bd111a5369 67 this->addReg(offset + 40001, value);
KhiemTran 0:54bd111a5369 68 }
KhiemTran 0:54bd111a5369 69
KhiemTran 0:54bd111a5369 70 bool Modbus::Hreg(uint16_t offset, uint16_t value) {
KhiemTran 0:54bd111a5369 71 return Reg(offset + 40001, value);
KhiemTran 0:54bd111a5369 72 }
KhiemTran 0:54bd111a5369 73
KhiemTran 0:54bd111a5369 74 uint16_t Modbus::Hreg(uint16_t offset) {
KhiemTran 0:54bd111a5369 75 return Reg(offset + 40001);
KhiemTran 0:54bd111a5369 76 }
KhiemTran 0:54bd111a5369 77
KhiemTran 0:54bd111a5369 78 #ifndef USE_HOLDING_REGISTERS_ONLY
KhiemTran 0:54bd111a5369 79 void Modbus::addCoil(uint16_t offset, bool value) {
KhiemTran 0:54bd111a5369 80 this->addReg(offset + 1, value?0xFF00:0x0000);
KhiemTran 0:54bd111a5369 81 }
KhiemTran 0:54bd111a5369 82
KhiemTran 0:54bd111a5369 83 void Modbus::addIsts(uint16_t offset, bool value) {
KhiemTran 0:54bd111a5369 84 this->addReg(offset + 10001, value?0xFF00:0x0000);
KhiemTran 0:54bd111a5369 85 }
KhiemTran 0:54bd111a5369 86
KhiemTran 0:54bd111a5369 87 void Modbus::addIreg(uint16_t offset, uint16_t value) {
KhiemTran 0:54bd111a5369 88 this->addReg(offset + 30001, value);
KhiemTran 0:54bd111a5369 89 }
KhiemTran 0:54bd111a5369 90
KhiemTran 0:54bd111a5369 91 bool Modbus::Coil(uint16_t offset, bool value) {
KhiemTran 0:54bd111a5369 92 return Reg(offset + 1, value?0xFF00:0x0000);
KhiemTran 0:54bd111a5369 93 }
KhiemTran 0:54bd111a5369 94
KhiemTran 0:54bd111a5369 95 bool Modbus::Ists(uint16_t offset, bool value) {
KhiemTran 0:54bd111a5369 96 return Reg(offset + 10001, value?0xFF00:0x0000);
KhiemTran 0:54bd111a5369 97 }
KhiemTran 0:54bd111a5369 98
KhiemTran 0:54bd111a5369 99 bool Modbus::Ireg(uint16_t offset, uint16_t value) {
KhiemTran 0:54bd111a5369 100 return Reg(offset + 30001, value);
KhiemTran 0:54bd111a5369 101 }
KhiemTran 0:54bd111a5369 102
KhiemTran 0:54bd111a5369 103 bool Modbus::Coil(uint16_t offset) {
KhiemTran 0:54bd111a5369 104 if (Reg(offset + 1) == 0xFF00) {
KhiemTran 0:54bd111a5369 105 return true;
KhiemTran 0:54bd111a5369 106 } else return false;
KhiemTran 0:54bd111a5369 107 }
KhiemTran 0:54bd111a5369 108
KhiemTran 0:54bd111a5369 109 bool Modbus::Ists(uint16_t offset) {
KhiemTran 0:54bd111a5369 110 if (Reg(offset + 10001) == 0xFF00) {
KhiemTran 0:54bd111a5369 111 return true;
KhiemTran 0:54bd111a5369 112 } else return false;
KhiemTran 0:54bd111a5369 113 }
KhiemTran 0:54bd111a5369 114
KhiemTran 0:54bd111a5369 115 uint16_t Modbus::Ireg(uint16_t offset) {
KhiemTran 0:54bd111a5369 116 return Reg(offset + 30001);
KhiemTran 0:54bd111a5369 117 }
KhiemTran 0:54bd111a5369 118 #endif
KhiemTran 0:54bd111a5369 119
KhiemTran 0:54bd111a5369 120
KhiemTran 0:54bd111a5369 121 void Modbus::receivePDU(uint8_t* frame) {
KhiemTran 0:54bd111a5369 122 uint8_t fcode = frame[0];
KhiemTran 0:54bd111a5369 123 uint16_t field1 = (uint16_t)frame[1] << 8 | (uint16_t)frame[2];
KhiemTran 0:54bd111a5369 124 uint16_t field2 = (uint16_t)frame[3] << 8 | (uint16_t)frame[4];
KhiemTran 0:54bd111a5369 125
KhiemTran 0:54bd111a5369 126 switch (fcode) {
KhiemTran 0:54bd111a5369 127
KhiemTran 0:54bd111a5369 128 case MB_FC_WRITE_REG:
KhiemTran 0:54bd111a5369 129 //field1 = reg, field2 = value
KhiemTran 0:54bd111a5369 130 this->writeSingleRegister(field1, field2);
KhiemTran 0:54bd111a5369 131 break;
KhiemTran 0:54bd111a5369 132
KhiemTran 0:54bd111a5369 133 case MB_FC_READ_REGS:
KhiemTran 0:54bd111a5369 134 //field1 = startreg, field2 = numregs
KhiemTran 0:54bd111a5369 135 this->readRegisters(field1, field2);
KhiemTran 0:54bd111a5369 136 break;
KhiemTran 0:54bd111a5369 137
KhiemTran 0:54bd111a5369 138 case MB_FC_WRITE_REGS:
KhiemTran 0:54bd111a5369 139 //field1 = startreg, field2 = status
KhiemTran 0:54bd111a5369 140 this->writeMultipleRegisters(frame,field1, field2, frame[5]);
KhiemTran 0:54bd111a5369 141 break;
KhiemTran 0:54bd111a5369 142
KhiemTran 0:54bd111a5369 143 #ifndef USE_HOLDING_REGISTERS_ONLY
KhiemTran 0:54bd111a5369 144 case MB_FC_READ_COILS:
KhiemTran 0:54bd111a5369 145 //field1 = startreg, field2 = numregs
KhiemTran 0:54bd111a5369 146 this->readCoils(field1, field2);
KhiemTran 0:54bd111a5369 147 break;
KhiemTran 0:54bd111a5369 148
KhiemTran 0:54bd111a5369 149 case MB_FC_READ_INPUT_STAT:
KhiemTran 0:54bd111a5369 150 //field1 = startreg, field2 = numregs
KhiemTran 0:54bd111a5369 151 this->readInputStatus(field1, field2);
KhiemTran 0:54bd111a5369 152 break;
KhiemTran 0:54bd111a5369 153
KhiemTran 0:54bd111a5369 154 case MB_FC_READ_INPUT_REGS:
KhiemTran 0:54bd111a5369 155 //field1 = startreg, field2 = numregs
KhiemTran 0:54bd111a5369 156 this->readInputRegisters(field1, field2);
KhiemTran 0:54bd111a5369 157 break;
KhiemTran 0:54bd111a5369 158
KhiemTran 0:54bd111a5369 159 case MB_FC_WRITE_COIL:
KhiemTran 0:54bd111a5369 160 //field1 = reg, field2 = status
KhiemTran 0:54bd111a5369 161 this->writeSingleCoil(field1, field2);
KhiemTran 0:54bd111a5369 162 break;
KhiemTran 0:54bd111a5369 163
KhiemTran 0:54bd111a5369 164 case MB_FC_WRITE_COILS:
KhiemTran 0:54bd111a5369 165 //field1 = startreg, field2 = numoutputs
KhiemTran 0:54bd111a5369 166 this->writeMultipleCoils(frame,field1, field2, frame[5]);
KhiemTran 0:54bd111a5369 167 break;
KhiemTran 0:54bd111a5369 168
KhiemTran 0:54bd111a5369 169 #endif
KhiemTran 0:54bd111a5369 170 default:
KhiemTran 0:54bd111a5369 171 this->exceptionResponse(fcode, MB_EX_ILLEGAL_FUNCTION);
KhiemTran 0:54bd111a5369 172 }
KhiemTran 0:54bd111a5369 173 }
KhiemTran 0:54bd111a5369 174
KhiemTran 0:54bd111a5369 175 void Modbus::exceptionResponse(uint8_t fcode, uint8_t excode) {
KhiemTran 0:54bd111a5369 176 //Clean frame buffer
KhiemTran 3:355a72e1d7d2 177 delete[] _frame;
KhiemTran 3:355a72e1d7d2 178 //free(_frame);
KhiemTran 0:54bd111a5369 179 _len = 2;
KhiemTran 3:355a72e1d7d2 180 _frame = new uint8_t[_len];
KhiemTran 3:355a72e1d7d2 181 //_frame = (uint8_t *) malloc(_len);
KhiemTran 0:54bd111a5369 182 _frame[0] = fcode + 0x80;
KhiemTran 0:54bd111a5369 183 _frame[1] = excode;
KhiemTran 0:54bd111a5369 184
KhiemTran 0:54bd111a5369 185 _reply = MB_REPLY_NORMAL;
KhiemTran 0:54bd111a5369 186 }
KhiemTran 0:54bd111a5369 187
KhiemTran 0:54bd111a5369 188 void Modbus::readRegisters(uint16_t startreg, uint16_t numregs) {
KhiemTran 0:54bd111a5369 189 //Check value (numregs)
KhiemTran 0:54bd111a5369 190 if (numregs < 0x0001 || numregs > 0x007D) {
KhiemTran 0:54bd111a5369 191 this->exceptionResponse(MB_FC_READ_REGS, MB_EX_ILLEGAL_VALUE);
KhiemTran 0:54bd111a5369 192 return;
KhiemTran 0:54bd111a5369 193 }
KhiemTran 0:54bd111a5369 194
KhiemTran 0:54bd111a5369 195 //Check Address
KhiemTran 0:54bd111a5369 196 //*** See comments on readCoils method.
KhiemTran 0:54bd111a5369 197 if (!this->searchRegister(startreg + 40001)) {
KhiemTran 0:54bd111a5369 198 this->exceptionResponse(MB_FC_READ_REGS, MB_EX_ILLEGAL_ADDRESS);
KhiemTran 0:54bd111a5369 199 return;
KhiemTran 0:54bd111a5369 200 }
KhiemTran 0:54bd111a5369 201
KhiemTran 0:54bd111a5369 202
KhiemTran 0:54bd111a5369 203 //Clean frame buffer
KhiemTran 3:355a72e1d7d2 204 delete[] _frame;
KhiemTran 3:355a72e1d7d2 205 //free(_frame);
KhiemTran 0:54bd111a5369 206 _len = 0;
KhiemTran 0:54bd111a5369 207
KhiemTran 0:54bd111a5369 208 //calculate the query reply message length
KhiemTran 0:54bd111a5369 209 //for each register queried add 2 bytes
KhiemTran 0:54bd111a5369 210 _len = 2 + numregs * 2;
KhiemTran 0:54bd111a5369 211
KhiemTran 3:355a72e1d7d2 212 _frame = new uint8_t[_len];
KhiemTran 3:355a72e1d7d2 213 //_frame = (uint8_t *) malloc(_len);
KhiemTran 0:54bd111a5369 214 if (!_frame) {
KhiemTran 0:54bd111a5369 215 this->exceptionResponse(MB_FC_READ_REGS, MB_EX_SLAVE_FAILURE);
KhiemTran 0:54bd111a5369 216 return;
KhiemTran 0:54bd111a5369 217 }
KhiemTran 0:54bd111a5369 218
KhiemTran 0:54bd111a5369 219 _frame[0] = MB_FC_READ_REGS;
KhiemTran 0:54bd111a5369 220 _frame[1] = _len - 2; //byte count
KhiemTran 0:54bd111a5369 221
KhiemTran 0:54bd111a5369 222 uint16_t val;
KhiemTran 0:54bd111a5369 223 uint16_t i = 0;
KhiemTran 0:54bd111a5369 224 while(numregs--) {
KhiemTran 0:54bd111a5369 225 //retrieve the value from the register bank for the current register
KhiemTran 0:54bd111a5369 226 val = this->Hreg(startreg + i);
KhiemTran 0:54bd111a5369 227 //write the high byte of the register value
KhiemTran 0:54bd111a5369 228 _frame[2 + i * 2] = val >> 8;
KhiemTran 0:54bd111a5369 229 //write the low byte of the register value
KhiemTran 0:54bd111a5369 230 _frame[3 + i * 2] = val & 0xFF;
KhiemTran 0:54bd111a5369 231 i++;
KhiemTran 0:54bd111a5369 232 }
KhiemTran 0:54bd111a5369 233
KhiemTran 0:54bd111a5369 234 _reply = MB_REPLY_NORMAL;
KhiemTran 0:54bd111a5369 235 }
KhiemTran 0:54bd111a5369 236
KhiemTran 0:54bd111a5369 237 void Modbus::writeSingleRegister(uint16_t reg, uint16_t value) {
KhiemTran 0:54bd111a5369 238 //No necessary verify illegal value (EX_ILLEGAL_VALUE) - because using word (0x0000 - 0x0FFFF)
KhiemTran 0:54bd111a5369 239 //Check Address and execute (reg exists?)
KhiemTran 0:54bd111a5369 240 if (!this->Hreg(reg, value)) {
KhiemTran 0:54bd111a5369 241 this->exceptionResponse(MB_FC_WRITE_REG, MB_EX_ILLEGAL_ADDRESS);
KhiemTran 0:54bd111a5369 242 return;
KhiemTran 0:54bd111a5369 243 }
KhiemTran 0:54bd111a5369 244
KhiemTran 0:54bd111a5369 245 //Check for failure
KhiemTran 0:54bd111a5369 246 if (this->Hreg(reg) != value) {
KhiemTran 0:54bd111a5369 247 this->exceptionResponse(MB_FC_WRITE_REG, MB_EX_SLAVE_FAILURE);
KhiemTran 0:54bd111a5369 248 return;
KhiemTran 0:54bd111a5369 249 }
KhiemTran 0:54bd111a5369 250
KhiemTran 0:54bd111a5369 251 _reply = MB_REPLY_ECHO;
KhiemTran 0:54bd111a5369 252 }
KhiemTran 0:54bd111a5369 253
KhiemTran 0:54bd111a5369 254 void Modbus::writeMultipleRegisters(uint8_t* frame,uint16_t startreg, uint16_t numoutputs, uint8_t bytecount) {
KhiemTran 0:54bd111a5369 255 //Check value
KhiemTran 0:54bd111a5369 256 if (numoutputs < 0x0001 || numoutputs > 0x007B || bytecount != 2 * numoutputs) {
KhiemTran 0:54bd111a5369 257 this->exceptionResponse(MB_FC_WRITE_REGS, MB_EX_ILLEGAL_VALUE);
KhiemTran 0:54bd111a5369 258 return;
KhiemTran 0:54bd111a5369 259 }
KhiemTran 0:54bd111a5369 260
KhiemTran 0:54bd111a5369 261 //Check Address (startreg...startreg + numregs)
KhiemTran 0:54bd111a5369 262 for (int k = 0; k < numoutputs; k++) {
KhiemTran 0:54bd111a5369 263 if (!this->searchRegister(startreg + 40001 + k)) {
KhiemTran 0:54bd111a5369 264 this->exceptionResponse(MB_FC_WRITE_REGS, MB_EX_ILLEGAL_ADDRESS);
KhiemTran 0:54bd111a5369 265 return;
KhiemTran 0:54bd111a5369 266 }
KhiemTran 0:54bd111a5369 267 }
KhiemTran 0:54bd111a5369 268
KhiemTran 0:54bd111a5369 269 //Clean frame buffer
KhiemTran 3:355a72e1d7d2 270 delete[] _frame;
KhiemTran 3:355a72e1d7d2 271 //free(_frame);
KhiemTran 0:54bd111a5369 272 _len = 5;
KhiemTran 3:355a72e1d7d2 273
KhiemTran 3:355a72e1d7d2 274 _frame = new uint8_t[_len];
KhiemTran 3:355a72e1d7d2 275 //_frame = (uint8_t *) malloc(_len);
KhiemTran 0:54bd111a5369 276 if (!_frame) {
KhiemTran 0:54bd111a5369 277 this->exceptionResponse(MB_FC_WRITE_REGS, MB_EX_SLAVE_FAILURE);
KhiemTran 0:54bd111a5369 278 return;
KhiemTran 0:54bd111a5369 279 }
KhiemTran 0:54bd111a5369 280
KhiemTran 0:54bd111a5369 281 _frame[0] = MB_FC_WRITE_REGS;
KhiemTran 0:54bd111a5369 282 _frame[1] = startreg >> 8;
KhiemTran 0:54bd111a5369 283 _frame[2] = startreg & 0x00FF;
KhiemTran 0:54bd111a5369 284 _frame[3] = numoutputs >> 8;
KhiemTran 0:54bd111a5369 285 _frame[4] = numoutputs & 0x00FF;
KhiemTran 0:54bd111a5369 286
KhiemTran 0:54bd111a5369 287 uint16_t val;
KhiemTran 0:54bd111a5369 288 uint16_t i = 0;
KhiemTran 0:54bd111a5369 289 while(numoutputs--) {
KhiemTran 0:54bd111a5369 290 val = (uint16_t)frame[6+i*2] << 8 | (uint16_t)frame[7+i*2];
KhiemTran 0:54bd111a5369 291 this->Hreg(startreg + i, val);
KhiemTran 0:54bd111a5369 292 i++;
KhiemTran 0:54bd111a5369 293 }
KhiemTran 0:54bd111a5369 294
KhiemTran 0:54bd111a5369 295 _reply = MB_REPLY_NORMAL;
KhiemTran 0:54bd111a5369 296 }
KhiemTran 0:54bd111a5369 297
KhiemTran 0:54bd111a5369 298 #ifndef USE_HOLDING_REGISTERS_ONLY
KhiemTran 0:54bd111a5369 299 void Modbus::readCoils(uint16_t startreg, uint16_t numregs) {
KhiemTran 0:54bd111a5369 300 //Check value (numregs)
KhiemTran 0:54bd111a5369 301 if (numregs < 0x0001 || numregs > 0x07D0) {
KhiemTran 0:54bd111a5369 302 this->exceptionResponse(MB_FC_READ_COILS, MB_EX_ILLEGAL_VALUE);
KhiemTran 0:54bd111a5369 303 return;
KhiemTran 0:54bd111a5369 304 }
KhiemTran 0:54bd111a5369 305
KhiemTran 0:54bd111a5369 306 //Check Address
KhiemTran 0:54bd111a5369 307 //Check only startreg. Is this correct?
KhiemTran 0:54bd111a5369 308 //When I check all registers in range I got errors in ScadaBR
KhiemTran 0:54bd111a5369 309 //I think that ScadaBR request more than one in the single request
KhiemTran 0:54bd111a5369 310 //when you have more then one datapoint configured from same type.
KhiemTran 0:54bd111a5369 311 if (!this->searchRegister(startreg + 1)) {
KhiemTran 0:54bd111a5369 312 this->exceptionResponse(MB_FC_READ_COILS, MB_EX_ILLEGAL_ADDRESS);
KhiemTran 0:54bd111a5369 313 return;
KhiemTran 0:54bd111a5369 314 }
KhiemTran 0:54bd111a5369 315
KhiemTran 0:54bd111a5369 316 //Clean frame buffer
KhiemTran 3:355a72e1d7d2 317 delete[] _frame;
KhiemTran 3:355a72e1d7d2 318 //free(_frame);
KhiemTran 0:54bd111a5369 319 _len = 0;
KhiemTran 0:54bd111a5369 320
KhiemTran 0:54bd111a5369 321 //Determine the message length = function type, byte count and
KhiemTran 0:54bd111a5369 322 //for each group of 8 registers the message length increases by 1
KhiemTran 0:54bd111a5369 323 _len = 2 + numregs/8;
KhiemTran 0:54bd111a5369 324 if (numregs%8) _len++; //Add 1 to the message length for the partial byte.
KhiemTran 0:54bd111a5369 325
KhiemTran 3:355a72e1d7d2 326 _frame = new uint8_t[_len];
KhiemTran 3:355a72e1d7d2 327 //_frame = (uint8_t *) malloc(_len);
KhiemTran 0:54bd111a5369 328 if (!_frame) {
KhiemTran 0:54bd111a5369 329 this->exceptionResponse(MB_FC_READ_COILS, MB_EX_SLAVE_FAILURE);
KhiemTran 0:54bd111a5369 330 return;
KhiemTran 0:54bd111a5369 331 }
KhiemTran 0:54bd111a5369 332
KhiemTran 0:54bd111a5369 333 _frame[0] = MB_FC_READ_COILS;
KhiemTran 0:54bd111a5369 334 _frame[1] = _len - 2; //byte count (_len - function code and byte count)
KhiemTran 0:54bd111a5369 335
KhiemTran 0:54bd111a5369 336 uint8_t bitn = 0;
KhiemTran 0:54bd111a5369 337 uint16_t totregs = numregs;
KhiemTran 0:54bd111a5369 338 uint16_t i;
KhiemTran 0:54bd111a5369 339 while (numregs--) {
KhiemTran 0:54bd111a5369 340 i = (totregs - numregs) / 8;
KhiemTran 0:54bd111a5369 341 if (this->Coil(startreg))
KhiemTran 0:54bd111a5369 342 bitSet(_frame[2+i], bitn);
KhiemTran 0:54bd111a5369 343 else
KhiemTran 0:54bd111a5369 344 bitClear(_frame[2+i], bitn);
KhiemTran 0:54bd111a5369 345 //increment the bit index
KhiemTran 0:54bd111a5369 346 bitn++;
KhiemTran 0:54bd111a5369 347 if (bitn == 8) bitn = 0;
KhiemTran 0:54bd111a5369 348 //increment the register
KhiemTran 0:54bd111a5369 349 startreg++;
KhiemTran 0:54bd111a5369 350 }
KhiemTran 0:54bd111a5369 351
KhiemTran 0:54bd111a5369 352 _reply = MB_REPLY_NORMAL;
KhiemTran 0:54bd111a5369 353 }
KhiemTran 0:54bd111a5369 354
KhiemTran 0:54bd111a5369 355 void Modbus::readInputStatus(uint16_t startreg, uint16_t numregs) {
KhiemTran 0:54bd111a5369 356 //Check value (numregs)
KhiemTran 0:54bd111a5369 357 if (numregs < 0x0001 || numregs > 0x07D0) {
KhiemTran 0:54bd111a5369 358 this->exceptionResponse(MB_FC_READ_INPUT_STAT, MB_EX_ILLEGAL_VALUE);
KhiemTran 0:54bd111a5369 359 return;
KhiemTran 0:54bd111a5369 360 }
KhiemTran 0:54bd111a5369 361
KhiemTran 0:54bd111a5369 362 //Check Address
KhiemTran 0:54bd111a5369 363 //*** See comments on readCoils method.
KhiemTran 0:54bd111a5369 364 if (!this->searchRegister(startreg + 10001)) {
KhiemTran 0:54bd111a5369 365 this->exceptionResponse(MB_FC_READ_COILS, MB_EX_ILLEGAL_ADDRESS);
KhiemTran 0:54bd111a5369 366 return;
KhiemTran 0:54bd111a5369 367 }
KhiemTran 0:54bd111a5369 368
KhiemTran 0:54bd111a5369 369 //Clean frame buffer
KhiemTran 3:355a72e1d7d2 370 delete[] _frame;
KhiemTran 3:355a72e1d7d2 371 //free(_frame);
KhiemTran 0:54bd111a5369 372 _len = 0;
KhiemTran 0:54bd111a5369 373
KhiemTran 0:54bd111a5369 374 //Determine the message length = function type, byte count and
KhiemTran 0:54bd111a5369 375 //for each group of 8 registers the message length increases by 1
KhiemTran 0:54bd111a5369 376 _len = 2 + numregs/8;
KhiemTran 0:54bd111a5369 377 if (numregs%8) _len++; //Add 1 to the message length for the partial byte.
KhiemTran 0:54bd111a5369 378
KhiemTran 3:355a72e1d7d2 379 _frame = new uint8_t[_len];
KhiemTran 3:355a72e1d7d2 380 //_frame = (uint8_t *) malloc(_len);
KhiemTran 0:54bd111a5369 381 if (!_frame) {
KhiemTran 0:54bd111a5369 382 this->exceptionResponse(MB_FC_READ_INPUT_STAT, MB_EX_SLAVE_FAILURE);
KhiemTran 0:54bd111a5369 383 return;
KhiemTran 0:54bd111a5369 384 }
KhiemTran 0:54bd111a5369 385
KhiemTran 0:54bd111a5369 386 _frame[0] = MB_FC_READ_INPUT_STAT;
KhiemTran 0:54bd111a5369 387 _frame[1] = _len - 2;
KhiemTran 0:54bd111a5369 388
KhiemTran 0:54bd111a5369 389 uint8_t bitn = 0;
KhiemTran 0:54bd111a5369 390 uint16_t totregs = numregs;
KhiemTran 0:54bd111a5369 391 uint16_t i;
KhiemTran 0:54bd111a5369 392 while (numregs--) {
KhiemTran 0:54bd111a5369 393 i = (totregs - numregs) / 8;
KhiemTran 0:54bd111a5369 394 if (this->Ists(startreg))
KhiemTran 0:54bd111a5369 395 bitSet(_frame[2+i], bitn);
KhiemTran 0:54bd111a5369 396 else
KhiemTran 0:54bd111a5369 397 bitClear(_frame[2+i], bitn);
KhiemTran 0:54bd111a5369 398 //increment the bit index
KhiemTran 0:54bd111a5369 399 bitn++;
KhiemTran 0:54bd111a5369 400 if (bitn == 8) bitn = 0;
KhiemTran 0:54bd111a5369 401 //increment the register
KhiemTran 0:54bd111a5369 402 startreg++;
KhiemTran 0:54bd111a5369 403 }
KhiemTran 0:54bd111a5369 404
KhiemTran 0:54bd111a5369 405 _reply = MB_REPLY_NORMAL;
KhiemTran 0:54bd111a5369 406 }
KhiemTran 0:54bd111a5369 407
KhiemTran 0:54bd111a5369 408 void Modbus::readInputRegisters(uint16_t startreg, uint16_t numregs) {
KhiemTran 0:54bd111a5369 409 //Check value (numregs)
KhiemTran 0:54bd111a5369 410 if (numregs < 0x0001 || numregs > 0x007D) {
KhiemTran 0:54bd111a5369 411 this->exceptionResponse(MB_FC_READ_INPUT_REGS, MB_EX_ILLEGAL_VALUE);
KhiemTran 0:54bd111a5369 412 return;
KhiemTran 0:54bd111a5369 413 }
KhiemTran 0:54bd111a5369 414
KhiemTran 0:54bd111a5369 415 //Check Address
KhiemTran 0:54bd111a5369 416 //*** See comments on readCoils method.
KhiemTran 0:54bd111a5369 417 if (!this->searchRegister(startreg + 30001)) {
KhiemTran 0:54bd111a5369 418 this->exceptionResponse(MB_FC_READ_COILS, MB_EX_ILLEGAL_ADDRESS);
KhiemTran 0:54bd111a5369 419 return;
KhiemTran 0:54bd111a5369 420 }
KhiemTran 0:54bd111a5369 421
KhiemTran 0:54bd111a5369 422 //Clean frame buffer
KhiemTran 3:355a72e1d7d2 423 delete[] _frame;
KhiemTran 3:355a72e1d7d2 424 //free(_frame);
KhiemTran 0:54bd111a5369 425 _len = 0;
KhiemTran 0:54bd111a5369 426
KhiemTran 0:54bd111a5369 427 //calculate the query reply message length
KhiemTran 0:54bd111a5369 428 //for each register queried add 2 bytes
KhiemTran 0:54bd111a5369 429 _len = 2 + numregs * 2;
KhiemTran 0:54bd111a5369 430
KhiemTran 3:355a72e1d7d2 431 _frame = new uint8_t[_len];
KhiemTran 3:355a72e1d7d2 432 //_frame = (uint8_t *) malloc(_len);
KhiemTran 0:54bd111a5369 433 if (!_frame) {
KhiemTran 0:54bd111a5369 434 this->exceptionResponse(MB_FC_READ_INPUT_REGS, MB_EX_SLAVE_FAILURE);
KhiemTran 0:54bd111a5369 435 return;
KhiemTran 0:54bd111a5369 436 }
KhiemTran 0:54bd111a5369 437
KhiemTran 0:54bd111a5369 438 _frame[0] = MB_FC_READ_INPUT_REGS;
KhiemTran 0:54bd111a5369 439 _frame[1] = _len - 2;
KhiemTran 0:54bd111a5369 440
KhiemTran 0:54bd111a5369 441 uint16_t val;
KhiemTran 0:54bd111a5369 442 uint16_t i = 0;
KhiemTran 0:54bd111a5369 443 while(numregs--) {
KhiemTran 0:54bd111a5369 444 //retrieve the value from the register bank for the current register
KhiemTran 0:54bd111a5369 445 val = this->Ireg(startreg + i);
KhiemTran 0:54bd111a5369 446 //write the high byte of the register value
KhiemTran 0:54bd111a5369 447 _frame[2 + i * 2] = val >> 8;
KhiemTran 0:54bd111a5369 448 //write the low byte of the register value
KhiemTran 0:54bd111a5369 449 _frame[3 + i * 2] = val & 0xFF;
KhiemTran 0:54bd111a5369 450 i++;
KhiemTran 0:54bd111a5369 451 }
KhiemTran 0:54bd111a5369 452
KhiemTran 0:54bd111a5369 453 _reply = MB_REPLY_NORMAL;
KhiemTran 0:54bd111a5369 454 }
KhiemTran 0:54bd111a5369 455
KhiemTran 0:54bd111a5369 456 void Modbus::writeSingleCoil(uint16_t reg, uint16_t status) {
KhiemTran 0:54bd111a5369 457 //Check value (status)
KhiemTran 0:54bd111a5369 458 if (status != 0xFF00 && status != 0x0000) {
KhiemTran 0:54bd111a5369 459 this->exceptionResponse(MB_FC_WRITE_COIL, MB_EX_ILLEGAL_VALUE);
KhiemTran 0:54bd111a5369 460 return;
KhiemTran 0:54bd111a5369 461 }
KhiemTran 0:54bd111a5369 462
KhiemTran 0:54bd111a5369 463 //Check Address and execute (reg exists?)
KhiemTran 0:54bd111a5369 464 if (!this->Coil(reg, (bool)status)) {
KhiemTran 0:54bd111a5369 465 this->exceptionResponse(MB_FC_WRITE_COIL, MB_EX_ILLEGAL_ADDRESS);
KhiemTran 0:54bd111a5369 466 return;
KhiemTran 0:54bd111a5369 467 }
KhiemTran 0:54bd111a5369 468
KhiemTran 0:54bd111a5369 469 //Check for failure
KhiemTran 0:54bd111a5369 470 if (this->Coil(reg) != (bool)status) {
KhiemTran 0:54bd111a5369 471 this->exceptionResponse(MB_FC_WRITE_COIL, MB_EX_SLAVE_FAILURE);
KhiemTran 0:54bd111a5369 472 return;
KhiemTran 0:54bd111a5369 473 }
KhiemTran 0:54bd111a5369 474
KhiemTran 0:54bd111a5369 475 _reply = MB_REPLY_ECHO;
KhiemTran 0:54bd111a5369 476 }
KhiemTran 0:54bd111a5369 477
KhiemTran 0:54bd111a5369 478 void Modbus::writeMultipleCoils(uint8_t* frame,uint16_t startreg, uint16_t numoutputs, uint8_t bytecount) {
KhiemTran 0:54bd111a5369 479 //Check value
KhiemTran 0:54bd111a5369 480 uint16_t bytecount_calc = numoutputs / 8;
KhiemTran 0:54bd111a5369 481 if (numoutputs%8) bytecount_calc++;
KhiemTran 0:54bd111a5369 482 if (numoutputs < 0x0001 || numoutputs > 0x07B0 || bytecount != bytecount_calc) {
KhiemTran 0:54bd111a5369 483 this->exceptionResponse(MB_FC_WRITE_COILS, MB_EX_ILLEGAL_VALUE);
KhiemTran 0:54bd111a5369 484 return;
KhiemTran 0:54bd111a5369 485 }
KhiemTran 0:54bd111a5369 486
KhiemTran 0:54bd111a5369 487 //Check Address (startreg...startreg + numregs)
KhiemTran 0:54bd111a5369 488 for (int k = 0; k < numoutputs; k++) {
KhiemTran 0:54bd111a5369 489 if (!this->searchRegister(startreg + 1 + k)) {
KhiemTran 0:54bd111a5369 490 this->exceptionResponse(MB_FC_WRITE_COILS, MB_EX_ILLEGAL_ADDRESS);
KhiemTran 0:54bd111a5369 491 return;
KhiemTran 0:54bd111a5369 492 }
KhiemTran 0:54bd111a5369 493 }
KhiemTran 0:54bd111a5369 494
KhiemTran 0:54bd111a5369 495 //Clean frame buffer
KhiemTran 3:355a72e1d7d2 496 delete[] _frame;
KhiemTran 3:355a72e1d7d2 497 //free(_frame);
KhiemTran 0:54bd111a5369 498 _len = 5;
KhiemTran 3:355a72e1d7d2 499 _frame = new uint8_t[_len];
KhiemTran 3:355a72e1d7d2 500 //_frame = (uint8_t *) malloc(_len);
KhiemTran 0:54bd111a5369 501 if (!_frame) {
KhiemTran 0:54bd111a5369 502 this->exceptionResponse(MB_FC_WRITE_COILS, MB_EX_SLAVE_FAILURE);
KhiemTran 0:54bd111a5369 503 return;
KhiemTran 0:54bd111a5369 504 }
KhiemTran 0:54bd111a5369 505
KhiemTran 0:54bd111a5369 506 _frame[0] = MB_FC_WRITE_COILS;
KhiemTran 0:54bd111a5369 507 _frame[1] = startreg >> 8;
KhiemTran 0:54bd111a5369 508 _frame[2] = startreg & 0x00FF;
KhiemTran 0:54bd111a5369 509 _frame[3] = numoutputs >> 8;
KhiemTran 0:54bd111a5369 510 _frame[4] = numoutputs & 0x00FF;
KhiemTran 0:54bd111a5369 511
KhiemTran 0:54bd111a5369 512 uint8_t bitn = 0;
KhiemTran 0:54bd111a5369 513 uint16_t totoutputs = numoutputs;
KhiemTran 0:54bd111a5369 514 uint16_t i;
KhiemTran 0:54bd111a5369 515 while (numoutputs--) {
KhiemTran 0:54bd111a5369 516 i = (totoutputs - numoutputs) / 8;
KhiemTran 0:54bd111a5369 517 this->Coil(startreg, bitRead(frame[6+i], bitn));
KhiemTran 0:54bd111a5369 518 //increment the bit index
KhiemTran 0:54bd111a5369 519 bitn++;
KhiemTran 0:54bd111a5369 520 if (bitn == 8) bitn = 0;
KhiemTran 0:54bd111a5369 521 //increment the register
KhiemTran 0:54bd111a5369 522 startreg++;
KhiemTran 0:54bd111a5369 523 }
KhiemTran 0:54bd111a5369 524
KhiemTran 0:54bd111a5369 525 _reply = MB_REPLY_NORMAL;
KhiemTran 0:54bd111a5369 526 }
KhiemTran 0:54bd111a5369 527 #endif
KhiemTran 0:54bd111a5369 528
KhiemTran 0:54bd111a5369 529
KhiemTran 0:54bd111a5369 530