Handle repeat service

Various fixes

examples/slave/nuttx/xmc4800/export_nuttx_archive.sh

@@ -20,13 +20,13 @@ esac
 
 
 
-nuttx_src=~/wdc_workspace/src/autre/nuttx_dev
-build=~/wdc_workspace/src/autre/KickCAT/nuttx_build
+nuttx_src=~/wdc_workspace/src/nuttxspace/nuttx
+build=~/wdc_workspace/src/KickCAT/build_slave
 src=~/wdc_workspace/src/KickCAT
 
 bin=xmc4800_$project
 
-nuttx_version=nuttx-export-12.7.0
+nuttx_version=nuttx-export-12.5.1
 
 rm -f ${nuttx_src}/${nuttx_version}.tar.gz
 rm -rf ${build}/${nuttx_version}

examples/slave/nuttx/xmc4800/od_populator.cc

@@ -105,20 +105,7 @@ namespace kickcat::CoE
             CoE::addEntry(object,2,32,63,static_cast<CoE::DataType>(7),"IMU",0x2);
             dictionary.push_back(std::move(object));
         }
-/*
-        for (uint16_t i = 0; i < 0x200; ++i)
-        {
-            CoE::Object object
-            {
-                uint16_t(i + 0x2000),
-                CoE::ObjectCode::VAR,
-                "Operation mode",
-                {}
-            };
-            CoE::addEntry(object,0,16,127,static_cast<CoE::DataType>(6),"",0x0);
-            dictionary.push_back(std::move(object));
-        }
-*/
+
         return dictionary;
     }
 }

lib/include/kickcat/Mailbox.h

@@ -190,7 +190,7 @@ namespace kickcat::mailbox::response
 
         void receive();  // Try to receive a message from the ESC
         void process();  // Process a message in the to_process_ queue if any
-        void send();  // Send a message in the to_send_ queue if any, keep it in the queue if the ESC is not ready yet
+        void send();     // Send a message in the to_send_ queue if any, keep it in the queue if the ESC is not ready yet
 
     private:
         void replyError(std::vector<uint8_t>&& raw_message, uint16_t code);
@@ -202,10 +202,13 @@ namespace kickcat::mailbox::response
         uint16_t max_msgs_;
 
         std::vector<std::function<std::shared_ptr<AbstractMessage>(Mailbox*, std::vector<uint8_t>&&)>> factories_;
-
-        std::list<std::shared_ptr<AbstractMessage>>  to_process_;   /// Received messages, waiting to be processed
-        std::queue<std::vector<uint8_t>> to_send_;      /// Messages to send (replies from a received messages)
         CoE::Dictionary dictionary_;
+
+        std::list<std::shared_ptr<AbstractMessage>> to_process_;    /// Received messages, waiting to be processed
+        std::queue<std::vector<uint8_t>> to_send_;                  /// Messages to send (replies from a received messages)
+
+        std::vector<uint8_t> last_sent_{};                          /// store the last sent message in case of repeat requested
+        std::vector<uint8_t> repeat_{};
     };
 }
 

lib/include/kickcat/protocol.h

@@ -368,6 +368,15 @@ namespace kickcat
     constexpr uint8_t SM_CONTROL_WATCHDOG_DISABLED = 0x00;
     constexpr uint8_t SM_CONTROL_WATCHDOG_ENABLED = 0x40;
 
+    constexpr uint8_t SM_ACTIVATE_REPEAT_REQ = (1 << 1);
+
+    constexpr uint8_t SM_PDI_CTRL_REPEAT_ACK = (1 << 1);
+
+    constexpr uint8_t MAILBOX_STATUS = (1 << 3);
+
+    constexpr uint8_t SYNC_MANAGER_CONTROL_OPERATION_MODE_MASK = (1 << 0);
+    constexpr uint8_t SYNC_MANAGER_CONTROL_DIRECTION_MASK = (1 << 1);
+
     enum SyncManagerType
     {
         Unused     = 0,
@@ -376,13 +385,8 @@ namespace kickcat
         Output     = 3,
         Input      = 4  // slave to master
     };
-    constexpr uint8_t MAILBOX_STATUS = (1 << 3);
-
-    constexpr uint8_t SYNC_MANAGER_CONTROL_OPERATION_MODE_MASK = (1 << 0);
-    constexpr uint8_t SYNC_MANAGER_CONTROL_DIRECTION_MASK = (1 << 1);
-
-
     char const* toString(SyncManagerType const& type);
