19 April 2008

GPS受信データで時刻同期

GPS受信データ(NMEAデータ)を用いて、システム時刻を同期させます。

屋外で利用するWindows CE機をターゲットにしていますが、通常のWindows機にも適用可能です。(CEではwcharのものを、WindowsではMBCSにすればOK)
組み込み用コンパイラ Microsoft eMbedded Visual C++ 4.0 でコンパイル可能なはずです。

別のところでGPLソフトウエアとして公開しているものの一部。主要処理のみ記載していますので、このままコンパイルしても動きませんよ…
シリアルポート通信部は こちら を参照。

ダイアログ宣言部

// CGpsTimeSetDlg ダイアログ
class CGpsTimeSetDlg : public CDialog
{
// コンストラクション
public:
CGpsTimeSetDlg(CWnd* pParent = NULL); // 標準コンストラクタ

// ダイアログ データ
enum { IDD = IDD_GPSTIMESET_DIALOG };

protected:
virtual void DoDataExchange(CDataExchange* pDX); // DDX/DDV サポート

// 実装
protected:
HICON m_hIcon;

// 生成された、メッセージ割り当て関数
virtual BOOL OnInitDialog();
afx_msg void OnPaint();
afx_msg HCURSOR OnQueryDragIcon();
DECLARE_MESSAGE_MAP()
public:
afx_msg void OnBnClickedBtnComopen();
afx_msg void OnBnClickedBtnComclose();
afx_msg void OnBnClickedOk();
afx_msg void OnBnClickedBtnSettime();

static UINT ThreadMain(LPVOID pParam);
UINT ThreadDataReceive(LPVOID pParam);

CWinThread * volatile m_pThread;
volatile int m_ThreadActivate;
volatile int m_ThreadTerminateCommand;
volatile int m_ThreadSetTime;
volatile int m_nBaudRate;
volatile char m_sPortName[7];

CComboBox m_ctrlComboPortname;
CComboBox m_ctrlComboBaud;

BOOL NmeaChecksum(const char * sNmea);

int GGA_Process(const char * sNmea, LPVOID pParam);
int ZDA_Process(const char * sNmea, LPVOID pParam);
void TimeSeparate(char * sTimeStr, int *nHour, int *nMinutes, int *nSecond);
char * SplitString(char *sBuf, char *sSeparator);
void PcTimeDisp(int nHour, int nMinutes, int nSecond, LPVOID pParam);
};

ダイアログ初期化時に、各種変数を初期化しておく

/********
OnInitDialog()
ダイアログ表示前の初期設定

内部変数の初期値設定を行う
ポート番号、通信速度ドロップダウンリストの初期設定を行う
********/

BOOL CGpsTimeSetDlg::OnInitDialog()
{
CDialog::OnInitDialog();

// このダイアログのアイコンを設定します。アプリケーションのメイン ウィンドウがダイアログでない場合、
// Framework は、この設定を自動的に行います。
SetIcon(m_hIcon, TRUE); // 大きいアイコンの設定
SetIcon(m_hIcon, FALSE); // 小さいアイコンの設定

// TODO: 初期化をここに追加します。


int i;
char sTemp[7];

// コンボボックス:COMポートの初期値を設定
for(i=1; i<20; i++)
{
sprintf(sTemp, "COM%d", i);
m_ctrlComboPortname.AddString(sTemp);
}

m_ctrlComboPortname.SetCurSel(3);

// コンボボックス:COMポート速度の初期値を設定
m_ctrlComboBaud.AddString(_T("1200"));
m_ctrlComboBaud.AddString(_T("2400"));
m_ctrlComboBaud.AddString(_T("4800"));
m_ctrlComboBaud.AddString(_T("9600"));
m_ctrlComboBaud.AddString(_T("14400"));
m_ctrlComboBaud.AddString(_T("19200"));
m_ctrlComboBaud.AddString(_T("38400"));
m_ctrlComboBaud.AddString(_T("57600"));
m_ctrlComboBaud.AddString(_T("115200"));

m_ctrlComboBaud.SetCurSel(6);


m_ThreadActivate = 0; // スレッド停止中
m_ThreadTerminateCommand = 0; // スレッド停止命令 無効

m_ThreadSetTime = 0; // 時刻設定コマンド 無効

m_pThread = NULL; // スレッドポインタ初期化

return TRUE; // フォーカスをコントロールに設定した場合を除き、TRUE を返します。
}

