/******************************************************************************/
/* Copyright 1992 MBARI                                                       */
/******************************************************************************/
/* Summary  : Thruster Map Module for the Tiburon Control System              */
/* Filename : map.c                                                           */
/* Author   : Janice Tarrant                                                  */
/* Project  : Tiburon                                                         */
/* Version  : Version 1.0                                                     */
/* Created  : 06/18/92                                                        */
/* Modified : 03/13/96                                                        */
/* Archived :                                                                 */
/******************************************************************************/
/* Modification History :                                                     */
/* $Header: map.c,v 1.1 97/12/04 15:28:03 oreilly Exp $
 * $Log:        map.c,v $
 * Revision 1.1  97/12/04  15:28:03  15:28:03  oreilly (Thomas C. O'Reilly)
 * Initial revision
 *
 *
 */
/******************************************************************************/

#include <vxWorks.h>            /* Vxworks system declarations                */
#include <stdioLib.h>           /* VxWorks standard I/O library               */
#include <math.h>               /* VxWorks floating point math library        */
#include <semLib.h>             /* VxWorks semaphore library                  */
#include <systime.h>            /* VxWorks system time declarations           */
#include <mbariTypes.h>         /* MBARI style guide type declarations        */
#include <datamgr.h>            /* data manager declarations                  */
#include <dm_errno.h>           /* data manager error declarations            */
#include <thrusterDM.h>         /* thruster definitions                       */

#include "control.h"            /* control system definitions                 */
#include "interface.h"          /* command interface definitions              */

#define COMMAND_DOF            5        /* command degrees of freedom (x, y,  */
                                        /* z, lateral and horizontal heading) */
#define MOTOR_STALL_ALARM_LVL 10        /* Consecutive motor stall counts     */
                                        /* from Andy's thruster.c             */

                                        /* map group id bits                  */
typedef struct
{
    DWord motorEnableBit[NUM_THRUSTERS];        /* motor enable bits          */
    DWord motorStallBit[NUM_THRUSTERS];         /* motor stall alarm bits     */
    DWord headingDisableTransferBit;            /* heading transfer bit       */
    DWord xThrustThresholdBit;                  /* x thrust threshold bit     */
} mapBits;

                                /* data manager error codes                   */
typedef struct
{
    Errno thrust[DOF];                  /* thrust command                     */
    Errno hcf;                          /* heading conversion factor          */
    Errno enable[NUM_THRUSTERS];        /* motor enable                       */
    Errno stall[NUM_THRUSTERS];         /* motor stall alarm                  */
    Errno transfer;                     /* heading transfer                   */
    Errno threshold;                    /* x thrust threshold                 */
    Errno control[NUM_THRUSTERS];       /* control thrust                     */
} dmStatus;

                                /* command degrees of freedom                 */
typedef enum { X, Y, Z, LAT_HEADING, HORIZ_HEADING } commandDof;

                                /* function prototypes                        */
#if __STDC__
LOCAL Errno initThrusterMap(dmOpId dmOp, controlDMItems *controlDmi,
                            DM_Group *mapGroup, mapBits *mapId,
                            dmStatus *status, SEM_ID mapSem[]);
LOCAL Void  getChanges(DWord mapGroupBits, mapBits *mapId,
                       controlDMItems *controlDmi,
                       motorEnableState motorEnable[], MBool motorStall[],
                       MBool *headingDisableTransfer, Int16 *xThrustThreshold);
LOCAL void  mapHeadingThrust(Flt32 *primaryHeadingThrust,
                             Flt32 *secondaryHeadingThrust, Flt32 thrustHCF,
                             Flt32 translationThrust, Flt32 thrustMax,
                             MBool headingDisableTransfer, MBool motorsEnabled,
                             MBool motorsStalled);
LOCAL void  checkControlThrust(Flt32 control[], Flt32 thrust[], Flt32 cos12);
#endif

/******************************************************************************/
/* Function : thrusterMap                                                     */
/* Purpose  : Uses the thruster geometric locations to determine required     */
/*            thruster control thrust from vehicle thrust commands.           */
/* Inputs   : Control data manager items structure pointer.                   */
/* Outputs  : None.                                                           */
/******************************************************************************/
    Void
