Skip to content

Instantly share code, notes, and snippets.

@GOROman

GOROman/gaitame.c

Last active March 2, 2025 14:58
Show Gist options
  • Save GOROman/5e021957c277cec700463c04326b666d to your computer and use it in GitHub Desktop.
Save GOROman/5e021957c277cec700463c04326b666d to your computer and use it in GitHub Desktop.
Download ZIP
#がいためIMU テスト
Raw
gaitame.c
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <sys/ioctl.h>
#include <time.h>
#include <inttypes.h>
#include <fcntl.h>
#include <stdio.h>
#include <unistd.h>
#include <signal.h>
#include <poll.h>
#include <errno.h>
#include <nuttx/sensors/cxd5602pwbimu.h>
#include <math.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#define CXD5602PWBIMU_DEVPATH "/dev/imu0"
#define itemsof(a) (sizeof(a)/sizeof(a[0]))
/* 重力加速度 (m/s^2) */
#define GRAVITY_EARTH 9.80665f
/* 角度変換 */
#define DEG_TO_RAD (M_PI / 180.0f)
#define RAD_TO_DEG (180.0f / M_PI)
/* カルマンフィルタのパラメータ */
#define KF_ACCEL_NOISE 0.1f /* 加速度計測ノイズ - 増加 */
#define KF_GYRO_NOISE 0.02f /* ジャイロ計測ノイズ - 増加 */
#define KF_ACCEL_BIAS_NOISE 0.01f /* 加速度バイアスノイズ - 増加 */
#define KF_GYRO_BIAS_NOISE 0.005f /* ジャイロバイアスノイズ - 増加 */
#define KF_INIT_COV 1000.0f /* 初期共分散 */
/****************************************************************************
* Private values
****************************************************************************/
/* クォータニオン構造体 */
typedef struct {
float w, x, y, z;
} quaternion_t;
/* オイラー角構造体 */
typedef struct {
float roll; /* x軸周りの回転 (ラジアン) */
float pitch; /* y軸周りの回転 (ラジアン) */
float yaw; /* z軸周りの回転 (ラジアン) */
} euler_t;
/* 3次元ベクトル構造体 */
typedef struct {
float x, y, z;
} vector3_t;
/* カルマンフィルタ状態 */
typedef struct {
/* 状態ベクトル */
quaternion_t q; /* 姿勢クォータニオン */
vector3_t gyro_bias; /* ジャイロバイアス */
vector3_t vel; /* 速度 */
vector3_t pos; /* 位置 */
/* 共分散行列 (簡略化のため対角成分のみ) */
float P_q[4]; /* クォータニオン共分散 */
float P_gyro_bias[3]; /* ジャイロバイアス共分散 */
float P_vel[3]; /* 速度共分散 */
float P_pos[3]; /* 位置共分散 */
/* 前回の時間 */
float last_time;
/* 初期化フラグ */
int initialized;
} kalman_state_t;
/****************************************************************************
* Private Functions
****************************************************************************/
/* クォータニオンの初期化 */
static void quaternion_init(quaternion_t *q)
{
q->w = 1.0f;
q->x = 0.0f;
q->y = 0.0f;
q->z = 0.0f;
}
/* クォータニオンの正規化 */
static void quaternion_normalize(quaternion_t *q)
{
float norm = sqrtf(q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z);
if (norm > 0.0f)
{
q->w /= norm;
q->x /= norm;
q->y /= norm;
q->z /= norm;
}
}
/* クォータニオンからオイラー角への変換 */
static void quaternion_to_euler(const quaternion_t *q, euler_t *euler)
{
/* ロール (x軸周り) */
euler->roll = atan2f(2.0f * (q->w * q->x + q->y * q->z),
1.0f - 2.0f * (q->x * q->x + q->y * q->y));
/* ピッチ (y軸周り) */
float sinp = 2.0f * (q->w * q->y - q->z * q->x);
if (fabsf(sinp) >= 1.0f)
{
euler->pitch = copysignf(M_PI / 2.0f, sinp); /* 90度に制限 */
}
else
{
euler->pitch = asinf(sinp);
}
/* ヨー (z軸周り) */
euler->yaw = atan2f(2.0f * (q->w * q->z + q->x * q->y),
1.0f - 2.0f * (q->y * q->y + q->z * q->z));
}
/* クォータニオンの積 q = q1 * q2 */
static void quaternion_multiply(const quaternion_t *q1, const quaternion_t *q2, quaternion_t *result)
{
result->w = q1->w * q2->w - q1->x * q2->x - q1->y * q2->y - q1->z * q2->z;
result->x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y;
result->y = q1->w * q2->y - q1->x * q2->z + q1->y * q2->w + q1->z * q2->x;
result->z = q1->w * q2->z + q1->x * q2->y - q1->y * q2->x + q1->z * q2->w;
}
/* 角速度からクォータニオン変化率を計算 */
static void gyro_to_quaternion_derivative(const quaternion_t *q, const vector3_t *gyro, quaternion_t *dq)
{
dq->w = -0.5f * (q->x * gyro->x + q->y * gyro->y + q->z * gyro->z);
dq->x = 0.5f * (q->w * gyro->x + q->y * gyro->z - q->z * gyro->y);
dq->y = 0.5f * (q->w * gyro->y - q->x * gyro->z + q->z * gyro->x);
dq->z = 0.5f * (q->w * gyro->z + q->x * gyro->y - q->y * gyro->x);
}
/* クォータニオンによるベクトル回転 v' = q * v * q^-1 */
static void quaternion_rotate_vector(const quaternion_t *q, const vector3_t *v, vector3_t *result)
{
quaternion_t p = {0, v->x, v->y, v->z};
quaternion_t q_inv = {q->w, -q->x, -q->y, -q->z};
quaternion_t temp, temp2;
quaternion_multiply(q, &p, &temp);
quaternion_multiply(&temp, &q_inv, &temp2);
result->x = temp2.x;
result->y = temp2.y;
result->z = temp2.z;
}
/* カルマンフィルタの初期化 */
static void kalman_filter_init(kalman_state_t *kf)
{
/* クォータニオンの初期化 */
quaternion_init(&kf->q);
/* バイアスの初期化 */
kf->gyro_bias.x = 0.0f;
kf->gyro_bias.y = 0.0f;
kf->gyro_bias.z = 0.0f;
/* 速度と位置の初期化 */
kf->vel.x = 0.0f;
kf->vel.y = 0.0f;
kf->vel.z = 0.0f;
kf->pos.x = 0.0f;
kf->pos.y = 0.0f;
kf->pos.z = 0.0f;
/* 共分散行列の初期化 */
for (int i = 0; i < 4; i++)
{
kf->P_q[i] = KF_INIT_COV;
}
for (int i = 0; i < 3; i++)
{
kf->P_gyro_bias[i] = KF_INIT_COV;
kf->P_vel[i] = KF_INIT_COV;
kf->P_pos[i] = KF_INIT_COV;
}
kf->initialized = 0;
}
/* カルマンフィルタの更新 */
static void kalman_filter_update(kalman_state_t *kf,
float gx, float gy, float gz,
float ax, float ay, float az,
float timestamp)
{
float dt;
vector3_t gyro, accel, accel_earth, accel_motion;
quaternion_t dq, q_pred;
/* 初回の場合、時間を初期化 */
if (!kf->initialized)
{
kf->last_time = timestamp;
kf->initialized = 1;
return;
}
/* 時間差分の計算 */
dt = timestamp - kf->last_time;
kf->last_time = timestamp;
if (dt <= 0.0f || dt > 0.1f)
{
/* 異常な時間差分は無視 */
return;
}
/* センサーデータの取得 */
gyro.x = gx - kf->gyro_bias.x;
gyro.y = gy - kf->gyro_bias.y;
gyro.z = gz - kf->gyro_bias.z;
accel.x = ax;
accel.y = ay;
accel.z = az;
/* 予測ステップ */
/* クォータニオンの更新 */
gyro_to_quaternion_derivative(&kf->q, &gyro, &dq);
q_pred.w = kf->q.w + dq.w * dt;
q_pred.x = kf->q.x + dq.x * dt;
q_pred.y = kf->q.y + dq.y * dt;
q_pred.z = kf->q.z + dq.z * dt;
quaternion_normalize(&q_pred);
/* 共分散の更新 */
for (int i = 0; i < 4; i++)
{
kf->P_q[i] += KF_GYRO_NOISE * dt;
}
for (int i = 0; i < 3; i++)
{
kf->P_gyro_bias[i] += KF_GYRO_BIAS_NOISE * dt;
}
/* 姿勢の更新 */
kf->q = q_pred;
/* 加速度を地球座標系に変換 */
quaternion_rotate_vector(&kf->q, &accel, &accel_earth);
/* 重力成分の除去 */
accel_motion.x = accel_earth.x;
accel_motion.y = accel_earth.y;
accel_motion.z = accel_earth.z - GRAVITY_EARTH;
/* 速度の更新 */
kf->vel.x += accel_motion.x * dt;
kf->vel.y += accel_motion.y * dt;
kf->vel.z += accel_motion.z * dt;
/* 位置の更新 */
kf->pos.x += kf->vel.x * dt;
kf->pos.y += kf->vel.y * dt;
kf->pos.z += kf->vel.z * dt;
/* 速度と位置の共分散更新 */
for (int i = 0; i < 3; i++)
{
kf->P_vel[i] += KF_ACCEL_NOISE * dt * dt;
kf->P_pos[i] += kf->P_vel[i] * dt * dt;
}
/* 補正ステップ - 観測更新 */
/* 静止状態の検出（加速度の大きさが重力に近いかチェック） */
float accel_magnitude = sqrtf(ax * ax + ay * ay + az * az);
float accel_diff = fabsf(accel_magnitude - GRAVITY_EARTH);
/* 静止状態と判断できる場合、姿勢を補正 */
if (accel_diff < 1.0f) /* 閾値: 1.0 m/s^2 - 緩和 */
{
/* 加速度から推定される重力方向 */
vector3_t gravity_measured = {ax, ay, az};
float gravity_norm = sqrtf(ax * ax + ay * ay + az * az);
/* 正規化 */
if (gravity_norm > 0.0f)
{
gravity_measured.x /= gravity_norm;
gravity_measured.y /= gravity_norm;
gravity_measured.z /= gravity_norm;
}
/* 現在の姿勢から予測される重力方向 */
vector3_t gravity_predicted = {0.0f, 0.0f, 1.0f}; /* 地球座標系での重力方向 */
vector3_t gravity_body;
/* 地球座標系から機体座標系への変換（クォータニオンの逆回転） */
quaternion_t q_inv = {kf->q.w, -kf->q.x, -kf->q.y, -kf->q.z};
quaternion_rotate_vector(&q_inv, &gravity_predicted, &gravity_body);
/* 測定値と予測値の差（クロス積）から回転軸を計算 */
vector3_t correction;
correction.x = gravity_measured.y * gravity_body.z - gravity_measured.z * gravity_body.y;
correction.y = gravity_measured.z * gravity_body.x - gravity_measured.x * gravity_body.z;
correction.z = gravity_measured.x * gravity_body.y - gravity_measured.y * gravity_body.x;
/* カルマンゲインの計算 */
/* 観測ノイズ共分散（加速度計の信頼度） */
float R_accel = 0.05f + 2.0f * accel_diff; /* 加速度の変動に対する感度を下げる */
/* 姿勢のカルマンゲイン計算 */
float K_q[4];
for (int i = 0; i < 4; i++)
{
/* K = P / (P + R) */
K_q[i] = kf->P_q[i] / (kf->P_q[i] + R_accel);
/* 共分散の更新 P = (1 - K) * P */
kf->P_q[i] = (1.0f - K_q[i]) * kf->P_q[i];
}
/* ジャイロバイアスのカルマンゲイン計算 */
float K_gyro_bias[3];
for (int i = 0; i < 3; i++)
{
/* バイアスは姿勢よりもゆっくり更新 */
K_gyro_bias[i] = kf->P_gyro_bias[i] / (kf->P_gyro_bias[i] + R_accel * 10.0f);
/* 共分散の更新 */
kf->P_gyro_bias[i] = (1.0f - K_gyro_bias[i]) * kf->P_gyro_bias[i];
}
/* 速度のカルマンゲイン計算（静止状態では速度は0に近づける） */
float K_vel[3];
for (int i = 0; i < 3; i++)
{
K_vel[i] = kf->P_vel[i] / (kf->P_vel[i] + R_accel * 0.5f);
/* 共分散の更新 */
kf->P_vel[i] = (1.0f - K_vel[i]) * kf->P_vel[i];
}
/* 姿勢の補正 */
quaternion_t q_correction;
q_correction.w = 1.0f;
q_correction.x = correction.x * K_q[1] * 0.5f;
q_correction.y = correction.y * K_q[2] * 0.5f;
q_correction.z = correction.z * K_q[3] * 0.5f;
/* 正規化 */
quaternion_normalize(&q_correction);
/* 姿勢の更新 */
quaternion_t q_updated;
quaternion_multiply(&kf->q, &q_correction, &q_updated);
quaternion_normalize(&q_updated);
kf->q = q_updated;
/* ジャイロバイアスの更新 */
kf->gyro_bias.x += correction.x * K_gyro_bias[0];
kf->gyro_bias.y += correction.y * K_gyro_bias[1];
kf->gyro_bias.z += correction.z * K_gyro_bias[2];
/* 速度ドリフトの補正（静止状態では速度は0に近づける） */
kf->vel.x *= (1.0f - K_vel[0]);
kf->vel.y *= (1.0f - K_vel[1]);
kf->vel.z *= (1.0f - K_vel[2]);
}
}
/* 角速度と加速度の単位変換 */
static void convert_sensor_data(cxd5602pwbimu_data_t *data, int adrange, int gdrange,
float *gx, float *gy, float *gz,
float *ax, float *ay, float *az)
{
/* 角速度の単位変換 (raw -> rad/s) */
float gyro_scale = 1.0f;
*gx = data->gx * gyro_scale;
*gy = data->gy * gyro_scale;
*gz = data->gz * gyro_scale;
/* 加速度の単位変換 (raw -> m/s^2) */
float accel_scale;
switch (adrange)
{
case 2: accel_scale = GRAVITY_EARTH; break;
case 4: accel_scale = GRAVITY_EARTH; break;
case 8: accel_scale = GRAVITY_EARTH; break;
case 16: accel_scale = GRAVITY_EARTH; break;
default: accel_scale = GRAVITY_EARTH;
}
*ax = data->ax * accel_scale;
*ay = data->ay * accel_scale;
*az = data->az * accel_scale;
}
static int start_sensing(int fd, int rate, int adrange, int gdrange,
int nfifos)
{
cxd5602pwbimu_range_t range;
int ret;
/*
* Set sampling rate. Available values (Hz) are below.
*
* 15 (default), 30, 60, 120, 240, 480, 960, 1920
*/
ret = ioctl(fd, SNIOC_SSAMPRATE, rate);
if (ret)
{
printf("ERROR: Set sampling rate failed. %d\n", errno);
return 1;
}
/*
* Set dynamic ranges for accelerometer and gyroscope.
* Available values are below.
*
* accel: 2 (default), 4, 8, 16
* gyro: 125 (default), 250, 500, 1000, 2000, 4000
*/
range.accel = adrange;
range.gyro = gdrange;
ret = ioctl(fd, SNIOC_SDRANGE, (unsigned long)(uintptr_t)&range);
if (ret)
{
printf("ERROR: Set dynamic range failed. %d\n", errno);
return 1;
}
/*
* Set hardware FIFO threshold.
* Increasing this value will reduce the frequency with which data is
* received.
*/
ret = ioctl(fd, SNIOC_SFIFOTHRESH, nfifos);
if (ret)
{
printf("ERROR: Set sampling rate failed. %d\n", errno);
return 1;
}
/*
* Start sensing, user can not change the all of configurations.
*/
ret = ioctl(fd, SNIOC_ENABLE, 1);
if (ret)
{
printf("ERROR: Enable failed. %d\n", errno);
return 1;
}
return 0;
}
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* sensor_main
****************************************************************************/
int main(int argc, FAR char *argv[])
{
int fd;
int ret;
struct pollfd fds[1];
struct timespec start, now, delta;
cxd5602pwbimu_data_t data;
kalman_state_t kf;
float gx, gy, gz; /* 角速度 (rad/s) */
float ax, ay, az; /* 加速度 (m/s^2) */
euler_t euler = {0.0f, 0.0f, 0.0f}; /* オイラー角（初期値は0） */
/* サンプリングレートと出力レートが同じなので分周は不要 */
/* Sensing parameters, see start sensing function. */
printf("IMUセンシング開始 - 数値積分による絶対座標推定\n");
printf("実行時間: 無限（Ctrl+Cで終了）\n");
/* 動き分析のためのパラメータ */
const int samplerate = 120; /* サンプリングレート (Hz) */
const int adrange = 4; /* 加速度レンジ (±4G) */
const int gdrange = 500; /* 角速度レンジ (±500dps) */
const int nfifos = 1; /* FIFOしきい値 */
/* カルマンフィルタの初期化 */
kalman_filter_init(&kf);
fd = open(CXD5602PWBIMU_DEVPATH, O_RDONLY);
if (fd < 0)
{
printf("ERROR: デバイス %s のオープンに失敗しました。 %d\n", CXD5602PWBIMU_DEVPATH, errno);
return 1;
}
fds[0].fd = fd;
fds[0].events = POLLIN;
ret = start_sensing(fd, samplerate, adrange, gdrange, nfifos);
if (ret)
{
close(fd);
return ret;
}
memset(&now, 0, sizeof(now));
/* 無限ループでデータを処理 */
while (1)
{
ret = poll(fds, 1, 1000);
if (ret < 0)
{
if (errno != EINTR)
{
printf("ERROR: pollに失敗しました。 %d\n", errno);
}
break;
}
if (ret == 0)
{
printf("タイムアウト!\n");
continue;
}
/* 最初のデータ取得時に時間測定を開始 */
static int first_data = 1;
if (first_data)
{
clock_gettime(CLOCK_MONOTONIC, &start);
first_data = 0;
}
if (fds[0].revents & POLLIN)
{
/* バッファにあるデータを全て読み取る */
while (1)
{
ret = read(fd, &data, sizeof(data));
if (ret != sizeof(data))
{
if (ret < 0 && errno == EAGAIN)
{
/* バッファが空になった */
break;
}
if (ret == 0)
{
/* データが読み取れなかった場合は再試行 */
usleep(10000); /* 10ms待機 */
continue;
}
printf("ERROR: 読み取りサイズが一致しません! %d (期待値: %d)\n",
ret, (int)sizeof(data));
break;
}
/* センサーデータを物理単位に変換 */
convert_sensor_data(&data, adrange, gdrange, &gx, &gy, &gz, &ax, &ay, &az);
/* カルマンフィルタで姿勢、速度、位置を更新 */
float timestamp = data.timestamp / 19200000.0f; /* 秒単位に変換 */
kalman_filter_update(&kf, gx, gy, gz, ax, ay, az, timestamp);
/* 120Hzでデータを出力（サンプリングレートと同じ） */
/* クォータニオンと位置情報のみを出力（オイラー角は出力しない） */
printf("%8.4f,%+8.4f,%+8.4f,%+8.4f,%+8.4f,%+8.4f,%+8.4f,%+8.4f\n",
timestamp,
kf.q.w, /* クォータニオン w成分 */
kf.q.x, /* クォータニオン x成分 */
kf.q.y, /* クォータニオン y成分 */
kf.q.z, /* クォータニオン z成分 */
kf.pos.x, /* X位置（m） */
kf.pos.y, /* Y位置（m） */
kf.pos.z); /* Z位置（m） */
}
}
/* 無限ループのため、時間チェックによる終了条件を削除 */
}
printf("\n計測終了\n");
return 0;
}
Raw
index.html
<!DOCTYPE html>
<html lang="ja">
<head>
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width, initial-scale=1.0">
<title>IMU 3D可視化 (WebSerial)</title>
<style>
body {
margin: 0;
padding: 0;
overflow: hidden;
font-family: 'Hiragino Sans GB', 'Arial', 'Helvetica', sans-serif;
}
#info {
position: absolute;
bottom: 10px;
left: 10px;
background-color: rgba(0, 0, 0, 0.7);
color: white;
padding: 10px;
border-radius: 5px;
font-size: 14px;
font-weight: bold;
z-index: 100;
}
#connection-status {
position: absolute;
top: 10px;
right: 10px;
background-color: rgba(0, 0, 0, 0.7);
color: white;
padding: 10px;
border-radius: 5px;
font-size: 14px;
z-index: 100;
}
.connected {
color: #4CAF50;
}
.disconnected {
color: #F44336;
}
canvas {
display: block;
}
#serial-controls {
position: absolute;
top: 10px;
left: 50%;
transform: translateX(-50%);
background-color: rgba(0, 0, 0, 0.7);
color: white;
padding: 10px;
border-radius: 5px;
font-size: 14px;
z-index: 100;
display: flex;
gap: 10px;
}
button {
padding: 5px 10px;
border: none;
border-radius: 3px;
background-color: #4CAF50;
color: white;
cursor: pointer;
}
button:hover {
background-color: #45a049;
}
button:disabled {
background-color: #cccccc;
cursor: not-allowed;
}
select {
padding: 5px;
border-radius: 3px;
}
</style>
</head>
<body>
<div id="info">
<div>Roll : <span id="roll">0.0000</span>°</div>
<div>Pitch: <span id="pitch">0.0000</span>°</div>
<div>Yaw : <span id="yaw">0.0000</span>°</div>
<div>Quaternion: [<span id="qw">0.0000</span>, <span id="qx">0.0000</span>, <span id="qy">0.0000</span>, <span id="qz">0.0000</span>]</div>
<div>Position (m):</div>
<div>X: <span id="pos-x">0.0000</span></div>
<div>Y: <span id="pos-y">0.0000</span></div>
<div>Z: <span id="pos-z">0.0000</span></div>
</div>
</div>
<div id="serial-controls">
<button id="connect-button">シリアルポートに接続</button>
<select id="baud-rate">
<option value="115200" selected>115200 bps</option>
<option value="230400">230400 bps</option>
<option value="250000">250000 bps</option>
</select>
</div>
<div id="connection-status">
接続状態: <span id="status" class="disconnected">未接続</span>
</div>
<!-- Three.jsライブラリの読み込み -->
<script src="https://cdn.jsdelivr.net/npm/[email protected]/build/three.min.js"></script>
<script src="https://cdn.jsdelivr.net/npm/[email protected]/examples/js/controls/OrbitControls.js"></script>
<script>
// WebSerial APIのサポートチェック
if (!('serial' in navigator)) {
alert('お使いのブラウザはWeb Serial APIをサポートしていません。Chrome、Edge、Operaなどの最新バージョンをお使いください。');
}
// シリアル通信の設定
let port = null;
let reader = null;
let readableStreamClosed;
let keepReading = false;
let decoder = new TextDecoder();
let lineBuffer = '';
// Three.jsの設定
let scene, camera, renderer, controls;
let arrowX, arrowY, arrowZ;
// 初期化関数
function init() {
// シーンの作成
scene = new THREE.Scene();
scene.background = new THREE.Color(0x222222);
// カメラの設定
camera = new THREE.PerspectiveCamera(75, window.innerWidth / window.innerHeight, 0.1, 1000);
camera.position.z = 3;
// レンダラーの設定
renderer = new THREE.WebGLRenderer({ antialias: true });
renderer.setSize(window.innerWidth, window.innerHeight);
document.body.appendChild(renderer.domElement);
// コントロールの設定
controls = new THREE.OrbitControls(camera, renderer.domElement);
controls.enableDamping = true;
controls.dampingFactor = 0.25;
// 座標軸の作成
createCoordinateSystem();
// 矢印の作成
createArrows();
// グリッドの追加
const gridHelper = new THREE.GridHelper(10, 10);
scene.add(gridHelper);
// ウィンドウリサイズ時の処理
window.addEventListener('resize', onWindowResize, false);
// シリアル接続ボタンの設定
document.getElementById('connect-button').addEventListener('click', connectToSerial);
// アニメーションループの開始
animate();
}
// 座標系の作成
function createCoordinateSystem() {
// X軸（赤）
const xAxis = new THREE.ArrowHelper(
new THREE.Vector3(1, 0, 0),
new THREE.Vector3(0, 0, 0),
1.5,
0xff0000,
0.1,
0.05
);
scene.add(xAxis);
// Y軸（緑）
const yAxis = new THREE.ArrowHelper(
new THREE.Vector3(0, 1, 0),
new THREE.Vector3(0, 0, 0),
1.5,
0x00ff00,
0.1,
0.05
);
scene.add(yAxis);
// Z軸（青）
const zAxis = new THREE.ArrowHelper(
new THREE.Vector3(0, 0, 1),
new THREE.Vector3(0, 0, 0),
1.5,
0x0000ff,
0.1,
0.05
);
scene.add(zAxis);
// 軸ラベルの追加
addAxisLabel('X', new THREE.Vector3(1.7, 0, 0), 0xff0000);
addAxisLabel('Y', new THREE.Vector3(0, 1.7, 0), 0x00ff00);
addAxisLabel('Z', new THREE.Vector3(0, 0, 1.7), 0x0000ff);
}
// 軸ラベルの追加
function addAxisLabel(text, position, color) {
const canvas = document.createElement('canvas');
const context = canvas.getContext('2d');
canvas.width = 64;
canvas.height = 64;
context.fillStyle = `rgb(${color >> 16 & 255}, ${color >> 8 & 255}, ${color & 255})`;
context.font = '48px Arial';
context.textAlign = 'center';
context.textBaseline = 'middle';
context.fillText(text, 32, 32);
const texture = new THREE.CanvasTexture(canvas);
const material = new THREE.SpriteMaterial({ map: texture });
const sprite = new THREE.Sprite(material);
sprite.position.copy(position);
sprite.scale.set(0.3, 0.3, 0.3);
scene.add(sprite);
}
// 矢印の作成
function createArrows() {
// 矢印のパラメータ
const arrowLength = 1; // 矢印の長さ
const headLength = 0.3; // 矢印の頭の長さ
const headWidth = 0.2; // 矢印の頭の幅
const cylinderRadius = 0.05; // 円柱の半径
// カスタム矢印を作成する関数
function createCustomArrow(direction, color) {
const group = new THREE.Group();
// 円柱部分（シャフト）
const cylinderHeight = arrowLength - headLength;
const cylinderGeometry = new THREE.CylinderGeometry(
cylinderRadius, cylinderRadius, cylinderHeight, 16
);
const cylinderMaterial = new THREE.MeshBasicMaterial({ color: color });
const cylinder = new THREE.Mesh(cylinderGeometry, cylinderMaterial);
// 円柱を適切な位置に配置（中心から半分上に移動）
cylinder.position.copy(direction.clone().multiplyScalar(cylinderHeight / 2));
// 円錐部分（矢印の頭）
const coneGeometry = new THREE.ConeGeometry(headWidth, headLength, 16);
const coneMaterial = new THREE.MeshBasicMaterial({ color: color });
const cone = new THREE.Mesh(coneGeometry, coneMaterial);
// 円錐を適切な位置に配置（シャフトの先端に）
cone.position.copy(direction.clone().multiplyScalar(arrowLength - (headLength / 2)));
// 方向に合わせて回転
if (!direction.equals(new THREE.Vector3(0, 1, 0))) {
const rotationAxis = new THREE.Vector3(0, 1, 0).cross(direction).normalize();
const angle = Math.acos(new THREE.Vector3(0, 1, 0).dot(direction));
cylinder.setRotationFromAxisAngle(rotationAxis, angle);
cone.setRotationFromAxisAngle(rotationAxis, angle);
}
group.add(cylinder);
group.add(cone);
return group;
}
// X方向の矢印（赤）
arrowX = createCustomArrow(new THREE.Vector3(1, 0, 0), 0xff0000);
scene.add(arrowX);
// Y方向の矢印（緑）
arrowY = createCustomArrow(new THREE.Vector3(0, 1, 0), 0x00ff00);
scene.add(arrowY);
// Z方向の矢印（青）
arrowZ = createCustomArrow(new THREE.Vector3(0, 0, 1), 0x0000ff);
scene.add(arrowZ);
}
// ウィンドウリサイズ時の処理
function onWindowResize() {
camera.aspect = window.innerWidth / window.innerHeight;
camera.updateProjectionMatrix();
renderer.setSize(window.innerWidth, window.innerHeight);
}
// シリアルポートに接続
async function connectToSerial() {
try {
// ユーザーにシリアルポートの選択を促す
port = await navigator.serial.requestPort();
// 選択されたボーレートを取得
const baudRate = parseInt(document.getElementById('baud-rate').value);
// ポートを開く
await port.open({ baudRate: baudRate });
// 接続状態を更新
document.getElementById('status').textContent = '接続済み';
document.getElementById('status').className = 'connected';
document.getElementById('connect-button').textContent = '切断';
document.getElementById('connect-button').removeEventListener('click', connectToSerial);
document.getElementById('connect-button').addEventListener('click', disconnectFromSerial);
// 読み取り開始
keepReading = true;
readSerialData();
console.log(`シリアルポートに接続しました (${baudRate} bps)`);
} catch (error) {
console.error('シリアルポート接続エラー:', error);
}
}
// シリアルポートから切断
async function disconnectFromSerial() {
if (port) {
try {
keepReading = false;
// リーダーを閉じる
if (reader) {
await reader.cancel();
await readableStreamClosed;
}
// ポートを閉じる
await port.close();
port = null;
// 接続状態を更新
document.getElementById('status').textContent = '未接続';
document.getElementById('status').className = 'disconnected';
document.getElementById('connect-button').textContent = 'シリアルポートに接続';
document.getElementById('connect-button').removeEventListener('click', disconnectFromSerial);
document.getElementById('connect-button').addEventListener('click', connectToSerial);
console.log('シリアルポートから切断しました');
} catch (error) {
console.error('シリアルポート切断エラー:', error);
}
}
}
// シリアルデータの読み取り
async function readSerialData() {
if (!port) return;
try {
reader = port.readable.getReader();
readableStreamClosed = reader.closed;
while (keepReading) {
const { value, done } = await reader.read();
if (done) {
// リーダーが閉じられた
break;
}
// 受信したデータをデコード
const text = decoder.decode(value);
processSerialData(text);
}
// リーダーを解放
reader.releaseLock();
} catch (error) {
console.error('シリアルデータ読み取りエラー:', error);
}
}
// シリアルデータの処理
function processSerialData(text) {
// 受信したテキストを行バッファに追加
lineBuffer += text;
// 完全な行を処理
let lineEndIndex;
while ((lineEndIndex = lineBuffer.indexOf('\n')) !== -1) {
const line = lineBuffer.substring(0, lineEndIndex).trim();
lineBuffer = lineBuffer.substring(lineEndIndex + 1);
if (line) {
console.log('受信:', line);
// NuttXの起動メッセージを検出した場合
if (line === "NuttShell (NSH) NuttX-12.3.0") {
console.log("「pwbimu」コマンドを送信します。");
sendSerialCommand("pwbimu\r\n");
continue;
}
// エラーメッセージの検出
if (line.includes("ERROR:")) {
console.error('IMUデバイスからエラーが報告されました:', line);
continue;
}
// データの解析
try {
// カンマで分割し、各値の前後の空白を削除してから浮動小数点に変換
const values = line.split(',').map(val => parseFloat(val.trim()));
if (values.length === 8 && !isNaN(values[0])) {
const [timestamp, qw, qx, qy, qz, posX, posY, posZ] = values;
// クォータニオンからオイラー角を計算
const euler = quaternionToEuler(qw, qx, qy, qz);
// データを更新
const data = {
timestamp: timestamp,
roll: euler.roll * 180 / Math.PI, // ラジアンから度に変換
pitch: euler.pitch * 180 / Math.PI, // ラジアンから度に変換
yaw: euler.yaw * 180 / Math.PI, // ラジアンから度に変換
quaternion: {
w: qw,
x: qx,
y: qy,
z: qz
},
position: {
x: posX,
y: posY,
z: posZ
}
};
updateIMUData(data);
} else {
console.warn('予期しないデータ形式です:', line);
}
} catch (error) {
console.error('データ解析エラー:', error);
}
}
}
}
// シリアルコマンドの送信
async function sendSerialCommand(command) {
if (!port) return;
try {
const encoder = new TextEncoder();
const writer = port.writable.getWriter();
await writer.write(encoder.encode(command));
writer.releaseLock();
} catch (error) {
console.error('シリアルコマンド送信エラー:', error);
}
}
// IMUデータの更新
function updateIMUData(data) {
// オイラー角の表示を更新（小数点以下2桁）
document.getElementById('roll').textContent = data.roll.toFixed(2);
document.getElementById('pitch').textContent = data.pitch.toFixed(2);
document.getElementById('yaw').textContent = data.yaw.toFixed(2);
// クォータニオンの表示を更新
document.getElementById('qw').textContent = data.quaternion.w.toFixed(4);
document.getElementById('qx').textContent = data.quaternion.x.toFixed(4);
document.getElementById('qy').textContent = data.quaternion.y.toFixed(4);
document.getElementById('qz').textContent = data.quaternion.z.toFixed(4);
// 位置情報の表示を更新（存在する場合）
if (data.position) {
document.getElementById('pos-x').textContent = data.position.x.toFixed(4);
document.getElementById('pos-y').textContent = data.position.y.toFixed(4);
document.getElementById('pos-z').textContent = data.position.z.toFixed(4);
}
// Three.jsのクォータニオンを作成
const q = data.quaternion;
const quaternion = createQuaternion(q.w, q.x, q.y, q.z);
// 矢印の方向を更新
updateArrow(arrowX, quaternion, 0);
updateArrow(arrowY, quaternion, 1);
updateArrow(arrowZ, quaternion, 2);
}
// クォータニオンからオイラー角への変換
function quaternionToEuler(w, x, y, z) {
// ロール (x軸周り)
const roll = Math.atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y));
// ピッチ (y軸周り)
let sinp = 2.0 * (w * y - z * x);
let pitch;
if (Math.abs(sinp) >= 1) {
pitch = Math.sign(sinp) * Math.PI / 2; // 90度に制限
} else {
pitch = Math.asin(sinp);
}
// ヨー (z軸周り)
const yaw = Math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
return { roll, pitch, yaw };
}
// Three.jsのクォータニオンを作成
function createQuaternion(w, x, y, z) {
// Three.jsのクォータニオンは(x, y, z, w)の順
return new THREE.Quaternion(x, y, z, w);
}
// 矢印の更新
function updateArrow(arrow, quaternion, axis) {
// 基本方向ベクトル
const baseVectors = [
new THREE.Vector3(1, 0, 0), // X軸
new THREE.Vector3(0, 1, 0), // Y軸
new THREE.Vector3(0, 0, 1) // Z軸
];
// クォータニオンを矢印全体に適用
arrow.quaternion.copy(quaternion);
}
// アニメーションループ
function animate() {
requestAnimationFrame(animate);
controls.update();
renderer.render(scene, camera);
}
// 初期化
init();
// 指定された値を表示
const fixedData = {
roll: -100.04,
pitch: -67.54,
yaw: 43.81,
quaternion: {
w: 0.6545,
x: -0.4577,
y: -0.5690,
z: -0.1959
},
position: {
x: 0.0000,
y: 0.0000,
z: 0.0000
}
};
// 初期表示を固定値に設定
updateIMUData(fixedData);
</script>
</body>
</html>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment