kopia lustrzana https://github.com/SP8EBC/ParaTNC
105 wiersze
3.5 KiB
C
105 wiersze
3.5 KiB
C
/*
|
|
* rtu_parser.c
|
|
*
|
|
* Created on: 27.08.2020
|
|
* Author: mateusz
|
|
*/
|
|
|
|
#include "./modbus_rtu/rtu_parser.h"
|
|
#include "./modbus_rtu/rtu_return_values.h"
|
|
|
|
#define MODBUS_RTU_MIN_03_04_RESP_LN 7 // one register to read
|
|
|
|
/**
|
|
* 2021-03-21 03:06:56 CET: SR6WXP>AKLPRZ,WIDE2-1,qAS,SQ6NDL:=5038.00N101747.00E# DigiW1 + WX ~ Lotnisko Polska Nowa Wies ~ https://www.facebook.com/aeroklub.opolski
|
|
* 2021-03-21 03:06:58 CET: SR6WXP>AKLPRZ,WIDE2-1,qAR,SR9NSK:T#237,023,004,001,000,122,10000110
|
|
* 2021-03-21 03:07:58 CET: SR6WXP>AKLPRZ,WIDE2-1,qAO,SR6NKB:>F1V FFED, F2V 47, F3V 0, F4V 0, F5V 0, F6V 0
|
|
* 2021-03-21 03:08:59 CET: SR6WXP>AKLPRZ,WIDE2-1,qAR,SR9NSK:!5038.00N/01747.00E_344/006g011t025r...p...P...b09952h71
|
|
* 2021-03-21 03:11:59 CET: SR6WXP>AKLPRZ,WIDE2-1,qAO,SR6NKB:>F1V FFEE, F2V 47, F3V 0, F4V 0, F5V 0, F6V 0
|
|
* 2021-03-21 03:13:00 CET: SR6WXP>AKLPRZ,WIDE2-1,qAO,SR6NKB:!5038.00N/01747.00E_352/005g009t025r...p...P...b09951h71
|
|
* 2021-03-21 03:16:00 CET: SR6WXP>AKLPRZ,WIDE2-1,qAO,SR6NKB:>F1V FFEE, F2V 47, F3V 0, F4V 0, F5V 0, F6V 0
|
|
* 2021-03-21 03:17:01 CET: SR6WXP>AKLPRZ,WIDE2-1,qAR,SR9NSK:!5038.00N/01747.00E_348/006g008t025r...p...P...b09950h71
|
|
*
|
|
* 2021-03-21 05:36:27 CET: SR6WXP>AKLPRZ,WIDE2-1,qAS,SQ6NDL:>F1V FFFA, F2V 46, F3V 0, F4V 0, F5V 0, F6V 0
|
|
* 2021-03-21 05:37:28 CET: SR6WXP>AKLPRZ,WIDE2-1,qAS,SQ6NDL:!5038.00N/01747.00E_339/003g007t027r...p...P...b09923h70
|
|
* 2021-03-21 05:37:30 CET: SR6WXP>AKLPRZ,WIDE2-1,qAR,SR9NSK:T#252,026,004,001,000,122,10000110
|
|
* 2021-03-21 05:40:30 CET: SR6WXP>AKLPRZ,WIDE2-1,qAS,SQ6NDL:>F1V FFFA, F2V 46, F3V 0, F4V 0, F5V 0, F6V 0
|
|
*/
|
|
|
|
int32_t rtu_parser_03_04_registers(uint8_t* input, uint16_t input_ln, rtu_register_data_t* output, rtu_exception_t* exception) {
|
|
uint32_t retval = MODBUS_RET_UNINITIALIZED;
|
|
|
|
uint16_t data = 0;
|
|
|
|
uint8_t slave_address_from_frame = 0;
|
|
|
|
// iterator through input table and registers table
|
|
int i = 0, j = 3;
|
|
|
|
// rewind the input buffer if the first byte is not valid modbus rtu slave address
|
|
if (*input < 1 || *input > 0xF7) {
|
|
// don't loop here and assume that the modbus response is shifted only
|
|
// by one byte
|
|
input++;
|
|
}
|
|
|
|
// 7 bytes is the shortest meaningful Modbus RTU frame
|
|
// with a value of single register
|
|
if (input_ln < MODBUS_RTU_MIN_03_04_RESP_LN) {
|
|
retval = MODBUS_RET_TOO_SHORT;
|
|
}
|
|
else {
|
|
// fetch slave address
|
|
data = *input;
|
|
|
|
// TODO: store slave address
|
|
slave_address_from_frame = data;
|
|
|
|
// fetch function code
|
|
data = *(input + 1);
|
|
|
|
// if the exception flag is set
|
|
if ((data & 0x80) > 0) {
|
|
// parse the exception value
|
|
*exception = rtu_exception_from_frame_data(data);
|
|
|
|
// and set the return value
|
|
retval = MODBUS_RET_GOT_EXCEPTION;
|
|
}
|
|
|
|
// check if the function code is correct or not
|
|
else if (data == 0x03 || data == 0x04) {
|
|
|
|
// check if this is an answer from the slave we expect
|
|
if (slave_address_from_frame == output->slave_address) {
|
|
// fetch the function result lenght in bytes
|
|
data = *(input + 2);
|
|
|
|
// store amount of registers in this response
|
|
output->number_of_registers = data / 2;
|
|
|
|
// get all registers values
|
|
for (int i = 0; i < output->number_of_registers && i < MODBUS_RTU_MAX_REGISTERS_AT_ONCE; i++) {
|
|
output->registers_values[i] = *(input + j) << 8 | *(input + j + 1);
|
|
|
|
// moving to next 16bit word with next register
|
|
j += 2;
|
|
}
|
|
retval = MODBUS_RET_OK;
|
|
}
|
|
else {
|
|
retval = MODBUS_RET_UNEXP_SLAVE_ADR;
|
|
}
|
|
|
|
}
|
|
else {
|
|
// if not exit with an error as this isn't
|
|
// correct parser for this modbus function
|
|
retval = MODBUS_RET_WRONG_FUNCTION;
|
|
}
|
|
}
|
|
|
|
return retval;
|
|
}
|
|
|