GPS受信機シールド > バイナリで読むスケッチ

#include <stdio.h>
#include <NewSoftSerial.h>
 
#define LED1 9
#define LED2 8
 
#define BAUD_RATE 9600
NewSoftSerial mySerial =  NewSoftSerial(2, 3); // Rx and Tx pins
 
void setup() 
{
  char buff[20];
  int i;
 
  // Initialize serial ports
  Serial.begin(BAUD_RATE);
  mySerial.begin(BAUD_RATE);
 
  // Initialize LEDs
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
 
  // Set LEDs off
  digitalWrite(LED1, HIGH);
  digitalWrite(LED2, HIGH);
 
/*
  // 再スタート要求コマンド
  delay(1000);
  buff[ 0] = 0xc1;
  buff[ 1] = 0x83;//オールクリアスタート
  buff[ 2] = 0x80;//予約
  buff[ 3] = 0x00;//パリティ
  for(i=0; i<=2; i++){ buff[3] ^= buff[i]; }
  buff[ 4] = 0xCA;//ターミネータ
  buff[ 5] = 0x00;
  mySerial.print(buff);
*/
 
  // 受信機パラメータ入力コマンド
  delay(1000);
  buff[ 0] = 0xc0;
  buff[ 1] = 0xa2;
  buff[ 2] = 0x86;//PDOP
  buff[ 3] = 0x86;//HDOP
  buff[ 4] = 0x85;//マスク仰角
  buff[ 5] = 0x81;//0xa8;//マスク信号レベル
  buff[ 6] = 0x80;//禁止衛星
  buff[ 7] = 0x80;//禁止衛星
  buff[ 8] = 0x80;//禁止衛星
  buff[ 9] = 0x80;//禁止衛星
  buff[10] = 0x80;//禁止衛星
  buff[11] = 0x82;//スムージング係数
  buff[12] = 0x82;//ダイナミック係数
  buff[13] = 0x80;//測地系番号
  buff[14] = 0x81;//測地系番号 WGS84
  buff[15] = 0x9E;//DGPS有効時間
  buff[16] = 0x80;
  buff[17] = 0x00;//パリティ
  for(i=0; i<=16; i++){ buff[17] ^= buff[i]; }
  buff[18] = 0xCA;
  buff[19] = 0x00;
  mySerial.print(buff);
 
 
  // 毎周期出力データ設定コマンド
  delay(1000);
  buff[0] = 0xc0;
  buff[1] = 0b10000011 | (0b01<<5);    // bit5:設定選択 bit6:確認要求
  buff[2] = 0x80 | 0b1111;             // bit0:位置データ, bit1:時刻データ, bit2:GPS 衛星情報, bit3:誤差指標情報
  buff[3] = buff[2]^(buff[1]^buff[0]); // パリティ
  buff[4] = 0xca;                      // ターミネータ
  buff[5] = 0x00;
  mySerial.print(buff);
} 
 
void loop() 
{ 
  gh81_read_task();
}
 
#define GH81_HEADER_POSITION 0xE0 // 23byte
#define GH81_HEADER_TIME     0xE1 // 10byte
#define GH81_HEADER_GPS      0xE2 // 76byte
#define GH81_HEADER_ERR      0xE3 // 15byte
 
#define GH81_LEN_POSITION 23
#define GH81_LEN_TIME     10
#define GH81_LEN_GPS      76
#define GH81_LEN_ERR      15
 
#define GH81_TERMINATOR_EA 0xEA
#define GH81_BUFF_SIZE 76
char gh81_buff[GH81_BUFF_SIZE];
int gh81_ofs = 0;
void gh81_read_task(void)
{
  int d;
  char c;
  while(1){
    d = mySerial.read();
    if(d==-1) return;
    c = (char)d;
    if(gh81_ofs==0){
      if((d==GH81_HEADER_POSITION)||(d==GH81_HEADER_TIME)||
         (d==GH81_HEADER_GPS)     ||(d==GH81_HEADER_ERR)){
        digitalWrite(LED1, LOW); // Turn LED1 on
        gh81_buff[0] = c;
        gh81_ofs = 1;
      }
    }else{
      gh81_buff[gh81_ofs] = c;
      gh81_ofs++;
      if(d==GH81_TERMINATOR_EA){ // header is E0-E3
        int l;
        switch(gh81_buff[0]){
        case GH81_HEADER_POSITION: l=GH81_LEN_POSITION; break;
        case GH81_HEADER_TIME:     l=GH81_LEN_TIME;     break;
        case GH81_HEADER_GPS:      l=GH81_LEN_GPS;      break;
        case GH81_HEADER_ERR:      l=GH81_LEN_ERR;      break;
        }
        if(l==gh81_ofs){
          digitalWrite(LED1, HIGH); // Turn LED1 off
          gh81_data_available();
          gh81_ofs = 0;
        }
      }
      if(GH81_BUFF_SIZE<=gh81_ofs){ // overflow!
        gh81_ofs = 0;
      }
    }
  }
}
 
