Makr add timeout support for gps service

.gitignore

@@ -1,4 +1,5 @@
 bazel-*
 .vscode
 MODULE.bazel.lock
-coverage-report
+coverage-report
+

.gitmodules

@@ -7,6 +7,3 @@
 [submodule "third_party/python/someipy"]
 	path = third_party/python/someipy
 	url = git@github.com:Simba-Avionic/someipy.git
-[submodule "libdoip"]
-	path = libdoip
-	url = https://github.com/Simba-Avionic/libdoip

apps/fc/gps_service/gps_app.cpp

@@ -20,6 +20,8 @@ namespace {
   constexpr auto kService_udp_instance = "srp/apps/GPSApp/GPSService_udp";
   constexpr auto KGPS_UART_path = "/dev/ttyS1";
   constexpr auto KGPS_UART_baudrate = B230400;
+  constexpr uint16_t KGps_expected_interval = 1000;
+  constexpr auto kGps_freq_tolerance = 100;
 }
 
 GPSDataStructure GPSApp::GetSomeIPData(const core::GPS_DATA_T& data) {
@@ -44,21 +46,40 @@ std::optional<GPSDataStructure> GPSApp::ParseGPSData(const std::vector<uint8_t>&
     return std::nullopt;
   }
   auto someip_data = GetSomeIPData(res.value());
-  // TODO(matikrajek42@gmail.com)  uncoment afer basn advice
-  // ara::log::LogDebug() << "GPS latitude: " << someip_data.latitude
-  //     << ", longtitude: " << someip_data.longitude << ",height(M):"
-  //     << res.value().height << "satelite_nr: " << res.value().satellite_nr;
+  ara::log::LogDebug() << "GPS latitude: " << someip_data.latitude
+      << ", longtitude: " << someip_data.longitude << ",height(M):"
+      << res.value().height << "satelite_nr: " << res.value().satellite_nr;
   return someip_data;
 }
 
