From e298ff4f450a7df8a19d3822c2753120307909d0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Nicolas=20Lembl=C3=A9?= Date: Mon, 9 Dec 2024 14:13:20 +0100 Subject: [PATCH] Refactor and clean up --- examples/master/easycat/easycat_example.cc | 6 +- lib/slave/include/kickcat/AbstractESC.h | 26 +-- lib/slave/include/kickcat/ESC/Lan9252.h | 30 +-- lib/slave/include/kickcat/ESC/XMC4800.h | 4 +- lib/slave/include/kickcat/FSM.h | 88 ++++++-- lib/slave/include/kickcat/Slave2.h | 39 +--- lib/slave/include/kickcat/SlaveFSM.h | 46 +---- lib/slave/src/AbstractESC.cc | 21 +- lib/slave/src/FSM.cc | 45 ++-- lib/slave/src/Slave.cc | 35 +++- lib/slave/src/SlaveFSM.cc | 227 +++++++-------------- 11 files changed, 244 insertions(+), 323 deletions(-) diff --git a/examples/master/easycat/easycat_example.cc b/examples/master/easycat/easycat_example.cc index c8428a01..da2b6dba 100644 --- a/examples/master/easycat/easycat_example.cc +++ b/examples/master/easycat/easycat_example.cc @@ -97,7 +97,6 @@ int main(int argc, char* argv[]) printf("Slave DL status: %s", toString(bus.slaves().at(0).dl_status).c_str()); }); - printf("HEREEEEEE\n"); bus.requestState(State::SAFE_OP); bus.waitForState(State::SAFE_OP, 1s); @@ -119,11 +118,8 @@ int main(int argc, char* argv[]) printInfo(slave); printESC(slave); } - printf("HERE2\n"); auto callback_error = [](DatagramState const&){ THROW_ERROR("something bad happened"); }; - auto callback_error2 = [](DatagramState const&){ THROW_ERROR("something bad happened2"); }; - printf("HERE3\n"); // Set valid output to exit safe op. auto& easycat = bus.slaves().at(0); @@ -142,7 +138,7 @@ int main(int argc, char* argv[]) //TODO: need a way to check expected working counter depending on state // -> in safe op write is not working } - bus.processDataWrite(callback_error2); + bus.processDataWrite(callback_error); try { diff --git a/lib/slave/include/kickcat/AbstractESC.h b/lib/slave/include/kickcat/AbstractESC.h index f52113c2..ae41896c 100644 --- a/lib/slave/include/kickcat/AbstractESC.h +++ b/lib/slave/include/kickcat/AbstractESC.h @@ -20,27 +20,11 @@ namespace kickcat SyncManagerType type; }; -#define SYNC_MANAGER_PI_IN(index, address, length) \ - SyncManagerConfig \ - { \ - index, address, length, 0x20, SyncManagerType::Input \ - } -#define SYNC_MANAGER_PI_OUT(index, address, length) \ - SyncManagerConfig \ - { \ - index, address, length, 0x64, SyncManagerType::Output \ - } + #define SYNC_MANAGER_PI_IN(index, address, length) SyncManagerConfig{index, address, length, 0x20, SyncManagerType::Input} + #define SYNC_MANAGER_PI_OUT(index, address, length) SyncManagerConfig{index, address, length, 0x64, SyncManagerType::Output} -#define SYNC_MANAGER_MBX_IN(index, address, length) \ - SyncManagerConfig \ - { \ - index, address, length, 0x02, SyncManagerType::MailboxIn \ - } -#define SYNC_MANAGER_MBX_OUT(index, address, length) \ - SyncManagerConfig \ - { \ - index, address, length, 0x06, SyncManagerType::MailboxOut \ - } + #define SYNC_MANAGER_MBX_IN(index, address, length) SyncManagerConfig{index, address, length, 0x02, SyncManagerType::MailboxIn} + #define SYNC_MANAGER_MBX_OUT(index, address, length) SyncManagerConfig{index, address, length, 0x06, SyncManagerType::MailboxOut} namespace mailbox::response @@ -48,7 +32,7 @@ namespace kickcat class Mailbox; } - + // Regarding the state machine, see ETG1000.6 6.4.1 AL state machine class AbstractESC { public: diff --git a/lib/slave/include/kickcat/ESC/Lan9252.h b/lib/slave/include/kickcat/ESC/Lan9252.h index 1acfe877..3d688508 100644 --- a/lib/slave/include/kickcat/ESC/Lan9252.h +++ b/lib/slave/include/kickcat/ESC/Lan9252.h @@ -3,8 +3,8 @@ #include "kickcat/AbstractESC.h" #include "kickcat/AbstractSPI.h" -#include "kickcat/OS/Time.h" #include "kickcat/protocol.h" +#include "kickcat/OS/Time.h" #include @@ -13,23 +13,23 @@ namespace kickcat // Host to Network byte order helper (Reminder: EtherCAT is LE, network is BE) // SPI INSTRUCTIONS - constexpr uint8_t READ = 0x03; - constexpr uint8_t WRITE = 0x02; + constexpr uint8_t READ = 0x03; + constexpr uint8_t WRITE = 0x02; constexpr uint8_t BYTE_TEST = 0x64; // Registers - constexpr uint16_t HW_CFG = 0x0074; // Is device ready - constexpr uint16_t RESET_CTL = 0x01F8; // reset register - constexpr uint16_t ECAT_CSR_DATA = 0x0300; // EtherCAT CSR Interface Data Register - constexpr uint16_t ECAT_CSR_CMD = 0x0304; // EtherCAT CSR Interface Command Register + constexpr uint16_t HW_CFG = 0x0074; // Is device ready + constexpr uint16_t RESET_CTL = 0x01F8; // reset register + constexpr uint16_t ECAT_CSR_DATA = 0x0300; // EtherCAT CSR Interface Data Register + constexpr uint16_t ECAT_CSR_CMD = 0x0304; // EtherCAT CSR Interface Command Register constexpr uint16_t ECAT_PRAM_RD_ADDR_LEN = 0X0308; constexpr uint16_t ECAT_PRAM_RD_CMD = 0X030C; constexpr uint16_t ECAT_PRAM_WR_ADDR_LEN = 0X0310; constexpr uint16_t ECAT_PRAM_WR_CMD = 0X0314; - constexpr uint16_t ECAT_PRAM_RD_DATA = 0x000; // until 0x01C - constexpr uint16_t ECAT_PRAM_WR_DATA = 0x020; // until 0x03C + constexpr uint16_t ECAT_PRAM_RD_DATA = 0x000; // until 0x01C + constexpr uint16_t ECAT_PRAM_WR_DATA = 0x020; // until 0x03C constexpr uint16_t NUM_BYTE_INPUT = 32; constexpr uint16_t NUM_BYTE_OUTPUT = 32; @@ -41,7 +41,7 @@ namespace kickcat constexpr uint32_t PRAM_BUSY = 0x1 << 31; constexpr uint32_t PRAM_AVAIL = 0x01; - constexpr uint32_t DIGITAL_RST = 0x01; + constexpr uint32_t DIGITAL_RST = 0x01; constexpr uint32_t BYTE_TEST_DEFAULT = 0x87654321; constexpr milliseconds TIMEOUT{10}; @@ -50,9 +50,9 @@ namespace kickcat constexpr uint8_t CSR_CMD_HEADER_SIZE = 3; struct InternalRegisterControl { - uint8_t instruction; // Read / write // TODO enum full. + uint8_t instruction; // Read / write // TODO enum full. uint16_t LAN9252_register_address; // address of SYSTEM CONTROL AND STATUS REGISTERS - uint8_t payload[64]; // Max payload size is 64 bytes (fifo). + uint8_t payload[64]; // Max payload size is 64 bytes (fifo). } __attribute__((__packed__)); @@ -62,8 +62,8 @@ namespace kickcat static constexpr uint8_t ESC_READ = 0xC0; uint16_t ethercat_register_address; - uint8_t ethercat_register_size; // only size 1,2,4 allowed (ECAT_CSR_CMD specification) - uint8_t ethercat_register_operation; // read / write + uint8_t ethercat_register_size; // only size 1,2,4 allowed (ECAT_CSR_CMD specification) + uint8_t ethercat_register_operation; // read / write } __attribute__((__packed__)); @@ -101,7 +101,7 @@ namespace kickcat int32_t writeData(uint16_t address, void const* data, uint16_t to_write); - std::shared_ptr spi_interface_; // TODO shared ptr like link in bus.h + std::shared_ptr spi_interface_; // TODO shared ptr like link in bus.h }; diff --git a/lib/slave/include/kickcat/ESC/XMC4800.h b/lib/slave/include/kickcat/ESC/XMC4800.h index 8a1348e0..f50f849f 100644 --- a/lib/slave/include/kickcat/ESC/XMC4800.h +++ b/lib/slave/include/kickcat/ESC/XMC4800.h @@ -6,13 +6,13 @@ namespace kickcat { constexpr uint32_t ECAT0_BASE_ADDRESS = 0x54010000; - constexpr uint32_t ECAT0_END_ADDRESS = 0x5401FFFF; + constexpr uint32_t ECAT0_END_ADDRESS = 0x5401FFFF; class XMC4800 final : public AbstractESC { public: - XMC4800() = default; + XMC4800() = default; ~XMC4800() = default; diff --git a/lib/slave/include/kickcat/FSM.h b/lib/slave/include/kickcat/FSM.h index a7357cff..c10f5cde 100644 --- a/lib/slave/include/kickcat/FSM.h +++ b/lib/slave/include/kickcat/FSM.h @@ -2,55 +2,105 @@ #define SLAVE_STACK_INCLUDE_FSM_H_ #include -#include -#include +#include "PDO.h" #include "kickcat/AbstractESC.h" #include "kickcat/protocol.h" namespace kickcat { + constexpr uint8_t NUMBER_OF_STATES = 4; + + // TODO: to rename ESM namespace FSM { class StateMachine; + struct ALControl + { + uint16_t value; + + State getRequestedState() + { + return static_cast(value & State::MASK_STATE); + } + }; + + struct ALStatus + { + uint16_t al_status; + uint16_t al_status_code; + uint16_t al_watchdog; + bool validOutputData{false}; + + State getState() + { + return static_cast(al_status & State::MASK_STATE); + } + + bool has_expired_watchdog() + { + return not(al_watchdog & 0x1); + } + + static ALStatus build(uint8_t state, uint8_t statusCode = StatusCode::NO_ERROR) + { + if (statusCode == NO_ERROR) + { + return ALStatus{state, statusCode, 0}; + } + else + { + return ALStatus{static_cast(state | State::ERROR_ACK), statusCode, 0}; + } + } + }; + class AbstractState + { friend StateMachine; public: - AbstractState(uint8_t id); - virtual ~AbstractState() = default; - - uint8_t id(); + AbstractState(uint8_t id, AbstractESC& esc, PDO& pdo); + void setMailbox(mailbox::response::Mailbox* mbx); protected: uint8_t id_; + AbstractESC& esc_; + PDO& pdo_; + mailbox::response::Mailbox* mbx_; + + virtual ALStatus routine(ALStatus currentStatus, + ALControl alControl); //return al_status and al_status_code + + virtual ALStatus routineInternal(ALStatus currentStatus, + ALControl alControl) = 0; //return al_status and al_status_code + + virtual void onEntry(uint8_t fromState) + { + (void)fromState; + }; private: - virtual std ::tuple routine( - uint16_t al_control, - uint16_t al_status, - uint16_t al_status_code) = 0; //return al_status and al_status_code - virtual void onEntry(uint8_t fromState); + uint8_t id(); }; - class StateMachine + class StateMachine final { public: - StateMachine(AbstractESC& esc, std::array&& states); + StateMachine(AbstractESC& esc, std::array&& states); + void setOutputDataValid(bool isValid); void start(); void play(); - protected: - AbstractState* getState(uint8_t id); - private: + AbstractState* findState(uint8_t id); + AbstractESC& esc_; AbstractState* currentState_; - std::array states_; - uint16_t al_status_{State::INIT}; - uint16_t al_status_code_{NO_ERROR}; + std::array states_; + ALStatus status_{}; }; } } diff --git a/lib/slave/include/kickcat/Slave2.h b/lib/slave/include/kickcat/Slave2.h index 28c62bac..a0b8fbf1 100644 --- a/lib/slave/include/kickcat/Slave2.h +++ b/lib/slave/include/kickcat/Slave2.h @@ -7,43 +7,18 @@ #include "kickcat/Mailbox.h" #include "kickcat/SlaveFSM.h" + +// TODO: to rename file namespace kickcat { class Slave final { public: - Slave(AbstractESC* esc, PDO* pdo) - : esc_{esc} - , pdo_{pdo} - { - } - - void set_mailbox(mailbox::response::Mailbox* mbx) - { - mbx_ = mbx; - init_.setMailbox(mbx); - preOp_.setMailbox(mbx); - safeOP_.setMailbox(mbx); - OP_.setMailbox(mbx); - } - - void start() - { - stateMachine.start(); - } - - void routine() - { - if (mbx_) - { - mbx_->receive(); - mbx_->process(); - mbx_->send(); - } - - - stateMachine.play(); - } + Slave(AbstractESC* esc, PDO* pdo); + + void set_mailbox(mailbox::response::Mailbox* mbx); + void start(); + void routine(); private: AbstractESC* esc_; diff --git a/lib/slave/include/kickcat/SlaveFSM.h b/lib/slave/include/kickcat/SlaveFSM.h index cb71f90f..7dc03881 100644 --- a/lib/slave/include/kickcat/SlaveFSM.h +++ b/lib/slave/include/kickcat/SlaveFSM.h @@ -11,72 +11,44 @@ #include "kickcat/Mailbox.h" #include "kickcat/protocol.h" - +// TODO: to rename file namespace kickcat { namespace FSM { - - class SlaveState : public AbstractState - { - public: - SlaveState(uint8_t id, AbstractESC& esc, PDO& pdo); - void setMailbox(mailbox::response::Mailbox* mbx); - - protected: - void set_al_status(State state); - State getRequestedState(uint16_t al_control); - std::tuple buildALStatus(uint8_t state, uint8_t statusCode); - void clear_error(); - - - AbstractESC& esc_; - PDO& pdo_; - mailbox::response::Mailbox* mbx_; - }; - - class Init : public SlaveState + class Init : public AbstractState { public: Init(AbstractESC& esc, PDO& pdo); void onEntry(uint8_t fromState) override; - std::tuple routine(uint16_t al_control, - uint16_t al_status, - uint16_t al_status_code) override; + ALStatus routineInternal(ALStatus currentStatus, ALControl control) override; }; - - class PreOP : public SlaveState + class PreOP : public AbstractState { public: PreOP(AbstractESC& esc, PDO& pdo); void onEntry(uint8_t fromState) override; - std::tuple routine(uint16_t al_control, - uint16_t al_status, - uint16_t al_status_code) override; + ALStatus routineInternal(ALStatus currentStatus, ALControl control) override; }; - class SafeOP : public SlaveState + class SafeOP : public AbstractState { public: SafeOP(AbstractESC& esc, PDO& pdo); void onEntry(uint8_t fromState) override; - std::tuple routine(uint16_t al_control, - uint16_t al_status, - uint16_t al_status_code) override; + ALStatus routineInternal(ALStatus currentStatus, ALControl control) override; }; - class OP : public SlaveState + class OP : public AbstractState { public: OP(AbstractESC& esc, PDO& pdo); - std::tuple routine(uint16_t al_control, - uint16_t al_status, - uint16_t al_status_code) override; + ALStatus routineInternal(ALStatus currentStatus, ALControl control) override; private: bool has_expired_watchdog(); diff --git a/lib/slave/src/AbstractESC.cc b/lib/slave/src/AbstractESC.cc index 75b20fa6..3ccd546c 100644 --- a/lib/slave/src/AbstractESC.cc +++ b/lib/slave/src/AbstractESC.cc @@ -29,25 +29,8 @@ namespace kickcat == (sm_ref.control & SYNC_MANAGER_CONTROL_DIRECTION_MASK)) and (sm_read.activate & SM_ACTIVATE_ENABLE); -// if (is_valid) -// { -// } -// else -// { -// 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); -// printf("NOT valid !!\n"); -// } +// // 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); return is_valid; } diff --git a/lib/slave/src/FSM.cc b/lib/slave/src/FSM.cc index 2101cfef..617d0f5e 100644 --- a/lib/slave/src/FSM.cc +++ b/lib/slave/src/FSM.cc @@ -4,18 +4,23 @@ namespace kickcat::FSM { - AbstractState::AbstractState(uint8_t id) - : id_{id} + AbstractState::AbstractState(uint8_t id, AbstractESC& esc, PDO& pdo) + : id_(id) + , esc_{esc} + , pdo_{pdo} { } + void AbstractState::setMailbox(mailbox::response::Mailbox* mbx) + { + mbx_ = mbx; + } + uint8_t AbstractState::id() { return id_; } - void AbstractState::onEntry(uint8_t) {}; - StateMachine::StateMachine(AbstractESC& esc, std::array&& states) : esc_{esc} , states_{std::move(states)} @@ -23,12 +28,16 @@ namespace kickcat::FSM currentState_ = states_[0]; } + void StateMachine::setOutputDataValid(bool isValid) + { + status_.validOutputData = isValid; + } void StateMachine::start() { currentState_->onEntry(currentState_->id()); } - AbstractState* StateMachine::getState(uint8_t id) + AbstractState* StateMachine::findState(uint8_t id) { auto it = std::find_if(std::begin(states_), std::end(states_), [&](auto* state) { return state->id() == id; }); if (it == states_.end()) @@ -41,21 +50,20 @@ namespace kickcat::FSM void StateMachine::play() { - uint16_t al_control = {0}; - // Get al control - esc_.read(reg::AL_CONTROL, &al_control, sizeof(al_control)); + ALControl control; + esc_.read(reg::AL_CONTROL, &control.value, sizeof(control.value)); - auto [al_status, al_status_code] = currentState_->routine(al_control, al_status_, al_status_code_); + // Update watchdog + esc_.read(reg::WDOG_STATUS, &status_.al_watchdog, sizeof(status_.al_watchdog)); - al_status_ = al_status; - al_status_code_ = al_status_code; + auto newStatus = currentState_->routine(status_, control); - uint8_t newStateId = al_status & State::MASK_STATE; + uint8_t newStateId = newStatus.getState(); if (newStateId != currentState_->id()) { - AbstractState* newState = getState(newStateId); + AbstractState* newState = findState(newStateId); if (not newState) { newState = states_[0]; @@ -68,8 +76,13 @@ namespace kickcat::FSM } } - // TODO: we shouldn't do that every loop - esc_.write(reg::AL_STATUS_CODE, &al_status_code_, sizeof(al_status_code_)); - esc_.write(reg::AL_STATUS, &al_status_, sizeof(al_status_)); + if (status_.al_status != newStatus.al_status or status_.al_status_code != newStatus.al_status_code) + { + status_ = newStatus; + + // Note : StatusCode MUST be set before status + esc_.write(reg::AL_STATUS_CODE, &status_.al_status_code, sizeof(status_.al_status_code)); + esc_.write(reg::AL_STATUS, &status_.al_status, sizeof(status_.al_status)); + } } } diff --git a/lib/slave/src/Slave.cc b/lib/slave/src/Slave.cc index 982a9feb..3951fc19 100644 --- a/lib/slave/src/Slave.cc +++ b/lib/slave/src/Slave.cc @@ -1,3 +1,36 @@ -#include "kickcat/Slave.h" +#include "kickcat/Slave2.h" +namespace kickcat +{ + Slave::Slave(AbstractESC* esc, PDO* pdo) + : esc_{esc} + , pdo_{pdo} + { + } + void Slave::set_mailbox(mailbox::response::Mailbox* mbx) + { + mbx_ = mbx; + init_.setMailbox(mbx); + preOp_.setMailbox(mbx); + safeOP_.setMailbox(mbx); + OP_.setMailbox(mbx); + } + + void Slave::start() + { + stateMachine.start(); + } + + void Slave::routine() + { + if (mbx_) + { + mbx_->receive(); + mbx_->process(); + mbx_->send(); + } + + stateMachine.play(); + } +} diff --git a/lib/slave/src/SlaveFSM.cc b/lib/slave/src/SlaveFSM.cc index b39ba8e5..bf0898e4 100644 --- a/lib/slave/src/SlaveFSM.cc +++ b/lib/slave/src/SlaveFSM.cc @@ -5,37 +5,8 @@ namespace kickcat::FSM { - SlaveState::SlaveState(uint8_t id, AbstractESC& esc, PDO& pdo) - : AbstractState(id) - , esc_{esc} - , pdo_{pdo} - { - } - - void SlaveState::setMailbox(mailbox::response::Mailbox* mbx) - { - mbx_ = mbx; - } - - State SlaveState::getRequestedState(uint16_t al_control) - { - return static_cast(al_control & State::MASK_STATE); - } - - std::tuple SlaveState::buildALStatus(uint8_t state, uint8_t statusCode) - { - if (statusCode == NO_ERROR) - { - return std::tuple(state, statusCode); - } - else - { - return std::tuple(state | State::ERROR_ACK, statusCode); - } - } - Init::Init(AbstractESC& esc, PDO& pdo) - : SlaveState(State::INIT, esc, pdo) + : AbstractState(State::INIT, esc, pdo) { } @@ -48,76 +19,83 @@ namespace kickcat::FSM pdo_.set_sm_activated(false); } - std::tuple Init::routine(uint16_t al_control, - uint16_t al_status, - uint16_t al_status_code) + + ALStatus AbstractState::routine(ALStatus currentStatus, ALControl control) { + + // TODO: update the comment // If master didn't aknowledge error but asked for the init, aknowledge it // If it didn't ask stay in init and don't aknowledge - if (al_status & ERROR_ACK and not(al_control & ERROR_ACK)) + if (currentStatus.al_status & ERROR_ACK and not(control.value & ERROR_ACK)) { - if (getRequestedState(al_control) == INIT) + if (control.getRequestedState() == INIT and currentStatus.getState() == INIT) { - return buildALStatus(INIT, NO_ERROR); + return ALStatus::build(INIT); } - else + + if (control.getRequestedState() == INIT) { - return buildALStatus(INIT, al_status_code); + return ALStatus::build(INIT, currentStatus.al_status_code); } + + return ALStatus::build(currentStatus.al_status, currentStatus.al_status_code); } - // asked for Next mode ? - if (getRequestedState(al_control) == State::PRE_OP) + // Unknown state request + auto requestedState = control.getRequestedState(); + if (currentStatus.getState() != State::OPERATIONAL and requestedState != State::BOOT and requestedState != State::INIT + and requestedState != State::PRE_OP and requestedState != State::SAFE_OP + and requestedState != State::OPERATIONAL) + { + return ALStatus::build(id_, StatusCode::UNKNOWN_REQUESTED_STATE); + } + + return routineInternal(currentStatus, control); + } + + ALStatus Init::routineInternal(ALStatus, ALControl control) + { + if (control.getRequestedState() == State::PRE_OP) { if (not mbx_) { - return buildALStatus(State::PRE_OP, StatusCode::NO_ERROR); + return ALStatus::build(State::PRE_OP); } if (mbx_->configureSm() == hresult::OK) { if (mbx_->is_sm_config_ok()) { - printf("go to preop\n"); - return buildALStatus(State::PRE_OP, StatusCode::NO_ERROR); + return ALStatus::build(State::PRE_OP); } else { - return buildALStatus(State::INIT, StatusCode::INVALID_MAILBOX_CONFIGURATION_PREOP); + return ALStatus::build(State::INIT, StatusCode::INVALID_MAILBOX_CONFIGURATION_PREOP); } } } // Invalid state request - if ((getRequestedState(al_control) == State::SAFE_OP) or (getRequestedState(al_control) == State::OPERATIONAL)) + if ((control.getRequestedState() == State::SAFE_OP) or (control.getRequestedState() == State::OPERATIONAL)) { - return buildALStatus(State::INIT, StatusCode::INVALID_REQUESTED_STATE_CHANGE); - } - - // Unknown state request - auto requestedState = getRequestedState(al_control); - if (requestedState != State::BOOT and requestedState != State::INIT and requestedState != State::PRE_OP - and requestedState != State::SAFE_OP and requestedState != State::OPERATIONAL) - { - return buildALStatus(id_, StatusCode::UNKNOWN_REQUESTED_STATE); + return ALStatus::build(State::INIT, StatusCode::INVALID_REQUESTED_STATE_CHANGE); } // BOOTSTRAP not supported yet. If implemented, need to check the SII to know if enabled. - if (getRequestedState(al_control) == State::BOOT) + if (control.getRequestedState() == State::BOOT) { - return std::tuple(id_, StatusCode::BOOTSTRAP_NOT_SUPPORTED); + return ALStatus::build(id_, StatusCode::BOOTSTRAP_NOT_SUPPORTED); } - return std::tuple(State::INIT, StatusCode::NO_ERROR); + return ALStatus::build(State::INIT); } PreOP::PreOP(AbstractESC& esc, PDO& pdo) - : SlaveState(State::PRE_OP, esc, pdo) + : AbstractState(State::PRE_OP, esc, pdo) { } void PreOP::onEntry(uint8_t) { - printf("Preop::onEntry\n"); if (mbx_) { mbx_->set_sm_activate(true); @@ -125,69 +103,48 @@ namespace kickcat::FSM pdo_.set_sm_activated(false); } - std::tuple PreOP::routine(uint16_t al_control, - uint16_t al_status, - uint16_t al_status_code) + ALStatus PreOP::routineInternal(ALStatus, ALControl control) { - if (al_status & ERROR_ACK and not(al_control & ERROR_ACK)) - { - if (getRequestedState(al_control) == INIT) - { - return buildALStatus(INIT, al_status_code); - } - - return buildALStatus(al_status, al_status_code); - } - if (mbx_ and not mbx_->is_sm_config_ok()) { - return std::tuple(State::INIT, StatusCode::INVALID_MAILBOX_CONFIGURATION_PREOP); + return ALStatus::build(State::INIT, StatusCode::INVALID_MAILBOX_CONFIGURATION_PREOP); } - if (getRequestedState(al_control) == State::SAFE_OP) + if (control.getRequestedState() == State::SAFE_OP) { if (pdo_.configure_pdo_sm() != hresult::OK) { - return std::tuple(id_, StatusCode::NO_ERROR); + return ALStatus::build(id_); } StatusCode pdo_sm_config_status_code = pdo_.is_sm_config_ok(); if (pdo_sm_config_status_code != StatusCode::NO_ERROR) { - return std::tuple(id_, pdo_sm_config_status_code); + return ALStatus::build(id_, pdo_sm_config_status_code); } - return std::tuple(State::SAFE_OP, StatusCode::NO_ERROR); + return ALStatus::build(State::SAFE_OP); } - if (getRequestedState(al_control) == State::INIT) + if (control.getRequestedState() == State::INIT) { - return std::tuple(INIT, StatusCode::NO_ERROR); + return ALStatus::build(INIT); } - if (getRequestedState(al_control) == State::OPERATIONAL or getRequestedState(al_control) == State::BOOT) - { - return std::tuple(id_, StatusCode::INVALID_REQUESTED_STATE_CHANGE); - } - - auto requestedState = getRequestedState(al_control); - if (requestedState != State::BOOT and requestedState != State::INIT and requestedState != State::PRE_OP - and requestedState != State::SAFE_OP and requestedState != State::OPERATIONAL) + if (control.getRequestedState() == State::OPERATIONAL or control.getRequestedState() == State::BOOT) { - return buildALStatus(id_, StatusCode::UNKNOWN_REQUESTED_STATE); + return ALStatus::build(id_, StatusCode::INVALID_REQUESTED_STATE_CHANGE); } - return std::tuple(id_, StatusCode::NO_ERROR); + return ALStatus::build(id_); } SafeOP::SafeOP(AbstractESC& esc, PDO& pdo) - : SlaveState(State::SAFE_OP, esc, pdo) + : AbstractState(State::SAFE_OP, esc, pdo) { } - - void SafeOP::onEntry(uint8_t fromState) { if (fromState == State::OPERATIONAL) @@ -200,122 +157,80 @@ namespace kickcat::FSM } } - std::tuple SafeOP::routine(uint16_t al_control, - uint16_t al_status, - uint16_t al_status_code) + ALStatus SafeOP::routineInternal(ALStatus currentStatus, ALControl control) { - if (al_status & ERROR_ACK and not(al_control & ERROR_ACK)) - { - if (getRequestedState(al_control) == INIT) - { - return buildALStatus(INIT, al_status_code); - } - - return buildALStatus(al_status, al_status_code); - } - pdo_.update_process_data_input(); pdo_.update_process_data_output(); if (mbx_ and not mbx_->is_sm_config_ok()) { - return std::tuple(State::INIT, StatusCode::INVALID_MAILBOX_CONFIGURATION_PREOP); + return ALStatus::build(State::INIT, StatusCode::INVALID_MAILBOX_CONFIGURATION_PREOP); } StatusCode pdo_sm_config_status_code = pdo_.is_sm_config_ok(); if (pdo_sm_config_status_code != StatusCode::NO_ERROR) { - return std::tuple(State::PRE_OP, pdo_sm_config_status_code); + return ALStatus::build(State::PRE_OP, pdo_sm_config_status_code); } - //TODO: check if valid_ output data - if (getRequestedState(al_control) == State::OPERATIONAL) + if (control.getRequestedState() == State::OPERATIONAL and currentStatus.validOutputData) { - return std::tuple(State::OPERATIONAL, StatusCode::NO_ERROR); + return ALStatus::build(State::OPERATIONAL); } - else if (getRequestedState(al_control) == State::PRE_OP or getRequestedState(al_control) == State::INIT) + else if (control.getRequestedState() == State::PRE_OP or control.getRequestedState() == State::INIT) { - return std::tuple(getRequestedState(al_control), StatusCode::NO_ERROR); + return ALStatus::build(control.getRequestedState()); } - if (getRequestedState(al_control) == State::BOOT) + if (control.getRequestedState() == State::BOOT) { - return std::tuple(State::SAFE_OP, INVALID_REQUESTED_STATE_CHANGE); + return ALStatus::build(State::SAFE_OP, INVALID_REQUESTED_STATE_CHANGE); } - auto requestedState = getRequestedState(al_control); - if (requestedState != State::BOOT and requestedState != State::INIT and requestedState != State::PRE_OP - and requestedState != State::SAFE_OP and requestedState != State::OPERATIONAL) - { - return buildALStatus(State::SAFE_OP, StatusCode::UNKNOWN_REQUESTED_STATE); - } - - return std::tuple(State::SAFE_OP, NO_ERROR); + return ALStatus::build(State::SAFE_OP); } OP::OP(AbstractESC& esc, PDO& pdo) - : SlaveState(State::OPERATIONAL, esc, pdo) + : AbstractState(State::OPERATIONAL, esc, pdo) { } - std::tuple OP::routine(uint16_t al_control, uint16_t al_status, uint16_t al_status_code) + ALStatus OP::routineInternal(ALStatus currentStatus, ALControl control) { - if (al_status & ERROR_ACK and not(al_control & ERROR_ACK)) - { - if (getRequestedState(al_control) == INIT) - { - return buildALStatus(INIT, al_status_code); - } - - return buildALStatus(al_status, al_status_code); - } - - pdo_.update_process_data_input(); pdo_.update_process_data_output(); - if (has_expired_watchdog()) + if (currentStatus.has_expired_watchdog()) { - return std::tuple(State::SAFE_OP, SYNC_MANAGER_WATCHDOG); + return ALStatus::build(State::SAFE_OP, SYNC_MANAGER_WATCHDOG); } if (mbx_ and not mbx_->is_sm_config_ok()) { - return std::tuple(INIT, INVALID_MAILBOX_CONFIGURATION_PREOP); - } - - if (getRequestedState(al_control) == State::BOOT) - { - return std::tuple(State::SAFE_OP, INVALID_REQUESTED_STATE_CHANGE); + return ALStatus::build(INIT, INVALID_MAILBOX_CONFIGURATION_PREOP); } StatusCode pdo_sm_config_status_code = pdo_.is_sm_config_ok(); if (pdo_sm_config_status_code != StatusCode::NO_ERROR) { - return std::tuple(PRE_OP, pdo_sm_config_status_code); + return ALStatus::build(PRE_OP, pdo_sm_config_status_code); } - if (getRequestedState(al_control) == State::BOOT) + if (control.getRequestedState() == State::BOOT) { - return std::tuple(SAFE_OP, INVALID_REQUESTED_STATE_CHANGE); + return ALStatus::build(State::SAFE_OP, INVALID_REQUESTED_STATE_CHANGE); } - auto requestedState = getRequestedState(al_control); + auto requestedState = control.getRequestedState(); if (requestedState != State::BOOT and requestedState != State::INIT and requestedState != State::PRE_OP and requestedState != State::SAFE_OP and requestedState != State::OPERATIONAL) { - return buildALStatus(SAFE_OP, StatusCode::UNKNOWN_REQUESTED_STATE); + return ALStatus::build(State::SAFE_OP, StatusCode::UNKNOWN_REQUESTED_STATE); } - return std::tuple(getRequestedState(al_control), NO_ERROR); - } - bool OP::has_expired_watchdog() - { - //TODO should be given in entry of the state - uint16_t watchdog{0}; - esc_.read(reg::WDOG_STATUS, &watchdog, sizeof(watchdog)); - return not(watchdog & 0x1); + + return ALStatus::build(control.getRequestedState()); } }