トップシステム > GPSセンサプログラム
GPSセンサプログラム

1.はじめに

最近、GPSの立ち上がりに時間がかかるようになった。今日は、1時間前後散歩したが、GPSが動作しなかった。 ハードの不具合かも知れないが、他に原因があるかもしれない。 まずは、WDKのツールでセンサーの動作状態を確認しよう[1]。

2.WDKのインストールおよびセンサ診断ツールの実行

WDKをインストールすると、「C:\Program Files(x86)\Windows Kits\8.1」に置かれる。 センサー診断ツールは、この下のToolsフォルダ(C:\Program Files(x86)\Windows Kits\8.1\Tools)にある。 この下には「x86」、「x64」の2つのフォルダがあり、32bit版(x86)と64bit版(x64)に別れている。 今回は sensordiagnostictool.exe を実行した。

センサに Broadcom GNSS 4752 Geolocation Sensor と Windows 位置情報取得機能 が含まれる。 現在の地図システムでは Windows 位置情報取得機能を使っている。

以前は Windows 位置情報取得機能が動作していたが、何らかの理由で正常に動作しなくなったようだ。 Broadcom GNSS 4752 Geolocation Sensor は正常に動作している。1秒間隔でイベントが発生している。 また、現在、標高と思っていたデータは ALTITUDE_ELLIPSOID_METERS で海抜は ALTITUDE_ANTENNA_SEALEVEL_METERS のようだ。 これだと、誤差は5〜10m未満のようだ。

直接 Broadcom GNSS 4752 Geolocation Sensor からイベントを採るべきでなく、 やはり Windows 位置情報取得機能 から得るべきかも知れない。 設定により、Broadcom GNSS 4752 Geolocation Sensor からWindows 位置情報取得機能へイベントが移るのであろう。 何らかの理由でこれが移らなくなったのはないか。

3.プログラムの見直し

現在のプログラムを下に示す。

        GeoCoordinateWatcher wtc = new GeoCoordinateWatcher(); 
        wtc.PositionChanged += new EventHandler
                <GeoPositionChangedEventArgs<GeoCoordinate>>(wtcPositionChanged); 
        wtc.Start(); 

    void wtcPositionChanged(object sender, GeoPositionChangedEventArgs<GeoCoordinate> e) { 
        TimeSpan ts = DateTime.Now - lastTime;
        if (ts.TotalSeconds < 2) return;
        double lon = e.Position.Location.Longitude;
        double lat = e.Position.Location.Latitude;
        double alt = e.Position.Location.Altitude;      // 高度(単位:メートル)
        if (Math.Abs(lon-lastLon) + Math.Abs(lat-lastLat) < 0.00001) return;
        lastLon = lon;
        lastLat = lat;
        string time = String.Format("{0:HH:mm:ss}", DateTime.Now);
        sLoc.Text = String.Format("{0}, {1:f7}, {2:f7}, {3:f1}", time, lon, lat, alt);
        listLoc.Add(new Loc(DateTime.Now, lon, lat, alt));
        lastTime = DateTime.Now;
        TimeSpan tsLog = DateTime.Now - Loc.lastLogTime;
        if (tsLog.TotalSeconds >= 600) {
            Loc.saveLocs(listLoc);
            Loc.lastLogTime = DateTime.Now;
        }
        if (fTracking) {
            map.GoTo(lon, lat);
            map.Refresh();
        }
    }

ストアアプリの地図では正しく位置情報が表示される。1秒間隔で位置が微小変動する。 Broadcom GNSS 4752 Geolocation Sensor の情報を使用しているものと思われる。 センサAPIをもう少し調べてみよう。

4.センサAPI

まず、文献[4]のプログラムをコンパイルしよう。

あれこれインストールが必要となり、手間取った。、

#include <atlbase.h>
#define INITGUID 1
#include "Guiddef.h"
#include "SensorsApi.h"
#include "Sensors.h"

