Issue #22. Implement reading and sending to rpi imu data

src/kpi_rover_ecu/include/IClient.h

@@ -0,0 +1,25 @@
+#ifndef ICLIENT_H
+#define ICLIENT_H
+
+#include <cstdint>
+#include <string>
+#include <vector>
+
+class IClient {
+   protected:
+    IClient() = default;
+
+   public:
+    virtual bool Send(const std::vector<std::uint8_t>& data) = 0;
+    virtual bool Receive(std::vector<std::uint8_t>& data) = 0;
+    virtual int Init(std::string ip_address, int port) = 0;
+    virtual void Destroy() = 0;
+    virtual ~IClient() = default;
+
+    IClient(const IClient&) = delete;
+    IClient& operator=(const IClient&) = delete;
+    IClient(IClient&&) = delete;
+    IClient& operator=(IClient&&) = delete;
+};
+
+#endif

src/kpi_rover_ecu/include/IMUController.h

@@ -0,0 +1,41 @@
+#ifndef IMUCONTROLLER_H
+#define IMUCONTROLLER_H
+
+#include <rc/mpu.h>
+
+#include <atomic>
+#include <cstddef>
+#include <cstdint>
+#include <vector>
+
+constexpr size_t kActualDataSize = 10;
+
+class IMUController {
+   public:
+    IMUController();
+    int Init();
+    void SetEnable();
+    void SetDisable();
+    void Stop();
+    std::vector<float> GetData();
+    uint8_t GetId();
+
+   private:
+    const int kIdGetCommand = 0x06;
+    const int kI2cBus = 2;
+    const int kGpioIntPinChip = 3;
+    const int kGpioIntPinPin = 21;
+    const int kDmpSampleRate = 100;
+    const int kEnableMagnetometer = 1;
+
+    std::atomic<bool> isStarted_;
+    rc_mpu_config_t configuration_;
+    std::vector<float> actualData_;
+    rc_mpu_data_t data_;
+
+    std::vector<float> GetAccel();
+    std::vector<float> GetGyro();
+    std::vector<float> GetQaternion();
+};
+
+#endif

src/kpi_rover_ecu/include/KPIRoverECU.h

@@ -5,29 +5,37 @@
 #include <cstdint>
 #include <thread>
 
+#include "IMUController.h"
 #include "TCPTransport.h"
+#include "UDPClient.h"
 #include "protocolHandler.h"
 
 // Constants for timing control
 static constexpr std::uint32_t kTimerPrecision = 100000;   // 100 milliseconds in microsecond (for timer)
 static constexpr std::uint32_t kOneSecondMicro = 1000000;  // 1 s in microseconds
 static constexpr std::uint32_t kOneSecondMilli = 1000;     // 1 s in milliseconds
