Improve logging

.gitmodules

@@ -1,3 +1,6 @@
 [submodule "external/librobotcontrol"]
 	path = external/librobotcontrol
 	url = git@github.com:KPI-Rover/external_librobotcontrol.git
+[submodule "external/glog"]
+	path = external/glog
+	url = https://github.com/google/glog.git

CMakeLists.txt

@@ -4,6 +4,7 @@ project(kpi_rover_ecu)
 if(BUILD_VARIANT STREQUAL "Target")
     option(BUILD_LIBROBOT_CONTROL "Build librobotcontrol using original Makefile" ON)
     add_subdirectory(external/librobotcontrol)
+    add_subdirectory(external/glog)
     add_subdirectory(src/kpi_rover_ecu)
 endif()
 

README.md

@@ -16,6 +16,7 @@ cd ecu_sw_bb
 git submodule init
 git submodule update
 ```
+<b> Necessarily check if your glog version is v0.5.0 ! </b> 
 
 ### Build Docker Image
 ```
@@ -142,7 +143,7 @@ sudo apt install -y locales
 sudo dpkg-reconfigure locales # Choose a locale en_US.UTF-8 = English, United States, UTF8
 sudo apt upgrade -y
 sudo apt install build-essential -y
-sudo apt-get install -y cpufrequtils
+sudo apt-get install -y cpufrequtils libgoogle-glog-dev
 sudo sed -i 's/GOVERNOR="ondemand"/GOVERNOR="performance"/g' /etc/init.d/cpufrequtils
 ```
 

docker/Dockerfile.check

@@ -20,6 +20,7 @@ RUN apt-get update -q && \
     build-essential \
     make \
     git \
+    libgoogle-glog-dev \
     && rm -rf /var/lib/apt/lists/*
 
 ENV AS=/usr/bin/arm-linux-gnueabihf-as \

src/kpi_rover_ecu/CMakeLists.txt

@@ -20,6 +20,7 @@ target_link_libraries(${PROJECT_NAME} PRIVATE
     kpi_rover_ecu_core
     Threads::Threads
     robotcontrol
+    glog
 )
 
 target_include_directories(${PROJECT_NAME} PRIVATE

src/kpi_rover_ecu/include/PIDRegulator.h

@@ -8,7 +8,7 @@
 class PIDRegulator {
    public:
     void Init(std::array<float, 3> _coeficients);
-    int Run(float kError, float kTimeDt);
+    int Run(float actual, float setpoint, float dt);
 
    private:
     float kp_ = 0;

src/kpi_rover_ecu/include/loggingIncludes.h

@@ -0,0 +1,13 @@
+#ifndef LOGGINGINCLUDES_H
+#define LOGGINGINCLUDES_H
+
+#pragma once
+#include <glog/logging.h>
+
+#define LOG_DEBUG VLOG(1) << "[DEBUG] "
+#define LOG_INFO LOG(INFO) << "[INFO] "
+#define LOG_WARNING LOG(WARNING) << "[WARNING] "
+#define LOG_ERROR LOG(ERROR) << "[ERROR] "
+#define LOG_FATAL LOG(FATAL) << "[FATAL ERROR] "
+
+#endif

src/kpi_rover_ecu/src/CMakeLists.txt

@@ -15,4 +15,7 @@ add_library(kpi_rover_ecu_core STATIC
 target_include_directories(kpi_rover_ecu_core PUBLIC
     ${CMAKE_SOURCE_DIR}/src/kpi_rover_ecu/include
     ${CMAKE_SOURCE_DIR}/external/librobotcontrol/library/include
+    ${CMAKE_SOURCE_DIR}/external/glog/
 )
+
+target_link_libraries(kpi_rover_ecu_core PUBLIC glog)

src/kpi_rover_ecu/src/IMUController.cpp

@@ -7,6 +7,8 @@
 #include <iostream>
 #include <vector>
 
+#include "loggingIncludes.h"
+
 IMUController::IMUController() : data_{}, isStarted_(false), configuration_(rc_mpu_default_config()), actualData_({}) {
     configuration_.i2c_bus = kI2cBus;
     configuration_.gpio_interrupt_pin_chip = kGpioIntPinChip;
@@ -32,20 +34,23 @@ std::vector<float> IMUController::GetData() {
     if (!isStarted_) {
         return {};
     }
-
+    LOG_DEBUG << "Read data from accel and gyro";
     rc_mpu_read_accel(&data_);
     rc_mpu_read_gyro(&data_);
 
     actualData_.resize(kActualDataSize);
     const std::vector<float> kAccelData = GetAccel();
+    LOG_DEBUG << "Get data from accelerometr";
     const int kAccelDataSize = static_cast<int>(kAccelData.size());
     std::copy(kAccelData.begin(), kAccelData.begin() + kAccelDataSize, actualData_.begin());
 
     const std::vector<float> kGyroData = GetGyro();
+    LOG_DEBUG << "Get data from gyroscope";
     const int kGyroDataSize = static_cast<int>(kGyroData.size());
     std::copy(kGyroData.begin(), kGyroData.begin() + kGyroDataSize, actualData_.begin() + kAccelDataSize);
 
     const std::vector<float> kQaternionData = GetQaternion();
+    LOG_DEBUG << "Get data from magnetometer (qaternion)";
     const int kQaternionDataSize = static_cast<int>(kQaternionData.size());
     std::copy(kQaternionData.begin(), kQaternionData.begin() + kQaternionDataSize,
               actualData_.begin() + kAccelDataSize + kGyroDataSize);

src/kpi_rover_ecu/src/KPIRoverECU.cpp

@@ -13,6 +13,7 @@
 #include "IMUController.h"
 #include "TCPTransport.h"
 #include "UDPClient.h"
+#include "loggingIncludes.h"
 #include "protocolHandler.h"
 
 KPIRoverECU::KPIRoverECU(ProtocolHanlder *_protocolHandler, TCPTransport *_tcpTransport, UDPClient *_udpClient,
@@ -27,12 +28,14 @@ KPIRoverECU::KPIRoverECU(ProtocolHanlder *_protocolHandler, TCPTransport *_tcpTr
 
 bool KPIRoverECU::Start() {
     tcp_transport_->Start();
+    LOG_DEBUG << "Starting all thread in KPIRoverECU";
     timerThread_ = std::thread([this] { TimerThreadFuction(this->protocol_handler_); });
     processingThread_ = std::thread([this] { ProcessingThreadFunction(); });
     imuThread_ = std::thread([this] { IMUThreadFucntion(this->imu_controller_); });
+    LOG_DEBUG << "All thread in KPIRoverECU started";
 
     if (!timerThread_.joinable() || !processingThread_.joinable() || !imuThread_.joinable()) {
-        std::cout << "Error creating thread" << '\n';
+        LOG_ERROR << "Error creating thread";
         return false;
     }
 
@@ -47,9 +50,11 @@ void KPIRoverECU::IMUThreadFucntion(IMUController *workClass) {
 
     while (runningProcess_) {
         if (destination_address.empty()) {
+            LOG_DEBUG << "Udp server address is unknown, get address from tcp server clients";
             destination_address = tcp_transport_->GetClientIp();
             destination_port = tcp_transport_->GetClientPort();
             if (!destination_address.empty()) {
+                LOG_DEBUG << "Get UDP server address, initializing UDPClient";
                 udp_client_->Init(destination_address, destination_port);
             }
 
@@ -58,9 +63,11 @@ void KPIRoverECU::IMUThreadFucntion(IMUController *workClass) {
             if (!kImuData.empty()) {
                 if (packet_number == k16MaxCount) {
                     packet_number = 0;
+                    LOG_DEBUG << "set UDP packet number to 0";
                 }
                 packet_number += 1;
 
+                LOG_DEBUG << "build UDP packet " << packet_number << " for UDP server";
                 std::vector<uint8_t> send_val;
                 send_val.push_back(workClass->GetId());
 
@@ -83,6 +90,8 @@ void KPIRoverECU::IMUThreadFucntion(IMUController *workClass) {
                         send_val.push_back(bytes[j]);
                     }
                 }
+
+                LOG_DEBUG << "use UDP client to send message";
                 udp_client_->Send(send_val);
             }
         }
@@ -97,13 +106,15 @@ void KPIRoverECU::TimerThreadFuction(ProtocolHanlder *workClass) {
         if (counter_ > 0) {
             rc_usleep(kTimerPrecision);
             counter_--;
+            LOG_DEBUG << "counter decrement: " << counter_;
 
             if (counter_ == 0) {
-                std::cout << "[INFO] Motor set to stop" << '\n';
+                LOG_INFO << "Motor set to stop";
             }
 
         } else if (counter_ == 0) {
             // command to stop all motors
+            LOG_DEBUG << "Send command to stop all motors";
             workClass->HandleMessage(kStopVector);
             imu_controller_->SetDisable();
             rc_usleep(kTimeStop * kOneSecondMicro);
@@ -112,34 +123,42 @@ void KPIRoverECU::TimerThreadFuction(ProtocolHanlder *workClass) {
 }
 
 void KPIRoverECU::ProcessingThreadFunction() {
+    LOG_DEBUG << "start cycle in main thread";
     while (runningProcess_) {
         std::vector<uint8_t> message;
         if (tcp_transport_->Receive(message)) {
+            LOG_DEBUG << "TCP packet received, processing it";
             imu_controller_->SetEnable();
 
             counter_.store(GetCounter());
+            LOG_DEBUG << "set timer counter to GetCounter()";
             const std::vector<uint8_t> kReturnMessage = protocol_handler_->HandleMessage(message);
+            LOG_DEBUG << "get TCP response";
             tcp_transport_->Send(kReturnMessage);
+            LOG_DEBUG << "Send TCP response";
         }
     }
 }
 
 void KPIRoverECU::Stop() {
-    std::cout << "END of program" << '\n';
-    std::cout << "joining threads" << '\n';
+    LOG_INFO << "END of program";
+    LOG_INFO << "joining threads";
     runningState_ = true;
     if (timerThread_.joinable()) {
         timerThread_.join();
+        LOG_DEBUG << "timerThread_ joined";
     }
     runningProcess_ = false;
     if (processingThread_.joinable()) {
         processingThread_.join();
+        LOG_DEBUG << "main thread joined";
     }
     if (imuThread_.joinable()) {
         imuThread_.join();
+        LOG_DEBUG << "IMUThread joined";
     }
 
-    std::cout << "destroying drivers" << '\n';
+    LOG_INFO << "destroying drivers";
     // tcp_transport_->Destroy();
 }
 

src/kpi_rover_ecu/src/PIDRegulator.cpp

@@ -9,23 +9,24 @@ void PIDRegulator::Init(std::array<float, 3> _coeficients) {
     kd_ = _coeficients[2];
 }
 
-int PIDRegulator::Run(float kError, float kTimeDt) {
-    const float kPTerm = kp_ * kError;
+int PIDRegulator::Run(float actual, float setpoint, float dt) {
+    float error = setpoint - actual;
+    float pTerm = kp_ * error;
 
-    integral_ += kError * kTimeDt;
-    if (integral_ > integralLimit_) {
-        integral_ = integralLimit_;
-    }
-    if (integral_ < -integralLimit_) {
-        integral_ = -integralLimit_;
-    }
+    integral_ += error * dt;
+    if (integral_ > integralLimit_) integral_ = integralLimit_;
+    if (integral_ < -integralLimit_) integral_ = -integralLimit_;
 
-    const float kITerm = ki_ * integral_;
+    float iTerm = ki_ * integral_;
+    float dTerm = kd_ * ((dt > 0.0F) ? (error - previousError_) / dt : 0.0F);
 
-    const float kDerivativePart = (kTimeDt > 0.0F) ? (kError - previousError_) / kTimeDt : 0.0F;
-    const float kDTerm = kd_ * kDerivativePart;
+    previousError_ = error;
 
-    previousError_ = kError;
+    float output = pTerm + iTerm + dTerm;
 
-    return static_cast<int>(kPTerm + kITerm + kDTerm);
+    // Optionally clamp output to max/min here if needed
+    // if (output > maxOutput_) output = maxOutput_;
+    // if (output < minOutput_) output = minOutput_;
+
+    return static_cast<int>(output);
 }

src/kpi_rover_ecu/src/TCPTransport.cpp

@@ -13,6 +13,8 @@
 #include <string>
 #include <vector>
 
+#include "loggingIncludes.h"
+
 constexpr std::size_t kBufferSize = 1024;
 constexpr int kTimeoutMs = 50;
 
@@ -29,9 +31,9 @@ TCPTransport::TCPTransport(const char *ip_address, int port)
 TCPTransport::~TCPTransport() {
     running_ = false;
     shutdown(sockfd_, SHUT_RDWR);
-    std::cout << "joining thread ... " << '\n';
+    LOG_INFO << "joining thread ... ";
     acceptThread_.join();
-    std::cout << "closing socket" << '\n';
+    LOG_INFO << "closing socket";
     close(client_sockfd_);
     close(sockfd_);
     delete[] server_address_;
@@ -45,31 +47,35 @@ int TCPTransport::Init() {
     sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
 
     if (sockfd_ < 0) {
-        perror("[ERROR] Socket()");
+        LOG_ERROR << "Socket()";
         return -1;
     }
+    LOG_DEBUG << "Created socket";
 
     if (setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, &reuseaddr, sizeof(int)) != 0) {
-        perror("[ERROR] setsockopt()");
+        LOG_ERROR << "setsockopt()";
         return -1;
     }
 
     memset(&serv_addr, 0, sizeof(serv_addr));
     serv_addr.sin_family = AF_INET;
     serv_addr.sin_addr.s_addr = inet_addr(server_address_);
     serv_addr.sin_port = htons(server_portnum_);
+    LOG_DEBUG << "Set socket parameters";
 
     if (bind(sockfd_, reinterpret_cast<struct sockaddr *>(&serv_addr), sizeof(serv_addr)) != 0) {
-        perror("[ERROR] bind()");
+        LOG_ERROR << "bind()";
         return -1;
     }
+    LOG_DEBUG << "Socket binded";
 
     if (listen(sockfd_, TCPTransport::kNumSlots) != 0) {
-        perror("[ERROR] listen");
+        LOG_ERROR << "listen()";
         return -1;
     }
+    LOG_DEBUG << "Start listening";
 
-    std::cout << "[INFO] Started server on " << server_address_ << ":" << server_portnum_ << '\n';
+    LOG_INFO << " Started server on " << server_address_ << ":" << server_portnum_;
 
     return sockfd_;
 }
@@ -80,19 +86,22 @@ void TCPTransport::Start() {
         socklen_t client_add_size = sizeof(client_addr);
 
         while (running_) {
-            std::cout << "Waiting for connection..." << '\n';
+            LOG_INFO << "Waiting for connection...";
             client_sockfd_ = accept(sockfd_, reinterpret_cast<sockaddr *>(&client_addr), &client_add_size);
             if (client_sockfd_ >= 0) {
                 inet_ntop(AF_INET, &client_addr.sin_addr, source_address_, INET_ADDRSTRLEN);
                 source_port_ = static_cast<int>(ntohs(client_addr.sin_port));
+                LOG_INFO << "Client connected, source address: " << std::string(source_address_)
+                         << " source port: " << source_port_;
                 while (true) {  // Use true instead of 1
                     std::uint8_t buffer[kBufferSize];
                     const ssize_t kBytesReceived = recv(client_sockfd_, buffer, sizeof(buffer), 0);
                     if (kBytesReceived > 0) {
                         const std::vector<std::uint8_t> kMessage(buffer, buffer + kBytesReceived);
                         messageQueue_.Push(kMessage);
+                        LOG_DEBUG << "Get message, put in MessageQueue";
                     } else {
-                        std::cout << "Client disconnected." << '\n';
+                        LOG_INFO << "Client disconnected.";
                         close(client_sockfd_);
                         client_sockfd_ = -1;
                         break;
@@ -107,6 +116,8 @@ bool TCPTransport::Send(const std::vector<std::uint8_t> &data) {
     if (client_sockfd_ < 0) {
         return false;
     }
+    LOG_DEBUG << "Send TCP packet to TCP client";
+
     return ::send(client_sockfd_, data.data(), data.size(), 0) != -1;
 }
 
@@ -119,9 +130,9 @@ int TCPTransport::GetClientPort() { return source_port_; }
 void TCPTransport::Destroy() {
     running_ = false;
     shutdown(sockfd_, SHUT_RDWR);
-    std::cout << "joining thread ... " << '\n';
+    LOG_INFO << "joining thread ... ";
     acceptThread_.join();
-    std::cout << "closing socket" << '\n';
+    LOG_INFO << "closing socket";
     close(client_sockfd_);
     close(sockfd_);
     delete[] server_address_;