thrusterMap(controlDMItems *controlDmi)
{
    extern FILE *fpw;                           /* file ptr for data logging  */
    static Int16     motorStallCount = 0;       /* motor stall counter        */
    SEM_ID           mapSem[DOF];               /* thruster map semaphores    */
    DM_Group         mapGroup;                  /* data manager group         */
    DWord            mapGroupBits;              /* map group changes          */
    mapBits          mapId;                     /* map id bits                */
    dmStatus         status;                    /* data manager error codes   */
    Flt32            thrust[COMMAND_DOF];       /* thrust commands            */
    Flt32            thrustHCF;                 /* heading conversion factor  */
    motorEnableState motorEnable[NUM_THRUSTERS];/* motor enable               */
    MBool            motorsEnabled;             /* heading motor pair enabled */
    MBool            motorStall[NUM_THRUSTERS] =/* motor stall                */
                     { FALSE, FALSE, FALSE, FALSE, FALSE, FALSE };
    MBool            motorsStalled;             /* heading motor pair stalled */
    MBool            headingDisableTransfer;    /* transfer heading on disable*/
    Int16            xThrustThreshold;          /* threshold for hdg transfer */
    Flt32            control[NUM_THRUSTERS];    /* thruster control thrust    */
    Int16            dof;                       /* degree of freedom counter  */
    thrusterId       thruster;                  /* thruster counter           */
    MBool            init;                      /* initialization flag        */
    DM_Time          sampleTime;                /* control thrust sample time */
                                                /* horizontal thruster offset */
    const Flt32      cos12 = cos(12.0 * PI / 180.0);


/* initialize task wakeup semaphores                                          */
    for (dof = 0; dof < DOF; dof++)
        if ((mapSem[dof] = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL)
        {
            logMsg("Control System FAILURE :\ncould not initialize thruster map semaphore\n");
            controlShutDown("tthrusterMap");
        }

/* start consumer for thrust commands, thrust heading conversion factor,      */
/* heading disable transfer, x thrust threshold, motor enable and motor stall */
    if (initThrusterMap(CONSUMER, controlDmi, &mapGroup, &mapId, &status,
                        mapSem) == ERROR)
        controlShutDown("tthrusterMap");

/* start provider for control thrust                                          */
    if (initThrusterMap(PROVIDER, controlDmi, &mapGroup, &mapId, &status,
                        mapSem) == ERROR)
        controlShutDown("tthrusterMap");

/* read initial values for the motor enables, heading disable transfer and x  */
/* thrust threshold                                                           */
    for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++)
        status.enable[thruster] =
        dm_read(controlDmi->motorEnable[thruster],
                (Void *) &motorEnable[thruster],
                sizeof(motorEnable[thruster]), (DM_Time *) NULL);
    status.transfer = dm_read(controlDmi->headingDisableTransfer,
                              (Void *) &headingDisableTransfer,
                              sizeof(headingDisableTransfer), (DM_Time *) NULL);
    status.threshold = dm_read(controlDmi->xThrustThreshold,
                               (Void *) &xThrustThreshold,
                               sizeof(xThrustThreshold), (DM_Time *) NULL);

/* initialization flag, first time through loop check data manager status     */
    init = TRUE;

    FOREVER
    {

/* wakeup when all commands are available                                     */
        for (dof = 0; dof < DOF; dof++)
            semTake(mapSem[dof], WAIT_FOREVER);

/* get thrust commands, x, y, z, and heading (laterals)                       */
        for (dof = 0; dof < DOF; dof++)
            status.thrust[dof] =
            dm_read(controlDmi->commandedThrust[dof], (Void *) &thrust[dof],
                    sizeof(thrust[dof]), (DM_Time *) NULL);

/* get thrust heading conversion factor                                       */
        status.hcf = dm_read(controlDmi->commandedThrustHCF,
                            (Void *) &thrustHCF, sizeof(thrustHCF),
                            (DM_Time *) NULL);

/* read other dm items if changed                                             */
        if ((mapGroupBits = dm_get_group_changes(mapGroup)) != 0)
            getChanges(mapGroupBits, &mapId, controlDmi, motorEnable,
                       motorStall, &headingDisableTransfer, &xThrustThreshold);

        if (init)                               /* verify initial reads only  */
            if (initThrusterMap(READER, controlDmi, &mapGroup, &mapId, &status,
                                mapSem) == ERROR)
                controlShutDown("tthrusterMap");

/* map heading thrust command between lateral and horizontal thrusters        */
        if (fabs(thrust[X]) > xThrustThreshold) /* horizontal thrusters during*/
        {                                       /* forward translation        */
            thrust[HORIZ_HEADING] = thrust[LAT_HEADING] * thrustHCF;
            thrust[LAT_HEADING] = 0.0;
            motorsEnabled = ((motorEnable[PORT_HORIZ] == ENABLE_MOTOR) &&
                             (motorEnable[STBD_HORIZ] == ENABLE_MOTOR));
            motorsStalled = motorStall[PORT_HORIZ] || motorStall[STBD_HORIZ];
            mapHeadingThrust(&thrust[HORIZ_HEADING], &thrust[LAT_HEADING],
                             (1.0 / thrustHCF), thrust[X], (Flt32) X_THRUST_MAX,
                             headingDisableTransfer, motorsEnabled,
                             motorsStalled);
        }
        else                                    /* lateral thrusters during   */
        {                                       /* hover                      */
            thrust[HORIZ_HEADING] = 0.0;
            motorsEnabled = ((motorEnable[FWD_LATERAL] == ENABLE_MOTOR) &&
                             (motorEnable[AFT_LATERAL] == ENABLE_MOTOR));
            motorsStalled = motorStall[FWD_LATERAL] || motorStall[AFT_LATERAL];
            mapHeadingThrust(&thrust[LAT_HEADING], &thrust[HORIZ_HEADING],
                             thrustHCF, thrust[Y], (Flt32) Y_THRUST_MAX,
                             headingDisableTransfer, motorsEnabled,
                             motorsStalled);
        }
#if 0
fprintf(fpw,"thrust  x %10.4f  y %10.4f  z %10.4f  lh %10.4f  hh %10.4f\n",
thrust[X], thrust[Y], thrust[Z], thrust[LAT_HEADING], thrust[HORIZ_HEADING]);
#endif

/* convert vehicle thrust commands to thruster control thrust                 */
/* standard coordinate system, x forward, z down                              */
        control[PORT_HORIZ]  = 0.5 *
                               (thrust[X] / cos12 + thrust[HORIZ_HEADING]);
        control[STBD_HORIZ]  = 0.5 *
                               (thrust[X] / cos12 - thrust[HORIZ_HEADING]);
        control[FWD_LATERAL] = 0.5 * ( thrust[Y] + thrust[LAT_HEADING]);
        control[AFT_LATERAL] = 0.5 * (-thrust[Y] + thrust[LAT_HEADING]);
        control[PORT_VERT]   = 0.5 * thrust[Z] / cos12;
        control[STBD_VERT]   = 0.5 * thrust[Z] / cos12;

/* if motor not enabled, set control thrust to zero                           */
        if (motorEnable[PORT_HORIZ] != ENABLE_MOTOR)
            control[PORT_HORIZ] = 0.0;
        if (motorEnable[STBD_HORIZ] != ENABLE_MOTOR)
            control[STBD_HORIZ] = 0.0;
        if (motorEnable[FWD_LATERAL] != ENABLE_MOTOR)
            control[FWD_LATERAL] = 0.0;
        if (motorEnable[AFT_LATERAL] != ENABLE_MOTOR)
            control[AFT_LATERAL] = 0.0;
        if (motorEnable[PORT_VERT] != ENABLE_MOTOR)
            control[PORT_VERT] = 0.0;
        if (motorEnable[STBD_VERT] != ENABLE_MOTOR)
            control[STBD_VERT] = 0.0;

/* check maximum control values                                               */
        checkControlThrust(control, thrust, cos12);
#if 0
        fprintf(fpw,"control ph %10.4f  sh %10.4f  fl %10.4f\n", control[0],
                control[1], control[2]);
        fprintf(fpw,"control al %10.4f  pv %10.4f  sv %10.4f\n", control[3],
                control[4], control[5]);
#endif

/* update motor control thrust                                                */
        gettimeofday(&sampleTime, (struct timezone *) NULL);
        for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++)
            status.control[thruster] =
            dm_write(controlDmi->controlThrust[thruster],
                     (Void *) &control[thruster], sizeof(control[thruster]),
                     &sampleTime);
        if (init)                               /* verify initial writes only */
            if (initThrusterMap(WRITER, controlDmi, &mapGroup, &mapId, &status,
                                mapSem) == ERROR)
                controlShutDown("tthrusterMap");

/* reset motor stall alarm flags                                              */
    for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++)
        if (motorStall[thruster])
        {
            motorStallCount++;
            if (motorStallCount > MOTOR_STALL_ALARM_LVL)
            {
                motorStallCount = 0;
                motorStall[thruster] = FALSE;
            }
        }

        if (init)
            init = FALSE;

    } /* FOREVER */

} /* thrusterMap */


