Spresenseによる、小規模な出展ブースでの人数カウントのためのシステムの製作を試みました。実環境でのテストまでには至りませんでしたが、基本的なシステム構成を組むことはできました。

実現したいこと

会議用長机が割り当てられるような出展ブースにおける来訪人数カウントを自動化したい。（たとえば技術書典やコミックマーケットなど）
ブース空間や電源の制約があるため、軽量・省電力な構成で実現したい。
カメラのために広い画角は取れないため、狭小な空間でもブース全体を取り込みたい。
検出結果だけを記録したい。（動画データそのものは記録しない）

ハードウェア構成

Spresense メインボード
Spresense 拡張ボード
HDRカメラ
- さまざまな環境に対応できるようHDRタイプ
ToFセンサー広角タイプ
- カメラによる検出結果に距離のデータを重ねて誤検出を取り除く
TFT-LCD 320x240
- 設置時のカメラ画角の調整用・検出プレビュー用

Edge Impulse

Edge Impulseは、エッジデバイスでの推論を実現するプラットフォームです。Edge ImpulseのWebサイト内で、データのアップロード、ラベル付け、トレーニング、学習したモデルのエクスポートができます。Spresenseにも公式に対応しています。
今回、人間の頭部を検出するために利用します。

Edge Impulse: https://edgeimpulse.com/

プロジェクトを作成：
Edge Impulseのプロフィールページから、"Create new project"で新しいプロジェクトを作成します。無料アカウントでは2つまで非公開プロジェクトを作成できます。

画像データのアップロード：
画像データを学習用・テスト用にわけてアップロードします。この時点での画像データのサイズ（解像度）は任意です。
プロジェクトのトップページから "Add existing data"でPC上の画像ファイルをフォルダごとアップロードできます。
今回、画像データはEdge Impulseのサンプルプロジェクト "Person Detection"で使用されているデータセットを利用しました。（npy形式でダウンロードして画像データを展開）

ラベル付け：
アップロードした画像に対して、どこにどのオブジェクトがあるかをラベル付けしていきます。
左のメニューから"Data aqquisions"ページを開き、"Labeling queue"でラベル付けをしていない画像が確認できます。
今回は、人間の頭部を"head"としてラベル付けしています。

実際に学習を実行する：
左のメニューから、"Create Impulse"でデータの処理の流れを設定します。とはいえカスタマイズできる箇所は画像のサイズくらいです。（今回の構成ではデフォルトの96x96のままです。128x128ではデバイス上での実行時にエラーが発生しました。）

"Image"ページの"Generate features"で"Generate features"を実行します。

"Object detection"ページではトレーニングを実行します。今回はデフォルト設定のままです。
ベースとなるモデルは選択できるようになっていますが、今回はFOMOしか試していませんが、選択肢としてはMobileNet-SSDやYOLOもあるため、別デバイスで試す機会はあるかもしれません。
なおFOMOでは複数オブジェクト検出や座標取得はできますが、YOLOのようにオブジェクトを囲む四角形(Bounding box)ではなく、オブジェクトの中央座標のみが得られます。（大きさはわからない。）

"Retain model"ページで"Train model"を実行します。

"Live classification"ではモデルを実際に実行して、結果を確認できます。また "Deploy" ページのQRコードをスマートフォンで読み取ると、ブラウザ上でモデルをリアルタイムに実行して結果が確認できます。

Arduinoライブラリとしてダウンロード：
"Deploy"ページからトレーニングしたモデルを、ArduinoライブラリのZIPアーカイブとしてダウンロードします。
"Search deployment options"からさまざまな形式を選ぶことができますが、そのなかから"Arduino library"を選びます。"Build"ボタンを押してしばらくすると、ZIPファイルがダウンロードされます。

Edge Impulse上での操作はこれで完了です。

製作

（プログラムのコードは記事末尾にまとめて掲載しています）

Edge ImpulseからダウンロードしたArduinoライブラリは、Arduino IDE上で「ZIP形式のライブラリをインポート」からIDEに取り込みます。ライブラリのなかにサンプルスケッチがあるので、ここを起点にカスタマイズしていきます。

カメラで撮影、ToFセンサーで距離データを取得、AIモデルの実行、結果のSDカードへの保存、を繰り返すコードを書きました。

ハードウェアは、Spresenseメインボードを拡張ボードにセット、ToFセンサーとHDRカメラをメインボードに装着、LCDを拡張ボードとケーブルで接続、簡易ケースを作成しました。

実行時ログと記録ファイルの例

記録ファイルの例：


0000004753[Detect] 0.523438,40,50
0000004753[ToF] 262.362427,-2.595520,6.213012,9.357299,301.724670,226.772644,129.987793,6.909240,301.608154,264.600281,271.800537,29.858215,310.590942,263.516113,257.908691,249.038269,329.120117,298.122986,269.874268,275.813293,344.730591,313.403748,312.643250,294.026001,342.755371,326.735413,319.684509,320.715271,334.726379,325.625916,318.415649,315.834045
0000007940[Detect] 0.988281,120,80
0000007940[ToF] 10.307373,5.987609,15.395568,2.237915,33.434265,26.497131,50.064087,30.685728,64.028564,47.869324,58.606628,59.582886,75.707947,53.820190,64.840942,67.368958,88.492432,69.903259,70.526123,60.960266,97.899109,86.404419,82.717407,61.041870,100.881287,95.204651,86.735962,81.499878,97.479309,92.561584,84.711975,79.424194
0000009532[Detect] 0.847656,146,60
0000009532[ToF] 411.170898,409.959167,-486.264038,-246.155334,441.330505,447.490845,-0.000061,-0.000061,335.957397,8.463684,118.084778,118.481995,91.104126,100.630127,120.025940,120.062500,114.537903,106.511841,115.845520,119.200500,122.106995,116.551575,115.578064,115.683838,110.789063,116.135132,115.969604,115.601563,107.887939,111.754639,114.490173,111.558655
0000014306[Detect] 0.703125,253,90
0000014306[ToF] 391.957153,398.273071,391.653687,372.246155,405.059326,400.461548,399.416748,401.304871,407.557434,405.141113,408.748535,406.785828,422.411743,408.516113,415.927795,346.963501,427.433899,425.955994,95.087646,99.685486,394.298096,143.173950,115.484558,118.800964,234.561157,109.800781,105.654114,113.844360,86.410950,119.591370,105.147766,109.146851

