diff --git a/components/flash/znvs.cpp b/components/flash/znvs.cpp index 5c01627..71c901b 100644 --- a/components/flash/znvs.cpp +++ b/components/flash/znvs.cpp @@ -2,163 +2,86 @@ #include -#include "sdk\components\flash\zsimple_flash.hpp" -#define TAG "config" - -#ifdef IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM - +#include "zsimple_flash.hpp" using namespace iflytop; using namespace std; -#define MARK_S 0x123456 -#define MARK_E 0x87654321 -#define GET_CFG(typename) \ - cfg_t* cfg = get_cfg(key); \ - if (cfg == nullptr || cfg->type != kcfg_type_##typename) { \ - allocate_cfg(key, kcfg_type_##typename, (uint8_t*)&default_val, sizeof(default_val)); \ - cfg = get_cfg(key); \ - } \ - return *(typename*)cfg->val; - -#define SET_CFG(typename) \ - cfg_t* cfg = get_cfg(key); \ - if (cfg == nullptr || cfg->type != kcfg_type_##typename) { \ - allocate_cfg(key, kcfg_type_##typename, (uint8_t*)&val, sizeof(val)); \ - cfg = get_cfg(key); \ - } else { \ - *(typename*)cfg->val = val; \ - flush(); \ - } +#define TAG "ZNVS" +#define MARK_S 0x12345678 +#define MARK_E 0x87654321 +#ifdef IFLYTOP_NVS_CONFIG_FLASH_SECTOR ZNVS& ZNVS::ins() { - static ZNVS instance; - return instance; -} -void ZNVS::initialize() { - zsimple_flash_init(IFLYTOP_NVS_CONFIG_FLASH_SECTOR); - zsimple_flash_read((uint8_t*)&m_cfg, sizeof(m_cfg)); - if (m_cfg.config_start != MARK_S || m_cfg.config_end != MARK_E) { - ZLOGW(TAG, "config uninitialized, initialize it"); - memset(&m_cfg, 0, sizeof(m_cfg)); - m_cfg.config_start = MARK_S; - m_cfg.config_end = MARK_E; - zsimple_flash_write((uint8_t*)&m_cfg, sizeof(m_cfg)); - } else { - ZLOGI(TAG, "config initialized"); - } + static ZNVS s_ins; + return s_ins; } -void ZNVS::factory_reset() { - memset(&m_cfg, 0, sizeof(m_cfg)); - m_cfg.config_start = MARK_S; - m_cfg.config_end = MARK_E; - zsimple_flash_write((uint8_t*)&m_cfg, sizeof(m_cfg)); +void ZNVS::initialize() { zsimple_flash_init(IFLYTOP_NVS_CONFIG_FLASH_SECTOR); } +bool ZNVS::alloc_config(const char* key, uint8_t* data, size_t datalen) { + ZASSERT(m_index_num < ZARRAY_SIZE(m_index)); + m_index[m_index_num].name = key; + m_index[m_index_num].len = datalen; + m_index[m_index_num].defaultval = data; + m_index_num++; + return true; } - -ZNVS::cfg_t* ZNVS::get_cfg(const char* key) { - for (int i = 0; i < IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM; i++) { - if (strcmp(m_cfg.cfgs[i].key, key) == 0) { - return &m_cfg.cfgs[i]; - } +bool ZNVS::init_config() { + int configtotallen = 0; + for (int i = 0; i < m_index_num; i++) { + configtotallen += m_index[i].len; } - return nullptr; -} + configtotallen = configtotallen + sizeof(config_t) /*head*/ + 4 /*tail*/; -void ZNVS::allocate_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len) { - cfg_t* cfg = get_cfg(key); - for (int i = 0; i < IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM; i++) { - if (m_cfg.cfgs[i].is_initialed == false) { - cfg = &m_cfg.cfgs[i]; - strcpy(cfg->key, key); - cfg->is_initialed = true; - cfg->type = (uint8_t)type; - memcpy(cfg->val, default_val, len); -#if 0 - /** - * @brief 刷新flash - * - * TODO:优化这里的代码，不要每次都刷新flash - */ - zsimple_flash_write((uint8_t*)&m_cfg, sizeof(m_cfg)); -#endif - break; - } + m_cfgcache = (config_t*)malloc(configtotallen); + ZASSERT(m_cfgcache); + m_cfgcachesize = configtotallen; + + zsimple_flash_read((uint8_t*)m_cfgcache, m_cfgcachesize); + if (m_cfgcache->config_start != MARK_S || m_cfgcache->data_size != (m_cfgcachesize - sizeof(config_t))) { + ZLOGW(TAG, "config uninitialized, initialize it"); + memset(m_cfgcache, 0, m_cfgcachesize); + m_cfgcache->config_start = MARK_S; + m_cfgcache->data_size = m_cfgcachesize - sizeof(config_t); + flush_all_to_configcache(); + zsimple_flash_write((uint8_t*)m_cfgcache, m_cfgcachesize); + } else { + ZLOGI(TAG, "config initialized successfully"); } + inited = true; + return true; } -ZNVS::cfg_t* ZNVS::get_and_create_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len) { - cfg_t* cfg = get_cfg(key); - if (cfg == nullptr) { - allocate_cfg(key, type, default_val, len); - cfg = get_cfg(key); +void ZNVS::flush_all_to_configcache() { + for (int i = 0; i < m_index_num; i++) { + memcpy(m_cfgcache->data, m_index[i].defaultval, m_index[i].len); } - return cfg; } -void ZNVS::dumpcfg() { - ZLOGI(TAG, "=================dump nvs config =================="); - ZLOGI(TAG, "="); - for (int i = 0; i < IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM; i++) { - if (m_cfg.cfgs[i].is_initialed) { - dumpcfg(&m_cfg.cfgs[i]); +bool ZNVS::get_config(const char* key, uint8_t* data, size_t len) { + ZASSERT(inited); + int dataoffset = 0; + for (int i = 0; i < m_index_num; i++) { + if (strcmp(m_index[i].name, key) == 0) { + ZASSERT(len == m_index[i].len); + memcpy(data, m_cfgcache->data + dataoffset, m_index[i].len); + return true; } + dataoffset += m_index[i].len; } - ZLOGI(TAG, "============="); + return false; } - -void ZNVS::dumpcfg(cfg_t* cfg) { - if (!cfg->is_initialed) return; - switch (cfg->type) { - case kcfg_type_int8_t: - ZLOGI(TAG, "= %s val:%d", cfg->key, *(int8_t*)cfg->val); - break; - case kcfg_type_int16_t: - ZLOGI(TAG, "= %s val:%d", cfg->key, *(int16_t*)cfg->val); - break; - case kcfg_type_int32_t: - ZLOGI(TAG, "= %s val:%d", cfg->key, *(int32_t*)cfg->val); - break; - - case kcfg_type_uint8_t: - ZLOGI(TAG, "= %s val:%u", cfg->key, *(int8_t*)cfg->val); - break; - case kcfg_type_uint16_t: - ZLOGI(TAG, "= %s val:%u", cfg->key, *(int16_t*)cfg->val); - break; - case kcfg_type_uint32_t: - ZLOGI(TAG, "= %s val:%u", cfg->key, *(int32_t*)cfg->val); - break; - case kcfg_type_float: - ZLOGI(TAG, "= %s val:%f", cfg->key, *(float*)cfg->val); - break; - case kcfg_type_bool: - ZLOGI(TAG, "= %s val:%d", cfg->key, *(bool*)cfg->val); - break; - default: - ZLOGI(TAG, "= %s val:unknow type", cfg->key); - break; +bool ZNVS::set_config(const char* key, uint8_t* data, size_t len) { + ZASSERT(inited); + int dataoffset = 0; + for (int i = 0; i < m_index_num; i++) { + if (strcmp(m_index[i].name, key) == 0) { + memcpy(m_cfgcache->data + dataoffset, data, len); + return true; + } + dataoffset += m_index[i].len; } - return; + return false; } - -int8_t ZNVS::get_config_int8(const char* key, int8_t default_val) { GET_CFG(int8_t); } -void ZNVS::set_config_int8(const char* key, int8_t val) { SET_CFG(int8_t); } -uint8_t ZNVS::get_config_uint8(const char* key, uint8_t default_val) { GET_CFG(uint8_t); } -void ZNVS::set_config_uint8(const char* key, uint8_t val) { SET_CFG(uint8_t); } -int16_t ZNVS::get_config_int16(const char* key, int16_t default_val) { GET_CFG(int16_t); } -void ZNVS::set_config_int16(const char* key, int16_t val) { SET_CFG(int16_t); } -uint16_t ZNVS::get_config_uint16(const char* key, uint16_t default_val) { GET_CFG(uint16_t); } -void ZNVS::set_config_uint16(const char* key, uint16_t val) { SET_CFG(uint16_t); } -int32_t ZNVS::get_config_int32(const char* key, int32_t default_val) { GET_CFG(int32_t); } -void ZNVS::set_config_int32(const char* key, int32_t val) { SET_CFG(int32_t); } -uint32_t ZNVS::get_config_uint32(const char* key, uint32_t default_val) { GET_CFG(uint32_t); } -void ZNVS::set_config_uint32(const char* key, uint32_t val) { SET_CFG(uint32_t); } -float ZNVS::get_config_float(const char* key, float default_val) { GET_CFG(float); } -void ZNVS::set_config_float(const char* key, float val) { SET_CFG(float); } -bool ZNVS::get_config_bool(const char* key, bool default_val) { GET_CFG(bool); } -void ZNVS::set_config_bool(const char* key, bool val) { SET_CFG(bool); } - -void ZNVS::flush() { zsimple_flash_write((uint8_t*)&m_cfg, sizeof(m_cfg)); } - +void ZNVS::flush() { zsimple_flash_write((uint8_t*)m_cfgcache, m_cfgcachesize); } #endif \ No newline at end of file diff --git a/components/flash/znvs.hpp b/components/flash/znvs.hpp index 457881e..43d067d 100644 --- a/components/flash/znvs.hpp +++ b/components/flash/znvs.hpp @@ -2,80 +2,49 @@ #include "project_configs.h" #include "sdk/os/zos.hpp" -#ifdef IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM - +#ifdef IFLYTOP_NVS_CONFIG_FLASH_SECTOR namespace iflytop { using namespace std; class ZNVS { - typedef enum { - kcfg_type_int8_t, - kcfg_type_uint8_t, - kcfg_type_int16_t, - kcfg_type_uint16_t, - kcfg_type_int32_t, - kcfg_type_uint32_t, - kcfg_type_float, - kcfg_type_bool, - } type_t; - -#pragma pack(1) typedef struct { - char key[64]; - uint8_t val[8]; - uint8_t type; - bool is_initialed; - } cfg_t; - + const char* name; + uint8_t* defaultval; + size_t len; + } cfg_index_t; +#pragma pack(1) typedef struct { uint32_t config_start; - cfg_t cfgs[IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM]; - uint32_t config_end; + + uint32_t data_size; + uint8_t data[]; } config_t; #pragma pack() - config_t m_cfg; + cfg_index_t m_index[20]; + int m_index_num = 0; + + bool inited = false; + + config_t* m_cfgcache = nullptr; + size_t m_cfgcachesize = 0; public: static ZNVS& ins(); void initialize(); - void factory_reset(); - void dumpcfg(); - void flush(); + bool alloc_config(const char* key, uint8_t* data, size_t datalen); + bool init_config(); - int8_t get_config_int8(const char* key, int8_t default_val); - void set_config_int8(const char* key, int8_t val); + bool get_config(const char* key, uint8_t* data, size_t len); + bool set_config(const char* key, uint8_t* data, size_t len); - uint8_t get_config_uint8(const char* key, uint8_t default_val); - void set_config_uint8(const char* key, uint8_t val); - - int16_t get_config_int16(const char* key, int16_t default_val); - void set_config_int16(const char* key, int16_t val); - - uint16_t get_config_uint16(const char* key, uint16_t default_val); - void set_config_uint16(const char* key, uint16_t val); - - int32_t get_config_int32(const char* key, int32_t default_val); - void set_config_int32(const char* key, int32_t val); - - uint32_t get_config_uint32(const char* key, uint32_t default_val); - void set_config_uint32(const char* key, uint32_t val); - - float get_config_float(const char* key, float default_val); - void set_config_float(const char* key, float val); - - bool get_config_bool(const char* key, bool default_val); - void set_config_bool(const char* key, bool val); + void flush(); private: - void dumpcfg(cfg_t* cfg); - - cfg_t* get_cfg(const char* key); - void allocate_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len); - cfg_t* get_and_create_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len); + void flush_all_to_configcache(); }; +#endif } // namespace iflytop -#endif \ No newline at end of file diff --git a/components/flash/znvs_bak.cpp b/components/flash/znvs_bak.cpp new file mode 100644 index 0000000..8e33044 --- /dev/null +++ b/components/flash/znvs_bak.cpp @@ -0,0 +1,167 @@ +#if 0 + +#include "znvs.hpp" + +#include + +#include "sdk\components\flash\zsimple_flash.hpp" +#define TAG "config" + +#ifdef IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM + + +using namespace iflytop; +using namespace std; +#define MARK_S 0x123456 +#define MARK_E 0x87654321 + +#define GET_CFG(typename) \ + cfg_t* cfg = get_cfg(key); \ + if (cfg == nullptr || cfg->type != kcfg_type_##typename) { \ + allocate_cfg(key, kcfg_type_##typename, (uint8_t*)&default_val, sizeof(default_val)); \ + cfg = get_cfg(key); \ + } \ + return *(typename*)cfg->val; + +#define SET_CFG(typename) \ + cfg_t* cfg = get_cfg(key); \ + if (cfg == nullptr || cfg->type != kcfg_type_##typename) { \ + allocate_cfg(key, kcfg_type_##typename, (uint8_t*)&val, sizeof(val)); \ + cfg = get_cfg(key); \ + } else { \ + *(typename*)cfg->val = val; \ + flush(); \ + } + +ZNVS& ZNVS::ins() { + static ZNVS instance; + return instance; +} +void ZNVS::initialize() { + zsimple_flash_init(IFLYTOP_NVS_CONFIG_FLASH_SECTOR); + zsimple_flash_read((uint8_t*)&m_cfg, sizeof(m_cfg)); + if (m_cfg.config_start != MARK_S || m_cfg.config_end != MARK_E) { + ZLOGW(TAG, "config uninitialized, initialize it"); + memset(&m_cfg, 0, sizeof(m_cfg)); + m_cfg.config_start = MARK_S; + m_cfg.config_end = MARK_E; + zsimple_flash_write((uint8_t*)&m_cfg, sizeof(m_cfg)); + } else { + ZLOGI(TAG, "config initialized"); + } +} + +void ZNVS::factory_reset() { + memset(&m_cfg, 0, sizeof(m_cfg)); + m_cfg.config_start = MARK_S; + m_cfg.config_end = MARK_E; + zsimple_flash_write((uint8_t*)&m_cfg, sizeof(m_cfg)); +} + +ZNVS::cfg_t* ZNVS::get_cfg(const char* key) { + for (int i = 0; i < IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM; i++) { + if (strcmp(m_cfg.cfgs[i].key, key) == 0) { + return &m_cfg.cfgs[i]; + } + } + return nullptr; +} + +void ZNVS::allocate_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len) { + cfg_t* cfg = get_cfg(key); + for (int i = 0; i < IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM; i++) { + if (m_cfg.cfgs[i].is_initialed == false) { + cfg = &m_cfg.cfgs[i]; + strcpy(cfg->key, key); + cfg->is_initialed = true; + cfg->type = (uint8_t)type; + memcpy(cfg->val, default_val, len); +#if 0 + /** + * @brief 刷新flash + * + * TODO:优化这里的代码，不要每次都刷新flash + */ + zsimple_flash_write((uint8_t*)&m_cfg, sizeof(m_cfg)); +#endif + break; + } + } +} + +ZNVS::cfg_t* ZNVS::get_and_create_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len) { + cfg_t* cfg = get_cfg(key); + if (cfg == nullptr) { + allocate_cfg(key, type, default_val, len); + cfg = get_cfg(key); + } + return cfg; +} + +void ZNVS::dumpcfg() { + ZLOGI(TAG, "=================dump nvs config =================="); + ZLOGI(TAG, "="); + for (int i = 0; i < IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM; i++) { + if (m_cfg.cfgs[i].is_initialed) { + dumpcfg(&m_cfg.cfgs[i]); + } + } + ZLOGI(TAG, "============="); +} + +void ZNVS::dumpcfg(cfg_t* cfg) { + if (!cfg->is_initialed) return; + switch (cfg->type) { + case kcfg_type_int8_t: + ZLOGI(TAG, "= %s val:%d", cfg->key, *(int8_t*)cfg->val); + break; + case kcfg_type_int16_t: + ZLOGI(TAG, "= %s val:%d", cfg->key, *(int16_t*)cfg->val); + break; + case kcfg_type_int32_t: + ZLOGI(TAG, "= %s val:%d", cfg->key, *(int32_t*)cfg->val); + break; + + case kcfg_type_uint8_t: + ZLOGI(TAG, "= %s val:%u", cfg->key, *(int8_t*)cfg->val); + break; + case kcfg_type_uint16_t: + ZLOGI(TAG, "= %s val:%u", cfg->key, *(int16_t*)cfg->val); + break; + case kcfg_type_uint32_t: + ZLOGI(TAG, "= %s val:%u", cfg->key, *(int32_t*)cfg->val); + break; + case kcfg_type_float: + ZLOGI(TAG, "= %s val:%f", cfg->key, *(float*)cfg->val); + break; + case kcfg_type_bool: + ZLOGI(TAG, "= %s val:%d", cfg->key, *(bool*)cfg->val); + break; + default: + ZLOGI(TAG, "= %s val:unknow type", cfg->key); + break; + } + return; +} + +int8_t ZNVS::get_config_int8(const char* key, int8_t default_val) { GET_CFG(int8_t); } +void ZNVS::set_config_int8(const char* key, int8_t val) { SET_CFG(int8_t); } +uint8_t ZNVS::get_config_uint8(const char* key, uint8_t default_val) { GET_CFG(uint8_t); } +void ZNVS::set_config_uint8(const char* key, uint8_t val) { SET_CFG(uint8_t); } +int16_t ZNVS::get_config_int16(const char* key, int16_t default_val) { GET_CFG(int16_t); } +void ZNVS::set_config_int16(const char* key, int16_t val) { SET_CFG(int16_t); } +uint16_t ZNVS::get_config_uint16(const char* key, uint16_t default_val) { GET_CFG(uint16_t); } +void ZNVS::set_config_uint16(const char* key, uint16_t val) { SET_CFG(uint16_t); } +int32_t ZNVS::get_config_int32(const char* key, int32_t default_val) { GET_CFG(int32_t); } +void ZNVS::set_config_int32(const char* key, int32_t val) { SET_CFG(int32_t); } +uint32_t ZNVS::get_config_uint32(const char* key, uint32_t default_val) { GET_CFG(uint32_t); } +void ZNVS::set_config_uint32(const char* key, uint32_t val) { SET_CFG(uint32_t); } +float ZNVS::get_config_float(const char* key, float default_val) { GET_CFG(float); } +void ZNVS::set_config_float(const char* key, float val) { SET_CFG(float); } +bool ZNVS::get_config_bool(const char* key, bool default_val) { GET_CFG(bool); } +void ZNVS::set_config_bool(const char* key, bool val) { SET_CFG(bool); } + +void ZNVS::flush() { zsimple_flash_write((uint8_t*)&m_cfg, sizeof(m_cfg)); } + +#endif +#endif \ No newline at end of file diff --git a/components/flash/znvs_bak.hpp b/components/flash/znvs_bak.hpp new file mode 100644 index 0000000..79f57be --- /dev/null +++ b/components/flash/znvs_bak.hpp @@ -0,0 +1,83 @@ +#pragma once +#if 0 +#include "project_configs.h" +#include "sdk/os/zos.hpp" + +#ifdef IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM + +namespace iflytop { +using namespace std; + +class ZNVS { + typedef enum { + kcfg_type_int8_t, + kcfg_type_uint8_t, + kcfg_type_int16_t, + kcfg_type_uint16_t, + kcfg_type_int32_t, + kcfg_type_uint32_t, + kcfg_type_float, + kcfg_type_bool, + } type_t; + +#pragma pack(1) + typedef struct { + char key[64]; + uint8_t val[8]; + uint8_t type; + bool is_initialed; + } cfg_t; + + typedef struct { + uint32_t config_start; + cfg_t cfgs[IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM]; + uint32_t config_end; + } config_t; +#pragma pack() + + config_t m_cfg; + + public: + static ZNVS& ins(); + + void initialize(); + void factory_reset(); + void dumpcfg(); + + void flush(); + + int8_t get_config_int8(const char* key, int8_t default_val); + void set_config_int8(const char* key, int8_t val); + + uint8_t get_config_uint8(const char* key, uint8_t default_val); + void set_config_uint8(const char* key, uint8_t val); + + int16_t get_config_int16(const char* key, int16_t default_val); + void set_config_int16(const char* key, int16_t val); + + uint16_t get_config_uint16(const char* key, uint16_t default_val); + void set_config_uint16(const char* key, uint16_t val); + + int32_t get_config_int32(const char* key, int32_t default_val); + void set_config_int32(const char* key, int32_t val); + + uint32_t get_config_uint32(const char* key, uint32_t default_val); + void set_config_uint32(const char* key, uint32_t val); + + float get_config_float(const char* key, float default_val); + void set_config_float(const char* key, float val); + + bool get_config_bool(const char* key, bool default_val); + void set_config_bool(const char* key, bool val); + + private: + void dumpcfg(cfg_t* cfg); + + cfg_t* get_cfg(const char* key); + void allocate_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len); + cfg_t* get_and_create_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len); +}; + +} // namespace iflytop +#endif +#endif \ No newline at end of file diff --git a/components/scriptcmder_module/xy_robot_script_cmder_module.cpp b/components/scriptcmder_module/xy_robot_script_cmder_module.cpp index 66e7aea..129454e 100644 --- a/components/scriptcmder_module/xy_robot_script_cmder_module.cpp +++ b/components/scriptcmder_module/xy_robot_script_cmder_module.cpp @@ -112,16 +112,17 @@ void XYRobotScriptCmderModule::regcmd() { // }); m_cmdScheduler->registerCmd("xy_robot_ctrl_move_by", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - SCRIPT_CMDER_CHECK(argc == 4); + SCRIPT_CMDER_CHECK(argc == 5); int32_t id = atoi(argv[1]); int32_t dx = atoi(argv[2]); int32_t dy = atoi(argv[3]); + int32_t v = atoi(argv[4]); I_XYRobotCtrlModule* module = findXYRobot(id); SCRIPT_CMDER_CHECK(module != nullptr); - int32_t ack = module->move_by(dx, dy, [this](int32_t status) { ZLOGI(TAG, "move_by status:%d", status); }); + int32_t ack = module->move_by(dx, dy, v, [this](int32_t status) { ZLOGI(TAG, "move_by status:%d", status); }); ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); }); @@ -296,4 +297,41 @@ void XYRobotScriptCmderModule::regcmd() { // ZLOGI(TAG, " run_to_zero_dec :%d", status.run_to_zero_dec); } }); + + m_cmdScheduler->registerCmd("xy_robot_ctrl_flsuh", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 2); + + int32_t id = atoi(argv[1]); + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->flush(); + ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); + }); + + m_cmdScheduler->registerCmd("xy_robot_ctrl_factory_reset", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 2); + + int32_t id = atoi(argv[1]); + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->factory_reset(); + ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); + }); + + m_cmdScheduler->registerCmd("xy_robot_ctrl_move_by_no_limit", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 5); + + int32_t id = atoi(argv[1]); + int32_t dx = atoi(argv[2]); + int32_t dy = atoi(argv[3]); + int32_t v = atoi(argv[4]); + + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->move_by_no_limit(dx, dy, v, [this](int32_t status) { ZLOGI(TAG, "move_by status:%d", status); }); + ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); + }); } \ No newline at end of file diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp index 49fd8cd..3a65b82 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -3,13 +3,14 @@ #include #include "sdk/components/errorcode/errorcode.hpp" +#include "sdk\components\flash\znvs.hpp" using namespace iflytop; using namespace std; #define TAG "XYRobotCtrlModule" void XYRobotCtrlModule::initialize(IStepperMotor* stepM1, // IStepperMotor* stepM2, // ZGPIO* Xgpio, // - ZGPIO* Ygpio) { + ZGPIO* Ygpio, const char* flashmark) { m_stepM1 = stepM1; m_stepM2 = stepM2; m_Xgpio = Xgpio; @@ -20,9 +21,15 @@ void XYRobotCtrlModule::initialize(IStepperMotor* stepM1, // ZASSERT(m_Ygpio); m_lock.init(); - create_default_cfg(m_cfg); - set_base_param(m_cfg); - // m_statu_lock.init(); + m_flashmark = flashmark; + // base_param_t cfg; + + if (m_flashmark) { + ZASSERT(ZNVS::ins().get_config(m_flashmark, (uint8_t*)&m_cfg, sizeof(m_cfg))); + } else { + create_default_cfg(m_cfg); + } + active_cfg(); m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal); } @@ -37,12 +44,21 @@ int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, int speed, action_cb_st if (m_cfg.maxspeed != 0 && speed > m_cfg.maxspeed) { runspeed = m_cfg.maxspeed; } + if (m_cfg.min_x != 0 && x < m_cfg.min_x) { + x = m_cfg.min_x; + } + if (m_cfg.max_x != 0 && x > m_cfg.max_x) { + x = m_cfg.max_x; + } + if (m_cfg.min_y != 0 && y < m_cfg.min_y) { + y = m_cfg.min_y; + } + if (m_cfg.max_y != 0 && y > m_cfg.max_y) { + y = m_cfg.max_y; + } // int32_t m1pos, m2pos; m_thread.stop(); - /** - * @TODO:添加限位检查 - */ m_thread.start([this, x, y, runspeed, status_cb]() { _motor_move_to(x, y, runspeed, m_cfg.acc, m_cfg.dec); while (!_motor_is_reach_target()) { @@ -57,21 +73,73 @@ int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, int speed, action_cb_st return 0; } -int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, action_cb_status_t status_cb) { +int32_t XYRobotCtrlModule::move_by_no_limit(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "move_by_no_limit dx:%d dy:%d", dx, dy); + + m_thread.stop(); + _motor_stop(); + + if (m_cfg.maxspeed != 0 && speed > m_cfg.maxspeed) { + speed = m_cfg.maxspeed; + } + int32_t s_x, s_y, to_x, to_y; + getnowpos(s_x, s_y); + to_x = s_x + dx; + to_y = s_y + dy; + + m_thread.start([this, to_x, to_y, speed, status_cb]() { + _motor_move_to(to_x, to_y, speed, m_cfg.acc, m_cfg.dec); + + while (!_motor_is_reach_target()) { + if (m_thread.getExitFlag()) break; + vTaskDelay(10); + } + + _motor_stop(); + int32_t end_x, end_y; + getnowpos(end_x, end_y); + + call_status_cb(status_cb, 0); + }); + return 0; +}; + +int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) { zlock_guard lock(m_lock); ZLOGI(TAG, "move_by dx:%d dy:%d", dx, dy); m_thread.stop(); _motor_stop(); - /** - * @TODO:添加限位检查 - */ - m_thread.start([this, dx, dy, status_cb]() { - int32_t s_x, s_y, to_x, to_y; - getnowpos(s_x, s_y); - to_x = s_x + dx; - to_y = s_y + dy; - _motor_move_to(to_x, to_y, m_cfg.maxspeed, m_cfg.acc, m_cfg.dec); + + if (m_cfg.maxspeed != 0 && speed > m_cfg.maxspeed) { + speed = m_cfg.maxspeed; + } + int32_t s_x, s_y, to_x, to_y; + getnowpos(s_x, s_y); + to_x = s_x + dx; + to_y = s_y + dy; + + if (m_cfg.min_x != 0 && to_x < m_cfg.min_x) { + to_x = m_cfg.min_x; + } + if (m_cfg.max_x != 0 && to_x > m_cfg.max_x) { + to_x = m_cfg.max_x; + } + if (m_cfg.min_y != 0 && to_y < m_cfg.min_y) { + to_y = m_cfg.min_y; + } + if (m_cfg.max_y != 0 && to_y > m_cfg.max_y) { + to_y = m_cfg.max_y; + } + + if (to_x == s_x && to_y == s_y) { + call_status_cb(status_cb, 0); + return 0; + } + + m_thread.start([this, to_x, to_y, speed, status_cb]() { + _motor_move_to(to_x, to_y, speed, m_cfg.acc, m_cfg.dec); while (!_motor_is_reach_target()) { if (m_thread.getExitFlag()) break; @@ -208,7 +276,7 @@ int32_t XYRobotCtrlModule::read_detailed_status(detailed_status_t& debug_info) { getnowpos(debug_info.x, debug_info.y); return 0; } -void XYRobotCtrlModule::create_default_cfg(base_param_t& cfg) { +void XYRobotCtrlModule::create_default_cfg(flash_config_t& cfg) { memset(&cfg, 0, sizeof(cfg)); cfg.robot_type = corexy; cfg.x_shaft = 0; @@ -237,18 +305,64 @@ void XYRobotCtrlModule::create_default_cfg(base_param_t& cfg) { return; } -int32_t XYRobotCtrlModule::set_base_param(const base_param_t& param) { +int32_t XYRobotCtrlModule::set_base_param(const base_param_t& inparam) { zlock_guard lock(m_lock); - m_cfg = param; + + m_cfg.robot_type = inparam.robot_type; + m_cfg.x_shaft = inparam.x_shaft; + m_cfg.y_shaft = inparam.y_shaft; + m_cfg.ihold = inparam.ihold; + m_cfg.irun = inparam.irun; + m_cfg.iholddelay = inparam.iholddelay; + m_cfg.distance_scale = inparam.distance_scale; + m_cfg.shift_x = inparam.shift_x; + m_cfg.shift_y = inparam.shift_y; + m_cfg.acc = inparam.acc; + m_cfg.dec = inparam.dec; + m_cfg.breakdec = inparam.breakdec; + m_cfg.maxspeed = inparam.maxspeed; + m_cfg.min_x = inparam.min_x; + m_cfg.max_x = inparam.max_x; + m_cfg.min_y = inparam.min_y; + m_cfg.max_y = inparam.max_y; + m_cfg.run_to_zero_max_d = inparam.run_to_zero_max_d; + m_cfg.leave_from_zero_max_d = inparam.leave_from_zero_max_d; + m_cfg.run_to_zero_speed = inparam.run_to_zero_speed; + m_cfg.run_to_zero_dec = inparam.run_to_zero_dec; + active_cfg(); + return 0; +}; + +void XYRobotCtrlModule::active_cfg() { m_stepM1->setScale(m_cfg.distance_scale / 1000.0); m_stepM2->setScale(m_cfg.distance_scale / 1000.0); m_stepM1->setIHOLD_IRUN(m_cfg.ihold, m_cfg.irun, m_cfg.iholddelay); m_stepM2->setIHOLD_IRUN(m_cfg.ihold, m_cfg.irun, m_cfg.iholddelay); - return 0; -}; +} + int32_t XYRobotCtrlModule::get_base_param(base_param_t& ack) { zlock_guard lock(m_lock); - ack = m_cfg; + ack.robot_type = m_cfg.robot_type; + ack.x_shaft = m_cfg.x_shaft; + ack.y_shaft = m_cfg.y_shaft; + ack.ihold = m_cfg.ihold; + ack.irun = m_cfg.irun; + ack.iholddelay = m_cfg.iholddelay; + ack.distance_scale = m_cfg.distance_scale; + ack.shift_x = m_cfg.shift_x; + ack.shift_y = m_cfg.shift_y; + ack.acc = m_cfg.acc; + ack.dec = m_cfg.dec; + ack.breakdec = m_cfg.breakdec; + ack.maxspeed = m_cfg.maxspeed; + ack.min_x = m_cfg.min_x; + ack.max_x = m_cfg.max_x; + ack.min_y = m_cfg.min_y; + ack.max_y = m_cfg.max_y; + ack.run_to_zero_max_d = m_cfg.run_to_zero_max_d; + ack.leave_from_zero_max_d = m_cfg.leave_from_zero_max_d; + ack.run_to_zero_speed = m_cfg.run_to_zero_speed; + ack.run_to_zero_dec = m_cfg.run_to_zero_dec; return 0; }; @@ -456,7 +570,47 @@ void XYRobotCtrlModule::forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, bool XYRobotCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTarget() && m_stepM2->isReachTarget(); } void XYRobotCtrlModule::call_status_cb(action_cb_status_t cb, int32_t status) { - if (cb) { - cb(status); - } + if (cb) cb(status); +} +void XYRobotCtrlModule::dumpcfg() { + ZLOGI(TAG, "robot_type :%d", m_cfg.robot_type); + ZLOGI(TAG, "x_shaft :%d", m_cfg.x_shaft); + ZLOGI(TAG, "y_shaft :%d", m_cfg.y_shaft); + ZLOGI(TAG, "ihold :%d", m_cfg.ihold); + ZLOGI(TAG, "irun :%d", m_cfg.irun); + ZLOGI(TAG, "iholddelay :%d", m_cfg.iholddelay); + ZLOGI(TAG, "distance_scale :%d", m_cfg.distance_scale); + ZLOGI(TAG, "shift_x :%d", m_cfg.shift_x); + ZLOGI(TAG, "shift_y :%d", m_cfg.shift_y); + ZLOGI(TAG, "acc :%d", m_cfg.acc); + ZLOGI(TAG, "dec :%d", m_cfg.dec); + ZLOGI(TAG, "breakdec :%d", m_cfg.breakdec); + ZLOGI(TAG, "maxspeed :%d", m_cfg.maxspeed); + ZLOGI(TAG, "min_x :%d", m_cfg.min_x); + ZLOGI(TAG, "max_x :%d", m_cfg.max_x); + ZLOGI(TAG, "min_y :%d", m_cfg.min_y); + ZLOGI(TAG, "max_y :%d", m_cfg.max_y); + ZLOGI(TAG, "run_to_zero_max_d :%d", m_cfg.run_to_zero_max_d); + ZLOGI(TAG, "leave_from_zero_max_d :%d", m_cfg.leave_from_zero_max_d); + ZLOGI(TAG, "run_to_zero_speed :%d", m_cfg.run_to_zero_speed); + ZLOGI(TAG, "run_to_zero_dec :%d", m_cfg.run_to_zero_dec); } + +int32_t XYRobotCtrlModule::flush() { // + ZLOGI(TAG, "flush"); + if (m_flashmark) { + ZNVS::ins().set_config(m_flashmark, (uint8_t*)&m_cfg, sizeof(m_cfg)); + ZNVS::ins().flush(); + } + return 0; +}; +int32_t XYRobotCtrlModule::factory_reset() { // + ZLOGI(TAG, "factory_reset"); + create_default_cfg(m_cfg); + if (m_flashmark) { + ZNVS::ins().set_config(m_flashmark, (uint8_t*)&m_cfg, sizeof(m_cfg)); + ZNVS::ins().flush(); + } + active_cfg(); + return 0; +}; \ No newline at end of file diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp index 43912a3..4a00c48 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp @@ -22,22 +22,27 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule { // uint8_t m_status = 0; - base_param_t m_cfg; + // base_param_t m_cfg; int32_t m_zero_shift_x = 0; int32_t m_zero_shift_y = 0; + const char* m_flashmark = nullptr; + flash_config_t m_cfg; + zmutex m_lock; // zmutex m_statu_lock; public: void initialize(IStepperMotor* stepM1, // - IStepperMotor* stepM2, // - ZGPIO* Xgpio, // - ZGPIO* Ygpio); + IStepperMotor* stepM2, // + ZGPIO* Xgpio, // + ZGPIO* Ygpio, const char* flashmark); virtual int32_t move_to(int32_t x, int32_t y, int speed, action_cb_status_t status_cb) override; - virtual int32_t move_by(int32_t dx, int32_t dy, action_cb_status_t status_cb) override; + virtual int32_t move_by(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) override; + virtual int32_t move_by_no_limit(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) override; + virtual int32_t move_to_zero(action_cb_status_t status_cb) override; virtual int32_t move_to_zero_with_calibrate(int32_t x, int32_t y, action_cb_status_t status_cb) override; virtual int32_t enable(bool venable) override; @@ -48,14 +53,18 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule { virtual int32_t read_status(status_t& status) override; virtual int32_t read_detailed_status(detailed_status_t& debug_info) override; - void create_default_cfg(base_param_t& ack); + static void create_default_cfg(flash_config_t& cfg); virtual int32_t set_base_param(const base_param_t& param) override; virtual int32_t get_base_param(base_param_t& ack) override; + virtual int32_t flush() override; + virtual int32_t factory_reset() override; void loop(); + void dumpcfg(); private: + void active_cfg(); void computeTargetMotorPos(); void getnowpos(int32_t& x, int32_t& y); diff --git a/components/zcancmder_master_module/zcan_xy_robot_master_module.cpp b/components/zcancmder_master_module/zcan_xy_robot_master_module.cpp index fb9909d..5b66ece 100644 --- a/components/zcancmder_master_module/zcan_xy_robot_master_module.cpp +++ b/components/zcancmder_master_module/zcan_xy_robot_master_module.cpp @@ -33,8 +33,11 @@ class ZCANXYRobotCtrlMasterModule : public I_XYRobotCtrlModule { ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_xy_robot_ctrl_move_to, OVERTIME, cmd->x = x; cmd->y = y; cmd->speed = speed;); } - virtual int32_t move_by(int32_t dx, int32_t dy, action_cb_status_t status_cb) override { // - ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_xy_robot_ctrl_move_by, OVERTIME, cmd->dx = dx; cmd->dy = dy;); + virtual int32_t move_by(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) override { // + ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_xy_robot_ctrl_move_by, OVERTIME, cmd->dx = dx; cmd->dy = dy; cmd->speed = speed;); + } + virtual int32_t move_by_no_limit(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) override { // + ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_xy_robot_ctrl_move_by_no_limit, OVERTIME, cmd->dx = dx; cmd->dy = dy; cmd->speed = speed;); } virtual int32_t move_to_zero(action_cb_status_t status_cb) override { @@ -79,6 +82,12 @@ class ZCANXYRobotCtrlMasterModule : public I_XYRobotCtrlModule { virtual int32_t get_base_param(base_param_t& ack) override { // ZCAN_SEND_CMD(kcmd_xy_robot_ctrl_get_base_param, ack, OVERTIME); } + virtual int32_t flush() override { // + ZCAN_SEND_CMD_NO_ACK(kcmd_xy_robot_ctrl_flush, OVERTIME); + } + virtual int32_t factory_reset() override { // + ZCAN_SEND_CMD_NO_ACK(kcmd_xy_robot_ctrl_factory_reset, OVERTIME); + } }; I_XYRobotCtrlModule* create_xyrobot_ctrl_module(ZCanCommnaderMaster* cancmder, int id) { diff --git a/components/zcancmder_module/zcan_xy_robot_module.cpp b/components/zcancmder_module/zcan_xy_robot_module.cpp index a29392e..7ec3e58 100644 --- a/components/zcancmder_module/zcan_xy_robot_module.cpp +++ b/components/zcancmder_module/zcan_xy_robot_module.cpp @@ -54,7 +54,18 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { END_PP(); PROCESS_PACKET(kcmd_xy_robot_ctrl_move_by, m_id) { // - errorcode = m_xyRobotCtrlModule->move_by(cmd->dx, cmd->dy, [this, cmdheader](int32_t status) { + errorcode = m_xyRobotCtrlModule->move_by(cmd->dx, cmd->dy, cmd->speed, [this, cmdheader](int32_t status) { + osDelay(5); // 用来保证回执消息在前面 + kcmd_xy_robot_ctrl_move_by_report_t report = {0}; + ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_by exec_status:%d", status); + report.exec_status = status; + m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); + }); + } + END_PP(); + + PROCESS_PACKET(kcmd_xy_robot_ctrl_move_by_no_limit, m_id) { // + errorcode = m_xyRobotCtrlModule->move_by(cmd->dx, cmd->dy, cmd->speed, [this, cmdheader](int32_t status) { osDelay(5); // 用来保证回执消息在前面 kcmd_xy_robot_ctrl_move_by_report_t report = {0}; ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_by exec_status:%d", status); @@ -93,6 +104,14 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { errorcode = m_xyRobotCtrlModule->get_base_param(ack->ack); } END_PP(); + PROCESS_PACKET(kcmd_xy_robot_ctrl_flush, m_id) { // + errorcode = m_xyRobotCtrlModule->flush(); + } + END_PP(); + PROCESS_PACKET(kcmd_xy_robot_ctrl_factory_reset, m_id) { // + errorcode = m_xyRobotCtrlModule->factory_reset(); + } + END_PP(); // PROCESS_PACKET(kcmd_xy_robot_ctrl_set_warning_limit_param, m_id) { // // errorcode = m_xyRobotCtrlModule->set_warning_limit_param(cmd->opt_type, cmd->param, ack->ack); diff --git a/components/zprotocols/zcancmder b/components/zprotocols/zcancmder index 59be09e..9199515 160000 --- a/components/zprotocols/zcancmder +++ b/components/zprotocols/zcancmder @@ -1 +1 @@ -Subproject commit 59be09e6b8a14de2e702e376210957783bfbf42e +Subproject commit 9199515dfc8db3a0760429d918e96cf2d8d31f32