【電気・電子】やっぱり、OpenGL上でジャイロセンサーMPU6050を使って3D紙ヒコーキを飛ばしてみた

　前回はジャイロセンサーを使ってワイヤーフレームモデルを動かしましたが、どうもワイヤーフレームだと奥行がわからず、あっているのかどうか分かり辛かったので、やっぱりOpenGLライブラリを使って3Dモデルを動かして確認することにしました。

　ただ、Raspberry Pi でOpenGLは使えなさそうだったので、どうしようか考えました。そこで、Raspberry Pi とパソコン間で通信してジャイロセンサー情報をパソコンに転送して、パソコン上のOpenGLで表示させることを思いつきました。

　元々、WiFi経由でパソコンの画面にRaspberry Pi のデスクトップを表示させて作業をしていますのでネットワーク通信はできている環境です。あとはジャイロセンサー情報を送信すればよいはずです。

紙ヒコーキ飛ばしました

　まずは、結果から紹介したいと思います。早速、動画をご覧ください。

　モニターの右半分はRaspberry Pi のデスクトップ画面で、こちらでPythonプログラムを実行してジャイロセンサー情報を取得して、TCP通信でロール、ピッチ、ヨーの角度情報を送信しています。

　モニターの左半分はパソコンのデスクトップ画面で、こちらでOpenGLを使った3Dビューワアプリを作成して、紙ヒコーキを表示させて、受信したロール、ピッチ、ヨーの角度情報を紙ヒコーキに反映して動かしています。ずいぶん立体感がわかりやすくなりました。

　最後にも動画をあげています。こちらは富士山に向けて紙ヒコーキを飛ばしたり、宇宙空間上に衛星を飛ばしたりしてみましたので合わせてご覧ください。

3Dビューワアプリの作成

　まずは、OpenGLライブラリを使って、3Dモデルを表示できるようにビューワアプリを作成します。

　OpenGL（Open Graphics Library）は、クロスプラットフォームのグラフィックスAPI（Application Programming Interface）で、2Dおよび3Dのコンピュータグラフィックスを描画するための標準的なライブラリです。主にゲーム、CAD、シミュレーション、VR/ARアプリケーションなどで使用されます。

私の最初の仕事はCAD/CAMの開発でした。ちょうどOpenGLが発表された頃で、調べたり確認したりしていました。現在、OpenGLのバージョンは4.xになっていますが、多少慣れていたこともありレガシーな2.ｘでプログラムしました。

OpenGL バージョン	OpenTK の対応状況
1.1 ～ 2.1	固定機能パイプライン
3.0 ～ 3.3	プログラマブルパイプライン
4.0 ～ 4.6	テッセレーションシェーダー、コンピュートシェーダーなど

　開発環境は、Visual Studio 2022 Communityを使い、プログラミング言語は、C#を使います。そこで、OpenGLライブラリのラッパーとしてOpenTKというNuGetライブラリを使いました。
　OpenTK（Open Toolkit） は、C# で OpenGL, OpenAL, OpenCL などの低レベルなグラフィックAPIを扱うための .NET ラッパーライブラリ です。
　また、3DモデルとしてSTLフォーマットを扱うことにしました。STLフォーマットは3Dプリンターで使われてるフォーマットとして知られており、フリーのモデルも多数公開されています。
STL（Stereolithography） は、3Dモデルの形状を三角形（ポリゴン）メッシュで表現するファイルフォーマットです。主に3DプリンターやCAD（設計ソフト）で使用される標準フォーマットです。
以下の投稿で3Dプリンターについて紹介しています。参考まで。

このSTLフォーマットのデータを読み込むためのライブラリとしてSurfaceAnalyzerも利用します。
SurfaceAnalyzerライブラリは、STL形式の3Dモデルを読み込み、移動や回転などの操作を行うことができます。

　作成した3Dビューワアプリはコメントも含めると約１キロライン（1000行）ほどありますので、今回はすべてのコードを掲載できませんでしたのでポイントとなる部分に絞って掲載します。

　今回の3Dビューワアプリは、２つのWindowsフォームを用意しました。１つはメインフォーム（左画面）、もう一つはビューワフォーム（右画面）です。メインフォームは操作パネル用の画面です。ビューワフォームは、3Dモデルを描画する画面です。

OpenGLの初期化から描画まで

　ここでは、ビューワフォーム側のプログラムです。OpenGL（OpenTK）の初期化から3Dモデル描画までのところを紹介します。
まずは、以下のNuGetパッケージのインストールします。
・OpenTK
・OoenTK.GLControl
・SurfaceAnalyzer
以下のようにインストール後にusingディレクティブを追加します。