(記録時のタイムスタンプ) [Detect] (たしからしさ) (X座標), (Y座標) が、検出した数だけ繰り返されます。
検出時のみToFセンサーの測定値も記録されます。
(記録時のタイムスタンプ) [ToF] (測定距離 x32点)の形式です。

実行時のデバッグ用シリアルコンソールの出力の例（上記とは別の実行）：


Init SDCard
Mounting SDCard.... OK.
Prepare camera
Set Auto white balance parameter
Set still picture format
Camera initialized
Ready.
Taking photo...
Predictions (DSP: 10 ms., Classification: 1319 ms., Anomaly: 0 ms.): 
Object detection bounding boxes:
Taking photo...
Predictions (DSP: 10 ms., Classification: 1319 ms., Anomaly: 0 ms.): 
Object detection bounding boxes:
Taking photo...
Predictions (DSP: 10 ms., Classification: 1319 ms., Anomaly: 0 ms.): 
Object detection bounding boxes:
  head (0.523438) [ x: 8, y: 16, width: 8, height: 8 ]
0000004753[Detect] 0.523438,40,50
0000004753[ToF] 262.362427,-2.595520,6.213012,9.357299,301.724670,226.772644,129.987793,6.909240,301.608154,264.600281,271.800537,29.858215,310.590942,263.516113,257.908691,249.038269,329.120117,298.122986,269.874268,275.813293,344.730591,313.403748,312.643250,294.026001,342.755371,326.735413,319.684509,320.715271,334.726379,325.625916,318.415649,315.834045
Taking photo...
Predictions (DSP: 10 ms., Classification: 1319 ms., Anomaly: 0 ms.): 
Object detection bounding boxes:

PC上で記録ファイルを人数カウントのみのCSVへ変換した例（上記とは別の実行）：


Timestamp[msec], Detected, Valid, MinDistance
4753, 1, 1, -2.59552
7940, 1, 1, 2.237915
9532, 1, 1, -486.264038
14306, 1, 1, 86.41095
15901, 1, 1, -496.616211
17491, 1, 1, 94.470764
19085, 1, 1, 95.98999
20677, 1, 1, 90.783142
22268, 1, 1, 78.12616
23862, 1, 1, -38.568848
25452, 1, 1, -6.1e-05
27046, 1, 1, -68.569397
28640, 1, 1, -86.542175
30230, 1, 1, -89.36084
31823, 1, 1, -17.297974
39787, 1, 0, 322.676025
41378, 1, 0, 317.825256
44562, 1, 0, 331.165771
46153, 2, 0, 325.243103
47749, 1, 0, 305.134338

ToFセンサーによる距離計測が負の値になっている箇所がありますが、これはデバイス上での異常値チェックがなされていないためです。経験上、極端に近い（今回のデバッグのようにセンサーの至近距離でいじっている）場合に起きるため、簡易的なしきい値チェックでは実用上の支障はないと思われます。

現状と今後の課題

当初想定していたレベルまでには到達できませんでしたが、カメラ→AI→検出結果のみを保存、という基本的な構造はおおむね構築できました。

AIによる人間の検出の課題：
学習データ・テストデータでの検出を見る限り、万全の結果とはいえない。
RAMの制約によりAIモデルに渡す画像サイズが96x96にまで縮小されていることが影響しているように思われるが、デバイス上での実行では画像サイズは上げられないようなので、ほかの手法でカバーする必要がある。

検出記録の課題：
RTCを外付けして、検出記録に日時が記録できると良い。

ハードウェア面での課題：
しっかりした外装を製作したい。

実際の環境で実用になるか？：
製作したものを実際の出展で運用するチャンスがなく検証できなかった。カメラの設置アングルが課題になるかもしれない。

イメージセンサーとToFセンサーの画角のマッチング：
目測でだいたい合っているようなのでしっかりと画角調整をしていない。ToFセンサーの解像度が4x8で合計32点とはいえ、なんらか確認はしておいたほうがよいと思う。

PC上での後処理の高度化：
現状では「ある時刻における検出人数」しかわからないが、人の移動を追跡できるようにしたい。検出した座標自体は記録されているので、移動量や移動方向から推定することはできるかもしれない。

ソースコード

以下に実際のソースコードの全文を掲載します。

main.ino : Spresense上で実行するメインのコード


/* Edge Impulse ingestion SDK
 * Copyright (c) 2023 EdgeImpulse Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

/* Includes ---------------------------------------------------------------- */

#include "LGFX_SPRESENSE_sample.hpp"
#include <LovyanGFX.hpp>

