151 lines
3.6 KiB
C++
151 lines
3.6 KiB
C++
#include "mission/modbus_trigger_service.hpp"
|
|
|
|
#include <arpa/inet.h>
|
|
#include <netinet/in.h>
|
|
#include <sys/socket.h>
|
|
#include <unistd.h>
|
|
|
|
#include <cstring>
|
|
#include <vector>
|
|
|
|
namespace lm {
|
|
|
|
namespace {
|
|
|
|
constexpr int kMinCoilId = 1001;
|
|
constexpr int kMaxCoilId = 2000;
|
|
|
|
bool coilIdValid(int coil_id)
|
|
{
|
|
return coil_id >= kMinCoilId && coil_id <= kMaxCoilId;
|
|
}
|
|
|
|
int modbusAddressToCoilId(uint16_t address)
|
|
{
|
|
return static_cast<int>(address);
|
|
}
|
|
|
|
} // namespace
|
|
|
|
ModbusTriggerService::ModbusTriggerService(MissionStore& store, EnqueueFn enqueue_fn, int tcp_port)
|
|
: store_(store), enqueue_fn_(std::move(enqueue_fn)), tcp_port_(tcp_port)
|
|
{
|
|
tcp_thread_ = std::thread([this] { tcpLoop(); });
|
|
}
|
|
|
|
ModbusTriggerService::~ModbusTriggerService()
|
|
{
|
|
stop_ = true;
|
|
if (tcp_thread_.joinable())
|
|
tcp_thread_.join();
|
|
}
|
|
|
|
nlohmann::json ModbusTriggerService::coilStates() const
|
|
{
|
|
std::lock_guard<std::mutex> lock(mu_);
|
|
nlohmann::json out = nlohmann::json::array();
|
|
for (int coil = kMinCoilId; coil <= kMaxCoilId; ++coil)
|
|
{
|
|
const bool value = coils_.count(coil) ? coils_.at(coil) : false;
|
|
if (value || store_.findTriggerByCoil(coil))
|
|
{
|
|
out.push_back({{"coil_id", coil}, {"value", value}});
|
|
}
|
|
}
|
|
return out;
|
|
}
|
|
|
|
bool ModbusTriggerService::writeCoil(int coil_id, bool value, std::string& err)
|
|
{
|
|
if (!coilIdValid(coil_id))
|
|
{
|
|
err = "coil_id must be between 1001 and 2000";
|
|
return false;
|
|
}
|
|
std::optional<std::string> mission_id;
|
|
{
|
|
std::lock_guard<std::mutex> lock(mu_);
|
|
const bool prev = coils_.count(coil_id) ? coils_.at(coil_id) : false;
|
|
coils_[coil_id] = value;
|
|
if (!prev && value)
|
|
{
|
|
const auto trigger = store_.findTriggerByCoil(coil_id);
|
|
if (trigger)
|
|
mission_id = trigger->value("mission_id", "");
|
|
}
|
|
}
|
|
if (mission_id)
|
|
{
|
|
nlohmann::json req = {{"mission_id", *mission_id}, {"source", "modbus:" + std::to_string(coil_id)}};
|
|
return enqueue_fn_(req, err);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool ModbusTriggerService::fireCoil(int coil_id, std::string& err)
|
|
{
|
|
return writeCoil(coil_id, true, err);
|
|
}
|
|
|
|
void ModbusTriggerService::handleTcpClient(int client_fd)
|
|
{
|
|
std::vector<uint8_t> buffer(260);
|
|
const ssize_t n = recv(client_fd, buffer.data(), buffer.size(), 0);
|
|
if (n < 12)
|
|
return;
|
|
|
|
const uint8_t function = buffer[7];
|
|
if (function == 0x05 && n >= 12)
|
|
{
|
|
const uint16_t address = (static_cast<uint16_t>(buffer[8]) << 8) | buffer[9];
|
|
const uint16_t value = (static_cast<uint16_t>(buffer[10]) << 8) | buffer[11];
|
|
const int coil_id = modbusAddressToCoilId(address);
|
|
std::string err;
|
|
writeCoil(coil_id, value == 0xFF00, err);
|
|
send(client_fd, buffer.data(), static_cast<size_t>(n), 0);
|
|
}
|
|
}
|
|
|
|
void ModbusTriggerService::tcpLoop()
|
|
{
|
|
const int server_fd = socket(AF_INET, SOCK_STREAM, 0);
|
|
if (server_fd < 0)
|
|
return;
|
|
|
|
int opt = 1;
|
|
setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
|
|
|
|
sockaddr_in addr{};
|
|
addr.sin_family = AF_INET;
|
|
addr.sin_addr.s_addr = INADDR_ANY;
|
|
addr.sin_port = htons(static_cast<uint16_t>(tcp_port_));
|
|
if (bind(server_fd, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0)
|
|
{
|
|
close(server_fd);
|
|
return;
|
|
}
|
|
if (listen(server_fd, 8) < 0)
|
|
{
|
|
close(server_fd);
|
|
return;
|
|
}
|
|
|
|
while (!stop_)
|
|
{
|
|
fd_set fds;
|
|
FD_ZERO(&fds);
|
|
FD_SET(server_fd, &fds);
|
|
timeval tv{0, 200000};
|
|
if (select(server_fd + 1, &fds, nullptr, nullptr, &tv) <= 0)
|
|
continue;
|
|
const int client = accept(server_fd, nullptr, nullptr);
|
|
if (client < 0)
|
|
continue;
|
|
handleTcpClient(client);
|
|
close(client);
|
|
}
|
|
close(server_fd);
|
|
}
|
|
|
|
} // namespace lm
|