using OpenTK;
using OpenTK.Graphics.OpenGL;
using SurfaceAnalyzer;
using System.Drawing.Imaging;
using System.Net;
using System.Net.Sockets;
using System.Runtime.InteropServices;
using System.Text;
using System.Text.Json;

次にglControlコントロールを追加します。NuGetでOoenTK.GLControlをインストールするとVisual StudioのツールボックスにOpenTK.GLControl部品が追加されますが、それをフォームに配置してもうまく追加できませんでした。そのため、AddglControl()メソッドでコントロールを追加しています。

 public Viewer(Form_Main parent)
 {
     control = parent;
     InitializeComponent();

     //ビューサイズ
     this.ClientSize = new Size(500, 500);

     AddglControl();
     // 親フォームの右端に子フォームを配置
     this.StartPosition = FormStartPosition.Manual;
     this.Left = parent.Right - 15; // 親フォームの右端に配置
     this.Top = parent.Top; // 親フォームの上端と揃える
     this.Polygon = parent.polygon; // 読み込んだポリゴンデータ
 }
 // Viewerフォームのロード
 private void Viewer_Load(object sender, EventArgs e)
 {
     if (glControl == null) return;
     onlineState = lineState.OFFLINE;
 }

 // glControlコントロールをフォームに追加
 private void AddglControl()
 {
     // フォームレイアウト更新の一時停止
     SuspendLayout();

     //GLControlの初期化
     glControl = new GLControl
     {
         Name = "3D Model Viewer",
         Size = new Size(this.Width, this.Height),
         Location = new System.Drawing.Point(0, 0),
         Dock = DockStyle.Fill
     };
     // ビューを最背面に移動（UIコントロールを前面に移動）
     glControl.SendToBack();

     //イベントハンドラ
     glControl.Load += new EventHandler(glControl_Load);
     glControl.Resize += new EventHandler(Viewer_Resize);
     glControl.MouseDown += new MouseEventHandler(Viewer_MouseDown);
     glControl.MouseMove += new MouseEventHandler(Viewer_MouseMove);
     glControl.MouseUp += new MouseEventHandler(Viewer_MouseUp);
     glControl.MouseWheel += new MouseEventHandler(Viewer_MouseWheel);

     // フォームに追加
     Controls.Add(glControl);

     // フォームレイアウト更新の再開
     ResumeLayout(false);
 }

　OpenGLによる環境設定です。描画によって何を有効化するかを細かく設定します。
・Zバッファの有効化
・ビューポートの設定
・ライティングの設定（必要に応じて）
・テクスチャの有効化（必要に応じて）
これの解説は割愛します。これを解説すると本1冊になってしまいそうです。
その代わり、参考文献を紹介します。以下のブログ等を参考にしました。

参考文献・ブログ（後で見てね）

大阪工業大学：感覚メディア研究室：OpenGL 一読しました。←2025/03/24 閲覧不可になりました。
和歌山大学：床井研究室(※)：OpenGL,GLSLブログ トラブルシュートで役に立ちました。
　　※本ブログは2027年3月に閉鎖される予定のようです。移転先は検討中とのことです。
株式会社エクサ：OpenGL解説 429頁におよぶ内容でPDFで公開されています。一読しました。
C#で3D形状データを表示・操作し、画像を保存する。 コードを流用させていただきました。

 // glControlコントロールのロード
 private void glControl_Load(object sender, EventArgs e)
 {
     if (glControl == null || control == null) return;

     GL.Enable(EnableCap.DepthTest);             // Zバッファを有効にする
     SetupViewport();                            // ビューポートの設定
     //ライティングとマテリアルの反射色の設定
     if (!control.priorityColor)
     {
         SetupLighting();
     }

     // OpenGLのコンテキストをglControlにバインドする
     glControl.MakeCurrent();
     if (control.backcolorMode)
     {
         // 背景色 RGB指定
         GL.ClearColor(control.backcolorRGBA[0] / 255,
             control.backcolorRGBA[1] / 255,
             control.backcolorRGBA[2] / 255,
             control.backcolorRGBA[3] / 255);
     }
     else
     {
         GL.ClearColor(System.Drawing.Color.Black);  // 背景色を設定
         // 背景色 Texture
         if (!string.IsNullOrEmpty(control.textureFile))
         {
             // テクスチャの読込み
             textureID = LoadTexture(control.textureFile);
             GL.Enable(EnableCap.Texture2D);         // 2Dテクスチャを有効化
             GL.TexEnv(TextureEnvTarget.TextureEnv, TextureEnvParameter.TextureEnvMode, (int)TextureEnvMode.Modulate);
         }
     }
 }

 // ビューポートの設定
 private void SetupViewport()
 {
     if (glControl == null) return;

     int width = glControl.Size.Width;
     int hight = glControl.Size.Height;

     GL.Viewport(0, 0, width, hight);
     GL.MatrixMode(MatrixMode.Projection);
     GL.LoadIdentity();
     float aspect = (float)width / (float)hight;

     // 透視投影行列の作成
     // fov(視野角）：45°（小：ズームイン効果大、大：広角効果大）
     // aspect ratio：画面サイズより算出
     // near（手前クリッピング面）： 1.0より手前のオブジェクトは表示しない
     // far（奥のクリッピング面）：256.0より奥のオブジェクトは表示しない
     // near,farはオブジェクトのサイズやオブジェクトを配置する広さによって決める。
     Matrix4 perspective = Matrix4.CreatePerspectiveFieldOfView((float)Math.PI / 4, aspect, 1.0f, 256.0f);
     GL.LoadMatrix(ref perspective);
 }

 // ライティングの設定
 private void SetupLighting()
 {
     if (control == null) return;
     
     // ライティングの有効化
     GL.Enable(EnableCap.Lighting);
     GL.Enable(EnableCap.Light0);
     GL.Enable(EnableCap.ColorMaterial);
     GL.ColorMaterial(MaterialFace.Front, ColorMaterialParameter.AmbientAndDiffuse);

     //ライティング設定
     float[] rgba = new float[4];
     for (int i = 0; i < rgba.Length; i++)
     {
         rgba[i] = control.lightingRGBA[i];
         rgba[i] /= 255;
     }
     float[] lightColor = { rgba[0], rgba[1], rgba[2], rgba[3] };
     // 上からのライティングポイントの定義
     float[] lightPos0 = { 0.0f, 10000.0f, 0.0f, 0.0f };
     float[] lightPos2 = { 0.0f, 0.0f, 10000.0f, 0.0f };

     // ライティングの設定
     GL.Light(LightName.Light0, LightParameter.Position, lightPos0);
     GL.Light(LightName.Light0, LightParameter.Ambient, lightColor);
 }

