Merge pull request #485 from ut-issl/feature/add-c2a-command-handling…

…-feature

Add C2A command sender for accelerated SILS test

data/sample/initialize_files/components/wings_command_sender_to_c2a/example.ops

@@ -0,0 +1,12 @@
+.# ===== Example =====
+.#
+.# コメントは読み飛ばされる
+.# 
+.MOBC_RT.Cmd_NOP
+ wait_sec 5
+ MOBC_RT.Cmd_MM_START_TRANSITION 2
+ wait_sec 3
+ check_value  # check_valueは今は使えず読み飛ばされる
+ let mode_id = 2 # letはいまはつかえない
+ AOBC_RT.Cmd_MM_START_TRANSITION {mode_id} # letを前提とした{}付きの引数があるとコマンドは送信されない
+.# 

data/sample/initialize_files/components/wings_command_sender_to_c2a/wings_command_sender_to_c2a.ini

@@ -0,0 +1,5 @@
+[WINGS_COMMAND_SENDER_TO_C2A]
+command_send_enable = ENABLE
+prescaler = 1
+c2a_command_database_file = ../../data/initialize_files/components/wings_command_sender_to_c2a/SAMPLE_MOBC_CMD_DB_CMD_DB.csv
+wings_operation_file = ../../data/initialize_files/components/wings_command_sender_to_c2a/example.ops

src/components/CMakeLists.txt

@@ -25,6 +25,7 @@ real/cdh/on_board_computer_with_c2a.cpp
 real/communication/antenna.cpp
 real/communication/antenna_radiation_pattern.cpp
 real/communication/ground_station_calculator.cpp
+real/communication/wings_command_sender_to_c2a.cpp
 
 examples/example_change_structure.cpp
 examples/example_serial_communication_with_obc.cpp

src/components/real/cdh/on_board_computer_with_c2a.cpp

@@ -1,6 +1,7 @@
 /*
  * @file on_board_computer_with_c2a.cpp
  * @brief Class to emulate on board computer with C2A flight software
+ * @note Used C2A functions: TMGR_init, C2A_core_init, WDT_init, TMGR_clear, TMGR_count_up_master_clock, TDSP_execute_pl_as_task_list
  */
 
 #include "on_board_computer_with_c2a.hpp"

src/components/real/communication/wings_command_sender_to_c2a.cpp