void gh81_data_available(void)
{
  char buf[10];
  char* p;
  for(p=gh81_buff; p<gh81_buff+gh81_ofs; p++){
    sprintf(buf, "%02X ", (unsigned char)*p);
    Serial.print(buf);
  }
  Serial.println();
}
 
 
 

# ruby GH81_test.rb /dev/cu.usbserial-A9005bvI
 
def main(argv)
  f = open(argv[0])
  gh81 = GH81.new
  f.each do |line|
    begin
      gh81.line_recvd(line)
    rescue GH81::InvalidData => e
      $stderr.puts e.inspect
    end
  end
end
 
class GH81
  def line_recvd(line)
    #log "line=[#{line}]"
    bytes = [line.gsub(/\s/,"")].pack("H*")
    # "size=#{bytes.size}"
    case bytes[0]
    when 0xE0 # POSITION
      process_position_data(bytes)
    when 0xE1 # TIME
      process_time_data(bytes)
    when 0xE2 # GPS
      process_gps_data(bytes)
    when 0xE3 # ERR
      process_err_data(bytes)
    else
      raise InvalidData.new("unknown header.")
    end
  end
 
  def process_position_data(bytes)
    _check_length(bytes, 23)
    mode   = bytes[1]
    status = bytes[2]
    lat    = __parse_7bit_signed_value(bytes[ 3,4]) #緯度(0.0001分)
    lng    = __parse_7bit_signed_value(bytes[ 7,4]) #経度
    alt    = __parse_7bit_signed_value(bytes[11,4]) #高度(0.1m)
    vel    = __parse_7bit_signed_value(bytes[15,3]) #速度(0.1km/h)
    dir    = __parse_7bit_signed_value(bytes[18,2]) #方位
    nsat   = __parse_7bit_signed_value(bytes[20,1]) #測位使用衛星数
    log "position: mode=%02x status=%02x latlng=(%14.8f,%14.8f)[%10d,%10d] alt=%4.1f vel=%4.1f dir=%5d nsat=%2d" % [mode, status, lat/600000.0, lng/600000.0, lat, lng, alt/10.0, vel/10.0, dir, nsat]
  end
 
  def __parse_7bit_signed_value(bytes)
    value = 0
    bytes.unpack("C*").each do |b|
      value = (value<<7) | (b & 0x7F)
    end
    return value
  end
 
  # http://maps.google.co.jp/?ie=UTF8&ll=35.667495,139.792142&spn=0.008681,0.01487&z=16
  # 60分=1度 => 分=度/60
  # 分/10000 = 度/600000
  #?> GeoKit.default_units
  #=> :kms
  #>> a = GeoKit::LatLng.new(35.667495,139.792142); b=a.dup; b.lat+=(1/600000.0); a.distance_to(b)
  #=> 0.000185303166988816
  #>> a = GeoKit::LatLng.new(35.667495,139.792142); b=a.dup; b.lng+=(1/600000.0); a.distance_to(b)
  #=> 0.000125123986001518
  # lat/lngの最小単位は10-20cmぐらい？
 
  def process_time_data(bytes)
    _check_length(bytes, 10)
#    log "time    : status=%02x 20%02d/%02d/%02d %02d:%02d:%02d" % [bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7]]
  end
  def process_gps_data(bytes)
    _check_length(bytes, 76)
#    log "gps:"
  end
  def process_err_data(bytes)
    _check_length(bytes, 15)
#    log "err:"
  end
 
  def _check_length(bytes, len)
    raise InvalidData.new("invalid length. #{bytes.length}!=#{len}") if bytes.length!=len
  end
  class InvalidData < StandardError
  end
 
  def log(s)
    $stderr.puts(s)
  end
end
 
main(ARGV)