「GPS受信開始ボタン」を押した時の処理 → スレッド開始

/********
OnBnClickedBtnComopen()
受信開始ボタンを押した時の処理

受信スレッド(ThreadMain)を起動する
********/

void CGpsTimeSetDlg::OnBnClickedBtnComopen()
{
// TODO : ここにコントロール通知ハンドラ コードを追加します。

if(m_pThread)
{ // スレッドが既に存在する場合
SetDlgItemText(IDC_EDIT_TIME_PC, (LPCTSTR)"Thread Already Running");
}
else
{
// COMポート番号設定を、コンボボックスより読み取る
GetDlgItemText(IDC_COMBO_PORTNAME, (LPTSTR)m_sPortName, 6);

// Baud Rate設定を、コンボボックスより読み取る
m_nBaudRate = GetDlgItemInt(IDC_COMBO_BAUD);

// スレッド生成(停止状態で生成)
m_pThread = ::AfxBeginThread(ThreadMain, (LPVOID)this, 0, 0, CREATE_SUSPENDED, NULL);

m_pThread->m_bAutoDelete = FALSE; // スレッド自動破棄フラグクリア
m_pThread->ResumeThread(); // スレッド動作開始
}
}

「GPS受信停止ボタン」を押した時の処理 → スレッド停止

/********
OnBnClickedBtnComclose()
受信中止ボタンを押した時の処理

受信スレッド(ThreadMain)を停止する
(スレッド停止のためのフラグをセットし、スレッド停止を待つ)
********/

void CGpsTimeSetDlg::OnBnClickedBtnComclose()
{
// TODO : ここにコントロール通知ハンドラ コードを追加します。
// スレッドが起動していない場合
if(m_ThreadActivate == 0)
{
SetDlgItemText(IDC_EDIT_TIME_PC, (LPCTSTR)"Thread Not Running");
return;
}

//スレッドに終了フラグを送信する
m_ThreadTerminateCommand = 1;

// スレッドが終了するのを待つ
for(int i=0; i<50; i++)
{
if(m_ThreadActivate == 0) break;
::Sleep(100);
}

// スレッドが終了しているか再確認
if(::WaitForSingleObject(m_pThread->m_hThread, 100) == WAIT_TIMEOUT)
{ // スレッド終了が確認できず、タイムアウトした
::TerminateThread(m_pThread->m_hThread, 0xffffffff);
}

// m_bAutoDelete = FALSE の場合、スレッドのオブジェクトを手動で削除
delete m_pThread;
m_pThread = NULL;

SetDlgItemText(IDC_EDIT_TIME_PC, (LPCTSTR)"Thread Stop");

m_ThreadActivate = 0; // スレッド停止中
m_ThreadTerminateCommand = 0; // スレッド停止命令 無効

}

「時刻あわせボタン」を押した時の処理

/********
OnBnClickedBtnSettime()
時刻設定ボタンを押した時の処理

受信スレッド(ThreadMain)に対して、時刻設定フラグをセットする
********/

void CGpsTimeSetDlg::OnBnClickedBtnSettime()
{
// TODO : ここにコントロール通知ハンドラ コードを追加します。

m_ThreadSetTime = 1; // 時刻設定コマンド 割り込みON
}

受信スレッド(ループ)


/********
ThreadMain(LPVOID pParam)
スレッド関数

入力)
メインダイアログクラスへのポインタ:pParam

依存)
 実際の受信処理は、ThreadDataReceive() 関数を呼び出している

********/

UINT CGpsTimeSetDlg::ThreadMain(LPVOID pParam)
{
// メインダイアログクラスへのポインタ
CGpsTimeSetDlg *pDlg = (CGpsTimeSetDlg*)pParam;

return pDlg->ThreadDataReceive(pParam);
}