#include <Camera.h>

#include <SDHCI.h>
#include <File.h>

#include <detection3_inferencing.h>
#include <edge-impulse-sdk/dsp/image/image.hpp>

#include <SPI.h>
#include "tofsensor.h"


SDClass SD;
File logfile;

LGFX tft;
ToFSensor tof(SPI5);


/*
 * IMPORTANT In Tools->Board select the Spresense device, then under Tools->Memory select 1536(kB).
 * We do not need the audio or multi-core memory layouts for this project, but we do need as much memory available as possible for NN and image storage.
*/

/* Constant defines -------------------------------------------------------- */
#define EI_CAMERA_RAW_FRAME_BUFFER_COLS           CAM_IMGSIZE_QVGA_H
#define EI_CAMERA_RAW_FRAME_BUFFER_ROWS           CAM_IMGSIZE_QVGA_V
#define EI_CAMERA_RAW_FRAME_BYTE_SIZE             2

#define ALIGN_PTR(p,a)   ((p & (a-1)) ?(((uintptr_t)p + a) & ~(uintptr_t)(a-1)) : p)

/* Edge Impulse ------------------------------------------------------------- */

typedef struct {
    size_t width;
    size_t height;
} ei_device_resize_resolutions_t;

/**
 * @brief      Check if new serial data is available
 *
 * @return     Returns number of available bytes
 */
int ei_get_serial_available(void)
{
    return Serial.available();
}

/**
 * @brief      Get next available byte
 *
 * @return     byte
 */
char ei_get_serial_byte(void)
{
    return Serial.read();
}

/* Private variables ------------------------------------------------------- */
static bool debug_nn = false; // Set this to true to see e.g. features generated from the raw signal
static bool is_initialised = false;

/*
** @brief points to the output of the capture
*/
static uint8_t *ei_camera_capture_out = NULL;

/* Function definitions ------------------------------------------------------- */
bool ei_camera_init(void);
void ei_camera_deinit(void);
bool ei_camera_capture(uint32_t img_width, uint32_t img_height, uint8_t *out_buf) ;
int calculate_resize_dimensions(uint32_t out_width, uint32_t out_height, uint32_t *resize_col_sz, uint32_t *resize_row_sz, bool *do_resize);
static int ei_camera_get_data(size_t offset, size_t length, float *out_ptr);


void setup() {
    Serial.begin(115200);

    for (int i = 0; i < 3000 && !Serial; i++) {
      delay(1);
    }

    while (!Serial);
    Serial.println("Edge Impulse Inferencing Demo");

    Serial.println("Init ToF sensor");
    tof.begin();

    Serial.println("Init SDCard");
    wait_prepare_sdcard();

    if (ei_camera_init() == false) {
        ei_printf("Failed to initialize Camera!\r\n");
    }
    else {
        ei_printf("Camera initialized\r\n");
    }

    tft.init();
    tft.setRotation(3);
    tft.clear();
    tft.setTextColor(TFT_WHITE);
    tft.setCursor(0, 0);
    tft.println("Ready");

    Serial.println("Ready.");
}


ToFData tofdata;

void loop() {
    static unsigned long start_millis = millis();

    // ei_printf("\nStarting inferencing in 2 seconds...\n");

    // if (ei_sleep(2000) != EI_IMPULSE_OK) {
    //     return;
    // }

    ei_printf("Taking photo...\n");

    if (ei_camera_capture((size_t)EI_CLASSIFIER_INPUT_WIDTH, (size_t)EI_CLASSIFIER_INPUT_HEIGHT, NULL) == false) {
        ei_printf("Failed to capture image\r\n");
        return;
    }

    ei::signal_t signal;
    signal.total_length = EI_CLASSIFIER_INPUT_WIDTH * EI_CLASSIFIER_INPUT_HEIGHT;
    signal.get_data = &ei_camera_get_data;

    // Run the classifier
    ei_impulse_result_t result = { 0 };

    EI_IMPULSE_ERROR err = run_classifier(&signal, &result, debug_nn);
    if (err != EI_IMPULSE_OK) {
        ei_printf("ERR: Failed to run classifier (%d)\n", err);
        return;
    }

    unsigned long current_millis = millis() - start_millis;
    log_start_write();

    // print the predictions
    ei_printf("Predictions (DSP: %d ms., Classification: %d ms., Anomaly: %d ms.): \n",
                result.timing.dsp, result.timing.classification, result.timing.anomaly);
#if EI_CLASSIFIER_OBJECT_DETECTION == 1
    ei_printf("Object detection bounding boxes:\r\n");
    for (uint32_t i = 0; i < result.bounding_boxes_count; i++) {
        ei_impulse_result_bounding_box_t bb = result.bounding_boxes[i];
        if (bb.value == 0) {
            continue;
        }
        ei_printf("  %s (%f) [ x: %u, y: %u, width: %u, height: %u ]\r\n",
                bb.label,
                bb.value,
                bb.x,
                bb.y,
                bb.width,
                bb.height);

        // TFT: 320x240, Estimation: 96x96
        uint16_t center_x = (float)(bb.x + bb.width / 2) * (320.0f / 96.0f);
        uint16_t center_y = (float)(bb.y + bb.height / 2) * (240.0f / 96.0f);
        tft.drawRect(center_x - 4, center_y - 4, 8, 8, TFT_YELLOW);

        log_append_detection(current_millis, bb.value, center_x, center_y);
    }

    // Print the prediction results (classification)
#else
    ei_printf("Predictions:\r\n");
    for (uint16_t i = 0; i < EI_CLASSIFIER_LABEL_COUNT; i++) {
        ei_printf("  %s: ", ei_classifier_inferencing_categories[i]);
        ei_printf("%.5f\r\n", result.classification[i].value);
    }
#endif

    // Print anomaly result (if it exists)
#if EI_CLASSIFIER_HAS_ANOMALY
    ei_printf("Anomaly prediction: %.3f\r\n", result.anomaly);
#endif

#if EI_CLASSIFIER_HAS_VISUAL_ANOMALY
    ei_printf("Visual anomalies:\r\n");
    for (uint32_t i = 0; i < result.visual_ad_count; i++) {
        ei_impulse_result_bounding_box_t bb = result.visual_ad_grid_cells[i];
        if (bb.value == 0) {
            continue;
        }
        ei_printf("  %s (%f) [ x: %u, y: %u, width: %u, height: %u ]\r\n",
                bb.label,
                bb.value,
                bb.x,
                bb.y,
                bb.width,
                bb.height);
    }
#endif

    if (result.bounding_boxes_count > 0) {
      float distancelist[32] = {};
      tof.export_list(tofdata, distancelist);
      log_append_tof(current_millis, distancelist);
    }

    log_end_write();
}

