Arduino IDEでソニーのIMUを動かしてみた 

ソニーの高性能IMUを入手した！
[ソニーSpresenseページ](https://developer.sony.com/ja/spresense/specifications)

Linux上のSDK環境では動作したが、ArduinoIDEでも動かしてみようとしてハマったので、
健忘録にこちらの記事を書くことにした。

## 結論
先に結論を書くと、ArduinoIDEでもIMUをプログラミングできる！

以前メーカーフェアでソニーの人がArduino環境でも内部ではSDKのNuttXが動いていて、
ArduinoからSDKのAPIを呼び出すことができると言っていた。

この方法を試してみて、うまく動いた。
ただし、3箇所ほどハマったので、他の人にも参考になれば。

ごちゃごちゃと試したことを書いているｇ最終的には、シンプルに動かせる。必要最小限にした動いたINOのコードは記事の最後に記載

## ハマりポイント
1. Arduinoコンパイラでのエラー
1. コンパイラエラー再び
1. IMUボードのドライバの初期化

## 試したこと
1. SDKにある[cxd5602pwbimu_logger](https://github.com/sonydevworld/spresense/blob/b308223fe058bb0a91887df51f6e2aa76f13e22d/examples/cxd5602pwbimu_logger/cxd5602pwbimu_logger_main.c)のコードをそのままINOにコピー
1. main()関数名をimu_main()に変更
1. setup()からimu_main()を呼び出す

setup()からimu_main()を呼び出す際、引数指定が必要になるので、
imu_main()関数の処理を見て、setup()を以下のように記載。

```c:setup()
void setup() {
  imu_main(1,NULL);
}
```

これで動くと思っていた。
いざコンパイルすると、以下のエラーが。

```
sketch_mar1a/sketch_mar1a.ino:49:10: fatal error: log_server.h: No such file or directory
   49 | #include "log_server.h"
      |          ^~~~~~~~~~~~~~
compilation terminated.
exit status 1

Compilation error: log_server.h: No such file or directory
```

単にサンプルコードに必要なヘッダが無いと言われているだけだった。
このヘッダはネットワーク経由でロギングしたい場合に必要なもののようで、
今回はUARTに出したいので、必要ないため、以下2行を削除して、
関連するコードも削除

削除したコードは以下の3箇所

```c
#include "log_server.h"
#include "server_conf.h"
```

```c
static int log2net(cxd5602pwbimu_data_t *dat, int num, void *arg)
{
  return logsvr_sendimudata(dat, num);
}
```

```c
  else if (!strncmp(outdev, "net", 4))
    {
      if (logsvr_initserver(SERVER_PORT_NUM) >= 0)
        {
          logfunc = log2net;
        }
      else
        {
          printf("Log Server could not start...\n");
          return -1;
        }
    }
```

これで動くはず。
で再度コンパイル。
またエラー。。。

```
/sketch_mar1a/sketch_mar1a.ino: In function 'int log2filetxt(cxd5602pwbimu_data_t*, int, void*)':
/sketch_mar1a/sketch_mar1a.ino:176:39: error: no matching function for call to 'conv_f2u_u::conv_f2u_u(float&)'
  176 |                   ((conv_f2u_t)dat[i].temp).u,
     |                                       ^~~~
/sketch_mar1a/sketch_mar1a.ino:62:7: note: candidate: 'conv_f2u_u::conv_f2u_u()'
   62 | union conv_f2u_u
      |       ^~~~~~~~~~
/sketch_mar1a/sketch_mar1a.ino:62:7: note:   candidate expects 0 arguments, 1 provided
/sketch_mar1a/sketch_mar1a.ino:62:7: note: candidate: 'constexpr conv_f2u_u::conv_f2u_u(const conv_f2u_u&)'
/sketch_mar1a/sketch_mar1a.ino:62:7: note:   no known conversion for argument 1 from 'float' to 'const conv_f2u_u&'
/sketch_mar1a/sketch_mar1a.ino:62:7: note: candidate: 'constexpr conv_f2u_u::conv_f2u_u(conv_f2u_u&&)'
```

コードを見ると、floatをunsigne intとして表示しようとしているっぽい。
SDKのコンパイラではOKでArduinoのコンパイラでは通らない。
（C＋＋だから？）
解消させるために、ポインタに直してキャストして、という方法に変更。

```diff_c
       fprintf(fp, "%08x,%08x,%08x,%08x,"
                   "%08x,%08x,%08x,%08x\n",
                   (unsigned int)dat[i].timestamp,
-                  ((conv_f2u_t)dat[i].temp).u,
-                  ((conv_f2u_t)dat[i].gx).u,
-                  ((conv_f2u_t)dat[i].gy).u,
-                  ((conv_f2u_t)dat[i].gz).u,
-                  ((conv_f2u_t)dat[i].ax).u,
-                  ((conv_f2u_t)dat[i].ay).u,
-                  ((conv_f2u_t)dat[i].az).u);
+                  *(unsigned int *)&dat[i].temp,
+                  *(unsigned int *)&dat[i].gx,
+                  *(unsigned int *)&dat[i].gy,
+                  *(unsigned int *)&dat[i].gz,
+                  *(unsigned int *)&dat[i].ax,
+                  *(unsigned int *)&dat[i].ay,
+                  *(unsigned int *)&dat[i].az);
     }
 
   return 0;
```

```diff_c
       printf("%08x,%08x,%08x,%08x,"
              "%08x,%08x,%08x,%08x\n",
              (unsigned int)dat[i].timestamp,
-             ((conv_f2u_t)dat[i].temp).u,
-             ((conv_f2u_t)dat[i].gx).u,
-             ((conv_f2u_t)dat[i].gy).u,
-             ((conv_f2u_t)dat[i].gz).u,
-             ((conv_f2u_t)dat[i].ax).u,
-             ((conv_f2u_t)dat[i].ay).u,
-             ((conv_f2u_t)dat[i].az).u);
+              *(unsigned int *)&dat[i].temp,
+              *(unsigned int *)&dat[i].gx,
+              *(unsigned int *)&dat[i].gy,
+              *(unsigned int *)&dat[i].gz,
+              *(unsigned int *)&dat[i].ax,
+              *(unsigned int *)&dat[i].ay,
+              *(unsigned int *)&dat[i].az);
     }
 
   return 0;
```

これでビルドは通った！
書き込みも成功！

しかし、Arduino IDEのSerial Monitorを起動してみると、、

```
Could not open the device:-1
```

とランタイムエラー。。
どうやら/dev/imu0というデバイスファイルが無いらしい。

Linuxのドライバでもregister_driver()等でデバイスファイルを作成しているから、その箇所がArduino環境では作られていないのか。

SDK側のコードを調べてみると、予想通りの箇所を発見。
[SDKのコードの箇所](https://github.com/SPRESENSE/nuttx/blob/487f65deff9f01b5878ad2e30e0783a64c305492/boards/arm/cxd56xx/spresense/src/cxd56_bringup.c#L270)

```c
#if defined(CONFIG_SENSORS_CXD5602PWBIMU) && \
    !defined(CONFIG_CXD56_CXD5602PWBIMU_LATE_INITIALIZE)
  ret = board_cxd5602pwbimu_initialize(5);
  if (ret < 0)
    {
      _err("ERROR: Failed to initialize CXD5602PWBIMU.\n");
    }
#endif
```

どうやらこれが有効になっていないのでデバイスファイルが作られないらしい。
Arduino環境に、この関数のシンボルはあったので、これを呼び出す。
インクルードファイルがどれかわからず、ひとまず適当にExternする。(ArduinoはC++コンパイラを使っているので、正確にはextern "C"）

```c:setup()
extern "C" int board_cxd5602pwbimu_initialize(int);
void setup() {
  board_cxd5602pwbimu_initialize(5);
  imu_main(1,NULL);
}
```

動いた！

```
15e33b8e,42126100,b979e556,bba56996,b8f9d118,c11baac4,bc88e270,bf8dd0b9
15e38c16,42126100,b953bbf8,bb888689,b943b638,c11bb3ea,bc93e452,bf8d7c9a
15e9da17,42126e80,37f3aa5c,3b88a2d0,39802f42,c11ba108,bc71f218,bf8e37b6
15ea26e4,42126e80,3956ecd2,3b72e7ad,3994df72,c11bb4ac,bc852fa9,bf8e766c
15ea772d,42126e80,b8da26d0,3b597216,395b4a11,c11bae98,bc6a77af,bf8ecf2a
15eac7b3,42126e80,b7e6fbec,3b660636,39bd06e9,c11ba42a,bc7933d6,bf8eb70d
15f263c6,42125f00,b9283620,bb376c16,b91f4f4f,c11baf1b,bc0862ee,bf8e5a54 
15f2ae63,42125f00,366715c0,bb096c48,b95ac5bf,c11ba730,bc14cbdd,bf8ea446 
15f2fed0,42125b80,b98c191e,baff8e06,b8882be8,c11ba902,bc3bec9b,bf8e78ee 
15f34537,42125b80,39403976,bac14fc7,b8dd9602,c11bacbf,bc236c7e,bf8ea466 
15f87571,42125700,ba06ed8e,3b2456b1,39bd0f86,c11b943e,bb8295f2,bf8ee8eb 
15f8c5d6,42126200,b9afa578,3b514195,390bc516,c11b974e,bba0543e,bf8efc81 
15f912ef,42126180,3974a4c2,3b5b07e1,3a152bbf,c11b9394,bc039107,bf8eae79 
15f95cf1,42126180,b89b50ca,3b685f33,39b35e7e,c11b9ea2,bbf38f5c,bf8f00eb 
1600dcb4,42125c80,b9193ba4,bba97a3c,b9c75348,c11bc06c,bc2f95d7,bf8f041c 
16012519,42125c80,b7f3a2cc,bba37c50,b9994e79,c11bbb74,bc281294,bf8ec996
```

## 結論とコード
コンパイラのエラーにハマってしまったが、
要は、最初に board_cxd5602pwbimu_initialize(5)を呼び出せばSDKのAPIをそのまま利用することが可能だ。

以下にコードを整理して、IMUデータをヘキサ表示ではなく浮動小数点表示にしたものを記載しておく
(見にくくなるのでエラーハンドリングも消している)

```c:spresense_imu.ino
#include <stdio.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <nuttx/sensors/cxd5602pwbimu.h>
#include <arch/board/cxd56_cxd5602pwbimu.h>

#define CXD5602PWBIMU_DEVPATH      "/dev/imu0"
#define MAX_NFIFO (4)

static cxd5602pwbimu_data_t g_data[MAX_NFIFO];

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;
}

static int log2uart(cxd5602pwbimu_data_t *dat, int num)
{
  int i;
  for (i = 0; i < num; i++)
    {
      printf("%08lx,%f,%f,%f,%f,%f,%f,%f\n", dat[i].timestamp,
              dat[i].temp, dat[i].gx, dat[i].gy, dat[i].gz,
                           dat[i].ax, dat[i].ay, dat[i].az);
    }

  return 0;
}

static int dump_data(int fd)
{
  int c;
  int ret;

  while (1)
    {
      ret = read(fd, g_data, sizeof(g_data[0]) * MAX_NFIFO);
      if (ret == sizeof(g_data[0]) * MAX_NFIFO)
        {
          log2uart(g_data, MAX_NFIFO);
        }
    }
}

void setup() {
  int devfd;

  board_cxd5602pwbimu_initialize(5);

  devfd = open(CXD5602PWBIMU_DEVPATH, O_RDONLY);

  start_sensing(devfd, 960, 16, 4000, MAX_NFIFO);
  drop_50msdata(devfd, 960);
  dump_data(devfd);
}

void loop() {
}
```