/******************************************************************************/
/* Function : initThrusterMap                                                 */
/* Purpose  : Performs thruster map data manager items initialization and     */
/*            checks.                                                         */
/* Inputs   : Data manager operation identifier, control data manager items   */
/*            data manager group and id bits, read/write status and wakeup    */
/*            semaphore.                                                      */
/* Outputs  : Returns OK or ERROR.                                            */
/******************************************************************************/
    Errno
initThrusterMap(dmOpId dmOp, controlDMItems *controlDmi, DM_Group *mapGroup,
                mapBits *mapId, dmStatus *status, SEM_ID mapSem[])
{
    thrusterId thruster;                        /* thruster counter           */

    switch (dmOp)
    {
/* start consumer items and check status                                      */
        case CONSUMER:
            status->thrust[X_INDEX] =
            dm_start_consumer(controlDmi->commandedThrust[X_INDEX],
                              CONTROL_PERIOD, mapSem[X_INDEX]);
            if (checkConsumer(status->thrust[X_INDEX],
                "Thruster Map - commanded thrust x", FALSE) == ERROR)
                return(ERROR);
            status->thrust[Y_INDEX] =
            dm_start_consumer(controlDmi->commandedThrust[Y_INDEX],
                              CONTROL_PERIOD, mapSem[Y_INDEX]);
            if (checkConsumer(status->thrust[Y_INDEX],
                "Thruster Map - commanded thrust y", FALSE) == ERROR)
                return(ERROR);
            status->thrust[Z_INDEX] =
            dm_start_consumer(controlDmi->commandedThrust[Z_INDEX],
                              CONTROL_PERIOD, mapSem[Z_INDEX]);
            if (checkConsumer(status->thrust[Z_INDEX],
                "Thruster Map - commanded thrust z", FALSE) == ERROR)
                return(ERROR);
            status->thrust[HEADING_INDEX] =
            dm_start_consumer(controlDmi->commandedThrust[HEADING_INDEX],
                              CONTROL_PERIOD, mapSem[HEADING_INDEX]);
            if (checkConsumer(status->thrust[HEADING_INDEX],
                "Thruster Map - commanded thrust heading", FALSE) == ERROR)
                return(ERROR);
            status->hcf = dm_start_consumer(controlDmi->commandedThrustHCF,
                                            CONTROL_PERIOD, SEM_NULL);
            if (checkConsumer(status->hcf,
                "Thruster Map - commanded thrust heading conversion factor",
                FALSE) == ERROR)
                return(ERROR);
            status->transfer =
            dm_start_consumer(controlDmi->headingDisableTransfer,
                              DM_STATIC, SEM_NULL);
            if (checkConsumer(status->transfer,
                "Thruster Map - heading disable transfer flag", FALSE) == ERROR)
                return(ERROR);
            status->threshold = dm_start_consumer(controlDmi->xThrustThreshold,
                                                  DM_STATIC, SEM_NULL);
            if (checkConsumer(status->threshold,
                "Thruster Map - x thruster threshold", FALSE) == ERROR)
                return(ERROR);
            for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++)
                status->enable[thruster] =
                dm_start_consumer(controlDmi->motorEnable[thruster], DM_STATIC,
                                  SEM_NULL);
            if (checkConsumer(status->enable[PORT_HORIZ],
                "Thruster Map - port-horizontal motor enable", FALSE) == ERROR)
                return(ERROR);
            if (checkConsumer(status->enable[STBD_HORIZ],
                "Thruster Map - starboard-horizontal motor enable", FALSE)
                == ERROR)
                return(ERROR);
            if (checkConsumer(status->enable[FWD_LATERAL],
                "Thruster Map - forward-lateral motor enable", FALSE) == ERROR)
                return(ERROR);
            if (checkConsumer(status->enable[AFT_LATERAL],
                "Thruster Map - aft-lateral motor enable", FALSE) == ERROR)
                return(ERROR);
            if (checkConsumer(status->enable[PORT_VERT],
                "Thruster Map - port-vertical motor enable", FALSE) == ERROR)
                return(ERROR);
            if (checkConsumer(status->enable[STBD_VERT],
                "Thruster Map - starboard-vertical motor enable", FALSE)
                == ERROR)
                return(ERROR);
            for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++)
                status->stall[thruster] =
                dm_start_consumer(controlDmi->motorStall[thruster], DM_STATIC,
                                  SEM_NULL);
            if (checkConsumer(status->stall[PORT_HORIZ],
                "Thruster Map - port-horizontal motor stall", FALSE) == ERROR)
                return(ERROR);
            if (checkConsumer(status->stall[STBD_HORIZ],
                "Thruster Map - starboard-horizontal motor stall", FALSE)
                == ERROR)
                return(ERROR);
            if (checkConsumer(status->stall[FWD_LATERAL],
                "Thruster Map - forward-lateral motor stall", FALSE) == ERROR)
                return(ERROR);
            if (checkConsumer(status->stall[AFT_LATERAL],
                "Thruster Map - aft-lateral motor stall", FALSE) == ERROR)
                return(ERROR);
            if (checkConsumer(status->stall[PORT_VERT],
                "Thruster Map - port-vertical motor stall", FALSE) == ERROR)
                return(ERROR);
            if (checkConsumer(status->stall[STBD_VERT],
                "Thruster Map - starboard-vertical motor stall", FALSE)
                == ERROR)
                return(ERROR);

