initial commit

2026-10-22 09:23:54 +08:00
commit 63f557a39f
4448 changed files with 584487 additions and 0 deletions
@@ -0,0 +1,104 @@
+#include "gamepad.h"
+#include "udp_receiver.h"
+#include <iostream>
+#include <thread>
+#include <chrono>
+
+/**
+ * @brief Constructor for the Gamepad class.
+ *
+ * Initializes the gamepad with the specified UDP port for data reception.
+ *
+ * @param port The UDP port to use for receiving gamepad data.
+ */
+template <typename KeysType>
+Gamepad<KeysType>::Gamepad(int port) : stop_thread_(false), port_(port) {
+  clock_gettime(CLOCK_MONOTONIC,&start_time_);
+}
+
+/**
+ * @brief Starts the data thread for continuous data reception.
+ */
+template <typename KeysType>
+void Gamepad<KeysType>::StartDataThread() {
+  data_thread_ = std::thread([this]() {
+    uint32_t updateCount = 0;
+    UdpReceiver udpReceiver(port_);
+
+    // Loop to continuously receive data
+    while (!stop_thread_) {
+      std::vector<uint8_t> receivedData = udpReceiver.ReceiveData();
+      bool ret = false;
+      if(true){
+        std::lock_guard<std::mutex> lock(mutex_);
+        ret = UpdateData(receivedData, keys_);
+      }
+      // If data is valid and updated, increase the update count
+      if (ret) {
+        updateCount++;
+
+        // If there is an update callback function, call the callback and pass the update count
+        if (updateCallback_) {
+          updateCallback_(updateCount);
+        }
+      }
+    }
+    // std::this_thread::sleep_for(std::chrono::milliseconds(25));
+  });
+  data_thread_.detach();
+}
+
+/**
+ * @brief Stops the data thread.
+ */
+template <typename KeysType>
+void Gamepad<KeysType>::StopDataThread() {
+  stop_thread_ = true;
+
+  // // Detach the thread after it has been stopped
+  // data_thread_.detach();
+}
+
+/**
+ * @brief Gets the current keys data.
+ *
+ * @return Reference to the keys data.
+ */
+template <typename KeysType>
+KeysType& Gamepad<KeysType>::GetKeys() {
+  std::lock_guard<std::mutex> lock(mutex_);
+  return keys_;
+}
+
+/**
+ * @brief Calculates the CRC-16 checksum for a given data array.
+ *
+ * @param data Pointer to the data array.
+ * @param length Length of the data array.
+ * @return The calculated CRC-16 checksum.
+ */
+template <typename KeysType>
+uint16_t Gamepad<KeysType>::CalculateCrc16(const uint8_t* data, size_t length) {
+  uint16_t crc = 0x00;
+
+  // Calculate the CRC-16 checksum
+  for (int i = 0; i < length; i++) {
+    crc += data[i];
+  }
+
+  return crc;
+}
+
+/**
+ * @brief Sets a callback function to be called on data update.
+ *
+ * @param callback The callback function taking an update count as a parameter.
+ */
+template <typename KeysType>
+void Gamepad<KeysType>::SetUpdateCallback(const std::function<void(uint32_t)>& callback) {
+  updateCallback_ = callback;
+}
+
+// Explicitly instantiate the template class for specific gamepad types
+template class Gamepad<RetroidKeys>;
+template class Gamepad<SkydroidKeys>;
@@ -0,0 +1,88 @@
+#include "retroid_gamepad.h"
+#include <cstdlib>
+#include <stdio.h>
+#include <string.h>
+#include <bitset>
+
+
+/**
+ * @brief Constructor for the RetroidGamepad class.
+ *
+ * Initializes the Retroid gamepad with the specified UDP port for data reception.
+ *
+ * @param port The UDP port to use for receiving gamepad data.
+ */
+RetroidGamepad::RetroidGamepad(int port) : Gamepad(port) {
+  button_status_.clear();
+  memset(&keys_, 0, sizeof(keys_));
+}
+
+/**
+ * @brief Updates the RETROID gamepad data buffer and keys.
+ *
+ * @param buffer The data buffer to be updated.
+ * @param keys The RETROID gamepad keys to be updated.
+ * @return True if the data is valid and updated, false otherwise.
+ */
+bool RetroidGamepad::UpdateData(std::vector<uint8_t>& buffer, RetroidKeys& keys) {
+  RetroidGamepadData data;
+  timespec timestamp;
+  memcpy(&data, buffer.data(), buffer.size()*sizeof(uint8_t));
+  // Perform data validity check in the child class
+  if (DataIsValid(data)) {
+    std::bitset<kChannlSize> value_bit(0);
+    int16_t ch[kChannlSize];
+    memcpy(ch, data.data, sizeof(ch));
+    for(int i = 0; i < kChannlSize; i++){
+      value_bit[i] = ch[i];
+    }
+    clock_gettime(CLOCK_MONOTONIC,&timestamp);
+    keys.time_stamp = (timestamp.tv_sec-start_time_.tv_sec)*1.e3 + double(timestamp.tv_nsec - start_time_.tv_nsec)/1.e6;
+    keys.value = value_bit.to_ulong();
+
+    keys.left  = (data.left_axis_x == -kJoystickRange) ? (uint8_t)KeyStatus::kPressed 
+                  : ((data.left_axis_x ==  kJoystickRange) ? (uint8_t)KeyStatus::kReleased : (uint8_t)KeyStatus::kReleased);
+    keys.right = (data.left_axis_x ==  kJoystickRange) ? (uint8_t)KeyStatus::kPressed 
+                  : ((data.left_axis_x == -kJoystickRange) ? (uint8_t)KeyStatus::kReleased : (uint8_t)KeyStatus::kReleased);
+    keys.up    = (data.left_axis_y ==  kJoystickRange) ? (uint8_t)KeyStatus::kPressed 
+                  : ((data.left_axis_y == -kJoystickRange) ? (uint8_t)KeyStatus::kReleased : (uint8_t)KeyStatus::kReleased);
+    keys.down  = (data.left_axis_y == -kJoystickRange) ? (uint8_t)KeyStatus::kPressed 
+                  : ((data.left_axis_y ==  kJoystickRange) ? (uint8_t)KeyStatus::kReleased : (uint8_t)KeyStatus::kReleased);
+
+    keys.left_axis_x = data.left_axis_x/(float)kJoystickRange;
+    keys.left_axis_y = data.left_axis_y/(float)kJoystickRange;
+    keys.right_axis_x = data.right_axis_x/(float)kJoystickRange;
+    keys.right_axis_y = data.right_axis_y/(float)kJoystickRange;
+
+    return true;
+  }
+  return false;
+}
+
+/**
+ * @brief Checks if the received data from the RETROID gamepad is valid.
+ *
+ * @param data The gamepad data to be validated.
+ * @return True if the data is valid, false otherwise.
+ */
+bool RetroidGamepad::DataIsValid(const RetroidGamepadData& data) {
+
+  // 1. check STX   
+  if (data.stx[0] != kHeader[0] || data.stx[1] != kHeader[1]) {
+    return false;
+  }
+
+  // 2. check device ID
+  if (data.id != (uint8_t)GamepadType::kRetroid) {
+    return false;
+  }
+  
+  // 3. CRC16
+  if(data.crc16 != CalculateCrc16(data.data, data.data_len)){
+    return false;
+  }
+
+  return true;  
+}
+
+
@@ -0,0 +1,83 @@
+#include "skydroid_gamepad.h"
+#include <cstdlib>
+#include <stdio.h>
+#include <string.h>
+#include <bitset>
+
+
+/**
+ * @brief Constructor for the SkydroidGamepad class.
+ *
+ * Initializes the Skydroid controller with the specified UDP port for data reception.
+ *
+ * @param port The UDP port to use for receiving controller data.
+ */
+SkydroidGamepad::SkydroidGamepad(int port) : Gamepad(port) {
+  button_status_.clear();
+  memset(&keys_, 0, sizeof(keys_));
+}
+
+/**
+ * @brief Updates the Skydroid controller data buffer and keys.
+ *
+ * @param buffer The data buffer to be updated.
+ * @param keys The Skydroid controller keys to be updated.
+ * @return True if the data is valid and updated, false otherwise.
+ */
+bool SkydroidGamepad::UpdateData(std::vector<uint8_t>& buffer, SkydroidKeys& keys) {
+  SkydroidGamepadData data;
+  timespec timestamp;
+  memcpy(&data, buffer.data(), buffer.size()*sizeof(uint8_t));
+  // Perform data validity check in the child class
+  if (DataIsValid(data)) {
+    std::bitset<kSkydroidButtonSize> keys_value_bit(0);
+    int16_t keys_ch[kSkydroidButtonSize];
+    memcpy(keys_ch, data.buttons, sizeof(keys_ch));
+    for(int i = 0; i < kSkydroidButtonSize; i++){
+      keys_value_bit[i] = keys_ch[i];
+    }
+    clock_gettime(CLOCK_MONOTONIC,&timestamp);
+    keys.time_stamp = (timestamp.tv_sec-start_time_.tv_sec)*1.e3 + double(timestamp.tv_nsec - start_time_.tv_nsec)/1.e6;
+    keys.keys_value = keys_value_bit.to_ulong();
+    keys.left_axis_x = data.left_axis_x/(float)kJoystickRange;
+    keys.left_axis_y = data.left_axis_y/(float)kJoystickRange;
+    keys.right_axis_x = data.right_axis_x/(float)kJoystickRange;
+    keys.right_axis_y = data.right_axis_y/(float)kJoystickRange;
+
+    keys.sw1 = data.switch_keys[0];
+    keys.sw2 = data.switch_keys[1];
+    keys.sw3 = data.switch_keys[2];
+    keys.sw4 = data.switch_keys[3];
+
+    return true;
+  }
+  return false;
+}
+
+/**
+ * @brief Checks if the received data from the Skydroid controller is valid.
+ *
+ * @param data The controller data to be validated.
+ * @return True if the data is valid, false otherwise.
+ */
+bool SkydroidGamepad::DataIsValid(const SkydroidGamepadData& data) {
+
+  // 1. check STX   
+  if (data.stx[0] != kHeader[0] || data.stx[1] != kHeader[1]) {
+    return false;
+  }
+
+  // 2. check device ID
+  if (data.id != (uint8_t)GamepadType::kSkydroid) {
+    return false;
+  }
+  
+  // 3. CRC16
+  if(data.crc16 != CalculateCrc16(data.data, data.data_len)){
+    return false;
+  }
+
+  return true;  
+}
+
+
@@ -0,0 +1,60 @@
+#include "udp_receiver.h"
+#include <iostream>
+#include <cstring>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+
+/**
+ * @brief Constructor for UdpReceiver.
+ * @param port The port to listen for UDP data.
+ */
+UdpReceiver::UdpReceiver(int port) : socket_fd_(-1) {
+    socket_fd_ = socket(AF_INET, SOCK_DGRAM, 0);
+    if (socket_fd_ == -1) {
+        std::cerr << "Failed to create socket." << std::endl;
+        return;
+    }
+
+    sockaddr_in serverAddress;
+    serverAddress.sin_family = AF_INET;
+    serverAddress.sin_addr.s_addr = INADDR_ANY;
+    serverAddress.sin_port = htons(port);
+
+    if (bind(socket_fd_, (struct sockaddr*)&serverAddress, sizeof(serverAddress)) < 0) {
+        std::cerr << "Failed to bind socket." << std::endl;
+        close(socket_fd_);
+        return;
+    }
+}
+
+/**
+ * @brief Receive UDP data.
+ * @return A vector containing received data as uint8_t.
+ */
+std::vector<uint8_t> UdpReceiver::ReceiveData() {
+    std::vector<uint8_t> buffer(1024);
+    sockaddr_in clientAddress;
+    socklen_t clientAddressLen = sizeof(clientAddress);
+
+    int bytesRead = recvfrom(socket_fd_, buffer.data(), buffer.size(), 0, (struct sockaddr*)&clientAddress, &clientAddressLen);
+    if (bytesRead == -1) {
+        std::cerr << "Failed to receive data." << std::endl;
+        return std::vector<uint8_t>();
+    }
+
+    buffer.resize(bytesRead);
+
+    return buffer;
+}
+
+/**
+ * @brief Destructor for UdpReceiver.
+ */
+UdpReceiver::~UdpReceiver() {
+    if (socket_fd_ != -1) {
+        close(socket_fd_);
+    }
+}