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@0:54bd111a5369, 2019-03-23 (annotated)

Committer:
KhiemTran
Date:
Sat Mar 23 02:30:36 2019 +0000
Revision:
0:54bd111a5369
Child:
3:355a72e1d7d2
OK

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