[driver] rewrite adns9800

modm-io · Aug 3, 2024 · 6ceed8e · 6ceed8e
1 parent 9b4b64c
commit 6ceed8e
Show file tree

Hide file tree

Showing 11 changed files with 1,122 additions and 450 deletions.
diff --git a/examples/blue_pill_f103/adns_9800/main.cpp b/examples/blue_pill_f103/adns_9800/main.cpp
@@ -1,8 +1,5 @@
 /*
- * Copyright (c) 2011, Fabian Greif
- * Copyright (c) 2013, Kevin Läufer
- * Copyright (c) 2013-2017, Niklas Hauser
- * Copyright (c) 2014, 2016, 2018, Sascha Schade
+ * Copyright (c) 2024, Thomas Sommer
  *
  * This file is part of the modm project.
  *
@@ -14,143 +11,69 @@
 
 #include <modm/board.hpp>
 #include <modm/debug/logger.hpp>
-#include <modm/processing/timer.hpp>
-#include <modm/processing/protothread.hpp>
-
+#include <modm/processing/fiber.hpp>
 #include <modm/driver/motion/adns9800.hpp>
+#include <modm/math/geometry/vector2.hpp>
 
-#include <inttypes.h>
-
-// ----------------------------------------------------------------------------
 // Set the log level
-#undef	MODM_LOG_LEVEL
-#define	MODM_LOG_LEVEL modm::log::DEBUG
+#undef MODM_LOG_LEVEL
+#define MODM_LOG_LEVEL modm::log::DEBUG
 
 using Usart2 = BufferedUart<UsartHal2, UartTxBuffer<256>>;
 // Create an IODeviceWrapper around the Uart Peripheral we want to use
-modm::IODeviceWrapper< Usart2, modm::IOBuffer::BlockIfFull > loggerDevice;
+modm::IODeviceWrapper<Usart2, modm::IOBuffer::BlockIfFull> loggerDevice;
 
 // Set all four logger streams to use the UART
 modm::log::Logger modm::log::debug(loggerDevice);
 modm::log::Logger modm::log::info(loggerDevice);
 modm::log::Logger modm::log::warning(loggerDevice);
 modm::log::Logger modm::log::error(loggerDevice);
 
-class BlinkThread : public modm::pt::Protothread
-{
-public:
-	BlinkThread()
-	{
-		timeout.restart(100ms);
-	}
-
-	bool
-	update()
-	{
-		PT_BEGIN();
-
-		while (true)
-		{
-			Board::LedGreen::reset();
+using Cs = GpioOutputA4;
 
-			PT_WAIT_UNTIL(timeout.isExpired());
-			timeout.restart(100ms);
+modm::Fiber<> adns9800_fiber([]() {
+	modm::Adns9800<SpiMaster1, Cs> adns9800;
+	modm::Vector2i position;
 
-			Board::LedGreen::set();
+	Cs::setOutput(modm::Gpio::High);
 
-			PT_WAIT_UNTIL(timeout.isExpired()) ;
-			timeout.restart(4.9s);
+	SpiMaster1::connect<GpioOutputA5::Sck, GpioInputA6::Miso, GpioOutputA7::Mosi>();
+	SpiMaster1::initialize<Board::SystemClock, 2.25_MHz>();
+	SpiMaster1::setDataMode(SpiMaster1::DataMode::Mode3);
 
-			MODM_LOG_INFO << "Seconds since reboot: " << uptime << modm::endl;
-
-			uptime += 5;
-		}
-
-		PT_END();
+	if(not adns9800.initialize()) {
+		MODM_LOG_INFO << "Failed to initialize ADNS9800" << modm::endl;
+		return;
 	}
 
-private:
-	modm::ShortTimeout timeout;
-	uint32_t uptime;
-};
-
-class Adns9800Thread : public modm::pt::Protothread
-{
-public:
-	Adns9800Thread() : timer(10ms), x(0), y(0)
-	{
-	}
+	adns9800.set(modm::adns9800::Resolution<8200>{});
+  adns9800.set(modm::adns9800::ShutterConfig{
+    period_min:   10000,
+    period_max:   40000,
+    exposure_max: 50000
+  });
 
-	bool
-	update()
+	while (true)
 	{
-		PT_BEGIN();
-
-		Cs::setOutput(modm::Gpio::High);
+		const auto data {adns9800.read<modm::adns9800::Data_FailFlags_Monitoring>()};
+		position += data.delta;
 
-		SpiMaster1::connect<GpioOutputA7::Mosi, GpioOutputA5::Sck, GpioInputA6::Miso>();
-		SpiMaster1::initialize<Board::SystemClock, 2.25_MHz>();
-		SpiMaster1::setDataMode(SpiMaster1::DataMode::Mode3);
+		MODM_LOG_INFO << "delta: " << data.delta << modm::endl;
+		MODM_LOG_INFO << "position: " << position << modm::endl;
+		MODM_LOG_INFO << modm::endl;
 
-		adns9800::initialise();
-
-		while (true)
-		{
-			PT_WAIT_UNTIL(timer.execute());
-
-			{
-				int16_t delta_x, delta_y;
-				adns9800::getDeltaXY(delta_x, delta_y);
-				MODM_LOG_INFO.printf("dx = %5" PRId16 ", dy = %5" PRId16"; x = %9" PRId32", y=%9" PRId32 "\n", delta_x, delta_y, x, y);
-
-				x += delta_x;
-				y += delta_y;
-			}
-		}
-
-		PT_END();
+		modm::this_fiber::sleep_for(100ms);
 	}
+});
 
-private:
-	modm::ShortPeriodicTimer timer;
-	int32_t x, y;
-
-	using Cs = GpioOutputA4;
-
-	using adns9800 = modm::Adns9800<
-		/* Spi = */ SpiMaster1,
-		/* Ncs = */ Cs >;
-};
-
-
-BlinkThread blinkThread;
-Adns9800Thread adns9800Thread;
-
-
-// ----------------------------------------------------------------------------
 int
 main()
 {
 	Board::initialize();
 
-	// initialize Uart2 for MODM_LOG_*
 	Usart2::connect<GpioOutputA2::Tx>();
 	Usart2::initialize<Board::SystemClock, 115200_Bd>();
 
-	// Use the logging streams to print some messages.
-	// Change MODM_LOG_LEVEL above to enable or disable these messages
-	MODM_LOG_DEBUG   << "debug"   << modm::endl;
-	MODM_LOG_INFO    << "info"    << modm::endl;
-	MODM_LOG_WARNING << "warning" << modm::endl;
-	MODM_LOG_ERROR   << "error"   << modm::endl;
-
-	MODM_LOG_INFO << "Welcome to ADNS 9800 demo." << modm::endl;
-
-	while (true)
-	{
-		blinkThread.update();
-		adns9800Thread.update();
-	}
-
+	modm::fiber::Scheduler::run();
 	return 0;
 }
diff --git a/examples/blue_pill_f103/adns_9800/project.xml b/examples/blue_pill_f103/adns_9800/project.xml
@@ -10,7 +10,7 @@
     <module>modm:platform:gpio</module>
     <module>modm:platform:spi:1</module>
     <module>modm:platform:uart:2</module>
-    <module>modm:processing:protothread</module>
+    <module>modm:processing:fiber</module>
     <module>modm:processing:timer</module>
     <module>modm:build:scons</module>
   </modules>