　ここが3Dモデルを描画する部分です。コメントを入れているので処理順序がわかるかなと思います。
OpenGLはステートマシンと言われています。設定した状態を維持しているため、その状態が未来に対しても反映されます。そのため、思わぬところで妙な動きをしてしまいます。その都度、その状態が必要なのか、不要なのかを意識してプログラムする必要があります。不要な場合は、その設定を解除するとか元に戻すことをしてやります。

// レンダーの更新
public new void Update()
{
    if (Polygon == null) return;
    Render();
    DisplayState();
}

// レンダー（描画）
public void Render()
{
    if (glControl == null || control == null) return;

    // カメラ設定
    Vector3 vec_rotate = new Vector3((float)rotateX, (float)rotateY, (float)rotateZ);
    Vector3 center = new Vector3(N2TK(Polygon.GravityPoint()));
    eye = center + vec_rotate * center.LengthFast / zoom;

    Matrix4 modelView = Matrix4.LookAt(eye, center, Vector3.UnitY);

    // 表示設定
    GL.MatrixMode(MatrixMode.Modelview);
    GL.LoadMatrix(ref modelView);

    // バッファのクリア
    GL.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit);

    if (!control.backcolorMode)
    {
        // 白色にしてテクスチャの色を再現する
        GL.Color3(1.0f, 1.0f, 1.0f);
        // テクスチャ対象を設定する
        GL.BindTexture(TextureTarget.Texture2D, textureID);
        // 画面全体を覆う四角形（クワッド）を描画
        GL.Begin(PrimitiveType.Quads);
        {
            // YZ平面にテクスチャを描画
            GL.TexCoord2(0.0, 0.0); GL.Vertex3(-50.0, 100.0, -150.0);   // 左下
            GL.TexCoord2(1.0, 0.0); GL.Vertex3(-50.0, 100.0, 150.0);    // 右下
            GL.TexCoord2(1.0, 1.0); GL.Vertex3(-50.0, -100.0, 150.0);   // 右上
            GL.TexCoord2(0.0, 1.0); GL.Vertex3(-50.0, -100.0, -150.0);  // 左上
        }
        GL.End();
        // テクスチャ設定対象を無効にする（他モデルへのテクスチャ反映を防止）
        GL.BindTexture(TextureTarget.Texture2D, 0);
    }

    GL.PushMatrix();  // ワールド座標系を保存
    {
        GL.Rotate(angleX, 1.0f, 0.0f, 0.0f);
        GL.Rotate(angleY, 0.0f, 1.0f, 0.0f);
        GL.Rotate(angleZ, 0.0f, 0.0f, 1.0f);
        // 3D形状の表示
        DrawPolygons();
    }
    GL.PopMatrix();  // ワールド座標系を復元

    // ワールド座標系の座標軸を常に最上面に表示
    CoordinateAxes();

    // バッファの入れ替え
    glControl.SwapBuffers();
}