/**
 * @brief   Setup image sensor & start streaming
 *
 * @retval  false if initialisation failed
 */
bool ei_camera_init(void)
{
    CamErr err;
    if (is_initialised) {
        return true;
    }

    Serial.println("Prepare camera");
    err = theCamera.begin();
    if (err != CAM_ERR_SUCCESS) {
        ei_printf("Camera begin failed\r\n");
        return false;
    }
    if (theCamera.getDeviceType() == CAM_DEVICE_TYPE_UNKNOWN) {
        ei_printf("Camera not found\r\n");
        return false;
    }

    /* Auto white balance configuration */
    Serial.println("Set Auto white balance parameter");
    err = theCamera.setAutoWhiteBalanceMode(CAM_WHITE_BALANCE_AUTO);
    if (err != CAM_ERR_SUCCESS) {
        ei_printf("Auto white balancing setup failed\r\n");
        return false;
    }

    /* Set parameters about still picture.
    * In the following case, QVGA and YUV422.
    */
    Serial.println("Set still picture format");
    err = theCamera.setStillPictureImageFormat(
        EI_CAMERA_RAW_FRAME_BUFFER_COLS,
        EI_CAMERA_RAW_FRAME_BUFFER_ROWS,
        CAM_IMAGE_PIX_FMT_YUV422);

    if (err != CAM_ERR_SUCCESS) {
        ei_printf("Camera set still image failed: %d\r\n", err);
        return false;
    }

    ei_camera_capture_out = (uint8_t*)ei_malloc(EI_CAMERA_RAW_FRAME_BUFFER_COLS * EI_CAMERA_RAW_FRAME_BUFFER_ROWS * 3 + 32);
    ei_camera_capture_out = (uint8_t *)ALIGN_PTR((uintptr_t)ei_camera_capture_out, 32);

    if (ei_camera_capture_out == nullptr) {
        ei_printf("ERR: Failed to allocate memory for capture buffer\r\n");
        return false;
    }

    is_initialised = true;

    return true;
}

/**
 * @brief      Stop streaming of sensor data
 */
void ei_camera_deinit(void)
{
    ei_free(ei_camera_capture_out);
    ei_camera_capture_out = nullptr;
    is_initialised = false;
}

/**
 * @brief      Capture, rescale and crop image
 *
 * @param[in]  img_width     width of output image
 * @param[in]  img_height    height of output image
 * @param[in]  out_buf       pointer to store output image, NULL may be used
 *                           if ei_camera_frame_buffer is to be used for capture and resize/cropping.
 *
 * @retval     false if not initialised, image captured, rescaled or cropped failed
 *
 */
bool ei_camera_capture(uint32_t img_width, uint32_t img_height, uint8_t *out_buf)
{
    bool do_resize = false;
    bool do_crop = false;

    if (!is_initialised) {
        ei_printf("ERR: Camera is not initialized\r\n");
        return false;
    }

    CamImage img = theCamera.takePicture();

    if (img.isAvailable() != true) {
        ei_printf("ERR: Failed to get snapshot\r\n");
        return false;
    }

    // Take ToF sensor
    tof.receive_data(tofdata);

    // we take snapshot in yuv422
    if (ei::EIDSP_OK != ei::image::processing::yuv422_to_rgb888(ei_camera_capture_out, img.getImgBuff(), img.getImgSize(), ei::image::processing::BIG_ENDIAN_ORDER)) {
        ei_printf("ERR: Conversion failed\n");
        return false;
    }

    // Draw on TFT
    tft.setSwapBytes(false);
    tft.pushImage(0, 0, 320, 240, (lgfx:: rgb888_t*)ei_camera_capture_out);


    uint32_t resize_col_sz;
    uint32_t resize_row_sz;
    // choose resize dimensions
    int res = calculate_resize_dimensions(img_width, img_height, &resize_col_sz, &resize_row_sz, &do_resize);
    if (res) {
        ei_printf("ERR: Failed to calculate resize dimensions (%d)\r\n", res);
        return false;
    }

    if ((img_width != resize_col_sz)
        || (img_height != resize_row_sz)) {
        do_crop = true;
    }

    if (do_resize) {
          ei::image::processing::crop_and_interpolate_rgb888(
          ei_camera_capture_out,
          EI_CAMERA_RAW_FRAME_BUFFER_COLS,
          EI_CAMERA_RAW_FRAME_BUFFER_ROWS,
          ei_camera_capture_out,
          resize_col_sz,
          resize_row_sz);
    }

    return true;
}