+    constexpr uint16_t addressSM(uint8_t index) { return reg::SYNC_MANAGER + index * sizeof(SyncManager); };
 
     struct FMMU
     {

lib/slave/include/kickcat/AbstractESC.h

@@ -66,6 +66,9 @@ namespace kickcat
 
         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();
 
@@ -74,7 +77,6 @@ namespace kickcat
 
         bool is_valid_sm(SyncManagerConfig const& sm_ref);
         bool are_valid_sm(std::vector<SyncManagerConfig> const& sm);
-        void set_sm_activate(std::vector<SyncManagerConfig> const& sync_managers, bool is_activated);
 
         void set_error(StatusCode code);
 

lib/slave/src/AbstractESC.cc

@@ -19,18 +19,15 @@ namespace kickcat
 
     bool AbstractESC::is_valid_sm(SyncManagerConfig const& sm_ref)
     {
-        auto create_sm_address = [](uint16_t reg, uint16_t sm_index) { return reg + sm_index * 8; };
-
         SyncManager sm_read;
-
-        read(create_sm_address(reg::SYNC_MANAGER, sm_ref.index), &sm_read, sizeof(sm_read));
+        read(addressSM(sm_ref.index), &sm_read, sizeof(sm_read));
 
         bool is_valid = (sm_read.start_address == sm_ref.start_address) and (sm_read.length == sm_ref.length)
                         and ((sm_read.control & SYNC_MANAGER_CONTROL_OPERATION_MODE_MASK)
                              == (sm_ref.control & SYNC_MANAGER_CONTROL_OPERATION_MODE_MASK))
                         and ((sm_read.control & SYNC_MANAGER_CONTROL_DIRECTION_MASK)
                              == (sm_ref.control & SYNC_MANAGER_CONTROL_DIRECTION_MASK))
-                        and sm_read.activate == 1;
+                        and ((sm_read.activate & 1) == 1);
 
         // printf("SM read %i: start address %x, length %u, control %x, status %x, activate %x \n", sm_ref.index, sm_read.start_address, sm_read.length, sm_read.control, sm_read.status, sm_read.activate);
         // printf("SM config %i: start address %x, length %u, control %x \n", sm_ref.index, sm_ref.start_address, sm_ref.length, sm_ref.control);
@@ -48,25 +45,58 @@ namespace kickcat
     }
 
 
-    void AbstractESC::set_sm_activate(std::vector<SyncManagerConfig> const& sync_managers, bool is_activated)
+    void AbstractESC::sm_activate(SyncManagerConfig const& sm_conf)
     {
-        auto create_sm_address = [](uint16_t reg, uint16_t sm_index) { return reg + sm_index * 8; };
+        auto const address = addressSM(sm_conf.index) + 7;
 
-        for (auto& sm_conf : sync_managers)
+        uint8_t pdi_control;
+        read(address, &pdi_control, sizeof(uint8_t));
+
+        uint8_t sm_deactivated = 0x1;
+        pdi_control &= ~sm_deactivated;
+        write(address, &pdi_control, sizeof(uint8_t));
+
+        do
+        {
+            read(address, &pdi_control, 1);
+        }
+        while ((pdi_control & 1) == 1);
+    }
+
+    void AbstractESC::sm_deactivate(SyncManagerConfig const& sm_conf)
+    {
+        auto const address = addressSM(sm_conf.index) + 7;
+
+        uint8_t pdi_control;
+        read(address, &pdi_control, sizeof(uint8_t));
+
+        uint8_t sm_deactivated = 0x1;
+        pdi_control |= sm_deactivated;
+        write(address, &pdi_control, sizeof(uint8_t));
+
+        while (true)
         {
-            SyncManager sm;
-            read(create_sm_address(reg::SYNC_MANAGER, sm_conf.index), &sm, sizeof(sm));
+            read(address, &pdi_control, sizeof(uint8_t));
+            if ((pdi_control & 1) == 1)
+            {
+                break;
+            }
+        }
+    }
+
 
-            uint8_t sm_deactivated = 0x1;
+    void AbstractESC::set_sm_activate(std::vector<SyncManagerConfig> const& sync_managers, bool is_activated)
+    {
+        for (auto& sm_conf : sync_managers)
+        {
             if (is_activated)
             {
-                sm.pdi_control &= ~sm_deactivated;
+                sm_activate(sm_conf);
             }
             else
             {
-                sm.pdi_control |= sm_deactivated;
+                sm_deactivate(sm_conf);
             }
-            write(create_sm_address(reg::SYNC_MANAGER, sm_conf.index), &sm, sizeof(sm));
         }
     }
 
@@ -90,7 +120,7 @@ namespace kickcat
         for (uint8_t i = 0; i < reg::SM_STATS; i++)
         {
             SyncManager sync;
-            read(reg::SYNC_MANAGER + sizeof(SyncManager) * i, &sync, sizeof(SyncManager));
+            read(addressSM(i), &sync, sizeof(SyncManager));
             if ((sync.control & 0x0F) == (controlMode & 0x0F))
             {
                 return std::tuple(i, sync);
@@ -225,7 +255,7 @@ namespace kickcat
     {
         if (not are_valid_sm({sm_mbx_input_, sm_mbx_output_}))
         {
-            set_sm_activate({sm_mbx_input_, sm_mbx_output_}, false);
+            set_sm_activate({sm_mbx_input_, sm_mbx_output_}, false); //AIE!
             set_al_status(State::INIT);
             set_error(StatusCode::INVALID_MAILBOX_CONFIGURATION_PREOP);
         }