// ワールド座標系の座標軸を常に最上面に表示
private void CoordinateAxes()
{
    // 深度テストを無効化（座標軸を最前面に描画するため）
    GL.Disable(EnableCap.DepthTest);
    GL.DepthMask(false);

    GL.LineWidth(2.0f); // 軸の太さを設定

    GL.Begin(PrimitiveType.Lines);
    {
        // X軸（赤）
        GL.Color3(1.0f, 0.0f, 0.0f);
        GL.Vertex3(0.0f, 0.0f, 0.0f);
        GL.Vertex3(1.0f, 0.0f, 0.0f);

        // Y軸（緑）
        GL.Color3(0.0f, 1.0f, 0.0f);
        GL.Vertex3(0.0f, 0.0f, 0.0f);
        GL.Vertex3(0.0f, 1.0f, 0.0f);

        // Z軸（青）
        GL.Color3(0.0f, 0.0f, 1.0f);
        GL.Vertex3(0.0f, 0.0f, 0.0f);
        GL.Vertex3(0.0f, 0.0f, 1.0f);
    }
    GL.End();
    GL.Begin(PrimitiveType.Polygon);
    {
        // Y座標軸の矢印
        GL.Color3(0.0f, 1.0f, 0.0f);
        GL.Vertex3(0.0f, 1.3f, 0.0f);
        GL.Vertex3(-0.1f, 1.0f, -0.1f);
        GL.Vertex3(0.1f, 1.0f, -0.1f);

        GL.Vertex3(0.0f, 1.3f, 0.0f);
        GL.Vertex3(0.1f, 1.0f, -0.1f);
        GL.Vertex3(0.1f, 1.0f, 0.1f);

        GL.Vertex3(0.0f, 1.3f, 0.0f);
        GL.Vertex3(0.1f, 1.0f, 0.1f);
        GL.Vertex3(-0.1f, 1.0f, 0.1f);

        GL.Vertex3(0.0f, 1.3f, 0.0f);
        GL.Vertex3(-0.1f, 1.0f, 0.1f);
        GL.Vertex3(-0.1f, 1.0f, -0.1f);
    }
    GL.End();
    GL.Begin(PrimitiveType.Polygon);
    {
        // X座標軸の矢印
        GL.Color3(1.0f, 0.0f, 0.0f);
        GL.Vertex3(1.3f, 0.0f, 0.0f);
        GL.Vertex3(1.0f, -0.1f, -0.1f);
        GL.Vertex3(1.0f, 0.1f, -0.1f);

        GL.Vertex3(1.3f, 0.0f, 0.0f);
        GL.Vertex3(1.0f, 0.1f, -0.1f);
        GL.Vertex3(1.0f, 0.1f, 0.1f);

        GL.Vertex3(1.3f, 0.0f, 0.0f);
        GL.Vertex3(1.0f, 0.1f, 0.1f);
        GL.Vertex3(1.0f, -0.1f, 0.1f);

        GL.Vertex3(1.3f, 0.0f, 0.0f);
        GL.Vertex3(1.0f, -0.1f, 0.1f);
        GL.Vertex3(1.0f, -0.1f, -0.1f);
    }
    GL.End();
    GL.Begin(PrimitiveType.Polygon);
    {
        // Z座標軸の矢印
        GL.Color3(0.0f, 0.0f, 1.0f);
        GL.Vertex3(0.0f, 0.0f, 1.3f);
        GL.Vertex3(-0.1f, -0.1f, 1.0f);
        GL.Vertex3(0.1f, -0.1f, 1.0f);

        GL.Vertex3(0.0f, 0.0f, 1.3f);
        GL.Vertex3(0.1f, -0.1f, 1.0f);
        GL.Vertex3(0.1f, 0.1f, 1.0f);

        GL.Vertex3(0.0f, 0.0f, 1.3f);
        GL.Vertex3(0.1f, 0.1f, 1.0f);
        GL.Vertex3(-0.1f, 0.1f, 1.0f);

        GL.Vertex3(0.0f, 0.0f, 1.3f);
        GL.Vertex3(-0.1f, 0.1f, 1.0f);
        GL.Vertex3(-0.1f, -0.1f, 1.0f);
    }
    GL.End();

    // 深度テストを再び有効化
    GL.Enable(EnableCap.DepthTest);
    GL.DepthMask(true);
}