@@ -0,0 +1,147 @@
+/*
+ * @file wings_command_sender_to_c2a.cpp
+ * @brief A component to send C2A command using WINGS operation file
+ * @note Used C2A functions: CCP_register_rtc, CCP_register_tlc
+ *       Used C2A structures: CMD_CODE
+ */
+
+#include "wings_command_sender_to_c2a.hpp"
+
+#include <library/initialize/initialize_file_access.hpp>
+#include <library/utilities/macros.hpp>
+#include <regex>
+
+#ifdef USE_C2A
+#if C2A_CORE_VER_MAJOR == 4
+// c2a-core v4
+#include "src_core/tlm_cmd/common_cmd_packet_util.h"
+#elif C2A_CORE_VER_MAJOR <= 3
+// c2a-core <= v3
+#include "src_core/TlmCmd/common_cmd_packet_util.h"
+#else
+#error "c2a-core version is not supported"
+#endif  // c2a-core version header
+#endif  // USE_C2A
+
+void WingsCommandSenderToC2a::MainRoutine(const int time_count) {
+  UNUSED(time_count);
+  if (is_enabled_ == false) return;
+  if (is_end_of_line_ == true) return;
+  if (wait_s_ <= 0.0) {
+    std::string line = wings_operation_file_.GetLatestLine();
+    if (line == "EOL") {
+      is_end_of_line_ = true;
+    } else {
+      ExecuteCommandLine(line);
+    }
+  } else {
+    wait_s_ -= step_width_s_;
+  }
+}
+
+void WingsCommandSenderToC2a::ExecuteCommandLine(const std::string line) {
+  // Separate with space
+  std::istringstream token_stream(line);
+  std::string token;
+  std::vector<std::string> tokens;
+  while (token_stream >> token) {
+    tokens.push_back(token);
+  }
+
+  // Handle WINGS commands
+  if (tokens[0].find("wait_sec") == 0) {
+    // wait process
+    wait_s_ = std::stod(tokens[1]);
+  } else if (tokens[0].find("check_value") == 0) {
+    // TODO: Support check_value
+    wait_s_ = 0;
+  } else if (tokens[0].find("let") == 0) {
+    // TODO: Support let command
+    wait_s_ = 0;
+  } else {
+    wait_s_ = 0;
+    AnalyzeC2aCommand(tokens);
+  }
+
+  return;
+}
+
+void WingsCommandSenderToC2a::AnalyzeC2aCommand(const std::vector<std::string> tokens) {
+#ifdef USE_C2A
+  // Recognize command
+  size_t first_underscore_position = tokens[0].find('_');
+  if (first_underscore_position == std::string::npos) return;
+  size_t first_dot_position = tokens[0].find('.');
+  if (first_underscore_position == std::string::npos) return;
+
+  std::string target_name = tokens[0].substr(0, first_underscore_position);
+  // TODO check target?
+  std::string command_type = tokens[0].substr(first_underscore_position + 1, first_dot_position - (first_underscore_position + 1));
+  std::string command_name = tokens[0].substr(first_dot_position + 1);
+
+  // Get command code
+  C2aCommandInformation cmd_info = c2a_command_database_.GetCommandInformation(command_name);
+  if (cmd_info.GetCommandName() == "Error") {
+    // TODO: error handling
+  }
+  CMD_CODE cmd_id = (CMD_CODE)cmd_info.GetCommandId();
+
+  // Get arguments
+  std::vector<std::string> arguments;
+  for (size_t i = 1; i < tokens.size(); i++) {
+    if (tokens[i].find("{") == 0) return;  // let command is not supported now TODO: support let command
+    arguments.push_back(tokens[i]);
+  }
+  // Check number of arguments
+  if (arguments.size() != cmd_info.GetNumberOfArgument()) {
+    // TODO: error handling
+  }
+
+  // Decode arguments
+  // Read TI
+  uint32_t ti = 0;
+  if (command_type == "TL" || command_type == "BL") {
+    ti = (uint32_t)std::stoi(arguments[0]);
+    arguments.erase(arguments.begin());
+  }
+  // Read arguments
+  uint8_t param[CSP_MAX_LEN];
+  uint16_t param_len = 0;
+  for (size_t arg_num = 0; arg_num < arguments.size(); arg_num++) {
+    size_t len = 0;
+    DecodeC2aCommandArgument(cmd_info.GetArgumentType(arg_num), arguments[arg_num], param + param_len, len);
+    param_len += (uint16_t)len;
+  }
+
+  // Send command
+  if (command_type == "RT") {
+    CCP_register_rtc(cmd_id, param, param_len);
+  } else if (command_type == "TL") {
+    CCP_register_tlc(ti, TLCD_ID_FROM_GS, cmd_id, param, param_len);
+  } else if (command_type == "BL") {
+    // TODO: BL実装したいが、register_blがない？
+  } else {
+    // Not reach
+  }
+#else
+  UNUSED(tokens);
+#endif
+}
+
+WingsCommandSenderToC2a InitWingsCommandSenderToC2a(ClockGenerator* clock_generator, const double compo_update_step_s,
+                                                    const std::string initialize_file) {
+  IniAccess ini_access(initialize_file);
+  std::string section = "WINGS_COMMAND_SENDER_TO_C2A";
+
+  bool is_enabled = ini_access.ReadEnable(section.c_str(), "command_send_enable");
+
+  int prescaler = ini_access.ReadInt(section.c_str(), "prescaler");
+  if (prescaler <= 1) prescaler = 1;
+
+  double step_width_s = (double)prescaler * compo_update_step_s;
+
+  std::string c2a_command_data_base_file = ini_access.ReadString(section.c_str(), "c2a_command_database_file");
+  std::string wings_operation_file = ini_access.ReadString(section.c_str(), "wings_operation_file");
+
+  return WingsCommandSenderToC2a(prescaler, clock_generator, step_width_s, c2a_command_data_base_file, wings_operation_file, is_enabled);
+}