/********
ThreadDataReceive(LPVOID pParam)
データ受信ループ

入力)
メインダイアログクラスへのポインタ:pParam

内部使用しているクラス変数)
スレッド呼び出し側に、スレッド稼動中を知らせる変数:int m_ThreadActivate
スレッド呼び出し側から、停止命令を受けるフラグ:int m_ThreadTerminateCommand
CSerialComで受信したデータを受け取る:BYTE Com.m_LineBufferEx[]

依存)
 シリアル通信のため、CSerialComクラスを生成する

********/

UINT CGpsTimeSetDlg::ThreadDataReceive(LPVOID pParam)
{
CString sTemp = "";
char sPortName[10];
int nSize;

// メインダイアログクラスへのポインタ
CGpsTimeSetDlg *pDlg = (CGpsTimeSetDlg*)pParam;

pDlg->m_ThreadActivate = 1; // スレッドはアクティブ
pDlg->m_ThreadTerminateCommand = 0; // 停止命令をクリア

CSerialCom Com;

// シリアルポートを開く
// Windows の場合は Com.Open("\\\\.\\COM4", 9600);
// CE の場合は Com.Open(_T("COM4:"), 9600);
sprintf(sPortName, "\\\\.\\%s", m_sPortName);
if(!Com.Open(sPortName, m_nBaudRate))
{
pDlg->SetDlgItemText(IDC_EDIT_TIME_PC, "COM Open Error");
return 0;
}

// シリアル受信 ループ
for(;;)
{
nSize = Com.ReadLineEx();

// スレッド停止命令フラグを受け取った場合、ループを抜ける
if(pDlg->m_ThreadTerminateCommand) break;

if(nSize == 0) continue; // 切り出しデータがゼロの時、リトライ
if(nSize < 0) break; // エラーのとき、ループを抜ける

Com.m_LineBufferEx[nSize] = (BYTE)0; // 文字列末端のNULLを付加

// NMEAチェックサム合格の時、GGAとZDAの解析と表示処理を行う
if(NmeaChecksum((LPCSTR)Com.m_LineBufferEx))
{
GGA_Process((LPCSTR)Com.m_LineBufferEx, pParam);
ZDA_Process((LPCSTR)Com.m_LineBufferEx, pParam);
}
}

// シリアルポートを閉じる
Com.Close();

pDlg->m_ThreadActivate = 0; // スレッドは停止
pDlg->m_ThreadTerminateCommand = 0; // 停止命令をクリア

return 0;

}

GGA, ZDA構文解析

/********
GGA_Process(const char * sNmea, LPVOID pParam)
GGA NMEAセンテンスを解析し、ダイアログに表示する。

入力)
 解析するNMEAセンテンス:const char * sNmea
ダイアログクラスへのポインタ:LPVOID pParam

依存)
 NMEAセンテンスの','で切り分けるため利用:SplitString()
時刻文字列を解析するため利用:TimeSeparate()
PCのシステム時間を表示するため利用:PcTimeDisp()

戻り値)
常に:0

参考)
NMEAセンテンスの例 (GGA - Global Positioning System Fix Data)
$GPGGA,031916.000,3512.3456,N,13512.3456,E,1,04,2.6,514.7,M,34.3,M,,0000*5C

$GPGGA,[時分秒],[緯度],[N/S],[経度],[W/E],[モード],[補足衛星数],[HDOP],[高度],[単位系],[ジオイド補正],[単位系],[DGPSエイジ],[DGPS基地コード]*[チェックサム]

参照:http://gpsd.berlios.de/NMEA.txt
********/

