コメント無し

# はじめに 　SpresenseのHDRカメラボードとIMUのデータを基に、スケール付き3次元点群を生成(SfM)したかったのでやってみました。既存技術として確立されており、スマホカメラと内蔵IMUの組み合わせの事例はよく見られますし、特に目新しいものではありませんが…何事も挑戦ということでやってみます。 　本ページで紹介する作品は「2025年 SPRESENSE™ 活用コンテスト by ソニーセミコンダクタソリューションズ株式会社」に応募するものであり、以下画像に示すSpresense メインボード、Spresense HDRカメラボードは、株式会社電波新聞社 Elchika事務局様からモニター提供を受けたものです。     # 作品概要 　作成したものを撮影した状況を以下に示します。     作成したハードウェアはざっくりいうと、 **「SpresenseのHDRカメラボードとマルチIMUアドオンボードのデータを同時にSDカードに保存する。」** というシンプルなものです。Spresense側では特に難しい処理はしていません。今回は写真とIMUの組み合わせですが、ほかにも組み合わせ次第でいろいろできそうな感じがして夢がありますね。 　SpresenseのHDRカメラボードは、暗所や逆光などの厳しい条件でもHDR機能によって高品質な写真を取得することができますので、使用する写真の質が重要なSfMにはピッタリ？？かもしれません。 　マルチIMUアドオンボードは、高頻度で低ノイズな加速度、角速度を計測することができます。このIMUで計測した加速度と、SfMで得られたカメラの加速度を比較することで、スケール情報を与えることができます。 　ざっくりした処理の流れを以下に示します。Spresenseで取得したIMUデータと写真データをCOLMAPとopenMVSに突っ込んでテクスチャ付きメッシュを生成するよということを言っています。  　作成したハードウェアは以下のような構成になっています。  　真に時刻同期したIMUデータとカメラ撮影データをSDカードに保存したかったのですが、実際には撮影指示時点のRTC時刻と、IMUからデータ受信したときのRTC時刻を保存しているだけです。シャッター信号とか、IMUからの同期信号を取ってうまいことやりたかったのですが・・・ 　SDカードに保存するための自作拡張ボードは自作しました。純正の拡張ボードでもよかったのですが、将来的にはロボットとかに乗せたかったので小型化したものを作成しました。SDIOのレベル変換ICやメインボードのコネクタは小さすぎて手はんだが厳しかったのでPCBAで実装してもらいました。   　筐体は以下のように3DCADで設計して3Dプリンタで造形しました。  ## 取得データ　 　cam_imu.recというファイルに画像とIMUデータをまとめて保存しています。これをデコードすると以下のような感じで、ナノ秒単位のタイムスタンプのファイル名で記録された画像と、時系列の角速度、加速度データのcsvが得られます。写真の撮影周期は7.5Hz、IMUのサンプル周期は240Hzです。写真は比較的容量が大きく、SDの保存速度がボトルネックになりました。サンプル周期は5Hzよりも早くしたいが解像度は落としたくない…などの制約で、JPEGのクオリティーを40にしております。IMUは写真に比べるとデータ量はかなり少ないので、もっと高頻度にすることも可能ですが、センサーデータのばらつきが非常に大きくなることから、240Hzくらいに落ち着きました。    　これらをCOLMAPで処理して疎な点群を生成し、その結果を使用してOpenMVSでテクスチャ付きメッシュを生成します。生成したものを以下に示します。   　COLMAPで推定されるカメラ位置はスケール情報が暫定値として計算されるようです。（実空間のサイズ比率が合っていない）そこで、本稿ではIMUの角速度、加速度情報から、スケールを何となく計算してみます。このような後処理でスケールを合わせる手法を疎結合（Loose Coupling）とか呼ぶそうで、先人の知恵として以下のような論文がありましたのでこれを参考にしながら計算しました。 “Janne Mustaniemi et. al., Inertial-Based Scale Estimation for Structure from Motion on Mobile Devices. ,”https://arxiv.org/abs/1611.09498, accessed: 2026-01-26. 　姿勢角はスケールに影響を受けないので、まずは姿勢角でタイムスタンプのズレを合わせます。以下のグラフのオレンジプロットがCOLMAPによって生成されたカメラ軌道を基にした時系列の姿勢角で、青色プロットがIMUの角速度・加速度をMagdwickフィルタにぶっこんで推定したカメラ姿勢です。 　タイムスタンプをちょっとずつずらして、こんな感じで一致するようなΔtを探索しました。     　時間軸がなんとなく合ったっぽいので、次は加速度を比較します。 　COLMAPで生成したカメラの位置差分を計算してカメラ速度、さらにその差分でカメラ加速度を計算します。カメラの撮影周期は約7.5Hz、IMUは240Hzなので、カメラ加速度の差分時間のIMU加速度を平均して比較します。この加速度比がスケールになるって寸法です。詳細な計算についてはSpresense作成物自体とは関連が薄いので省略します。 　つぎに、加速度を基にスケールを合わせた3Dモデルで測った寸法と真値を比べてみます。 　Spresense メインボードの長辺0.05 mに対して、3Dモデルは0.0496 mでした。誤差1%程度で非常に良い…と言いたいところですが、数メートル規模の自動車とかを計測するとパラメータフィッティングが必要なところもあり、実験レベルに留まる感じです。もう少し文献調査して安定した精度が得られる方法を探すつもりです。   # ソースコード

