From f1db3049d4a254667657f85dec9dd8665a5d4764 Mon Sep 17 00:00:00 2001 From: Thomas Sommer Date: Thu, 27 Jun 2024 12:55:47 +0200 Subject: [PATCH] [driver] rewrite adns9800 --- examples/blue_pill_f103/adns_9800/main.cpp | 137 ++--- examples/blue_pill_f103/adns_9800/project.xml | 3 +- src/modm/driver/motion/adns9800.fiber.hpp | 430 +++++++++++++++ src/modm/driver/motion/adns9800.hpp | 103 ---- src/modm/driver/motion/adns9800.lb | 34 +- src/modm/driver/motion/adns9800.rf.hpp | 498 ++++++++++++++++++ src/modm/driver/motion/adns9800_data.hpp | 134 +++++ ...0_srom_a4.hpp => adns9800_firmware_a4.cpp} | 11 +- ...srom_a4b.hpp => adns9800_firmware_a4b.cpp} | 11 +- ...0_srom_a5.hpp => adns9800_firmware_a5.cpp} | 11 +- ...0_srom_a6.hpp => adns9800_firmware_a6.cpp} | 11 +- src/modm/driver/motion/adns9800_impl.hpp | 220 -------- 12 files changed, 1134 insertions(+), 469 deletions(-) create mode 100644 src/modm/driver/motion/adns9800.fiber.hpp delete mode 100644 src/modm/driver/motion/adns9800.hpp create mode 100644 src/modm/driver/motion/adns9800.rf.hpp create mode 100644 src/modm/driver/motion/adns9800_data.hpp rename src/modm/driver/motion/{adns9800_srom_a4.hpp => adns9800_firmware_a4.cpp} (99%) rename src/modm/driver/motion/{adns9800_srom_a4b.hpp => adns9800_firmware_a4b.cpp} (99%) rename src/modm/driver/motion/{adns9800_srom_a5.hpp => adns9800_firmware_a5.cpp} (99%) rename src/modm/driver/motion/{adns9800_srom_a6.hpp => adns9800_firmware_a6.cpp} (99%) delete mode 100644 src/modm/driver/motion/adns9800_impl.hpp diff --git a/examples/blue_pill_f103/adns_9800/main.cpp b/examples/blue_pill_f103/adns_9800/main.cpp index 4ba52d34fe..7eda962128 100644 --- a/examples/blue_pill_f103/adns_9800/main.cpp +++ b/examples/blue_pill_f103/adns_9800/main.cpp @@ -12,23 +12,22 @@ */ // ---------------------------------------------------------------------------- +#include + #include #include -#include -#include - #include - -#include +#include +#include // ---------------------------------------------------------------------------- // 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>; // Create an IODeviceWrapper around the Uart Peripheral we want to use -modm::IODeviceWrapper< Usart2, modm::IOBuffer::BlockIfFull > loggerDevice; +modm::IODeviceWrapper loggerDevice; // Set all four logger streams to use the UART modm::log::Logger modm::log::debug(loggerDevice); @@ -36,121 +35,53 @@ 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); - } +using Cs = GpioOutputA4; +using Adns9800 = modm::Adns9800; - bool - update() - { - PT_BEGIN(); - - while (true) - { - Board::LedGreen::reset(); - - PT_WAIT_UNTIL(timeout.isExpired()); - timeout.restart(100ms); +modm::Vector2i position; - Board::LedGreen::set(); +modm::Fiber<> adns9800_fiber([]() { + Adns9800::Data data; + Adns9800 adns9800{data}; - PT_WAIT_UNTIL(timeout.isExpired()) ; - timeout.restart(4.9s); + Cs::setOutput(modm::Gpio::High); - MODM_LOG_INFO << "Seconds since reboot: " << uptime << modm::endl; + SpiMaster1::connect(); + SpiMaster1::initialize(); + SpiMaster1::setDataMode(SpiMaster1::DataMode::Mode3); - uptime += 5; - } - - PT_END(); - } - -private: - modm::ShortTimeout timeout; - uint32_t uptime; -}; - -class Adns9800Thread : public modm::pt::Protothread -{ -public: - Adns9800Thread() : timer(10ms), x(0), y(0) - { - } + adns9800.initialize(); + adns9800.setResolution(Adns9800::Resolution<8200>()); + Adns9800::Shutter shutter = { + period_min: 10000, + period_max: 40000, + exposure_max: 50000 + }; + adns9800.setShutter(shutter); - bool - update() + while (true) { - PT_BEGIN(); - - Cs::setOutput(modm::Gpio::High); - - SpiMaster1::connect(); - SpiMaster1::initialize(); - SpiMaster1::setDataMode(SpiMaster1::DataMode::Mode3); - - adns9800::initialise(); + adns9800.read(); + position += data; - while (true) - { - PT_WAIT_UNTIL(timer.execute()); + MODM_LOG_INFO << "increment: " << data << modm::endl; + MODM_LOG_INFO << "position: " << position << modm::endl; + MODM_LOG_INFO << modm::endl; - { - 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(10ms); } +}); -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(); Usart2::initialize(); - // 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 index 4f54e9196d..d5430bf129 100644 --- a/examples/blue_pill_f103/adns_9800/project.xml +++ b/examples/blue_pill_f103/adns_9800/project.xml @@ -2,6 +2,7 @@ modm:blue-pill-f103 + @@ -10,7 +11,7 @@ modm:platform:gpio modm:platform:spi:1 modm:platform:uart:2 - modm:processing:protothread + modm:processing:fiber modm:processing:timer modm:build:scons diff --git a/src/modm/driver/motion/adns9800.fiber.hpp b/src/modm/driver/motion/adns9800.fiber.hpp new file mode 100644 index 0000000000..90e429ec00 --- /dev/null +++ b/src/modm/driver/motion/adns9800.fiber.hpp @@ -0,0 +1,430 @@ +/* + * Copyright (c) 2024, Thomas Sommer + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#pragma once + +#include +#include +#include +#include +#include + +EXTERN_FLASH_STORAGE(uint8_t adns9800_firmware[3070]); + +namespace modm { + +/// @ingroup modm_driver_adns9800 +struct adns9800 { + /// @cond + enum class Register : uint8_t { + Product_ID = 0x00, + Revision_ID = 0x01, + Motion = 0x02, + Delta_X_L = 0x03, + Delta_X_H = 0x04, + Delta_Y_L = 0x05, + Delta_Y_H = 0x06, + SQUAL = 0x07, // Surface Qaulity + Pixel_Sum = 0x08, + Maximum_Pixel = 0x09, + Minimum_Pixel = 0x0a, + Shutter_Lower = 0x0b, + Shutter_Upper = 0x0c, + Frame_Period_Lower = 0x0d, + Frame_Period_Upper = 0x0e, + Configuration_I = 0x0f, // Resolution + Configuration_II = 0x10, + Frame_Capture = 0x12, + SROM_Enable = 0x13, + Run_Downshift = 0x14, + Rest1_Rate = 0x15, + Rest1_Downshift = 0x16, + Rest2_Rate = 0x17, + Rest2_Downshift = 0x18, + Rest3_Rate = 0x19, + Frame_Period_Max_Bound_Lower = 0x1a, + Frame_Period_Max_Bound_Upper = 0x1b, + Frame_Period_Min_Bound_Lower = 0x1c, + Frame_Period_Min_Bound_Upper = 0x1d, + Shutter_Max_Bound_Lower = 0x1e, + Shutter_Max_Bound_Upper = 0x1f, + LASER_CTRL0 = 0x20, // Laser Control + Observation = 0x24, + Data_Out_Lower = 0x25, + Data_Out_Upper = 0x26, + SROM_ID = 0x2a, + Lift_Detection_Thr = 0x2e, + Configuration_V = 0x2f, + Configuration_IV = 0x39, + Power_Up_Reset = 0x3a, + Shutdown = 0x3b, + Snap_Angle = 0x42, + Inverse_Product_ID = 0x3f, + Motion_Burst = 0x50, + SROM_Load_Burst = 0x62, + Pixel_Burst = 0x64, + }; + /// @endcond + + /** + * @tparam Cpi Counts per inch + * Allowed values: 200 to 8200 in steps of 200 + */ + template + requires (Cpi >= 200) and (!Cpi % 200 == 0) and (Cpi <= 8200) + struct Resolution: public modm::Register8 { + Resolution() : modm::Register8(Cpi / 200) {}; + }; + using Resolution_t = modm::Register8; + + enum class LaserControl : uint8_t { + ForceDisable = Bit0, + AlwaysOn = Bit2 + }; + MODM_FLAGS8(LaserControl); + + enum class ConfigurationII : uint8_t { + Cpi_Reporting_Mode = Bit2, + Fixed_FrameRate = Bit3, + Disable_AGC = Bit4, + Rest_Enable = Bit5, + Force_Rest0 = Bit6, + Force_Rest1 = Bit7, + }; + MODM_FLAGS8(ConfigurationII); + + // forward declarations + struct Data; + struct DataAndFaildetect; + struct DataAndFaildetectAndMonitoring; +}; + +/** + * ADNS9800 Laser Motion Sensor + * + * @tparam SpiMaster + * @tparam Cs + * + * @author Thomas Sommer + * @ingroup modm_driver_adns9800 + */ +template +class Adns9800 : public adns9800, public modm::SpiDevice { +public: + static constexpr size_t FrameSize = 30 * 30; // Size of CMOS + + /** + * @brief ShutterConfig boundaries which may be selected by the automatic frame rate control. + * Periods are expressed as ticks of the device, running at 50MHz. + * In "constant Framerate mode" period_max determines the framerate. + * + * @param period_min Frame period min bound -> max allowed fps + * @param period_max Frame period max bound -> min allowed fps. + * @param exposure_max Exposure period max bound -> max exposure time + * + * @ingroup modm_driver_adns9800 + */ + struct ShutterConfig { + using PeriodType = uint16_t; + using Duration = std::chrono::duration>; + + PeriodType period_min, period_max, exposure_max; + + // The datasheet refers to "wait for one frame" without better specification. + Duration getOneFrameTime() const { + return Duration(exposure_max); + } + + /// @cond + // Always validate configuration before write: There's no internal protection. + // Malformed periods may result in undefined behaviour. + bool isValid() { + return modm_assert_continue_ignore(period_max >= period_min + exposure_max, "adns9800.shutter", + "Invalid shutter configuration: !(period_max >= period_min + exposure_max)"); + } + /// @endcond + }; + + /** + * @brief Default Initialization routine + */ + void + initialize() { + powerUp(); + if(verifyIdentity()) { + writeFirmware(); + laserEnable(); + } + } + + /** + * @brief powerUp() resets the device's internal state after power-loss or user invoked shutdown(). + */ + void + powerUp() { + /// @see power-up sequence, datasheet page 20 + Cs::reset(); + modm::this_fiber::sleep_for(10ns); + Cs::set(); + modm::this_fiber::sleep_for(10ns); + + writeRegister(Register::Power_Up_Reset, 0x5a); + modm::this_fiber::sleep_for(50ms); + read(); // read and discard motion data + } + + /** + * @brief Put the device into low power mode. Do not use this for power management + * in normal operation due to slow recovery time. + */ + void + shutdown() { + writeRegister(Register::Shutdown, 0xb6); + } + + /** + * @brief Verify presence of the correct device by reading and checking it's product and revision id. + */ + bool + verifyIdentity() { + bool isValid = true; + + const uint8_t product_id = readRegister(Register::Product_ID); + isValid &= modm_assert_continue_fail(product_id == uint8_t(0x33), "adns9800.id", "Unexpected Product Id", product_id); + + const uint8_t inverse_product_id = readRegister(Register::Inverse_Product_ID); + isValid &= modm_assert_continue_fail(inverse_product_id == uint8_t(~0x33), "adns9800.iid", "Unexpected Inverse Product Id", inverse_product_id); + + const uint8_t revision_id = readRegister(Register::Revision_ID); + isValid &= modm_assert_continue_fail(revision_id == uint8_t(0x03), "adns9800.rid", "Unexpected Revision Id", revision_id); + + return isValid; + } + + void + laserEnable(bool enable = true) { + LaserControl_t control(readRegister(Register::LASER_CTRL0)); + control.update(LaserControl::ForceDisable, !enable); + writeRegister(Register::LASER_CTRL0, control.value); + } + + void + laserAlwaysOn(bool enable = true) { + LaserControl_t control(readRegister(Register::LASER_CTRL0)); + control.update(LaserControl::AlwaysOn, enable); + writeRegister(Register::LASER_CTRL0, control.value); + } + + void + set(const ConfigurationII_t config) { + writeRegister(Register::Configuration_II, config.value); + } + + void + set(const Resolution_t resolution) { + writeRegister(Register::Configuration_I, resolution.value); + } + + void + set(const ShutterConfig shutter_new) { + if (shutter_new.isValid()) { + shutter_config = shutter_new; + + // @todo waiting for #690 that brings 16bit Spi transfers + writeRegister(Register::Frame_Period_Max_Bound_Lower, shutter_config.period_max & 0xff); + writeRegister(Register::Frame_Period_Max_Bound_Upper, shutter_config.period_max >> 8); + + writeRegister(Register::Frame_Period_Min_Bound_Lower, shutter_config.period_min & 0xff); + writeRegister(Register::Frame_Period_Min_Bound_Upper, shutter_config.period_min >> 8); + + writeRegister(Register::Shutter_Max_Bound_Lower, shutter_config.exposure_max & 0xff); + writeRegister(Register::Shutter_Max_Bound_Upper, shutter_config.exposure_max >> 8); + } + } + + /// In fixed framerate mode (Register ConfigurationII::Fixed_FrameRate: 1), period_max selects the framerate + void + setFramePeriodMax(const uint16_t period_max) { + const uint16_t recover = shutter_config.period_max; + shutter_config.period_max = period_max; + + if (shutter_config.isValid()) { + // @todo waiting for #690 that brings 16bit Spi transfers + writeRegister(Register::Frame_Period_Max_Bound_Lower, shutter_config.period_max & 0xff); + writeRegister(Register::Frame_Period_Max_Bound_Upper, shutter_config.period_max >> 8); + } else { + shutter_config.period_max = recover; + } + } + + void + setFramePeriodMin(const uint16_t period_min) { + const uint16_t recover = shutter_config.period_min; + shutter_config.period_min = period_min; + + if (shutter_config.isValid()) { + // @todo waiting for #690 that brings 16bit Spi transfers + writeRegister(Register::Frame_Period_Min_Bound_Lower, shutter_config.period_min & 0xff); + writeRegister(Register::Frame_Period_Min_Bound_Upper, shutter_config.period_min >> 8); + } else { + shutter_config.period_min = recover; + } + } + + void + setExposureMax(const uint16_t shutter_max) { + const uint16_t recover = shutter_config.shutter_max; + shutter_config.shutter_max = shutter_max; + + if (shutter_config.assert()) { + // @todo waiting for #690 that brings 16bit Spi transfers + writeRegister(Register::Shutter_Max_Bound_Lower, shutter_config.shutter_max & 0xff); + writeRegister(Register::Shutter_Max_Bound_Upper, shutter_config.shutter_max >> 8); + } else { + shutter_config.shutter_max = recover; + } + } + + /** + * Enable or disable the Angle Snapping function. When enabled, actual movement ranges from ±5° from X or Y-axis, + * it will be snapped to the closest axis. For example, if the sensor moves at ±2° from X-axis, the output motion + * data will be snapped to X-axis at 0°. + */ + void + setSnapAngle(bool const value) { + const uint8_t preserved = readRegister(Register::Snap_Angle) & ~0x80; + writeRegister(Register::Snap_Angle, preserved | value ? 0x80 : 0x00); + } + + bool + hasNewMotionData() { + return readRegister(Register::Motion) & Bit7; + } + + /** + * Read the latest motion delta from the sensor and update data with it content + * Reading the motion delta also reloads the Motion interrupt (Pin7) + * + * @tparam One of Data, DataObserve or DataAnalysis depending on your need for details + */ + template + requires std::is_base_of_v + D + read() { + uint8_t buffer[D::length]; + + readTransaction([this, &buffer]() { + SpiMaster::transfer(static_cast(Register::Motion_Burst)); + modm::this_fiber::sleep_for(shutter_config.getOneFrameTime()); + SpiMaster::transfer(nullptr, buffer, sizeof(buffer)); + }); + + return D(buffer); + } + + // @todo test captureFrame + void + captureFrame(std::span buffer) { + readTransaction([&buffer]() { + writeRegister(Register::Frame_Capture, 0x93); + writeRegister(Register::Frame_Capture, 0xc5); + modm::this_fiber::poll(readRegister(Register::Motion) & Bit0); // wait for first pixel + SpiMaster::transfer(nullptr, buffer, FrameSize); + }); + } + +private: + template + void + readTransaction(Callable&& closure) { + while(not ready_to_read.isExpired()) modm::this_fiber::yield(); + while(not this->acquireMaster()) modm::this_fiber::yield(); + Cs::reset(); + + closure(); + + ready_to_write.restart(20us); // tSRW: [t]ime [S]pi between [R]rite and [W]rite + ready_to_read.restart(20us); // tSRR: [t]ime [S]pi between [R]rite and [R]ead + + modm::this_fiber::sleep_for(120ns); // tSCLK_NCS_read: Cs persistance after last SCLK for read + Cs::set(); + } + + template + void + writeTransaction(Callable&& closure) { + while(not ready_to_write.isExpired()) modm::this_fiber::yield(); + while(not this->acquireMaster()) modm::this_fiber::yield(); + Cs::reset(); + + closure(); + + ready_to_write.restart(20us); // tSWW [t]ime [S]pi between [W]rite and [W]rite + ready_to_read.restart(120us); // tSWR [t]ime [S]pi between [W]rite and [R]ead + modm::this_fiber::sleep_for(20us); // tSCLK_NCS_write: Cs persistance after last SCLK for write + Cs::set(); + } + +public: + uint8_t + readRegister(const Register reg) { + uint8_t ret; + + readTransaction([reg, &ret]() { + SpiMaster::transfer(static_cast(reg)); + modm::this_fiber::sleep_for(100us); // tSRAD: [time] [S]pi between [R]ead [A]dress and [D]ata + SpiMaster::transfer(nullptr, &ret, 1); + }); + + return ret; + } + + void + writeRegister(const Register reg, const uint8_t data) { + writeTransaction([=]() { + SpiMaster::transfer(static_cast(reg) | Bit7); // Bit7 = 1 indicates a write + SpiMaster::transfer(data); + }); + } + + void + writeFirmware() { + writeRegister(Register::Configuration_IV, 0x02); // Enable 3k firmware mode + writeRegister(Register::SROM_Enable, 0x1d); // Initialize SROM + modm::this_fiber::sleep_for(shutter_config.getOneFrameTime()); + writeRegister(Register::SROM_Enable, 0x18); // Start SROM download + + writeTransaction([=]() { + SpiMaster::transfer(static_cast(Register::SROM_Load_Burst) | Bit7); // Bit7 = 1 indicates a write + accessor::Flash flash(adns9800_firmware); + for(size_t ii = 0; ii < sizeof(adns9800_firmware); ++ii) + { + modm::this_fiber::sleep_for(15us); + SpiMaster::transfer(*flash++); + } + }); + modm::this_fiber::sleep_for(shutter_config.getOneFrameTime()); + + // Additionaly one could request a CRC check of the firmware. @see datasheet P31 + } + +private: + // Default power-up values, @see datasheet P22 + ShutterConfig shutter_config{ + period_min: 4000, // 0xa00f + period_max: 24000, // 0xc05d + exposure_max: 20000 // 0x204e + }; + + modm::ShortTimeout ready_to_read{0s}, ready_to_write{0s}; +}; + +} // namespace modm \ No newline at end of file diff --git a/src/modm/driver/motion/adns9800.hpp b/src/modm/driver/motion/adns9800.hpp deleted file mode 100644 index 1f79266698..0000000000 --- a/src/modm/driver/motion/adns9800.hpp +++ /dev/null @@ -1,103 +0,0 @@ -/* - * Copyright (c) 2016, Sascha Schade - * - * This file is part of the modm project. - * - * This Source Code Form is subject to the terms of the Mozilla Public - * License, v. 2.0. If a copy of the MPL was not distributed with this - * file, You can obtain one at http://mozilla.org/MPL/2.0/. - */ -// ---------------------------------------------------------------------------- - -#ifndef MODM_ADNS9800_HPP -#define MODM_ADNS9800_HPP - -namespace modm -{ - -/// @ingroup modm_driver_adns9800 -struct adns9800 -{ -public: - /// The addresses of the Configuration and Data Registers - enum class - Register : uint8_t - { - Product_ID = 0x00, - Revision_ID = 0x01, - Motion = 0x02, - Delta_X_L = 0x03, - Delta_X_H = 0x04, - Delta_Y_L = 0x05, - Delta_Y_H = 0x06, - SQUAL = 0x07, - Pixel_Sum = 0x08, - Maximum_Pixel = 0x09, - Minimum_Pixel = 0x0a, - Shutter_Lower = 0x0b, - Shutter_Upper = 0x0c, - Frame_Period_Lower = 0x0d, - Frame_Period_Upper = 0x0e, - Configuration_I = 0x0f, - Configuration_II = 0x10, - Frame_Capture = 0x12, - SROM_Enable = 0x13, - Run_Downshift = 0x14, - Rest1_Rate = 0x15, - Rest1_Downshift = 0x16, - Rest2_Rate = 0x17, - Rest2_Downshift = 0x18, - Rest3_Rate = 0x19, - Frame_Period_Max_Bound_Lower = 0x1a, - Frame_Period_Max_Bound_Upper = 0x1b, - Frame_Period_Min_Bound_Lower = 0x1c, - Frame_Period_Min_Bound_Upper = 0x1d, - Shutter_Max_Bound_Lower = 0x1e, - Shutter_Max_Bound_Upper = 0x1f, - LASER_CTRL0 = 0x20, - Observation = 0x24, - Data_Out_Lower = 0x25, - Data_Out_Upper = 0x26, - SROM_ID = 0x2a, - Lift_Detection_Thr = 0x2e, - Configuration_V = 0x2f, - Configuration_IV = 0x39, - Power_Up_Reset = 0x3a, - Shutdown = 0x3b, - Inverse_Product_ID = 0x3f, - Motion_Burst = 0x50, - SROM_Load_Burst = 0x62, - Pixel_Burst = 0x64, - }; -}; - -/// @ingroup modm_driver_adns9800 -template < typename Spi, typename Cs > -class Adns9800 : public adns9800 -{ -public: - static bool - initialise(); - - static bool - isNewMotionDataAvailable(); - - static void - getDeltaXY(int16_t &delta_x, int16_t &delta_y); - -protected: - static uint8_t - readReg(Register const reg); - - static void - writeReg(Register const reg, uint8_t const data); - - static void - uploadFirmware(); -}; - -} // modm namespace - -#include "adns9800_impl.hpp" - -#endif // MODM_ADNS9800_HPP diff --git a/src/modm/driver/motion/adns9800.lb b/src/modm/driver/motion/adns9800.lb index d22b64bc3b..84bf2bf1bf 100644 --- a/src/modm/driver/motion/adns9800.lb +++ b/src/modm/driver/motion/adns9800.lb @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # -*- coding: utf-8 -*- # -# Copyright (c) 2018, Niklas Hauser +# Copyright (c) 2024, Thomas Sommer # # This file is part of the modm project. # @@ -14,10 +14,11 @@ def init(module): module.name = ":driver:adns9800" module.description = """\ -# ADNS-9800 Laser Motion Sensor +# ADNS9800 Laser Motion Sensor -Based on work of Alexander Entinger, MSc / LXRobotics -Based on https://github.com/mrjohnk/ADNS-9800 +The ADNS-9800 comprises of sensor and VCSEL in a single chip-on-board (COB) package. +It provides enhanced features like programmable frame rate, programmable resolution, +configurable sleep and wake up time. """ def prepare(module, options): @@ -27,15 +28,28 @@ def prepare(module, options): enumeration=["a4", "a4b", "a5", "a6"], description="Select the firmware version to initialize the device with.", default="a6")) + module.add_option( + BooleanOption( + name="use_fiber", + description="Use an advanced driver without backwards compatibilty for protothreads.", + default="false")) module.depends( - ":architecture:delay", - ":architecture:spi", - ":debug") + ":processing:resumable", + ":architecture:spi.device", + ":math:geometry", + ":architecture:assert", + ) return True +# @fixme check if modm:processing:protothread:use_fiber is true, otherwise use_pure_fiber wont work + def build(env): env.outbasepath = "modm/src/modm/driver/motion" - env.copy("adns9800.hpp") - env.copy("adns9800_impl.hpp") - env.copy("adns9800_srom_{}.hpp".format(env["firmware"]), "adns9800_srom.hpp") + env.copy("adns9800_firmware_{}.cpp".format(env["firmware"]), "adns9800_firmware.cpp") + + if env["use_fiber"]: + env.copy("adns9800.fiber.hpp", "adns9800.hpp") + env.copy("adns9800_data.hpp") + else: + env.copy("adns9800.rf.hpp", "adns9800.hpp") diff --git a/src/modm/driver/motion/adns9800.rf.hpp b/src/modm/driver/motion/adns9800.rf.hpp new file mode 100644 index 0000000000..c95cce1cc4 --- /dev/null +++ b/src/modm/driver/motion/adns9800.rf.hpp @@ -0,0 +1,498 @@ +/* + * Copyright (c) 2024, Thomas Sommer + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#pragma once + +#include +#include +#include +#include +#include + +EXTERN_FLASH_STORAGE(uint8_t adns9800_firmware[3070]); + +#define RF_SLEEP_FOR_TIMER modm::PreciseTimeout timeout; +#define RF_SLEEP_FOR(time) timeout.restart(time); \ + RF_WAIT_UNTIL(timeout.isExpired()); + +namespace modm { + +/// @ingroup modm_driver_adns9800 +struct adns9800 { + /// @cond + enum class Register : uint8_t { + Product_ID = 0x00, + Revision_ID = 0x01, + Motion = 0x02, + Delta_X_L = 0x03, + Delta_X_H = 0x04, + Delta_Y_L = 0x05, + Delta_Y_H = 0x06, + SQUAL = 0x07, // Surface Qaulity + Pixel_Sum = 0x08, + Maximum_Pixel = 0x09, + Minimum_Pixel = 0x0a, + Shutter_Lower = 0x0b, + Shutter_Upper = 0x0c, + Frame_Period_Lower = 0x0d, + Frame_Period_Upper = 0x0e, + Configuration_I = 0x0f, // Resolution + Configuration_II = 0x10, + Frame_Capture = 0x12, + SROM_Enable = 0x13, + Run_Downshift = 0x14, + Rest1_Rate = 0x15, + Rest1_Downshift = 0x16, + Rest2_Rate = 0x17, + Rest2_Downshift = 0x18, + Rest3_Rate = 0x19, + Frame_Period_Max_Bound_Lower = 0x1a, + Frame_Period_Max_Bound_Upper = 0x1b, + Frame_Period_Min_Bound_Lower = 0x1c, + Frame_Period_Min_Bound_Upper = 0x1d, + Shutter_Max_Bound_Lower = 0x1e, + Shutter_Max_Bound_Upper = 0x1f, + LASER_CTRL0 = 0x20, // Laser Control + Observation = 0x24, + Data_Out_Lower = 0x25, + Data_Out_Upper = 0x26, + SROM_ID = 0x2a, + Lift_Detection_Thr = 0x2e, + Configuration_V = 0x2f, + Configuration_IV = 0x39, + Power_Up_Reset = 0x3a, + Shutdown = 0x3b, + Snap_Angle = 0x42, + Inverse_Product_ID = 0x3f, + Motion_Burst = 0x50, + SROM_Load_Burst = 0x62, + Pixel_Burst = 0x64, + }; + /// @endcond + + /** + * @tparam Cpi Counts per inch + * Allowed values: 200 to 8200 in steps of 200 + */ + template + requires (Cpi >= 200) and (Cpi % 200 == 0) and (Cpi <= 8200) + struct Resolution: public modm::Register8 { + Resolution() : modm::Register8(Cpi / 200) {}; + }; + using Resolution_t = modm::Register8; + + enum class LaserControl : uint8_t { + ForceDisable = Bit0, + AlwaysOn = Bit2, + }; + MODM_FLAGS8(LaserControl); + + enum class ConfigurationII : uint8_t { + Cpi_Reporting_Mode = Bit2, + Fixed_FrameRate = Bit3, + Disable_AGC = Bit4, + Rest_Enable = Bit5, + Force_Rest0 = Bit6, + Force_Rest1 = Bit7, + }; + MODM_FLAGS8(ConfigurationII); + + using Data = modm::Vector; +}; + +/** + * ADNS9800 Laser Motion Sensor + * + * @tparam SpiMaster + * @tparam Cs + * + * @author Thomas Sommer + * @ingroup modm_driver_adns9800 + */ +template +class Adns9800 : public adns9800, public modm::SpiDevice, protected modm::NestedResumable<3> { + // Adns9800 internal clock + static constexpr uint32_t Clock = 50_MHz; + + /// @cond + // @todo is there a way to use implicit literal construction, + // so we get something like "static constexpr auto tSWW(20us);" ? + static constexpr std::chrono::microseconds tSWW{20}; // [t]ime [S]pi between [W]rite and [W]rite + static constexpr std::chrono::microseconds tSWR{120}; // [t]ime [S]pi between [W]rite and [R]ead + static constexpr std::chrono::microseconds tSRW{20}; // [t]ime [S]pi between [R]rite and [W]rite + static constexpr std::chrono::microseconds tSRR{tSRW}; // [t]ime [S]pi between [R]rite and [R]ead + + static constexpr std::chrono::microseconds tSRAD{100}; // [time] [S]pi between [R]ead [A]dress and [D]ata + + static constexpr std::chrono::nanoseconds tSCLK_NCS_read{120}; // time before releasing NCS after last SCLK for read + static constexpr std::chrono::microseconds tSCLK_NCS_write{20}; // time before releasing NCS after last SCLK for write + /// @endcond + +public: + using Data = adns9800::Data; + + Adns9800(Data &data) : data(data) + {} + + modm::ResumableResult + initialize() { + RF_BEGIN(); + + RF_CALL(powerUp()); + RF_CALL(verifyIdentity()); + RF_CALL(writeFirmware()); + RF_SLEEP_FOR(10ms); + + RF_END(); + } + + modm::ResumableResult + powerUp() { + RF_BEGIN(); + + // @see power-up sequence, datasheet page 20 + RF_CALL(writeRegister(Register::Power_Up_Reset, 0x5a)); + RF_SLEEP_FOR(50ms); + // flush motion data + RF_CALL(read()); + data.x = data.y = 0; + + RF_END(); + } + + // Run powerUp() to deassert shutdown mode + modm::ResumableResult + shutdown() { + RF_BEGIN(); + RF_END_RETURN_CALL(writeRegister(Register::Shutdown, 0xb6)); + } + + ResumableResult + verifyIdentity() { + RF_BEGIN(); + + bool isPresent = true; + + const uint8_t product_id = RF_CALL(readRegister(Register::Product_ID)); + isPresent &= modm_assert_continue_fail(product_id == uint8_t(0x33), "adns9800.id", "Unexpected Product Id", product_id); + + const uint8_t inverse_product_id = RF_CALL(readRegister(Register::Inverse_Product_ID)); + isPresent &= modm_assert_continue_fail(inverse_product_id == uint8_t(~0x33), "adns9800.iid", "Unexpected Inverse Product Id", inverse_product_id); + + const uint8_t revision_id = RF_CALL(readRegister(Register::Revision_ID)); + isPresent &= modm_assert_continue_fail(revision_id == uint8_t(0x03), "adns9800.rid", "Unexpected Revision Id", revision_id); + + RF_END_RETURN(isPresent); + } + + modm::ResumableResult + set(const ConfigurationII_t config) { + RF_BEGIN(); + + RF_END_RETURN_CALL(writeRegister(Register::Configuration_II, config.value)); + } + + modm::ResumableResult + set(LaserControl_t config) { + RF_BEGIN(); + + uint8_t preserved = RF_CALL(readRegister(Register::LASER_CTRL0)) & 0xf0; + RF_END_RETURN_CALL(writeRegister(Register::LASER_CTRL0, preserved | config.value)); + } + + modm::ResumableResult + set(Resolution_t resolution) { + RF_BEGIN(); + + RF_END_RETURN_CALL(writeRegister(Register::Configuration_I, resolution.value)); + } + + /** + * @brief Shutter boundaries which may be selected by the automatic frame rate control. + * Periods are expressed as ticks of the device, running at 50MHz. + * In "constant Framerate mode" period_max determines the framerate. + * + * @param period_min Frame period min bound -> max allowed fps + * @param period_max Frame period max bound -> min allowed fps. + * @param exposure_max Exposure period max bound -> max exposure time + * + * @ingroup modm_driver_adns9800 + */ + // @todo move into namespace adns9800 {...} ? + struct Shutter { + uint16_t period_min, period_max, exposure_max; + + /// Validate shutter values before transmitting them to the device to prevent undefined behaviour + bool isValid() { + return modm_assert_continue_ignore(period_max >= period_min + exposure_max, "adns9800.shutter", + "Invalid shutter configuration: !(period_max >= period_min + exposure_max)"); + } + + /// For developement, you can get human readable values using these getters + float getMinFps() const { return Clock / static_cast(period_min); } + float getMaxFps() const { return Clock / static_cast(period_max); } + // @todo implement getExposureMax + // std::chrono::milliseconds getExposureMax() const { return ... ; } + }; + + modm::ResumableResult + set(Shutter shutter_new) { + RF_BEGIN(); + + if (shutter_new.isValid()) { + shutter = shutter_new; + + // @todo use SpiMaster 16bit-transfer @see #690 + // @todo use bulk write + RF_CALL(writeRegister(Register::Frame_Period_Max_Bound_Lower, shutter.period_max & 0xff)); + RF_CALL(writeRegister(Register::Frame_Period_Max_Bound_Upper, shutter.period_max >> 8)); + + RF_CALL(writeRegister(Register::Frame_Period_Min_Bound_Lower, shutter.period_min & 0xff)); + RF_CALL(writeRegister(Register::Frame_Period_Min_Bound_Upper, shutter.period_min >> 8)); + + RF_CALL(writeRegister(Register::Shutter_Max_Bound_Lower, shutter.exposure_max & 0xff)); + RF_CALL(writeRegister(Register::Shutter_Max_Bound_Upper, shutter.exposure_max >> 8)); + + } + + RF_END(); + } + + /// In fixed framerate mode (Register ConfigurationII::Fixed_FrameRate: 1), period_max selects the framerate + modm::ResumableResult + setFramePeriodMax(const uint16_t period_max) { + RF_BEGIN(); + + const uint16_t period_max_recover = period_max; + shutter.period_max = period_max; + + if (shutter.isValid()) { + // @todo use SpiMaster 16 bit transfer @see #690 + RF_CALL(writeRegister(Register::Frame_Period_Max_Bound_Lower, shutter.period_max & 0xff)); + RF_CALL(writeRegister(Register::Frame_Period_Max_Bound_Upper, shutter.period_max >> 8)); + + } else { + shutter.period_max = period_max_recover; + } + + RF_END(); + } + + modm::ResumableResult + setFramePeriodMin(const uint16_t period_min) { + RF_BEGIN(); + + const uint16_t period_min_recover = period_min; + shutter.period_min = period_min; + + if (shutter.isValid()) { + // @todo use SpiMaster 16 bit transfer @see #690 + RF_CALL(writeRegister(Register::Frame_Period_Min_Bound_Lower, shutter.period_min & 0xff)); + RF_CALL(writeRegister(Register::Frame_Period_Min_Bound_Upper, shutter.period_min >> 8)); + + } else { + shutter.period_min = period_min_recover; + } + + RF_END(); + } + + modm::ResumableResult + setExposureMax(const uint16_t shutter_max) { + RF_BEGIN(); + + const uint16_t shutter_max_recover = shutter_max; + shutter.shutter_max = shutter_max; + + if (shutter.assert()) { + // @todo use SpiMaster 16 bit transfer @see #690 + RF_CALL(writeRegister(Register::Shutter_Max_Bound_Lower, shutter.shutter_max & 0xff)); + RF_CALL(writeRegister(Register::Shutter_Max_Bound_Upper, shutter.shutter_max >> 8)); + + } else { + shutter.shutter_max = shutter_max_recover; + } + + RF_END(); + } + + /** + * Enable or disable the Angle Snapping function. When enabled, actual movement ranges from ±5° from X or Y-axis, + * it will be snapped to the closest axis. For example, if the sensor moves at ±2° from X-axis, the output motion + * data will be snapped to X-axis at 0°. + */ + modm::ResumableResult + setSnapAngle(bool value) { + RF_BEGIN(); + + const uint8_t preserved = RF_CALL(readRegister(Register::Snap_Angle)) & ~0x80; + RF_END_RETURN_CALL(writeRegister(Register::Snap_Angle, preserved | value ? 0x80 : 0x00)); + } + + modm::ResumableResult + hasNewMotionData() { + RF_BEGIN(); + + const uint8_t motion_reg = RF_CALL(readRegister(Register::Motion)); + + RF_END_RETURN(motion_reg & Bit7); + } + + /** + * Read the latest motion delta from the sensor and update data with it content + * Reading the motion delta also reloads the Motion interrupt (Pin7) + */ + modm::ResumableResult + read() { + RF_BEGIN(); + + RF_WAIT_UNTIL(this->acquireMaster()); + RF_WAIT_UNTIL(timeout_next_read.isExpired()); + + Cs::reset(); + RF_CALL(SpiMaster::transfer(static_cast(Register::Motion_Burst))); + RF_SLEEP_FOR(tSRAD); + RF_CALL(SpiMaster::transfer(nullptr, buffer, 6)); + data.x = buffer[3] << 8 | buffer[2]; + data.y = buffer[5] << 8 | buffer[4]; + RF_END_RETURN_CALL(readConclusion()); + } + + /* modm::ResumableResult + captureFrame(uint8_t &frame_buffer) { + RF_BEGIN(); + + RF_WAIT_UNTIL(this->acquireMaster()); + RF_WAIT_UNTIL(timeout_next_read.isExpired()); + + Cs::reset(); + RF_CALL(writeRegister(Register::Frame_Capture, 0x93)); + RF_CALL(writeRegister(Register::Frame_Capture, 0xc5)); + RF_WAIT_UNTIL(readRegister(Register::Motion) & Bit0); // wait for first pixel + // Continue read from Pixel_Burst register until all 900 pixels are transferred. + + RF_END(); + } */ + +private: + modm::ResumableResult + readConclusion() { + RF_BEGIN(); + + timeout_next_read.restart(tSRR); + timeout_next_write.restart(tSRW); + RF_SLEEP_FOR(tSCLK_NCS_read); + Cs::set(); + + RF_END(); + } + + modm::ResumableResult + writeConclusion() { + RF_BEGIN(); + + /** + * @optimize Here's some margin for speedups: + * According to the datasheet, tSWR and tSWW range until THE END of transmission of next address register. + * @see datasheet page 16 + * + * If the SpiMaster's current baudrate is known, the timeouts can be reduced by the calculated tByte: + * + * timeout_next_read.restart(tSWR - tByte); + * timeout_next_write.restart(tSWW - 2 * tByte); + */ + + timeout_next_read.restart(tSWR); + timeout_next_write.restart(tSWW); + + RF_SLEEP_FOR(tSCLK_NCS_write); + Cs::set(); + + RF_END(); + } + + modm::ResumableResult + readRegister(Register reg) { + RF_BEGIN(); + + RF_WAIT_UNTIL(this->acquireMaster()); + RF_WAIT_UNTIL(timeout_next_read.isExpired()); + + Cs::reset(); + RF_CALL(SpiMaster::transfer(static_cast(reg) & ~Bit7)); // Bit7(MSBit) = 0 indicates a read + RF_SLEEP_FOR(tSRAD); + RF_CALL(SpiMaster::transfer(nullptr, buffer, 1)); + RF_CALL(readConclusion()); + + RF_END_RETURN(buffer[0]); + } + + modm::ResumableResult + writeRegister(Register const reg, uint8_t const data) { + RF_BEGIN(); + + RF_WAIT_UNTIL(this->acquireMaster()); + RF_WAIT_UNTIL(timeout_next_write.isExpired()); + + Cs::reset(); + RF_CALL(SpiMaster::transfer(static_cast(reg) | Bit7)); // Bit7(MSBit) = 1 indicates a write + RF_CALL(SpiMaster::transfer(data)); + RF_END_RETURN_CALL(writeConclusion()); + } + + modm::ResumableResult + writeFirmware() { + RF_BEGIN(); + + RF_CALL(writeRegister(Register::Configuration_IV, 0x02)); // set 3k firmware mode + RF_CALL(writeRegister(Register::SROM_Enable, 0x1d)); // Initialize SROM + + // @todo wait for one frame + // 1s * shutter.getExposureMax / Clock + // 10us is arbitrary + static constexpr std::chrono::nanoseconds tFrame(10us); + RF_SLEEP_FOR(tFrame); + + RF_CALL(writeRegister(Register::SROM_Enable, 0x18)); // Initiate SROM download + + RF_WAIT_UNTIL(this->acquireMaster()); + RF_WAIT_UNTIL(timeout_next_write.isExpired()); + + Cs::reset(); + RF_CALL(SpiMaster::transfer(static_cast(Register::SROM_Load_Burst) | Bit7)); // Bit7(MSBit) = 1 indicates a write + accessor::Flash flash(adns9800_firmware); + for(size_t ii = 0; ii < sizeof(adns9800_firmware); ++ii) + { + RF_SLEEP_FOR(15us); + RF_CALL(SpiMaster::transfer(*flash++)); + } + // @warning Datasheets says that there's 160us waittime + // after firmware write until next read, not default tSWR (120us) + RF_CALL(writeConclusion()); + + RF_END(); + } + + Data &data; + uint8_t buffer[6]; + + // defaults from datasheet P22 + Shutter shutter{ + period_min: 4000, // 0xa00f + period_max: 24000, // 0xc05d + exposure_max: 20000 // 0x204e + }; + + RF_SLEEP_FOR_TIMER; + modm::ShortTimeout timeout_next_read{0s}, timeout_next_write{0s}; +}; + +} // namespace modm \ No newline at end of file diff --git a/src/modm/driver/motion/adns9800_data.hpp b/src/modm/driver/motion/adns9800_data.hpp new file mode 100644 index 0000000000..70ab2982d3 --- /dev/null +++ b/src/modm/driver/motion/adns9800_data.hpp @@ -0,0 +1,134 @@ +// coding: utf-8 +/* + * Copyright (c) 2024, Thomas Sommer + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#pragma once + +#include +#include +#include +#include + +#include "adns9800.hpp" + +/** + * You can stream Data in 3 levels of detail with increasing size: Data, + * DataObserve and DataAnalysis. Of course, there's a tradeof with RAM + * consumption and bus time. + * + * @author Thomas Sommer + * + * @ingroup modm_driver_adns9800 + */ +namespace modm { + +/// Minimum Payload: only retrieve relative movement from the sensor +struct adns9800::Data { + modm::Vector delta; + + +protected: + static constexpr size_t length = 6; + Data(std::span buffer) + : delta{ + buffer[3] << 8 | buffer[2], // delta x + buffer[5] << 8 | buffer[4] // delta y + } {} + + template + friend class Adns9800; +}; + +/// Retrieve relative movement from the sensor plus Laser fault detection +struct adns9800::DataAndFaildetect : public adns9800::Data { + // @todo Looks wastefull - use a bitfield or similar instead + const bool LaserFaultDetected; + const bool LaserPowerValid; + const bool isRunningSROMCode; + +protected: + DataAndFaildetect(std::span buffer) + : Data(buffer), + LaserFaultDetected(buffer[0] & Bit6), + LaserPowerValid(buffer[0] & Bit5), + isRunningSROMCode(buffer[1] & Bit6) { + } + + template + friend class Adns9800; +}; + +/// Retrieve relative movement from the sensor, Laser fault detection +/// and monitoring metrics from the shutter unit. +struct adns9800::DataAndFaildetectAndMonitoring + : public adns9800::DataAndFaildetect { + struct Statistics { + /** + * The SQUAL (Surface quality) register is a measure of the number of valid + * features visible by the sensor in the current frame. Use the following + * formula to find the total number of valid features. Number of Features = + * SQUAL Register Value * 4 The maximum SQUAL register value is 169. Since + * small changes in the current frame can result in changes in SQUAL, + * variations in SQUAL when looking at a surface are expected. The graph + * below shows 800 sequentially acquired SQUAL values, while a sensor was + * moved slowly over white paper. SQUAL is nearly equal to zero if there is + * no surface below the sensor. SQUAL remains fairly high throughout the + * Z-height range which allows illumination of most pixels in the sensor. + */ + uint8_t surface_quality; + /** + * This register is used to find the average pixel value. It reports the + * upper byte of a 17-bit counter which sums all 900 pixels in the current + * frame. It may be described as the full sum divided by 512. To find the + * average pixel value, follows the formula below. Average Pixel = Register + * Value * 512 / 900 @ Register Value / 1.76 The maximum register value is + * 223 (127 * 900 / 512 truncated to an integer). The minimum register value + * is 0. The pixel sum value can change every frame. + */ + uint8_t pixel_sum; + /** + * Minium and maximum Pixel value in current frame. Range: + * 0 to 127. + * The minimum and maximum pixel value can change every frame. + */ + uint8_t max_pixel; + uint8_t min_pixel; + } statistics; + + // Automated shutter selections + struct Shutter { + /// The current exposure in cycles of Clock. + /// exposure <= Adns9800::Shutter::exposure_max + uint16_t exposure; + /// The Frame period in cycles of Clock. + /// Adns9800::Shutter::period_min <= period <= Adns9800::Shutter::period_max + uint16_t period; + } shutter; + +protected: + static constexpr size_t length = 14; + DataAndFaildetectAndMonitoring(std::span buffer) + : DataAndFaildetect(buffer.subspan<0, DataAndFaildetect::length>()), + statistics{ + surface_quality : buffer[6], + pixel_sum : buffer[7], + max_pixel : buffer[8], + min_pixel : buffer[9] + }, + shutter{ + exposure : buffer[10] << 8 | buffer[11], + period : buffer[12] << 8 | buffer[13], + } {} + + template + friend class Adns9800; +}; +} // namespace modm \ No newline at end of file diff --git a/src/modm/driver/motion/adns9800_srom_a4.hpp b/src/modm/driver/motion/adns9800_firmware_a4.cpp similarity index 99% rename from src/modm/driver/motion/adns9800_srom_a4.hpp rename to src/modm/driver/motion/adns9800_firmware_a4.cpp index 4252c8c10f..35043f5336 100644 --- a/src/modm/driver/motion/adns9800_srom_a4.hpp +++ b/src/modm/driver/motion/adns9800_firmware_a4.cpp @@ -8,13 +8,9 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ // ---------------------------------------------------------------------------- +#include -#ifndef ADNS_9800_SROM_A4_HPP -#define ADNS_9800_SROM_A4_HPP - -static constexpr uint16_t firmware_length = 3070; - -static constexpr uint8_t firmware_data[firmware_length] = +FLASH_STORAGE(uint8_t adns9800_firmware[]) = { 0x03, 0xa4, 0x6e, 0x16, 0x6d, 0x41, 0xf4, 0x8f, 0x95, 0x6a, 0xa1, 0xe1, 0x5c, 0xeb, 0xb1, 0x2a, 0xa0, 0xb4, 0x16, 0x82, 0x62, 0x71, 0x5d, 0x0b, 0xbe, 0x01, 0xeb, 0x4a, 0x61, 0x54, 0x42, 0x98, @@ -208,5 +204,4 @@ static constexpr uint8_t firmware_data[firmware_length] = 0x27, 0xcc, 0xfb, 0x55, 0x09, 0x71, 0x41, 0xe1, 0x21, 0xa1, 0xa1, 0xc0, 0xe3, 0x25, 0xa9, 0xd0, 0x22, 0xc6, 0xef, 0x5c, 0x1b, 0xb4, 0xea, 0x56, 0x2e, 0xbf, 0xfc, 0x5b, 0x34, 0xcb, 0x14, 0x8b, 0x94, 0xaa, 0xb7, 0xec, 0x5a, 0x36, 0xcf, 0x1c, 0xba, 0xf6, 0x9f, 0xdc, 0x35, 0xf3, -}; -#endif +}; \ No newline at end of file diff --git a/src/modm/driver/motion/adns9800_srom_a4b.hpp b/src/modm/driver/motion/adns9800_firmware_a4b.cpp similarity index 99% rename from src/modm/driver/motion/adns9800_srom_a4b.hpp rename to src/modm/driver/motion/adns9800_firmware_a4b.cpp index 1c611c6806..d1d29dac4c 100644 --- a/src/modm/driver/motion/adns9800_srom_a4b.hpp +++ b/src/modm/driver/motion/adns9800_firmware_a4b.cpp @@ -8,13 +8,9 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ // ---------------------------------------------------------------------------- +#include -#ifndef ADNS_9800_SROM_A4B_HPP -#define ADNS_9800_SROM_A4B_HPP - -static constexpr uint16_t firmware_length = 3070; - -static constexpr uint8_t firmware_data[firmware_length] = +FLASH_STORAGE(uint8_t adns9800_firmware[]) = { 0x03, 0xa4, 0x6e, 0x16, 0x6d, 0x89, 0x3e, 0xfe, 0x5f, 0x1c, 0xb8, 0xf2, 0x47, 0x0c, 0x7b, 0x74, 0x6a, 0x56, 0x0f, 0x7d, 0x76, 0x71, 0x4b, 0x0c, 0x97, 0xb6, 0xcf, 0xfd, 0x78, 0x72, 0x66, 0x2f, @@ -208,5 +204,4 @@ static constexpr uint8_t firmware_data[firmware_length] = 0x27, 0xcc, 0xfb, 0x55, 0x09, 0x71, 0x41, 0xe1, 0x21, 0xa1, 0xa1, 0xc0, 0xe3, 0x25, 0xa9, 0xd0, 0x22, 0xc6, 0xef, 0x5c, 0x1b, 0xb4, 0xea, 0x56, 0x2e, 0xbf, 0xfc, 0x5b, 0x34, 0xcb, 0x14, 0x8b, 0x94, 0xaa, 0xb7, 0xec, 0x5a, 0x36, 0xcf, 0x1c, 0xba, 0xf6, 0x4d, 0xdb, 0x35, 0xf3 -}; -#endif +}; \ No newline at end of file diff --git a/src/modm/driver/motion/adns9800_srom_a5.hpp b/src/modm/driver/motion/adns9800_firmware_a5.cpp similarity index 99% rename from src/modm/driver/motion/adns9800_srom_a5.hpp rename to src/modm/driver/motion/adns9800_firmware_a5.cpp index 53c7f87810..3eab773a9c 100644 --- a/src/modm/driver/motion/adns9800_srom_a5.hpp +++ b/src/modm/driver/motion/adns9800_firmware_a5.cpp @@ -8,13 +8,9 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ // ---------------------------------------------------------------------------- +#include -#ifndef ADNS_9800_SROM_A5_HPP -#define ADNS_9800_SROM_A5_HPP - -static constexpr uint16_t firmware_length = 3070; - -static constexpr uint8_t firmware_data[firmware_length] = +FLASH_STORAGE(uint8_t adns9800_firmware[]) = { 0x03, 0xa5, 0x6d, 0x12, 0x65, 0x99, 0x1e, 0xbe, 0xdf, 0x1c, 0xb8, 0xf2, 0x47, 0x0c, 0x9a, 0x97, 0x8d, 0x79, 0x51, 0x01, 0x8e, 0x81, 0x8a, 0x8e, 0x93, 0xbe, 0xdf, 0x3c, 0xdb, 0x34, 0xea, 0x56, @@ -208,5 +204,4 @@ static constexpr uint8_t firmware_data[firmware_length] = 0x54, 0x6b, 0x18, 0x2c, 0xa4, 0x52, 0x22, 0x78, 0x08, 0x77, 0xb8, 0x52, 0x09, 0x4f, 0xe9, 0x06, 0x82, 0x65, 0xed, 0x39, 0xd1, 0x20, 0xa3, 0xc4, 0xeb, 0x35, 0xe8, 0x52, 0x26, 0xaf, 0xdc, 0x3a, 0xf6, 0x4f, 0xfd, 0x59, 0x30, 0xe2, 0x46, 0x0e, 0x7f, 0x5d, 0x6f, 0xda, 0x50, 0x7e -}; -#endif +}; \ No newline at end of file diff --git a/src/modm/driver/motion/adns9800_srom_a6.hpp b/src/modm/driver/motion/adns9800_firmware_a6.cpp similarity index 99% rename from src/modm/driver/motion/adns9800_srom_a6.hpp rename to src/modm/driver/motion/adns9800_firmware_a6.cpp index ddb11902fe..6ae60b2cbd 100644 --- a/src/modm/driver/motion/adns9800_srom_a6.hpp +++ b/src/modm/driver/motion/adns9800_firmware_a6.cpp @@ -8,13 +8,9 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ // ---------------------------------------------------------------------------- +#include -#ifndef ADNS_9800_SROM_A6_HPP -#define ADNS_9800_SROM_A6_HPP - -static constexpr uint16_t firmware_length = 3070; - -static constexpr uint8_t firmware_data[firmware_length] = +FLASH_STORAGE(uint8_t adns9800_firmware[]) = { 0x03, 0xa6, 0x68, 0x1e, 0x7d, 0x10, 0x7e, 0x7e, 0x5f, 0x1c, 0xb8, 0xf2, 0x47, 0x0c, 0x7b, 0x74, 0x4b, 0x14, 0x8b, 0x75, 0x66, 0x51, 0x0b, 0x8c, 0x76, 0x74, 0x4b, 0x14, 0xaa, 0xd6, 0x0f, 0x9c, @@ -208,5 +204,4 @@ static constexpr uint8_t firmware_data[firmware_length] = 0x76, 0xa7, 0xa5, 0xcc, 0x62, 0x13, 0x00, 0x60, 0x31, 0x58, 0x44, 0x9b, 0xf5, 0x64, 0x14, 0xf5, 0x11, 0xc5, 0x54, 0x52, 0x83, 0xd4, 0x73, 0x01, 0x16, 0x0e, 0xb3, 0x7a, 0x29, 0x69, 0x35, 0x56, 0xd4, 0xee, 0x8a, 0x17, 0xa2, 0x99, 0x24, 0x9c, 0xd7, 0x8f, 0xdb, 0x55, 0xb5, 0x3e, -}; -#endif +}; \ No newline at end of file diff --git a/src/modm/driver/motion/adns9800_impl.hpp b/src/modm/driver/motion/adns9800_impl.hpp deleted file mode 100644 index 1456202df0..0000000000 --- a/src/modm/driver/motion/adns9800_impl.hpp +++ /dev/null @@ -1,220 +0,0 @@ -/* - * Copyright (c) 2016, 2018, Sascha Schade - * Copyright (c) 2018, Niklas Hauser - * - * This file is part of the modm project. - * - * This Source Code Form is subject to the terms of the Mozilla Public - * License, v. 2.0. If a copy of the MPL was not distributed with this - * file, You can obtain one at http://mozilla.org/MPL/2.0/. - */ -// ---------------------------------------------------------------------------- - -/** - * Based on work of Alexander Entinger, MSc / LXRobotics - * Based on https://github.com/mrjohnk/ADNS-9800 - */ - -#ifndef MODM_ADNS9800_HPP -#error "Don't include this file directly, use 'adns9800.hpp' instead!" -#endif - -#include -#include - -#include "adns9800.hpp" -#include "adns9800_srom.hpp" - -// Set the log level -#undef MODM_LOG_LEVEL -#define MODM_LOG_LEVEL modm::log::DEBUG - -template < typename Spi, typename Cs > -bool -modm::Adns9800< Spi, Cs>::isNewMotionDataAvailable() -{ - uint8_t const motion_reg = readReg(Register::Motion); - bool const new_data_available = (motion_reg & 0x80) > 0; - return new_data_available; -} - -template < typename Spi, typename Cs > -void -modm::Adns9800< Spi, Cs>::getDeltaXY(int16_t &delta_x, int16_t &delta_y) -{ - Cs::reset(); - modm::delay_us(100); // tSRAD - - Spi::transferBlocking(static_cast(Register::Motion_Burst)); - - // Delay tSRAD - modm::delay_us(100); - - static constexpr uint8_t buf_size = 6; - uint8_t tx_buf[buf_size]; - uint8_t rx_buf[buf_size]; - - Spi::transferBlocking(tx_buf, rx_buf, buf_size); - - modm::delay_ns(120); // tSCLK-NCS for read operation is 120ns - Cs::set(); - - uint8_t delta_x_l = rx_buf[2]; - uint8_t delta_x_h = rx_buf[3]; - uint8_t delta_y_l = rx_buf[4]; - uint8_t delta_y_h = rx_buf[5]; - - delta_x = delta_x_h << 8 | delta_x_l; - delta_y = delta_y_h << 8 | delta_y_l; -} - -template < typename Spi, typename Cs > -uint8_t -modm::Adns9800< Spi, Cs>::readReg(Register const reg) -{ - Cs::reset(); - - uint8_t address = static_cast(reg); - - // send adress of the register, with MSBit = 0 to indicate it's a read - Spi::transferBlocking(address & 0x7f); - modm::delay_us(100); // tSRAD - - // read data - uint8_t data = Spi::transferBlocking(0); - - modm::delay_ns(120); // tSCLK-NCS for read operation is 120ns - Cs::set(); - modm::delay_us(19); // tSRW/tSRR (=20us) minus tSCLK-NCS - - return data; -} - -template < typename Spi, typename Cs > -void -modm::Adns9800< Spi, Cs>::writeReg(Register const reg, uint8_t const data) -{ - Cs::reset(); - - uint8_t address = static_cast(reg); - - //send adress of the register, with MSBit = 1 to indicate it's a write - Spi::transferBlocking(address | 0x80); - - //send data - Spi::transferBlocking(data); - - modm::delay_us(20); // tSCLK-NCS for write operation - Cs::set(); - modm::delay_us(100); // tSWW/tSWR (=120us) minus tSCLK-NCS. Could be shortened, but is looks like a safe lower bound -} - -template < typename Spi, typename Cs > -void -modm::Adns9800< Spi, Cs >::uploadFirmware() -{ - // set the configuration_IV register in 3k firmware mode - writeReg(Register::Configuration_IV, 0x02); // bit 1 = 1 for 3k mode, other bits are reserved - - // write 0x1d in SROM_enable reg for initializing - writeReg(Register::SROM_Enable, 0x1d); - - // wait for more than one frame period - modm::delay_ms(10); // assume that the frame rate is as low as 100fps... even if it should never be that low - - // write 0x18 to SROM_enable to start SROM download - writeReg(Register::SROM_Enable, 0x18); - - // write the SROM file (=firmware data) - Cs::reset(); - - // write burst destination address - uint8_t address = static_cast(Register::SROM_Load_Burst) | 0x80; - Spi::transferBlocking(address); - modm::delay_us(15); - - // send all bytes of the firmware - for(int ii = 0; ii < firmware_length; ++ii) - { - Spi::transferBlocking(firmware_data[ii]); - modm::delay_us(15); - } - - Cs::set(); -} - -template < typename Spi, typename Cs > -bool -modm::Adns9800< Spi, Cs >::initialise() -{ - bool success = true; - - // ensure that the serial port is reset - Cs::set(); - Cs::reset(); - Cs::set(); - - writeReg(Register::Power_Up_Reset, 0x5a); // force reset - modm::delay_ms(50); // wait for it to reboot - - // Read Product ID - uint8_t id = readReg(Register::Product_ID); - static constexpr uint8_t id_expected = 0x33; - static constexpr uint8_t id_inverse = ~id_expected; - - if (id != id_expected) - { - MODM_LOG_ERROR.printf("Product Id = %02x. Expected %02x\n", id, id_expected); - success = false; - } - - static constexpr uint8_t rev_expected = 0x03; - id = readReg(Register::Revision_ID); - if (id != rev_expected) - { - MODM_LOG_DEBUG.printf("Revision Id = %02x. Expected %02x\n", id, rev_expected); - success = false; - } - - id = readReg(Register::Inverse_Product_ID); - if (id != id_inverse) - { - MODM_LOG_DEBUG.printf("Inverse Product Id = %02x. Expected %02x\n", id, ~id_expected); - success = false; - } - - // read registers 0x02 to 0x06 (and discard the data) - readReg(Register::Motion); - readReg(Register::Delta_X_L); - readReg(Register::Delta_X_H); - readReg(Register::Delta_Y_L); - readReg(Register::Delta_Y_H); - - uploadFirmware(); - modm::delay_ms(10); - - // enable laser(bit 0 = 0b), in normal mode (bits 3,2,1 = 000b) - // reading the actual value of the register is important because the real - // default value is different from what is said in the datasheet, and if you - // change the reserved bytes (like by writing 0x00...) it would not work. - - // Laser always on - // Do not read Motion - - uint8_t laser_ctrl0 = readReg(Register::LASER_CTRL0); - writeReg(Register::LASER_CTRL0, (laser_ctrl0 & 0xf0) | (0b0100) ); - - // Configure for robotics - // 8200 cpi - writeReg(Register::Configuration_I, 0xa4); - - // Fixed frame rate - writeReg(Register::Configuration_II, 0b00001000); - - writeReg(Register::Frame_Period_Max_Bound_Lower, 0x20); - writeReg(Register::Frame_Period_Max_Bound_Upper, 0x1b); - - modm::delay_ms(1); - - return success; -}