/****************************************************************************/
/* 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.2 2000/02/08 00:01:05 pean Exp $
  *-----------------------------------------------------------------------*/

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

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


int main(int argc, char **argv)
{
     AHRSIF *ahrs;
     double roll, pitch, heading, 
	  rollRate, pitchRate, headingRate,
	  rollDeg, pitchDeg, headingDeg,
	  rollRateDeg, pitchRateDeg, headingRateDeg;
     long time;

     try {
	  ahrs = new AHRSIF("ahrs");
     } 
     catch(SharedObjectClient::MissingServer e) {
	  Syslog::write("ahrsTest.cc -- Couldn't find an AHRS IF server: %s", e.msg);
     }
     catch(...) {
	  Syslog::write("ahrsTest.cc -- Failed to instantiate AHRS IF");
     }


     while( !kbhit() )
     {
	  try {
	       ahrs->all( &roll, &pitch, &heading,
			  &rollRate, &pitchRate, &headingRate,
			  &time );
	  } 
	  catch(SharedObjectClient::MissingServer e) 
	  {
	       Syslog::write("ahrsTest.cc -- The AHRS server disappeared");
	  }


	  rollDeg        = Math::radToDeg( roll );
	  pitchDeg       = Math::radToDeg( pitch );
	  headingDeg     = Math::radToDeg( heading );
	  rollRateDeg    = Math::radToDeg( rollRate );
	  pitchRateDeg   = Math::radToDeg( pitchRate );
	  headingRateDeg = Math::radToDeg( headingRate );

	  printf("r = %6.3lf, dr = %6.3lf : p = %6.3lf, dp = %6.3lf : "
		 "h = %7.3lf, dh = %7.3lf\n",
		 rollDeg, rollRateDeg,
		 pitchDeg, pitchRateDeg,
		 headingDeg, headingRateDeg );

	  sleep(1);
     }

     delete ahrs;

     return 0;
}