// 詠み込んだポリゴンの描画
private void DrawPolygons()
{
    if (Polygon == null) return;

    //描画
    GL.Begin(PrimitiveType.Triangles);
    {
        //ファセット（三角形）を描画
        for (int l = 0; l < Polygon.Faces.Count; l++)
        {
            // ファセットに対する法線ベクトル
            var normal = Polygon.Faces[l].Normal();
            // 光源リスト（3つの光源）
            List<Vector3> lightSources = new List<Vector3>
            {
                // Y軸が上の場合
                new Vector3(1.0f, 1.0f, 1.0f),      // 光源1（右上）
                new Vector3(-1.0f, 1.0f, -0.5f),    // 光源2（左上）
                new Vector3(-1.0f, -1.0f, -1.0f),   // 光源3（左下）
                new Vector3(0.7f, -0.5f, 0.3f),     // 光源3（左下）
            };
            // 法線ベクトルからのファセット面の反射色の算出
            OpenTK.Vector3 vcr3 = N2TK(normal);
            Vector4 color = ComputeReflectedColor(vcr3, lightSources);

            GL.Color4(color);  // 三角形の色を法線に基づいて設定
            GL.Normal3(vcr3);
            GL.Vertex3(N2TK(Polygon.Faces[l].Vertices[0].P));
            GL.Vertex3(N2TK(Polygon.Faces[l].Vertices[1].P));
            GL.Vertex3(N2TK(Polygon.Faces[l].Vertices[2].P));
        }
    }
    GL.End();
}

// Numerics.Vector3をOpenTK.Vector3に変換
private static OpenTK.Vector3 N2TK(System.Numerics.Vector3 vec3) => new Vector3(vec3.X, vec3.Z, vec3.Y);

// 法線ベクトルに対する反射色の算出
private Vector4 ComputeReflectedColor(Vector3 normal, List<Vector3> lightSources)
{
    // 基本色
    float[] rgba = new float[4];

    for (int i = 0; i < rgba.Length; i++)
    {
        rgba[i] = control.materialRGBA[i];
        rgba[i] /= 255;
    }
    Vector4 baseColor = new Vector4(rgba[0], rgba[1], rgba[2], rgba[3]);

    // 初期値（黒 / 光が当たらない場合）
    Vector4 finalColor = Vector4.Zero;

    // 各光源に対して拡散光を計算
    foreach (var lightDir in lightSources)
    {
        Vector3 lightDirNormalized = lightDir.Normalized(); // 光源の方向を正規化
        float diffuseFactor = Math.Max(Vector3.Dot(normal, lightDirNormalized), 0.0f); // 拡散光計算

        finalColor += baseColor * diffuseFactor; // 拡散光の影響を加算
    }

    // RGBの値を (0.0～1.0) の範囲にクランプ
    finalColor.X = Math.Min(finalColor.X, 1.0f);
    finalColor.Y = Math.Min(finalColor.Y, 1.0f);
    finalColor.Z = Math.Min(finalColor.Z, 1.0f);

    return finalColor;
}

Raspberry Pi とパソコン間の通信

　さて、Raspberry Pi とパソコン間の通信についてです。ここが要ですが、OpenGLで描画する部分が想像以上にコストがかかってしまいました。
　通信にはTCP/IP通信を行い、パソコン側、つまり3Dビューワアプリ側がリスナーになり、口を開けてデータが送信されてくるのを待つ側になります。Raspberry Pi 側、つまりPythonプログラム側がジャイロセンサー情報を送信します。
　通信には、プロトコルという通信手順があります。通信相手同士の取り決めです。例えば、「データ送って！」と伝えて、「分かった。データ送るね！」、「OK、待っているよ」、「データ送ったよ」、「データ受け取ったよ」、「じゃね。」、「じゃね」という具合です。
　今回は、一方的にジャイロセンサー情報を受け取るだけなので音楽配信のようにストリーム通信が適しています。
・リアルタイム性が必要：遅延があるとジャイロセンサーの動きに対して遅延してしまう
・データの順序保証が必要（TCP通信）：データの順がずれるとジャイロセンサーの動きに対して不正動作になる
・連続性のデータ：音楽のような連続データではないが、繰り返しセンサー情報が送信される
以上からもTCP通信によるストリーム通信が最適と思います。

ジャイロセンサー情報の送信（by Raspberry Pi ）

　前回のPythonプログラムに朱書きした部分を追加したものです。ただし、変更していない箇所は省略しています。省略した部分は前回の投稿を見てください。
パソコンにセンサー情報を送信する関数SendGyroInfo(roll, pitch, yaw)を１つ追加しました。

import smbus2
import time
import math
import collections
import pygame
import curses
import socket
import json

# MPU6050のI2Cのアドレス定義
（省略）

# MPU6050オブジェクトモデルの頂点定義
（省略）

# オブジェクトの辺の定義
（省略）
# TCP通信
HOST = "192.168.ｘ.ｘ"  # WindowsPCのIPアドレスにします
PORT = 60001

def read_sensor_data(addr):
（省略）

def get_mpu6050_data():
（省略）

# 角速度のバイアスを補正するためのキャリブレーション
def calibrate_gyro(samples=500):
（省略）