/* create data manager group and add asynchronously changeable items          */
            *mapGroup = dm_create_group();
            status->transfer = dm_group_add_item(*mapGroup,
                               controlDmi->headingDisableTransfer,
                               &mapId->headingDisableTransferBit);
            if (checkGroup(status->transfer,
                "Thruster Map - heading disable transfer") == ERROR)
                return(ERROR);
            status->threshold = dm_group_add_item(*mapGroup,
                                controlDmi->xThrustThreshold,
                                &mapId->xThrustThresholdBit);
            if (checkGroup(status->threshold,
                "Thruster Map - heading disable transfer") == ERROR)
                return(ERROR);
            for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++)
                status->enable[thruster] =
                dm_group_add_item(*mapGroup, controlDmi->motorEnable[thruster],
                                  &mapId->motorEnableBit[thruster]);
            if (checkGroup(status->enable[PORT_HORIZ],
                "Thruster Map - port-horizontal motor enable") == ERROR)
                return(ERROR);
            if (checkGroup(status->enable[STBD_HORIZ],
                "Thruster Map - starboard-horizontal motor enable") == ERROR)
                return(ERROR);
            if (checkGroup(status->enable[FWD_LATERAL],
                "Thruster Map - forward-lateral motor enable") == ERROR)
                return(ERROR);
            if (checkGroup(status->enable[AFT_LATERAL],
                "Thruster Map - aft-lateral motor enable") == ERROR)
                return(ERROR);
            if (checkGroup(status->enable[PORT_VERT],
                "Thruster Map - port-vertical motor enable") == ERROR)
                return(ERROR);
            if (checkGroup(status->enable[STBD_VERT],
                "Thruster Map - starboard-vertical motor enable") == ERROR)
                return(ERROR);
            for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++)
                status->stall[thruster] =
                dm_group_add_item(*mapGroup, controlDmi->motorStall[thruster],
                                  &mapId->motorStallBit[thruster]);
            if (checkGroup(status->stall[PORT_HORIZ],
                "Thruster Map - port-horizontal motor stall") == ERROR)
                return(ERROR);
            if (checkGroup(status->stall[STBD_HORIZ],
                "Thruster Map - starboard-horizontal motor stall") == ERROR)
                return(ERROR);
            if (checkGroup(status->stall[FWD_LATERAL],
                "Thruster Map - forward-lateral motor stall") == ERROR)
                return(ERROR);
            if (checkGroup(status->stall[AFT_LATERAL],
                "Thruster Map - aft-lateral motor stall") == ERROR)
                return(ERROR);
            if (checkGroup(status->stall[PORT_VERT],
                "Thruster Map - port-vertical motor stall") == ERROR)
                return(ERROR);
            if (checkGroup(status->stall[STBD_VERT],
                "Thruster Map - starboard-vertical motor stall") == ERROR)
                return(ERROR);
            break;

