wip code for extruder master

master
Eggert Jung 3 years ago
parent e8d73df01d
commit 715acd439c

@ -4,6 +4,7 @@ FILES = $(SRCS:%.c=%) #main uart avrIOhelper/io-helper #uart#hier alle c-Datei
MCU = atmega2560
PROGC = m2560
CC = avr-gcc
#TOOL = stk500 -P /dev/ttyUSB0
TOOL = atmelice_isp
#TOOL = avrispmkii
#TOOL = usbasp-clone

@ -68,13 +68,18 @@ void ioHelperEdgeDetector(void);
#define BitPD6 22 //D22
#define BitPJ4 23 //D23
#define LED_AMPEL_ROT BitPJ4
#define LED_AMPEL_GELB BitPD6
#define LED_AMPEL_GRUEN BitPD5
#define LED_GRN_NOTAUS_ANLAGE BitPE4
#define LED_ROT_NOTAUS_ANLAGE BitPE5
#define LED_GRN_NOTAUS_SCHRANK BitPG5
#define LED_ROT_NOTAUS_SCHRANK BitPE3
#define LED_PLC_OK BitPH5
#define LED_BUS_OK BitPD4
#define LED_GRN_NOTAUS_SCHRANK BitPB6
#define LED_ROT_NOTAUS_SCHRANK BitPB7
#define LED_GRN_NOTAUS_ANLAGE BitPL1
#define LED_ROT_NOTAUS_ANLAGE BitPL2
#define LED_GRN_NOTAUS_ANLAGE_R BitPL0
#define LED_ROT_NOTAUS_ANLAGE_R BitPD4
#define LED_PLC_OK BitPL5
#define LED_BUS_OK BitPL3
//Inputs
@ -90,11 +95,23 @@ void ioHelperEdgeDetector(void);
#define BitPinK0 8 //A8
#define BitPinK1 9 //A9
#define BitPinK2 10 //A10
#define BitPinK3 11 //A11
#define BitPinK4 12 //A12
#define BitPinK5 13 //A13
#define BitPinK6 14 //A14
#define BitPinK7 15 //A15
#define BitPinD7 16 //I16
#define BitPinG2 17 //I17
#define BitPinG1 18 //I18
#define BitPinD3 10 //INO
#define BitPinD2 11 //IN1
#define IN_ANLAGE_EIN_INV BitPinF5
#define IN_NOTAUS_ANLAGE BitPinF7
#define IN_NOTAUS_SCHRANK BitPinK0
#define IN_ANLAGE_EIN_INV BitPinF7
#define IN_NOTAUS_ANLAGE_R BitPinK5
#define IN_NOTAUS_ANLAGE BitPinK4
#define IN_NOTAUS_SCHRANK BitPinK6
#endif

@ -0,0 +1,53 @@
#include <avr/io.h>
#include <stdio.h>
#include "kraftsensor.h"
#include "modbus.h"
#include "modbus-master.h"
int32_t kraftsensor_value;
uint8_t kraftsensor_valid;
int32_t kraftsensor_zero_offset = 0/*197700*/;
void do_kraftsensor(){
//static int32_t last_read;
//static int32_t old_value;
uint16_t m_data[4];
//int32_t kraftsensor_read;
/* read 2 16bit values and merge to 32bit signed integer */
readReg(1,0,2);
if(wait_receive(2, m_data, 10)){
kraftsensor_valid = 0;
printf("modbus error\n\r");
}
else{
kraftsensor_valid = 1;
int32_t tmp = (uint32_t)m_data[1]<<16;
tmp |= m_data[0];
//kraftsensor_value = tmp;
/* conversion magic to milliNewton */
kraftsensor_value = (((tmp /* + 539363*/)*9.81)/177.380)+kraftsensor_zero_offset;
//if(abs(kraftsensor_read - old_value) > 10000){
// if(abs(last_read - kraftsensor_read) > 10000){
// kraftsensor_value = old_value;
// //printf("delta: %ld\tvalue:%ld\n", kraftsensor_read - old_value, kraftsensor_read);
// //printf("spike\n");
// }
// else{
// kraftsensor_value = kraftsensor_read;
// //printf("jump\n");
// }
//}
//else{
// kraftsensor_value = kraftsensor_read;
//}
//last_read = kraftsensor_read;
//old_value = kraftsensor_value;
}
}

@ -0,0 +1,15 @@
#ifndef _MODBUS_MASTER_H_
#define _MODBUS_MASTER_H_
#include <stdint.h>
#define receiveOkay (modbusGetBusState() & (1<<ReceiveCompleted))
extern int32_t kraftsensor_value;
extern uint8_t kraftsensor_valid;
extern int32_t kraftsensor_zero_offset;
void do_kraftsensor(void);
#endif

243
main.c