def get_roll_pitch_yaw(dt, ax, ay, az, gx, gy, gz, prev_roll, prev_pitch, prev_yaw):
（省略）

def rotateX(vertex, angle):
（省略）

def rotateY(vertex, angle):
（省略）

def rotateZ(vertex, angle):
（省略）

def project(vertex):
（省略）

def display_cube(stdscr):
    # 画面描画更新のためのクロックオブジェクトの作成
    clock = pygame.time.Clock()
    # ロール・ピッチ・ヨーの前回値の初期化
    prev_roll, prev_pitch, prev_yaw = 0, 0, 0
    # サンプリング時間の前回値の初期化
    prev_time = time.time()

    while True:
        # サンプリング時間dtの算出（開始時）
        current_time = time.time()
        dt = current_time - prev_time
        prev_time = current_time
        
        # MPU6050から加速度、ジャイロ測定値を取得
        ax, ay, az, gx, gy, gz = get_mpu6050_data()
        # 姿勢角度 ロール、ピッチ、ヨーに変換
        roll, pitch, yaw = get_roll_pitch_yaw(dt, 
                                              ax, ay, az, gx, gy ,gz, 
                                              prev_roll, prev_pitch, prev_yaw)
        prev_roll, prev_pitch, prev_yaw = roll, pitch, yaw

        # ターミナルへのデータの表示
        stdscr.clear()
        stdscr.addstr(1, 2, "MPU6050 Sensor Data")
        stdscr.addstr(2, 2, f"Sampling dt:{dt:8.3f}")
        stdscr.addstr(3, 2, f"Calibration: gx:{gx_bias:8.3f}, gy:{gy_bias:8.3f}, gz:{gz_bias:8.3f}")
        stdscr.addstr(4, 2, f"Accel X:%8.3f, Y:%8.3f, Z:%8.3f" % (ax, ay, az))
        stdscr.addstr(5, 2, f"Gyro  X:%8.3f, Y:%8.3f, Z:%8.3f" % (gx, gy, gz))
        stdscr.addstr(6, 2, f"roll: {roll:8.3f}, pitch: {pitch:8.3f}, yaw: {yaw:8.3f}")

        # オブジェクト描画画面のクリア
        screen.fill((0, 0, 0))
        
        # オブジェクト描画画面終了のイベント処理
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                pygame.quit()
                return
        # ロール・ピッチ・ヨーをストリーム処理
        SendGyroInfo(roll, pitch, yaw)

        # 現在の姿勢角度におけるオブジェクトの座標計算
        rotated_vertices = [rotateZ(rotateY(rotateX(v, math.radians(roll)), math.radians(pitch)),math.radians(yaw)) for v in vertices]
        projected_vertices = [project(v) for v in rotated_vertices]

        # オブジェクトの描画
        for edge in edges:
            pygame.draw.line(screen, (255, 255, 255), projected_vertices[edge[0]], projected_vertices[edge[1]], 2)

        # 画面の描画更新
        pygame.display.flip()
        # サンプリング時間の表示
        #stdscr.addstr(5, 2,f"dt: {dt:6.4f}")
        # ターミナルの描画更新
        stdscr.refresh()
        # 画面更新調整時間
        clock.tick(30)

def SendGyroInfo(roll, pitch, yaw):
    with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
        s.connect((HOST, PORT))
        data = {
                "angleX" : roll,
                "angleY" : pitch,
                "angleZ" : yaw,
                }
        json_data = json.dumps(data)
        s.sendall(json_data.encode('utf-8'))

if __name__ == "__main__":
　（省略）

    try:
        # カーシスライブラリの初期化
        curses.initscr()
        # ジャイロセンサーに同期したオブジェクトの描画処理
        curses.wrapper(display_cube)
    except KeyboardInterrupt:  # When 'Ctrl+C' is pressed, the program will be executed.
        pass
    except socket.error as e:
        print(f"Socket error: {e}")
    except OSError as e:
        print(f"OSError: {e}")
    finally:
        # pygameライブラリの終了
        pygame.quit()
        # カーシスライブラリの終了
        curses.endwin()

def SendGyroInfo(roll, pitch, yaw):

SendGyroInfo(roll, pitch, yaw)は、roll, pitch, yawのジャイロセンサーの傾き角度を示す3つのパラメータ（X, Y, Z軸）を渡します。

with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:

　socket.socket(socket.AF_INET, socket.SOCK_STREAM)は、AF_INETでIPv4アドレスを指定し、SOCK_STREAMでTCPプロトコルを使用することを指定しています。with を使うことで、関数を抜けるときに自動的にソケットが閉じられます。ちょっと性能に影響あるかもしれませんが安全です。

s.connect((HOST, PORT))