/* start provider items and check status */
        case PROVIDER:
            for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++)
                status->control[thruster] =
                dm_start_provider(controlDmi->controlThrust[thruster],
                                  CONTROL_PERIOD);
            if (checkProvider(status->control[PORT_HORIZ],
                "Thruster Map - port-horizontal control thrust") == ERROR)
                return(ERROR);
            if (checkProvider(status->control[STBD_HORIZ],
                "Thruster Map - starboard-horizontal control thrust") == ERROR)
                return(ERROR);
            if (checkProvider(status->control[FWD_LATERAL],
                "Thruster Map - forward-lateral control thrust") == ERROR)
                return(ERROR);
            if (checkProvider(status->control[AFT_LATERAL],
                "Thruster Map - aft-lateral control thrust") == ERROR)
                return(ERROR);
            if (checkProvider(status->control[PORT_VERT],
                "Thruster Map - port-vertical control thrust") == ERROR)
                return(ERROR);
            if (checkProvider(status->control[STBD_VERT],
                "Thruster Map - starboard-vertical control thrust") == ERROR)
                return(ERROR);
            break;

/* check read status */
        case READER:
            if (checkRead(status->thrust[X_INDEX], "Thruster Map - thrust x",
                TRUE) == ERROR)
                return(ERROR);
            if (checkRead(status->thrust[Y_INDEX], "Thruster Map - thrust y",
                TRUE) == ERROR)
                return(ERROR);
            if (checkRead(status->thrust[Z_INDEX], "Thruster Map - thrust z",
                TRUE) == ERROR)
                return(ERROR);
            if (checkRead(status->thrust[HEADING_INDEX],
                "Thruster Map - thrust heading", TRUE) == ERROR)
                return(ERROR);
            if (checkRead(status->hcf,
                "Thruster Map - thrust heading conversion factor", TRUE)
                == ERROR)
                return(ERROR);
            if (checkRead(status->transfer,
                "Thruster Map - heading disable transfer flag", TRUE) == ERROR)
                return(ERROR);
            if (checkRead(status->threshold,
                "Thruster Map - x thrust threshold", TRUE) == ERROR)
                return(ERROR);
            if (checkRead(status->enable[PORT_HORIZ],
                  "Thruster Map - port-horizontal motor enable", TRUE) == ERROR)
                return(ERROR);
            if (checkRead(status->enable[STBD_HORIZ],
                "Thruster Map - starboard-horizontal motor enable", TRUE)
                == ERROR)
                return(ERROR);
            if (checkRead(status->enable[FWD_LATERAL],
                "Thruster Map - forward-lateral motor enable", TRUE) == ERROR)
                return(ERROR);
            if (checkRead(status->enable[AFT_LATERAL],
                "Thruster Map - aft-lateral motor enable", TRUE) == ERROR)
                return(ERROR);
            if (checkRead(status->enable[PORT_VERT],
                "Thruster Map - port-horizontal motor enable", TRUE) == ERROR)
                return(ERROR);
            if (checkRead(status->enable[STBD_VERT],
                "Thruster Map - starboard-horizontal motor enable", TRUE)
                == ERROR)
                return(ERROR);
            break;

