#include "Dvl.h"
#include "MathP.h"

Dvl::Dvl(const char *name, NavSensors *sensors, int maxBad)
  : NavSensor(name, sensors, maxBad)
{
  _DvlIF = 0;
}



TaskInterface *Dvl::createTaskIF(int timeout)
{
  try {
    _DvlIF = new DvlIF(name(), timeout);
  }
  catch (...) {
    _DvlIF = 0;
  }

  return _DvlIF;
}
//
// This is called once each control sample period by Navigation::readSensors().
//
DeviceIF::Status Dvl::readTaskIF(Boolean *valid, 
				 TimeIF::TimeSpec *sampleTime)
{
  DeviceIF::Status status = _DvlIF->get(&data, &m_newData);

  *valid = True;
  if( status != OK ) *valid = False;

  sampleTime->seconds = data.sampleTime.seconds;
  sampleTime->nanoSeconds = data.sampleTime.nanoSeconds;

  // Determine sensor status and return

  return status;
}
//
// Return a True if there is new data:
//
Boolean Dvl::newData()
{
  if(m_newData) return True;
  return False;
}
//
// Compute the altitude:
//
double Dvl::altitude(NavigationIF::Attitude *attitude)
{
  //
  // The AHRS roll and pitch are space-fixed, not Euler, angles.
  //
  return( data.range * cos(attitude->roll) * cos(attitude->pitch) );
}