　HOST, PORTでパソコンのIPアドレスとポート番号を指定してパソコンに接続します。ポート番号は空いている番号を指定します。
# TCP通信
HOST = “192.168.ｘ.ｘ” # WindowsPCのIPアドレスにします
PORT = 60001

　ポート番号の範囲は、0～65535です。しかし、0～1023の範囲は、サーバアプリケーション用に予約されています。また、1024～49151の範囲で、よく利用されるアプリケーションのサーバ側ポート番号で使われています。一般にテンポラリで使えるのは49152～65535の範囲です。それでもパソコンによっては、他のアプリケーションで使われている場合があります。

　もし、重複したポート番号を指定すると、s.connect()でエラーになります。たまたま先に使ってしまうと別のアプリケーションでエラーになってしまいます。

Windowsパソコンで使用されているポート番号を確認する方法は、以下の通りです。
１．検索ボックスに「Cmd」と入力します。
２．コマンドプロンプトを開きます。
３．「netstat -a」と入力します。
以下のように出力されます。ローカルアドレスのコロン（:）の後ろにある番号がポート番号です。
例えば443は、HTTPSで使われているポート番号です

data = {
"angleX" : roll,
"angleY" : pitch,
"angleZ" : yaw,
}

　送信するデータを辞書形式（Pythonのdict）で定義します。それぞれ angleX（ロール角）, angleY（ピッチ角）, angleZ（ヨー角）に対応します。

json_data = json.dumps(data)

　json.dumps(data) により、Pythonの辞書（dict）を JSON形式の文字列に変換しています。

s.sendall(json_data.encode('utf-8'))

　json_data.encode(‘utf-8’)は、文字列をUTF-8エンコーディングに変換し、バイト列（bytes）しています。TCP通信ではバイト列を送信する必要があるためです。
s.sendall()は、途中で途切れることなくデータを全て送信します。send() だと一部のデータしか送信されない可能性があるため sendall() を使用しています。

def display_cube(stdscr):
  （中略）
    while True:
　　 （中略）  
      # MPU6050から加速度、ジャイロ測定値を取得
        ax, ay, az, gx, gy, gz = get_mpu6050_data()
        # 姿勢角度 ロール、ピッチ、ヨーに変換
        roll, pitch, yaw = get_roll_pitch_yaw(dt, 
                                              ax, ay, az, gx, gy ,gz, 
                                              prev_roll, prev_pitch, prev_yaw)
        prev_roll, prev_pitch, prev_yaw = roll, pitch, yaw

        # ターミナルへのデータの表示
     　　（中略）   
       
        # ロール・ピッチ・ヨーをストリーム処理
        SendGyroInfo(roll, pitch, yaw)

　　　　（中略）

display_cube(stdscr)の関数の中でsendGyroInfo(roll, pitch, yaw)をwhileループで送信し続けます。

ジャイロセンサー情報の受信（by Windowsパソコン）

　Raspberry Pi から送信されたジャイロセンサー情報をWindowsパソコン側で受信します。

// ジャイロセンサーの通信サーバー
public async void StartGyroSensorServer(int port)
{
    // 既存のキャンセルトークンをリセット
    cts?.Dispose(); // 以前のトークンを解放
    cts = new CancellationTokenSource();

    try
    {
        // 指定ポートに対してTCPリスナー開始
        server = new TcpListener(IPAddress.Any, port);
        server.Start();

        onlineState = lineState.ONLINE_WAIT;
        Update();
        while (true)
        {
            if (cts.Token.IsCancellationRequested)
            {
                onlineState = lineState.OFFLINE;
                break;
            }
            // 非同期で受信処理
            TcpClient client = await server.AcceptTcpClientAsync();
            _ = ProcessClientAsync(client, cts.Token); // クライアントごとに非同期処理
        }
    }
    catch (OperationCanceledException ex)
    {
　　　（中略）
    }
    finally
    {
        onlineState = lineState.OFFLINE;
        server?.Stop();
    }
}

// ジャイロセンサーの受信処理
private async Task ProcessClientAsync(TcpClient client, CancellationToken token)
{
    try
    {
        using (client)
        using (NetworkStream stream = client.GetStream())
        {
            byte[] buffer = new byte[1024];
            while (true)
            {
                if (cts.Token.IsCancellationRequested)
                {
                    this.Invoke((MethodInvoker)delegate
                    {
                        onlineState = lineState.OFFLINE;
                        Update();
                    });
                    break;
                }
                int bytesRead = await stream.ReadAsync(buffer, 0, buffer.Length, token);
                if (bytesRead == 0) break;

                string jsonData = Encoding.UTF8.GetString(buffer, 0, bytesRead);
                var sensorData = JsonSerializer.Deserialize<SensorData>(jsonData);

                if (sensorData == null) continue;
                this.Invoke((MethodInvoker)delegate
                {
                    // Raspberry Pi(Z軸)の座標系をOpenGL(Y軸)への座標系変更
                    angleX = -sensorData.angleX;
                    angleY = sensorData.angleZ;
                    angleZ = sensorData.angleY;
                    toolStripStatusLabel_Angle.Text = $"Roll, Pitch, Yaw: {-angleX,+6:F3} ,  {angleZ,+6:F3} ,  {angleY,+6:F3}";
                    onlineState = lineState.ONLINE_RECIEVE;
                    Update();
                });
            }
        }
    }
    catch (Exception ex)
    {
        Console.WriteLine($"クライアント処理エラー: {ex.Message}");
    }
}

