/***************************************************************************/
/* Copyright 1995 MBARI                                                    */
/***************************************************************************/
/* Summary  :                                                              */
/* Filename :                                                              */
/* Author   : Michael B. Matthews                                          */
/* Project  : Tiburon ROV                                                  */
/* Version  : Version 1.0                                                  */
/* Created  : February 12, 1996                                            */
/* Modified :                                                              */
/* Archived :                                                              */
/***************************************************************************/

#include <vxWorks.h>                    /* VxWorks systems declarations      */
#include <stdioLib.h>                   /* VxWorks standard I/O library      */
#include <math.h>                       /* VxWorks floating point math libs  */
#include <semLib.h>                     /* VxWorks semaphore library         */
#include <wdLib.h>                      /* VxWorks watchdog timer library    */
#include <systime.h>                    /* VxWorks system time declarations  */
#include <strLib.h>                     /* VxWorks string library            */
#include <stdlib.h>

#include <mbariTypes.h>                 /* MBARI type declarations           */
#include <trig.h>                       /* MBARI trigonometric declarations  */
#include <usrTime.h>                    /* MBARI time declarations           */
#include <kalmanDM.h>                   /* kalman data manager definitions   */
#include <sensorDcon.h>                 /* sensor data concentrator definitns*/
#include <vmeIbc.h>                     /* VME IBC definitions               */
#include <sensorsDm.h>                  /*                                   */
#include <dopplerDM.h>                  /*                                   */
#include <datamgr.h>                    /* data manager declarations         */
#include <dm_errno.h>                   /* data manager error declarations   */
#include <rovPriority.h>                /* task priority definitions         */
#include "fppLib.h"                     /* vxWorks vxTas routine definition  */

#include "mv162.h"
#include "arch/mc68k/ivMc68k.h"
#include "drv/vme/vmechip2.h"

#include "motionpak.h"
#include "kalman.h"

#define DM_ITEM_NAME_LEN   60           /* data manager item name length     */

/* updated state vector */
static Flt32 xh[4] = { 0, 0, 0, 0 };

/* propagated state vector */
static Flt32 xb[4] = { 0, 0, 0, 0 };

/* state transition matrix - computed from MATLAB file hf_gain.m */
static Flt32 F[4][4] = {
{1.00000000000000,   0.01998001332667,                  0,                  0},
{               0,   0.99800199866733,                  0,                  0},
{0.01980132669324,   0.00019854070381,   0.98019867330676,                  0},
{               0,                  0,                  0,   0.99999999998000}};

/* Kalman gain matrix - computed from MATLAB file hf_gain.m */
static Flt32 K[4][2] = {
{0.00021803643205,   0.14541039372563},
{0.00001005633277,   0.57012040195761},
{0.00020880266997,   0.01805758740424},
{-0.00019997884465,  0.00001292653258}};

#define COMPASS_VARIATION (Flt32) (15.05) /* magnetic deviation from chart   */
                                          /* value for calendar year 1996    */

/*****************************************************************************/
/* Function : initHeadingFilter                                              */
/* Purpose  :                                                                */
/* Inputs   :                                                                */
/* Outputs  :                                                                */
/*****************************************************************************/

STATUS initHeadingFilter(Heading_State* x0)
{

  /* define state vector and load initial values
     xh[0]   psi        heading
     xh[1]   r  heading rate
     xh[2]   xi compass state
     xh[3]   d          drift           */

  xh[0] = (Flt32) x0->psi;
  xh[1] = (Flt32) x0->r;
  xh[2] = (Flt32) x0->xi;
  xh[3] = (Flt32) x0->d;

  return OK;
}


/*****************************************************************************/
/* Function : stateSwitch                                                    */
/* Purpose  :                                                                */
/* Inputs   :                                                                */
/* Outputs  :                                                                */
/*****************************************************************************/