/**
 * @brief      Convert rgb565 data to rgb888
 *
 * @param[in]  src_buf  The rgb565 data
 * @param      dst_buf  The rgb888 data
 * @param      src_len  length of rgb565 data
 */

bool RBG565ToRGB888(uint8_t *src_buf, uint8_t *dst_buf, uint32_t src_len)
{
    uint8_t hb, lb;
    uint32_t pix_count = src_len / 2;

    for(uint32_t i = 0; i < pix_count; i ++) {
        hb = *src_buf++;
        lb = *src_buf++;

        *dst_buf++ = hb & 0xF8;
        *dst_buf++ = (hb & 0x07) << 5 | (lb & 0xE0) >> 3;
        *dst_buf++ = (lb & 0x1F) << 3;
    }

    return true;
}

static int ei_camera_get_data(size_t offset, size_t length, float *out_ptr)
{
    // we already have a RGB888 buffer, so recalculate offset into pixel index
    size_t pixel_ix = offset * 3;
    size_t pixels_left = length;
    size_t out_ptr_ix = 0;

    while (pixels_left != 0) {
        out_ptr[out_ptr_ix] = (ei_camera_capture_out[pixel_ix] << 16) + (ei_camera_capture_out[pixel_ix + 1] << 8) + ei_camera_capture_out[pixel_ix + 2];

        // go to the next pixel
        out_ptr_ix++;
        pixel_ix+=3;
        pixels_left--;
    }

    // and done!
    return 0;
}

/**
 * @brief      Determine whether to resize and to which dimension
 *
 * @param[in]  out_width     width of output image
 * @param[in]  out_height    height of output image
 * @param[out] resize_col_sz       pointer to frame buffer's column/width value
 * @param[out] resize_row_sz       pointer to frame buffer's rows/height value
 * @param[out] do_resize     returns whether to resize (or not)
 *
 */
int calculate_resize_dimensions(uint32_t out_width, uint32_t out_height, uint32_t *resize_col_sz, uint32_t *resize_row_sz, bool *do_resize)
{
    size_t list_size = 6;
    const ei_device_resize_resolutions_t list[list_size] = {
        {64, 64},
        {96, 96},
        {160, 120},
        {160, 160},
        {320, 240},
    };

    // (default) conditions
    *resize_col_sz = EI_CAMERA_RAW_FRAME_BUFFER_COLS;
    *resize_row_sz = EI_CAMERA_RAW_FRAME_BUFFER_ROWS;
    *do_resize = false;

    for (size_t ix = 0; ix < list_size; ix++) {
        if ((out_width <= list[ix].width) && (out_height <= list[ix].height)) {
            *resize_col_sz = list[ix].width;
            *resize_row_sz = list[ix].height;
            *do_resize = true;
            break;
        }
    }

    return 0;
}

#if !defined(EI_CLASSIFIER_SENSOR) || EI_CLASSIFIER_SENSOR != EI_CLASSIFIER_SENSOR_CAMERA
#error "Invalid model for current sensor"
#endif


void wait_prepare_sdcard() {
  Serial.print("Mounting SDCard.");
  Serial.flush();
  while (!SD.begin()) {
    Serial.print(".");
    Serial.flush();
    delay(1000);
  }
  Serial.println(" OK.");
}

void log_start_write() {
  logfile = SD.open("log.txt", FILE_WRITE);
}

void log_end_write() {
  logfile.close();
}

void log_append_detection(uint32_t number, float probability, uint16_t pos_x, uint16_t pos_y) {
  if (!logfile) {
    return;
  }
  Serial.printf("%010d[Detect] %f,%d,%d\r\n", number, probability, pos_x, pos_y);
  logfile.printf("%010d[Detect] %f,%d,%d\r\n", number, probability, pos_x, pos_y);
}

void log_append_tof(uint32_t number, float distancelist[]) {
  if (!logfile) {
    return;
  }
  Serial.printf("%010d[ToF] ", number);
  logfile.printf("%010d[ToF] ", number);
  for (int i = 0; i < 32; i++) {
    if (i != 0) {
      Serial.print(",");
      logfile.print(",");
    }
    Serial.printf("%f", distancelist[i]);
    logfile.printf("%f", distancelist[i]);
  }
  Serial.print("\r\n");
  logfile.print("\r\n");
}

LGFX_SPRESENSE_sample.hpp : LovyanGFX用のディスプレイ構成記述


#pragma once

#define LGFX_USE_V1
#include <LovyanGFX.hpp>

class LGFX : public lgfx::LGFX_Device
{
  lgfx::Panel_ILI9341     _panel_instance;