int CGpsTimeSetDlg::GGA_Process(const char * sNmea, LPVOID pParam)
{
char *sSeparator = ",*";
char *sToken;
CString sTemp;
int nYear=0, nMonth=0, nDay=0, nHour=0, nMinutes=0, nSecond=0;
char sSplitBuffer[COM_BUFFERSIZE+1];

CGpsTimeSetDlg *pDlg = (CGpsTimeSetDlg*)pParam;

::strcpy(sSplitBuffer, sNmea);
sToken = SplitString(sSplitBuffer, sSeparator);

if(::stricmp(sSplitBuffer, "$GPGGA")) return -1;

// 時刻フィールド
sToken = SplitString(NULL, sSeparator);
if(sToken == NULL)
{
pDlg->SetDlgItemText(IDC_EDIT_TIME, "----");
}
else
{
TimeSeparate(sToken, &nHour, &nMinutes, &nSecond);
sTemp.Format("%02d:%02d:%02d", nHour, nMinutes, nSecond);
pDlg->SetDlgItemText(IDC_EDIT_TIME, sTemp);

PcTimeDisp(nHour, nMinutes, nSecond, pParam);
}

// 緯度フィールド
sTemp = "";
sToken = SplitString(NULL, sSeparator);
if(sToken != NULL) sTemp = sToken;
sToken = SplitString(NULL, sSeparator);
if(sToken != NULL) sTemp = sTemp + " " + sToken;
if(sTemp == "") sTemp = "----";
pDlg->SetDlgItemText(IDC_EDIT_LATITUDE, sTemp);

// 経度フィールド
sTemp = "";
sToken = SplitString(NULL, sSeparator);
if(sToken != NULL) sTemp = sToken;
sToken = SplitString(NULL, sSeparator);
if(sToken != NULL) sTemp = sTemp + " " + sToken;
if(sTemp == "") sTemp = "----";
pDlg->SetDlgItemText(IDC_EDIT_LONGITUDE, sTemp);

// 受信FIXモード
sToken = SplitString(NULL, sSeparator);

// 衛星数フィールド
sToken = SplitString(NULL, sSeparator);
if(sToken == NULL) pDlg->SetDlgItemText(IDC_EDIT_SAT, "----");
else pDlg->SetDlgItemText(IDC_EDIT_SAT, sToken);

// HDOPフィールド
sToken = SplitString(NULL, sSeparator);
if(sToken == NULL) pDlg->SetDlgItemText(IDC_EDIT_HDOP, "----");
else pDlg->SetDlgItemText(IDC_EDIT_HDOP, sToken);

// 高度フィールド
sToken = SplitString(NULL, sSeparator);
if(sToken == NULL) pDlg->SetDlgItemText(IDC_EDIT_ALTITUDE, "----");
else pDlg->SetDlgItemText(IDC_EDIT_ALTITUDE, sToken);

// 単位系モード
sToken = SplitString(NULL, sSeparator);

// ジオイド補正フィールド
sToken = SplitString(NULL, sSeparator);
if(sToken == NULL) pDlg->SetDlgItemText(IDC_EDIT_GEOID, "----");
else pDlg->SetDlgItemText(IDC_EDIT_GEOID, sToken);

return 0;
}


/********
ZDA_Process(const char * sNmea, LPVOID pParam)
ZDA NMEAセンテンスを解析し、ダイアログに表示する。

入力)
 解析するNMEAセンテンス:const char * sNmea
ダイアログクラスへのポインタ:LPVOID pParam

依存)
 NMEAセンテンスの','で切り分けるため利用:SplitString()
時刻文字列を解析するため利用:TimeSeparate()
PCのシステム時間を表示するため利用:PcTimeDisp()

戻り値)
常に:0

参考)
NMEAセンテンスの例 (ZDA - Time & Date - UTC, day, month, year and local time zone)
$GPZDA,031921.000,19,04,2008,,*58

$GPZDA,[時分秒],[日],[月],[年],[+-TZ hour],[TZ minutes]*[チェックサム]

参照:http://gpsd.berlios.de/NMEA.txt
********/

