Implement EEPROM position storage for focuser state persistence

Author: andre-stefanov

app/CMakeLists.txt

@@ -12,6 +12,7 @@ project(app LANGUAGES C CXX)
 target_sources(app PRIVATE
 	src/main.cpp
 	src/Focuser.cpp
+	src/EepromPositionStore.cpp
 	src/FocuserThread.cpp
 	src/UartHandler.cpp
 	src/UartThread.cpp

app/boards/esp32s3_devkitc_procpu.overlay

@@ -12,6 +12,10 @@
  */
 
 / {
+	aliases {
+		eeprom-0 = &eeprom0;
+	};
+
 	chosen {
 		zephyr,console = &uart1;
 		zephyr,log-uart = &focuser_log_uarts;
@@ -20,6 +24,17 @@
 		focuser,stepper-drv = &focuser_stepper_drv;
 	};
 
+	eeprom0: eeprom@0 {
+		compatible = "zephyr,emu-eeprom";
+		label = "focuser-eeprom";
+		reg = <0x0 0x20>;
+		size = <32>;
+		pagesize = <0x1000>;
+		partition = <&storage_partition>;
+		rambuf;
+		status = "okay";
+	};
+
 	focuser_log_uarts: focuser_log_uarts {
 		compatible = "zephyr,log-uart";
 		uarts = <&uart1>;

app/prj.conf

@@ -21,6 +21,9 @@ CONFIG_UART_CONSOLE=y
 # Enable Logging (LOG_INF, LOG_ERR)
 CONFIG_LOG=y
 
+# Persist focuser state in flash-backed EEPROM.
+CONFIG_EEPROM=y
+
 # Set the application log level.
 CONFIG_APP_LOG_LEVEL_INF=y
 
@@ -32,3 +35,6 @@ CONFIG_STEPPER_ADI_TMC2209=y
 
 # Grow the main thread stack to accommodate the C++ runtime initialisation.
 CONFIG_MAIN_STACK_SIZE=3072
+
+# Enable MCUBoot bootloader support.
+CONFIG_BOOTLOADER_MCUBOOT=y

app/src/EepromPositionStore.cpp

@@ -0,0 +1,126 @@
+#include "EepromPositionStore.hpp"
+
+#include <zephyr/device.h>
+#include <zephyr/devicetree.h>
+#include <zephyr/drivers/eeprom.h>
+#include <zephyr/logging/log.h>
+#include <zephyr/sys/util.h>
+
+LOG_MODULE_REGISTER(position_store, CONFIG_APP_LOG_LEVEL);
+
+namespace
+{
+#if DT_HAS_ALIAS(eeprom_0)
+#define FOCUSER_EEPROM_NODE DT_ALIAS(eeprom_0)
+#endif
+
+#ifdef FOCUSER_EEPROM_NODE
+const struct device *const k_eeprom = DEVICE_DT_GET(FOCUSER_EEPROM_NODE);
+#endif
+
+constexpr uint32_t kPositionMagic = 0x464F4350U; // "FOCP"
+constexpr off_t kPositionOffset = 0;
+
+} // namespace
+
+EepromPositionStore::EepromPositionStore()
+{
+}
+
+bool EepromPositionStore::load(uint16_t &position_out)
+{
+#ifndef FOCUSER_EEPROM_NODE
+	ARG_UNUSED(position_out);
+	return false;
+#else
+	if (!ensure_ready())
+	{
+		return false;
+	}
+
+	PositionRecord record{};
+	const int ret = eeprom_read(k_eeprom, kPositionOffset, &record, sizeof(record));
+	if (ret != 0)
+	{
+		LOG_WRN("Failed to read position from EEPROM (%d)", ret);
+		return false;
+	}
+
+	if ((record.magic != kPositionMagic) || (record.checksum != checksum(record.position)))
+	{
+		return false;
+	}
+
+	position_out = record.position;
+	m_last_value = record.position;
+	m_has_value = true;
+	return true;
+#endif
+}
+
+void EepromPositionStore::save(uint16_t position)
+{
+#ifndef FOCUSER_EEPROM_NODE
+	ARG_UNUSED(position);
+	return;
+#else
+	if (m_has_value && (position == m_last_value))
+	{
+		return;
+	}
+
+	if (!ensure_ready())
+	{
+		return;
+	}
+
+	PositionRecord record{
+		.magic = kPositionMagic,
+		.position = position,
+		.checksum = checksum(position),
+	};
+
+	const int ret = eeprom_write(k_eeprom, kPositionOffset, &record, sizeof(record));
+	if (ret != 0)
+	{
+		LOG_ERR("Failed to save position to EEPROM (%d)", ret);
+		return;
+	}
+
+	m_last_value = position;
+	m_has_value = true;
+	LOG_DBG("Saved focuser position 0x%04x (%u) to EEPROM", position, position);
+#endif
+}
+
+bool EepromPositionStore::ensure_ready()
+{
+#ifndef FOCUSER_EEPROM_NODE
+	return false;
+#else
+	if (!m_ready)
+	{
+		if (!device_is_ready(k_eeprom))
+		{
+			LOG_WRN("EEPROM device not ready");
+			return false;
+		}
+
+		m_eeprom_size = eeprom_get_size(k_eeprom);
+		if (m_eeprom_size < sizeof(PositionRecord))
+		{
+			LOG_ERR("EEPROM too small (%zu < %zu)", m_eeprom_size, sizeof(PositionRecord));
+			return false;
+		}
+
+		m_ready = true;
+	}
+
+	return true;
+#endif
+}
+
+uint16_t EepromPositionStore::checksum(uint16_t position) const
+{
+	return static_cast<uint16_t>(((kPositionMagic >> 16) ^ (kPositionMagic & 0xFFFFU) ^ position) & 0xFFFFU);
+}

app/src/EepromPositionStore.hpp

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <cstddef>
+#include <cstdint>
+
+#include "PositionStore.hpp"
+
+class EepromPositionStore final : public PositionStore
+{
+public:
+	EepromPositionStore();
+
+	bool load(uint16_t &position_out) override;
+	void save(uint16_t position) override;
+
+private:
+	struct PositionRecord
+	{
+		uint32_t magic;
+		uint16_t position;
+		uint16_t checksum;
+	};
+
+	bool ensure_ready();
+	uint16_t checksum(uint16_t position) const;
+
+	bool m_ready{false};
+	bool m_has_value{false};
+	uint16_t m_last_value{0U};
+	size_t m_eeprom_size{0U};
+};

app/src/Focuser.cpp

@@ -22,8 +22,8 @@ namespace
 
 } // namespace
 
-Focuser::Focuser(FocuserStepper &stepper, const char *firmware_version)
-	: m_firmware_version(firmware_version), m_stepper(stepper)
+Focuser::Focuser(FocuserStepper &stepper, PositionStore *store, const char *firmware_version)
+	: m_firmware_version(firmware_version), m_stepper(stepper), m_store(store)
 {
 	(void)set_stepper_driver_enabled(true);
 }
@@ -50,6 +50,8 @@ int Focuser::initialise()
 		return ret;
 	}
 
+	restore_position();
+
 	uint64_t interval_ns = 0;
 	{
 		MutexLock lock(m_state.lock);
@@ -139,6 +141,7 @@ void Focuser::stop()
 	(void)m_stepper.stop();
 	(void)set_stepper_driver_enabled(false);
 	k_sem_give(&m_state.move_sem);
+	save_position(actual16);
 	LOG_INF("stop()");
 }
 
@@ -157,19 +160,7 @@ uint16_t Focuser::getCurrentPosition()
 
 void Focuser::setCurrentPosition(uint16_t position)
 {
-	LOG_DBG("setCurrentPosition()");
-	int ret = m_stepper.set_reference_position(static_cast<int32_t>(position));
-	if (ret != 0)
-	{
-		LOG_ERR("Failed to set reference position (%d)", ret);
-	}
-
-	MutexLock lock(m_state.lock);
-	m_state.staged_position = position;
-	m_state.desired_position = position;
-	m_state.move_request = false;
-	m_state.cancel_move = false;
-	LOG_INF("setCurrentPosition 0x%04x (%u)", position, position);
+	applyCurrentPosition(position, true);
 }
 
 uint16_t Focuser::getNewPosition()
@@ -356,6 +347,8 @@ void Focuser::move_to(uint16_t target)
 	{
 		(void)set_stepper_driver_enabled(false);
 	}
+
+	save_position(actual16);
 	LOG_DBG("Motion complete -> 0x%04x (%d)", static_cast<uint16_t>(actual & 0xFFFF), actual);
 }
 