  lgfx::Bus_SPI       _bus_instance;   // SPIバスのインスタンス

public:
  LGFX(void)
  {
    {
      auto cfg = _bus_instance.config();

      cfg.spi_mode = 0;             // SPI通信モードを設定 (0 ~ 3)
      cfg.freq_write = 40000000;    // 送信時のSPIクロック
      cfg.freq_read  = 16000000;    // 受信時のSPIクロック
      cfg.pin_dc   =  8;            // SPIのD/Cピン番号を設定  (-1 = disable)
      cfg.spi_port =  4;            // Arduino拡張ボードの場合は 4 

      _bus_instance.config(cfg);
      _panel_instance.setBus(&_bus_instance);
    }

    {
      auto cfg = _panel_instance.config();

//    cfg.pin_cs           =    -1;  // CSが接続されているピン番号   (-1 = disable) HW CSピンの場合は-1を指定
      cfg.pin_rst          =     9;  // RSTが接続されているピン番号  (-1 = disable)
      cfg.pin_busy         =    -1;  // BUSYが接続されているピン番号 (-1 = disable)

      // ※ 以下の設定値はパネル毎に一般的な初期値が設定されていますので、不明な項目はコメントアウトして試してみてください。

      cfg.panel_width      =   240;  // 実際に表示可能な幅
      cfg.panel_height     =   320;  // 実際に表示可能な高さ
      cfg.offset_x         =     0;  // パネルのX方向オフセット量
      cfg.offset_y         =     0;  // パネルのY方向オフセット量
      cfg.offset_rotation  =     0;  // 回転方向の値のオフセット 0~7 (4~7は上下反転)
      cfg.dummy_read_pixel =     8;  // ピクセル読出し前のダミーリードのビット数
      cfg.dummy_read_bits  =     1;  // ピクセル以外のデータ読出し前のダミーリードのビット数
      cfg.readable         =  true;  // データ読出しが可能な場合 trueに設定
      cfg.invert           = false;  // パネルの明暗が反転してしまう場合 trueに設定
      cfg.rgb_order        = false;  // パネルの赤と青が入れ替わってしまう場合 trueに設定
      cfg.dlen_16bit       = false;  // データ長を16bit単位で送信するパネルの場合 trueに設定
      cfg.bus_shared       =  true;  // SDカードとバスを共有している場合 trueに設定(drawJpgFile等でバス制御を行います)
      _panel_instance.config(cfg);
    }
    setPanel(&_panel_instance);
  }
};

tofsensor.h : ToFセンサーのユーティリティクラスのヘッダー


#pragma once

#include <Arduino.h>
#include <SPI.h>


struct ToFData {
  char      bMagic;
  char      bSeq1;
  char      bRsv1;
  char      bRsv2;
  uint32_t  iRange1;
  uint16_t  wLight1;
  uint32_t  iRange[4*8];
  uint16_t  wLight[4*8];
  char      bRsv3[53];
  char      bSeq2;
};


class ToFSensor {
public:
  const int ROWSIZE = 4;
  const int COLSIZE = 8;
  const int PIXELS = 4 * 8;

  SPIClass& spi;
  uint8_t prevSequence = 0;

  ToFSensor(SPIClass& spi) : spi(spi) { }

  void begin();

  void send_command(uint8_t cmd, uint8_t val);

  void receive_data(ToFData& data);
  void dump_data(ToFData& data);
  void export_list(ToFData& data, float* out);

private:
  uint8_t receive_1byte();
  uint16_t receive_2bytes();
  uint32_t receive_4bytes();
  size_t receive_nbytes(uint8_t* buffer, size_t len);
};

tofsensor.cpp : ToFセンサーのユーティリティクラスの本体


#include "tofsensor.h"


static	void	print_hex(int val)
{
	static const char	*bin2asc = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz";
  int a,b,c;

	c	=	(val&0xFF000000) >> 24;
  a  = c >> 4;
  b  = c & 0xF;
  Serial.write(bin2asc[a]);
  Serial.write(bin2asc[b]);
    
	c	=	(val&0x00FF0000) >> 16;
  a  = c >> 4;
  b  = c & 0xF;
  Serial.write(bin2asc[a]);
  Serial.write(bin2asc[b]);
    
	c	=	(val&0x0000FF00) >> 8;
  a  = c >> 4;
  b  = c & 0xF;
  Serial.write(bin2asc[a]);
  Serial.write(bin2asc[b]);

	c	=	(val&0x000000FF);
  a  = c >> 4;
  b  = c & 0xF;
  Serial.write(bin2asc[a]);
  Serial.write(bin2asc[b]);
  Serial.print(",");
}
    
static const	char*	dec2s = "0123456789";


static void	put_logline_9_22(int val) {

	int	i, l;
	float	a,b;
	
	print_hex(val);
	
//	if ((val & 0x80000000)) {
//		Serial.print("-");
//		val = 0x100000000 - val;		/* 0xffffffff -> 1 */
//	}
//	
//	b = float((val >> 22) & 0x1ff);
//	l = (val  & 0x3fffff);
//	a=0.00000023841*l+b;  /* (2^-22)xl */
//		Serial.print(a,9);     /*  a (m)  */
//                Serial.print(",");     
//  ---------------------

	if ((val & 0x80000000)) {
		Serial.print("-");
		val = 0x100000000 - val;		/* 0xffffffff -> 1 */
	}
	
	l = (val >> 22) & 0x1ff;
	i = 10;
	while (l >= i)
		i *= 10;
	do {
		i /= 10;
		Serial.write((const uint8_t*)dec2s + ((l / i) % 10), 1);
	} while (i > 1);
	Serial.print(".");
	
	i = 1000000;
	l = ((long long)i * (val & 0x3fffff)/* + 0x200000*/) / 0x400000;
	do {
		i /= 10;
		Serial.write((const uint8_t*)dec2s + ((l / i) % 10), 1);
	} while (i > 1);
  Serial.print(",");     
//  ---------------------
}