+int64_t GPSApp::GetTimeDelata() const {
+  auto now = std::chrono::high_resolution_clock::now();
+  return std::chrono::duration_cast<std::chrono::milliseconds>(now - last_frame).count();
+}
+
 int GPSApp::Run(const std::stop_token& token) {
+  uint32_t warn_num = 0;
   while (!token.stop_requested()) {
+    if (GetTimeDelata() > KGps_expected_interval + kGps_freq_tolerance) {
+      if (warn_num < 1) {
+        ara::log::LogWarn() << "Missing GPS frame";
+      }
+      warn_num +=1;
+    }
+
     auto data = uart_->Read();
     if (!data.has_value()) {
       continue;
     }
     auto res = ParseGPSData(data.value());
     if (res.has_value()) {
+      auto delta = GetTimeDelata();
+      if (std::abs(delta - KGps_expected_interval) > kGps_freq_tolerance) {
+        ara::log::LogWarn() << "GPS frequency deviation detected: interval = " << std::to_string(delta) << " ms";
+      }
+      ara::log::LogDebug() << "GPS frequency deviation detected: interval = " << std::to_string(delta) << " ms";
+      warn_num = 0;
+      last_frame = std::chrono::high_resolution_clock::now();
       service_ipc->GPSStatusEvent.Update(res.value());
       service_udp->GPSStatusEvent.Update(res.value());
     }
@@ -76,14 +97,15 @@ int GPSApp::Initialize(const std::map<ara::core::StringView, ara::core::StringVi
     auto uart_d = std::make_unique<core::uart::UartDriver>();
     Init(std::move(uart_d));
   }
-  if (!this->uart_->Open(KGPS_UART_path, KGPS_UART_baudrate)) {
+  if (!this->uart_->Open(KGPS_UART_path, KGPS_UART_baudrate, 1)) {
     return 1;
   }
   service_ipc = std::make_unique<apps::GPSServiceSkeleton>(service_ipc_instance);
   service_udp = std::make_unique<apps::GPSServiceSkeleton>(service_udp_instance);
   service_ipc->StartOffer();
   service_udp->StartOffer();
-  ara::log::LogInfo() << "End initialization";
+  ara::log::LogDebug() << "End initialization";
+  last_frame = std::chrono::high_resolution_clock::now();
   return 0;
 }
 

apps/fc/gps_service/gps_app.hpp

@@ -17,6 +17,7 @@
 #include <map>
 #include <memory>
 #include <mutex>  // NOLINT
+#include <chrono>  // NOLINT
 
 #include "ara/exec/adaptive_application.h"
 #include "srp/apps/GPSServiceSkeleton.h"
@@ -35,6 +36,10 @@ class GPSApp final : public ara::exec::AdaptiveApplication {
   std::unique_ptr<apps::GPSServiceSkeleton> service_udp;
   std::unique_ptr<core::uart::IUartDriver> uart_;
 
+  std::chrono::high_resolution_clock::time_point last_frame;
+
+  int64_t GetTimeDelata() const;
+
  public:
   static std::optional<GPSDataStructure> ParseGPSData(const std::vector<uint8_t>& data);
   void Init(std::unique_ptr<core::uart::IUartDriver> uart);
@@ -52,8 +57,6 @@ class GPSApp final : public ara::exec::AdaptiveApplication {
    */
   int Initialize(const std::map<ara::core::StringView, ara::core::StringView>
                       parms) override;
-
- public:
   ~GPSApp();
   GPSApp();
 };

apps/fc/gps_service/ut/BUILD

@@ -8,5 +8,6 @@ cc_test(
         "@com_google_googletest//:gtest_main",
         "//apps/fc/gps_service:gps_app_lib",
         "//core/uart:mock_uart",
+        "@srp_platform//ara/log",
     ],
-)
+)

apps/fc/gps_service/ut/gps_app_test.cc

@@ -12,6 +12,7 @@
 #include <gtest/gtest.h>
 #include "apps/fc/gps_service/gps_app.hpp"
 #include "core/uart/mock_uart_driver.hpp"
+#include "ara/log/log.h"
 
 using ::testing::Return;
 
@@ -107,6 +108,7 @@ TEST(GPSAppTest2, ParseGPSData_ZeroSatellites_ReturnsNullopt) {
 }
 
 TEST(GPSAppTest2, ParseGPSData) {
+    ara::log::LoggingMenager::Create("123", ara::log::LogMode::kConsole);
     std::string str = "$GNGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47";
     std::vector<uint8_t> vec(str.begin(), str.end());
     auto res = srp::apps::GPSApp::ParseGPSData(vec);
@@ -115,6 +117,7 @@ TEST(GPSAppTest2, ParseGPSData) {
     EXPECT_NEAR(res.value().longitude, 1131.000, 0.001);
 }
 TEST(GPSAppTest2, ParseGPSData2) {
+    ara::log::LoggingMenager::Create("123", ara::log::LogMode::kConsole);
     std::string str = "$GNGGA,123519,4807.038,S,01131.000,W,1,08,0.9,545.4,M,46.9,M,,*47";
     std::vector<uint8_t> vec(str.begin(), str.end());
     auto res = srp::apps::GPSApp::ParseGPSData(vec);

apps/fc/main_service/mainService.cpp

@@ -23,10 +23,10 @@ namespace service {
 namespace {
 constexpr auto kService_ipc_name = "srp/apps/MainService/MainService_ipc";
 constexpr auto kService_udp_name = "srp/apps/RecoveryService/MainService_udp";
-using RocketState_t = apps::RocketState_t;
 const auto kMain_loop_delay_ms = 10;
 const auto kSend_event_time = 1000;
 }
+using RocketState_t = apps::RocketState_t;
 
 MainService::MainService() {}
 

apps/fc/main_service/rocketController.cpp

@@ -32,11 +32,6 @@ void RocketController::Loop() {
     break;
     case RocketState_t::DISARM:
     break;
-    case RocketState_t::TANK:
-    if (last_state_ != RocketState_t::TANK) {
-        ActivateTankActuators();
-    }
-    break;
     case RocketState_t::ARM:
     if (last_state_ != RocketState_t::ARM) {
         ArmRocket();
@@ -81,27 +76,17 @@ void RocketController::Loop() {
         // TODO(simba) open all actuator and disable power on actuator
     }
     break;
-    case RocketState_t::ABORD:
+    case RocketState_t::ABORT:
         // TODO(simba) open vent valve
         // TODO(simba) disable all actuator power
     break;
-    case RocketState_t::LOST_CONN: {
-        // NOW UNUSED
-        // if (last_state_ != RocketState_t::LOST_CONN) {
-        //     LossConnSeq();
-        // }
-    break;
     }
-}
-last_state_ = now_state;
+    last_state_ = now_state;
 }
 
 core::ErrorCode RocketController::InitializeCompleted() {
     return core::ErrorCode::kOk;
 }
-core::ErrorCode RocketController::ActivateTankActuators() {
-    return core::ErrorCode::kOk;
-}
 core::ErrorCode RocketController::ArmRocket() {
     // enable power on all stages
     return core::ErrorCode::kOk;

apps/fc/main_service/rocket_state.h

@@ -22,15 +22,13 @@ static std::shared_ptr<RocketState> rocket_state = nullptr;
 enum RocketState_t {
   INIT = 0,
   DISARM = 1,
-  TANK = 2,
-  ARM = 3,
-  CUTDOWN = 4,
-  CUTDOWN_END = 5,
-  FLIGHT = 6,
-  FALL = 7,
-  LANDED = 8,
-  ABORD = 50,
-  LOST_CONN = 52
+  ARM = 2,
+  CUTDOWN = 3,
+  CUTDOWN_END = 4,
+  FLIGHT = 5,
+  FALL = 6,
+  LANDED = 7,
+  ABORT = 50,
 };
 
 class RocketState {

apps/fc/main_service/service.hpp

@@ -20,15 +20,13 @@ namespace srp {
 namespace apps {
 static const std::unordered_map<RocketState_t, std::vector<RocketState_t>> allowed_transitions{
     {RocketState_t::INIT,         {RocketState_t::DISARM}},
-    {RocketState_t::DISARM,       {RocketState_t::TANK, RocketState_t::ABORD}},
-    {RocketState_t::TANK,         {RocketState_t::ARM, RocketState_t::ABORD}},
-    {RocketState_t::ARM,          {RocketState_t::CUTDOWN, RocketState_t::ABORD}},
-    {RocketState_t::CUTDOWN,      {RocketState_t::CUTDOWN_END, RocketState_t::ABORD}},
+    {RocketState_t::DISARM,       {RocketState_t::ARM, RocketState_t::ABORT}},
+    {RocketState_t::ARM,          {RocketState_t::CUTDOWN, RocketState_t::ABORT}},
+    {RocketState_t::CUTDOWN,      {RocketState_t::CUTDOWN_END, RocketState_t::ABORT}},
     {RocketState_t::CUTDOWN_END,  {RocketState_t::FLIGHT}},
     {RocketState_t::FLIGHT,       {RocketState_t::FALL}},
     {RocketState_t::FALL,         {RocketState_t::LANDED}},
-    {RocketState_t::ABORD,        {RocketState_t::DISARM}},
-    {RocketState_t::LOST_CONN,    {RocketState_t::DISARM}},
+    {RocketState_t::ABORT,        {RocketState_t::DISARM}}
 };
 class MyMainServiceSkeleton: public MainServiceSkeleton {
  private:
@@ -38,10 +36,6 @@ class MyMainServiceSkeleton: public MainServiceSkeleton {
       ara::log::LogWarn() <<
           "CUTDOWN_END from CUTDOWN:This change needs to be called automatically by mainService, not from someip!";
     }
-    if (req_state == RocketState_t::DISARM && actual_state == RocketState_t::LOST_CONN) {
-        ara::log::LogWarn() <<
-            "DISARM from LOST_CONN: This change needs to be called automatically by mainService, not from someip!";
-    }
     if (req_state == RocketState_t::DISARM && actual_state == RocketState_t::INIT) {
       ara::log::LogWarn() <<
           "DISARM from INIT: This change needs to be called automatically by mainService, not from someip!";

apps/fc/radio_service/ut/BUILD

@@ -1,16 +1,16 @@
-# cc_test(
-#     name = "radio_app_test",
-#     srcs = [
-#         "radio_app_test.cc"
-#     ],
-#     visibility = ["//visibility:public"],
-#     deps = [
-#         "@com_google_googletest//:gtest_main",
-#         "//apps/fc/radio_service:radio_app_lib",
-#         "//core/uart:mock_uart",
-#         "//core/timestamp:mock_timestamp_controller",
-#     ],
-# )
+cc_test(
+    name = "radio_app_test",
+    srcs = [
+        "radio_app_test.cc"
+    ],
+    visibility = ["//visibility:public"],
+    deps = [
+        "@com_google_googletest//:gtest_main",
+        "//apps/fc/radio_service:radio_app_lib",
+        "//core/uart:mock_uart",
+        "//core/timestamp:mock_timestamp_controller",
+    ],
+)
 
 cc_test(
     name = "event_data_test",

apps/fc/radio_service/ut/radio_app_test.cc

@@ -17,6 +17,7 @@ using ::testing::_;
 using ::testing::Invoke;
 
 TEST(RADIOAPPTEST, InitializeTestNoUart) {
+    ara::log::LoggingMenager::Create("123", ara::log::LogMode::kConsole);
     const std::map<ara::core::StringView, ara::core::StringView> map;
     srp::apps::RadioApp app;
     EXPECT_EQ(app.Initialize(map), 1);

core/pd-33x/rs485/ut/rs485_test.cc

@@ -24,7 +24,7 @@ TEST(RS485Test, InitSuccess) {
     auto uart_mock = std::make_unique<MockUartDriver>();
     auto gpio_mock = std::make_unique<srp::mock::MOCKGPIOController>();
 
-    EXPECT_CALL(*uart_mock, Open(_, _)).WillOnce(Return(true));
+    EXPECT_CALL(*uart_mock, Open(_, _, _)).WillOnce(Return(true));
     EXPECT_CALL(*uart_mock, Close()).Times(1);
 
     EXPECT_TRUE(rs485.Init(config, std::move(uart_mock), std::move(gpio_mock)));
@@ -36,7 +36,7 @@ TEST(RS485Test, InitFailure) {
     auto uart_mock = std::make_unique<MockUartDriver>();
     auto gpio_mock = std::make_unique<srp::mock::MOCKGPIOController>();
 
-    EXPECT_CALL(*uart_mock, Open(_, _)).WillOnce(Return(false));
+    EXPECT_CALL(*uart_mock, Open(_, _, _)).WillOnce(Return(false));
     EXPECT_CALL(*uart_mock, Close()).Times(1);
 
     EXPECT_FALSE(rs485.Init(config, std::move(uart_mock), std::move(gpio_mock)));
@@ -51,7 +51,7 @@ TEST(RS485Test, WriteReadSuccess) {
     auto uart_mock = std::make_unique<MockUartDriver>();
     auto gpio_mock = std::make_unique<srp::mock::MOCKGPIOController>();
 
-    EXPECT_CALL(*uart_mock, Open(_, _)).WillOnce(Return(true));
+    EXPECT_CALL(*uart_mock, Open(_, _, _)).WillOnce(Return(true));
     EXPECT_CALL(*gpio_mock, SetPinValue(1, 1)).WillOnce(Return(srp::core::ErrorCode::kOk));
     EXPECT_CALL(*uart_mock, Write(data)).WillOnce(Return(srp::core::ErrorCode::kOk));
     EXPECT_CALL(*gpio_mock, SetPinValue(1, 0)).WillOnce(Return(srp::core::ErrorCode::kOk));
@@ -73,7 +73,7 @@ TEST(RS485Test, WriteReadFail1) {
     auto uart_mock = std::make_unique<MockUartDriver>();
     auto gpio_mock = std::make_unique<srp::mock::MOCKGPIOController>();
 
-    EXPECT_CALL(*uart_mock, Open(_, _)).WillOnce(Return(true));
+    EXPECT_CALL(*uart_mock, Open(_, _, _)).WillOnce(Return(true));
     EXPECT_CALL(*gpio_mock, SetPinValue(1, 1)).WillOnce(Return(srp::core::ErrorCode::kNotDefine));
     EXPECT_CALL(*uart_mock, Close()).Times(1);
 
@@ -91,7 +91,7 @@ TEST(RS485Test, WriteReadFail2) {
     auto uart_mock = std::make_unique<MockUartDriver>();
     auto gpio_mock = std::make_unique<srp::mock::MOCKGPIOController>();
 
-    EXPECT_CALL(*uart_mock, Open(_, _)).WillOnce(Return(true));
+    EXPECT_CALL(*uart_mock, Open(_, _, _)).WillOnce(Return(true));
     EXPECT_CALL(*gpio_mock, SetPinValue(1, 1)).WillOnce(Return(srp::core::ErrorCode::kOk));
     EXPECT_CALL(*uart_mock, Write(data)).WillOnce(Return(srp::core::ErrorCode::kNotDefine));
     EXPECT_CALL(*uart_mock, Close()).Times(1);
@@ -109,7 +109,7 @@ TEST(RS485Test, WriteReadFail3) {
     auto uart_mock = std::make_unique<MockUartDriver>();
     auto gpio_mock = std::make_unique<srp::mock::MOCKGPIOController>();
 
-    EXPECT_CALL(*uart_mock, Open(_, _)).WillOnce(Return(true));
+    EXPECT_CALL(*uart_mock, Open(_, _, _)).WillOnce(Return(true));
     EXPECT_CALL(*gpio_mock, SetPinValue(1, 1)).WillOnce(Return(srp::core::ErrorCode::kOk));
     EXPECT_CALL(*uart_mock, Write(data)).WillOnce(Return(srp::core::ErrorCode::kOk));
     EXPECT_CALL(*gpio_mock, SetPinValue(1, 0)).WillOnce(Return(srp::core::ErrorCode::kNotDefine));

core/timestamp/mock_timestamp_driver.h

@@ -19,6 +19,7 @@ class MockTimestampController : public srp::core::timestamp::ITimestampControlle
  public:
   MOCK_METHOD(std::optional<int64_t>, GetNewTimeStamp, (), (override));
   MOCK_METHOD(bool, Init, (), (override));
+  MOCK_METHOD(int64_t, GetDeltaTime, (const int64_t now, const int64_t previous), (override));
 };
 
 class MockTimestampMaster : public srp::core::timestamp::ITimestampMaster {

core/timestamp/ut/timestamp_driver_test.cc

@@ -18,7 +18,7 @@
 namespace srp {
 namespace core {
 namespace timestamp {
-constexpr auto KBasicCorrection = 50;
+constexpr auto KBasicCorrection = 0.1;
 class TimestampMasterTest : public ::testing::Test {
  protected:
   TimestampMaster master;
@@ -38,9 +38,9 @@ TEST_F(TimestampMasterTest, GetNewTimeStampReturnsElapsedTime) {
     std::this_thread::sleep_for(std::chrono::milliseconds(K3));
     int64_t timestamp3 = master.GetNewTimeStamp();
 
-    EXPECT_NEAR(timestamp, K1, KBasicCorrection);
-    EXPECT_NEAR(timestamp2, K1 + K2, KBasicCorrection);
-    EXPECT_NEAR(timestamp3, K1 + K2 + K3, 3 * KBasicCorrection);
+    EXPECT_NEAR(timestamp, K1, K1 * KBasicCorrection);
+    EXPECT_NEAR(timestamp2, K1 + K2, (K1 + K2) * KBasicCorrection);
+    EXPECT_NEAR(timestamp3, K1 + K2 + K3, (K1 + K2 + K3) * KBasicCorrection);
 }
 
 TEST_F(TimestampMasterTest, CorrectStartPointAdjustsStartTime) {

core/uart/Iuart_driver.hpp

@@ -23,7 +23,7 @@ namespace uart {
 
 class IUartDriver {
  public:
-  virtual bool Open(const std::string& portName, const uint32_t& baudrate = B9600) = 0;
+  virtual bool Open(const std::string& portName, const uint32_t& baudrate = B9600, const uint8_t timeout = 10) = 0;
   virtual std::optional<std::vector<uint8_t>> Read(const uint16_t size = 0) = 0;
   virtual void Close() = 0;
   virtual core::ErrorCode Write(const std::vector<uint8_t>& data) = 0;

core/uart/mock_uart_driver.hpp

@@ -18,7 +18,7 @@
 
 class MockUartDriver : public srp::core::uart::IUartDriver {
  public:
-    MOCK_METHOD(bool, Open, (const std::string& portName, const uint32_t& baudrate), (override));
+    MOCK_METHOD(bool, Open, (const std::string& portName, const uint32_t& baudrate, const uint8_t timeout), (override));
     MOCK_METHOD(std::optional<std::vector<uint8_t>>, Read, (const uint16_t size), (override));
     MOCK_METHOD(void, Close, (), (override));
     MOCK_METHOD(srp::core::ErrorCode, Write, (const std::vector<uint8_t>& data), (override));