　簡単に説明すると以下になります。

public async void StartGyroSensorServer(int port)

　指定したポート番号に対して非同期で受信を行う関数です。

server = new TcpListener(IPAddress.Any, port);
server.Start();

　任意のIPアドレスで指定ポート（ここでは60001）に対して受信を開始する処理です。

TcpClient client = await server.AcceptTcpClientAsync();
_ = ProcessClientAsync(client, cts.Token); // クライアントごとに非同期処理

　非同期で受信処理をします。 ProcessClientAsync(client, cts.Token)の関数の中で受信処理をします。

using (client)
using (NetworkStream stream = client.GetStream())

　TcpClient clientは、Raspberry Piから接続されたクライアントソケットです。
NetworkStream stream = client.GetStream()は、クライアントとのデータ送受信用のストリームを取得します。using を使うことで 処理が終了したら自動で閉じることができます。

 int bytesRead = await stream.ReadAsync(buffer, 0, buffer.Length, token);

stream.ReadAsync(buffer, 0, buffer.Length, token)は、非同期でデータを受信します。非同期とすることで描画やメインフォームの操作がブロックされないようにしています。

string jsonData = Encoding.UTF8.GetString(buffer, 0, bytesRead);
var sensorData = JsonSerializer.Deserialize<SensorData>(jsonData);

受信データ（バイト列）をUTF-8の文字列に変換して、JsonSerializer.Deserialize(jsonData) により、JSONデータを SensorData クラスに変換します。

public class SensorData
{
    public float angleX { get; set; }
    public float angleY { get; set; }
    public float angleZ { get; set; }
}

SensorDataクラスはメインフォーム側で定義しています。

this.Invoke((MethodInvoker)delegate
{
    // Raspberry Pi(Z軸)の座標系をOpenGL(Y軸)への座標系変更
    angleX = -sensorData.angleX;
    angleY = sensorData.angleZ;
    angleZ = sensorData.angleY;
    toolStripStatusLabel_Angle.Text = $"Roll, Pitch, Yaw: {-angleX,+6:F3} ,  {angleZ,+6:F3} ,  {angleY,+6:F3}";
    onlineState = lineState.ONLINE_RECIEVE;
    Update();
});

NetworkStream.ReadAsync() は バックグラウンドスレッド で動作するため、スレッドを跨いで処理するためには、Invoke((MethodInvoker)delegate { … }) を使ってUIスレッドでデータ更新しています。

Raspberry Pi上のジャイロセンサーの座標系とOpenGL上の座標系が異なるため、以下のように変換しています。
angleX = -sensorData.angleXでは、X軸の符号を反転
angleY = sensorData.angleZでは、Raspberry PiのZ軸をOpenGLのY軸に変更
angleZ = sensorData.angleYでは、Raspberry PiのY軸をOpenGLのZ軸に変更
最後にUpdate()を実行して、受信したデータで描画を更新しています。

実行結果

　最初と同じ動画ですがご覧ください。

テクスチャマッピングで遊んでみました

　OpenGLライブラリを使うと、画像をテクスチャマッピングで貼り付けることができます。

そこで、「富・士・山！富・士・山！」というわけで紙ヒコーキを富士山めがけて飛ばしてみました。

もうひとつ、カッシーニ探査機に土星のリングを探査飛行させてみました。ジャイロによる動作とマウス操作を組み合わせています。なお、宇宙の画像と探査機の3DモデルはNASA公式ページよりダウンロードして利用してます。NASA公式ページの利用ガイドラインも確認しています。

まとめ

　今回は、OpenGLというグラフィックライブラリを使って3Dモデルを3次元空間に描画して、ジャイロセンサーで動かしてみました。やはりワイヤーフレームと比べものにならないぐらい立体感が味わうことができました。ジャイロの動作もあっていることを確認できました。
　OpenGL 2.xというレガシーなバージョンを使いましたが使い方を理解することができました。時間を見つけて今回作成した3DビューワをOpenGL 4.xの最新バージョンで書き換えてみたいと思います。