static void put_logline_9_22_fixeddigits(uint32_t val) {
  int	i, l;
  float	a,b;

  size_t buflen = 12;
  char buf[13] = {};
  int chidx = 0;

// print_hex(val);

//	if ((val & 0x80000000)) {
//		Serial.print("-");
//		val = 0x100000000 - val;		/* 0xffffffff -> 1 */
//	}
//	
//	b = float((val >> 22) & 0x1ff);
//	l = (val  & 0x3fffff);
//	a=0.00000023841*l+b;  /* (2^-22)xl */
//		Serial.print(a,9);     /*  a (m)  */
//                Serial.print(",");     
//  ---------------------

  if ((val & 0x80000000)) {
    // Serial.print("-");
    buf[chidx++] = '-';
    val = 0x100000000 - val;		/* 0xffffffff -> 1 */
  }

  l = (val >> 22) & 0x1ff;
  i = 10;

  while (l >= i) {
    i *= 10;
  }

  do {
    i /= 10;
    // Serial.write((const uint8_t*)dec2s + ((l / i) % 10), 1);
    char ch = dec2s[(l / i) % 10];
    buf[chidx++] = ch;
  } while (i > 1);

  // Serial.print(".");
  buf[chidx++] = '.';

  i = 1000000;
  l = ((long long)i * (val & 0x3fffff)/* + 0x200000*/) / 0x400000;
  do {
    i /= 10;
    // Serial.write((const uint8_t*)dec2s + ((l / i) % 10), 1);
    char ch = dec2s[(l / i) % 10];
    buf[chidx++] = ch;
  } while (i > 1);
  // Serial.print(", ");

  size_t len = strlen(buf);
  int startidx = buflen - len;
  memmove(&buf[startidx], buf, len);
  for (int i = 0; i < startidx; i++) {
    buf[i] = ' ';
  }

  Serial.printf("[%s] ", buf);
}

static void print_9_22(int val) {
  
	int	i, l;
	float	a,b;
	
	// print_hex(val);
	
//	if ((val & 0x80000000)) {
//		Serial.print("-");
//		val = 0x100000000 - val;		/* 0xffffffff -> 1 */
//	}
//	
//	b = float((val >> 22) & 0x1ff);
//	l = (val  & 0x3fffff);
//	a=0.00000023841*l+b;  /* (2^-22)xl */
//		Serial.print(a,9);     /*  a (m)  */
//                Serial.print(",");     
//  ---------------------

	if ((val & 0x80000000)) {
		Serial.print("-");
		val = 0x100000000 - val;		/* 0xffffffff -> 1 */
	}
	
	l = (val >> 22) & 0x1ff;
	i = 10;
	while (l >= i)
		i *= 10;
	do {
		i /= 10;
		Serial.write((const uint8_t*)dec2s + ((l / i) % 10), 1);
	} while (i > 1);
	Serial.print(".");
	
	i = 1000000;
	l = ((long long)i * (val & 0x3fffff)/* + 0x200000*/) / 0x400000;
	do {
		i /= 10;
		Serial.write((const uint8_t*)dec2s + ((l / i) % 10), 1);
	} while (i > 1);
  Serial.print(",");     
//  ---------------------
}



static float convert_9_22(int val) {
  int	i, l;
  float	a,b;
  float result;

  size_t buflen = 12;
  char buf[13] = {};
  int chidx = 0;

// print_hex(val);

//	if ((val & 0x80000000)) {
//		Serial.print("-");
//		val = 0x100000000 - val;		/* 0xffffffff -> 1 */
//	}
//	
//	b = float((val >> 22) & 0x1ff);
//	l = (val  & 0x3fffff);
//	a=0.00000023841*l+b;  /* (2^-22)xl */
//		Serial.print(a,9);     /*  a (m)  */
//                Serial.print(",");     
//  ---------------------

  if ((val & 0x80000000)) {
    // Serial.print("-");
    buf[chidx++] = '-';
    val = 0x100000000 - val;		/* 0xffffffff -> 1 */
  }

  l = (val >> 22) & 0x1ff;
  i = 10;

  while (l >= i) {
    i *= 10;
  }

  do {
    i /= 10;
    // Serial.write((const uint8_t*)dec2s + ((l / i) % 10), 1);
    char ch = dec2s[(l / i) % 10];
    buf[chidx++] = ch;
  } while (i > 1);

  // Serial.print(".");
  buf[chidx++] = '.';

  i = 1000000;
  l = ((long long)i * (val & 0x3fffff)/* + 0x200000*/) / 0x400000;
  do {
    i /= 10;
    char ch = dec2s[(l / i) % 10];
    buf[chidx++] = ch;
  } while (i > 1);
  
  result = (float)atof(buf);
  return result;
}


static	void	put_logline_12_4(int val)
{
	int	i, l;
	float	a,b;
	b=float(val >> 4);
	l = (val  & 0xf);
	a=0.0625*l+b;
		Serial.print(a,4);     
                Serial.print(",");     
}


void ToFSensor::begin() {
  this->spi.begin();
  this->spi.beginTransaction(SPISettings(8000000, MSBFIRST, SPI_MODE3));

  receive_nbytes(nullptr, 256);
  delay(500);

  // Sync mode
  send_command(0x00, 0xff);  
  delay(500);
  receive_nbytes(nullptr, 256);
  // Sync
  receive_nbytes(nullptr, receive_1byte());

  // Normal mode
  send_command(0x00, 0x00);
  delay(500);

  // Set long range
  send_command(0x12, 1);
  // Set short range
  // send_command(0x12, 0);

  delay(500);
  receive_nbytes(nullptr, 256);

  // Set 10frames/sec
  send_command(0x10, 0);

  delay(500);
  receive_nbytes(nullptr, 256);
}