void main() {
    CoInitializeEx(NULL, COINIT_MULTITHREADED);

    // センサーマネージャを取り出す為の変数 
    CComPtr<ISensorManager> pSensorManager = NULL; 

    // 取得したセンサーを格納する為の変数 
    CComPtr<ISensor> pAmbientSensor = NULL; 
 
    // センサーマネージャの取得 
    HRESULT hr = CoCreateInstance( 
        __uuidof(SensorManager), NULL, CLSCTX_INPROC_SERVER, 
        __uuidof(ISensorManager), (LPVOID*)&pSensorManager); 
 
    // センサーコレクション変数 
    CComPtr<ISensorCollection> pAmbientSensors; 

    // 照度センサーを取り出す 
    hr = pSensorManager->GetSensorsByType(SENSOR_TYPE_AMBIENT_LIGHT, &pAmbientSensors); 
    if (SUCCEEDED(hr)){ 
        // 取得に成功したので、複数の候補から所望のセンサーを取り出す 
        DWORD count = 0; 
        hr = pAmbientSensors->GetCount(&count); 
        for (DWORD i=0; i<count; i++) { 
            CComPtr<ISensor> pCandidate; 
            hr = pAmbientSensors->GetAt(i, &pCandidate); 
            if (SUCCEEDED(hr)){ 
                // 例えばフレンドリーネームを条件にする場合 
                BSTR friendlyName; 
                hr = pCandidate->GetFriendlyName(&friendlyName); 
                // 取得したフレンドリーネームを基に比較等を行い、確定 
                //... 
                pAmbientSensor = pCandidate; 
                // 使い終わった変数は解放 
                SysFreeString(friendlyName); 
                break; 
            } 
        } 
    }
}

コンパイルすると、次のように警告が出る。ググってみると、色んな人が経験している。 取り敢えずは先に進もう。

c:\gis>cl gps\sapi01.cpp
Microsoft(R) C/C++ Optimizing Compiler Version 18.00.31101 for x86
Copyright (C) Microsoft Corporation.  All rights reserved.

sapi01.cpp
Microsoft (R) Incremental Linker Version 12.00.31101.0
Copyright (C) Microsoft Corporation.  All rights reserved.

/out:sapi01.exe
sapi01.obj
sapi01.obj : warning LNK4254: セクション 'ATL' (50000040) は '.rdata' (40000040)
 に異なる属性を伴ってマージされています

照度センサーをGPSセンサに置き換える。GPSは1個だけなので、for文を省く。

まず、次のプログラムをコンパイル・実行し count=27 を得た。

#include <atlbase.h>
#define INITGUID 1
#include "Guiddef.h"
#include "SensorsApi.h"
#include "Sensors.h"

int main() {
    CoInitializeEx(NULL, COINIT_MULTITHREADED);

    CComPtr<ISensorManager> pSensorManager = NULL;  // センサーマネージャを格納する変数
    CComPtr<ISensor> pGPSSensor = NULL;             // 取得したセンサーを格納する変数 
    IPortableDeviceValues* pPropsToSet = NULL;      // Input
    IPortableDeviceValues* pPropsReturn = NULL;     // Output
    ULONG ulReportInterval;

    // センサーがサポートするプロパティのキーを格納するコレクション 
    IPortableDeviceKeyCollection* pKeys; 
    // センサーがサポートするプロパティ値を格納するコレクション。キーと値のペアを格納 
    IPortableDeviceValues* pValues; 

    // センサーマネージャの取得 
    HRESULT hr = CoCreateInstance(__uuidof(SensorManager), NULL, CLSCTX_INPROC_SERVER, 
                                  __uuidof(ISensorManager), (LPVOID*)&pSensorManager); 
 
    CComPtr<ISensorCollection> pGPSSensors;         // センサーコレクション変数 

    // GPSセンサーを取り出す 
    hr = pSensorManager->GetSensorsByType(SENSOR_TYPE_LOCATION_GPS, &pGPSSensors); 
    if (!SUCCEEDED(hr)) { 
        printf("GPSセンサの取得に失敗しました");
        return 1;
    }
    CComPtr<ISensor> pSensor; 
    hr = pGPSSensors->GetAt(0, &pSensor); 

    hr = pSensor->GetSupportedDataFields(&pKeys); 
    if (!SUCCEEDED(hr)) { 
        printf("プロパティ・キーの取得に失敗しました");
        return 1;
    }
    if (SUCCEEDED(hr)){ 
        DWORD count = 0; 
        hr = pKeys->GetCount(&count); 
        printf("count=%d\n", count);
    } 
    return 0;
}