@ -1,6 +1,7 @@
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h> // WatchDog
#include <stdint.h>
#include <string.h>
#include "Ethernet/socket.h"
@ -10,14 +11,17 @@
#include "Internet/MQTT/MQTTClient.h"
#include "avrIOhelper/io-helper.h"
#include "kraftsensor.h"
#include "millis.h"
#include "uart.h"
#include "spi.h"
#include "mqtt.h"
#include "modbus.h"
#include "modbus-master.h"
#include "util/delay.h"
#define PLC_MQTT_ENABLED 0
#define PLC_MQTT_ENABLED 1
Client mqtt_client;
@ -42,6 +46,8 @@ static void avr_init(void);
void timer0_init(void);
void print_network_information(void);
volatile uint16_t ADC_reading = 0;
void IO_LIBRARY_Init(void) {
uint8_t bufSize[] = {2, 2, 2, 2, 2, 2, 2, 2};
@ -55,41 +61,146 @@ void IO_LIBRARY_Init(void) {
}
void do_notaus(){
if(read_Input(IN_NOTAUS_ANLAGE, LEVEL) || read_Input(IN_NOTAUS_SCHRANK, LEVEL)){ // NOTAUS
set_Output(LED_GRN_NOTAUS_SCHRANK, OFF); // disable green lamps
if(read_Input(IN_NOTAUS_ANLAGE, LEVEL) || read_Input(IN_NOTAUS_SCHRANK, LEVEL) || read_Input(IN_NOTAUS_ANLAGE_R, LEVEL)){
/* at least one pressed */
//notaus_state = POWER_OFF;
ioHelperSetBit(outStates, LED_AMPEL_ROT, 1);
ioHelperSetBit(outStates, LED_AMPEL_GELB, 0);
ioHelperSetBit(outStates, LED_AMPEL_GRUEN, 0);
set_Output(LED_GRN_NOTAUS_SCHRANK, OFF);
set_Output(LED_GRN_NOTAUS_ANLAGE, OFF);
}
set_Output(LED_GRN_NOTAUS_ANLAGE_R, OFF);
if(read_Input(IN_NOTAUS_ANLAGE, LEVEL) && read_Input(IN_NOTAUS_SCHRANK, LEVEL)){ // both activated
set_Output(LED_ROT_NOTAUS_ANLAGE, BLINK);
set_Output(LED_ROT_NOTAUS_SCHRANK, BLINK);
}
else if(read_Input(IN_NOTAUS_ANLAGE, LEVEL)){ // top one activated
if(read_Input(IN_NOTAUS_ANLAGE, LEVEL)){
set_Output(LED_ROT_NOTAUS_ANLAGE, BLINK);
set_Output(LED_ROT_NOTAUS_SCHRANK, ON);
set_Output(LED_ROT_NOTAUS_ANLAGE_R, ON);
}
else if(read_Input(IN_NOTAUS_SCHRANK, LEVEL)){ // bottom one activated
if(read_Input(IN_NOTAUS_SCHRANK, LEVEL)){
set_Output(LED_ROT_NOTAUS_ANLAGE, ON);
set_Output(LED_ROT_NOTAUS_SCHRANK, BLINK);
set_Output(LED_ROT_NOTAUS_ANLAGE_R, ON);
}
if(read_Input(IN_NOTAUS_ANLAGE_R, LEVEL)){
set_Output(LED_ROT_NOTAUS_ANLAGE, ON);
set_Output(LED_ROT_NOTAUS_SCHRANK, ON);
set_Output(LED_ROT_NOTAUS_ANLAGE_R, BLINK);
}
else{ // none activated
set_Output(LED_ROT_NOTAUS_SCHRANK, OFF);
set_Output(LED_ROT_NOTAUS_ANLAGE, OFF);
if(!read_Input(IN_ANLAGE_EIN_INV, LEVEL)){
set_Output(LED_GRN_NOTAUS_ANLAGE, ON);
}
else if(!read_Input(IN_ANLAGE_EIN_INV, LEVEL)){
/* nothing pressed, but power not on */
//notaus_state = ARMED;
ioHelperSetBit(outStates, LED_AMPEL_ROT, 0);
ioHelperSetBit(outStates, LED_AMPEL_GELB, 1);
ioHelperSetBit(outStates, LED_AMPEL_GRUEN, 0);
set_Output(LED_ROT_NOTAUS_ANLAGE, ON);
set_Output(LED_ROT_NOTAUS_SCHRANK, ON);
set_Output(LED_ROT_NOTAUS_ANLAGE_R, ON);
set_Output(LED_GRN_NOTAUS_SCHRANK, ON);
set_Output(LED_GRN_NOTAUS_ANLAGE, ON);
set_Output(LED_GRN_NOTAUS_ANLAGE_R, ON);
}
else{
set_Output(LED_GRN_NOTAUS_ANLAGE, ON);
/* powered on */
//notaus_state = POWER_ON;
ioHelperSetBit(outStates, LED_AMPEL_ROT, 0);
ioHelperSetBit(outStates, LED_AMPEL_GELB, 0);
ioHelperSetBit(outStates, LED_AMPEL_GRUEN, 1);
set_Output(LED_GRN_NOTAUS_SCHRANK, ON);
set_Output(LED_ROT_NOTAUS_ANLAGE, ON);
set_Output(LED_ROT_NOTAUS_SCHRANK, ON);
set_Output(LED_GRN_NOTAUS_ANLAGE, ON);
set_Output(LED_GRN_NOTAUS_ANLAGE_R, ON);
set_Output(LED_ROT_NOTAUS_ANLAGE, OFF);
set_Output(LED_ROT_NOTAUS_SCHRANK, OFF);
set_Output(LED_ROT_NOTAUS_ANLAGE_R, OFF);
}
}
void send_values(void){
char msg[10];
sprintf(msg, "%d", ADC_reading);
mqtt_pub(&mqtt_client, "/Filamentanlage/01_Extruder/state/temp", msg, strlen(msg));
}
int main()
{
void modbus_set_Output(uint8_t slave_adr, uint8_t nr, uint8_t state) {
switch (state) {
case BLINK:
//ioHelperSetBit(outStatesBlinking, nr, ON);
writeCoil(slave_adr,100+nr,1);
wait_write(10);
break;
//case TOGGLE:
// ioHelperSetBit(outStatesBlinking, nr, OFF);
// if (ioHelperReadBit(outStates, nr)) {
// ioHelperSetBit(outStates, nr, OFF);
// } else {
// ioHelperSetBit(outStates, nr, ON);
// }
// break;
case ON:
//ioHelperSetBit(outStates, nr, ON);
//ioHelperSetBit(outStatesBlinking, nr, OFF);
writeCoil(slave_adr,nr,1);
wait_write(10);
break;
case OFF:
writeCoil(slave_adr,nr,0);
wait_write(10);
//ioHelperSetBit(outStates, nr, OFF);
//ioHelperSetBit(outStatesBlinking, nr, OFF);
break;
}
}
uint8_t modbus_read_Input(uint8_t nr, uint8_t type) {
uint8_t state = 0;
switch (type) {
case LEVEL:
state = ioHelperReadBit(inStates, nr);
break;
case EDGE:
state = ioHelperReadBit(inStatesBothEdges, nr);
ioHelperSetBit(inStatesBothEdges, nr, 0);
break;
case RISING:
state = ioHelperReadBit(inStatesRisingEdge, nr);
ioHelperSetBit(inStatesRisingEdge, nr, 0);
break;
case FALLING:
state = ioHelperReadBit(inStatesFallingEdge, nr);
ioHelperSetBit(inStatesFallingEdge, nr, 0);
break;
}
//reset edges after being read once
// if (state) {
// if (type == EDGE)
// ioHelperSetBit(inStatesBothEdges, nr, OFF);
// if (type == RISING)
// ioHelperSetBit(inStatesRisingEdge, nr, OFF);
// if (type == FALLING)
// ioHelperSetBit(inStatesFallingEdge, nr, OFF);
// }
return state;
}
int main(){
// INIT MCU
avr_init();
spi_init(); //SPI Master, MODE0, 4Mhz(DIV4), CS_PB.3=HIGH - suitable for WIZNET 5x00(1/2/5)
@ -136,7 +247,7 @@ int main()
}
// Subscribe to all topics
char SubString[] = "/Filamentanlage/02_Wasserbecken/#";
char SubString[] = "/Filamentanlage/01_Extruder/set/#";
//char SubString[] = "/Filamentanlage/03_Wasserbecken/#";
mqtt_rc = MQTTSubscribe(&mqtt_client, SubString, QOS0, messageArrived);
printf("Subscribed (%s) %ld\r\n", SubString, mqtt_rc);
@ -144,12 +255,15 @@ int main()
ioHelperSetBit(outStatesBlinking, LED_PLC_OK, 1);
ioHelperSetBit(outStates, BitPG5, 1);
uint32_t timer_blink_outs = millis();
uint32_t timer_send_uptime = millis();
uint32_t timer_send_info = millis();
printf("anlage: %x\n\r", read_Input(IN_ANLAGE_EIN_INV, LEVEL));
//printf("anlage: %x\n\r", read_Input(IN_ANLAGE_EIN_INV, LEVEL));
uint16_t tgl = 0;
while(1)
{
wdt_reset(); // WDT reset at least every sec
@ -162,7 +276,26 @@ int main()
if(millis() - timer_blink_outs > 500){
outStates[0] ^= outStatesBlinking[0];
outStates[1] ^= outStatesBlinking[1];
outStates[2] ^= outStatesBlinking[2];
outStates[3] ^= outStatesBlinking[3];
timer_blink_outs = millis();
do_kraftsensor();
char msg[64];
sprintf(msg, "%d", kraftsensor_value);
mqtt_pub(&mqtt_client, "/Filamentanlage/01_Extruder/kraft", msg, strlen(msg));
//tgl ^= 0x01;
//modbus_set_Output(4, 0, OFF);
//modbus_set_Output(4, 1, ON);
//modbus_set_Output(4, 2, BLINK);
//modbus_set_Output(4, 3, tgl);
//readInputReg(4, 0, 1);
//uint16_t inp;
//wait_receive(1, &inp, 100);
//printf("read inputs: 0x%X\n", inp);
}
#if PLC_MQTT_ENABLED
@ -171,14 +304,22 @@ int main()
timer_send_uptime += 5000;
char msg[64];
sprintf(msg, "%ld", millis()/1000);
mqtt_pub(&mqtt_client, "/Filamentanlage/02_Wasserbecken/uptime", msg, strlen(msg));
mqtt_pub(&mqtt_client, "/Filamentanlage/01_Extruder/uptime", msg, strlen(msg));
}
#endif
if(read_Input(IN_ANLAGE_EIN_INV, FALLING)){
printf("anlage ein\n\r");
#if PLC_MQTT_ENABLED
// send misc info
if(millis() - timer_send_info > 200){
timer_send_info += 200;
send_values(); // 10ms
//send_info(); // 27ms every 200ms
}
#endif
//if(read_Input(IN_ANLAGE_EIN_INV, FALLING)){
// printf("anlage ein\n\r");
//}
// ioHelperSetBit(outStates, LED_GRN_NOTAUS_SCHRANK, 1);
//}
//else{
@ -208,6 +349,40 @@ void timer0_init(void)
TIMSK0 |= 1<<OCIE0A; //IRQ on TIMER0 output compareA
}
void timer1_init()
{
TCCR3A |= (1<<COM3B1);
TCCR3B |= _BV(WGM33);
ICR3 = 100;
OCR3A = 50;
DDRE |= 1 << 4;
TIMSK3 |= 1<<TOIE3;
TCCR3B |= _BV(CS31);
}
void timer3_init()
{
TCCR3A |= (1<<COM3B1);
TCCR3B |= _BV(WGM33);
ICR3 = 100;
OCR3B = 50;
DDRE |= 1 << 4;
TCCR3B |= _BV(CS31);
}
void initADC(void)
{
ADMUX = 1 << REFS0 | 0 << REFS1; //Select external Vref
ADCSRA = _BV(ADEN) | _BV(ADIE); // enable adc, enable interrupt
ADCSRA |= 1 << ADPS2 | 1 << ADPS1 | 1 << ADPS0; // set clock-prescaler to 128
ADCSRA |= 1 << ADSC; // start conversion
}
static void avr_init(void)
{
// Initialize device here.
@ -216,13 +391,21 @@ static void avr_init(void)
wdt_reset(); // wdt reset ~ every <2000ms
timer0_init();// Timer0 millis engine init
//timer3_init();
DDRL |= 1<<6;
uart_init();
initADC();
modbus_master_init();
sei(); //re-enable global interrupts
return;
}
void print_network_information(void)
{
@ -245,3 +428,11 @@ void print_network_information(void)
printf("DNS Server : %d.%d.%d.%d\n\r",gWIZNETINFO.dns[0],gWIZNETINFO.dns[1],gWIZNETINFO.dns[2],gWIZNETINFO.dns[3]);
}
ISR(ADC_vect)
{
//Reading 10bit conversion result
ADC_reading = ADCL; //copy the first LSB bits
ADC_reading |= ADCH << 8; //copy remaing byte
ADCSRA |= (1 << ADSC); //Start next conversion
}

@ -0,0 +1,114 @@
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include "modbus.h"
#define receiveOkay (modbusGetBusState() & (1<<ReceiveCompleted))
void timer2_init()
{
TCCR2A = (1<<WGM21); //TIMER0 SET-UP: CTC MODE
TCCR2B|=(1<<CS21); //prescaler 8
OCR2A = 200;
TIMSK2|=(1<<OCIE2A);
}
void modbus_master_init(){
modbusSetAddress(1); //better set this to sth.
modbusInit();
timer2_init(); // modbus tick timer
}
int8_t wait_receive(uint8_t len, uint16_t dest[], uint8_t timeout){
uint8_t breaker = timeout;
while(!receiveOkay && breaker) { //wait for client response, time out after 1s
breaker--;
_delay_ms(1);
if(breaker==0)
return -1;
}
if(receiveOkay) { //if this fails, there was either no response or a crc error
if(rxbuffer[1]&0x80) { //client responded with an error code
//handle the error
return -1;
}
else {
for(uint8_t x=0;x<len;x++) { //rxbuffer[2] should be 8 (4 registers => 8 bytes). You might want to check this at this point.
dest[x]=(rxbuffer[3+x*2]<<8)+rxbuffer[4+x*2]; //do sth with the acquired data.
}
}
}
return 0;
}
int8_t wait_write(uint8_t timeout){
uint8_t breaker = timeout;
while(!receiveOkay && breaker) { //wait for client response, time out after 1s
breaker--;
_delay_ms(1);
if(breaker==0)
return -1;
}
if(receiveOkay) { //if this fails, there was either no response or a crc error
if(rxbuffer[1]&0x80) { //client responded with an error code
return rxbuffer[1]&0x80;
}
}
return 0;
}
void readReg(uint8_t slaveid, uint16_t address, uint8_t amount) {
_delay_ms(2);
rxbuffer[0]=slaveid;
modbusSetAddress(slaveid);
rxbuffer[1]=0x03;
rxbuffer[2]=(address>>8)&0xFF;
rxbuffer[3]=address&0xFF;
rxbuffer[4]=0x00;
rxbuffer[5]=amount;
modbusSendMessage(5);
}
void writeReg(uint8_t slaveid, uint16_t address, uint16_t value) {
_delay_ms(2);
rxbuffer[0]=slaveid;
modbusSetAddress(slaveid);
rxbuffer[1]=0x06;
rxbuffer[2]=(address>>8)&0xFF;
rxbuffer[3]=address&0xFF;
rxbuffer[4]=(value>>8)&0xFF;;
rxbuffer[5]=value&0xFF;
modbusSendMessage(5);
}
void readInputReg(uint8_t slaveid, uint16_t address, uint16_t amount) {
_delay_ms(2);
rxbuffer[0]=slaveid;
modbusSetAddress(slaveid);
rxbuffer[1]=0x04;
rxbuffer[2]=(address>>8)&0xFF;
rxbuffer[3]=address&0xFF;
rxbuffer[4]=0;
rxbuffer[5]=amount;
modbusSendMessage(5);
}
void writeCoil(uint8_t slaveid, uint16_t address, uint16_t value) {
_delay_ms(2);
rxbuffer[0]=slaveid;
modbusSetAddress(slaveid);
rxbuffer[1]=0x05;
rxbuffer[2]=(address>>8)&0xFF;
rxbuffer[3]=address&0xFF;
rxbuffer[4]=value?0xFF:0x00;
rxbuffer[5]=0;
modbusSendMessage(5);
}
ISR(TIMER2_COMPA_vect) { //this ISR is called 9765.625 times per second
modbusTickTimer();
}

@ -0,0 +1,11 @@
#include <stdint.h>
void timer2_init(void);
void modbus_master_init(void);
int8_t wait_receive(uint8_t len, uint16_t dest[], uint8_t timeout);
int8_t wait_write(uint8_t timeout);
void readReg(uint8_t slaveid, uint16_t address, uint8_t amount);
void writeReg(uint8_t slaveid, uint16_t address, uint16_t value);
void readInputReg(uint8_t slaveid, uint16_t address, uint16_t amount);
void readInputStatus(uint8_t slaveid, uint16_t address, uint16_t amount);
void writeCoil(uint8_t slaveid, uint16_t address, uint16_t value);

@ -0,0 +1,436 @@
/*************************************************************************
Title: Yet another (small) modbus (server) implementation for the avr.
Author: Max Brueggemann
Hardware: any AVR with hardware UART, tested on Atmega 88/168 at 20Mhz
License: BSD-3-Clause
DESCRIPTION:
Refer to the header file yaMBSiavr.h.
USAGE:
Refer to the header file yaMBSiavr.h.
LICENSE:
Copyright 2017 Max Brueggemann, www.maxbrueggemann.de
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its contributors
may be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#include <avr/io.h>
#include "modbus.h"
#include <avr/interrupt.h>
volatile unsigned char BusState = 0;
volatile uint16_t modbusTimer = 0;
volatile unsigned char rxbuffer[MaxFrameIndex+1];
volatile uint16_t DataPos = 0;
volatile unsigned char PacketTopIndex = 7;
volatile unsigned char modBusStaMaStates = 0;
volatile uint16_t modbusDataAmount = 0;
volatile uint16_t modbusDataLocation = 0;
/* @brief: save address and amount
*
*/
void modbusSaveLocation(void)
{
modbusDataLocation=(rxbuffer[3]|(rxbuffer[2]<<8));
if (rxbuffer[1]==fcPresetSingleRegister || rxbuffer[1]==fcForceSingleCoil) modbusDataAmount=1;
else modbusDataAmount=(rxbuffer[5]|(rxbuffer[4]<<8));
}
/* @brief: returns 1 if data location adr is touched by current command
*
* Arguments: - adr: address of the data object
*
*/
uint8_t modbusIsInRange(uint16_t adr)
{
if((modbusDataLocation <= adr) && (adr<(modbusDataLocation+modbusDataAmount)))
return 1;
return 0;
}
/* @brief: returns 1 if range of data locations is touched by current command
*
* Arguments: - startAdr: address of first data object in range
* - lastAdr: address of last data object in range
*
*/
uint8_t modbusIsRangeInRange(uint16_t startAdr, uint16_t lastAdr)
{
if(modbusIsInRange(startAdr) && modbusIsInRange(lastAdr))
return 1;
return 0;
}
uint8_t modbusGetBusState(void)
{
return BusState;
}
#if ADDRESS_MODE == SINGLE_ADR
volatile unsigned char Address = 0x00;
uint8_t modbusGetAddress(void)
{
return Address;
}
void modbusSetAddress(unsigned char newadr)
{
Address = newadr;
}
#endif
#if PHYSICAL_TYPE == 485
void transceiver_txen(void)
{
TRANSCEIVER_ENABLE_PORT|=(1<<TRANSCEIVER_ENABLE_PIN);
TRANSCEIVER_ENABLE_PORT|=(1<<TRANSCEIVER_ENABLE_PIN_2);
}
void transceiver_rxen(void)
{
TRANSCEIVER_ENABLE_PORT&=~(1<<TRANSCEIVER_ENABLE_PIN);
TRANSCEIVER_ENABLE_PORT&=~(1<<TRANSCEIVER_ENABLE_PIN_2);
}
#endif
/* @brief: A fairly simple Modbus compliant 16 Bit CRC algorithm.
*
* Returns 1 if the crc check is positive, returns 0 and saves the calculated CRC bytes
* at the end of the data array if it fails.
*
*/
uint8_t crc16(volatile uint8_t *ptrToArray,uint8_t inputSize) //A standard CRC algorithm
{
uint16_t out=0xffff;
uint16_t carry;
unsigned char n;
inputSize++;
for (int l=0; l<inputSize; l++) {
out ^= ptrToArray[l];
for (n = 0; n < 8; n++) {
carry = out & 1;
out >>= 1;
if (carry) out ^= 0xA001;
}
}
//out=0x1234;
if ((ptrToArray[inputSize]==out%256) && (ptrToArray[inputSize+1]==out/256)) //check
{
return 1;
} else {
ptrToArray[inputSize]=out%256; //append Lo
ptrToArray[inputSize+1]=out/256; //append Hi
return 0;
}
}
/* @brief: copies a single bit from one char to another char (or arrays thereof)
*
*
*/
void listBitCopy(volatile uint8_t *source, uint16_t sourceNr,volatile uint8_t *target, uint16_t targetNr)
{
if(*(source+(sourceNr/8))&(1<<(sourceNr-((sourceNr/8)*8))))
{
*(target+(targetNr/8))|=(1<<(targetNr-((targetNr/8)*8)));
} else *(target+(targetNr/8))&=~(1<<(targetNr-((targetNr/8)*8)));
}
/* @brief: Back to receiving state.
*
*/
void modbusReset(void)
{
BusState=(1<<TimerActive); //stop receiving (error)
modbusTimer=0;
}
void modbusTickTimer(void)
{
if (BusState&(1<<TimerActive))
{
modbusTimer++;
if (BusState&(1<<Receiving)) //we are in receiving mode
{
if ((modbusTimer==modbusInterCharTimeout)) {
BusState|=(1<<GapDetected);
} else if ((modbusTimer==modbusInterFrameDelayReceiveEnd)) { //end of message
#if ADDRESS_MODE == MULTIPLE_ADR
if (crc16(rxbuffer,DataPos-3)) { //perform crc check only. This is for multiple/all address mode.
modbusSaveLocation();
BusState=(1<<ReceiveCompleted);
} else modbusReset();
#endif
#if ADDRESS_MODE == SINGLE_ADR
if (rxbuffer[0]==Address && crc16(rxbuffer,DataPos-3)) { //is the message for us? => perform crc check
modbusSaveLocation();
BusState=(1<<ReceiveCompleted);
} else modbusReset();
#endif
}
} else if (modbusTimer==modbusInterFrameDelayReceiveStart){
BusState|=(1<<BusTimedOut);
}
}
}
ISR(UART_RECEIVE_INTERRUPT)
{
unsigned char data;
data = UART_DATA;
//printf("%x ", data);
modbusTimer=0; //reset timer
if (!(BusState & (1<<ReceiveCompleted)) && !(BusState & (1<<TransmitRequested)) && !(BusState & (1<<Transmitting)) && (BusState & (1<<Receiving)) && !(BusState & (1<<BusTimedOut)))
{
if (DataPos>MaxFrameIndex) modbusReset();
else
{
rxbuffer[DataPos]=data;
DataPos++; //TODO: maybe prevent this from exceeding 255?
}
} else
// Bus state is Timed out..
if (!(BusState & (1<<ReceiveCompleted)) && !(BusState & (1<<TransmitRequested)) && !(BusState & (1<<Transmitting)) && !(BusState & (1<<Receiving)) && (BusState & (1<<BusTimedOut)))
{
rxbuffer[0]=data;
BusState=((1<<Receiving)|(1<<TimerActive));
DataPos=1;
}
}
ISR(UART_TRANSMIT_INTERRUPT)
{
BusState&=~(1<<TransmitRequested);
BusState|=(1<<Transmitting);
UART_DATA=rxbuffer[DataPos];
DataPos++;
if (DataPos==(PacketTopIndex+1)) {
UART_CONTROL&=~(1<<UART_UDRIE);
}
}
ISR(UART_TRANSMIT_COMPLETE_INTERRUPT)
{
#if PHYSICAL_TYPE == 485
transceiver_rxen();
#endif
modbusReset();
}
void modbusInit(void)
{
UBRRH = (unsigned char)((_UBRR) >> 8);
UBRRL = (unsigned char) _UBRR;
UART_STATUS = (1<<U2X); //double speed mode.
#ifdef URSEL // if UBRRH and UCSRC share the same I/O location , e.g. ATmega8
UCSRC = (1<<URSEL)|(3<<UCSZ0); //Frame Size
#else
UCSRC = (3<<UCSZ0); //Frame Size
#endif
UART_CONTROL = (1<<TXCIE)|(1<<RXCIE)|(1<<RXEN)|(1<<TXEN); // USART receiver and transmitter and receive complete interrupt
#if PHYSICAL_TYPE == 485
TRANSCEIVER_ENABLE_PORT_DDR|=(1<<TRANSCEIVER_ENABLE_PIN);
TRANSCEIVER_ENABLE_PORT_DDR|=(1<<TRANSCEIVER_ENABLE_PIN_2);
transceiver_rxen();
#endif
BusState=(1<<TimerActive);
}
/* @brief: Sends a response.
*
* Arguments: - packtop: Position of the last byte containing data.
* modbusSendException is a good usage example.
*/
void modbusSendMessage(unsigned char packtop)
{
PacketTopIndex=packtop+2;
crc16(rxbuffer,packtop);
BusState|=(1<<TransmitRequested);
DataPos=0;
#if PHYSICAL_TYPE == 485
transceiver_txen();
#endif
UART_CONTROL|=(1<<UART_UDRIE);
BusState&=~(1<<ReceiveCompleted);
}
/* @brief: Sends an exception response.
*
* Arguments: - exceptionCode
*
*/
void modbusSendException(unsigned char exceptionCode)
{
rxbuffer[1]|=(1<<7); //setting MSB of the function code (the exception flag)
rxbuffer[2]=exceptionCode; //Exceptioncode. Also the last byte containing data
modbusSendMessage(2);
}
/* @brief: Returns the amount of requested data objects (coils, discretes, registers)
*
*/
uint16_t modbusRequestedAmount(void)
{
return modbusDataAmount;
}
/* @brief: Returns the address of the first requested data object (coils, discretes, registers)
*
*/
uint16_t modbusRequestedAddress(void)
{
return modbusDataLocation;
}
/* @brief: copies a single or multiple bytes from one array of bytes to an array of 16-bit-words
*
*/
void intToModbusRegister(volatile uint16_t *inreg, volatile uint8_t *outreg, uint8_t amount)
{
for (uint8_t c=0; c<amount; c++)
{
*(outreg+c*2) = (uint8_t)(*(inreg+c) >> 8);
*(outreg+1+c*2) = (uint8_t)(*(inreg+c));
}
}
/* @brief: copies a single or multiple 16-bit-words from one array of integers to an array of bytes
*
*/
void modbusRegisterToInt(volatile uint8_t *inreg, volatile uint16_t *outreg, uint8_t amount)
{
for (uint8_t c=0; c<amount; c++)
{
*(outreg+c) = (*(inreg+c*2) << 8) + *(inreg+1+c*2);
}
}
/* @brief: Handles single/multiple register reading and single/multiple register writing.
*
* Arguments: - ptrToInArray: pointer to the user's data array containing registers
* - startAddress: address of the first register in the supplied array
* - size: input array size in the requested format (16bit-registers)
*
*/
uint8_t modbusExchangeRegisters(volatile uint16_t *ptrToInArray, uint16_t startAddress, uint16_t size)
{
if ((modbusDataLocation>=startAddress) && ((startAddress+size)>=(modbusDataAmount+modbusDataLocation))) {
if ((rxbuffer[1]==fcReadHoldingRegisters) || (rxbuffer[1]==fcReadInputRegisters) )
{
if ((modbusDataAmount*2)<=(MaxFrameIndex-4)) //message buffer big enough?
{
rxbuffer[2]=(unsigned char)(modbusDataAmount*2);
intToModbusRegister(ptrToInArray+(modbusDataLocation-startAddress),rxbuffer+3,modbusDataAmount);
modbusSendMessage(2+rxbuffer[2]);
return 1;
} else modbusSendException(ecIllegalDataValue);
}
else if (rxbuffer[1]==fcPresetMultipleRegisters)
{
if (((rxbuffer[6])>=modbusDataAmount*2) && ((DataPos-9)>=rxbuffer[6])) //enough data received?
{
modbusRegisterToInt(rxbuffer+7,ptrToInArray+(modbusDataLocation-startAddress),(unsigned char)(modbusDataAmount));
modbusSendMessage(5);
return 1;
} else modbusSendException(ecIllegalDataValue);//too few data bytes received
}
else if (rxbuffer[1]==fcPresetSingleRegister)
{
modbusRegisterToInt(rxbuffer+4,ptrToInArray+(modbusDataLocation-startAddress),1);
modbusSendMessage(5);
return 1;
}
//modbusSendException(ecSlaveDeviceFailure); //inapropriate call of modbusExchangeRegisters
return 0;
} else {
modbusSendException(ecIllegalDataValue);
return 0;
}
}
/* @brief: Handles single/multiple input/coil reading and single/multiple coil writing.
*
* Arguments: - ptrToInArray: pointer to the user's data array containing bits
* - startAddress: address of the first bit in the supplied array
* - size: input array size in the requested format (bits)
*
*/
uint8_t modbusExchangeBits(volatile uint8_t *ptrToInArray, uint16_t startAddress, uint16_t size)
{
if ((modbusDataLocation>=startAddress) && ((startAddress+size)>=(modbusDataAmount+modbusDataLocation)))
{
if ((rxbuffer[1]==fcReadInputStatus) || (rxbuffer[1]==fcReadCoilStatus))
{
if (modbusDataAmount<=((MaxFrameIndex-4)*8)) //message buffer big enough?
{
rxbuffer[2]=(modbusDataAmount/8);
if (modbusDataAmount%8>0)
{
rxbuffer[(uint8_t)(modbusDataAmount/8)+3]=0x00; //fill last data byte with zeros
rxbuffer[2]++;
}
for (uint16_t c = 0; c<modbusDataAmount; c++)
{
listBitCopy(ptrToInArray,modbusDataLocation-startAddress+c,rxbuffer+3,c);
}
modbusSendMessage(rxbuffer[2]+2);
return 1;
} else modbusSendException(ecIllegalDataValue); //too many bits requested within single request
}
else if (rxbuffer[1]==fcForceMultipleCoils)
{
if (((rxbuffer[6]*8)>=modbusDataAmount) && ((DataPos-9)>=rxbuffer[6])) //enough data received?
{
for (uint16_t c = 0; c<modbusDataAmount; c++)
{
listBitCopy(rxbuffer+7,c,ptrToInArray,modbusDataLocation-startAddress+c);
}
modbusSendMessage(5);
return 1;
} else modbusSendException(ecIllegalDataValue);//exception too few data bytes received
}
else if (rxbuffer[1]==fcForceSingleCoil) {
listBitCopy(rxbuffer+4,0,ptrToInArray,modbusDataLocation-startAddress);
modbusSendMessage(5);
return 1;
}
//modbusSendException(ecSlaveDeviceFailure); //inanpropriate call of modbusExchangeBits
return 0;
} else
{
modbusSendException(ecIllegalDataValue);
return 0;
}
}

@ -0,0 +1,421 @@
#ifndef yaMBIavr_H
#define yaMBIavr_H
/************************************************************************
Title: Yet another (small) Modbus (server) implementation for the avr.
Author: Max Brueggemann
Hardware: any AVR with hardware UART, tested on Atmega 88/168 at 20Mhz
License: BSD-3-Clause
LICENSE:
Copyright 2017 Max Brueggemann, www.maxbrueggemann.de
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its contributors
may be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
THE POSSIBILITY OF SUCH DAMAGE.
************************************************************************/
#include <avr/io.h>
/**
* @code #include <yaMBSIavr.h> @endcode
*
* @brief Interrupt-based Modbus implementation for small avr microcontrollers.
* The Modbus implementation guidelines at modbus.org call for response
* timeouts in the range of several seconds , hence only timing critical
* parts have been implemented within ISRs. The actual handling of the Modbus
* frame can easily be done in the main while loop.
*
* @author Max Brueggemann www.maxbrueggemann.de
*/
/* define baudrate of modbus */
#ifndef BAUD
#define BAUD 38400L
#endif
/*
* Definitions for transceiver enable pin.
*/
#ifndef TRANSCEIVER_ENABLE_PORT
#define TRANSCEIVER_ENABLE_PORT PORTJ
#endif
#ifndef TRANSCEIVER_ENABLE_PIN
#define TRANSCEIVER_ENABLE_PIN 6
#define TRANSCEIVER_ENABLE_PIN_2 5
#endif
#ifndef TRANSCEIVER_ENABLE_PORT_DDR
#define TRANSCEIVER_ENABLE_PORT_DDR DDRJ
#endif
/**
* @brief
* At the moment the user has to set the value for Baudrate and
* speed mode manually. The values depend on the operating frequency
* of your AVR and can be found in its datasheet.
*/
#if defined(__AVR_ATtiny2313__)
#define UART_TRANSMIT_COMPLETE_INTERRUPT USART_TX_vect
#define UART_RECEIVE_INTERRUPT USART_RX_vect
#define UART_TRANSMIT_INTERRUPT USART_UDRE_vect
#define UART_STATUS UCSRA
#define UART_CONTROL UCSRB
#define UART_DATA UDR
#define UART_UDRIE UDRIE
#elif defined(__AVR_ATmega164P__)
#define UART_TRANSMIT_COMPLETE_INTERRUPT USART1_TX_vect
#define UART_RECEIVE_INTERRUPT USART1_RX_vect
#define UART_TRANSMIT_INTERRUPT USART1_UDRE_vect
#define UART_STATUS UCSR1A
#define UART_CONTROL UCSR1B
#define UART_DATA UDR1
#define UART_UDRIE UDRIE1
#define UCSRC UCSR1C
#define RXCIE RXCIE1
#define TXCIE TXCIE1
#define RXEN RXEN1
#define TXEN TXEN1
#define UCSZ0 UCSZ10
#define U2X U2X1
#define UBRRH UBRR1H
#define UBRRL UBRR1L
#elif defined(__AVR_ATmega168PA__)|(__AVR_ATmega88PA__)|(__AVR_ATmega328P__)|(__AVR_ATmega168P__)|(__AVR_ATmega88P__)
#define UART_TRANSMIT_COMPLETE_INTERRUPT USART_TX_vect
#define UART_RECEIVE_INTERRUPT USART_RX_vect
#define UART_TRANSMIT_INTERRUPT USART_UDRE_vect
#define UART_STATUS UCSR0A
#define UART_CONTROL UCSR0B
#define UART_DATA UDR0
#define UART_UDRIE UDRIE0
#define UCSRC UCSR0C
#define RXCIE RXCIE0
#define TXCIE TXCIE0
#define RXEN RXEN0
#define TXEN TXEN0
#define UCSZ0 UCSZ00
#define U2X U2X0
#define UBRRH UBRR0H
#define UBRRL UBRR0L
#elif defined(__AVR_ATmega328PB__)
#define UART_TRANSMIT_COMPLETE_INTERRUPT USART0_TX_vect
#define UART_RECEIVE_INTERRUPT USART0_RX_vect
#define UART_TRANSMIT_INTERRUPT USART0_UDRE_vect
#define UART_STATUS UCSR0A
#define UART_CONTROL UCSR0B
#define UART_DATA UDR0
#define UART_UDRIE UDRIE0
#define UCSRC UCSR0C
#define RXCIE RXCIE0
#define TXCIE TXCIE0
#define RXEN RXEN0
#define TXEN TXEN0
#define UCSZ0 UCSZ00
#define U2X U2X0
#define UBRRH UBRR0H
#define UBRRL UBRR0L
#elif defined(__AVR_ATtiny441__)
#define UART_TRANSMIT_COMPLETE_INTERRUPT USART0_TX_vect
#define UART_RECEIVE_INTERRUPT USART0_RX_vect
#define UART_TRANSMIT_INTERRUPT USART0_UDRE_vect
#define UART_STATUS UCSR0A
#define UART_CONTROL UCSR0B
#define UART_DATA UDR0
#define UART_UDRIE UDRIE0
#define UCSRC UCSR0C
#define RXCIE RXCIE0
#define TXCIE TXCIE0
#define RXEN RXEN0
#define TXEN TXEN0
#define UCSZ0 UCSZ00
#define U2X U2X0
#define UBRRH UBRR0H
#define UBRRL UBRR0L
#elif defined(__AVR_ATmega8__)|| defined(__AVR_ATmega16__) || defined(__AVR_ATmega32__) || defined(__AVR_ATmega323__)
#define UART_TRANSMIT_COMPLETE_INTERRUPT USART_TXC_vect
#define UART_RECEIVE_INTERRUPT USART_RXC_vect
#define UART_TRANSMIT_INTERRUPT USART_UDRE_vect
#define UART_STATUS UCSRA
#define UART_CONTROL UCSRB
#define UART_DATA UDR
#define UART_UDRIE UDRIE
#elif defined(__AVR_AT90PWM3B__)
#define UART_TRANSMIT_COMPLETE_INTERRUPT USART_TX_vect
#define UART_RECEIVE_INTERRUPT USART_RX_vect
#define UART_TRANSMIT_INTERRUPT USART_UDRE_vect
#define UART_STATUS UCSRA
#define UART_CONTROL UCSRB
#define UART_DATA UDR
#define UART_UDRIE UDRIE
#elif defined(__AVR_ATmega1284P__)
#define UART_TRANSMIT_COMPLETE_INTERRUPT USART0_TX_vect
#define UART_RECEIVE_INTERRUPT USART0_RX_vect
#define UART_TRANSMIT_INTERRUPT USART0_UDRE_vect
#define UART_STATUS UCSR0A
#define UART_CONTROL UCSR0B
#define UART_DATA UDR0
#define UART_UDRIE UDRIE0
#define UCSRC UCSR0C
#define RXCIE RXCIE0
#define TXCIE TXCIE0
#define RXEN RXEN0
#define TXEN TXEN0
#define UCSZ0 UCSZ00
#define U2X U2X0
#define UBRRH UBRR0H
#define UBRRL UBRR0L
#elif defined(__AVR_ATmega2560__)
#define UART_TRANSMIT_COMPLETE_INTERRUPT USART3_TX_vect
#define UART_RECEIVE_INTERRUPT USART3_RX_vect
#define UART_TRANSMIT_INTERRUPT USART3_UDRE_vect
#define UART_STATUS UCSR3A
#define UART_CONTROL UCSR3B
#define UART_DATA UDR3
#define UART_UDRIE UDRIE3
#define UCSRC UCSR3C
#define RXCIE RXCIE3
#define TXCIE TXCIE3
#define RXEN RXEN3
#define TXEN TXEN3
#define UCSZ0 UCSZ30
#define U2X U2X0
#define UBRRH UBRR3H
#define UBRRL UBRR3L
#else
#error "no definition available"
#endif
#ifndef F_CPU
#error " F_CPU not defined "
#else
#define _UBRR (F_CPU / 8 / BAUD ) -1
#endif /* F_CPU */
/*
* Available address modes.
*/
#define MULTIPLE_ADR 2
#define SINGLE_ADR 1
/*
* Use SINGLE_ADR or MULTIPLE_ADR, default: SINGLE_ADR
* This is useful for building gateways, routers or clients that for whatever reason need multiple addresses.
*/
#define ADDRESS_MODE SINGLE_ADR
/*
* Use 485 or 232, default: 485
* Use 232 for testing purposes or very simple applications that do not require RS485 and bus topology.
*/
#define PHYSICAL_TYPE 485 //possible values: 485, 232
#if BAUD>=19200
#define modbusInterFrameDelayReceiveStart 16
#define modbusInterFrameDelayReceiveEnd 18
#define modbusInterCharTimeout 7
#else
#define modbusBlocksize 10
#define modbusBlockTime ((float)modbusBlocksize*1000000)/((float) BAUD) //is 260 for 38400
#define timerISROccurenceTime 100 //time in microseconds between two calls of modbusTickTimer
#define modbusInterFrameDelayReceiveStart (uint16_t)(modbusBlockTime*3.5/(float)timerISROccurenceTime)
#define modbusInterFrameDelayReceiveEnd (uint16_t)(modbusBlockTime*4/(float)timerISROccurenceTime)
#define modbusInterCharTimeout (uint16_t)(modbusBlockTime*1.5/(float)timerISROccurenceTime)
#endif
/**
* @brief Defines the maximum Modbus frame size accepted by the device. 255 is the default
* and also the maximum value. However, it might be useful to set this to lower
* values, with 8 being the lowest possible value, in order to save on ram space.
*/
#define MaxFrameIndex 255
/**
* @brief Modbus Function Codes
* Refer to modbus.org for further information.
* It's good practice to return exception code 01 in case you receive a function code
* that you haven't implemented in your application.
*/
#define fcReadCoilStatus 1 //read single/multiple coils
#define fcReadInputStatus 2 //read single/multiple inputs
#define fcReadHoldingRegisters 3 //read analog output registers
#define fcReadInputRegisters 4 //read analog input registers (2 Bytes per register)
#define fcForceSingleCoil 5 //write single bit
#define fcPresetSingleRegister 6 //write analog output register (2 Bytes)
#define fcForceMultipleCoils 15 //write multiple bits
#define fcPresetMultipleRegisters 16 //write multiple analog output registers (2 Bytes each)
#define fcReportSlaveID 17 //read device description, run status and other device specific information
/**
* @brief Modbus Exception Codes
* Refer to modbus.org for further information.
* It's good practice to return exception code 01 in case you receive a function code
* that you haven't implemented in your application.
*/
#define ecIllegalFunction 1
#define ecIllegalDataAddress 2
#define ecIllegalDataValue 3
#define ecSlaveDeviceFailure 4
#define ecAcknowledge 5
#define ecSlaveDeviceBusy 6
#define ecNegativeAcknowledge 7
#define ecMemoryParityError 8
/**
* @brief Internal bit definitions
*/
#define BusTimedOut 0
#define Receiving 1
#define Transmitting 2
#define ReceiveCompleted 3
#define TransmitRequested 4
#define TimerActive 5
#define GapDetected 6
/**
* @brief Configures the UART. Call this function only once.
*/
extern void modbusInit(void);
/**
* @brief receive/transmit data array
*/
extern volatile unsigned char rxbuffer[MaxFrameIndex+1];
/**
* @brief Current receive/transmit position
*/
extern volatile uint16_t DataPos;
/**
* This only applies to single address mode.
*/
#if ADDRESS_MODE == SINGLE_ADR
/**
* @brief: Read the device address
*/
extern uint8_t modbusGetAddress(void);
/**
* @brief: Set the device address
* Arguments: - newadr: the new device address
*/
extern void modbusSetAddress(unsigned char newadr);
#endif
/* @brief: Sends a response.
*
* Arguments: - packtop, index of the last byte in rxbuffer
* that contains payload. Maximum value is
* MaxFrameIndex-2.
*/
extern void modbusSendMessage(unsigned char packtop);
/* @brief: Sends a Modbus exception.
*
* Arguments: - exceptionCode
*/
extern void modbusSendException(unsigned char exceptionCode);
/* @brief: Discards the current transaction. For MULTIPLE_ADR-mode and general
* testing purposes. Call this function if you don't want to reply at all.
*/
void modbusReset(void);
/**
* @brief Call this function whenever possible and check if its return value has the ReceiveCompleted Bit set.
* Preferably do this in the main while. I do not recommend calling this function within ISRs.
* @example if (modbusGetBusState() & (1<<ReceiveCompleted)) {
* modbusSendExcepton(ecIllegalFunction);
* }
*/
extern uint8_t modbusGetBusState(void);
/**
* @brief Call every 100us using a timer ISR.
*/
extern void modbusTickTimer(void);
/**
* @brief Returns amount of bits/registers requested.
*/
extern uint16_t modbusRequestedAmount(void);
/**
* @brief Returns the address of the first requested bit/register.
*/
extern uint16_t modbusRequestedAddress(void);
/* A fairly simple and hopefully Modbus compliant 16 Bit CRC algorithm.
* Returns 1 if the crc check is positive, returns 0 if it fails.
* Appends two crc bytes to the array.
*/
extern uint8_t crc16(volatile uint8_t *ptrToArray,uint8_t inputSize);
/* @brief: Handles single/multiple input/coil reading and single/multiple coil writing.
*
* Arguments: - ptrToInArray: pointer to the user's data array containing bits
* - startAddress: address of the first bit in the supplied array
* - size: input array size in the requested format (bits)
*
*/
extern uint8_t modbusExchangeBits(volatile uint8_t *ptrToInArray, uint16_t startAddress, uint16_t size);
/* @brief: Handles single/multiple register reading and single/multiple register writing.
*
* Arguments: - ptrToInArray: pointer to the user's data array containing registers
* - startAddress: address of the first register in the supplied array
* - size: input array size in the requested format (16bit-registers)
*
*/
extern uint8_t modbusExchangeRegisters(volatile uint16_t *ptrToInArray, uint16_t startAddress, uint16_t size);
/* @brief: returns 1 if data location adr is touched by current command
*
* Arguments: - adr: address of the data object
*
*/
extern uint8_t modbusIsInRange(uint16_t adr);
/* @brief: returns 1 if range of data locations is touched by current command
*
* Arguments: - startAdr: address of first data object in range
* - lastAdr: address of last data object in range
*
*/
extern uint8_t modbusIsRangeInRange(uint16_t startAdr, uint16_t lastAdr);
extern volatile uint16_t modbusDataAmount;
extern volatile uint16_t modbusDataLocation;
#endif

@ -4,8 +4,8 @@
uint8_t mqtt_readBuffer[MQTT_BUFFER_SIZE];
volatile uint16_t mes_id;
wiz_NetInfo netInfo = { .mac = {0x00, 0x08, 0xdc, 0xab, 0xcd, 0xf1}, // Mac address
.ip = {192, 168, 2, 1}, // IP address
wiz_NetInfo netInfo = { .mac = {0x00, 0x08, 0xdc, 0xab, 0xcd, 0xf2}, // Mac address
.ip = {192, 168, 1, 1}, // IP address
.sn = {255, 255, 0, 0}, // Subnet mask
.dns = {0,0,0,0}, // DNS address (google dns)
.gw = {192, 168, 0, 1}, // Gateway address
@ -39,7 +39,7 @@ void mqtt_pub(Client* mqtt_client, char * mqtt_topic, char * mqtt_msg, int mqtt_
static uint8_t mqtt_err_cnt = 0;
int32_t mqtt_rc;
printf(">>MQTT pub msg nr%lu ", ++mqtt_pub_count);
//printf(">>MQTT pub msg nr%lu ", ++mqtt_pub_count);
MQTTMessage pubMessage;
pubMessage.qos = QOS0;
pubMessage.id = mes_id++;
@ -50,11 +50,11 @@ void mqtt_pub(Client* mqtt_client, char * mqtt_topic, char * mqtt_msg, int mqtt_
if (mqtt_rc == SUCCESSS)
{
mqtt_err_cnt = 0;
printf(" - OK\r\n");
//printf(" - OK\r\n");
}
else
{
printf(" - ERROR\r\n");
//printf(" - ERROR\r\n");
//Reboot device after 20 continuous errors (~ 20sec)
//while(1);
if(mqtt_err_cnt++ > 20)