-static constexpr std::uint32_t kTimeStop = 5;              // 5 seconds
+static constexpr std::uint32_t kTimeStop = 1;              // 5 seconds
+static constexpr std::uint32_t k16MaxCount = 65535;
 
 class KPIRoverECU {
    public:
     void TimerThreadFuction(ProtocolHanlder *workClass);
     void ProcessingThreadFunction();
+    void IMUThreadFucntion(IMUController *workClass);
 
-    KPIRoverECU(ProtocolHanlder *_protocolHandler, TCPTransport *_tcpTransport);
+    KPIRoverECU(ProtocolHanlder *_protocolHandler, TCPTransport *_tcpTransport, UDPClient *_udpClient,
+                IMUController *_imuController);
     bool Start();
     void Stop();
 
    private:
     ProtocolHanlder *protocol_handler_;
     TCPTransport *tcp_transport_;
+    IMUController *imu_controller_;
+    UDPClient *udp_client_;
     std::thread timerThread_;
     std::thread processingThread_;
+    std::thread imuThread_;
     std::atomic<bool> runningProcess_;
     std::atomic<bool> runningState_;
     std::atomic<int> counter_;

src/kpi_rover_ecu/include/TCPTransport.h

@@ -1,6 +1,8 @@
 #ifndef TCPTRANSPORT_H
 #define TCPTRANSPORT_H
 
+#include <arpa/inet.h>
+
 #include <atomic>
 #include <cstdint>
 #include <cstring>
@@ -28,9 +30,14 @@ class TCPTransport : public ITransport {
     int Init() override;
     void Destroy() override;
 
+    std::string GetSourceIp();
+    int GetSourcePort();
+
    private:
     int sockfd_, client_sockfd_;
     char* server_address_;
+    char source_address_[INET_ADDRSTRLEN];
+    int source_port_;
     int server_portnum_;
     std::atomic<bool> running_;
     std::thread acceptThread_;

src/kpi_rover_ecu/include/UDPClient.h

@@ -0,0 +1,35 @@
+#ifndef UDPCLIENT_H
+#define UDPCLIENT_H
+
+#include <arpa/inet.h>
+#include <netinet/in.h>
+
+#include <cstdint>
+#include <string>
+#include <vector>
+
+#include "IClient.h"
+
+class UDPClient : public IClient {
+   public:
+    UDPClient();
+
+    UDPClient(const UDPClient&) = delete;
+    UDPClient& operator=(const UDPClient&) = delete;
+    UDPClient(UDPClient&&) = delete;
+    UDPClient& operator=(UDPClient&&) = delete;
+
+    int Init(std::string ip_address, int port) override;
+    bool Send(const std::vector<std::uint8_t>& data) override;
+    bool Receive(std::vector<std::uint8_t>& data) override;
+    void Destroy() override;
+    ~UDPClient() override;
+
+   private:
+    int sockfd_;
+    sockaddr_in serverStruct_;
+    char server_address_[INET_ADDRSTRLEN];
+    int server_portnum_;
+};
+
+#endif

src/kpi_rover_ecu/src/CMakeLists.txt

@@ -7,6 +7,8 @@ add_library(kpi_rover_ecu_core STATIC
     motorConfig.cpp
     messageQueue.cpp
     PIDRegulator.cpp
+    IMUController.cpp
+    UDPClient.cpp
 )
 
 # Include public headers

src/kpi_rover_ecu/src/IMUController.cpp

@@ -0,0 +1,89 @@
+#include "IMUController.h"
+
+#include <rc/mpu.h>
+
+#include <algorithm>
+#include <cstdint>
+#include <iostream>
+#include <vector>
+
+IMUController::IMUController() : data_{}, isStarted_(false), configuration_(rc_mpu_default_config()), actualData_({}) {
+    configuration_.i2c_bus = kI2cBus;
+    configuration_.gpio_interrupt_pin_chip = kGpioIntPinChip;
+    configuration_.gpio_interrupt_pin = kGpioIntPinPin;
+    configuration_.dmp_sample_rate = kDmpSampleRate;
+    configuration_.enable_magnetometer = kEnableMagnetometer;
+}
+
+int IMUController::Init() {
+    if (rc_mpu_initialize_dmp(&data_, configuration_) != 0) {
+        std::cout << "Initializing MPU failed" << '\n';
+        return -1;
+    }
+
+    return 0;
+}
+
+void IMUController::SetEnable() { isStarted_ = true; }
+
+void IMUController::SetDisable() { isStarted_ = false; }
+
+std::vector<float> IMUController::GetData() {
+    if (!isStarted_) {
+        return {};
+    }
+
+    rc_mpu_read_accel(&data_);
+    rc_mpu_read_gyro(&data_);
+
+    actualData_.resize(kActualDataSize);
+    const std::vector<float> kAccelData = GetAccel();
+    const int kAccelDataSize = static_cast<int>(kAccelData.size());
+    std::copy(kAccelData.begin(), kAccelData.begin() + kAccelDataSize, actualData_.begin());
+
+    const std::vector<float> kGyroData = GetGyro();
+    const int kGyroDataSize = static_cast<int>(kGyroData.size());
+    std::copy(kGyroData.begin(), kGyroData.begin() + kGyroDataSize, actualData_.begin() + kAccelDataSize);
+
+    const std::vector<float> kQaternionData = GetQaternion();
+    const int kQaternionDataSize = static_cast<int>(kQaternionData.size());
+    std::copy(kQaternionData.begin(), kQaternionData.begin() + kQaternionDataSize,
+              actualData_.begin() + kAccelDataSize + kGyroDataSize);
+
+    return actualData_;
+}
+
+std::vector<float> IMUController::GetAccel() {
+    std::vector<float> ret_val;
+
+    ret_val.push_back(static_cast<float>(data_.accel[0]));
+    ret_val.push_back(static_cast<float>(data_.accel[1]));
+    ret_val.push_back(static_cast<float>(data_.accel[2]));
+
+    return ret_val;
+}
+
+std::vector<float> IMUController::GetGyro() {
+    std::vector<float> ret_val;
+
+    ret_val.push_back(static_cast<float>(data_.gyro[0]));
+    ret_val.push_back(static_cast<float>(data_.gyro[1]));
+    ret_val.push_back(static_cast<float>(data_.gyro[2]));
+
+    return ret_val;
+}
+
+std::vector<float> IMUController::GetQaternion() {
+    std::vector<float> ret_val;
+
+    ret_val.push_back(static_cast<float>(data_.dmp_quat[QUAT_W]));
+    ret_val.push_back(static_cast<float>(data_.dmp_quat[QUAT_X]));
+    ret_val.push_back(static_cast<float>(data_.dmp_quat[QUAT_Y]));
+    ret_val.push_back(static_cast<float>(data_.dmp_quat[QUAT_Z]));
+
+    return ret_val;
+}
+
+void IMUController::Stop() { rc_mpu_power_off(); }
+
+uint8_t IMUController::GetId() { return kIdGetCommand; }

src/kpi_rover_ecu/src/KPIRoverECU.cpp

@@ -1,19 +1,24 @@
 #include "KPIRoverECU.h"
 
+#include <arpa/inet.h>
 #include <rc/time.h>
 
 #include <csignal>
 #include <cstdint>
-#include <cstring>
 #include <iostream>
+#include <string>
 #include <vector>
 
 #include "TCPTransport.h"
+#include "UDPClient.h"
 #include "protocolHandler.h"
 
-KPIRoverECU::KPIRoverECU(ProtocolHanlder* _protocolHandler, TCPTransport* _tcpTransport)
+KPIRoverECU::KPIRoverECU(ProtocolHanlder *_protocolHandler, TCPTransport *_tcpTransport, UDPClient *_udpClient,
+                         IMUController *_imuController)
     : protocol_handler_(_protocolHandler),
       tcp_transport_(_tcpTransport),
+      imu_controller_(_imuController),
+      udp_client_(_udpClient),
       counter_(GetCounter()),
       runningProcess_(true),
       runningState_(false) {}
@@ -22,6 +27,7 @@ bool KPIRoverECU::Start() {
     tcp_transport_->Start();
     timerThread_ = std::thread([this] { TimerThreadFuction(this->protocol_handler_); });
     processingThread_ = std::thread([this] { ProcessingThreadFunction(); });
+    imuThread_ = std::thread([this] { IMUThreadFucntion(this->imu_controller_); });
 
     if (!timerThread_.joinable() || !processingThread_.joinable()) {
         std::cout << "Error creating thread" << '\n';
@@ -31,7 +37,57 @@ bool KPIRoverECU::Start() {
     return true;
 }
 
-void KPIRoverECU::TimerThreadFuction(ProtocolHanlder* workClass) {
+void KPIRoverECU::IMUThreadFucntion(IMUController *workClass) {
+    uint16_t packet_number = 0;
+
+    std::string destination_address;
+    int destination_port = 0;
+
+    while (destination_address.empty()) {
+        destination_address = tcp_transport_->GetSourceIp();
+        destination_port = tcp_transport_->GetSourcePort();
+    }
+
+    udp_client_->Init(destination_address, destination_port);
+
+    while (runningProcess_) {
+        const std::vector<float> kImuData = workClass->GetData();
+        if (!kImuData.empty()) {
+            if (packet_number == k16MaxCount) {
+                packet_number = 0;
+            }
+            packet_number += 1;
+
+            std::vector<uint8_t> send_val;
+            send_val.push_back(workClass->GetId());
+
+            uint16_t send_packet_number = htons(packet_number);
+            auto *bytes = reinterpret_cast<uint8_t *>(&send_packet_number);
+            for (size_t i = 0; i < 2; ++i) {
+                send_val.push_back(bytes[i]);
+            }
+
+            float insert_value = 0;
+            uint32_t value = 0;
+
+            for (const float kImuValue : kImuData) {
+                insert_value = kImuValue;
+                std::memcpy(&value, &insert_value, sizeof(float));
+                value = ntohl(value);
+                bytes = reinterpret_cast<uint8_t *>(&value);
+
+                for (size_t j = 0; j < sizeof(uint32_t); ++j) {
+                    send_val.push_back(bytes[j]);
+                }
+            }
+
+            udp_client_->Send(send_val);
+        }
+        rc_usleep(kTimerPrecision);
+    }
+}
+
+void KPIRoverECU::TimerThreadFuction(ProtocolHanlder *workClass) {
     const std::vector<uint8_t> kStopVector = workClass->MotorsStopMessage();
 
     while (!runningState_) {
@@ -46,7 +102,7 @@ void KPIRoverECU::TimerThreadFuction(ProtocolHanlder* workClass) {
         } else if (counter_ == 0) {
             // command to stop all motors
             workClass->HandleMessage(kStopVector);
-
+            imu_controller_->SetDisable();
             rc_usleep(kTimeStop * kOneSecondMicro);
         }
     }
@@ -56,6 +112,8 @@ void KPIRoverECU::ProcessingThreadFunction() {
     while (runningProcess_) {
         std::vector<uint8_t> message;
         if (tcp_transport_->Receive(message)) {
+            imu_controller_->SetEnable();
+
             counter_.store(GetCounter());
             const std::vector<uint8_t> kReturnMessage = protocol_handler_->HandleMessage(message);
             tcp_transport_->Send(kReturnMessage);
@@ -75,7 +133,7 @@ void KPIRoverECU::Stop() {
         processingThread_.join();
     }
     std::cout << "destroying drivers" << '\n';
-    tcp_transport_->Destroy();
+    // tcp_transport_->Destroy();
 }
 
 int KPIRoverECU::GetCounter() { return (kTimeStop * kOneSecondMicro) / kTimerPrecision; }