Added frame capture support and demo

2021-06-15 14:45:05 +01:00 · 2021-06-15 14:45:05 +01:00 · a06fff3cbf
commit a06fff3cbf
--- a/drivers/pmw3901/pmw3901.cpp
+++ b/drivers/pmw3901/pmw3901.cpp
@ -4,17 +4,20 @@
 #include <math.h>
 #include <cstdio>
 #include <algorithm>
+#include <cstring>

 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
+    ID                  = 0x00,
+    REVISION            = 0x01,
+    DATA_READY          = 0x02,
+    MOTION_BURST        = 0x16,
+    POWER_UP_RESET      = 0x3a,
+    ORIENTATION         = 0x5b,
+    RESOLUTION          = 0x4e,  // PAA5100 only
+    RAWDATA_GRAB        = 0x58,
+    RAWDATA_GRAB_STATUS = 0x59,
  };

  bool PMW3901::init() {
@ -134,16 +137,15 @@ namespace pimoroni {
      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]);
+      x_out = (int16_t)((int32_t)buf[3] << 8 | buf[2]);
+      y_out = (int16_t)((int32_t)buf[5] << 8 | buf[4]);
      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)))
+      if((dr & 0b10000000) && !((quality < 0x19) && (shutter_upper == 0x1f)))
        return true;

      sleep_ms(1);
@ -158,9 +160,8 @@ namespace pimoroni {
      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);
+      x_out = (int16_t)((int32_t)buf[2] << 8 | buf[1]);
+      y_out = (int16_t)((int32_t)buf[4] << 8 | buf[3]);
      if(dr & 0b10000000)
        return true;

@ -170,8 +171,68 @@ namespace pimoroni {
    return false;
  }