27個のキー・値があるが、当面、GPSセンサーから次のデータを取り出すことにする。

SENSOR_DATA_TYPE_TIMESTAMP
SENSOR_DATA_TYPE_LATITUDE_DEGREES
SENSOR_DATA_TYPE_LONGITUDE_DEGREES
SENSOR_DATA_TYPE_ALTITUDE_ANTENNA_SEALEVEL_METERS

5.GPSセンサ基本プログラム

例外処理はまだサボっているが、基本プログラムは完成した。

100行程度であるが、この種のプログラムは初めてのため、手間取った。 地図システムは C# で作成しているので、連携が問題である。

多分、dll化して、インポートするのが一番楽であろう。 あるいはプロセス間通信が考えられる。

プロセス間でリングバッファを共有できれば、それでもいいはず。 データは C++ から C# へ一方向である。

#include <atlbase.h>
#include <windows.h>
#define INITGUID 1
#include "Guiddef.h"
#include "SensorsApi.h"
#include "Sensors.h"

class CSensorEventSink : public ISensorEvents { 
  public: 
    // ISensorEvents で定義されているイベント 
    STDMETHOD (OnStateChanged)(ISensor* pSensor, SensorState state); 
    STDMETHOD (OnLeave)(REFSENSOR_ID sensorID); 
    STDMETHOD (OnDataUpdated)(ISensor* pSensor, ISensorDataReport* pNewData); 
    STDMETHOD (OnEvent)(ISensor* pSensor, REFGUID eventId, IPortableDeviceValues* pEventData);
 
    // 以下は、COMのおまじない 
    CSensorEventSink() : m_lRefCount(0) {} 

    STDMETHODIMP QueryInterface(REFIID riid, void **ppObject ) { 
        *ppObject = 0 ; 
        if (riid == __uuidof(ISensorEvents)) { 
            *ppObject = reinterpret_cast<ISensorEvents*>(this); 
        } else if (riid == IID_IUnknown) { 
            *ppObject = reinterpret_cast<IUnknown*>(this); 
        } else { 
            return E_NOINTERFACE; 
        } 
        (reinterpret_cast<IUnknown*>(*ppObject))->AddRef(); 
        return S_OK; 
    } 

    ULONG _stdcall AddRef() { return ++m_lRefCount; } 

    ULONG _stdcall Release() { 
        ULONG lRet = --m_lRefCount; 
        if (m_lRefCount == 0) delete this; 
        return lRet; 
    } 
 
private: 
    long m_lRefCount; 
};

