/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : ahrsTest.cc                                                   */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
/*-----------------------------------------------------------------------*
  PROGRAM: kvhTest

  DESCRIPTION: Binary wrapped for Kvh instruments through the AHRS IF

  $Id: ahrsTest.cc,v 1.3 2001/06/02 21:33:39 hthomas Exp $
  *-----------------------------------------------------------------------*/

#include <stdio.h>
#include <string.h>
#include <conio.h>

#include "MathP.h"
#include "Syslog.h"
#include "Exception.h"
#include "AHRSIF.h"


int main(int argc, char **argv)
{
     AHRSIF *_ahrs;
     double pitch,roll,yaw;
     long time;

     try{
	  _ahrs = new AHRSIF( "ahrs" );
     } 
     catch(SharedObjectClient::MissingServer e)
     {
	  Syslog::write("ahrsTest.cc -- Couldn't attach to AHRS server");
     }
     catch(...)
     {
	  Syslog::write("ahrsTest.cc -- Some exception");  
     }


     while(!kbhit()) {
	  _ahrs->attitude( &roll, &pitch, &yaw, &time );

	  printf("r = %7.3lf : p = %7.3lf : y = %7.3lf\n",
		 Math::radToDeg( roll ),
		 Math::radToDeg( pitch ),
		 Math::radToDeg( yaw ) );

	  sleep(1);

     }

  return 0;
}