このコードは、下記の外部成果物（または配布物に含まれるモジュール）を使用しています。 著作権は各権利者に帰属し、利用条件は各ライセンスに従います。

- 対象: `struct`, `io`（`BytesIO`）, `pathlib`（`Path`） - ライセンス: Python Software Foundation License（PSF License） - 権利者: Python Software Foundation および貢献者

- 対象: `numpy` - ライセンス: BSD 3-Clause License - 権利者: NumPy Developers および貢献者

- 対象: `PIL`（`Image`, `ImageFile`） - ライセンス: HPND License（PIL互換ライセンス） - 権利者: Pillow Contributors / PIL Authors ほか（配布物の記載に準拠） ※本記事のサンプルコード自体のライセンスは、別途明記がない限り著作権者（著者）に留保されます。 ```python:decode.py import struct from io import BytesIO from pathlib import Path import numpy as np from PIL import Image, ImageFile ImageFile.LOAD_TRUNCATED_IMAGES = True REC_PATH = r"cam_imu.rec" OUT_DIR = r"out_dataset" REC_MAGIC = 0xA55A REC_IMU = 0 REC_JPEG = 1 REC_HDR_FMT = "<HBBQI" # 16 bytes REC_HDR_SIZE = struct.calcsize(REC_HDR_FMT) START_MARKER = b"\xA5\x5A" END_MARKER = b"\x0D\x0A" PAYLOAD_SIZE = 136 ENDIAN = "<" TIMER_FREQ_HZ = 19_200_000.0 TIMER_WRAP_MOD = 2**32 TIMER_WRAP_SECONDS = TIMER_WRAP_MOD / TIMER_FREQ_HZ WRAP_DETECT_THRESHOLD_SEC = 200.0 GYRO_SCALE = 1.0 ACC_SCALE = 1.0 def build_payload_dtype(endian: str) -> np.dtype: header_dtype = np.dtype([ ("unix_s", endian + "u4"), ("nsec", endian + "i4"), ]) set_dtype = np.dtype([ ("ts_u32", endian + "u4"), ("temp", endian + "f4"), ("gx", endian + "f4"), ("gy", endian + "f4"), ("gz", endian + "f4"), ("ax", endian + "f4"), ("ay", endian + "f4"), ("az", endian + "f4"), ]) payload_dtype = np.dtype([ ("header", header_dtype), ("sets", set_dtype, (4,)), ]) if payload_dtype.itemsize != PAYLOAD_SIZE: raise ValueError(f"dtype size mismatch: expected {PAYLOAD_SIZE}, got {payload_dtype.itemsize}") return payload_dtype class TimerUnwrapper: __slots__ = ("freq_hz", "wrap_seconds", "wrap_threshold_sec", "last_sec", "cycle_count") def __init__(self, freq_hz: float, wrap_seconds: float, wrap_threshold_sec: float) -> None: self.freq_hz = float(freq_hz) self.wrap_seconds = float(wrap_seconds) self.wrap_threshold_sec = float(wrap_threshold_sec) self.last_sec = None self.cycle_count = 0 def unwrap(self, ts_u32: int) -> float: sec = float(ts_u32) / self.freq_hz if self.last_sec is not None: if (self.last_sec - sec) > self.wrap_threshold_sec: self.cycle_count += 1 self.last_sec = sec return sec + self.cycle_count * self.wrap_seconds def iter_payloads_from_bytes(buf: bytes): n = len(buf) i = 0 need = 2 + PAYLOAD_SIZE + 2 while i + need <= n: if buf[i:i+2] == START_MARKER: payload = buf[i+2:i+2+PAYLOAD_SIZE] end = buf[i+2+PAYLOAD_SIZE:i+2+PAYLOAD_SIZE+2] if end == END_MARKER: yield payload i += need continue i += 1 def decode_imu_payload_stream_to_rows(imu_stream: bytes, endian: str = "<"): payload_dtype = build_payload_dtype(endian) ref_unwrap = TimerUnwrapper(TIMER_FREQ_HZ, TIMER_WRAP_SECONDS, WRAP_DETECT_THRESHOLD_SEC) imu_unwrap = TimerUnwrapper(TIMER_FREQ_HZ, TIMER_WRAP_SECONDS, WRAP_DETECT_THRESHOLD_SEC) rows = [] for payload in iter_payloads_from_bytes(imu_stream): try: rec = np.frombuffer(payload, dtype=payload_dtype, count=1)[0] except Exception: continue unix_s = int(rec["header"]["unix_s"]) nsec = int(rec["header"]["nsec"]) frame_time_sec = float(unix_s) + float(nsec) * 1e-9 sets = rec["sets"] try: ts_ref = int(sets[3]["ts_u32"]) except Exception: continue ref_imu_s = ref_unwrap.unwrap(ts_ref) for i in range(4): s = sets[i] ts_u32 = int(s["ts_u32"]) gx = float(s["gx"]) * GYRO_SCALE gy = float(s["gy"]) * GYRO_SCALE gz = float(s["gz"]) * GYRO_SCALE ax = float(s["ax"]) * ACC_SCALE ay = float(s["ay"]) * ACC_SCALE az = float(s["az"]) * ACC_SCALE imu_s = imu_unwrap.unwrap(ts_u32) t_sync_s = frame_time_sec - ref_imu_s + imu_s t_ns = int(round(t_sync_s * 1_000_000_000.0)) t_imu_ns = int(round(imu_s * 1_000_000_000.0)) rows.append((t_imu_ns, t_ns, gx, gy, gz, ax, ay, az)) rows.sort(key=lambda x: x[0]) buf = np.array(rows) buf[:,0] = buf[:,0] - buf[0,0] + buf[0,1] buf = np.delete(buf, 1, 1) rows = buf.tolist() return rows def write_unique(path: Path, data: bytes) -> Path: path.parent.mkdir(parents=True, exist_ok=True) if not path.exists(): path.write_bytes(data) return path stem, suf = path.stem, path.suffix k = 1 while True: p2 = path.with_name(f"{stem}_{k}{suf}") if not p2.exists(): p2.write_bytes(data) return p2 k += 1 def jpeg_bytes_to_png_bytes(jpeg_bytes: bytes) -> bytes: if not jpeg_bytes.endswith(b"\xFF\xD9"): jpeg_bytes = jpeg_bytes + b"\xFF\xD9" try: with Image.open(BytesIO(jpeg_bytes)) as im: im.load() if im.mode not in ("RGB", "L"): im = im.convert("RGB") out = BytesIO() # im.save(out, format="PNG") im.save(out, format="JPEG") return out.getvalue() except Exception as e: return None def export_rec_to_orbslam3(rec_path: Path, out_dir: Path): cam_dir = out_dir / "mav0" / "cam0" / "data" imu_dir = out_dir / "mav0" / "imu0" cam_dir.mkdir(parents=True, exist_ok=True) imu_dir.mkdir(parents=True, exist_ok=True) img_entries = [] imu_stream_chunks = [] bad_jpeg = 0 file_size = rec_path.stat().st_size rec_idx = 0 MAX_PAYLOAD = 20_000_000 with rec_path.open("rb") as f: while True: hdr_pos = f.tell() hdr = f.read(REC_HDR_SIZE) if not hdr: break if len(hdr) != REC_HDR_SIZE: print(f"[WARN] EOF in header at pos={hdr_pos} (remain={file_size-hdr_pos} bytes). stop.") break try: magic, rtype, rsv, t_ns, length = struct.unpack(REC_HDR_FMT, hdr) except Exception as e: print(f"[WARN] header unpack failed at pos={hdr_pos}: {e}. resync by 1 byte.") f.seek(hdr_pos + 1) continue if magic != REC_MAGIC: f.seek(hdr_pos + 1) continue payload_pos = f.tell() remain = file_size - payload_pos if length > MAX_PAYLOAD or length > remain: if length > remain: print(f"[WARN] EOF in payload at rec#{rec_idx} pos={payload_pos} " f"type={rtype} t_ns={t_ns} need={length} remain={remain}. stop (drop last record).") break else: print(f"[WARN] suspicious length at rec#{rec_idx} pos={payload_pos} " f"type={rtype} t_ns={t_ns} length={length}. resync by 1 byte.") f.seek(hdr_pos + 1) continue payload = f.read(length) if len(payload) != length: print(f"[WARN] EOF in payload(read) at rec#{rec_idx} pos={payload_pos} " f"type={rtype} t_ns={t_ns} need={length} got={len(payload)}. stop.") break rec_idx += 1 if rtype == REC_JPEG: png_name = f"{int(t_ns)}.jpg" png_path = cam_dir / png_name png_bytes = jpeg_bytes_to_png_bytes(payload) if png_bytes is None: bad_jpeg += 1 continue final_path = write_unique(png_path, png_bytes) img_entries.append((int(t_ns), final_path.name)) elif rtype == REC_IMU: imu_stream_chunks.append(payload) img_entries.sort(key=lambda x: x[0]) print("bad_jpeg:", bad_jpeg) print("records:", rec_idx) (out_dir / "timestamps.txt").write_text( "\n".join(str(t) for (t, _) in img_entries) + "\n", encoding="utf-8" ) cam_csv = out_dir / "mav0" / "cam0" / "data.csv" with cam_csv.open("w", encoding="utf-8", newline="") as g: g.write("#timestamp [ns],filename\n") for t, fname in img_entries: g.write(f"{t},{fname}\n") imu_stream = b"".join(imu_stream_chunks) imu_rows = decode_imu_payload_stream_to_rows(imu_stream, endian=ENDIAN) imu_csv = out_dir / "mav0" / "imu0" / "data.csv" with imu_csv.open("w", encoding="utf-8", newline="") as g: g.write("#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],w_RS_S_z [rad s^-1]," "a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2]\n") for t_ns, gx, gy, gz, ax, ay, az in imu_rows: g.write(f"{t_ns},{gx:.9g},{gy:.9g},{gz:.9g},{ax:.9g},{ay:.9g},{az:.9g}\n") print("done") print("rec :", rec_path) print("out :", out_dir) print("images:", len(img_entries), "imu_rows:", len(imu_rows)) if __name__ == "__main__": export_rec_to_orbslam3(Path(REC_PATH), Path(OUT_DIR)) ```

