Merge pull request #187 from nicolas71640/poc_fsm_abstracesc

Refactor AbstractESC FSM

CMakeLists.txt

@@ -42,6 +42,11 @@ if (GTest_FOUND)
                               unit/protocol-t.cc
                               unit/slave-t.cc
                               unit/socket-t.cc
+                              unit/ESM-t.cc
+                              unit/ESMStateOP-t.cc
+                              unit/ESMStateInit-t.cc
+                              unit/ESMStatePreOP-t.cc
+                              unit/ESMStateSafeOP-t.cc
                               unit/Time.cc
                               unit/CoE/protocol-t.cc
                               unit/mailbox/request-t.cc
@@ -58,7 +63,7 @@ if (GTest_FOUND)
     append_coverage_compiler_flags()
     set(GCOVR_ADDITIONAL_ARGS --exclude-unreachable-branches --exclude-throw-branches)
 
-    set(EXCLUDE_FILES "unit/*" ".*gtest.*" "example" ".*gmock.*" ".*/OS/.*" "tools/*" "conan/*")
+    set(EXCLUDE_FILES "unit/*" ".*gtest.*" "examples/*" ".*gmock.*" ".*/OS/.*" "tools/*" "conan/*" "*conan2/*")
     setup_target_for_coverage_gcovr_html(
         NAME coverage
         EXECUTABLE kickcat_unit

examples/slave/nuttx/lan9252/main.cc

@@ -1,11 +1,13 @@
 
-#include "kickcat/nuttx/SPI.h"
 #include "kickcat/ESC/Lan9252.h"
-#include "kickcat/protocol.h"
 #include "kickcat/OS/Time.h"
+#include "kickcat/PDO.h"
+#include "kickcat/nuttx/SPI.h"
+#include "kickcat/protocol.h"
+#include "kickcat/slave/Slave.h"
 
-#include <nuttx/board.h>
 #include <arch/board/board.h>
+#include <nuttx/board.h>
 
 
 using namespace kickcat;
@@ -17,23 +19,24 @@ int main(int argc, char *argv[])
     (void) argv;
     std::shared_ptr<SPI> spi_driver = std::make_shared<SPI>();
     Lan9252 esc = Lan9252(spi_driver);
+    PDO pdo(&esc);
+    slave::Slave slave(&esc, &pdo);
 
     constexpr uint32_t pdo_size = 32;
 
     uint8_t buffer_in[pdo_size];
     uint8_t buffer_out[pdo_size];
 
     // init values
-    for (uint32_t i=0; i < pdo_size; ++i)
+    for (uint32_t i = 0; i < pdo_size; ++i)
     {
         buffer_in[i] = i;
         buffer_out[i] = 0xFF;
     }
 
-    esc.set_process_data_input(buffer_in);
-    esc.set_process_data_output(buffer_out);
+    pdo.setInput(buffer_in);
+    pdo.setOutput(buffer_out);
 
-    esc.init();
 
     uint8_t esc_config;
     esc.read(reg::ESC_CONFIG, &esc_config, sizeof(esc_config));
@@ -45,24 +48,26 @@ int main(int argc, char *argv[])
     esc.read(reg::PDI_CONFIGURATION, &pdi_config, sizeof(pdi_config));
     printf("pdi config 0x%x \n", pdi_config);
 
-    while(true)
+    slave.start();
+
+    while (true)
     {
-        esc.routine();
+        slave.routine();
         // Print received data
     //    for (uint8_t i = 0; i < pdo_size; ++i)
     //    {
     //        printf("%x", buffer_out[i]);
     //    }
     //    printf("\n");
 
-       if (esc.al_status() & State::SAFE_OP)
-       {
-           if (buffer_out[1] != 0xFF)
-           {
-               esc.set_valid_output_data_received(true);
-           }
-       }
-       sleep(1ms);
+        if (slave.state() == State::SAFE_OP)
+        {
+            if (buffer_out[1] != 0xFF)
+            {
+                slave.validateOutputData();
+            }
+        }
+        sleep(1ms);
     }
     return 0;
 }

