add new joystick
This commit is contained in:
phamvannhat
parent
14cfc38d89
commit
11c1d0be39
|
|
@ -0,0 +1,89 @@
|
|||
cmake_minimum_required(VERSION 3.10)
|
||||
project(libdc_udp_demo LANGUAGES CXX)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
|
||||
find_package(Threads REQUIRED)
|
||||
find_package(PkgConfig QUIET)
|
||||
|
||||
# Try pkg-config first
|
||||
set(LIBDATACHANNEL_LIBS "")
|
||||
set(LIBDATACHANNEL_INCLUDE_DIRS "")
|
||||
set(LIBDATACHANNEL_LIBRARY_DIRS "")
|
||||
|
||||
if (PKG_CONFIG_FOUND)
|
||||
pkg_check_modules(LIBDATACHANNEL libdatachannel)
|
||||
endif()
|
||||
|
||||
if (LIBDATACHANNEL_FOUND)
|
||||
message(STATUS "Found libdatachannel via pkg-config")
|
||||
list(APPEND LIBDATACHANNEL_INCLUDE_DIRS ${LIBDATACHANNEL_INCLUDE_DIRS})
|
||||
list(APPEND LIBDATACHANNEL_LIBRARY_DIRS ${LIBDATACHANNEL_LIBRARY_DIRS})
|
||||
set(LIBDATACHANNEL_LIBS ${LIBDATACHANNEL_LIBRARIES})
|
||||
else()
|
||||
message(STATUS "pkg-config couldn't find libdatachannel.")
|
||||
message(STATUS "You can export PKG_CONFIG_PATH or set environment variables LIBDATACHANNEL_INCLUDE_DIR and LIBDATACHANNEL_LIB before running cmake.")
|
||||
# Allow manual override via environment variables:
|
||||
if (DEFINED ENV{LIBDATACHANNEL_INCLUDE_DIR})
|
||||
set(LIBDATACHANNEL_INCLUDE_DIRS $ENV{LIBDATACHANNEL_INCLUDE_DIR})
|
||||
message(STATUS "Using LIBDATACHANNEL_INCLUDE_DIR = ${LIBDATACHANNEL_INCLUDE_DIRS}")
|
||||
else()
|
||||
# common default
|
||||
set(LIBDATACHANNEL_INCLUDE_DIRS /usr/local/include)
|
||||
message(STATUS "Defaulting LIBDATACHANNEL_INCLUDE_DIRS = ${LIBDATACHANNEL_INCLUDE_DIRS}")
|
||||
endif()
|
||||
|
||||
if (DEFINED ENV{LIBDATACHANNEL_LIB})
|
||||
# user can provide full path or linker name(s)
|
||||
set(LIBDATACHANNEL_LIBS $ENV{LIBDATACHANNEL_LIB})
|
||||
message(STATUS "Using LIBDATACHANNEL_LIB = ${LIBDATACHANNEL_LIBS}")
|
||||
else()
|
||||
# sensible default linker names (adjust if your lib files differ)
|
||||
set(LIBDATACHANNEL_LIBS datachannel sctp ssl crypto pthread dl)
|
||||
message(STATUS "Defaulting LIBDATACHANNEL_LIBS = ${LIBDATACHANNEL_LIBS}")
|
||||
endif()
|
||||
|
||||
# optional library directories (if user set PKG_CONFIG_PATH not used)
|
||||
if (DEFINED ENV{LIBDATACHANNEL_LIBRARY_DIR})
|
||||
set(LIBDATACHANNEL_LIBRARY_DIRS $ENV{LIBDATACHANNEL_LIBRARY_DIR})
|
||||
link_directories(${LIBDATACHANNEL_LIBRARY_DIRS})
|
||||
else()
|
||||
# we assume /usr/local/lib is in link path - you can add more if needed
|
||||
link_directories(/usr/local/lib)
|
||||
endif()
|
||||
endif()
|
||||
|
||||
# include dirs for our headers
|
||||
include_directories(${CMAKE_SOURCE_DIR}/src)
|
||||
# also include libdatachannel headers
|
||||
include_directories(${LIBDATACHANNEL_INCLUDE_DIRS})
|
||||
|
||||
# add signaling static lib
|
||||
add_library(signaling STATIC src/udp_signaling.cpp src/udp_signaling.hpp)
|
||||
|
||||
# targets
|
||||
add_executable(peer_server src/peer_server.cpp)
|
||||
add_executable(peer_client src/peer_client.cpp)
|
||||
|
||||
# make sure signaling target exposes include dir (for consumers)
|
||||
target_include_directories(signaling PUBLIC ${CMAKE_SOURCE_DIR}/src)
|
||||
|
||||
# Link libraries: correct ordering and visibility
|
||||
# Note: target_link_libraries(<target> <PRIVATE|PUBLIC|INTERFACE> <libs...>)
|
||||
target_link_libraries(peer_server
|
||||
PRIVATE
|
||||
signaling
|
||||
${LIBDATACHANNEL_LIBS}
|
||||
Threads::Threads
|
||||
)
|
||||
|
||||
target_link_libraries(peer_client
|
||||
PRIVATE
|
||||
signaling
|
||||
${LIBDATACHANNEL_LIBS}
|
||||
Threads::Threads
|
||||
)
|
||||
|
||||
# install rules (optional)
|
||||
install(TARGETS peer_server peer_client DESTINATION bin)
|
||||
|
|
@ -0,0 +1,275 @@
|
|||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <vector>
|
||||
#include <cstring>
|
||||
#include <algorithm>
|
||||
#include "udp_signaling.hpp"
|
||||
#include "rtc/rtc.hpp"
|
||||
|
||||
//--------------------------------------------------
|
||||
// ENUM COMMAND
|
||||
//--------------------------------------------------
|
||||
typedef enum {
|
||||
CMD_NONE = 0x00,
|
||||
CMD_PING = 0x01,
|
||||
CMD_GET_STATUS = 0x02,
|
||||
CMD_GET_SENSOR = 0x03,
|
||||
|
||||
CMD_SET_SPEED = 0x10,
|
||||
CMD_SET_DIRECTION = 0x11,
|
||||
CMD_SET_STEERING = 0x12,
|
||||
CMD_SET_BRAKE = 0x13,
|
||||
|
||||
CMD_START_MOTOR = 0x20,
|
||||
CMD_STOP_MOTOR = 0x21,
|
||||
|
||||
CMD_MAX
|
||||
} Command_t;
|
||||
|
||||
// ACK
|
||||
typedef enum {
|
||||
ACK_OK = 0x00,
|
||||
ACK_INVALID_CMD = 0x01,
|
||||
ACK_INVALID_PAYLOAD = 0x02,
|
||||
ACK_CRC_ERROR = 0x03,
|
||||
ACK_BUSY = 0x04,
|
||||
ACK_FAIL = 0x05
|
||||
} AckCode_t;
|
||||
|
||||
//--------------------------------------------------
|
||||
// Frame Struct
|
||||
//--------------------------------------------------
|
||||
typedef struct {
|
||||
uint8_t sof;
|
||||
uint8_t msg_id;
|
||||
uint8_t cmd;
|
||||
uint8_t length;
|
||||
uint8_t payload[256];
|
||||
uint8_t checksum;
|
||||
} Frame_t;
|
||||
|
||||
//--------------------------------------------------
|
||||
// Checksum
|
||||
//--------------------------------------------------
|
||||
uint8_t calc_checksum(const Frame_t& f) {
|
||||
uint8_t sum = f.sof ^ f.msg_id ^ f.cmd ^ f.length;
|
||||
for (int i = 0; i < f.length; i++)
|
||||
sum ^= f.payload[i];
|
||||
return sum;
|
||||
}
|
||||
|
||||
//--------------------------------------------------
|
||||
// Build ACK frame
|
||||
//--------------------------------------------------
|
||||
std::vector<uint8_t> build_frame(uint8_t msg_id, uint8_t cmd, const uint8_t* payload, uint8_t len) {
|
||||
Frame_t f{};
|
||||
f.sof = 0xAA;
|
||||
f.msg_id = msg_id;
|
||||
f.cmd = cmd;
|
||||
f.length = len;
|
||||
|
||||
if (len > 0 && payload != nullptr)
|
||||
memcpy(f.payload, payload, len);
|
||||
|
||||
f.checksum = calc_checksum(f);
|
||||
|
||||
std::vector<uint8_t> out;
|
||||
out.reserve(4 + len + 1);
|
||||
out.push_back(f.sof);
|
||||
out.push_back(f.msg_id);
|
||||
out.push_back(f.cmd);
|
||||
out.push_back(f.length);
|
||||
for (int i = 0; i < len; i++)
|
||||
out.push_back(f.payload[i]);
|
||||
out.push_back(f.checksum);
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
//--------------------------------------------------
|
||||
// Parse frame
|
||||
//--------------------------------------------------
|
||||
bool parse_frame(const uint8_t* buf, size_t size, Frame_t& out) {
|
||||
if (size < 5) return false;
|
||||
if (buf[0] != 0xAA) return false;
|
||||
|
||||
out.sof = buf[0];
|
||||
out.msg_id = buf[1];
|
||||
out.cmd = buf[2];
|
||||
out.length = buf[3];
|
||||
|
||||
if (4 + out.length + 1 != size)
|
||||
return false;
|
||||
|
||||
memcpy(out.payload, &buf[4], out.length);
|
||||
out.checksum = buf[4 + out.length];
|
||||
|
||||
return out.checksum == calc_checksum(out);
|
||||
}
|
||||
|
||||
//--------------------------------------------------
|
||||
// Base64
|
||||
//--------------------------------------------------
|
||||
static const std::string b64_table =
|
||||
"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
|
||||
|
||||
static std::string base64_encode(const std::string &in) {
|
||||
std::string out;
|
||||
int val = 0, valb = -6;
|
||||
for (unsigned char c : in) {
|
||||
val = (val << 8) + c;
|
||||
valb += 8;
|
||||
while (valb >= 0) {
|
||||
out.push_back(b64_table[(val >> valb) & 0x3F]);
|
||||
valb -= 6;
|
||||
}
|
||||
}
|
||||
if (valb > -6)
|
||||
out.push_back(b64_table[((val << 8) >> (valb + 8)) & 0x3F]);
|
||||
while (out.size() % 4)
|
||||
out.push_back('=');
|
||||
return out;
|
||||
}
|
||||
|
||||
static std::string base64_decode(const std::string &in) {
|
||||
std::vector<int> T(256, -1);
|
||||
for (int i = 0; i < 64; i++)
|
||||
T[(unsigned char)b64_table[i]] = i;
|
||||
|
||||
std::string out;
|
||||
int val = 0, valb = -8;
|
||||
|
||||
for (unsigned char c : in) {
|
||||
if (T[c] == -1)
|
||||
break;
|
||||
val = (val << 6) + T[c];
|
||||
valb += 6;
|
||||
|
||||
if (valb >= 0) {
|
||||
out.push_back(char((val >> valb) & 0xFF));
|
||||
valb -= 8;
|
||||
}
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
//--------------------------------------------------
|
||||
// Helper: convert vector<uint8_t> -> rtc::binary (vector<std::byte>)
|
||||
//--------------------------------------------------
|
||||
static rtc::binary vec_u8_to_rtcbin(const std::vector<uint8_t>& v) {
|
||||
rtc::binary out(v.size());
|
||||
for (size_t i = 0; i < v.size(); ++i) out[i] = std::byte(v[i]);
|
||||
return out;
|
||||
}
|
||||
|
||||
//--------------------------------------------------
|
||||
// MAIN SERVER
|
||||
//--------------------------------------------------
|
||||
int main() {
|
||||
const int SERVER_PORT = 6000;
|
||||
|
||||
std::cout << "[Server] UDP Signaling on port " << SERVER_PORT << std::endl;
|
||||
|
||||
UdpSignaling signaling("", SERVER_PORT);
|
||||
signaling.start();
|
||||
|
||||
rtc::Configuration config;
|
||||
config.iceServers.emplace_back("stun:stun.l.google.com:19302");
|
||||
|
||||
auto pc = std::make_shared<rtc::PeerConnection>(config);
|
||||
|
||||
// Send local SDP
|
||||
pc->onLocalDescription([&signaling](rtc::Description desc) {
|
||||
std::string msg = "SDP|" + base64_encode(std::string(desc));
|
||||
signaling.send(msg);
|
||||
std::cout << "[Server] Sent SDP\n";
|
||||
});
|
||||
|
||||
// Send ICE
|
||||
pc->onLocalCandidate([&signaling](rtc::Candidate cand) {
|
||||
std::string msg = "ICE|" + base64_encode(cand.candidate())
|
||||
+ "|" + base64_encode(cand.mid());
|
||||
signaling.send(msg);
|
||||
std::cout << "[Server] Sent ICE\n";
|
||||
});
|
||||
|
||||
std::shared_ptr<rtc::DataChannel> dc_remote;
|
||||
|
||||
//--------------------------------------------------
|
||||
// DataChannel RECEIVED from client
|
||||
//--------------------------------------------------
|
||||
pc->onDataChannel([&](std::shared_ptr<rtc::DataChannel> dc) {
|
||||
std::cout << "[Server] DataChannel opened: " << dc->label() << "\n";
|
||||
dc_remote = dc;
|
||||
|
||||
// When client sends a frame
|
||||
dc->onMessage([dc](rtc::message_variant msg) {
|
||||
if (!std::holds_alternative<rtc::binary>(msg)) {
|
||||
std::cout << "[Server] Non-binary message ignored\n";
|
||||
return;
|
||||
}
|
||||
|
||||
auto bin = std::get<rtc::binary>(msg);
|
||||
|
||||
Frame_t f{};
|
||||
bool ok = parse_frame(
|
||||
reinterpret_cast<const uint8_t*>(bin.data()),
|
||||
bin.size(),
|
||||
f
|
||||
);
|
||||
|
||||
if (!ok) {
|
||||
std::cout << "[Server] Invalid frame CRC\n";
|
||||
return;
|
||||
}
|
||||
|
||||
std::cout << "[Server] Received CMD=" << (int)f.cmd
|
||||
<< " msg_id=" << (int)f.msg_id
|
||||
<< " len=" << (int)f.length << "\n";
|
||||
|
||||
//--------------------------------------------------
|
||||
// Build ACK and send back
|
||||
//--------------------------------------------------
|
||||
uint8_t ack_payload[1] = { ACK_OK };
|
||||
auto ack = build_frame(f.msg_id, f.cmd, ack_payload, 1);
|
||||
|
||||
// convert to rtc::binary and send
|
||||
rtc::binary bin_ack = vec_u8_to_rtcbin(ack);
|
||||
try {
|
||||
dc->send(bin_ack);
|
||||
std::cout << "[Server] Sent ACK for cmd=" << (int)f.cmd << "\n";
|
||||
} catch (const std::exception &e) {
|
||||
std::cout << "[Server] ACK send exception: " << e.what() << "\n";
|
||||
} catch (...) {
|
||||
std::cout << "[Server] ACK send unknown exception\n";
|
||||
}
|
||||
});
|
||||
});
|
||||
|
||||
//--------------------------------------------------
|
||||
// Signaling via UDP
|
||||
//--------------------------------------------------
|
||||
signaling.onMessage([&](const std::string &msg, const std::string &ip, int port) {
|
||||
signaling.setRemote(ip, port);
|
||||
|
||||
if (msg.rfind("SDP|", 0) == 0) {
|
||||
std::string sdp = base64_decode(msg.substr(4));
|
||||
pc->setRemoteDescription(rtc::Description(sdp));
|
||||
pc->setLocalDescription(); // generate answer
|
||||
}
|
||||
else if (msg.rfind("ICE|", 0) == 0) {
|
||||
size_t p = msg.find('|', 4);
|
||||
if (p != std::string::npos) {
|
||||
std::string c = base64_decode(msg.substr(4, p-4));
|
||||
std::string mid = base64_decode(msg.substr(p+1));
|
||||
pc->addRemoteCandidate(rtc::Candidate(c, mid));
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
std::cout << "[Server] Ready. Waiting for WebRTC client...\n";
|
||||
|
||||
while (1) std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
}
|
||||
|
|
@ -0,0 +1,313 @@
|
|||
#include <atomic>
|
||||
#include <cstdint>
|
||||
#include <thread>
|
||||
#include <iostream>
|
||||
#include <chrono>
|
||||
#include <cstring>
|
||||
#include <linux/joystick.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
|
||||
/* ============================================================
|
||||
* BASIC TYPES
|
||||
* ============================================================ */
|
||||
using u8 = uint8_t;
|
||||
using u16 = uint16_t;
|
||||
using u32 = uint32_t;
|
||||
using s16 = int16_t;
|
||||
|
||||
/* ============================================================
|
||||
* TIME
|
||||
* ============================================================ */
|
||||
static inline u32 now_ms()
|
||||
{
|
||||
using namespace std::chrono;
|
||||
return duration_cast<milliseconds>(
|
||||
steady_clock::now().time_since_epoch()).count();
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* CONTROL STATE (AUTOSAR FULL STATE)
|
||||
* ============================================================ */
|
||||
struct ControlState
|
||||
{
|
||||
std::atomic<s16> steering{0}; // [-32767..32767]
|
||||
std::atomic<u8> throttle{0}; // [0..100]
|
||||
std::atomic<bool> brake{true};
|
||||
std::atomic<u8> direction{0}; // 0=FWD, 1=REV
|
||||
std::atomic<u32> seq{0};
|
||||
};
|
||||
|
||||
static ControlState g_state;
|
||||
|
||||
/* ============================================================
|
||||
* SAFETY STATE
|
||||
* ============================================================ */
|
||||
struct SafetyState
|
||||
{
|
||||
std::atomic<bool> emergency{false};
|
||||
std::atomic<u32> last_input_ms{0};
|
||||
};
|
||||
|
||||
static SafetyState g_safety;
|
||||
|
||||
/* ============================================================
|
||||
* EVENT DEFINITIONS
|
||||
* ============================================================ */
|
||||
enum class EventType : u8
|
||||
{
|
||||
STEERING,
|
||||
THROTTLE,
|
||||
BRAKE,
|
||||
DIRECTION,
|
||||
EMERGENCY
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @brief Enumeration of emergency reasons for vehicle safety system.
|
||||
*
|
||||
* This enum defines all possible causes of emergency state in the system.
|
||||
* Each value corresponds to a specific failure or trigger condition.
|
||||
*/
|
||||
enum class EmergencyReason : uint8_t
|
||||
{
|
||||
NONE = 0U, /**< No emergency condition */
|
||||
BUTTON = 1U, /**< Emergency Stop button pressed by driver */
|
||||
WATCHDOG = 2U, /**< Input signal timeout detected by watchdog */
|
||||
SENSOR_FAIL = 3U, /**< Critical sensor failure (e.g., speed, steering angle) */
|
||||
COMM_ERROR = 4U, /**< Communication error (CAN, Ethernet, FlexRay) */
|
||||
BRAKE_FAIL = 5U, /**< Brake system failure */
|
||||
STEERING_FAIL = 6U, /**< Steering system failure */
|
||||
POWER_FAIL = 7U, /**< Power supply or ECU failure */
|
||||
SOFTWARE_FAULT = 8U /**< Software fault or CPU overload */
|
||||
};
|
||||
|
||||
struct EventPDU
|
||||
{
|
||||
EventType type;
|
||||
s16 s16_value;
|
||||
u8 u8_value;
|
||||
u32 timestamp;
|
||||
};
|
||||
|
||||
/* ============================================================
|
||||
* LOCK-FREE RING BUFFER (SPSC)
|
||||
* ============================================================ */
|
||||
template<typename T, size_t N>
|
||||
class RingBuffer
|
||||
{
|
||||
public:
|
||||
bool push(const T& v)
|
||||
{
|
||||
size_t h = head.load(std::memory_order_relaxed);
|
||||
size_t n = (h + 1) % N;
|
||||
if (n == tail.load(std::memory_order_acquire))
|
||||
return false;
|
||||
|
||||
buf[h] = v;
|
||||
head.store(n, std::memory_order_release);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool pop(T& out)
|
||||
{
|
||||
size_t t = tail.load(std::memory_order_relaxed);
|
||||
if (t == head.load(std::memory_order_acquire))
|
||||
return false;
|
||||
|
||||
out = buf[t];
|
||||
tail.store((t + 1) % N, std::memory_order_release);
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
T buf[N];
|
||||
std::atomic<size_t> head{0};
|
||||
std::atomic<size_t> tail{0};
|
||||
};
|
||||
|
||||
static RingBuffer<EventPDU, 128> g_event_ring;
|
||||
|
||||
/* ============================================================
|
||||
* AUTOSAR PDUs
|
||||
* ============================================================ */
|
||||
#pragma pack(push,1)
|
||||
|
||||
struct FullStatePDU
|
||||
{
|
||||
u8 pdu_type; // 0x01
|
||||
s16 steering;
|
||||
u8 throttle;
|
||||
u8 brake;
|
||||
u8 direction;
|
||||
u32 seq;
|
||||
};
|
||||
|
||||
struct AckPDU
|
||||
{
|
||||
u8 pdu_type; // 0x02
|
||||
u32 seq;
|
||||
u8 result; // 0=OK
|
||||
};
|
||||
|
||||
struct EmergencyPDU
|
||||
{
|
||||
u8 pdu_type; // 0x03
|
||||
EmergencyReason reason; // 1=Button, 2=Watchdog
|
||||
u32 timestamp;
|
||||
};
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
/* ============================================================
|
||||
* SAFETY HELPERS
|
||||
* ============================================================ */
|
||||
static inline void trigger_emergency(EmergencyReason reason)
|
||||
{
|
||||
bool expected = false;
|
||||
if (g_safety.emergency.compare_exchange_strong(expected, true))
|
||||
{
|
||||
g_state.brake.store(true);
|
||||
std::cerr << "[EMERGENCY] reason=" << static_cast<int>(reason) << "\n";
|
||||
}
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* WATCHDOG THREAD
|
||||
* ============================================================ */
|
||||
void watchdog_thread()
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
|
||||
u32 now = now_ms();
|
||||
u32 last = g_safety.last_input_ms.load();
|
||||
|
||||
if (!g_safety.emergency.load() && (now - last) > 300)
|
||||
{
|
||||
std::cerr << "[WD] Input timeout\n";
|
||||
trigger_emergency(EmergencyReason::WATCHDOG);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* JOYSTICK INPUT THREAD
|
||||
* ============================================================ */
|
||||
void joystick_thread()
|
||||
{
|
||||
int fd = open("/dev/input/js2", O_RDONLY | O_NONBLOCK);
|
||||
if (fd < 0) {
|
||||
perror("open /dev/input/js2");
|
||||
return;
|
||||
}
|
||||
|
||||
js_event e{};
|
||||
|
||||
while (true)
|
||||
{
|
||||
if (read(fd, &e, sizeof(e)) != sizeof(e)) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(2));
|
||||
continue;
|
||||
}
|
||||
|
||||
e.type &= ~JS_EVENT_INIT;
|
||||
g_safety.last_input_ms.store(now_ms());
|
||||
|
||||
if (e.type == JS_EVENT_AXIS)
|
||||
{
|
||||
if (e.number == 0) // steering
|
||||
{
|
||||
g_state.steering.store(e.value);
|
||||
g_event_ring.push({EventType::STEERING, e.value, 0, now_ms()});
|
||||
}
|
||||
else if (e.number == 1) // throttle
|
||||
{
|
||||
u8 th = (e.value + 32767) * 100 / 65534;
|
||||
g_state.throttle.store(th);
|
||||
g_state.brake.store(false);
|
||||
g_event_ring.push({EventType::THROTTLE, 0, th, now_ms()});
|
||||
}
|
||||
}
|
||||
else if (e.type == JS_EVENT_BUTTON)
|
||||
{
|
||||
if (e.number == 0 && e.value == 1)
|
||||
{
|
||||
trigger_emergency(EmergencyReason::WATCHDOG);
|
||||
g_event_ring.push({EventType::EMERGENCY, 0, 1, now_ms()});
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* EVENT HANDLER + FRAME BUILDER
|
||||
* ============================================================ */
|
||||
void event_handler_thread()
|
||||
{
|
||||
EventPDU ev{};
|
||||
|
||||
while (true)
|
||||
{
|
||||
if (!g_event_ring.pop(ev)) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
continue;
|
||||
}
|
||||
|
||||
/* ---------- EMERGENCY ---------- */
|
||||
if (g_safety.emergency.load())
|
||||
{
|
||||
EmergencyPDU em{};
|
||||
em.pdu_type = 0x03;
|
||||
em.reason = EmergencyReason::BUTTON;
|
||||
em.timestamp= now_ms();
|
||||
|
||||
std::cout << "[SEND] EMERGENCY PDU\n";
|
||||
continue;
|
||||
}
|
||||
|
||||
/* ---------- FULL STATE SNAPSHOT ---------- */
|
||||
FullStatePDU fs{};
|
||||
fs.pdu_type = 0x01;
|
||||
fs.steering = g_state.steering.load();
|
||||
fs.throttle = g_state.throttle.load();
|
||||
fs.brake = g_state.brake.load();
|
||||
fs.direction = g_state.direction.load();
|
||||
fs.seq = g_state.seq.fetch_add(1);
|
||||
|
||||
std::cout << "[SEND] FULL "
|
||||
<< "seq=" << fs.seq
|
||||
<< " steer=" << fs.steering
|
||||
<< " thr=" << (int)fs.throttle
|
||||
<< " brake=" << (int)fs.brake
|
||||
<< "\n";
|
||||
|
||||
/*
|
||||
* TODO:
|
||||
* - send via WebRTC DataChannel
|
||||
* - wait ACK
|
||||
* - retry / timeout
|
||||
*/
|
||||
}
|
||||
}
|
||||
|
||||
/* ============================================================
|
||||
* MAIN
|
||||
* ============================================================ */
|
||||
int main()
|
||||
{
|
||||
std::cout << "=== AUTOSAR PEER CLIENT CORE ===\n";
|
||||
|
||||
g_safety.last_input_ms.store(now_ms());
|
||||
|
||||
std::thread t_js(joystick_thread);
|
||||
std::thread t_ev(event_handler_thread);
|
||||
std::thread t_wd(watchdog_thread);
|
||||
|
||||
t_js.join();
|
||||
t_ev.join();
|
||||
t_wd.join();
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,113 @@
|
|||
#include "udp_signaling.hpp"
|
||||
#include <cstring>
|
||||
#include <iostream>
|
||||
#include <arpa/inet.h>
|
||||
#include <sys/socket.h>
|
||||
#include <unistd.h>
|
||||
|
||||
UdpSignaling::UdpSignaling(const std::string& local_ip, int local_port,
|
||||
const std::string& remote_ip, int remote_port)
|
||||
{
|
||||
sockfd_ = socket(AF_INET, SOCK_DGRAM, 0);
|
||||
if (sockfd_ < 0) {
|
||||
perror("socket");
|
||||
throw std::runtime_error("Cannot create UDP socket");
|
||||
}
|
||||
|
||||
std::memset(&local_addr_, 0, sizeof(local_addr_));
|
||||
local_addr_.sin_family = AF_INET;
|
||||
local_addr_.sin_port = htons(local_port);
|
||||
if (local_ip.empty()) {
|
||||
local_addr_.sin_addr.s_addr = INADDR_ANY;
|
||||
} else {
|
||||
if (inet_pton(AF_INET, local_ip.c_str(), &local_addr_.sin_addr) != 1) {
|
||||
close(sockfd_);
|
||||
throw std::runtime_error("Invalid local IP");
|
||||
}
|
||||
}
|
||||
|
||||
if (bind(sockfd_, (sockaddr*)&local_addr_, sizeof(local_addr_)) < 0) {
|
||||
perror("bind");
|
||||
close(sockfd_);
|
||||
throw std::runtime_error("Cannot bind UDP socket");
|
||||
}
|
||||
|
||||
if (!remote_ip.empty() && remote_port > 0) {
|
||||
std::memset(&remote_addr_, 0, sizeof(remote_addr_));
|
||||
remote_addr_.sin_family = AF_INET;
|
||||
remote_addr_.sin_port = htons(remote_port);
|
||||
if (inet_pton(AF_INET, remote_ip.c_str(), &remote_addr_.sin_addr) != 1) {
|
||||
close(sockfd_);
|
||||
throw std::runtime_error("Invalid remote IP");
|
||||
}
|
||||
remote_set_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
UdpSignaling::~UdpSignaling() {
|
||||
stop();
|
||||
if (sockfd_ >= 0) close(sockfd_);
|
||||
}
|
||||
|
||||
void UdpSignaling::start() {
|
||||
running_ = true;
|
||||
recv_thread_ = std::thread([this]() { receiveLoop(); });
|
||||
}
|
||||
|
||||
void UdpSignaling::stop() {
|
||||
running_ = false;
|
||||
if (recv_thread_.joinable()) recv_thread_.join();
|
||||
}
|
||||
|
||||
void UdpSignaling::send(const std::string& msg) {
|
||||
std::lock_guard<std::mutex> lk(send_mutex_);
|
||||
if (!remote_set_) {
|
||||
std::cerr << "[UdpSignaling] remote not set, cannot send\n";
|
||||
return;
|
||||
}
|
||||
ssize_t n = sendto(sockfd_, msg.c_str(), msg.size(), 0,
|
||||
(sockaddr*)&remote_addr_, sizeof(remote_addr_));
|
||||
if (n < 0) perror("sendto");
|
||||
}
|
||||
|
||||
void UdpSignaling::onMessage(std::function<void(const std::string&, const std::string&, int)> cb) {
|
||||
message_callback_ = cb;
|
||||
}
|
||||
|
||||
void UdpSignaling::setRemote(const std::string& remote_ip, int remote_port) {
|
||||
std::memset(&remote_addr_, 0, sizeof(remote_addr_));
|
||||
remote_addr_.sin_family = AF_INET;
|
||||
remote_addr_.sin_port = htons(remote_port);
|
||||
if (inet_pton(AF_INET, remote_ip.c_str(), &remote_addr_.sin_addr) != 1) {
|
||||
throw std::runtime_error("Invalid remote ip");
|
||||
}
|
||||
remote_set_ = true;
|
||||
}
|
||||
|
||||
void UdpSignaling::receiveLoop() {
|
||||
char buf[65536];
|
||||
while (running_) {
|
||||
sockaddr_in src{};
|
||||
socklen_t srclen = sizeof(src);
|
||||
ssize_t n = recvfrom(sockfd_, buf, sizeof(buf)-1, 0, (sockaddr*)&src, &srclen);
|
||||
if (n > 0) {
|
||||
buf[n] = '\0';
|
||||
char ipstr[INET_ADDRSTRLEN];
|
||||
inet_ntop(AF_INET, &src.sin_addr, ipstr, sizeof(ipstr));
|
||||
int port = ntohs(src.sin_port);
|
||||
|
||||
// if remote not set, learn it (server mode)
|
||||
if (!remote_set_) {
|
||||
remote_addr_ = src;
|
||||
remote_set_ = true;
|
||||
}
|
||||
|
||||
if (message_callback_) {
|
||||
message_callback_(std::string(buf, n), std::string(ipstr), port);
|
||||
}
|
||||
} else {
|
||||
// small sleep to avoid busy-loop on some errors
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
#pragma once
|
||||
#include <string>
|
||||
#include <functional>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <mutex>
|
||||
#include <netinet/in.h>
|
||||
|
||||
class UdpSignaling {
|
||||
public:
|
||||
// local_ip: nếu rỗng sẽ bind INADDR_ANY
|
||||
// remote_ip/remote_port có thể để rỗng/0 cho server (server sẽ learn từ packet đầu tiên)
|
||||
UdpSignaling(const std::string& local_ip, int local_port,
|
||||
const std::string& remote_ip = "", int remote_port = 0);
|
||||
|
||||
~UdpSignaling();
|
||||
|
||||
// start listener thread
|
||||
void start();
|
||||
|
||||
// stop listener
|
||||
void stop();
|
||||
|
||||
// send one datagram to remote (must have remote_set or call setRemote first)
|
||||
void send(const std::string& msg);
|
||||
|
||||
// set callback: (message, src_ip, src_port)
|
||||
void onMessage(std::function<void(const std::string&, const std::string&, int)> cb);
|
||||
|
||||
// set remote explicitly (client mode)
|
||||
void setRemote(const std::string& remote_ip, int remote_port);
|
||||
|
||||
private:
|
||||
void receiveLoop();
|
||||
|
||||
int sockfd_;
|
||||
sockaddr_in local_addr_;
|
||||
sockaddr_in remote_addr_;
|
||||
bool remote_set_ = false;
|
||||
|
||||
std::atomic<bool> running_{false};
|
||||
std::thread recv_thread_;
|
||||
std::mutex send_mutex_;
|
||||
|
||||
std::function<void(const std::string&, const std::string&, int)> message_callback_;
|
||||
};
|
||||
|
|
@ -17,6 +17,12 @@
|
|||
#include <sched.h>
|
||||
#include <sstream>
|
||||
#include <csignal>
|
||||
#include <linux/joystick.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
|
||||
|
||||
#include "udp_signaling.hpp"
|
||||
#include "rtc/rtc.hpp"
|
||||
|
|
@ -59,6 +65,22 @@ typedef struct {
|
|||
uint8_t checksum;
|
||||
} Frame_t;
|
||||
|
||||
static inline uint8_t axis_to_angle(int16_t axis)
|
||||
{
|
||||
// clamp
|
||||
if (axis > 32767) axis = 32767;
|
||||
if (axis < -32767) axis = -32767;
|
||||
|
||||
// map [-32767, 32767] -> [0, 120]
|
||||
int angle = (axis + 32767) * 120 / 65534;
|
||||
|
||||
if (angle < 0) angle = 0;
|
||||
if (angle > 120) angle = 120;
|
||||
|
||||
return static_cast<uint8_t>(angle);
|
||||
}
|
||||
|
||||
|
||||
// ---------- Checksum ----------
|
||||
uint8_t calc_checksum(const Frame_t& f) {
|
||||
uint8_t sum = f.sof ^ f.msg_id ^ f.cmd ^ f.length;
|
||||
|
|
@ -378,6 +400,71 @@ void rx_handler_thread_fn() {
|
|||
}
|
||||
}
|
||||
|
||||
void steering_joystick_thread_fn()
|
||||
{
|
||||
const char* jsdev = "/dev/input/js2";
|
||||
int fd = open(jsdev, O_RDONLY | O_NONBLOCK);
|
||||
if (fd < 0) {
|
||||
log_enqueue("[JOY] Cannot open /dev/input/js2");
|
||||
return;
|
||||
}
|
||||
|
||||
log_enqueue("[JOY] Steering joystick opened (/dev/input/js2)");
|
||||
|
||||
struct js_event e;
|
||||
int16_t last_axis = 0;
|
||||
uint8_t last_angle = 255; // invalid
|
||||
|
||||
const int DEADZONE = 800; // chống rung
|
||||
const int SEND_DELTA = 1; // chỉ gửi nếu đổi >= 1 độ
|
||||
|
||||
while (!stopping.load()) {
|
||||
ssize_t n = read(fd, &e, sizeof(e));
|
||||
if (n != sizeof(e)) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
||||
continue;
|
||||
}
|
||||
|
||||
e.type &= ~JS_EVENT_INIT;
|
||||
|
||||
if (e.type == JS_EVENT_AXIS && e.number == 0) {
|
||||
int16_t axis = e.value;
|
||||
|
||||
if (std::abs(axis) < DEADZONE)
|
||||
axis = 0;
|
||||
|
||||
if (axis == last_axis)
|
||||
continue;
|
||||
|
||||
last_axis = axis;
|
||||
|
||||
uint8_t angle = axis_to_angle(axis);
|
||||
|
||||
if (last_angle != 255 &&
|
||||
std::abs(int(angle) - int(last_angle)) < SEND_DELTA)
|
||||
continue;
|
||||
|
||||
last_angle = angle;
|
||||
|
||||
uint8_t payload[1] = { angle };
|
||||
|
||||
bool ok = send_cmd_with_ack(CMD_SET_STEERING, payload, 1);
|
||||
if (!ok) {
|
||||
log_enqueue("[JOY] Steering ACK failed");
|
||||
} else {
|
||||
char buf[64];
|
||||
snprintf(buf, sizeof(buf),
|
||||
"[JOY] axis=%d -> angle=%u",
|
||||
axis, angle);
|
||||
log_enqueue(buf);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
close(fd);
|
||||
log_enqueue("[JOY] Joystick thread exit");
|
||||
}
|
||||
|
||||
// Control loop: periodic, build status frames and/or commands and push to txRing
|
||||
void control_loop_thread_fn(int period_ms = 10) {
|
||||
auto next = std::chrono::steady_clock::now();
|
||||
|
|
@ -572,6 +659,8 @@ int main(int argc, char** argv) {
|
|||
std::thread control_loop_t([](){ control_loop_thread_fn(10); });
|
||||
std::thread tx_dispatcher_t(tx_dispatcher_thread_fn);
|
||||
std::thread keyboard_t(keyboard_input_thread_fn);
|
||||
std::thread joystick_t(steering_joystick_thread_fn);
|
||||
|
||||
|
||||
set_thread_realtime(control_loop_t, 80, 1);
|
||||
set_thread_realtime(tx_dispatcher_t, 70, 1);
|
||||
|
|
@ -597,6 +686,9 @@ int main(int argc, char** argv) {
|
|||
logCv.notify_all();
|
||||
logger_t.join();
|
||||
}
|
||||
if (joystick_t.joinable())
|
||||
joystick_t.join();
|
||||
|
||||
|
||||
try {
|
||||
if (g_dc) g_dc.reset();
|
||||
|
|
|
|||
Loading…
Reference in New Issue