Air quality sensor
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
esp-airsensor/main/modbus_fn.c

168 lines
4.2 KiB

#include <assert.h>
#include "modbus_fn.h"
#include "modbus_crc.h"
#include <esp_log.h>
static const char *TAG = "mb";
static bool crc_matches(const uint8_t *buf, size_t len) {
assert(buf);
uint16_t real = modbus_crc(buf, len - 2);
uint16_t given = ((uint16_t) buf[len - 2] << 8) | (uint16_t) buf[len - 1];
return real == given;
}
// Read holding registers
int mb_build_fc3(uint8_t *buf, size_t len, size_t *resplen, uint8_t mbAddr, uint16_t ref, uint16_t qty) {
assert(buf);
assert(resplen);
if (len < 8) {
ESP_LOGE(TAG, "Buf len too short for FC3");
return -1;
}
if (qty > 127) {
ESP_LOGE(TAG, "FC3 qty too high");
return -1;
}
size_t wp = 0;
buf[wp++] = mbAddr;
buf[wp++] = 0x03;
buf[wp++] = (ref & 0xFF00) >> 8;
buf[wp++] = (ref & 0xFF);
buf[wp++] = (qty & 0xFF00) >> 8;
buf[wp++] = (qty & 0xFF);
uint16_t crc = modbus_crc(buf, wp);
buf[wp++] = (crc & 0xFF00) >> 8;
buf[wp++] = (crc & 0xFF);
*resplen = 5 + qty * 2;
return (int) wp;
}
static int mb_parse_fc3_4(const uint8_t *buf, size_t len, uint16_t *dst, size_t capacity, int fcX) {
assert(buf);
if (!crc_matches(buf, len)) {
ESP_LOGE(TAG, "FC%d CRC mismatch!", fcX);
return -1;
}
//11 03 06 AE41 5652 4340 49AD
if (len == 5 && buf[1] == (0x80+fcX)) {
ESP_LOGE(TAG, "FC%d exception: %d", fcX, buf[2]);
return -1;
}
if (buf[1] != fcX) {
ESP_LOGE(TAG, "FC%d decode fail: not FC%d", fcX, fcX);
return -1;
}
int num = buf[2] / 2; // this is the number of bytes
if (capacity < num) {
ESP_LOGE(TAG, "FC%d decode fail: dst too small for %d registers", fcX, num);
return -1;
}
for (int i = 0; i < num; i++) {
*dst++ = (((uint16_t) buf[3 + i * 2]) << 8) | (((uint16_t) buf[3 + i * 2 + 1]));
}
return num;
}
int mb_parse_fc3(const uint8_t *buf, size_t len, uint16_t *dst, size_t capacity) {
return mb_parse_fc3_4(buf, len, dst, capacity, 3);
}
//// Read input registers
int mb_build_fc4(uint8_t *buf, size_t len, size_t *resplen, uint8_t mbAddr, uint16_t ref, uint16_t qty) {
assert(buf);
assert(resplen);
if (len < 8) {
ESP_LOGE(TAG, "Buf len too short for FC4");
return -1;
}
if (qty > 127) {
ESP_LOGE(TAG, "FC3 qty too high");
return -1;
}
size_t wp = 0;
buf[wp++] = mbAddr;
buf[wp++] = 0x04;
buf[wp++] = (ref & 0xFF00) >> 8;
buf[wp++] = (ref & 0xFF);
buf[wp++] = (qty & 0xFF00) >> 8;
buf[wp++] = (qty & 0xFF);
uint16_t crc = modbus_crc(buf, wp);
buf[wp++] = (crc & 0xFF00) >> 8;
buf[wp++] = (crc & 0xFF);
*resplen = 5 + qty * 2;
return (int) wp;
}
int mb_parse_fc4(const uint8_t *buf, size_t len, uint16_t *dst, size_t capacity) {
return mb_parse_fc3_4(buf, len, dst, capacity, 4);
}
// Write holding registers
int mb_build_fc16(uint8_t *buf, size_t len, size_t *resplen, uint8_t mbAddr, uint16_t ref, const uint16_t *data, uint16_t qty) {
assert(buf);
assert(data);
assert(resplen);
if (len < 9 + qty * 2) {
ESP_LOGE(TAG, "Buf len too short for FC16, or bad len");
return -1;
}
if (qty > 127) {
ESP_LOGE(TAG, "FC16 qty too high");
return -1;
}
size_t wp = 0;
buf[wp++] = mbAddr;
buf[wp++] = 0x10;
buf[wp++] = (ref & 0xFF00) >> 8;
buf[wp++] = (ref & 0xFF);
buf[wp++] = (qty & 0xFF00) >> 8;
buf[wp++] = (qty & 0xFF);
buf[wp++] = qty * 2;
for (int i = 0; i < qty; i++) {
buf[wp++] = (data[i] & 0xFF00) >> 8;
buf[wp++] = (data[i] & 0xFF);
}
uint16_t crc = modbus_crc(buf, wp);
buf[wp++] = (crc & 0xFF00) >> 8;
buf[wp++] = (crc & 0xFF);
*resplen = 8;
return (int) wp;
}
bool mb_parse_fc16(const uint8_t *buf, size_t len)
{
assert(buf);
if (!crc_matches(buf, len)) {
ESP_LOGE(TAG, "FC16 CRC mismatch!");
return -1;
}
//11 10 0001 0002 1298
if (len == 5 && buf[1] == 0x90) {
ESP_LOGE(TAG, "FC16 exception: %d", buf[2]);
return -1;
}
if (buf[1] != 16) {
ESP_LOGE(TAG, "FC16 decode fail: not FC16");
return -1;
}
return 0;
}