-  void PMW3901::frame_capture(uint16_t timeout_ms) {
+  bool PMW3901::frame_capture(uint8_t (&raw_data_out)[RAW_DATA_LEN], uint16_t& data_size_out, uint16_t timeout_ms) {
+    bool success = false;

+    data_size_out = 0;
+
+    uint8_t buf[] = {
+      0x7f, 0x07,
+      0x4c, 0x00,
+      0x7f, 0x08,
+      0x6a, 0x38,
+      0x7f, 0x00,
+      0x55, 0x04,
+      0x40, 0x80,
+      0x4d, 0x11,
+
+      WAIT, 0x0a,
+
+      0x7f, 0x00,
+      0x58, 0xff
+    };
+    write_buffer(buf, sizeof(buf));
+
+    uint8_t status = 0;
+    uint32_t start_time = millis();
+    while(millis() - start_time < timeout_ms) {
+      status = read_register(reg::RAWDATA_GRAB_STATUS);
+      if(status & 0b11000000) {
+        success = true;
+        break;
+      }
+    }
+
+    if(success) {
+      write_register(reg::RAWDATA_GRAB, 0x00);
+      memset(raw_data_out, 0, RAW_DATA_LEN * sizeof(uint8_t));
+      
+      uint8_t data = 0;
+      uint16_t x = 0;
+
+      success = false;
+      start_time = millis();
+      while(millis() - start_time < timeout_ms) {
+        data = read_register(reg::RAWDATA_GRAB);
+        if((data & 0b11000000) == 0b01000000) {  // Upper 6-bits
+          raw_data_out[x] &= ~0b11111100;
+          raw_data_out[x] |= (data & 0b00111111) << 2;         // Held in 5:0
+        }
+        if((data & 0b11000000) == 0b10000000) {  // Lower 2-bits
+          raw_data_out[x] &= ~0b00000011;
+          raw_data_out[x] |= (data & 0b00001100) >> 2;  // Held in 3:2
+          x++;
+        }
+        if(x == RAW_DATA_LEN) {
+          success = true;
+          break;
+        }
+      }
+
+      data_size_out = x;
+    }
+
+    return success;
  }

  void PMW3901::cs_select() {
@ -179,7 +240,9 @@ namespace pimoroni {
  }

  void PMW3901::cs_deselect() {
+    sleep_us(1);
    gpio_put(cs, true);
+    sleep_us(1);
  }

  void PMW3901::write_register(uint8_t reg, uint8_t data) {
@ -192,14 +255,12 @@ namespace pimoroni {
  }

  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);
+        write_register(buf[i], buf[i + 1]);
    }
-    cs_deselect();
  }

  void PMW3901::read_registers(uint8_t reg, uint8_t *buf, uint16_t len) {
--- a/drivers/pmw3901/pmw3901.hpp
+++ b/drivers/pmw3901/pmw3901.hpp
@ -12,8 +12,11 @@ namespace pimoroni {
    //--------------------------------------------------
    // Constants
    //--------------------------------------------------
+  public:
+    static const uint8_t FRAME_SIZE     = 35;
+    static const uint16_t RAW_DATA_LEN  = 1225;
  private:
-  static const uint8_t WAIT = -1;
+    static const uint8_t WAIT = -1;

    //--------------------------------------------------
    // Enums
@ -85,7 +88,7 @@ namespace pimoroni {
    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);
+    bool frame_capture(uint8_t (&raw_data_out)[RAW_DATA_LEN], uint16_t& data_size_out, uint16_t timeout_ms = 10000);

  protected:
    virtual void secret_sauce();
--- a/examples/breakout_pmw3901/CMakeLists.txt
+++ b/examples/breakout_pmw3901/CMakeLists.txt
@ -1,16 +1,2 @@
-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})
+add_subdirectory(motion)
+add_subdirectory(frame_capture)
--- a/examples/breakout_pmw3901/frame_capture/CMakeLists.txt
+++ b/examples/breakout_pmw3901/frame_capture/CMakeLists.txt
@ -0,0 +1,16 @@
+set(OUTPUT_NAME pmw3901_framecapture)
+
+add_executable(
+  ${OUTPUT_NAME}
+  frame_capture.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})
--- a/examples/breakout_pmw3901/frame_capture/frame_capture.cpp
+++ b/examples/breakout_pmw3901/frame_capture/frame_capture.cpp
@ -0,0 +1,71 @@
+#include <stdio.h>
+#include "pico/stdlib.h"
+#include <wchar.h>
+#include <string>
+
+#include "breakout_pmw3901.hpp"
+
+using namespace pimoroni;
+
+BreakoutPMW3901 flo(BG_SPI_FRONT);
+BreakoutPMW3901::Degrees rotation = BreakoutPMW3901::DEGREES_0;
+const uint8_t SIZE = BreakoutPMW3901::FRAME_SIZE;
+uint8_t data[BreakoutPMW3901::RAW_DATA_LEN];
+
+std::string value_to_char(uint8_t value) {
+  const std::string charmap = " .:-=+*#%@";
+  float val = (float)value / 255.0f;
+  val *= charmap.length() - 1;
+  value = (uint8_t)val;
+  std::string chosen_char = charmap.substr(val, 1);
+  return chosen_char.append(chosen_char);  // Double chars to - sort of - correct aspect ratio
+}
+
+int main() {
+  stdio_init_all();
+
+  flo.init();
+  flo.set_rotation(rotation);
+
+  uint8_t offset = 0;
+  uint8_t value = 0;
+  
+  while(true) {
+    printf("Capturing...\n");
+    uint16_t data_size = 0;
+    if(flo.frame_capture(data, data_size)) {
+      for(uint8_t y = 0; y < SIZE; y++) {
+        if(rotation == BreakoutPMW3901::DEGREES_180 || rotation == BreakoutPMW3901::DEGREES_270)
+          y = SIZE - y - 1;
+
+        for(uint8_t x = 0; x < SIZE; x++) {
+          if(rotation == BreakoutPMW3901::DEGREES_180 || rotation == BreakoutPMW3901::DEGREES_90)
+            x = SIZE - x - 1;
+
+          if(rotation == BreakoutPMW3901::DEGREES_90 || rotation == BreakoutPMW3901::DEGREES_270)
+            offset = (x * 35) + y;
+          else
+            offset = (y * 35) + x;
+
+          value = data[offset];
+          printf("%s", value_to_char(value).c_str());
+        }
+        printf("\n");
+      }
+    }
+    else {
+      printf("Capture failed. %d bytes received, of %d. Recapturing in ", data_size, BreakoutPMW3901::RAW_DATA_LEN);
+    }
+    printf("5...\n");
+    sleep_ms(1000);
+    printf("4...\n");
+    sleep_ms(1000);
+    printf("3...\n");
+    sleep_ms(1000);
+    printf("2...\n");
+    sleep_ms(1000);
+    printf("Get Ready!\n");
+    sleep_ms(1000);
+  };
+  return 0;
+}
--- a/examples/breakout_pmw3901/motion/CMakeLists.txt
+++ b/examples/breakout_pmw3901/motion/CMakeLists.txt
@ -0,0 +1,16 @@
+set(OUTPUT_NAME pmw3901_motion)
+
+add_executable(
+  ${OUTPUT_NAME}
+  motion.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})
--- a/examples/breakout_pmw3901/motion/motion.cpp
+++ b/examples/breakout_pmw3901/motion/motion.cpp
@ -5,24 +5,22 @@

 using namespace pimoroni;

-BreakoutPMW3901 flo;
+BreakoutPMW3901 flo(BG_SPI_FRONT);

 int main() {
  stdio_init_all();

-  sleep_ms(10000);
-
  flo.init();
  flo.set_rotation(BreakoutPMW3901::DEGREES_0);
-
-  //uint8_t tx = 0, ty = 0;
-
+  int16_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);
+    if(flo.get_motion(x, y)) {
+      tx += x;
+      ty += y;
+      printf("Relative: x %6d, y %6d | Absolute: tx %6d, ty %6d\n", x, y, tx, ty);
+    }
+    sleep_ms(10);
  };
  return 0;
 }