STDMETHODIMP CSensorEventSink::OnDataUpdated(ISensor *pS, ISensorDataReport *pData) { 
    SYSTEMTIME st; 
    PROPVARIANT lonValue, latValue, altValue; 
    if (pData == NULL || pS == NULL) return E_INVALIDARG;
    HRESULT hr = pData->GetTimestamp(&st); 
    hr = pData->GetSensorValue(SENSOR_DATA_TYPE_LONGITUDE_DEGREES, &lonValue); 
    hr = pData->GetSensorValue(SENSOR_DATA_TYPE_LATITUDE_DEGREES, &latValue); 
    hr = pData->GetSensorValue(SENSOR_DATA_TYPE_ALTITUDE_ANTENNA_SEALEVEL_METERS, &altValue); 
    if (SUCCEEDED(hr)){ 
        DOUBLE lonvalue = lonValue.dblVal; 
        DOUBLE latvalue = latValue.dblVal; 
        DOUBLE altvalue = altValue.dblVal; 
        printf("%d/%02d/%02d %02d:%02d:%02d ", st.wYear, 
                st.wMonth, st.wDay, st.wHour, st.wMinute, st.wSecond);
        printf("%.6f %.6f %.1f\n", lonvalue, latvalue, altvalue);
    } 
    return S_OK; 
}

STDMETHODIMP CSensorEventSink::OnStateChanged(ISensor* pSensor, SensorState state) {
    return S_OK; 
}
     
STDMETHODIMP CSensorEventSink::OnLeave(REFSENSOR_ID sensorID) {
    return S_OK; 
}

STDMETHODIMP CSensorEventSink::OnEvent(ISensor* pSensor, REFGUID eventId, 
                                        IPortableDeviceValues* pEventData) {
    return S_OK; 
}

int main() {
    CoInitializeEx(NULL, COINIT_MULTITHREADED);

    CComPtr<ISensorManager> pSensorManager = NULL;  // センサーマネージャを格納する変数
    CComPtr<ISensor> pGPSSensor = NULL;             // 取得したセンサーを格納する変数 
    IPortableDeviceValues* pPropsToSet = NULL;      // Input
    IPortableDeviceValues* pPropsReturn = NULL;     // Output
    ULONG ulReportInterval;

    // センサーマネージャの取得 
    HRESULT hr = CoCreateInstance(__uuidof(SensorManager), NULL, CLSCTX_INPROC_SERVER, 
                                  __uuidof(ISensorManager), (LPVOID*)&pSensorManager); 
 
    CComPtr<ISensorCollection> pGPSSensors;         // センサーコレクション変数 

    // GPSセンサーを取り出す 
    hr = pSensorManager->GetSensorsByType(SENSOR_TYPE_LOCATION_GPS, &pGPSSensors); 
    if (!SUCCEEDED(hr)) { 
        printf("GPSセンサの取得に失敗しました");
        return 1;
    }
    CComPtr<ISensor> pSensor; 
    hr = pGPSSensors->GetAt(0, &pSensor);   // 先頭のセンサを取得する
    CSensorEventSink* pEventSink = new CSensorEventSink(); 
    pSensor->SetEventSink(pEventSink);
    Sleep(100000);
    return 0;
}

6.DLL化

C#プログラムに組み込むために、DLL化を行った。

DLLで公開する関数には __declspec(dllexport) を関数の前におく。 また C++の仕様で関数名が変わってしまうのを防ぐために extern "C" も必要となる。

現在のプログラムでは bool GetGPSData(GPSData *pGPSData) 関数により、最新のデータを返すようにしている。 将来、必要があれば、取り込みが遅れた場合、最新データではなく、 未取り込みのデータを返すように変更する。

// cl /LD /Fe./bin\gpsmonitor.dll /EHsc gps\gpsmonitor.cpp
#include <atlbase.h>
#include <windows.h>
#define  INITGUID 1
#include "Guiddef.h"
#include "SensorsApi.h"
#include "Sensors.h"

typedef struct {
    SYSTEMTIME st;
    double lon, lat, alt;
} GPSData;

const int   Size = 100;
GPSData     Data[Size];
int         Pos = -1;

class CSensorEventSink : public ISensorEvents { 
  public: 
    // ISensorEvents で定義されているイベント 
    STDMETHOD (OnStateChanged)(ISensor* pSensor, SensorState state); 
    STDMETHOD (OnLeave)(REFSENSOR_ID sensorID); 
    STDMETHOD (OnDataUpdated)(ISensor* pSensor, ISensorDataReport* pNewData); 
    STDMETHOD (OnEvent)(ISensor* pSensor, REFGUID eventId, IPortableDeviceValues* pEventData);
 