STATUS stateSwitch()
{
  Flt64 time = 0, dtime = 0;
  DM_Item kalmanStateDmi;
  kalmanDMItems kalmanDmi;
  DM_Group sensorGroup;
  sensorBits sensorId;
  SEM_ID  wakeupSem;                    /* watch dog timer wake up    */

  Heading_State headingState = { 0, 0, 0, 0 };

  Measurements measurements = {
    FALSE, NAN, NAN, 0,  /* gyro, gyro rate, and offset  */
    FALSE, NAN,          /* compass      */
    FALSE, NAN,          /* pitch sensor */
    FALSE, NAN,          /* roll sensor  */
    FALSE, NAN,          /* depth sensor */
    FALSE, FALSE, FALSE, /* ADV Flags    */
    NAN, NAN, NAN        /* ADV sensor   */
  };

  Position position = {
    NAN, NAN, NAN,      /* position     */
    NAN, NAN, NAN       /* velocity     */
  };

  PositionStatus positionStatus = {
    INVALID, INVALID, INVALID,  /* position status      */
    INVALID, INVALID, INVALID   /* velocity status      */
  };

  Attitude attitude = {
    NAN, NAN, NAN,      /* attitude             */
    NAN, NAN, NAN       /* attitude rate        */
  };

  AttitudeStatus attitudeStatus = {
    INVALID, INVALID, INVALID,  /* attitude status      */
    INVALID, INVALID, INVALID   /* attitude rate status */
  };

  State state = {
    FILTER_OFF,         /* filter status                */

    NAN, NAN, NAN,      /* position                     */
    NAN, NAN, NAN,      /* velocity                     */
    NAN, NAN, NAN,      /* acceleration                 */
    NAN, NAN, NAN,      /* attitude                     */
    NAN, NAN, NAN,      /* attitude rate                */
    NAN, NAN, NAN,      /* Euler rate                   */

    NAN, NAN, NAN,      /* position variance            */
    NAN, NAN, NAN,      /* velocity variance            */
    NAN, NAN, NAN,      /* acceleration variance        */
    NAN, NAN, NAN,      /* attitude variance            */
    NAN, NAN, NAN,      /* attitude rate variance       */
    NAN, NAN, NAN,      /* Euler rate variance          */

    INVALID, INVALID, INVALID,  /* position status      */
    INVALID, INVALID, INVALID,  /* velocity status      */
    INVALID, INVALID, INVALID,  /* acceleration status  */
    INVALID, INVALID, INVALID,  /* attitude status      */
    INVALID, INVALID, INVALID,  /* attitude rate status */
    INVALID, INVALID, INVALID,  /* Euler rate status    */

    SENSOR_INVALID,                     /* compass status               */
    SENSOR_INVALID, SENSOR_INVALID,     /* gyro, gyro rate status       */
    SENSOR_INVALID,                     /* MotionPak status             */
    SENSOR_INVALID, SENSOR_INVALID,     /* pitch/roll sensor status     */
    SENSOR_INVALID,                     /* ADV status                   */
    SENSOR_INVALID, SENSOR_INVALID      /* USBL/LBL status              */
  };

  initState(&kalmanStateDmi, &kalmanDmi, &sensorGroup, &sensorId);

  initHeadingFilter(&headingState);

  taskPrioritySet(taskIdSelf(), KALMAN_STATE_TASK_PRIORITY);

  /* create periodic control loop timing semaphore */
  if ((wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL){
    logMsg("stateSwitch: Error initializing semaphore wakeupSem\n");
    return;
  }

  FOREVER {

    /* wakeup at 20Hz rate */
    semTake(wakeupSem, sysClkRateGet() / 20);

    /* read Kalman filter struct DM if available, else use heading filter */
    if (readKalmanFilter(kalmanStateDmi, &state) == OK &&
        state.filterStatus == FILTER_NORMAL){
      position.sx = state.sx;
      position.sy = state.sy;
      position.sz = state.sz;
      positionStatus.sxStatus = state.sxStatus;
      positionStatus.syStatus = state.syStatus;
      positionStatus.szStatus = state.szStatus;

      position.vx = state.vx;
      position.vy = state.vy;
      position.vz = state.vz;
      positionStatus.vxStatus = state.vxStatus;
      positionStatus.vyStatus = state.vyStatus;
      positionStatus.vzStatus = state.vzStatus;

      attitude.heading = state.heading;
      attitude.pitch = state.pitch;
      attitude.roll = state.roll;
      attitude.headingRate = state.headingRate;
      attitude.pitchRate = state.pitchRate;
      attitude.rollRate = state.rollRate;

      attitudeStatus.headingStatus = state.headingStatus;
      attitudeStatus.pitchStatus = state.pitchStatus;
      attitudeStatus.rollStatus = state.rollStatus;
      attitudeStatus.headingRateStatus = state.headingRateStatus;
      attitudeStatus.pitchRateStatus = state.pitchRateStatus;
      attitudeStatus.rollRateStatus = state.rollRateStatus;
    }
    else{

      /* read in compass, gyro, pitch/roll sensor, and ADV data */
      getMeasurements(&kalmanDmi, sensorGroup,
                      &sensorId, &measurements);

      /* no positon data available - to change when integrated with USBL */
      position.sx = NAN;
      position.sx = NAN;
      position.sz = NAN;
      positionStatus.sxStatus = INVALID;
      positionStatus.syStatus = INVALID;
      positionStatus.szStatus = INVALID;

      if (measurements.depthSensorFlag == TRUE){
        position.sz = measurements.depthSensor;
        positionStatus.szStatus = DEPTH_SENSOR;
      }

      /* use direct ADV measurements for velocity if available */
      if (measurements.velocityADVFlag[0] == TRUE){
        position.vx = measurements.velocityADV[0];
        positionStatus.vxStatus = ADV;
      }
      else{
        position.vx = NAN;
        positionStatus.vxStatus = INVALID;
      }
      if (measurements.velocityADVFlag[1] == TRUE){
        position.vy = measurements.velocityADV[1];
        positionStatus.vyStatus = ADV;
      }
      else{
        position.vy = NAN;
        positionStatus.vyStatus = INVALID;
      }
      if (measurements.velocityADVFlag[2] == TRUE){
        position.vz = measurements.velocityADV[2];
        positionStatus.vzStatus = ADV;
      }
      else{
        position.vz = NAN;
        positionStatus.vzStatus = INVALID;
      }

      /* run simple heading filter on gyro and compass data */
      if (headingFilter(&measurements, &headingState) == OK){
        attitude.heading = headingState.psi;
        attitudeStatus.headingStatus = GYRO_COMPASS_FILTERED;
      }
      else{
        if (measurements.compassFlag == TRUE)
          attitude.heading = measurements.compass;
        attitudeStatus.headingStatus = COMPASS;
      }

      if (measurements.pitchSensorFlag == TRUE)
        attitude.pitch = measurements.pitchSensor;
      attitudeStatus.pitchStatus = PITCH_SENSOR;

      if (measurements.rollSensorFlag == TRUE)
        attitude.roll = measurements.rollSensor;
      attitudeStatus.rollStatus = ROLL_SENSOR;

      if (measurements.gyroFlag == TRUE)
        attitude.headingRate = measurements.gyroRate;
      attitudeStatus.headingRateStatus = GYRO_RATE;

      attitude.pitchRate = NAN;
      attitude.rollRate = NAN;
      attitudeStatus.pitchRateStatus = INVALID;
      attitudeStatus.rollRateStatus = INVALID;
    }

    /* send everything out */
    outputState(&kalmanDmi,
                &position, &positionStatus,
                &attitude, &attitudeStatus);

  } /* FOREVER */

  return(OK);
}


/*****************************************************************************/
/* Function : headingFilter                                                  */
/* Purpose  :                                                                */
/* Inputs   :                                                                */
/* Outputs  :                                                                */
/*****************************************************************************/

STATUS headingFilter(Measurements* zk, Heading_State* x)
{
  Reg Int16 i,j;

  if ((zk->compassFlag == FALSE) || (zk->gyroFlag == FALSE))
    return(ERROR);

  /* compute state update xb(k) = F * xh(k-1) */
  for (i = 0; i < 4; i++){
    xb[i] = 0;
    for (j = 0; j < 4; j++)
      xb[i] += F[i][j] * xh[j];
  }

  /* compute state update xh(k) = xb(k) + K * ( z(k) - H * xb(k) ) */
  for (i = 0; i < 4; i++){
    xh[i] = xb[i] + K[i][0] * zk->compass - xb[2]
                  + K[i][1] * zk->gyro -  xb[0] -  xb[3] - zk->gyroOffset;
  }

  x->psi = xh[0];
  x->r = xh[1];
  x->xi = xh[2];
  x->d = xh[3];

  return(OK);
}


/*****************************************************************************/
/* Function : readKalmanFilter                                               */
/* Purpose  :                                                                */
/* Inputs   :                                                                */
/* Outputs  :                                                                */
/*****************************************************************************/

STATUS readKalmanFilter(DM_Item kalmanStateDmi, State* state)
{
  dm_read(kalmanStateDmi, (Void *) state, sizeof(State), (DM_Time * ) NULL);
  return(OK);
}


/*****************************************************************************/
/* Function : getMeasurements                                                */
/* Purpose  : Reads compass, gyro, pitch, roll DVL, and USBL DM data         */
/* Inputs   :                                                                */
/* Outputs  :                                                                */
/*****************************************************************************/

STATUS getMeasurements(kalmanDMItems* kalmanDmi,
                       DM_Group sensorGroup,
                       sensorBits* sensorId,
                       Measurements* z)
{
  static MBool gyroValid = FALSE;
  DWord sensorGroupBits;
  MBool compassDataValid;
  MBool gyroDataValid;
  MBool velocityDataValid;
  Flt32 compassOffset;

  /* default to data marker and false flags */
  z->compassFlag = FALSE;
  z->gyroFlag = FALSE;
  z->pitchSensorFlag = FALSE;
  z->rollSensorFlag = FALSE;
  z->depthSensorFlag = FALSE;
  z->velocityADVFlag[0] = FALSE;
  z->velocityADVFlag[1] = FALSE;
  z->velocityADVFlag[2] = FALSE;

  /* check for sensor value change */
  sensorGroupBits = dm_get_group_changes(sensorGroup);

  /* update compass heading */
  if (sensorGroupBits & sensorId->compassBit){
    dm_read(kalmanDmi->compassDataValid, (Void *) &compassDataValid,
            sizeof(compassDataValid), (DM_Time * ) NULL);
    if (compassDataValid){
      dm_read(kalmanDmi->compassRadians, (Void *) &z->compass,
              sizeof(z->compass), (DM_Time * ) NULL);

      dm_read(kalmanDmi->compassOffset, (Void *) &compassOffset,
              sizeof(compassOffset), (DM_Time * ) NULL);

      compassOffset = DEGS_TO_RADS(compassOffset);
      z->compass = z->compass + compassOffset;
      if (z->compass > M_PI2)
          z->compass -= M_PI2;
      else if (z->compass < 0)
          z->compass += M_PI2;

      z->compassFlag = TRUE;
    }
  }

  /* update gyro heading and heading rate */
  if (sensorGroupBits & sensorId->gyroBit){
    dm_read(kalmanDmi->gyroDataValid, (Void *) &gyroDataValid,
            sizeof(gyroDataValid), (DM_Time * ) NULL);

    gyroDataValid = FALSE;          /* pean 10/17/96 delete gyro */

    if (gyroDataValid){
      dm_read(kalmanDmi->gyroRadians, (Void *) &z->gyro,
              sizeof(z->gyro), (DM_Time * ) NULL);
      dm_read(kalmanDmi->gyroRateRadians, (Void *) &z->gyroRate,
              sizeof(z->gyroRate), (DM_Time * ) NULL);
      z->gyroFlag = TRUE;
      if (gyroValid == FALSE){
        z->gyroOffset = z->gyro - z->compass;
        gyroValid = TRUE;
      }
    }
    else
      gyroValid = FALSE;
  }

  /* update pitch measurement */
  if (sensorGroupBits & sensorId->pitchBit){
    dm_read(kalmanDmi->pitchSensor, (Void *) &z->pitchSensor,
            sizeof(z->pitchSensor), (DM_Time * ) NULL);
    z->pitchSensorFlag = TRUE;
  }

  /* update roll measurement */
  if (sensorGroupBits & sensorId->rollBit){
    dm_read(kalmanDmi->rollSensor, (Void *) &z->rollSensor,
            sizeof(z->rollSensor), (DM_Time * ) NULL);
    z->rollSensorFlag = TRUE;
  }

  /* update depth measurement */
  if (sensorGroupBits & sensorId->depthBit){
    dm_read(kalmanDmi->depthSensor, (Void *) &z->depthSensor,
            sizeof(z->depthSensor), (DM_Time * ) NULL);
    z->depthSensorFlag = TRUE;
  }

  /* update velocity measurement */
  if (sensorGroupBits & sensorId->ADVBit[0]){
    dm_read(kalmanDmi->dataValidADV[0], (Void *) &velocityDataValid,
            sizeof(velocityDataValid), (DM_Time * ) NULL);
    if (velocityDataValid){
      dm_read(kalmanDmi->velocityADV[0], (Void *) &z->velocityADV[0],
              sizeof(z->velocityADV[0]), (DM_Time * ) NULL);
      z->velocityADVFlag[0] = TRUE;
    }
  }
  if (sensorGroupBits & sensorId->ADVBit[1]){
    dm_read(kalmanDmi->dataValidADV[1], (Void *) &velocityDataValid,
            sizeof(velocityDataValid), (DM_Time * ) NULL);
    if (velocityDataValid){
      dm_read(kalmanDmi->velocityADV[1], (Void *) &z->velocityADV[1],
              sizeof(z->velocityADV[1]), (DM_Time * ) NULL);
      z->velocityADVFlag[1] = TRUE;
    }
  }
  if (sensorGroupBits & sensorId->ADVBit[2]){
    dm_read(kalmanDmi->dataValidADV[2], (Void *) &velocityDataValid,
            sizeof(velocityDataValid), (DM_Time * ) NULL);
    if (velocityDataValid){
      dm_read(kalmanDmi->velocityADV[2], (Void *) &z->velocityADV[2],
              sizeof(z->velocityADV[2]), (DM_Time * ) NULL);
      z->velocityADVFlag[2] = TRUE;
    }
  }

  return OK;
}


/*****************************************************************************/
/* Function : outputState                                                    */
/* Purpose  :                                                                */
/* Inputs   :                                                                */
/* Outputs  :                                                                */
/*****************************************************************************/

STATUS outputState(kalmanDMItems* kalmanDmi,
                   Position* position,
                   PositionStatus* positionStatus,
                   Attitude* attitude,
                   AttitudeStatus* attitudeStatus)
{
  Errno status;
  DM_Time sampleTime;

  gettimeofday(&sampleTime, (struct timezone *) NULL);

  status = dm_write(kalmanDmi->position, (Void *) position,
                    sizeof(Position), &sampleTime);
  status = dm_write(kalmanDmi->positionStatus, (Void *) positionStatus,
                    sizeof(PositionStatus), &sampleTime);
  status = dm_write(kalmanDmi->attitude, (Void *) attitude,
                    sizeof(Attitude), &sampleTime);
  status = dm_write(kalmanDmi->attitudeStatus, (Void *) attitudeStatus,
                    sizeof(AttitudeStatus), &sampleTime);

  return(OK);
}


/*****************************************************************************/
/* Function : initState                                                      */
/* Purpose  : Initializes the state switch                                   */
/* Inputs   :                                                                */
/* Outputs  : Return OK or ERROR.                                            */
/*****************************************************************************/

STATUS initState(DM_Item* kalmanStateDmi,
                 kalmanDMItems* kalmanDmi,
                 DM_Group* sensorGroup,
                 sensorBits* sensorId)
{
  Errno status;                         /* error code                        */
  Int16 item;                           /* item counter                      */
  char sensorDMPrefix[] = SENSOR_DM_PREFIX;
  char *sensorDMName;
  MLocal char compassOffsetName[DM_ITEM_NAME_LEN];
  MLocal char compassRadiansName[DM_ITEM_NAME_LEN];
  MLocal char compassDataValidName[DM_ITEM_NAME_LEN];
  MLocal char gyroRadiansName[DM_ITEM_NAME_LEN];
  MLocal char gyroRateRadiansName[DM_ITEM_NAME_LEN];
  MLocal char gyroDataValidName[DM_ITEM_NAME_LEN];
  MLocal char pitchRadiansName[DM_ITEM_NAME_LEN];
  MLocal char rollRadiansName[DM_ITEM_NAME_LEN];
  MLocal Flt32 compassMagVar = COMPASS_VARIATION;

  /* data manager items create values   */
  struct
  {
    char *name;                         /* data manager array name           */
    DM_Type type;                       /* data manager item type            */
    DM_Num  num;                        /* item number                       */
    DM_Item *item;                      /* data manager item handle          */
  } dmiCreate[] = {
  { compassOffsetName,  DM_FLT32, 1, &kalmanDmi->compassOffset },
  { compassRadiansName, DM_FLT32, 1, &kalmanDmi->compassRadians },
  { gyroRadiansName, DM_FLT32, 1, &kalmanDmi->gyroRadians },
  { gyroRateRadiansName, DM_FLT32, 1, &kalmanDmi->gyroRateRadians },
  { compassDataValidName, DM_MBOOL, 1, &kalmanDmi->compassDataValid },
  { gyroDataValidName, DM_MBOOL, 1, &kalmanDmi->gyroDataValid },
  { pitchRadiansName, DM_FLT32, 1, &kalmanDmi->pitchSensor },
  { rollRadiansName, DM_FLT32, 1, &kalmanDmi->rollSensor },
  { VEHICLE_DEPTH_DM, DM_FLT64, 1, &kalmanDmi->depthSensor },
  { VEHICLE_DEPTH_DATA_VALID_DM, DM_MBOOL, 1, &kalmanDmi->depthDataValid },
  { DOPPLER_BOTTOM_TRACK_VELOCITY_DM, DM_FLT32, 3, kalmanDmi->velocityADV },
  { DOPPLER_BOTTOM_TRACK_DATA_VALID_DM, DM_MBOOL, 3, kalmanDmi->dataValidADV },
  { NULL, DM_ENDT, 0, NO_ITEM }
  };

  /* create sensor data manager item names */
  sensorDMName = COMPASS_MAG_VARIATION_DM;
  strcpy(compassOffsetName, sensorDMPrefix);
  strcat(compassOffsetName, sensorDMName);

  sensorDMName = COMPASS_RADIANS_DM;
  strcpy(compassRadiansName, sensorDMPrefix);
  strcat(compassRadiansName, sensorDMName);

  sensorDMName = COMPASS_DATA_VALID_DM;
  strcpy(compassDataValidName, sensorDMPrefix);
  strcat(compassDataValidName, sensorDMName);

  sensorDMName = GYRO_RADIANS_DM;
  strcpy(gyroRadiansName, sensorDMPrefix);
  strcat(gyroRadiansName, sensorDMName);

  sensorDMName = GYRO_RATE_RADIANS_DM;
  strcpy(gyroRateRadiansName, sensorDMPrefix);
  strcat(gyroRateRadiansName, sensorDMName);

  sensorDMName = GYRO_DATA_VALID_DM;
  strcpy(gyroDataValidName, sensorDMPrefix);
  strcat(gyroDataValidName, sensorDMName);

  sensorDMName = PITCH_RADIANS_DM;
  strcpy(pitchRadiansName, sensorDMPrefix);
  strcat(pitchRadiansName, sensorDMName);

  sensorDMName = ROLL_RADIANS_DM;
  strcpy(rollRadiansName, sensorDMPrefix);
  strcat(rollRadiansName, sensorDMName);

  /* create data manager items */
  for (item = 0; dmiCreate[item].item != NO_ITEM; item++){
    status = dm_create(dmiCreate[item].name, dmiCreate[item].num,
                       dmiCreate[item].item, dmiCreate[item].type, 1,
                       DM_ENDT);
  }

  /* create vehicle naviagtional state DM item */
  status = dm_create(NAVIGATION_STATE_DM, 1,
                     kalmanStateDmi,
                     DM_ENUM, 1,
                     DM_FLT32, 36,
                     DM_ENUM, 27,
                     DM_ENDT, 0);
  status = dm_create(POSITION_DM, 1,
                     &kalmanDmi->position,
                     DM_FLT32, 6,
                     DM_ENDT, 0);
  status = dm_create(POSITION_STATUS_DM, 1,
                     &kalmanDmi->positionStatus,
                     DM_ENUM, 6,
                     DM_ENDT, 0);
  status = dm_create(ATTITUDE_DM, 1,
                     &kalmanDmi->attitude,
                     DM_FLT32, 6,
                     DM_ENDT, 0);
  status = dm_create(ATTITUDE_STATUS_DM, 1,
                     &kalmanDmi->attitudeStatus,
                     DM_ENUM, 6,
                     DM_ENDT, 0);


  /* start consumers */
  status = dm_start_consumer(*kalmanStateDmi,
                             STATE_DM_PERIOD,
                             SEM_NULL);
  status = dm_start_consumer(kalmanDmi->compassOffset,
                             DM_STATIC,
                             SEM_NULL);
  status = dm_start_consumer(kalmanDmi->compassRadians,
                             STATE_DM_PERIOD,
                             SEM_NULL);
  status = dm_start_consumer(kalmanDmi->gyroRadians,
                             STATE_DM_PERIOD,
                             SEM_NULL);
  status = dm_start_consumer(kalmanDmi->gyroRateRadians,
                             STATE_DM_PERIOD,
                             SEM_NULL);
  status = dm_start_consumer(kalmanDmi->compassDataValid,
                             STATE_DM_PERIOD,
                             SEM_NULL);
  status = dm_start_consumer(kalmanDmi->gyroDataValid,
                             STATE_DM_PERIOD,
                             SEM_NULL);
  status = dm_start_consumer(kalmanDmi->pitchSensor,
                             STATE_DM_PERIOD,
                             SEM_NULL);
  status = dm_start_consumer(kalmanDmi->rollSensor,
                             STATE_DM_PERIOD,
                             SEM_NULL);
  status = dm_start_consumer(kalmanDmi->depthSensor,
                             STATE_DM_PERIOD,
                             SEM_NULL);
  status = dm_start_consumer(kalmanDmi->depthDataValid,
                             STATE_DM_PERIOD,
                             SEM_NULL);
  status = dm_start_consumer(kalmanDmi->velocityADV[0],
                             STATE_DM_PERIOD,
                             SEM_NULL);
  status = dm_start_consumer(kalmanDmi->velocityADV[1],
                             STATE_DM_PERIOD,
                             SEM_NULL);
  status = dm_start_consumer(kalmanDmi->velocityADV[2],
                             STATE_DM_PERIOD,
                             SEM_NULL);
  status = dm_start_consumer(kalmanDmi->dataValidADV[0],
                             STATE_DM_PERIOD,
                             SEM_NULL);
  status = dm_start_consumer(kalmanDmi->dataValidADV[1],
                             STATE_DM_PERIOD,
                             SEM_NULL);
  status = dm_start_consumer(kalmanDmi->dataValidADV[2],
                             STATE_DM_PERIOD,
                             SEM_NULL);


  /* create data manager group */
  *sensorGroup = dm_create_group();

  /* add consumer items to group */
  status = dm_group_add_item(*sensorGroup, kalmanDmi->compassRadians,
                             &sensorId->compassBit);
  status = dm_group_add_item(*sensorGroup, kalmanDmi->gyroRadians,
                             &sensorId->gyroBit);
  status = dm_group_add_item(*sensorGroup, kalmanDmi->pitchSensor,
                             &sensorId->pitchBit);
  status = dm_group_add_item(*sensorGroup, kalmanDmi->rollSensor,
                             &sensorId->rollBit);
  status = dm_group_add_item(*sensorGroup, kalmanDmi->depthSensor,
                             &sensorId->depthBit);
  status = dm_group_add_item(*sensorGroup, kalmanDmi->velocityADV[0],
                             &sensorId->ADVBit[0]);
  status = dm_group_add_item(*sensorGroup, kalmanDmi->velocityADV[1],
                             &sensorId->ADVBit[1]);
  status = dm_group_add_item(*sensorGroup, kalmanDmi->velocityADV[2],
                             &sensorId->ADVBit[2]);


  /* start provider for vehicle position and check status */
  status = dm_start_provider(kalmanDmi->position, STATE_DM_PERIOD);
  status = dm_start_provider(kalmanDmi->positionStatus, STATE_DM_PERIOD);
  status = dm_start_provider(kalmanDmi->attitude, STATE_DM_PERIOD);
  status = dm_start_provider(kalmanDmi->attitudeStatus, STATE_DM_PERIOD);

  urgentWriteDmItem(kalmanDmi->compassOffset, &compassMagVar,
                    sizeof(compassMagVar));
}


/*****************************************************************************/
/* Function : Get_Time                                                       */
/* Purpose  :                                                                */
/* Inputs   :                                                                */
/* Outputs  :                                                                */
/* Notes    : This routine has about 16uS of overhead                        */
/*****************************************************************************/

void Get_Time(Flt64* t, Flt64* dt)
{
  static MBool init = TRUE;             /* initialization flag        */
  DM_Time sampleTime;                   /* update time                */
  Flt64 time = 0.0;                     /* absolute time              */
  static Flt64 time_last = 0.0;         /* absolute time              */
  static Flt64 timeOffset;              /* time offset                */

  gettimeofday(&sampleTime, (struct timezone *) NULL);
  time = (Flt64) sampleTime.tv_sec +
         (Flt64) sampleTime.tv_usec / 1000000.0;
  if (init){
    timeOffset = time;
    init = FALSE;
  }
  time -= timeOffset;
  *dt = time - time_last;
  *t = time;
  time_last = time;
}


/*****************************************************************************/
/* Function : testData                                                       */
/* Purpose  : generates fake gyro data for testing                           */
/* Inputs   :                                                                */
/* Outputs  :                                                                */
/* Notes    :                                                                */
/*****************************************************************************/

STATUS testData()
{
  Errno status;                         /* error code                        */
  Int16 item;                           /* item counter                      */
  char sensorDMPrefix[] = SENSOR_DM_PREFIX;
  char *sensorDMName;
  MLocal char compassRadiansName[DM_ITEM_NAME_LEN];
  MLocal char compassDataValidName[DM_ITEM_NAME_LEN];
  MLocal char gyroRadiansName[DM_ITEM_NAME_LEN];
  MLocal char gyroRateRadiansName[DM_ITEM_NAME_LEN];
  MLocal char gyroDataValidName[DM_ITEM_NAME_LEN];
  MLocal char pitchRadiansName[DM_ITEM_NAME_LEN];
  MLocal char rollRadiansName[DM_ITEM_NAME_LEN];

  DM_Item gyroDmi;
  DM_Item gyroRateDmi;
  DM_Item gyroDataValidDmi;
  DM_Item compassDmi;
  DM_Item compassDataValidDmi;
  DM_Item pitchSensorDmi;
  DM_Item rollSensorDmi;
  SEM_ID  wakeupSem;
  Flt32 gyroData = 1.2345;
  Flt32 gyroRateData = 6.789;
  MBool gyroDataValid = TRUE;
  Flt32 compassData = 0.9876;
  Flt32 pitchSensor = 11111;
  Flt32 rollSensor = 22222;
  MBool compassDataValid = TRUE;
  DM_Time sampleTime;

  struct
  {
    char *name;                         /* data manager array name           */
    DM_Type type;                       /* data manager item type            */
    DM_Num  num;                        /* item number                       */
    DM_Item *item;                      /* data manager item handle          */
  } dmiCreate[] = {
  { compassRadiansName, DM_FLT32, 1, &compassDmi },
  { gyroRadiansName, DM_FLT32, 1, &gyroDmi },
  { gyroRateRadiansName, DM_FLT32, 1, &gyroRateDmi },
  { compassDataValidName, DM_MBOOL, 1, &compassDataValidDmi },
  { gyroDataValidName, DM_MBOOL, 1, &gyroDataValidDmi },
  { pitchRadiansName, DM_FLT32, 1, &pitchSensorDmi },
  { rollRadiansName, DM_FLT32, 1, &rollSensorDmi },
  { NULL, DM_ENDT, 0, NO_ITEM }
  };

  /* create sensor data manager item names */
  sensorDMName = COMPASS_RADIANS_DM;
  strcpy(compassRadiansName, sensorDMPrefix);
  strcat(compassRadiansName, sensorDMName);

  sensorDMName = COMPASS_DATA_VALID_DM;
  strcpy(compassDataValidName, sensorDMPrefix);
  strcat(compassDataValidName, sensorDMName);

  sensorDMName = GYRO_RADIANS_DM;
  strcpy(gyroRadiansName, sensorDMPrefix);
  strcat(gyroRadiansName, sensorDMName);

  sensorDMName = GYRO_RATE_RADIANS_DM;
  strcpy(gyroRateRadiansName, sensorDMPrefix);
  strcat(gyroRateRadiansName, sensorDMName);

  sensorDMName = GYRO_DATA_VALID_DM;
  strcpy(gyroDataValidName, sensorDMPrefix);
  strcat(gyroDataValidName, sensorDMName);

  sensorDMName = PITCH_RADIANS_DM;
  strcpy(pitchRadiansName, sensorDMPrefix);
  strcat(pitchRadiansName, sensorDMName);

  sensorDMName = ROLL_RADIANS_DM;
  strcpy(rollRadiansName, sensorDMPrefix);
  strcat(rollRadiansName, sensorDMName);

  /* create data manager items */
  for (item = 0; dmiCreate[item].item != NO_ITEM; item++){
    status = dm_create(dmiCreate[item].name, dmiCreate[item].num,
                       dmiCreate[item].item, dmiCreate[item].type, 1,
                       DM_ENDT);
    if (checkCreate(status, dmiCreate[item].name, TRUE) == ERROR)
      return(ERROR);
  }

  status = dm_start_provider(gyroDmi, STATE_DM_PERIOD);
  if (checkProvider(status, "gyroDmi") == ERROR)
    return(ERROR);
  status = dm_start_provider(gyroRateDmi, STATE_DM_PERIOD);
  if (checkProvider(status, "gyroRateDmi") == ERROR)
    return(ERROR);
  status = dm_start_provider(gyroDataValidDmi, STATE_DM_PERIOD);
  if (checkProvider(status, "gyroDataValidDmi") == ERROR)
    return(ERROR);
  status = dm_start_provider(compassDmi, STATE_DM_PERIOD);
  if (checkProvider(status, "compassDmi") == ERROR)
    return(ERROR);
  status = dm_start_provider(compassDataValidDmi, STATE_DM_PERIOD);
  if (checkProvider(status, "compassDataValidDmi") == ERROR)
    return(ERROR);
  status = dm_start_provider(pitchSensorDmi, STATE_DM_PERIOD);
  if (checkProvider(status, "pitchSensorDmi") == ERROR)
    return(ERROR);
  status = dm_start_provider(rollSensorDmi, STATE_DM_PERIOD);
  if (checkProvider(status, "rollSensorDmi") == ERROR)
    return(ERROR);

  taskPrioritySet(taskIdSelf(), KALMAN_STATE_TASK_PRIORITY);

  /* create periodic control loop timing semaphore */
  if ((wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL){
    logMsg("stateSwitch: Error initializing semaphore wakeupSem\n");
    return;
  }

  FOREVER {

    /* wakeup at 20Hz rate */
    semTake(wakeupSem, sysClkRateGet() / 20);

    gettimeofday(&sampleTime, (struct timezone *) NULL);

    status = dm_write(gyroDmi, (Void *) &gyroData,
                      sizeof(DM_FLT32), &sampleTime);
    status = dm_write(gyroRateDmi, (Void *) &gyroRateData,
                      sizeof(DM_FLT32), &sampleTime);
    status = dm_write(gyroDataValidDmi, (Void *) &gyroDataValid,
                      sizeof(gyroDataValid), &sampleTime);
    status = dm_write(compassDmi, (Void *) &compassData,
                      sizeof(DM_FLT32), &sampleTime);
    status = dm_write(compassDataValidDmi, (Void *) &compassDataValid,
                      sizeof(compassDataValid), &sampleTime);
    status = dm_write(pitchSensorDmi, (Void *) &pitchSensor,
                      sizeof(DM_FLT32), &sampleTime);
    status = dm_write(rollSensorDmi, (Void *) &rollSensor,
                      sizeof(DM_FLT32), &sampleTime);

  }
  return(OK);
}