#include "Ahrs.h"
#include "Syslog.h"

Ahrs::Ahrs(const char *name, NavSensors *sensors, int maxBad)
  : NavSensor(name, sensors, maxBad)
{
  _ahrsIF = 0;
  _magneticCompass = False;

  _attitude.roll = _attitude.pitch = _attitude.yaw = 
    _attitude.rollRate = _attitude.pitchRate = _attitude.yawRate = 0.;
}



TaskInterface *Ahrs::createTaskIF(int timeout)
{
  try {
    Syslog::write("ahrs if is %s\n", name());
    _ahrsIF = new AhrsIF(name(), timeout);
    _magneticCompass = _ahrsIF->magneticCompass();
  }
  catch (...) {
    _ahrsIF = 0;
  }

  return _ahrsIF;
}


DeviceIF::Status Ahrs::readTaskIF(Boolean *valid, 
				  TimeIF::TimeSpec *sampleTime)
{
  *valid = True;

  return _ahrsIF->getAttitude(&_attitude, sampleTime);
}



void Ahrs::attitude(NavigationIF::Attitude *navAttitude)
{
  navAttitude->roll = _attitude.roll;
  navAttitude->omega_B_x = _attitude.rollRate;
  navAttitude->pitch = _attitude.pitch;
  navAttitude->omega_B_y = _attitude.pitchRate;
  navAttitude->yaw = _attitude.yaw;
  navAttitude->omega_B_z = _attitude.yawRate;
}


Boolean Ahrs::magneticCompass()
{
  return _magneticCompass;
}