@@ -391,6 +384,64 @@ int32_t Focuser::read_actual_position()
 	return actual;
 }
 
+void Focuser::restore_position()
+{
+	if (m_store == nullptr)
+	{
+		LOG_INF("No position store configured; using default position 0");
+		return;
+	}
+
+	uint16_t persisted = 0U;
+	if (m_store->load(persisted))
+	{
+		LOG_INF("Restoring focuser position 0x%04x (%u) from store", persisted, persisted);
+		applyCurrentPosition(persisted, false);
+	}
+	else
+	{
+		LOG_INF("No persisted focuser position found, defaulting to 0");
+	}
+}
+
+void Focuser::applyCurrentPosition(uint16_t position, bool persist)
+{
+	LOG_DBG("setCurrentPosition()");
+	int ret = m_stepper.set_reference_position(static_cast<int32_t>(position));
+	if (ret != 0)
+	{
+		LOG_ERR("Failed to set reference position (%d)", ret);
+	}
+
+	{
+		MutexLock lock(m_state.lock);
+		m_state.staged_position = position;
+		m_state.desired_position = position;
+		m_state.move_request = false;
+		m_state.cancel_move = false;
+	}
+
+	if (persist)
+	{
+		save_position(position);
+		LOG_INF("setCurrentPosition 0x%04x (%u)", position, position);
+	}
+	else
+	{
+		LOG_INF("setCurrentPosition 0x%04x (%u) (no persist)", position, position);
+	}
+}
+
+void Focuser::save_position(uint16_t position)
+{
+	if (m_store == nullptr)
+	{
+		return;
+	}
+
+	m_store->save(position);
+}
+
 int Focuser::set_stepper_driver_enabled(bool enable)
 {
 	const int ret = m_stepper.enable_driver(enable);

app/src/Focuser.hpp

@@ -8,11 +8,12 @@
 #include <string>
 
 #include "FocuserStepper.hpp"
+#include "PositionStore.hpp"
 
 class Focuser final : public moonlite::Handler
 {
 public:
-	explicit Focuser(FocuserStepper &stepper, const char *firmware_version);
+	explicit Focuser(FocuserStepper &stepper, PositionStore *store, const char *firmware_version);
 
 	int initialise();
 	void loop();
@@ -70,9 +71,13 @@ class Focuser final : public moonlite::Handler
 	int apply_step_interval(uint64_t interval_ns);
 	int32_t read_actual_position();
 	int set_stepper_driver_enabled(bool enable);
+	void restore_position();
+	void applyCurrentPosition(uint16_t position, bool persist);
+	void save_position(uint16_t position);
 
 	const char *m_firmware_version;
 
 	FocuserState m_state{};
 	FocuserStepper &m_stepper;
+	PositionStore *m_store;
 };

app/src/PositionStore.hpp

@@ -0,0 +1,12 @@
+#pragma once
+
+#include <cstdint>
+
+class PositionStore
+{
+public:
+	virtual ~PositionStore() = default;
+
+	virtual bool load(uint16_t &position_out) = 0;
+	virtual void save(uint16_t position) = 0;
+};

app/src/main.cpp

@@ -12,6 +12,7 @@
 #include <errno.h>
 
 #include "Configuration.hpp"
+#include "EepromPositionStore.hpp"
 #include "Focuser.hpp"
 #include "FocuserThread.hpp"
 #include "UartHandler.hpp"
@@ -23,8 +24,9 @@ LOG_MODULE_REGISTER(focuser, CONFIG_APP_LOG_LEVEL);
 namespace
 {
 
+	EepromPositionStore g_position_store;
 	ZephyrFocuserStepper g_stepper_adapter(config::devices::stepper, config::devices::stepper_drv);
-	Focuser g_focuser(g_stepper_adapter, config::version);
+	Focuser g_focuser(g_stepper_adapter, &g_position_store, config::version);
 	FocuserThread g_focuser_thread(g_focuser);
 	UartHandler g_uart_handler(config::devices::uart);
 	UartThread g_uart_thread(g_focuser, g_uart_handler);