/* check write status */
        case WRITER:
            if (checkWrite(status->control[PORT_HORIZ],
                "Thruster Map - port-horizontal control thrust") == ERROR)
                return(ERROR);
            if (checkWrite(status->control[STBD_HORIZ],
                "Thruster Map - starboard-horizontal control thrust") == ERROR)
                return(ERROR);
            if (checkWrite(status->control[FWD_LATERAL],
                "Thruster Map - forward-lateral control thrust") == ERROR)
                return(ERROR);
            if (checkWrite(status->control[AFT_LATERAL],
                "Thruster Map - aft-lateral control thrust") == ERROR)
                return(ERROR);
            if (checkWrite(status->control[PORT_VERT],
                "Thruster Map - port-vertical control thrust") == ERROR)
                return(ERROR);
            if (checkWrite(status->control[STBD_VERT],
                "Thruster Map - starboard-vertical control thrust") == ERROR)
                return(ERROR);
            break;
    }

    return(OK);

} /* initThrusterMap */


/******************************************************************************/
/* Function : getChanges                                                      */
/* Purpose  : Reads data manager items that have changed.                     */
/* Inputs   : Map group changes, map id bits, control data manager items,     */
/*            motor enables, heading disable transfer flag and x thrust       */
/*            threshold.                                                      */
/* Outputs  : None.                                                           */
/******************************************************************************/
    Void