int CGpsTimeSetDlg::ZDA_Process(const char * sNmea, LPVOID pParam)
{
char *sSeparator = ",*";
char *sToken;
CString sTemp;
int nYear=0, nMonth=0, nDay=0, nHour=0, nMinutes=0, nSecond=0;
char sSplitBuffer[COM_BUFFERSIZE+1];


CGpsTimeSetDlg *pDlg = (CGpsTimeSetDlg*)pParam;

::strcpy(sSplitBuffer, sNmea);
sToken = SplitString(sSplitBuffer, sSeparator);

if(::stricmp(sToken, "$GPZDA")) return -1;

sTemp = "";

// 時刻
sToken = SplitString(NULL, sSeparator);
if(sToken != NULL)
TimeSeparate(sToken, &nHour, &nMinutes, &nSecond);

// 日
sToken = SplitString(NULL, sSeparator);
if(sToken != NULL) nDay = ::atoi(sToken);
// 月
sToken = SplitString(NULL, sSeparator);
if(sToken != NULL) nMonth = ::atoi(sToken);
// 年
sToken = SplitString(NULL, sSeparator);
if(sToken != NULL) nYear = ::atoi(sToken);

sTemp.Format("%04d/%02d/%02d %02d:%02d:%02d", nYear, nMonth, nDay, nHour, nMinutes, nSecond);
pDlg->SetDlgItemText(IDC_EDIT_TIME_ZDA, sTemp);

PcTimeDisp(nHour, nMinutes, nSecond, pParam);

return 0;
}

システム時間の表示と修正

/********
PcTimeDisp(int nHour, int nMinutes, int nSecond, LPVOID pParam)
PCのシステム時刻の表示と、システム時刻の変更

入力)
GPSで得た時分秒:int nHour, int nMinutes, int nSecond
メインダイアログクラスへのポインタ:pParam

内部使用しているクラス変数)
スレッド呼び出し側から、時刻設定命令を受けるフラグ:int m_ThreadSetTime

依存)
 Win32SDK SetLocalTime は システム時刻変更権限が無い場合、何も処理しない

********/

void CGpsTimeSetDlg::PcTimeDisp(int nHour, int nMinutes, int nSecond, LPVOID pParam)
{
SYSTEMTIME tmSystem;
TIME_ZONE_INFORMATION tzInfo;
DWORD nTzResult;
double nTz;
long int nTimeNow;
CString sTemp;

CGpsTimeSetDlg *pDlg = (CGpsTimeSetDlg*)pParam;

::GetLocalTime(&tmSystem); // システムのローカルタイムを得る
nTzResult = ::GetTimeZoneInformation(&tzInfo); // システムのタイムゾーン設定を得る

// システムのローカルタイムをダイアログに表示する
sTemp.Format("%04d/%02d/%02d %02d:%02d:%02d", tmSystem.wYear, tmSystem.wMonth, tmSystem.wDay, tmSystem.wHour, tmSystem.wMinute, tmSystem.wSecond);
pDlg->SetDlgItemText(IDC_EDIT_TIME_PC, sTemp);

// システムのタイムゾーンをダイアログに表示する
nTz = (double)tzInfo.Bias / 60.0;
sTemp.Format("%+02.1f", nTz);
pDlg->SetDlgItemText(IDC_EDIT_TIME_TZ, sTemp);

// 時刻設定
if(pDlg->m_ThreadSetTime)
{
// GPS時刻にタイムゾーンを加算して、ローカルタイムにする
nTimeNow = nHour*60*60 + nMinutes*60 + nSecond - tzInfo.Bias*60;

// 0 ~ 24 時間内に収める処理(ムチャクチャ適当)
if(nTimeNow < 0) nTimeNow += 60*60*12; // マイナスになったら、12時間進める
else if(nTimeNow > 24*60*60) nTimeNow -= 60*60*12; // 24時間以上は、12時間戻す

// GPS時刻から得たローカルタイムを、年月日・時分秒変数に代入
tmSystem.wHour = (WORD)(nTimeNow / 60 / 60);
tmSystem.wMinute = (WORD)((nTimeNow - tmSystem.wHour*60*60) / 60);
tmSystem.wSecond = (WORD)(nTimeNow - tmSystem.wHour*60*60 - tmSystem.wMinute*60);

// システムのローカルタイムを変更する(要:時刻変更権限)
::SetLocalTime(&tmSystem);

pDlg->m_ThreadSetTime = 0; // 時刻設定命令フラグをクリア
}
}

雑処理

