diff --git a/servo_controller/prj.conf b/servo_controller/prj.conf index e704802..d79b853 100644 --- a/servo_controller/prj.conf +++ b/servo_controller/prj.conf @@ -7,7 +7,7 @@ CONFIG_I2C=y CONFIG_SENSOR=y CONFIG_INA3221=y -# Mux +# MUX CONFIG_CD74HC4067=y # Serial @@ -34,6 +34,11 @@ CONFIG_USBD_CDC_ACM_LOG_LEVEL_OFF=y # This removes a pointless warning CONFIG_LOG_DEFAULT_LEVEL=3 CONFIG_LOG_MODE_IMMEDIATE=y +# SETTINGS +CONFIG_FLASH=y +CONFIG_FLASH_MAP=y +CONFIG_NVS=y + # DEBUG CONFIG_DEBUG_THREAD_INFO=y # CONFIG_DEBUG=y diff --git a/servo_controller/scripts/send_cmd.py b/servo_controller/scripts/send_cmd.py index 14865e1..b2c075d 100644 --- a/servo_controller/scripts/send_cmd.py +++ b/servo_controller/scripts/send_cmd.py @@ -48,9 +48,11 @@ COMMANDS = { "DIGITAL_LEG_OUT": 12, "POWER_MONITOR_SET_READ": 13, "POWER_MONITOR_READ": 14, - "MOVE_COMMAND_START": 15, - "TEST": 16, - "MOVE_COMMAND_END": 17, + "SETTINGS_TEST_SAVE": 15, + "SETTINGS_TEST_READ": 16, + "MOVE_COMMAND_START": 17, + "TEST": 18, + "MOVE_COMMAND_END": 19, } COMMAND_NAMES = {v: k for k, v in COMMANDS.items()} @@ -63,6 +65,7 @@ COMMANDS_DATA_OPTIONAL = { "LED_TOGGLE", "ADC_READ_RAW_ALL", "ADC_READ_ALL", + "SETTINGS_TEST_READ", "TEST", } diff --git a/servo_controller/src/command_handler.c b/servo_controller/src/command_handler.c index 93838c7..3224017 100644 --- a/servo_controller/src/command_handler.c +++ b/servo_controller/src/command_handler.c @@ -6,6 +6,7 @@ #include "usb.h" #include "digital_out.h" #include "power_monitor.h" +#include "settings.h" #include @@ -153,6 +154,25 @@ int command_handler_rx(struct command_message_t *msg) { break; } + case SETTINGS_TEST_SAVE: { + uint32_t test; + memcpy(&test, &msg->data[0], sizeof(uint32_t)); + + settings_save_test(test); + + break; + } + + case SETTINGS_TEST_READ: { + uint32_t test = settings_read_test(); + + struct command_message_t *reply = cmd_get_next_tx_buf(); + command_create_message(reply, sizeof(test), msg->command, (uint8_t *)&test); + command_handler_tx(reply); + + break; + } + default: { LOG_WRN("Unknown command received: %d", msg->command); return -EINVAL; diff --git a/servo_controller/src/command_message.h b/servo_controller/src/command_message.h index a9e4184..1142953 100644 --- a/servo_controller/src/command_message.h +++ b/servo_controller/src/command_message.h @@ -26,6 +26,8 @@ typedef enum { DIGITAL_LEG_OUT, POWER_MONITOR_SET_READ, POWER_MONITOR_READ, + SETTINGS_TEST_SAVE, + SETTINGS_TEST_READ, MOVE_COMMAND_START, // All the commands going to robot thread go here diff --git a/servo_controller/src/main.c b/servo_controller/src/main.c index fcbacd3..44cf153 100644 --- a/servo_controller/src/main.c +++ b/servo_controller/src/main.c @@ -7,6 +7,7 @@ #include "power_monitor.h" #include "command_handler.h" #include "robot.h" +#include "settings.h" #include LOG_MODULE_REGISTER(main, LOG_LEVEL_INF); @@ -15,6 +16,13 @@ LOG_MODULE_REGISTER(main, LOG_LEVEL_INF); int main(void) { int ret; + // SETTINGS init + ret = settings_init(); + if (ret != 0) { + LOG_ERR("Failed to enable SETTINGS"); + return 0; + } + // COMMAND HANDLER init ret = command_handler_init(); if (ret != 0) { diff --git a/servo_controller/src/settings.c b/servo_controller/src/settings.c new file mode 100644 index 0000000..78f50e7 --- /dev/null +++ b/servo_controller/src/settings.c @@ -0,0 +1,112 @@ +#include "settings.h" + +#include +#include +#include +#include +#include + +LOG_MODULE_REGISTER(settings, LOG_LEVEL_INF); + + +// NVS +#define SETTINGS_NVS_ID 1 +static struct nvs_fs fs; +#define SETTINGS_PARTITION settings_partition + +// SETTINGS +struct settings_t robot_settings; + + +static void settings_set_defaults() { + robot_settings.header.magic = SETTINGS_MAGIC; + robot_settings.header.version = SETTINGS_VERSION; + robot_settings.header.crc = 0; + robot_settings.test = 123; +} + +int settings_save() { + ssize_t ret; + + robot_settings.header.magic = SETTINGS_MAGIC; + robot_settings.header.version = SETTINGS_VERSION; + + // TODO: calculate the crc + + ret = nvs_write(&fs, SETTINGS_NVS_ID, &robot_settings, sizeof(robot_settings)); + + if (ret < 0) { + LOG_ERR("nvs_write failed: %d", ret); + return ret; + } + + return 0; +} + +int settings_load() { + ssize_t ret; + + ret = nvs_read(&fs, SETTINGS_NVS_ID, &robot_settings, sizeof(robot_settings)); + + if (ret == sizeof(robot_settings)) { + if (robot_settings.header.magic != SETTINGS_MAGIC) { + LOG_WRN("Invalid settings magic"); + settings_set_defaults(); + settings_save(); + } + + if (robot_settings.header.version != SETTINGS_VERSION) { + LOG_WRN("Unsupported settings version: %u", robot_settings.header.version); + settings_set_defaults(); + settings_save(); + } + + // TODO: check crc + + return 0; + } + + return ret; +} + +int settings_init() { + int ret; + struct flash_pages_info info; + + fs.flash_device = PARTITION_DEVICE(SETTINGS_PARTITION); + if (!device_is_ready(fs.flash_device)) { + printk("Flash device %s is not ready\n", fs.flash_device->name); + return 0; + } + + fs.offset = PARTITION_OFFSET(SETTINGS_PARTITION); + ret = flash_get_page_info_by_offs(fs.flash_device, fs.offset, &info); + if (ret != 0) { + printk("Unable to get page info, ret=%d\n", ret); + return 0; + } + fs.sector_size = info.size; + // TODO: calculate sector count + fs.sector_count = 3U; + + ret = nvs_mount(&fs); + if (ret != 0) { + LOG_ERR("nvs_mount failed: %d", ret); + return ret; + } + + LOG_INF("NVS initialized"); + + settings_load(); + + return 0; +} + +uint32_t settings_read_test() { + return robot_settings.test; +} + +int settings_save_test(uint32_t new_test) { + robot_settings.test = new_test; + return settings_save(); +} \ No newline at end of file diff --git a/servo_controller/src/settings.h b/servo_controller/src/settings.h new file mode 100644 index 0000000..e7abd2a --- /dev/null +++ b/servo_controller/src/settings.h @@ -0,0 +1,28 @@ +#ifndef SETTINGS_H +#define SETTINGS_H + + +#include + + +#define SETTINGS_MAGIC 0x53484954 +#define SETTINGS_VERSION 0x01 + +struct settings_header_t { + uint32_t magic; + uint8_t version; + uint16_t crc; +} __attribute__((packed)); + +struct settings_t { + struct settings_header_t header; + uint32_t test; +} __attribute__((packed)); + + +int settings_init(void); +uint32_t settings_read_test(); +int settings_save_test(uint32_t new_test); + + +#endif // SETTINGS_H diff --git a/servo_controller/src/usb.c b/servo_controller/src/usb.c index ba87628..9695b54 100644 --- a/servo_controller/src/usb.c +++ b/servo_controller/src/usb.c @@ -112,6 +112,7 @@ static void usb_rx_thread(void *p1, void *p2, void *p3) { if ((len == (COMMAND_HEADER_SIZE - 1)) && (buf_header[1] == COMMAND_ID) && (buf_header[0] <= COMMAND_DATA_SIZE)) { uint8_t is_move = 0; if (buf_header[2] > MOVE_COMMAND_START && buf_header[2] < MOVE_COMMAND_END) { + // Use robot thread buffers for move commands msg = robot_get_next_tx_buf(); is_move = 1; }