getChanges(DWord mapGroupBits, mapBits *mapId, controlDMItems *controlDmi,
           motorEnableState motorEnable[], MBool motorStall[],
           MBool *headingDisableTransfer, Int16 *xThrustThreshold)
{
    thrusterId thruster;                        /* thruster counter           */

    for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++)
    {
        if (mapGroupBits & mapId->motorEnableBit[thruster])
            dm_read(controlDmi->motorEnable[thruster],
                    (Void *) &motorEnable[thruster],
                    sizeof(motorEnable[thruster]), (DM_Time *) NULL);
        if (mapGroupBits & mapId->motorStallBit[thruster])
            motorStall[thruster] = TRUE;
    }

    if (mapGroupBits & mapId->headingDisableTransferBit)
        dm_read(controlDmi->headingDisableTransfer,
                (Void *) headingDisableTransfer,
                sizeof(*headingDisableTransfer), (DM_Time *) NULL);

    if (mapGroupBits & mapId->xThrustThresholdBit)
        dm_read(controlDmi->xThrustThreshold, (Void *) xThrustThreshold,
                sizeof(*xThrustThreshold), (DM_Time *) NULL);

} /* getChanges */


/******************************************************************************/
/* Function : mapHeadingThrust                                                */
/* Purpose  : Maps the heading thrust command to the lateral and/or horizontal*/
/*            thrusters.                                                      */
/* Inputs   : Primary heading thrust, secondary heading thrust, heading thrust*/
/*            conversion factor, translation thrust and motor enables.        */
/* Outputs  : None.                                                           */
/******************************************************************************/
    Void