uint8_t ToFSensor::receive_1byte() {
  return this->spi.transfer(0x00) & 0xff;
}

uint16_t ToFSensor::receive_2bytes() {
  uint16_t val = 0;
  val |= receive_1byte() << 8;
  val |= receive_1byte();
  return val;
}

uint32_t ToFSensor::receive_4bytes() {
  uint32_t val = 0;
  for (int i = 0; i < 4; i++) {
    val |= receive_1byte();
    val = val << 8;
  }
  return val;
}

size_t ToFSensor::receive_nbytes(uint8_t *buffer, size_t len) {
  size_t readlen = 0;
  for (; readlen < len; readlen += 1) {
    uint8_t val = receive_1byte();
    if (buffer != nullptr) {
      buffer[readlen] = val;
    }
  }
  return readlen;
}

void ToFSensor::send_command(uint8_t cmd, uint8_t val) {
  this->spi.transfer(0xeb);
  this->spi.transfer(cmd);
  this->spi.transfer(0x01);
  this->spi.transfer(val);
  this->spi.transfer(0xed);
  // Skip 251 bytes
  receive_nbytes(nullptr, 256 - 5);
}


void ToFSensor::receive_data(ToFData& data) {
  // struct StSpiData GetData;
  for (int retry = 0; retry < 4; retry++) {

    /*マジック*/
    data.bMagic = receive_1byte();
    /*シーケンス*/
    data.bSeq1 = receive_1byte();
    /*reserved*/
    receive_nbytes(nullptr, 2);
    /*1D距離*/
    data.iRange1 = receive_4bytes();
    /*1Dの光量*/
    data.wLight1 = receive_2bytes();
    /*3Dの距離*/
    for(int i = 0; i < PIXELS ; i++) {
      data.iRange[i] = receive_4bytes();
    }
    /*3Dの光量*/
    for(int i = 0; i < PIXELS ;i++) {
      data.wLight[i] = receive_2bytes();
    }
    /*パディング*/
    receive_nbytes(nullptr, 256 - 11 - 6 * PIXELS);
    /*シーケンス2*/
    data.bSeq2 = receive_1byte();

    if (data.bMagic != 0xe9) {
      continue;     
    }
    if (data.bSeq1 != data.bSeq2) {
      continue;
    }

    
    if (data.bSeq1 - prevSequence < 0) {
      prevSequence = data.bSeq1;
      continue;
    }
    prevSequence = data.bSeq1;

    break;
  }
}

void ToFSensor::dump_data(ToFData& data) {
    Serial.printf("--------\r\n");

    /*1D距離*/
    Serial.print("1D distance: ");
    put_logline_9_22(data.iRange1);
    Serial.println();

    /*1Dの光量*/
    // Serial.print("1D luminance: ");
    // put_logline_12_4(data.wLight1);
    // Serial.println();

    /*3Dの距離*/
    Serial.println("3D distance: ");
    for (int i=0; i<PIXELS; i++) {
      uint32_t r = ( data.iRange[i] );
      put_logline_9_22_fixeddigits(r);
      if (i != 0 && i % 8 == 7) {
        Serial.println();
      }
    }
    Serial.println();
}

void ToFSensor::export_list(ToFData& data, float* out) {
  if (out == nullptr) { return; }

  for (int i=0; i < PIXELS; i++) {
    uint32_t r = data.iRange[i];
    out[i] = convert_9_22(r);
  }
}

parser.py : PC上で実行する、結果をパースしてCSVで出力するスクリプト


import sys
import re

DISTANCE_THRESHOLD = 150

def main():
    inputfilepath = sys.argv[1]
    with open(inputfilepath, "r", encoding="utf-8") as f:
        input = f.readlines()

    outputfilepath = inputfilepath + "_count.csv"

    records = []

    for line in input:
        elems = re.match(r"^(\d+)\[(.+)\] (.+)$", line).groups()
        timestamp = int(elems[0].lstrip("0"))
        record_type = elems[1]
        record_data = elems[2]
        print(timestamp, record_type, record_data)

        if record_type == "Detect":
            dataelems = re.match(r"([\d.]+),([\d.]+),([\d.]+)", record_data).groups()
            probability = float(dataelems[0])
            posx = float(dataelems[1])
            posy = float(dataelems[2])
            records.append(( timestamp, "Detect", probability, posx, posy ))

        elif record_type == "ToF":
            tof_datalist = [ float(d) for d in record_data.split(",") ]
            records.append(( timestamp, "ToF", tof_datalist))

    records.sort(key=lambda record: record[0])
    timestamps = []
    for record in records:
        timestamp = record[0]
        if timestamp not in timestamps:
            timestamps.append(timestamp)


    content = "Timestamp[msec], Detected, Valid, MinDistance\n"

    for timestamp in timestamps:
        detections = [ record for record in records if record[0] == timestamp and record[1] == "Detect" ]
        tof_data = ([ record[2] for record in records if record[0] == timestamp and record[1] == "ToF" ])[0]
        tof_min = min(tof_data)
        valid_flag = "1" if tof_min < DISTANCE_THRESHOLD else "0"
        content += f"{timestamp}, {len(detections)}, {valid_flag}, {tof_min}\n"

    with open(outputfilepath, "w", encoding="utf-8") as f:
        f.write(content)

if __name__ == '__main__':
    main()

コメント無し