#!/bin/sh
# -*-Tcl-*- \
exec tclsh "$0" ${1+"$@"}
#
# Example startup script for gps-pocolibs on the gps-base RTK reference
#

set prefix	"/opt/openrobots"
set exec_prefix	"${prefix}"
set libdir	"${exec_prefix}/lib"
lappend auto_path ${libdir}

set h2		${exec_prefix}/bin/h2
set gps		${exec_prefix}/bin/gps-pocolibs
set gpsclient	${libdir}/genom/pocolibs/plugins/gps

set default_tty		/dev/ttyS1
set default_baud	115200
set default_fix		{
  llh {
    latitude	43.56221677048
    longitude	1.47730192335
    height	206.232826165
  }
  sigma {
    north	0.1165
    east	0.0926
    up		0.2156
  }
}

if {$argc == 0} {
  set argv [list $default_tty $default_baud]
  set argc [llength $argv]
}
if {$argc != 2} {
  puts stderr "usage: [lindex [file split $argv0] end] tty baud"
  exit 2
}
lassign $argv tty baud

proc configure {} {
  global tty baud default_fix

  # connect device
  gps::connect [list serial $tty baud $baud]

  # set LAAS fix
  gps::fix $default_fix

  # stream RTCM
  gps::set_rtk_port 8083

  # base station coordinates @30s
  gps::send_rtcm 1005 30 0

  # observations: extended L1/L2 GPS & GLONASS @3s + 1s
  gps::send_rtcm 1004 3 1
  gps::send_rtcm 1012 3 1

  # MSM7: code, phase, carrier-to-noise and doppler for GPS & GLONASS @2Hz
  gps::send_rtcm 1077 0.5 0
  gps::send_rtcm 1087 0.5 0

  # for older devices
  # MSM4: code, phase, carrier-to-noise for GPS @2s
  gps::send_rtcm 1074 2 0
}


# --- main -----------------------------------------------------------------
#
try {
  package require genomix
  set g [genomix::localhost]

  exec $h2 init >@stdout 2>@stderr << n ;# prevent overwriting existing h2 devs

  exec $gps -i gps-base -b >@stdout 2>@stderr
  $g load $gpsclient -i gps-base as gps

  configure

} on error e {
  catch { exec $h2 end }
  puts stderr $e
  exit 2
}
