From 77f082a9845e83e82e03fb4c396451edf7c1812f Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Fri, 21 May 2021 17:35:01 +0100 Subject: [PATCH] Initial setup for PMW/PAA flow sensor --- drivers/CMakeLists.txt | 1 + drivers/pmw3901/CMakeLists.txt | 1 + drivers/pmw3901/pmw3901.cmake | 10 + drivers/pmw3901/pmw3901.cpp | 365 ++++++++++++++++++ drivers/pmw3901/pmw3901.hpp | 103 +++++ examples/CMakeLists.txt | 1 + examples/breakout_pmw3901/CMakeLists.txt | 16 + examples/breakout_pmw3901/demo.cpp | 28 ++ libraries/CMakeLists.txt | 2 + libraries/breakout_paa5100/CMakeLists.txt | 1 + .../breakout_paa5100/breakout_paa5100.cmake | 11 + .../breakout_paa5100/breakout_paa5100.cpp | 5 + .../breakout_paa5100/breakout_paa5100.hpp | 8 + libraries/breakout_pmw3901/CMakeLists.txt | 1 + .../breakout_pmw3901/breakout_pmw3901.cmake | 11 + .../breakout_pmw3901/breakout_pmw3901.cpp | 5 + .../breakout_pmw3901/breakout_pmw3901.hpp | 8 + 17 files changed, 577 insertions(+) create mode 100644 drivers/pmw3901/CMakeLists.txt create mode 100644 drivers/pmw3901/pmw3901.cmake create mode 100644 drivers/pmw3901/pmw3901.cpp create mode 100644 drivers/pmw3901/pmw3901.hpp create mode 100644 examples/breakout_pmw3901/CMakeLists.txt create mode 100644 examples/breakout_pmw3901/demo.cpp create mode 100644 libraries/breakout_paa5100/CMakeLists.txt create mode 100644 libraries/breakout_paa5100/breakout_paa5100.cmake create mode 100644 libraries/breakout_paa5100/breakout_paa5100.cpp create mode 100644 libraries/breakout_paa5100/breakout_paa5100.hpp create mode 100644 libraries/breakout_pmw3901/CMakeLists.txt create mode 100644 libraries/breakout_pmw3901/breakout_pmw3901.cmake create mode 100644 libraries/breakout_pmw3901/breakout_pmw3901.cpp create mode 100644 libraries/breakout_pmw3901/breakout_pmw3901.hpp diff --git a/drivers/CMakeLists.txt b/drivers/CMakeLists.txt index 948b56ed..3c89e768 100644 --- a/drivers/CMakeLists.txt +++ b/drivers/CMakeLists.txt @@ -3,6 +3,7 @@ add_subdirectory(esp32spi) add_subdirectory(ioexpander) add_subdirectory(ltp305) add_subdirectory(ltr559) +add_subdirectory(pmw3901) add_subdirectory(sgp30) add_subdirectory(st7735) add_subdirectory(st7789) diff --git a/drivers/pmw3901/CMakeLists.txt b/drivers/pmw3901/CMakeLists.txt new file mode 100644 index 00000000..9a206ea9 --- /dev/null +++ b/drivers/pmw3901/CMakeLists.txt @@ -0,0 +1 @@ +include(pmw3901.cmake) diff --git a/drivers/pmw3901/pmw3901.cmake b/drivers/pmw3901/pmw3901.cmake new file mode 100644 index 00000000..5313cc4e --- /dev/null +++ b/drivers/pmw3901/pmw3901.cmake @@ -0,0 +1,10 @@ +set(DRIVER_NAME pmw3901) +add_library(${DRIVER_NAME} INTERFACE) + +target_sources(${DRIVER_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/${DRIVER_NAME}.cpp) + +target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR}) + +# Pull in pico libraries that we need +target_link_libraries(${DRIVER_NAME} INTERFACE pico_stdlib hardware_spi hardware_pwm hardware_dma) diff --git a/drivers/pmw3901/pmw3901.cpp b/drivers/pmw3901/pmw3901.cpp new file mode 100644 index 00000000..cc620489 --- /dev/null +++ b/drivers/pmw3901/pmw3901.cpp @@ -0,0 +1,365 @@ +#include "pmw3901.hpp" + +#include +#include +#include +#include + +namespace pimoroni { + + enum reg : uint8_t { + ID = 0x00, + REVISION = 0x01, + DATA_READY = 0x02, + MOTION_BURST = 0x16, + POWER_UP_RESET = 0x3a, + ORIENTATION = 0x5b, + RESOLUTION = 0x4e, // PAA5100 only + }; + + bool PMW3901::init() { + // configure spi interface and pins + spi_init(spi, spi_baud); + + gpio_set_function(cs, GPIO_FUNC_SIO); + gpio_set_dir(cs, GPIO_OUT); + + gpio_set_function(sck, GPIO_FUNC_SPI); + gpio_set_function(mosi, GPIO_FUNC_SPI); + gpio_set_function(miso, GPIO_FUNC_SPI); + + if(interrupt != PIN_UNUSED) { + gpio_set_function(interrupt, GPIO_FUNC_SIO); + gpio_set_dir(interrupt, GPIO_IN); + gpio_pull_up(interrupt); + } + + cs_select(); + sleep_ms(50); + cs_deselect(); + + write_register(reg::POWER_UP_RESET, 0x5a); + sleep_ms(20); + for(uint8_t offset = 0; offset < 5; offset++) { + uint8_t data = 0; + read_registers(reg::DATA_READY + offset, &data, 1); + } + + secret_sauce(); + + uint8_t product_id = get_id(); + uint8_t revision = get_revision(); + + printf("Product ID and Revision for PMW3901: %d, %d\n", product_id, revision); + + if(product_id != 0x49 || revision != 0x00) { + return false; + } + + return true; + } + + spi_inst_t* PMW3901::get_spi() const { + return spi; + } + + int PMW3901::get_cs() const { + return cs; + } + + int PMW3901::get_sck() const { + return sck; + } + + int PMW3901::get_mosi() const { + return mosi; + } + + int PMW3901::get_miso() const { + return miso; + } + + int PMW3901::get_int() const { + return interrupt; + } + + uint8_t PMW3901::get_id() { + uint8_t data = 0; + read_registers(reg::ID, &data, 1); + return data; + } + + uint8_t PMW3901::get_revision() { + uint8_t data = 0; + read_registers(reg::REVISION, &data, 1); + return data; + } + + void PMW3901::set_rotation(Degrees degrees) { + switch(degrees) { + default: + case DEGREES_0: + set_orientation(true, true, true); + break; + + case DEGREES_90: + set_orientation(false, true, false); + break; + + case DEGREES_180: + set_orientation(false, false, true); + break; + + case DEGREES_270: + set_orientation(true, false, false); + break; + } + } + + void PMW3901::set_orientation(bool invert_x, bool invert_y, bool swap_xy) { + uint8_t value = 0; + if(swap_xy) + value |= 0b10000000; + if(invert_y) + value |= 0b01000000; + if(invert_x) + value |= 0b00100000; + write_register(reg::ORIENTATION, value); + } + + bool PMW3901::get_motion(int16_t& x_out, int16_t& y_out, uint16_t timeout_ms) { + uint32_t start_time = millis(); + while(millis() - start_time < timeout_ms) { + uint8_t buf[12]; + read_registers(reg::MOTION_BURST, buf, 12); + uint8_t dr = buf[0]; + //uint8_t obs = buf[1]; + x_out = (int16_t)((int32_t)buf[2] << 8 | buf[3]); + y_out = (int16_t)((int32_t)buf[4] << 8 | buf[5]); + uint8_t quality = buf[6]; + //uint8_t raw_sum = buf[7]; + //uint8_t raw_max = buf[8]; + //uint8_t raw_min = buf[9]; + uint8_t shutter_upper = buf[10]; + //uint8_t shutter_lower = buf[11]; + printf("dr = %d, x = %d, y = %d\n",dr, x_out, y_out); + if((dr & 0b10000000) && !((quality < 0x19) && (shutter_upper = 0x1f))) + return true; + + sleep_ms(1); + } + + return false; + } + + bool PMW3901::get_motion_slow(int16_t& x_out, int16_t& y_out, uint16_t timeout_ms) { + uint32_t start_time = millis(); + while(millis() - start_time < timeout_ms) { + uint8_t buf[5]; + read_registers(reg::DATA_READY, buf, 5); + uint8_t dr = buf[0]; + x_out = (int16_t)((int32_t)buf[1] << 8 | buf[2]); + y_out = (int16_t)((int32_t)buf[3] << 8 | buf[4]); + printf("dr = %d, x = %d, y = %d\n",dr, x_out, y_out); + if(dr & 0b10000000) + return true; + + sleep_ms(1); + } + + return false; + } + + void PMW3901::frame_capture(uint16_t timeout_ms) { + + } + + void PMW3901::cs_select() { + gpio_put(cs, false); // Active low + } + + void PMW3901::cs_deselect() { + gpio_put(cs, true); + } + + void PMW3901::write_register(uint8_t reg, uint8_t data) { + uint8_t buf[2]; + buf[0] = reg | 0x80; + buf[1] = data; + cs_select(); + spi_write_blocking(spi, buf, 2); + cs_deselect(); + } + + void PMW3901::write_buffer(uint8_t *buf, uint16_t len) { + cs_select(); + for(uint8_t i = 0; i < len; i += 2) { + if(buf[i] == WAIT) + sleep_ms(buf[i + 1]); + else + spi_write_blocking(spi, &buf[i], 2); + } + cs_deselect(); + } + + void PMW3901::read_registers(uint8_t reg, uint8_t *buf, uint16_t len) { + cs_select(); + spi_write_blocking(spi, ®, 1); + spi_read_blocking(spi, 0, buf, len); + cs_deselect(); + } + + uint8_t PMW3901::read_register(uint8_t reg) { + uint8_t data = 0; + cs_select(); + spi_write_blocking(spi, ®, 1); + spi_read_blocking(spi, 0, &data, 1); + cs_deselect(); + return data; + } + + void PMW3901::secret_sauce() { + uint8_t buf[] = { + 0x7f, 0x00, + 0x55, 0x01, + 0x50, 0x07, + + 0x7f, 0x0e, + 0x43, 0x10 + }; + write_buffer(buf, sizeof(buf)); + + if(read_register(0x67) & 0b10000000) + write_register(0x48, 0x04); + else + write_register(0x48, 0x02); + + uint8_t buf2[] = { + 0x7f, 0x00, + 0x51, 0x7b, + + 0x50, 0x00, + 0x55, 0x00, + 0x7f, 0x0E + }; + write_buffer(buf2, sizeof(buf2)); + + if(read_register(0x73) == 0x00) { + uint8_t c1 = read_register(0x70); + uint8_t c2 = read_register(0x71); + if(c1 <= 28) + c1 += 14; + if(c1 > 28) + c1 += 11; + c1 = std::max((uint8_t)0, std::min((uint8_t)0x3F, c1)); + c2 = (c2 * 45); // 100 + + uint8_t buf3[] = { + 0x7f, 0x00, + 0x61, 0xad, + 0x51, 0x70, + 0x7f, 0x0e + }; + write_buffer(buf3, sizeof(buf3)); + + write_register(0x70, c1); + write_register(0x71, c2); + } + + uint8_t buf4[] = { + 0x7f, 0x00, + 0x61, 0xad, + 0x7f, 0x03, + 0x40, 0x00, + 0x7f, 0x05, + + 0x41, 0xb3, + 0x43, 0xf1, + 0x45, 0x14, + 0x5b, 0x32, + 0x5f, 0x34, + 0x7b, 0x08, + 0x7f, 0x06, + 0x44, 0x1b, + 0x40, 0xbf, + 0x4e, 0x3f, + 0x7f, 0x08, + 0x65, 0x20, + 0x6a, 0x18, + + 0x7f, 0x09, + 0x4f, 0xaf, + 0x5f, 0x40, + 0x48, 0x80, + 0x49, 0x80, + + 0x57, 0x77, + 0x60, 0x78, + 0x61, 0x78, + 0x62, 0x08, + 0x63, 0x50, + 0x7f, 0x0a, + 0x45, 0x60, + 0x7f, 0x00, + 0x4d, 0x11, + + 0x55, 0x80, + 0x74, 0x21, + 0x75, 0x1f, + 0x4a, 0x78, + 0x4b, 0x78, + + 0x44, 0x08, + 0x45, 0x50, + 0x64, 0xff, + 0x65, 0x1f, + 0x7f, 0x14, + 0x65, 0x67, + 0x66, 0x08, + 0x63, 0x70, + 0x7f, 0x15, + 0x48, 0x48, + 0x7f, 0x07, + 0x41, 0x0d, + 0x43, 0x14, + + 0x4b, 0x0e, + 0x45, 0x0f, + 0x44, 0x42, + 0x4c, 0x80, + 0x7f, 0x10, + + 0x5b, 0x02, + 0x7f, 0x07, + 0x40, 0x41, + 0x70, 0x00, + WAIT, 0x0A, // Sleep for 10ms + + 0x32, 0x44, + 0x7f, 0x07, + 0x40, 0x40, + 0x7f, 0x06, + 0x62, 0xf0, + 0x63, 0x00, + 0x7f, 0x0d, + 0x48, 0xc0, + 0x6f, 0xd5, + 0x7f, 0x00, + + 0x5b, 0xa0, + 0x4e, 0xa8, + 0x5a, 0x50, + 0x40, 0x80, + WAIT, 0xF0, + + 0x7f, 0x14, // Enable LED_N pulsing + 0x6f, 0x1c, + 0x7f, 0x00 + }; + write_buffer(buf4, sizeof(buf4)); + } + + uint32_t PMW3901::millis() { + return to_ms_since_boot(get_absolute_time()); + } +} \ No newline at end of file diff --git a/drivers/pmw3901/pmw3901.hpp b/drivers/pmw3901/pmw3901.hpp new file mode 100644 index 00000000..538a5d62 --- /dev/null +++ b/drivers/pmw3901/pmw3901.hpp @@ -0,0 +1,103 @@ +#pragma once + +#include "hardware/spi.h" +#include "hardware/gpio.h" +#include "../../common/pimoroni_common.hpp" + +namespace pimoroni { + + class PMW3901 { + spi_inst_t *spi = PIMORONI_SPI_DEFAULT_INSTANCE; + + //-------------------------------------------------- + // Constants + //-------------------------------------------------- + private: + static const uint8_t WAIT = -1; + + //-------------------------------------------------- + // Enums + //-------------------------------------------------- + public: + enum Degrees { + DEGREES_0 = 0, + DEGREES_90, + DEGREES_180, + DEGREES_270, + }; + + + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + private: + // interface pins with our standard defaults where appropriate + uint cs = SPI_BG_FRONT_CS; + uint sck = SPI_DEFAULT_SCK; + uint mosi = SPI_DEFAULT_MOSI; + uint miso = SPI_DEFAULT_MISO; + uint interrupt = SPI_BG_FRONT_PWM; + + uint32_t spi_baud = 400000; + + + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + PMW3901() {} + + PMW3901(BG_SPI_SLOT slot) { + switch(slot) { + case BG_SPI_FRONT: + cs = SPI_BG_FRONT_CS; + interrupt = SPI_BG_FRONT_PWM; + break; + case BG_SPI_BACK: + cs = SPI_BG_BACK_CS; + interrupt = SPI_BG_BACK_PWM; + break; + } + } + + PMW3901(spi_inst_t *spi, + uint cs, uint sck, uint mosi, uint miso, uint interrupt) : + spi(spi), + cs(cs), sck(sck), mosi(mosi), miso(miso), interrupt(interrupt) {} + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + bool init(); + + spi_inst_t* get_spi() const; + int get_cs() const; + int get_sck() const; + int get_mosi() const; + int get_miso() const; + int get_int() const; + + uint8_t get_id(); + uint8_t get_revision(); + void set_rotation(Degrees degrees = DEGREES_0); + void set_orientation(bool invert_x = true, bool invert_y = true, bool swap_xy = true); + bool get_motion(int16_t& x_out, int16_t& y_out, uint16_t timeout_ms = 5000); + bool get_motion_slow(int16_t& x_out, int16_t& y_out, uint16_t timeout_ms = 5000); + void frame_capture(uint16_t timeout_ms = 10000); + + protected: + virtual void secret_sauce(); + + private: + void cs_select(); + void cs_deselect(); + void write_register(uint8_t reg, uint8_t data); + void write_buffer(uint8_t *buf, uint16_t len); + void read_registers(uint8_t reg, uint8_t *buf, uint16_t len); + uint8_t read_register(uint8_t reg); + uint32_t millis(); + }; + +} diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index d514d21d..27716eff 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -7,6 +7,7 @@ add_subdirectory(breakout_roundlcd) add_subdirectory(breakout_rgbmatrix5x5) add_subdirectory(breakout_matrix11x7) add_subdirectory(breakout_mics6814) +add_subdirectory(breakout_pmw3901) add_subdirectory(breakout_potentiometer) add_subdirectory(breakout_rtc) add_subdirectory(breakout_trackball) diff --git a/examples/breakout_pmw3901/CMakeLists.txt b/examples/breakout_pmw3901/CMakeLists.txt new file mode 100644 index 00000000..2494aac7 --- /dev/null +++ b/examples/breakout_pmw3901/CMakeLists.txt @@ -0,0 +1,16 @@ +set(OUTPUT_NAME pmw3901_demo) + +add_executable( + ${OUTPUT_NAME} + demo.cpp +) + +# enable usb output, disable uart output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) +pico_enable_stdio_uart(${OUTPUT_NAME} 1) + +# Pull in pico libraries that we need +target_link_libraries(${OUTPUT_NAME} pico_stdlib breakout_pmw3901) + +# create map/bin/hex file etc. +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/breakout_pmw3901/demo.cpp b/examples/breakout_pmw3901/demo.cpp new file mode 100644 index 00000000..7728b964 --- /dev/null +++ b/examples/breakout_pmw3901/demo.cpp @@ -0,0 +1,28 @@ +#include +#include "pico/stdlib.h" + +#include "breakout_pmw3901.hpp" + +using namespace pimoroni; + +BreakoutPMW3901 flo; + +int main() { + stdio_init_all(); + + sleep_ms(10000); + + flo.init(); + flo.set_rotation(BreakoutPMW3901::DEGREES_0); + + //uint8_t tx = 0, ty = 0; + + int16_t x = 0, y = 0; + while(true) { + flo.get_motion(x, y); + printf("tick\n"); + //tx; + sleep_ms(1000); + }; + return 0; +} diff --git a/libraries/CMakeLists.txt b/libraries/CMakeLists.txt index 0aec3bdf..74711d57 100644 --- a/libraries/CMakeLists.txt +++ b/libraries/CMakeLists.txt @@ -8,6 +8,8 @@ add_subdirectory(breakout_roundlcd) add_subdirectory(breakout_rgbmatrix5x5) add_subdirectory(breakout_matrix11x7) add_subdirectory(breakout_mics6814) +add_subdirectory(breakout_paa5100) +add_subdirectory(breakout_pmw3901) add_subdirectory(breakout_potentiometer) add_subdirectory(breakout_rtc) add_subdirectory(breakout_trackball) diff --git a/libraries/breakout_paa5100/CMakeLists.txt b/libraries/breakout_paa5100/CMakeLists.txt new file mode 100644 index 00000000..5283e26e --- /dev/null +++ b/libraries/breakout_paa5100/CMakeLists.txt @@ -0,0 +1 @@ +include(breakout_paa5100.cmake) diff --git a/libraries/breakout_paa5100/breakout_paa5100.cmake b/libraries/breakout_paa5100/breakout_paa5100.cmake new file mode 100644 index 00000000..75562cc9 --- /dev/null +++ b/libraries/breakout_paa5100/breakout_paa5100.cmake @@ -0,0 +1,11 @@ +set(LIB_NAME breakout_paa5100) +add_library(${LIB_NAME} INTERFACE) + +target_sources(${LIB_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/${LIB_NAME}.cpp +) + +target_include_directories(${LIB_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR}) + +# Pull in pico libraries that we need +target_link_libraries(${LIB_NAME} INTERFACE pico_stdlib pmw3901) diff --git a/libraries/breakout_paa5100/breakout_paa5100.cpp b/libraries/breakout_paa5100/breakout_paa5100.cpp new file mode 100644 index 00000000..bd491e26 --- /dev/null +++ b/libraries/breakout_paa5100/breakout_paa5100.cpp @@ -0,0 +1,5 @@ +#include "breakout_paa5100.hpp" + +namespace pimoroni { + +} diff --git a/libraries/breakout_paa5100/breakout_paa5100.hpp b/libraries/breakout_paa5100/breakout_paa5100.hpp new file mode 100644 index 00000000..146baebd --- /dev/null +++ b/libraries/breakout_paa5100/breakout_paa5100.hpp @@ -0,0 +1,8 @@ +#pragma once + +#include "drivers/pmw3901/pmw3901.hpp" + +namespace pimoroni { + + typedef PMW3901 BreakoutPAA5100; +} diff --git a/libraries/breakout_pmw3901/CMakeLists.txt b/libraries/breakout_pmw3901/CMakeLists.txt new file mode 100644 index 00000000..67d36428 --- /dev/null +++ b/libraries/breakout_pmw3901/CMakeLists.txt @@ -0,0 +1 @@ +include(breakout_pmw3901.cmake) diff --git a/libraries/breakout_pmw3901/breakout_pmw3901.cmake b/libraries/breakout_pmw3901/breakout_pmw3901.cmake new file mode 100644 index 00000000..8eb82ed2 --- /dev/null +++ b/libraries/breakout_pmw3901/breakout_pmw3901.cmake @@ -0,0 +1,11 @@ +set(LIB_NAME breakout_pmw3901) +add_library(${LIB_NAME} INTERFACE) + +target_sources(${LIB_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/${LIB_NAME}.cpp +) + +target_include_directories(${LIB_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR}) + +# Pull in pico libraries that we need +target_link_libraries(${LIB_NAME} INTERFACE pico_stdlib pmw3901) diff --git a/libraries/breakout_pmw3901/breakout_pmw3901.cpp b/libraries/breakout_pmw3901/breakout_pmw3901.cpp new file mode 100644 index 00000000..a2f752ee --- /dev/null +++ b/libraries/breakout_pmw3901/breakout_pmw3901.cpp @@ -0,0 +1,5 @@ +#include "breakout_pmw3901.hpp" + +namespace pimoroni { + +} diff --git a/libraries/breakout_pmw3901/breakout_pmw3901.hpp b/libraries/breakout_pmw3901/breakout_pmw3901.hpp new file mode 100644 index 00000000..9e7f44f8 --- /dev/null +++ b/libraries/breakout_pmw3901/breakout_pmw3901.hpp @@ -0,0 +1,8 @@ +#pragma once + +#include "drivers/pmw3901/pmw3901.hpp" + +namespace pimoroni { + + typedef PMW3901 BreakoutPMW3901; +}