From 6ceed8e4f988f9073af81c6e4484bebb0c53dbb1 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 | 141 ++--- examples/blue_pill_f103/adns_9800/project.xml | 2 +- src/modm/driver/motion/adns9800.hpp | 501 +++++++++++++++--- src/modm/driver/motion/adns9800.lb | 30 +- src/modm/driver/motion/adns9800.rf.hpp | 484 +++++++++++++++++ src/modm/driver/motion/adns9800_data.hpp | 150 ++++++ ...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 -------- 11 files changed, 1122 insertions(+), 450 deletions(-) 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..1871b9c58d 100644 --- 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,21 +11,17 @@ #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 +29,51 @@ 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 adns9800; + modm::Vector2i position; - Board::LedGreen::set(); + Cs::setOutput(modm::Gpio::High); - PT_WAIT_UNTIL(timeout.isExpired()) ; - timeout.restart(4.9s); + SpiMaster1::connect(); + SpiMaster1::initialize(); + 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()}; + position += data.delta; - SpiMaster1::connect(); - SpiMaster1::initialize(); - 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(); 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..a49056e800 100644 --- a/examples/blue_pill_f103/adns_9800/project.xml +++ b/examples/blue_pill_f103/adns_9800/project.xml @@ -10,7 +10,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.hpp b/src/modm/driver/motion/adns9800.hpp index 1f79266698..0f7139f684 100644 --- a/src/modm/driver/motion/adns9800.hpp +++ b/src/modm/driver/motion/adns9800.hpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2016, Sascha Schade + * Copyright (c) 2024, Thomas Sommer * * This file is part of the modm project. * @@ -9,95 +9,438 @@ */ // ---------------------------------------------------------------------------- -#ifndef MODM_ADNS9800_HPP -#define MODM_ADNS9800_HPP +#pragma once -namespace modm -{ +#include +#include +#include +#include +#include +#include +#include + +EXTERN_FLASH_STORAGE(uint8_t adns9800_firmware[3070]); + +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, - }; +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 Dpi Dots per inch (aproximately!) + * Range: 200 to 8200 in steps of 200 + */ + template + requires (Dpi >= 200) and (Dpi % 200 == 0) and (Dpi <= 8200) + struct Resolution: public modm::Register8 { + Resolution() : modm::Register8(Dpi / 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); + + // Time periods as ticks of Adns9800 running at 50MHz + using Period = uint16_t; + using Duration = std::chrono::duration>; + using Delta = int16_t; + + using Pixel = modm::color::BrightnessT; + static constexpr size_t pixel_count = 30 * 30; + using FrameBuffer = std::array; + + /** + * @brief ShutterConfig boundaries which may be selected by the automatic frame rate control. + * 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 { + Period period_min; + Period period_max; + Period exposure_max; + + Duration getOneFrameTime() const { + return Duration(exposure_max); + } + + /// @cond + // Validate before write: There's no internal protection against malformed combinations. + 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 IC verification results containing success flags + struct Verification { + bool ProductId: 1; + bool ProductIdInverse: 1; + bool RevisionId: 1; + + operator bool() const { + return ProductId and ProductIdInverse and RevisionId; + } + }; + + // forward declarations + struct Data; + struct Data_FailFlags; + struct Data_FailFlags_Monitoring; }; -/// @ingroup modm_driver_adns9800 -template < typename Spi, typename Cs > -class Adns9800 : public adns9800 -{ +/** + * ADNS9800 Laser Motion Sensor + * + * @tparam SpiMaster + * @tparam Cs + * + * @author Thomas Sommer + * @ingroup modm_driver_adns9800 + */ +template +class Adns9800 : public adns9800, public modm::SpiDevice { + // @see Default values found in datasheet P22 + ShutterConfig shutter_config{ + period_min: 4000, // 0xa00f + period_max: 24000, // 0xc05d + exposure_max: 20000 // 0x204e + }; + + // Adns9800 has specific deadtimes between consequtive transactions. + // Timelocks are used to fulfill the requirements: + modm::ShortTimeout ready_to_read{0s}; + modm::ShortTimeout ready_to_write{0s}; + + 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(); + + std::forward(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::delay(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(); + + std::forward(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(); + } + + 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([&]() { + // Setting Bit7 indicates a write + SpiMaster::transfer(static_cast(reg) | Bit7); + 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([&]() { + // Setting Bit7 indicates a write + SpiMaster::transfer(static_cast(Register::SROM_Load_Burst) | Bit7); + + 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, a CRC of the firmware may be requested. @see datasheet P31 + } + public: - static bool - initialise(); + /// @brief Reset the device's internal state after power-loss or user invoked shutdown(). + void + powerUp() { + // power-up sequence @see datasheet P20 + Cs::reset(); + modm::delay(10ns); + Cs::set(); + modm::delay(10ns); - static bool - isNewMotionDataAvailable(); + writeRegister(Register::Power_Up_Reset, 0x5a); + modm::this_fiber::sleep_for(50ms); + read(); // read and discard motion data + } - static void - getDeltaXY(int16_t &delta_x, int16_t &delta_y); + /// @brief Put the device into low power mode. Do not use this for power management in normal operation. + void + shutdown() { + writeRegister(Register::Shutdown, 0xb6); + } -protected: - static uint8_t - readReg(Register const reg); + /// @brief Verify presence of the device by validating various id registers. + Verification + verify() { + return { + ProductId: readRegister(Register::Product_ID) == static_cast(0x33), + ProductIdInverse: readRegister(Register::Inverse_Product_ID) == static_cast(~0x33), + RevisionId: readRegister(Register::Revision_ID) == static_cast(0x03) + }; + } - static void - writeReg(Register const reg, uint8_t const data); + /// @brief Default protocoll to initialize the device + Verification + initialize() { + powerUp(); + const Verification verification = verify(); + if(verification) { + writeFirmware(); + laserEnable(); + } - static void - uploadFirmware(); -}; + return verification; + } + + void + laserEnable(const bool enable = true) { + LaserControl_t control(readRegister(Register::LASER_CTRL0)); + control.update(LaserControl::ForceDisable, !enable); + writeRegister(Register::LASER_CTRL0, control.value); + } + + void + laserAlwaysOn(const bool enable = true) { + LaserControl_t control(readRegister(Register::LASER_CTRL0)); + control.update(LaserControl::AlwaysOn, enable); + writeRegister(Register::LASER_CTRL0, control.value); + } + + /** + * 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 + snapAngle(const bool value = true) { + const uint8_t preserved = readRegister(Register::Snap_Angle) & ~0x80; + writeRegister(Register::Snap_Angle, preserved | value ? 0x80 : 0x00); + } + + 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(ShutterConfig shutter_new) { + if (shutter_new.isValid()) { + shutter_config = shutter_new; -} // modm namespace + 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 Period period_max) { + const Period recover = shutter_config.period_max; + shutter_config.period_max = period_max; + + if (shutter_config.isValid()) { + 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 Period period_min) { + const Period recover = shutter_config.period_min; + shutter_config.period_min = period_min; + + if (shutter_config.isValid()) { + 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 Period exposure_max) { + const Period recover = shutter_config.exposure_max; + shutter_config.exposure_max = exposure_max; + + if (shutter_config.isValid()) { + writeRegister(Register::Shutter_Max_Bound_Lower, shutter_config.exposure_max & 0xff); + writeRegister(Register::Shutter_Max_Bound_Upper, shutter_config.exposure_max >> 8); + } else { + shutter_config.exposure_max = recover; + } + } + + bool + hasNewMotionData() const { + return readRegister(Register::Motion) & Bit7; + } + + /** + * @brief Read a Data packet from the sensor. + * Calling read() also resets the motion interrupt (Pin7) + * + * @tparam D Data packet type to read. @see adns9800_data.hpp for details + */ + template + requires std::is_base_of_v + D + read() { + std::array buffer; + + readTransaction([this, &buffer]() { + SpiMaster::transfer(static_cast(Register::Motion_Burst)); + modm::this_fiber::sleep_for(shutter_config.getOneFrameTime()); + SpiMaster::transfer(nullptr, buffer.data(), buffer.size()); + }); + + return D(buffer); + } + + // @todo test captureFrame + /** + * @brief Capture the next frame into the SROM of Adns9800. Afterwards copy it to + * &buffer. Since this overwrites the Firmware, you have to restore normal + * operation by calling initialize(). + * + * @param buffer Buffer to store the captured frame + */ + void + captureFrame(FrameBuffer& buffer) { + readTransaction([&buffer]() { + writeRegister(Register::Frame_Capture, 0x93); + writeRegister(Register::Frame_Capture, 0xc5); + modm::this_fiber::poll(readRegister(Register::Motion) & Bit0); // wait until frame is captured + SpiMaster::transfer(nullptr, buffer.data(), buffer.size()); + }); + } +}; -#include "adns9800_impl.hpp" +} // namespace modm -#endif // MODM_ADNS9800_HPP +#include "adns9800_data.hpp" \ No newline at end of file diff --git a/src/modm/driver/motion/adns9800.lb b/src/modm/driver/motion/adns9800.lb index d22b64bc3b..34c596eb95 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,13 @@ 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. + +Great Video showing the working of such a sensor: https://youtu.be/SAaESb4wTCM """ def prepare(module, options): @@ -29,13 +32,22 @@ def prepare(module, options): default="a6")) module.depends( - ":architecture:delay", - ":architecture:spi", - ":debug") + ":processing:resumable", + ":architecture:spi.device", + ":math:geometry", + ":ui:color" + ) 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_firmware_{}.cpp".format(env["firmware"]), "adns9800_firmware.cpp") + + if not env.has_module(":processing:fiber"): + env.copy("adns9800.rf.hpp", "adns9800.hpp") + return + env.copy("adns9800.hpp") - env.copy("adns9800_impl.hpp") - env.copy("adns9800_srom_{}.hpp".format(env["firmware"]), "adns9800_srom.hpp") + env.copy("adns9800_data.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..b50442c24c --- /dev/null +++ b/src/modm/driver/motion/adns9800.rf.hpp @@ -0,0 +1,484 @@ +/* + * 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 ADNS9800_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); + + // Time periods as ticks of Adns9800 running at 50MHz + using Period = uint16_t; + using Duration = std::chrono::duration>; + using Delta = int16_t; + + using Pixel = modm::color::BrightnessT; + static constexpr size_t pixel_count = 30 * 30; + using FrameBuffer = std::array; + + /** + * @brief ShutterConfig boundaries which may be selected by the automatic frame rate control. + * 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 { + Period period_min; + Period period_max; + Period exposure_max; + + Duration getOneFrameTime() const { + return Duration(exposure_max); + } + + /// @cond + // Validate before write: There's no internal protection against malformed combinations. + 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 IC verification results containing success flags + struct Verification { + bool ProductId: 1; + bool ProductIdInverse: 1; + bool RevisionId: 1; + + operator bool() const { + return ProductId and ProductIdInverse and RevisionId; + } + }; + + 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> { + Data &data; + uint8_t buffer[6]; + + // defaults from datasheet P22 + Shutter shutter{ + period_min: 4000, // 0xa00f + period_max: 24000, // 0xc05d + exposure_max: 20000 // 0x204e + }; + + modm::ShortTimeout timeout_next_read{0s}, timeout_next_write{0s}; + + modm::ResumableResult + readTransacionEnd() { + RF_BEGIN(); + + timeout_next_read.restart(20us); + timeout_next_write.restart(20us); + + modm::sleep(120ns); + Cs::set(); + + RF_END(); + } + + modm::ResumableResult + writeTransactionEnd() { + RF_BEGIN(); + + timeout_next_read.restart(120us); + timeout_next_write.restart(20us); + + ADNS9800_RF_SLEEP_FOR(20us); + Cs::set(); + + RF_END(); + } + + modm::ResumableResult + readRegister(Register reg) { + RF_BEGIN(); + + RF_WAIT_UNTIL(timeout_next_read.isExpired()); + RF_WAIT_UNTIL(this->acquireMaster()); + + Cs::reset(); + RF_CALL(SpiMaster::transfer(static_cast(reg) & ~Bit7)); // Bit7(MSBit) = 0 indicates a read + ADNS9800_RF_SLEEP_FOR(100us); // tSRAD [time] [S]pi between [R]ead [A]dress and [D]ata + RF_CALL(SpiMaster::transfer(nullptr, buffer, 1)); + RF_CALL(readTransacionEnd()); + + RF_END_RETURN(buffer[0]); + } + + modm::ResumableResult + writeRegister(Register const reg, uint8_t const data) { + RF_BEGIN(); + + RF_WAIT_UNTIL(timeout_next_write.isExpired()); + RF_WAIT_UNTIL(this->acquireMaster()); + + 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(writeTransactionEnd()); + } + + 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 + + ADNS9800_RF_SLEEP_FOR(shutter_config.getOneFrameTime()); + + RF_CALL(writeRegister(Register::SROM_Enable, 0x18)); // Initiate SROM download + + RF_WAIT_UNTIL(timeout_next_write.isExpired()); + RF_WAIT_UNTIL(this->acquireMaster()); + + 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) + { + ADNS9800_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(writeTransactionEnd()); + + RF_END(); + } + +public: + Adns9800(Data &data) : data(data) + {} + + modm::ResumableResult + powerUp() { + RF_BEGIN(); + + // @see power-up sequence, datasheet page 20 + RF_CALL(writeRegister(Register::Power_Up_Reset, 0x5a)); + ADNS9800_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)); + } + + modm::ResumableResult + initialize() { + RF_BEGIN(); + + RF_CALL(powerUp()); + RF_CALL(verify()); + RF_CALL(writeFirmware()); + ADNS9800_RF_SLEEP_FOR(10ms); + + RF_END(); + } + + ResumableResult + verify() { + RF_BEGIN(); + + Verification verification; + verification.ProductId = RF_CALL(readRegister(Register::Product_ID)) == uint8_t(0x33); + verification.ProductIdInverse = RF_CALL(readRegister(Register::Inverse_Product_ID)) == uint8_t(~0x33); + verification.RevisionId = RF_CALL(readRegister(Register::Revision_ID)) == uint8_t(0x03); + + RF_END_RETURN(verification); + } + + 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)); + } + + /** + * 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(const bool value = true) { + 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 + 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 exposure_max) { + RF_BEGIN(); + + const uint16_t exposure_max_recover = exposure_max; + shutter.exposure_max = exposure_max; + + if (shutter.isValid()) { + // @todo use SpiMaster 16 bit transfer @see #690 + RF_CALL(writeRegister(Register::exposure_max_Bound_Lower, shutter.exposure_max & 0xff)); + RF_CALL(writeRegister(Register::exposure_max_Bound_Upper, shutter.exposure_max >> 8)); + + } else { + shutter.exposure_max = exposure_max_recover; + } + + RF_END(); + } + + 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(timeout_next_read.isExpired()); + RF_WAIT_UNTIL(this->acquireMaster()); + + Cs::reset(); + RF_CALL(SpiMaster::transfer(static_cast(Register::Motion_Burst))); + ADNS9800_RF_SLEEP_FOR(100us); // tSRAD [time] [S]pi between [R]ead [A]dress and [D]ata + 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(readTransacionEnd()); + } + + // @todo test captureFrame + /** + * @brief Capture the next frame into the SROM of Adns9800. Afterwards copy it to + * &buffer. Since this overwrites the Firmware, you have to restore normal + * operation by calling initialize(). + * + * @param buffer Buffer to store the captured frame + */ + modm::ResumableResult + captureFrame(FrameBuffer &buffer) { + RF_BEGIN(); + + RF_WAIT_UNTIL(timeout_next_read.isExpired()); + RF_WAIT_UNTIL(this->acquireMaster()); + + Cs::reset(); + RF_CALL(writeRegister(Register::Frame_Capture, 0x93)); + RF_CALL(writeRegister(Register::Frame_Capture, 0xc5)); + RF_WAIT_UNTIL(readRegister(Register::Motion) & Bit0); // wait until frame is captured + RF_CALL(SpiMaster::transfer(nullptr, buffer.data(), buffer.size())); + + RF_END_RETURN_CALL(readTransacionEnd()); + } +}; + +} // 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..2894345e72 --- /dev/null +++ b/src/modm/driver/motion/adns9800_data.hpp @@ -0,0 +1,150 @@ +// 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" + +/** + * @brief Control the amount of information to stream from the sensor + * by choosing one of three Data types with increasing details. + * @author Thomas Sommer + * @ingroup modm_driver_adns9800 + */ +namespace modm +{ + +/** + * @brief The smallest data packet contains: + * - relative motion vector + */ +struct adns9800::Data +{ + using Span = std::span; + + const modm::Vector delta; + +protected: + Data(Span data) + : delta{ + static_cast(data[3] << 8 | data[2]), + static_cast(data[5] << 8 | data[4]) + } + {} + + template + friend class Adns9800; +}; + +/** + * @brief This data packet contains: + * - relative motion vector + * - laser fault detection flags + */ +struct adns9800::Data_FailFlags : adns9800::Data +{ + const bool LaserFaultDetected : 1; + const bool LaserPowerValid : 1; + const bool isRunningSROMCode : 1; + +protected: + Data_FailFlags(Span data) + : Data(data), + LaserFaultDetected(data[0] & Bit6), + LaserPowerValid(data[0] & Bit5), + isRunningSROMCode(data[1] & Bit6) + {} + + template + friend class Adns9800; +}; + +/** + * @brief The biggest data packet contains: + * - relative motion vector + * - laser fault detection flags + * - shutter metrics like exposure time or image mean + */ +struct adns9800::Data_FailFlags_Monitoring : adns9800::Data_FailFlags +{ + using Span = std::span; + + struct Metrics + { + /** + * Number of features visible by the sensor in the current frame. Range 0 to 169. + * Total number of features: 4 * surface_quality. + * Changes are expected when moving over a surface. + * Convergates to 0 if there is no surface below the sensor. + * surface_quality remains fairly high throughout the Z-height. + */ + const uint8_t surface_quality; + // pixel_sum containes the average pixel value. Range 0 to 223. + // It reports the upper byte of a 17-bit counter which sums all 900 pixels in the current + // frame + const uint8_t pixel_sum; + // Minium and maximum Pixel value in current frame. Range: 0 to 127. + const Pixel max_pixel; + const Pixel min_pixel; + } metrics; + + Pixel + getMean() const + { + return metrics.pixel_sum << 9 / pixel_count; + } + + struct Shutter + { + // exposure <= ShutterConfig::exposure_max! + const Period exposure; + // ShutterConfig::period_min <= period <= ShutterConfig::period_max! + const Period period; + } shutter; + + Duration + getExposureTime() const + { + return Duration(shutter.exposure); + }; + Duration + getFrameTime() const + { + return Duration(shutter.period); + }; + +protected: + Data_FailFlags_Monitoring(Span data) + : Data_FailFlags(data.subspan<0, Data_FailFlags::Span::extent>()), + metrics{ + // @optimize: construct the Data class in union with the buffer, + // -> no more byte copies required: + surface_quality : data[6], + pixel_sum : data[7], + max_pixel : data[8], + min_pixel : data[9] + }, + shutter{ + exposure : static_cast(data[10] << 8 | data[11]), + period : static_cast(data[12] << 8 | data[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; -}