examples/slave/nuttx/xmc4800/main_foot.cc

@@ -1,9 +1,11 @@
+#include "kickcat/CoE/OD.h"
+#include "kickcat/CoE/mailbox/response.h"
 #include "kickcat/EEPROM/XMC4800EEPROM.h"
-#include "kickcat/OS/Time.h"
 #include "kickcat/ESC/XMC4800.h"
-#include "kickcat/CoE/OD.h"
 #include "kickcat/Mailbox.h"
-#include "kickcat/CoE/mailbox/response.h"
+#include "kickcat/OS/Time.h"
+#include "kickcat/PDO.h"
+#include "kickcat/slave/Slave.h"
 
 namespace foot
 {
@@ -49,8 +51,9 @@ int main(int, char *[])
 
     XMC4800 esc;
     XMC4800EEPROM eeprom;
+    PDO pdo(&esc);
+    slave::Slave slave(&esc, &pdo);
 
-    esc.init();
     eeprom.init();
     uint16_t al_status;
     uint16_t al_control;
@@ -65,9 +68,9 @@ int main(int, char *[])
     auto dictionary = CoE::createOD();
     mbx.enableCoE(std::move(dictionary));
 
-    esc.set_mailbox(&mbx);
-    esc.set_process_data_input(reinterpret_cast<uint8_t *>(&input_PDO));
-    esc.set_process_data_output(reinterpret_cast<uint8_t *>(&output_PDO));
+    slave.setMailbox(&mbx);
+    pdo.setInput(&input_PDO);
+    pdo.setOutput(&output_PDO);
 
     uint8_t esc_config;
     esc.read(reg::ESC_CONFIG, &esc_config, sizeof(esc_config));
@@ -78,15 +81,18 @@ int main(int, char *[])
     esc.read(reg::PDI_CONFIGURATION, &pdi_config, sizeof(pdi_config));
     printf("pdi config 0x%x \n", pdi_config);
 
-    while(true)
+
+    slave.start();
+
+    while (true)
     {
         eeprom.process();
-        esc.routine();
-        if (esc.al_status() & State::SAFE_OP)
+        slave.routine();
+        if (slave.state() == State::SAFE_OP)
         {
             if (output_PDO.watchdog_counter != 0x00)
             {
-               esc.set_valid_output_data_received(true);
+                slave.validateOutputData();
             }
         }
 

examples/slave/nuttx/xmc4800/main_relax.cc

@@ -1,18 +1,21 @@
 
 #include "kickcat/EEPROM/XMC4800EEPROM.h"
-#include "kickcat/OS/Time.h"
 #include "kickcat/ESC/XMC4800.h"
+#include "kickcat/OS/Time.h"
+#include "kickcat/PDO.h"
+#include "kickcat/slave/Slave.h"
 
-int main(int, char *[])
+int main(int, char*[])
 {
     using namespace kickcat;
 
     printf("\n\n\n\n XMC hello relax\n");
 
     XMC4800 esc;
     XMC4800EEPROM eeprom;
+    PDO pdo(&esc);
+    slave::Slave slave(&esc, &pdo);
 
-    esc.init();
     eeprom.init();
     uint16_t al_status;
     uint16_t al_control;
@@ -26,14 +29,14 @@ int main(int, char *[])
     uint8_t buffer_out[pdo_size];
 
     // init values
-    for (uint32_t i=0; i < pdo_size; ++i)
+    for (uint32_t i = 0; i < pdo_size; ++i)
     {
         buffer_in[i] = 3*i;
         buffer_out[i] = 0xFF;
     }
 
-    esc.set_process_data_input(buffer_in);
-    esc.set_process_data_output(buffer_out);
+    pdo.setInput(buffer_in);
+    pdo.setOutput(buffer_out);
 
     uint8_t esc_config;
     esc.read(reg::ESC_CONFIG, &esc_config, sizeof(esc_config));
@@ -44,15 +47,17 @@ int main(int, char *[])
     esc.read(reg::PDI_CONFIGURATION, &pdi_config, sizeof(pdi_config));
     printf("pdi config 0x%x \n", pdi_config);
 
-    while(true)
+    slave.start();
+
+    while (true)
     {
         eeprom.process();
-        esc.routine();
-        if (esc.al_status() & State::SAFE_OP)
+        slave.routine();
+        if (slave.state() == State::SAFE_OP)
         {
             if (buffer_out[1] != 0xFF)
             {
-               esc.set_valid_output_data_received(true);
+                slave.validateOutputData();
             }
         }
     }

lib/include/kickcat/Mailbox.h

@@ -186,7 +186,9 @@ namespace kickcat::mailbox::response
         void enableCoE(CoE::Dictionary&& dictionary);
         CoE::Dictionary& getDictionary(){return dictionary_;}
 
-        std::tuple<SyncManagerConfig, SyncManagerConfig> configureSm();
+        hresult configure();
+        bool isConfigOk();
+        void activate(bool is_activated);
 
         void receive();  // Try to receive a message from the ESC
         void process();  // Process a message in the to_process_ queue if any
@@ -199,8 +201,9 @@ namespace kickcat::mailbox::response
         void replyError(std::vector<uint8_t>&& raw_message, uint16_t code);
 
         AbstractESC* esc_;
-        SyncManagerConfig mbx_in_;
-        SyncManagerConfig mbx_out_;
+        SyncManagerConfig mbx_in_{};
+        SyncManagerConfig mbx_out_{};
+
         uint16_t max_allocated_ram_by_msg_;
         uint16_t max_msgs_;
 

lib/include/kickcat/protocol.h

@@ -390,7 +390,7 @@ namespace kickcat
         Input      = 4  // slave to master
     };
     char const* toString(SyncManagerType const& type);
-    constexpr uint16_t addressSM(uint8_t index) { return reg::SYNC_MANAGER + index * sizeof(SyncManager); };
+    constexpr uint16_t addressSM(uint8_t index) { return static_cast<uint16_t>(reg::SYNC_MANAGER + index * sizeof(SyncManager)); };
 
     struct FMMU
     {

lib/slave/CMakeLists.txt

@@ -1,13 +1,18 @@
 target_sources(kickcat PRIVATE
   ${CMAKE_CURRENT_SOURCE_DIR}/src/AbstractEmulatedEEPROM.cc
   ${CMAKE_CURRENT_SOURCE_DIR}/src/AbstractESC.cc
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/ESM.cc
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/ESMStates.cc
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/PDO.cc
   ${CMAKE_CURRENT_SOURCE_DIR}/src/AbstractSPI.cc
 
   ${CMAKE_CURRENT_SOURCE_DIR}/src/EEPROM/EEPROM_factory.cc
 
   ${CMAKE_CURRENT_SOURCE_DIR}/src/ESC/EmulatedESC.cc
   ${CMAKE_CURRENT_SOURCE_DIR}/src/ESC/Lan9252.cc
   ${CMAKE_CURRENT_SOURCE_DIR}/src/ESC/XMC4800.cc
+
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/slave/Slave.cc
 )
 
 target_include_directories(kickcat PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include/kickcat)

lib/slave/include/kickcat/AbstractESC.h

@@ -4,15 +4,12 @@
 #include <cstdint>
 #include <vector>
 
-#include "kickcat/protocol.h"
 #include "kickcat/Error.h"
+#include "kickcat/protocol.h"
 
 
 namespace kickcat
 {
-    // TODO define to a proper place
-    void reportError(hresult const& rc);
-
     struct SyncManagerConfig
     {
         uint8_t index;
@@ -38,69 +35,19 @@ namespace kickcat
     class AbstractESC
     {
     public:
+        AbstractESC()          = default;
         virtual ~AbstractESC() = default;
 
-        virtual hresult init()                                                   = 0;
+        virtual hresult init() { return hresult::OK; }
         virtual int32_t read(uint16_t address, void* data, uint16_t size)        = 0;
         virtual int32_t write(uint16_t address, void const* data, uint16_t size) = 0;
 
-        void routine();
-
-
-        // Mailbox that the ESC will try to configure before passing to preop
-        void set_mailbox(mailbox::response::Mailbox* mailbox);
-        void set_process_data_input(uint8_t* buffer);
-        void set_process_data_output(uint8_t* buffer);
-        void set_valid_output_data_received(bool are_valid_output);
-        std::tuple<uint8_t, SyncManager> find_sm(uint16_t controlMode);
-
-        void routine_init();
-        void routine_preop();
-        void routine_safeop();
-        void routine_op();
-
-        void set_state_on_error(State state, StatusCode error_code);
-        void clear_error();
-
-        uint16_t al_status() { return al_status_;};
-
-        bool has_expired_watchdog() { return not (watchdog_ & 0x1); }
-
-        void sm_activate(SyncManagerConfig const& sm);
-        void sm_deactivate(SyncManagerConfig const& sm);
-        void set_sm_activate(std::vector<SyncManagerConfig> const& sync_managers, bool is_activated);
-    private:
-        bool configure_pdo_sm();
-
-        void update_process_data_input();
-        void update_process_data_output();
-
-        bool is_valid_sm(SyncManagerConfig const& sm_ref);
-        bool are_valid_sm(std::vector<SyncManagerConfig> const& sm);
-
-        void set_error(StatusCode code);
-
-        /// \brief Set only the state, preserve the other fields.
-        void set_al_status(State state);
-
-        StatusCode al_status_code_ = {StatusCode::NO_ERROR};
-        uint16_t al_status_ = {0};
-
-        uint16_t al_control_ = {0};
-        uint16_t watchdog_ = {0};
-
-        uint8_t* process_data_input_ = {nullptr};
-        SyncManagerConfig sm_pd_input_ = {};
-
-        uint8_t* process_data_output_ = {nullptr};
-        SyncManagerConfig sm_pd_output_ = {};
-
-        SyncManagerConfig sm_mbx_input_  = {};
-        SyncManagerConfig sm_mbx_output_ = {};
-
-        bool are_valid_output_data_ = false;
+        std::tuple<uint8_t, SyncManager> findSm(uint16_t controlMode);
+        void activateSm(SyncManagerConfig const& sm);
+        void deactivateSm(SyncManagerConfig const& sm);
 
-        mailbox::response::Mailbox* mbx_;
+        bool isSmValid(SyncManagerConfig const& sm_ref);
+        void setSmActivate(std::vector<SyncManagerConfig> const& sync_managers, bool is_activated);
     };
 
 }

lib/slave/include/kickcat/ESC/EmulatedESC.h

@@ -25,7 +25,6 @@ namespace kickcat
 
         void processDatagram(DatagramHeader* header, uint8_t* data, uint16_t* wkc);
 
-        hresult init() override { return hresult::OK; }
         int32_t read (uint16_t address, void* data,       uint16_t size) override;
         int32_t write(uint16_t address, void const* data, uint16_t size) override;
 

lib/slave/include/kickcat/ESC/Lan9252.h

@@ -74,12 +74,12 @@ namespace kickcat
         Lan9252(std::shared_ptr<AbstractSPI> spi_interface);
         ~Lan9252() = default;
 
-        hresult init() override;
 
         int32_t read(uint16_t address, void* data, uint16_t size) override;
         int32_t write(uint16_t address, void const* data, uint16_t size) override;
 
     private:
+        hresult init() override;
         template <typename T>
         void readInternalRegister(uint16_t address, T& payload)
         {