SprsenseでMM-S50MVを使ってみた話

Qiita Advent calendar2021の記事です。

Spresenseの関係会社に勤務していますが、業務外の個人活動の紹介です。
当記事は会社と関係ありません。

誤った基板上のピン接続により故障する可能性があります。
自己責任でお願いします。

ToFセンサーボード（Spresense用）を使ってみました。
https://www.sunhayato.co.jp/material2/ett09/item_1187

####spresense側

spidemo2.inoの変更箇所だけ抜粋

// loopの中のfor(;;)について

  for (;;) 
  {
    static  int count = 0;
    static  int oldseq = -1;
    static  int ledr = 0;
    static  int ledg = 0;
    static  int ledb = 0;

    int j = 0;
    int ranges[32];
    
    int magic0 = spigetb();         // 1byte読む
    int seq0 = spigetb();           // 1byte読む　シーケンスID1
    spiskip(2);   /* ver h/l */     // 2byte飛ばす reserve分
    int range = spigetw();          // 4byte読む　1Dの距離
    
    spiskip(2);   /* ver h/l */     // 2byte飛ばす 1Dの光量(1)分    //

    int diss[32];
    for(j = 0; j<32; j++)
    {
      ranges[j] = spigetw();        // 4byte読む　3Dの距離
      diss[j] = ranges[j] / (0x400000 / 1000);  
    }

    spiskip(256 - 9 - 2 - 4 * 32);  //
        
    int seq1 = spigetb();           // シーケンスID2
    
    if (magic0 != 0xe9)
      continue;
    if (seq0 != seq1)
      continue;
    if (oldseq < 0)
      ;
    else if (((oldseq - seq0) & 0x80) == 0)
      continue;
    oldseq = seq0;
    int dis = range / (0x400000 / 1000);

    delay(30);    // ちょっと待ってあげる
    if (1) 
    {

      for(j = 0; j<32; j++)
      {
        if((diss[j] < 20000) and (diss[j] > 0))
        {
          Serial.print(diss[j]);          
        } 
        else
        {
          Serial.print("-1");            
        }

        Serial.print(",");
      }
      Serial.println("");
    }
    int i = dis & 0xff;

// switch (dis >> 8)　以降はそのまま 
    ...

####PC側
こちらの"Python Example on Using the SCI Interface"を修正しました。

testUART2a.py

# Modified the example for using the AFBR-S50 API with UART 
# Refered from https://broadcom.github.io/AFBR-S50-API/hw_api.html#hw_example
# pySerial（import serial）を初めて使うときは"pip install pyserial"しておく

import serial
 
import matplotlib.pyplot as plt
import seaborn as sns
import pandas as pd
import numpy as np

import cv2
img = np.zeros((320, 160, 3), np.uint8)

# input parameters
port = "COM12"
baudrate = 115200   # baudrate = 2000000

sample_count = 100000
  
# byte stuffing definitions
start_byte = b'\x02'
stop_byte = b'\x03'
esc_byte = b'\x1B'

data0 =    np.array([[0,1,2,3,0,1,2,3,0,1,2,3,0,1,2,3,0,1,2,3,0,1,2,3,0,1,2,3,0,1,2,3],
            [0,0,0,0,1,1,1,1,2,2,2,2,3,3,3,3,4,4,4,4,5,5,5,5,6,6,6,6,7,7,7,7]])
data0 = data0.reshape((2,32))
 
# open serial port w/ "11500,8,N,1", no timeout
print("Open Serial Port " + port)
with serial.Serial(port, baudrate) as ser:
    print("Serial Open " + port + ": " + str(ser.is_open))

    # #read measurement data
    # print("read measurement data")

    for i in range(sample_count):
        # read()
        result = ser.readline()
        result = result.decode(encoding='utf-8')
        result = result.split(',')
        del result[-1]
        result_f = [float(s) for s in result]

        result1 = np.array(result_f)

        if(len(result1) == 32):
            i1 = 0
            for i1 in range(8):
                j1 = 0
                for j1 in range(4):
                    if(result1[i1 * 4 + j1]>0):
                        depth = 500 - result1[i1 * 4 + j1]
                    else:
                        depth = 0
                    if(depth > 255):
                        depth=255
                    else:
                        if (depth<0):
                            depth=0
                    depth = int(depth)
                    print(result1[i1 * 4 + j1], "/", depth, end = ",\t")
                    cv2.circle(img, ((20+40*j1), 320-(20+40*i1)), 20, (depth, depth, 0), thickness=-1)                        
                    j1 +=1
                i1 += 1
                print("")

            cv2.imshow('img', img)
            cv2.waitKey(1)

            print("\n")

    # starting measurements
    print("stop measurements")
    # write(bytes.fromhex('02 12 F7 03'))
    # read()
 
    ser.close()             # close port
    cv2.destroyAllWindows()

####実行結果

早く提供されるSDKが充実してほしいです。