mapHeadingThrust(Flt32 *primaryHeadingThrust, Flt32 *secondaryHeadingThrust,
                 Flt32 thrustHCF, Flt32 translationThrust, Flt32 thrustMax,
                 MBool headingDisableTransfer, MBool motorsEnabled,
                 MBool motorsStalled)
{
    Flt32 thrustRemainder;                      /* primary thrust remaining   */
                                                /* after translation command  */

/* either primary thruster disabled or stalled, use secondary thrusters       */
    if (headingDisableTransfer)
        if ((!motorsEnabled) || motorsStalled)
        {
            *secondaryHeadingThrust = *primaryHeadingThrust * thrustHCF;
            *primaryHeadingThrust = 0.0;
            return;
        }

/* one or both primary thrusters enabled, determine proportion of heading     */
/* thrust from primary and secondary thrusters                                */
/* heading thrust command uses the primary thrust remaining after translation */
/* command, the difference between the remaining thrust and the heading thrust*/
/* command comes from the secondary thrusters                                 */
    thrustRemainder = thrustMax - fabs(translationThrust);
    if ((fabs(*primaryHeadingThrust)) > thrustRemainder)
    {
        if (*primaryHeadingThrust > 0.0)
        {
            *secondaryHeadingThrust = (*primaryHeadingThrust - thrustRemainder)
                                       * thrustHCF;
            *primaryHeadingThrust = thrustRemainder;
        }
        else if (*primaryHeadingThrust < 0.0)
        {
            *secondaryHeadingThrust = (*primaryHeadingThrust + thrustRemainder)
                                       * thrustHCF;
            *primaryHeadingThrust = -thrustRemainder;
        }
    }

    return;

} /* mapHeadingThrust */


/******************************************************************************/
/* Function : checkControlThrust                                              */
/* Purpose  : Checks motor control thrust range.                              */
/* Inputs   : Control thrust pointer and commanded thrust pointer.            */
/* Outputs  : None.                                                           */
/******************************************************************************/
    LOCAL Void
checkControlThrust(Flt32 control[], Flt32 thrust[], Flt32 cos12)
{
    thrusterId thruster;                        /* thruster counter           */

/* horizontal thrusters                                                       */
/* heading thrust is limited to the horizontal thrusters remainder after      */
/* satisfying x translation                                                   */
    for (thruster = PORT_HORIZ; thruster <= STBD_HORIZ; thruster++)
    {
        if (control[thruster] > MAX_THRUST)
        {
            thrust[HORIZ_HEADING] = X_THRUST_MAX - fabs(thrust[X]);
            control[PORT_HORIZ] = 0.5 *
                                  (thrust[X] / cos12 + thrust[HORIZ_HEADING]);
            control[STBD_HORIZ] = 0.5 *
                                  (thrust[X] / cos12 - thrust[HORIZ_HEADING]);
        }
        else if (control[thruster] < -MAX_THRUST)
        {
            thrust[HORIZ_HEADING] = -(X_THRUST_MAX - fabs(thrust[X]));
            control[PORT_HORIZ] = 0.5 *
                                  (thrust[X] / cos12 + thrust[HORIZ_HEADING]);
            control[STBD_HORIZ] = 0.5 *
                                  (thrust[X] / cos12 - thrust[HORIZ_HEADING]);
        }
    }

/* lateral thrusters                                                          */
/* heading thrust is limited to the lateral thrusters remainder after         */
/* satisfying y translation                                                   */
    for (thruster = FWD_LATERAL; thruster <= AFT_LATERAL; thruster++)
    {
        if (control[thruster] > MAX_THRUST)
        {
            thrust[LAT_HEADING] = Y_THRUST_MAX - fabs(thrust[Y]);
            control[FWD_LATERAL] = 0.5 * ( thrust[Y] + thrust[LAT_HEADING]);
            control[AFT_LATERAL] = 0.5 * (-thrust[Y] + thrust[LAT_HEADING]);
        }
        else if (control[thruster] < -MAX_THRUST)
        {
            thrust[LAT_HEADING] = -(Y_THRUST_MAX - fabs(thrust[Y]));
            control[FWD_LATERAL] = 0.5 * ( thrust[Y] + thrust[LAT_HEADING]);
            control[AFT_LATERAL] = 0.5 * (-thrust[Y] + thrust[LAT_HEADING]);
        }
    }

/* all thrusters                                                              */
    for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++)
    {
        if (control[thruster] > MAX_THRUST)
            control[thruster] = MAX_THRUST;
        else if (control[thruster] < -MAX_THRUST)
            control[thruster] = -MAX_THRUST;
    }

} /* checkControlThrust */