src/components/real/communication/wings_command_sender_to_c2a.hpp

@@ -0,0 +1,79 @@
+/*
+ * @file wings_command_sender_to_c2a.hpp
+ * @brief A component to send command using WINGS operation file
+ */
+
+#ifndef S2E_COMPONENTS_REAL_COMMUNICATION_C2A_COMMAND_SENDER_HPP_
+#define S2E_COMPONENTS_REAL_COMMUNICATION_C2A_COMMAND_SENDER_HPP_
+
+#include <library/initialize/c2a_command_database.hpp>
+#include <library/initialize/wings_operation_file.hpp>
+
+#include "../../base/component.hpp"
+
+/*
+ * @class C2aCommandSender
+ * @brief A component to send C2A command using WINGS operation file
+ */
+class WingsCommandSenderToC2a : public Component {
+ public:
+  /**
+   * @fn WingsCommandSenderToC2a
+   * @brief Constructor
+   * @param [in]
+   */
+  WingsCommandSenderToC2a(int prescaler, ClockGenerator* clock_generator, const double step_width_s, const std::string command_database_file,
+                          const std::string operation_file, const bool is_enabled)
+      : Component(prescaler, clock_generator),
+        c2a_command_database_(command_database_file),
+        wings_operation_file_(operation_file),
+        is_enabled_(is_enabled),
+        step_width_s_(step_width_s) {}
+
+  /**
+   * @fn ~WingsCommandSenderToC2a
+   * @brief Destructor
+   */
+  ~WingsCommandSenderToC2a() {}
+
+ protected:
+  C2aCommandDatabase c2a_command_database_;  //!< Command database
+  WingsOperationFile wings_operation_file_;  //!< WINGS operation file
+  bool is_enabled_;                          //!< Enable flag
+  const double step_width_s_;                //!< Step width to execute this component [s]
+  double wait_s_ = 0.0;                      //!< Wait counter [s]
+  bool is_end_of_line_ = false;              //!< Flag to detect end of line
+
+  // Override functions for Component
+  /**
+   * @fn MainRoutine
+   * @brief Main routine to send command
+   */
+  void MainRoutine(const int time_count) override;
+
+  /**
+   * @fn ExecuteCommandLine
+   * @brief Execute command line
+   * @param[in] line: Executed line
+   */
+  void ExecuteCommandLine(const std::string line);
+
+  /**
+   * @fn AnalyzeC2aCommand
+   * @brief Analyze C2A command Line
+   * @param[in] tokens: Command line after space separation
+   */
+  void AnalyzeC2aCommand(const std::vector<std::string> tokens);
+};
+
+/**
+ * @fn InitWingsCommandSenderToC2a
+ * @brief Initialize WingsCommandSenderToC2a
+ * @param[in] clock_generator: Clock generator
+ * @param[in] compo_update_step_s: Component update step time [s]
+ * @param[in] initialize_file: Initialize file name
+ */
+WingsCommandSenderToC2a InitWingsCommandSenderToC2a(ClockGenerator* clock_generator, const double compo_update_step_s,
+                                                    const std::string initialize_file);
+
+#endif  // S2E_COMPONENTS_REAL_COMMUNICATION_C2A_COMMAND_SENDER_HPP_

src/library/CMakeLists.txt

@@ -8,6 +8,8 @@ add_library(${PROJECT_NAME} STATIC
   geodesy/geodetic_position.cpp
 
   initialize/initialize_file_access.cpp
+  initialize/c2a_command_database.cpp
+  initialize/wings_operation_file.cpp
 
   logger/logger.cpp
   logger/initialize_log.cpp