このコードは以下のライブラリを使用して作成されています。 1. `<MP.h>``<Camera.h>``<SDHCI.h>``<RTC.h>`: - GNU Lesser General Public License (LGPL) v2.1 （https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html）を参照 - 詳細: https://developer.sony.com/develop/spresense/ - このライブラリの著作権: Copyright 2019 Sony Semiconductor Solutions Corporation. 各ライブラリのライセンス条項については、それぞれの公式サイトをご参照ください。 ```arduino:sfm_main.ino #include "init.h" APacket apacket; BPacket bpacket; ImgSlot g_imgq[IMG_SLOTS]; volatile uint8_t g_img_w = 0; volatile uint8_t g_img_r = 0; volatile uint32_t g_img_drop = 0; volatile uint32_t g_img_oversize = 0; static uint32_t g_last_flush_ms = 0; static inline uint64_t rtc_now_ns() { RtcTime t = RTC.getTime(); // epoch ns = seconds*1e9 + nsec return (uint64_t)t.unixtime() * 1000000000ULL + (uint64_t)t.nsec(); } static void write_record(File &f, uint8_t type, uint64_t t_ns, const uint8_t* p, uint32_t len) { RecHdr h; h.magic = REC_MAGIC; h.type = type; h.rsv = 0; h.t_ns = t_ns; h.len = len; f.write((uint8_t*)&h, sizeof(h)); f.write(p, len); } void CamCB(CamImage img) { uint64_t t_ns_tmp = rtc_now_ns(); if (!img.isAvailable()) return; ImgSlot &slot = g_imgq[g_img_w]; if (slot.ready) { g_img_drop++; return; } uint32_t n = img.getImgSize(); if (n > IMG_MAX_BYTES) { g_img_oversize++; return; } digitalWrite(LED_IMGSAVE, HIGH); memcpy(slot.buf, img.getImgBuff(), n); slot.size = n; slot.t_ns = t_ns_tmp; slot.ready = 1; g_img_w = (uint8_t)((g_img_w + 1) % IMG_SLOTS); digitalWrite(LED_IMGSAVE, LOW); } String toSdPath(const char* name) { String s = name; while (s.indexOf("//") >= 0) s.replace("//", "/"); const String mount = "/mnt/sd0"; if (s == mount) s = "/"; if (s.startsWith(mount + "/")) { s = s.substring(mount.length()); // "/mnt/sd0/aaa" -> "/aaa" } if (s.length() > 0 && s[0] != '/') s = "/" + s; return s; } void deleteRootFilesOnly() { File root = SD.open("/"); if (!root) { Serial.println("Failed to open /"); return; } if (!root.isDirectory()) { Serial.println("/ is not a directory"); root.close(); return; } while (true) { File entry = root.openNextFile(); if (!entry) break; if (!entry.isDirectory()) { const char* n = entry.name(); entry.close(); String path = toSdPath(n); if (SD.remove(path.c_str())) { Serial.print("Deleted: "); Serial.println(path); } else { Serial.print("Failed : "); Serial.println(path); } } else { entry.close(); } } root.close(); } void setup() { Serial.begin(CONSOLE_BAUDRATE); delay(100); pinMode(LED_IMGSAVE, OUTPUT); digitalWrite(LED_IMGSAVE, LOW); RTC.begin(); Serial.println("RTC OK"); Serial.println("Insert SD card."); while (!SD.begin()) { ; } deleteRootFilesOnly(); Serial.println(F("Remove all file")); Serial.println("SD OK"); reclogfile = SD.open(REC_FILENAME, FILE_WRITE); if (!reclogfile) { Serial.println("REC open failed"); while (1) ; } Serial.println("Prepare camera (streaming JPEG)"); CamErr err = theCamera.begin(2, CAM_VIDEO_FPS_7_5 , CAM_IMGSIZE_QUADVGA_H, CAM_IMGSIZE_QUADVGA_V, CAM_IMAGE_PIX_FMT_JPG); if (err != CAM_ERR_SUCCESS) { Serial.println("Camera begin failed"); } theCamera.setJPEGQuality(40); theCamera.setHDR(CAM_HDR_MODE_ON); theCamera.setAbsoluteExposure(20); err = theCamera.startStreaming(true, CamCB); if (err != CAM_ERR_SUCCESS) { Serial.println("startStreaming failed"); } Serial.println("Start streaming"); int ret = MP.begin(subcore); if (ret < 0) { Serial.printf("MP.begin(%d) error=%d\n", subcore, ret); } MP.RecvTimeout(MP_RECV_BLOCKING); int8_t msgid; void* adder; MP.Recv(&msgid, &adder, subcore); MP.Send(msgid, &adder, subcore); memset(&apacket, 0, sizeof(apacket)); memset(&bpacket, 0, sizeof(bpacket)); apacket.status = 0; bpacket.status = 0; MP.Send(msgid, &apacket, subcore); MP.Send(msgid, &bpacket, subcore); MP.RecvTimeout(MP_RECV_POLLING); } void loop() { if (apacket.status == 1) { write_record(reclogfile, REC_IMU, rtc_now_ns(), apacket.data_buf, apacket.data_size); apacket.status = 0; apacket.data_size = 0; } if (bpacket.status == 1) { write_record(reclogfile, REC_IMU, rtc_now_ns(), bpacket.data_buf, bpacket.data_size); bpacket.status = 0; bpacket.data_size = 0; } for (int k = 0; k < 2; k++) { ImgSlot &s = g_imgq[g_img_r]; if (!s.ready) break; write_record(reclogfile, REC_JPEG, s.t_ns, s.buf, s.size); s.ready = 0; g_img_r = (uint8_t)((g_img_r + 1) % IMG_SLOTS); } uint32_t ms = millis(); if (ms - g_last_flush_ms > 4000) { reclogfile.flush(); g_last_flush_ms = ms; } } ``` ```arduino:init.h #include <SDHCI.h> #include <MP.h> #include <RTC.h> #include <Camera.h> const char* REC_FILENAME = "cam_imu.rec"; #define CONSOLE_BAUDRATE 1000000 SDClass SD; File reclogfile; CamImage img; RtcTime now; int subcore = 1; static const int LED_IMGSAVE = LED3; struct APacket { volatile int status = 0; uint32_t data_size = 0; uint8_t data_buf[4200]; }; extern APacket apacket; struct BPacket { volatile int status = 0; uint32_t data_size = 0; uint8_t data_buf[4200]; }; extern BPacket bpacket; static inline uint64_t rtc_now_us() { RtcTime t = RTC.getTime(); return (uint64_t)t.unixtime() * 1000000ULL + (uint64_t)(t.nsec() / 1000); } static const uint32_t IMG_INTERVAL_MS = 66; static const int IMG_SLOTS = 2; static const uint32_t IMG_MAX_BYTES = 200 * 1024; struct ImgSlot { volatile uint8_t ready = 0; uint32_t size = 0; uint64_t t_ns = 0; uint8_t buf[IMG_MAX_BYTES]; }; extern ImgSlot g_imgq[IMG_SLOTS]; extern volatile uint8_t g_img_w; extern volatile uint8_t g_img_r; extern volatile uint32_t g_img_drop; extern volatile uint32_t g_img_oversize; static const uint16_t REC_MAGIC = 0xA55A; enum RecType : uint8_t { REC_IMU = 0, REC_JPEG = 1 }; struct __attribute__((packed)) RecHdr { uint16_t magic; uint8_t type; uint8_t rsv; uint64_t t_ns; uint32_t len; }; ``` ## サブコア このコードは以下のライブラリを使用して作成されています。 1. `<MP.h>``<RTC.h>`: - GNU Lesser General Public License (LGPL) v2.1 （https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html）を参照 - 詳細: https://developer.sony.com/develop/spresense/ - このライブラリの著作権: Copyright 2019 Sony Semiconductor Solutions Corporation. 各ライブラリのライセンス条項については、それぞれの公式サイトをご参照ください。 ```arduino:sfm_sub1.ino #include "init.h" static int start_sensing(int fd, int rate, int adrange, int gdrange, int nfifos) { cxd5602pwbimu_range_t range; ioctl(fd, SNIOC_SSAMPRATE, rate); range.accel = adrange; range.gyro = gdrange; ioctl(fd, SNIOC_SDRANGE, (unsigned long)(uintptr_t)&range); ioctl(fd, SNIOC_SFIFOTHRESH, nfifos); ioctl(fd, SNIOC_ENABLE, 1); return 0; } static int drop_50msdata(int fd, int samprate) { int cnt = samprate / 20; /* data size of 50ms */ cnt = ((cnt + MAX_NFIFO - 1) / MAX_NFIFO) * MAX_NFIFO; if (cnt == 0) cnt = MAX_NFIFO; while (cnt) { read(fd, g_data, sizeof(g_data[0]) * MAX_NFIFO); cnt -= MAX_NFIFO; } return 0; } void status_check() { static uint32_t last_blink = 0; static bool blink_on = false; const uint32_t now_ms = millis(); if (g_state == CALMING && (now_ms - last_blink) > 200) { last_blink = now_ms; blink_on = !blink_on; } set_leds(g_state, blink_on); const float gyro2 = gx_g * gx_g + gy_g * gy_g + gz_g * gz_g; const float accm = f_sqrt(ax_g * ax_g + ay_g * ay_g + az_g * az_g); push_window(gyro2, accm); float gyro_rms = 0, acc_mean = 0, acc_std = 0; bool stat_ok = window_stats(gyro_rms, acc_mean, acc_std); bool still = false; bool moving = false; if (stat_ok) { const float acc_gerr = fabsf(acc_mean - G0); still = (gyro_rms < GYRO_RMS_ON) && (acc_std < ACC_STD_ON) && (acc_gerr < ACC_GERR_ON); moving = (gyro_rms > GYRO_RMS_OFF) || (acc_std > ACC_STD_OFF) || (acc_gerr > ACC_GERR_OFF); } if (g_state == CALMING) { if (still) { if (g_still_start_ms == 0) g_still_start_ms = now_ms; if ((now_ms - g_still_start_ms) >= STABLE_HOLD_MS) { g_state = READY; } } else { g_still_start_ms = 0; } } else if (g_state == READY) { if (moving) { g_state = MOVING; } } else { } set_leds(g_state, blink_on); } static int dump_data(int fd) { int c; int ret; int buffer_sw = 0; while (1) { ret = read(fd, g_data, sizeof(g_data[0]) * MAX_NFIFO); if (ret == sizeof(g_data[0]) * MAX_NFIFO) { imu_ts_g = g_data[0].timestamp; temp_c_g = g_data[0].temp; gx_g = g_data[0].gx, gy_g = g_data[0].gy, gz_g = g_data[0].gz; ax_g = g_data[0].ax, ay_g = g_data[0].ay, az_g = g_data[0].az; RtcTime now = RTC.getTime(); uint32_t now_unixtime = now.unixtime(); long now_nsec = now.nsec(); if (buffer_sw == 0) { memcpy(&apacket->data_buf[addr_buffa], &header_data, sizeof(header_data)); addr_buffa = addr_buffa + sizeof(header_data); memcpy(&apacket->data_buf[addr_buffa], &now_unixtime, sizeof(now_unixtime)); addr_buffa = addr_buffa + sizeof(now_unixtime); memcpy(&apacket->data_buf[addr_buffa], &now_nsec, sizeof(now_nsec)); addr_buffa = addr_buffa + sizeof(now_nsec); memcpy(&apacket->data_buf[addr_buffa], reinterpret_cast<const uint8_t *>(&g_data), sizeof(g_data)); addr_buffa = addr_buffa + sizeof(g_data[0]) * MAX_NFIFO; memcpy(&apacket->data_buf[addr_buffa], &footer_data, sizeof(footer_data)); addr_buffa = addr_buffa + sizeof(footer_data); if (addr_buffa > sizeof(apacket->data_buf) - sizeof(g_data[0]) * MAX_NFIFO - ADD_DATASIZE) { apacket->data_size = addr_buffa; apacket->status = 1; addr_buffa = 0; buffer_sw = 1; } } else { memcpy(&bpacket->data_buf[addr_buffb], &header_data, sizeof(header_data)); addr_buffb = addr_buffb + sizeof(header_data); memcpy(&bpacket->data_buf[addr_buffb], &now_unixtime, sizeof(now_unixtime)); addr_buffb = addr_buffb + sizeof(now_unixtime); memcpy(&bpacket->data_buf[addr_buffb], &now_nsec, sizeof(now_nsec)); addr_buffb = addr_buffb + sizeof(now_nsec); memcpy(&bpacket->data_buf[addr_buffb], reinterpret_cast<const uint8_t *>(&g_data), sizeof(g_data)); addr_buffb = addr_buffb + sizeof(g_data[0]) * MAX_NFIFO; memcpy(&bpacket->data_buf[addr_buffb], &footer_data, sizeof(footer_data)); addr_buffb = addr_buffb + sizeof(footer_data); if (addr_buffb > sizeof(bpacket->data_buf) - sizeof(g_data[0]) * MAX_NFIFO - ADD_DATASIZE) { bpacket->data_size = addr_buffb; bpacket->status = 1; addr_buffb = 0; buffer_sw = 0; } } } status_check(); } } static inline float f_sqrt(float x) { return sqrtf(x); } static void push_window(float gyro2, float accm) { if (g_count == WINDOW_N) { float old_g2 = g_gyro2_buf[g_idx]; float old_am = g_accm_buf[g_idx]; g_sum_gyro2 -= old_g2; g_sum_accm -= old_am; g_sum_accm2 -= (double)old_am * (double)old_am; } else { g_count++; } g_gyro2_buf[g_idx] = gyro2; g_accm_buf[g_idx] = accm; g_sum_gyro2 += gyro2; g_sum_accm += accm; g_sum_accm2 += (double)accm * (double)accm; g_idx++; if (g_idx >= WINDOW_N) g_idx = 0; } static bool window_stats(float &gyro_rms, float &acc_mean, float &acc_std) { if (g_count < 10) return false; const double n = (double)g_count; const double mean_g2 = g_sum_gyro2 / n; gyro_rms = (float)sqrt(mean_g2); const double mean_am = g_sum_accm / n; acc_mean = (float)mean_am; const double mean_am2 = g_sum_accm2 / n; double var = mean_am2 - mean_am * mean_am; if (var < 0) var = 0; acc_std = (float)sqrt(var); return true; } static void set_leds(State st, bool blink_wait) { digitalWrite(LED_READY, (st == READY) ? HIGH : LOW); digitalWrite(LED_MOVE, (st == MOVING) ? HIGH : LOW); if (st == CALMING) { digitalWrite(LED_WAIT, blink_wait ? HIGH : LOW); } else { digitalWrite(LED_WAIT, LOW); } } void setup() { Serial.begin(CONSOLE_BAUDRATE); MP.begin(); MP.RecvTimeout(MP_RECV_BLOCKING); RTC.begin(); int8_t msgid = 0; void *adder; MP.Send(msgid, &adder); MP.Recv(&msgid, &adder); MP.Recv(&msgid, &apacket); MP.Recv(&msgid, &bpacket); MP.RecvTimeout(MP_RECV_POLLING); int devfd; board_cxd5602pwbimu_initialize(5); devfd = open(CXD5602PWBIMU_DEVPATH, O_RDONLY); start_sensing(devfd, S_FQ, 4, 500, MAX_NFIFO); drop_50msdata(devfd, S_FQ); dump_data(devfd); } void loop() { } ``` ```arduino:init.h #include "stdint.h" #include <MP.h> #include <sys/ioctl.h> #include <fcntl.h> #include <nuttx/sensors/cxd5602pwbimu.h> #include <arch/board/cxd56_cxd5602pwbimu.h> #include <RTC.h> #define CXD5602PWBIMU_DEVPATH "/dev/imu0" #define MAX_NFIFO (4) #define CONSOLE_BAUDRATE 1000000 //1920, 960, 480, 240, 120, 60, 30, 15 #define S_FQ 240 #define ADD_DATASIZE 12 static const int LED_READY = LED0; static const int LED_WAIT = LED1; static const int LED_MOVE = LED2; enum State : uint8_t { CALMING = 0, READY = 1, MOVING = 2 }; static const float GYRO_RMS_ON = 0.02f; static const float ACC_STD_ON = 0.05f; static const float ACC_GERR_ON = 0.20f; static const float G0 = 9.80665f; static const float GYRO_RMS_OFF = 0.04f; static const float ACC_STD_OFF = 0.10f; static const float ACC_GERR_OFF = 0.40f; static const uint32_t STABLE_HOLD_MS = 3000; static const int WINDOW_N = 200; static State g_state = CALMING; static uint32_t g_still_start_ms = 0; static float g_gyro2_buf[WINDOW_N]; static float g_accm_buf[WINDOW_N]; static int g_idx = 0; static int g_count = 0; static double g_sum_gyro2 = 0.0; static double g_sum_accm = 0.0; static double g_sum_accm2 = 0.0; uint32_t imu_ts_g; float temp_c_g; float gx_g, gy_g, gz_g; float ax_g, ay_g, az_g; uint8_t header_data[2] = {0xa5, 0x5a}; uint8_t footer_data[2] = {0x0d, 0x0a}; static cxd5602pwbimu_data_t g_data[MAX_NFIFO]; struct APacket { volatile int status = 0; uint32_t data_size = 0; uint8_t data_buf[4200]; }; APacket *apacket; uint32_t addr_buffa = 0; struct BPacket { volatile int status = 0; uint32_t data_size = 0; uint8_t data_buf[4200]; }; BPacket *bpacket; uint32_t addr_buffb = 0; ```