    // 以下は、COMのおまじない 
    CSensorEventSink() : m_lRefCount(0) {} 

    STDMETHODIMP QueryInterface(REFIID riid, void **ppObject ) { 
        *ppObject = 0; 
        if (riid == __uuidof(ISensorEvents)) { 
            *ppObject = reinterpret_cast<ISensorEvents*>(this); 
        } else if (riid == IID_IUnknown) { 
            *ppObject = reinterpret_cast<IUnknown*>(this); 
        } else { 
            return E_NOINTERFACE; 
        } 
        (reinterpret_cast<IUnknown*>(*ppObject))->AddRef(); 
        return S_OK; 
    } 

    ULONG _stdcall AddRef() { return ++m_lRefCount; } 

    ULONG _stdcall Release() { 
        ULONG lRet = --m_lRefCount; 
        if (m_lRefCount == 0) delete this; 
        return lRet; 
    } 
 
private: 
    long m_lRefCount; 
};

STDMETHODIMP CSensorEventSink::OnDataUpdated(ISensor *pS, ISensorDataReport *pData) { 
    SYSTEMTIME st; 
    PROPVARIANT lonValue, latValue, altValue; 
    if (pData == NULL || pS == NULL) return E_INVALIDARG;
    HRESULT hr = pData->GetTimestamp(&st); 
    hr = pData->GetSensorValue(SENSOR_DATA_TYPE_LONGITUDE_DEGREES, &lonValue); 
    hr = pData->GetSensorValue(SENSOR_DATA_TYPE_LATITUDE_DEGREES, &latValue); 
    hr = pData->GetSensorValue(SENSOR_DATA_TYPE_ALTITUDE_ANTENNA_SEALEVEL_METERS, &altValue); 
    if (SUCCEEDED(hr)){ 
        Pos = (Pos + 1) % Size;
        GPSData *pData = &Data[Pos];
        pData->st = st;
        pData->lon = lonValue.dblVal;
        pData->lat = latValue.dblVal;
        pData->alt = altValue.dblVal;
    } 
    return S_OK; 
}

STDMETHODIMP CSensorEventSink::OnStateChanged(ISensor* pSensor, SensorState state) {
    return S_OK; 
}
     
STDMETHODIMP CSensorEventSink::OnLeave(REFSENSOR_ID sensorID) {
    return S_OK; 
}

STDMETHODIMP CSensorEventSink::OnEvent(ISensor* pSensor, REFGUID eventId, 
                                        IPortableDeviceValues* pEventData) {
    return S_OK; 
}

extern "C" __declspec(dllexport)
bool GPSMonitor() {
    CoInitializeEx(NULL, COINIT_MULTITHREADED);

    CComPtr<ISensorManager> pSensorManager = NULL;  // センサーマネージャを格納する変数
    CComPtr<ISensor> pGPSSensor = NULL;             // 取得したセンサーを格納する変数 
    IPortableDeviceValues* pPropsToSet = NULL;      // Input
    IPortableDeviceValues* pPropsReturn = NULL;     // Output
    ULONG ulReportInterval;

    // センサーマネージャの取得 
    HRESULT hr = CoCreateInstance(__uuidof(SensorManager), NULL, CLSCTX_INPROC_SERVER, 
                                  __uuidof(ISensorManager), (LPVOID*)&pSensorManager); 
 
    CComPtr<ISensorCollection> pGPSSensors;         // センサーコレクション変数 

    // GPSセンサーを取り出す 
    hr = pSensorManager->GetSensorsByType(SENSOR_TYPE_LOCATION_GPS, &pGPSSensors); 
    if (!SUCCEEDED(hr)) { 
        printf("GPSセンサの取得に失敗しました\n");
        return false;
    }
    CComPtr<ISensor> pSensor; 
    hr = pGPSSensors->GetAt(0, &pSensor);   // 先頭のセンサを取得する
    if (!SUCCEEDED(hr)) { 
        printf("先頭GPSセンサの取得に失敗しました\n");
        return false;
    }
    printf("GPSセンサの取得に成功しました\n");
    CSensorEventSink* pEventSink = new CSensorEventSink(); 
    hr = pSensor->SetEventSink(pEventSink);

    for (;;) Sleep(10000);

    return true;
}

