Added first implementation of NVM settings

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

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",
 }
 

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 <zephyr/logging/log.h>
 
@@ -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;

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

servo_controller/src/main.c

@@ -7,6 +7,7 @@
 #include "power_monitor.h"
 #include "command_handler.h"
 #include "robot.h"
+#include "settings.h"
 
 #include <zephyr/logging/log.h>
 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) {

servo_controller/src/settings.c

@@ -0,0 +1,112 @@
+#include "settings.h"
+
+#include <zephyr/kernel.h>
+#include <zephyr/drivers/flash.h>
+#include <zephyr/storage/flash_map.h>
+#include <zephyr/kvss/nvs.h>
+#include <zephyr/logging/log.h>
+
+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();
+}

servo_controller/src/settings.h

@@ -0,0 +1,28 @@
+#ifndef SETTINGS_H
+#define SETTINGS_H
+
+
+#include <stdint.h>
+
+
+#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

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;
 					}