/********
NmeaChecksum(const char * sNmea)
NMEAセンテンスのチェックサムを検査する

入力)
 解析するNMEAセンテンス:const char * sNmea

戻り値)
チェックサム一致:1
チェックサム不一致:0
********/

BOOL CGpsTimeSetDlg::NmeaChecksum(const char * sNmea)
{
char cksm = 0;
char sCksm[5];

// 文字列の先頭から、一文字ずつスキャンしてチェックサム計算
for(unsigned int i=0; i<strlen(sNmea); i++)
{
// '$' と '!' は計算しない(読み飛ばす)
if(sNmea[i] == '$' || sNmea[i] == '!') continue;

// '*' が検出されたら終了
if(sNmea[i] == '*') break;

cksm = cksm ^ sNmea[i];
}

if(i >= strlen(sNmea)) return 0; // チェックサム デリミタ "*" が無かった

sprintf(sCksm, "%02X\r\n", cksm);
if(!stricmp(sNmea+i+1, sCksm))
return 1; // チェックサムが一致した場合

return 0; // チェックサム不一致
}

/********
TimeSeparate(char * sTimeStr, int *nHour, int *nMinutes, int *nSecond)
時刻(時分秒)文字列を切り分け、変数に代入する関数

入力)
 解析する時分秒センテンス:char * sTimeStr
切り分けた時分秒を格納する変数:int *nHour, int *nMinutes, int *nSecond

※ sTimeStrは破壊されない(strtok利用前に、作業用文字列にコピー)

********/

void CGpsTimeSetDlg::TimeSeparate(char * sTimeStr, int *nHour, int *nMinutes, int *nSecond)
{
char *sSeparator = ".";
char *sToken;
char sTemp[3];
char sSplitBuffer[COM_BUFFERSIZE+1];

*nHour = 0;
*nMinutes = 0;
*nSecond = 0;

::strcpy(sSplitBuffer, sTimeStr); // 破壊を避けるため、作業用文字列にコピー

sToken = ::strtok(sSplitBuffer, sSeparator); // '.' で切り分け

// 時分秒文字列は、6文字であることを確認
if(::strlen(sToken) != 6) return;

sTemp[2] = (char)0; // 時・分・秒は2文字ずつのため、3文字目にNULLをセット

// 時
sTemp[0] = sToken[0];
sTemp[1] = sToken[1];
*nHour = ::atoi(sTemp);

// 分
sTemp[0] = sToken[2];
sTemp[1] = sToken[3];
*nMinutes = ::atoi(sTemp);

// 秒
sTemp[0] = sToken[4];
sTemp[1] = sToken[5];
*nSecond = ::atoi(sTemp);

return;
}


/********
SplitString(char *sBuf, char *sSeparator)
文字列を、セパレータで切り分ける

※ strtokはセパレータが連続した場合読み飛ばすが、この関数はNULL文字列を返す。
例:入力文字列が "A,,,B" で ','を用いて切り出す場合
strtok -> "A","B"
SplitString -> "A","","","B"

入力)
 解析する文字列:char *sBuf
前回の結果から、連続して切り出す場合NULLをセット(strtok仕様と同じ)
セパレータ:char *sSeparator

※ sBufは破壊される(切り出しでNULLが順次挿入される)

********/

char * CGpsTimeSetDlg::SplitString(char *sBuf, char *sSeparator)
{
static int nEndPt=0;
static int nStartPt=0;
static int nStrlen=0;
static char *sBufCur=NULL;

// 初回
if(sBuf != NULL)
{
nStrlen = (int)::strlen(sBuf);
nEndPt = 0;
sBufCur = sBuf;
}
// 連続切り出し
else
{
sBufCur += nEndPt + 1;
}

// 文字列末端のとき
if(sBufCur[0] == (char)NULL) return NULL;

nEndPt = (int)::strcspn(sBufCur, sSeparator);

// セパレータが見つからない時は、文字列末端までの長さが返される

// セパレータをNULLに置換
if(nEndPt >= 0)
{
sBufCur[nEndPt] = (char)NULL;
}

nEndPt = (int)::strlen(sBufCur);

return sBufCur;

}