extern "C" __declspec(dllexport)
bool GetGPSData(GPSData *pGPSData) {
    if (Pos < 0) return false;
    memcpy(pGPSData, &Data[Pos], sizeof(GPSData));
    return true;
}

テストプログラムを下に示す。

// c:\gis>csc /out:bin\gps.exe src\gps.cs /platform:x86
using System;
using System.Runtime.InteropServices;
using System.Threading;

public struct SYSTEMTIME {
    public ushort wYear, wMonth, wDayOfWeek, wDay; 
    public ushort wHour, wMinute, wSecond, wMilliseconds; 
}

public struct GPSData {
    public SYSTEMTIME st;
    public double lon, lat, alt;
}

class GPS {
    [DllImport("gpsmonitor.dll")]
    private extern static bool GPSMonitor();

    [DllImport("gpsmonitor.dll")]
    private extern static bool GetGPSData(ref GPSData pGPSData);

    static GPSData data;

    [STAThread]
    static void Main(string[] args) {
        Thread thread = new Thread(new ThreadStart(RunGPSMonitor));
        thread.Start();   // スレッドを開始する
        while (true) {
            Thread.Sleep(1000);
            if (!GetGPSData(ref data) || data.st.wYear == 0) continue;           
            string s = string.Format("{0}/{1}/{2} {3}:{4}:{5} {6:f6} {7:f6} {8:f1}",
                data.st.wYear, data.st.wMonth, data.st.wDay, 
                data.st.wHour, data.st.wMinute, data.st.wSecond,
                data.lon, data.lat, data.alt);
            Console.WriteLine(s);
        }
    }

    static void RunGPSMonitor() { GPSMonitor(); }
}

このままでは、地図システムをクローズしても、別スレッドで動くGPSMonitorは停止しないことが分かった。 そこで、プログラムを次のように変更し、地図システムをクローズするとき、GetGPSData(data, 0) をコールして、 GPSMonitorを閉じるようにした。

extern "C" __declspec(dllexport)
bool GPSMonitor() {

    // 省略

    fRunning = true;
    while (fRunning) Sleep(3000);

    printf("GPSセンサ監視を終了しました\n");
    return true;
}

extern "C" __declspec(dllexport)
bool GetGPSData(GPSData *pGPSData, int n) {
    if (Pos < 0) return false;
    if (n == 0) fRunning = false;
    else memcpy(pGPSData, &Data[Pos], sizeof(GPSData));
    return true;
}

マルチスレッドで動いているため、厳密には GPSData Data[] のアクセスに排他制御を入れるべきであろう。 しかし、GPSMonotorスレッドは書き込み、主スレッドは読み込みだけであり、 仮に極めて稀に読み込んだデータに誤りがあったとしても、支障ない。 元々、GPSセンサから取得したデータには、ときどき、大きな誤りがあるし、衛星から受信できないこともしばしば ある。

少なくとも、1日使った段階では何も問題はなかった。


参考文献

[1] Windows 8で利用できる各種センサーをテストする
[2] Sensor API イベントの使用
[3] Windows の Sensor and Location プラットフォームの紹介
[4] [C++] Native Code (VC++) で Windows 7 Sensor API を使う
[5] プログラミング用メモ帳
[6] 位置情報とセンサーを使った魅力的なアプリの開発