/******************************************************************************/ /* Copyright 1992 MBARI */ /******************************************************************************/ /* Summary : Configure Mode Module for Tiburon Control System */ /* Filename : configure.c */ /* Author : Janice Tarrant */ /* Project : Tiburon */ /* Version : Version 1.0 */ /* Created : 09/01/92 */ /* Modified : 01/24/97 */ /* Archived : */ /******************************************************************************/ /* Modification History : */ /* $Header: configure.c,v 1.1 97/12/04 15:27:58 oreilly Exp $ * $Log: configure.c,v $ * Revision 1.1 97/12/04 15:27:58 15:27:58 oreilly (Thomas C. O'Reilly) * Initial revision * * */ /******************************************************************************/ #include /* VxWorks systems declarations */ #include /* VxWorks standard I/O library */ #include /* VxWorks semaphore library */ #include /* VxWorks system time declarations */ #include /* MBARI style guide type declarations */ #include /* data manager declarations */ #include /* data manager error declarations */ #include /* control system data manager definitions */ #include /* motion pak definitions */ #include /* Kalman filter definitions */ #include "control.h" /* control system definitions */ #include "trajectory.h" /* trajectory plan definitions */ /* configure mode group id bits */ typedef struct { DWord joystickModeBit; /* joystick command mode bit */ DWord autoPositionBit; /* auto position flag bit */ DWord autoDepthBit; /* auto depth flag bit */ DWord autoAltitudeBit; /* auto altitude flag bit */ DWord autoHeadingBit; /* auto heading flag bit */ DWord kalmanFilterBit; /* Kalman filter flag bit */ DWord positionValidBit; /* position valid bit */ DWord depthValidBit; /* depth valid bit */ DWord altitudeValidBit; /* altitude valid bit */ DWord headingValidBit; /* heading valid bit */ DWord headingRateValidBit; /* heading rate valid bit */ DWord setpointMovedBit[DOF]; /* trajectory setpoint moved flag bit */ } modeBits; /* control mode connection data manager items */ typedef struct { DM_Item dmiAutoError; /* auto control error flag */ DM_Item dmiCommandedThrust; /* commanded thrust */ DM_Item dmiServoThrust; /* servo thrust */ DM_Item dmiModelThrust; /* model thrust */ DM_Item dmiThrustCommand; /* thrust command */ char* servoErrMsg; /* servo error message */ char* modelErrMsg; /* model error message */ char* thrustErrMsg; /* thrust error message */ } controlModeDmi; /* gain types */ typedef enum { PROPORTIONAL, INTEGRAL, DERIVATIVE } gainType; /* function prototypes */ #if __STDC__ Errno modeDMInit(controlDMItems *controlDmi, SEM_ID modeSem, DM_Group *modeGroup, modeBits *modeId, MBool *autoPositionError, MBool *autoHeadingError, MBool *autoZError); Errno controlModeRead(controlDMItems *controlDmi, joystickCmdMode *joystickMode, MBool *autoPosition, MBool *autoDepth, MBool *autoAltitude, MBool *autoHeading, MBool *positionValid, MBool *depthValid, MBool *altitudeValid, MBool *headingValid, MBool *headingRateValid, MBool setpointMoved[], MBool *kalmanFilter); Void initControlDmi(controlDMItems *controlDmi, controlModeDmi *xPosition, controlModeDmi *yPosition, controlModeDmi *zPosition, controlModeDmi *heading, controlModeDmi *hcf); Void connectControl(MBool autoControl, MBool sensorValid, MBool *autoControlError, MBool setpointMovedFlag, joystickCmdMode joystickMode, controlModeDmi *connect); Void connectHeading(MBool kalmanFilter, controlDMItems *dmi); Void connectZ(MBool autoDepth, MBool autoAltitude, controlDMItems *dmi); Void connectOpenLoop(joystickCmdMode joystickMode, MBool autoControl, MBool sensorValid, MBool setpointMovedFlag, controlModeDmi *connect); Void connectDmi(DM_Item source, DM_Item destination, char *errorMsg); #endif /******************************************************************************/ /* Function : configureMode */ /* Purpose : Configures connections between data manager items, depending on */ /* command and control modes, and on sensor source, either direct */ /* or via the Kalman filter. */ /* Inputs : Control data manager items structure pointer. */ /* Outputs : None. */ /******************************************************************************/ Void configureMode(controlDMItems *controlDmi) { Errno status; /* error code */ SEM_ID modeSem; /* configure mode semaphore */ DM_Group modeGroup; /* configure mode data manager group */ DWord modeGroupBits; /* mode group changes bit vector */ modeBits modeId; /* mode change id bits */ joystickCmdMode joystickMode; /* joystick command mode */ MBool autoPosition; /* auto position flag */ MBool autoDepth; /* auto depth flag */ MBool autoAltitude; /* auto altitude flag */ MBool autoZ; /* auto z (depth/altitude) flag */ MBool autoHeading; /* auto heading flag */ MBool positionValid; /* position valid */ MBool prevPositionValid; /* previous position valid */ MBool depthValid; /* depth valid */ MBool prevDepthValid; /* previous depth valid */ MBool altitudeValid; /* altitude valid */ MBool prevAltitudeValid; /* previous altitude valid */ MBool headingValid; /* heading valid */ MBool prevHeadingValid; /* previous heading valid */ MBool headingRateValid; /* heading rate valid */ MBool prevHeadingRateValid;/* previous heading rate valid */ MBool autoHeadingValid; /* heading and rate valid */ MBool zValid; /* z (depth/altitude) valid */ MBool autoPositionError; /* auto position error */ MBool autoHeadingError; /* auto heading error */ MBool autoZError; /* auto z (depth/altitude) error */ MBool setpointMoved[DOF]; /* setpoint moved flags */ MBool positionSetpointMoved;/* x or y setpoint moved flag */ MBool kalmanFilter; /* Kalman filter flag */ controlModeDmi xPosition; /* control mode connection data */ controlModeDmi yPosition; /* manager items */ controlModeDmi zPosition; controlModeDmi heading; controlModeDmi hcf; /* initialize task wakeup semaphore */ if ((modeSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { logMsg("Control System FAILURE :\ncould not initialize configure mode semaphore\n"); controlShutDown("tconfigureMode"); } /* start consumer and create data manager group for joystick command mode, */ /* auto control flags, Kalman filter flag and attitude status, sensor valid */ /* flags and setpoint moved flags, start provider for and initialize auto */ /* control error flags */ if (modeDMInit(controlDmi, modeSem, &modeGroup, &modeId, &autoPositionError, &autoHeadingError, &autoZError) == ERROR) controlShutDown("tconfigureMode"); /* get joystick command mode, auto control flags, sensor valid flags setpoint */ /* moved flags, Kalman filter flag and attitude status */ if (controlModeRead(controlDmi, &joystickMode, &autoPosition, &autoDepth, &autoAltitude, &autoHeading, &positionValid, &depthValid, &altitudeValid, &headingValid, &headingRateValid, setpointMoved, &kalmanFilter) == ERROR) controlShutDown("tconfigureMode"); prevPositionValid = positionValid; prevDepthValid = depthValid; prevAltitudeValid = altitudeValid; prevHeadingValid = headingValid; prevHeadingRateValid = headingRateValid; /* initialize control mode connection data manager items */ initControlDmi(controlDmi, &xPosition, &yPosition, &zPosition, &heading, &hcf); /* set z valid flag, default is depth */ if (autoAltitude) zValid = altitudeValid; else zValid = depthValid; /* set auto heading valid flag */ autoHeadingValid = headingValid && headingRateValid; /* configure initial connections for the heading sensor */ connectHeading(kalmanFilter, controlDmi); /* configure initial connections for auto z (depth/altitude) flag, sensor z */ /* position and rate, auto z (depth/altitude) error and setpointZ */ autoZ = autoDepth || autoAltitude; connectZ(autoDepth, autoAltitude, controlDmi); /* configure initial connections for thrust commands and heading */ /* conversion factors */ positionSetpointMoved = setpointMoved[X_POSITION] || setpointMoved[Y_POSITION]; connectControl(autoPosition, positionValid, &autoPositionError, positionSetpointMoved, joystickMode, &xPosition); connectControl(autoPosition, positionValid, &autoPositionError, positionSetpointMoved, joystickMode, &yPosition); connectControl(autoZ, zValid, &autoZError, setpointMoved[Z_POSITION], joystickMode, &zPosition); connectControl(autoHeading, autoHeadingValid, &autoHeadingError, setpointMoved[HEADING], joystickMode, &heading); connectControl(autoHeading, autoHeadingValid, &autoHeadingError, setpointMoved[HEADING], joystickMode, &hcf); FOREVER { /* wait for change in command or control mode, or sensor source */ semTake(modeSem, WAIT_FOREVER); /* determine which mode has changed */ modeGroupBits = dm_get_group_changes(modeGroup); /* reconfigure for command mode change */ if (modeGroupBits & modeId.joystickModeBit) { dm_read(controlDmi->joystickMode, (Void *) &joystickMode, sizeof(joystickMode), (DM_Time *) NULL); positionSetpointMoved = setpointMoved[X_POSITION] || setpointMoved[Y_POSITION]; connectOpenLoop(joystickMode, autoPosition, positionValid, positionSetpointMoved, &xPosition); connectOpenLoop(joystickMode, autoPosition, positionValid, positionSetpointMoved, &yPosition); connectOpenLoop(joystickMode, autoZ, zValid, setpointMoved[Z_POSITION], &zPosition); connectOpenLoop(joystickMode, autoHeading, autoHeadingValid, setpointMoved[HEADING], &heading); connectOpenLoop(joystickMode, autoHeading, autoHeadingValid, setpointMoved[HEADING], &hcf); } /* reconfigure for auto position control change */ if (modeGroupBits & modeId.autoPositionBit) { dm_read(controlDmi->autoPosition, (Void *) &autoPosition, sizeof(autoPosition), (DM_Time *) NULL); dm_disconnect(controlDmi->commandedThrust[X_POSITION]); connectControl(autoPosition, positionValid, &autoPositionError, positionSetpointMoved, joystickMode, &xPosition); dm_disconnect(controlDmi->commandedThrust[Y_POSITION]); connectControl(autoPosition, positionValid, &autoPositionError, positionSetpointMoved, joystickMode, &yPosition); } /* reconfigure for auto position x setpoint moved */ if (modeGroupBits & modeId.setpointMovedBit[X_POSITION]) { dm_read(controlDmi->setpointMoved[X_POSITION], (Void *) &setpointMoved[X_POSITION], sizeof(setpointMoved[X_POSITION]), (DM_Time *) NULL); positionSetpointMoved = setpointMoved[X_POSITION] || setpointMoved[Y_POSITION]; dm_disconnect(controlDmi->commandedThrust[X_POSITION]); connectControl(autoPosition, positionValid, &autoPositionError, positionSetpointMoved, joystickMode, &xPosition); dm_disconnect(controlDmi->commandedThrust[Y_POSITION]); connectControl(autoPosition, positionValid, &autoPositionError, positionSetpointMoved, joystickMode, &yPosition); } /* reconfigure for auto position y setpoint moved */ if (modeGroupBits & modeId.setpointMovedBit[Y_POSITION]) { dm_read(controlDmi->setpointMoved[Y_POSITION], (Void *) &setpointMoved[Y_POSITION], sizeof(setpointMoved[Y_POSITION]), (DM_Time *) NULL); positionSetpointMoved = setpointMoved[X_POSITION] || setpointMoved[Y_POSITION]; dm_disconnect(controlDmi->commandedThrust[X_POSITION]); connectControl(autoPosition, positionValid, &autoPositionError, positionSetpointMoved, joystickMode, &xPosition); dm_disconnect(controlDmi->commandedThrust[Y_POSITION]); connectControl(autoPosition, positionValid, &autoPositionError, positionSetpointMoved, joystickMode, &yPosition); } /* reconfigure for position valid change */ if (modeGroupBits & modeId.positionValidBit) { dm_read(controlDmi->positionValid, (Void *) &positionValid, sizeof(positionValid), (DM_Time *) NULL); if ( (autoPosition) && (!positionValid) ) logMsg("Control System WARNING : position sensor fault\n"); if ((autoPosition) && (positionValid != prevPositionValid)) { dm_disconnect(controlDmi->commandedThrust[X_POSITION]); connectControl(autoPosition, positionValid, &autoPositionError, positionSetpointMoved, joystickMode, &xPosition); dm_disconnect(controlDmi->commandedThrust[Y_POSITION]); connectControl(autoPosition, positionValid, &autoPositionError, positionSetpointMoved, joystickMode, &yPosition); } prevPositionValid = positionValid; } /* reconfigure for auto z (depth/altitude) control change */ if ((modeGroupBits & modeId.autoDepthBit) || (modeGroupBits & modeId.autoAltitudeBit)) { dm_read(controlDmi->autoDepth, (Void *) &autoDepth, sizeof(autoDepth), (DM_Time *) NULL); dm_read(controlDmi->autoAltitude, (Void *) &autoAltitude, sizeof(autoAltitude), (DM_Time *) NULL); autoZ = autoDepth || autoAltitude; if (autoAltitude) zValid = altitudeValid; else zValid = depthValid; connectZ(autoDepth, autoAltitude, controlDmi); dm_disconnect(controlDmi->commandedThrust[Z_POSITION]); connectControl(autoZ, zValid, &autoZError, setpointMoved[Z_POSITION], joystickMode, &zPosition); } /* reconfigure for auto z (depth/altitude) position moved */ if (modeGroupBits & modeId.setpointMovedBit[Z_POSITION]) { dm_read(controlDmi->setpointMoved[Z_POSITION], (Void *) &setpointMoved[Z_POSITION], sizeof(setpointMoved[Z_POSITION]), (DM_Time *) NULL); dm_disconnect(controlDmi->commandedThrust[Z_POSITION]); connectControl(autoZ, zValid, &autoZError, setpointMoved[Z_POSITION], joystickMode, &zPosition); } /* reconfigure for depth valid change */ if (modeGroupBits & modeId.depthValidBit) { dm_read(controlDmi->depthValid, (Void *) &depthValid, sizeof(depthValid), (DM_Time *) NULL); if ( (autoDepth) && (!depthValid) ) logMsg("Control System WARNING : depth sensor fault\n"); if ((autoDepth) && (depthValid != prevDepthValid)) { zValid = depthValid; dm_disconnect(controlDmi->commandedThrust[Z_POSITION]); connectControl(autoZ, zValid, &autoZError, setpointMoved[Z_POSITION], joystickMode, &zPosition); } prevDepthValid = depthValid; } /* reconfigure for altitude valid change */ if (modeGroupBits & modeId.altitudeValidBit) { dm_read(controlDmi->altitudeValid, (Void *) &altitudeValid, sizeof(altitudeValid), (DM_Time *) NULL); if ( (autoAltitude) && (!altitudeValid) ) logMsg("Control System WARNING : altitude sensor fault\n"); if ((autoAltitude) && (altitudeValid != prevAltitudeValid)) { zValid = altitudeValid; dm_disconnect(controlDmi->commandedThrust[Z_POSITION]); connectControl(autoZ, zValid, &autoZError, setpointMoved[Z_POSITION], joystickMode, &zPosition); } prevAltitudeValid = altitudeValid; } /* reconfigure for auto heading control change */ if (modeGroupBits & modeId.autoHeadingBit) { dm_read(controlDmi->autoHeading, (Void *) &autoHeading, sizeof(autoHeading), (DM_Time *) NULL); dm_disconnect(controlDmi->commandedThrust[HEADING]); connectControl(autoHeading, autoHeadingValid, &autoHeadingError, setpointMoved[HEADING], joystickMode, &heading); connectControl(autoHeading, autoHeadingValid, &autoHeadingError, setpointMoved[HEADING], joystickMode, &hcf); } /* reconfigure for auto heading setpoint moved */ if (modeGroupBits & modeId.setpointMovedBit[HEADING]) { dm_read(controlDmi->setpointMoved[HEADING], (Void *) &setpointMoved[HEADING], sizeof(setpointMoved[HEADING]), (DM_Time *) NULL); dm_disconnect(controlDmi->commandedThrust[HEADING]); connectControl(autoHeading, autoHeadingValid, &autoHeadingError, setpointMoved[HEADING], joystickMode, &heading); connectControl(autoHeading, autoHeadingValid, &autoHeadingError, setpointMoved[HEADING], joystickMode, &hcf); } /* reconfigure for heading source change */ if (modeGroupBits & modeId.kalmanFilterBit) { dm_read(controlDmi->kalmanFilter, (Void *) &kalmanFilter, sizeof(kalmanFilter), (DM_Time *) NULL); connectHeading(kalmanFilter, controlDmi); } /* reconfigure for heading valid change */ if (modeGroupBits & modeId.headingValidBit) { dm_read(controlDmi->headingValid, (Void *) &headingValid, sizeof(headingValid), (DM_Time *) NULL); if ( (autoHeading) && (!headingValid) ) logMsg("Control System WARNING : heading sensor fault\n"); autoHeadingValid = headingValid && headingRateValid; if ((autoHeading) && (headingValid != prevHeadingValid)) { dm_disconnect(controlDmi->commandedThrust[HEADING]); connectControl(autoHeading, autoHeadingValid, &autoHeadingError, setpointMoved[HEADING], joystickMode, &heading); connectControl(autoHeading, autoHeadingValid, &autoHeadingError, setpointMoved[HEADING], joystickMode, &hcf); } prevHeadingValid = headingValid; } /* reconfigure for heading rate valid change */ if (modeGroupBits & modeId.headingRateValidBit) { dm_read(controlDmi->headingRateValid, (Void *) &headingRateValid, sizeof(headingRateValid), (DM_Time *) NULL); if ( (autoHeading) && (!headingRateValid) ) logMsg("Control System WARNING : heading rate sensor fault\n"); autoHeadingValid = headingValid && headingRateValid; if ((autoHeading) && (headingRateValid != prevHeadingRateValid)) { dm_disconnect(controlDmi->commandedThrust[HEADING]); connectControl(autoHeading, autoHeadingValid, &autoHeadingError, setpointMoved[HEADING], joystickMode, &heading); connectControl(autoHeading, autoHeadingValid, &autoHeadingError, setpointMoved[HEADING], joystickMode, &hcf); } prevHeadingRateValid = headingRateValid; } } /* FOREVER */ } /* configureMode */ /******************************************************************************/ /* Function : modeDMInit */ /* Purpose : Starts data manager consumers, creates data manager group, */ /* starts and initializes data manager items and checks status. */ /* Inputs : Control data manager items structure pointer, wakeup semaphore, */ /* configure mode data manager group and group id bits pointers, */ /* and auto control error flag pointers. */ /* Outputs : Returns OK or ERROR. */ /******************************************************************************/ Errno modeDMInit(controlDMItems *controlDmi, SEM_ID modeSem, DM_Group *modeGroup, modeBits *modeId, MBool *autoPositionError, MBool *autoHeadingError, MBool *autoZError) { Errno status, moveStatus[DOF]; /* error codes */ autoCtrlDof dof; /* control dof counter */ DM_Time sampleTime; /* update time */ /* start consumer items */ status = dm_start_consumer(controlDmi->joystickMode, DM_ASYNC, modeSem); if (checkConsumer(status, "Configure Mode - joystick mode", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->autoPosition, DM_ASYNC, modeSem); if (checkConsumer(status, "Configure Mode - auto position flag", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->autoDepth, DM_ASYNC, modeSem); if (checkConsumer(status, "Configure Mode - auto depth flag", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->autoAltitude, DM_ASYNC, modeSem); if (checkConsumer(status, "Configure Mode - auto altitude flag", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->autoHeading, DM_ASYNC, modeSem); if (checkConsumer(status, "Configure Mode - auto heading flag", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->kalmanFilter, DM_ASYNC, modeSem); if (checkConsumer(status, "Configure Mode - Kalman filter flag", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->positionValid, DM_ASYNC, modeSem); if (checkConsumer(status, "Configure Mode - position valid flag", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->depthValid, DM_ASYNC, modeSem); if (checkConsumer(status, "Configure Mode - depth valid flag", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->altitudeValid, DM_ASYNC, modeSem); if (checkConsumer(status, "Configure Mode - altitude valid flag", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->headingValid, DM_ASYNC, modeSem); if (checkConsumer(status, "Configure Mode - heading valid flag", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->headingRateValid, DM_ASYNC, modeSem); if (checkConsumer(status, "Configure Mode - heading rate valid flag", FALSE) == ERROR) return(ERROR); for (dof = X_POSITION; dof <= HEADING; dof++) moveStatus[dof] = dm_start_consumer(controlDmi->setpointMoved[dof], DM_ASYNC, modeSem); if (checkConsumer(moveStatus[X_POSITION], "Configure Mode - x position setpoint moved flag", FALSE) == ERROR) return(ERROR); if (checkConsumer(moveStatus[Y_POSITION], "Configure Mode - y position setpoint moved flag", FALSE) == ERROR) return(ERROR); if (checkConsumer(moveStatus[Z_POSITION], "Configure Mode - z (depth/altitude) setpoint moved flag", FALSE) == ERROR) return(ERROR); if (checkConsumer(moveStatus[HEADING], "Configure Mode - heading setpoint moved flag", FALSE) == ERROR) return(ERROR); /* create data manager group */ *modeGroup = dm_create_group(); /* add data manager items to group */ status = dm_group_add_item(*modeGroup, controlDmi->joystickMode, &modeId->joystickModeBit); if (checkGroup(status, "Configure Mode - joystick mode") == ERROR) return(ERROR); status = dm_group_add_item(*modeGroup, controlDmi->autoPosition, &modeId->autoPositionBit); if (checkGroup(status, "Configure Mode - auto position flag") == ERROR) return(ERROR); status = dm_group_add_item(*modeGroup, controlDmi->autoDepth, &modeId->autoDepthBit); if (checkGroup(status, "Configure Mode - auto depth flag") == ERROR) return(ERROR); status = dm_group_add_item(*modeGroup, controlDmi->autoAltitude, &modeId->autoAltitudeBit); if (checkGroup(status, "Configure Mode - auto altitude flag") == ERROR) return(ERROR); status = dm_group_add_item(*modeGroup, controlDmi->autoHeading, &modeId->autoHeadingBit); if (checkGroup(status, "Configure Mode - auto heading flag") == ERROR) return(ERROR); status = dm_group_add_item(*modeGroup, controlDmi->kalmanFilter, &modeId->kalmanFilterBit); if (checkGroup(status, "Configure Mode - Kalman filter flag") == ERROR) return(ERROR); status = dm_group_add_item(*modeGroup, controlDmi->positionValid, &modeId->positionValidBit); if (checkGroup(status, "Configure Mode - position valid flag") == ERROR) return(ERROR); status = dm_group_add_item(*modeGroup, controlDmi->depthValid, &modeId->depthValidBit); if (checkGroup(status, "Configure Mode - depth valid flag") == ERROR) return(ERROR); status = dm_group_add_item(*modeGroup, controlDmi->altitudeValid, &modeId->altitudeValidBit); if (checkGroup(status, "Configure Mode - altitude valid flag") == ERROR) return(ERROR); status = dm_group_add_item(*modeGroup, controlDmi->headingValid, &modeId->headingValidBit); if (checkGroup(status, "Configure Mode - heading valid flag") == ERROR) return(ERROR); status = dm_group_add_item(*modeGroup, controlDmi->headingRateValid, &modeId->headingRateValidBit); if (checkGroup(status, "Configure Mode - heading rate valid flag") == ERROR) return(ERROR); for (dof = X_POSITION; dof <= HEADING; dof++) moveStatus[dof] = dm_group_add_item(*modeGroup, controlDmi->setpointMoved[dof], &modeId->setpointMovedBit[dof]); if (checkGroup(moveStatus[X_POSITION], "Configure Mode - x position setpoint moved flag") == ERROR) return(ERROR); if (checkGroup(moveStatus[Y_POSITION], "Configure Mode - y position setpoint moved flag") == ERROR) return(ERROR); if (checkGroup(moveStatus[Z_POSITION], "Configure Mode - z (depth/altitude) setpoint moved flag") == ERROR) return(ERROR); if (checkGroup(moveStatus[HEADING], "Configure Mode - heading setpoint moved flag") == ERROR) return(ERROR); /* start provider items */ status = dm_start_provider(controlDmi->autoHeadingError, DM_STATIC); if (checkProvider(status, "Configure Mode - auto heading error") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->autoPositionError, DM_STATIC); if (checkProvider(status, "Configure Mode - auto position error") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->autoZError, DM_STATIC); if (checkProvider(status, "Configure Mode - auto z (depth/altitude) error") == ERROR) return(ERROR); /* initialize provider items */ *autoPositionError = FALSE; *autoHeadingError = FALSE; *autoZError = FALSE; gettimeofday(&sampleTime, (struct timezone *) NULL); status = dm_write(controlDmi->autoPositionError, (Void *) autoPositionError, sizeof(*autoPositionError), &sampleTime); if (checkWrite(status, "Configure Mode - auto position error") == ERROR) return(ERROR); status = dm_write(controlDmi->autoHeadingError, (Void *) autoHeadingError, sizeof(*autoHeadingError), &sampleTime); if (checkWrite(status, "Configure Mode - auto heading error") == ERROR) return(ERROR); status = dm_write(controlDmi->autoZError, (Void *) autoZError, sizeof(*autoZError), &sampleTime); if (checkWrite(status, "Configure Mode - auto z (depth/altitude) error") == ERROR) return(ERROR); return(OK); } /* modeDMInit */ /******************************************************************************/ /* Function : controlModeRead */ /* Purpose : Reads control mode data manager items and checks status. */ /* Inputs : Control data manager items structure pointer; joystick mode */ /* pointer; auto position, auto depth, auto altitude and auto */ /* heading flag pointers; position, depth, altitude, heading and */ /* heading rate valid pointers, setpoint moved flags pointer and */ /* Kalman filter flag. */ /* Outputs : Returns OK or ERROR. */ /******************************************************************************/ Errno controlModeRead(controlDMItems *controlDmi, joystickCmdMode *joystickMode, MBool *autoPosition, MBool *autoDepth, MBool *autoAltitude, MBool *autoHeading, MBool *positionValid, MBool *depthValid, MBool *altitudeValid, MBool *headingValid, MBool *headingRateValid, MBool setpointMoved[], MBool *kalmanFilter) { Errno status, moveStatus[DOF]; /* error codes */ autoCtrlDof dof; /* control dof counter */ status = dm_read(controlDmi->joystickMode, (Void *) joystickMode, sizeof(*joystickMode), (DM_Time *) NULL); if (checkRead(status, "Configure Mode - joystick mode", TRUE) == ERROR) return(ERROR); status = dm_read(controlDmi->autoPosition, (Void *) autoPosition, sizeof(*autoPosition), (DM_Time *) NULL); if (checkRead(status, "Configure Mode - auto position flag", TRUE) == ERROR) return(ERROR); status = dm_read(controlDmi->autoDepth, (Void *) autoDepth, sizeof(*autoDepth), (DM_Time *) NULL); if (checkRead(status, "Configure Mode - auto depth flag", TRUE) == ERROR) return(ERROR); status = dm_read(controlDmi->autoAltitude, (Void *) autoAltitude, sizeof(*autoAltitude), (DM_Time *) NULL); if (checkRead(status, "Configure Mode - auto altitude flag", TRUE) == ERROR) return(ERROR); status = dm_read(controlDmi->autoHeading, (Void *) autoHeading, sizeof(*autoHeading), (DM_Time *) NULL); if (checkRead(status, "Configure Mode - auto heading flag", TRUE) == ERROR) return(ERROR); status = dm_read(controlDmi->positionValid, (Void *) positionValid, sizeof(*positionValid), (DM_Time *) NULL); if (checkRead(status, "Configure Mode - position valid", TRUE) == ERROR) return(ERROR); status = dm_read(controlDmi->depthValid, (Void *) depthValid, sizeof(*depthValid), (DM_Time *) NULL); if (checkRead(status, "Configure Mode - depth valid", TRUE) == ERROR) return(ERROR); status = dm_read(controlDmi->altitudeValid, (Void *) altitudeValid, sizeof(*altitudeValid), (DM_Time *) NULL); if (checkRead(status, "Configure Mode - altitude valid", TRUE) == ERROR) return(ERROR); status = dm_read(controlDmi->headingValid, (Void *) headingValid, sizeof(*headingValid), (DM_Time *) NULL); if (checkRead(status, "Configure Mode - heading valid", TRUE) == ERROR) return(ERROR); status = dm_read(controlDmi->headingRateValid, (Void *) headingRateValid, sizeof(*headingRateValid), (DM_Time *) NULL); if (checkRead(status, "Configure Mode - heading rate valid", TRUE) == ERROR) return(ERROR); for (dof = X_POSITION; dof <= HEADING; dof++) moveStatus[dof] = dm_read(controlDmi->setpointMoved[dof], (Void *) &setpointMoved[dof], sizeof(setpointMoved[dof]), (DM_Time *) NULL); if (checkRead(moveStatus[X_POSITION], "Configure Mode - x setpoint moved flag", TRUE) == ERROR) return(ERROR); if (checkRead(moveStatus[Y_POSITION], "Configure Mode - y setpoint moved flag", TRUE) == ERROR) return(ERROR); if (checkRead(moveStatus[Z_POSITION], "Configure Mode - z setpoint moved flag", TRUE) == ERROR) return(ERROR); if (checkRead(moveStatus[HEADING], "Configure Mode - heading setpoint moved flag", TRUE) == ERROR) return(ERROR); status = dm_read(controlDmi->kalmanFilter, (Void *) kalmanFilter, sizeof(*kalmanFilter), (DM_Time *) NULL); if (checkRead(status, "Configure Mode - Kalman filter flag", TRUE) == ERROR) return(ERROR); return(OK); } /* controlModeRead */ /******************************************************************************/ /* Function : initControlDmi */ /* Purpose : Initializes control mode connection data manager items. */ /* Inputs : Control data manager items structure pointer; x, y, z position */ /* and heading control mode connection data manager items. */ /* Outputs : None. */ /******************************************************************************/ Void initControlDmi(controlDMItems *controlDmi, controlModeDmi *xPosition, controlModeDmi *yPosition, controlModeDmi *zPosition, controlModeDmi *heading, controlModeDmi *hcf) { xPosition->dmiAutoError = controlDmi->autoPositionError; xPosition->dmiCommandedThrust = controlDmi->commandedThrust[X_POSITION]; xPosition->dmiServoThrust = controlDmi->servoThrust[X_POSITION]; xPosition->dmiModelThrust = controlDmi->modelThrustX; xPosition->dmiThrustCommand = controlDmi->thrustCommandX; xPosition->servoErrMsg = "Configure Mode - servo thrust x"; xPosition->modelErrMsg = "Configure Mode - model thrust x"; xPosition->thrustErrMsg = "Configure Mode - thrust command x"; yPosition->dmiAutoError = controlDmi->autoPositionError; yPosition->dmiCommandedThrust = controlDmi->commandedThrust[Y_POSITION]; yPosition->dmiServoThrust = controlDmi->servoThrust[Y_POSITION]; yPosition->dmiModelThrust = controlDmi->modelThrustY; yPosition->dmiThrustCommand = controlDmi->thrustCommandY; yPosition->servoErrMsg = "Configure Mode - servo thrust y"; yPosition->modelErrMsg = "Configure Mode - model thrust y"; yPosition->thrustErrMsg = "Configure Mode - thrust command y"; zPosition->dmiAutoError = controlDmi->autoZError; zPosition->dmiCommandedThrust = controlDmi->commandedThrust[Z_POSITION]; zPosition->dmiServoThrust = controlDmi->servoThrust[Z_POSITION]; zPosition->dmiModelThrust = controlDmi->modelThrustZ; zPosition->dmiThrustCommand = controlDmi->thrustCommandZ; zPosition->servoErrMsg = "Configure Mode - servo thrust z"; zPosition->modelErrMsg = "Configure Mode - model thrust z"; zPosition->thrustErrMsg = "Configure Mode - thrust command z"; heading->dmiAutoError = controlDmi->autoHeadingError; heading->dmiCommandedThrust = controlDmi->commandedThrust[HEADING]; heading->dmiServoThrust = controlDmi->servoThrust[HEADING]; heading->dmiModelThrust = controlDmi->modelThrustHeading; heading->dmiThrustCommand = controlDmi->thrustCommandHeading; heading->servoErrMsg = "Configure Mode - servo thrust heading"; heading->modelErrMsg = "Configure Mode - model thrust heading"; heading->thrustErrMsg = "Configure Mode - thrust command heading"; hcf->dmiAutoError = controlDmi->autoHeadingError; hcf->dmiCommandedThrust = controlDmi->commandedThrustHCF; hcf->dmiServoThrust = controlDmi->servoThrustHCF; hcf->dmiModelThrust = controlDmi->modelThrustHCF; hcf->dmiThrustCommand = controlDmi->thrustCommandHCF; hcf->servoErrMsg = "Configure Mode - servo thrust heading conversion factor"; hcf->modelErrMsg = "Configure Mode - model thrust heading conversion factor"; hcf->thrustErrMsg = "Configure Mode - thrust command heading conversion factor"; } /* initControlDmi */ /******************************************************************************/ /* Function : connectHeading */ /* Purpose : Configures sensor heading, sensor heading rate, heading valid */ /* and heading rate valid connections depending on choice of gyro */ /* or Kalman filter sensor. */ /* Inputs : Kalman filter flag and control data manager items structure */ /* pointer. */ /* Outputs : None. */ /******************************************************************************/ Void connectHeading(MBool kalmanFilter, controlDMItems *dmi) { if (kalmanFilter) /* Kalman filter source */ { connectDmi(dmi->kalmanHeading, dmi->sensorHeading, "Configure Mode - heading sensor"); connectDmi(dmi->kalmanRateHeading, dmi->sensorRateHeading, "Configure Mode - heading rate sensor"); connectDmi(dmi->kalmanHeadingDataValid, dmi->headingValid, "Configure Mode - heading valid"); connectDmi(dmi->kalmanHeadingDataValid, dmi->headingRateValid, "Configure Mode - heading rate valid"); connectDmi(dmi->setpointKalmanHeading, dmi->setpointHeading, "Configure Mode - setpoint heading"); } else { connectDmi(dmi->gyroRadians, dmi->sensorHeading, "Configure Mode - heading sensor"); connectDmi(dmi->gyroRateRadians, dmi->sensorRateHeading, "Configure Mode - heading rate sensor"); connectDmi(dmi->gyroDataValid, dmi->headingValid, "Configure Mode - heading valid"); connectDmi(dmi->gyroDataValid, dmi->headingRateValid, "Configure Mode - heading rate valid"); connectDmi(dmi->setpointGyro, dmi->setpointHeading, "Configure Mode - setpoint heading"); } return; } /* connectHeading */ /******************************************************************************/ /* Function : connectZ */ /* Purpose : Configures auto z flag, sensor z and sensor z rate connections */ /* depending on choice of depth or altitude sensor for auto z */ /* control mode. */ /* Inputs : Auto depth control flag, auto altitude control flag and control */ /* data manager items structure pointer. */ /* Outputs : None. */ /******************************************************************************/ Void connectZ(MBool autoDepth, MBool autoAltitude, controlDMItems *dmi) { if ((autoAltitude) && (!autoDepth)) /* auto altitude */ { connectDmi(dmi->autoAltitude, dmi->autoZ, "Configure Mode - auto altitude"); connectDmi(dmi->sensorAltitude, dmi->sensorZ, "Configure Mode - altitude sensor"); connectDmi(dmi->sensorRateAltitude, dmi->sensorRateZ, "Configure Mode - altitude sensor rate"); dm_disconnect(dmi->autoDepthError); connectDmi(dmi->autoZError, dmi->autoAltitudeError, "Configure Mode - altitude error"); connectDmi(dmi->setpointAltitude, dmi->setpointZ, "Configure Mode - altitude setpoint"); connectDmi(dmi->altitudeGain[PROPORTIONAL], dmi->proportionalGain[Z_INDEX], "Configure Mode - altitude proportional gain"); connectDmi(dmi->altitudeGain[INTEGRAL], dmi->integralGain[Z_INDEX], "Configure Mode - altitude integral gain"); connectDmi(dmi->altitudeGain[DERIVATIVE], dmi->derivativeGain[Z_INDEX], "Configure Mode - altitude derivative gain"); } else /* auto depth or default */ { connectDmi(dmi->autoDepth, dmi->autoZ, "Configure Mode - auto depth"); connectDmi(dmi->sensorDepth, dmi->sensorZ, "Configure Mode - depth sensor"); connectDmi(dmi->sensorRateDepth, dmi->sensorRateZ, "Configure Mode - depth sensor rate"); dm_disconnect(dmi->autoAltitudeError); connectDmi(dmi->autoZError, dmi->autoDepthError, "Configure Mode - depth error"); connectDmi(dmi->setpointDepth, dmi->setpointZ, "Configure Mode - depth setpoint"); connectDmi(dmi->depthGain[PROPORTIONAL], dmi->proportionalGain[Z_INDEX], "Configure Mode - depth proportional gain"); connectDmi(dmi->depthGain[INTEGRAL], dmi->integralGain[Z_INDEX], "Configure Mode - depth integral gain"); connectDmi(dmi->depthGain[DERIVATIVE], dmi->derivativeGain[Z_INDEX], "Configure Mode - depth derivative gain"); } return; } /* connectZ */ /******************************************************************************/ /* Function : connectControl */ /* Purpose : Configures thrust or heading conversion factor (HCF) connections*/ /* for either open or closed loop control, depending on control */ /* mode and setpoint status. */ /* Inputs : Auto control flag, sensor valid flag, auto control error flag */ /* setpoint moved flag, joystick mode and connect mode data manager*/ /* items structure pointer. */ /* Outputs : None. */ /******************************************************************************/ Void connectControl(MBool autoControl, MBool sensorValid, MBool *autoControlError, MBool setpointMovedFlag, joystickCmdMode joystickMode, controlModeDmi *connect) { DM_Time sampleTime; if ((autoControl) && (!setpointMovedFlag)) /* closed loop control */ { if (sensorValid) /* no control error */ { if (*autoControlError) { *autoControlError = FALSE; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(connect->dmiAutoError, (Void *) autoControlError, sizeof(*autoControlError), &sampleTime); } connectDmi(connect->dmiServoThrust, connect->dmiCommandedThrust, connect->servoErrMsg); } else /* control error */ { if ( !(*autoControlError) ) { *autoControlError = TRUE; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(connect->dmiAutoError, (Void *) autoControlError, sizeof(*autoControlError), &sampleTime); } if (joystickMode == SPEED) /* open loop control, */ /* velocity commanded */ connectDmi(connect->dmiModelThrust, connect->dmiCommandedThrust, connect->modelErrMsg); else /* joystickMode == THRUST, */ /* open loop control, */ /* thrust commanded */ connectDmi(connect->dmiThrustCommand, connect->dmiCommandedThrust, connect->thrustErrMsg); } } else if (joystickMode == SPEED) /* open loop control, */ /* velocity commanded */ connectDmi(connect->dmiModelThrust, connect->dmiCommandedThrust, connect->modelErrMsg); else /* joystickMode == THRUST, */ /* open loop control, */ /* thrust commanded */ connectDmi(connect->dmiThrustCommand, connect->dmiCommandedThrust, connect->thrustErrMsg); } /* connectControl */ /******************************************************************************/ /* Function : connectOpenLoop */ /* Purpose : Configures thrust or heading conversion factor (HCF) connections*/ /* for open loop control, depending on control mode and setpoint */ /* status. */ /* Inputs : Joystick mode, auto control flag, sensor valid flag, setpoint */ /* moved flag and connect mode data manager items structure */ /* pointer. */ /* Outputs : None. */ /******************************************************************************/ Void connectOpenLoop(joystickCmdMode joystickMode, MBool autoControl, MBool sensorValid, MBool setpointMovedFlag, controlModeDmi *connect) { if ( !( ((autoControl) && (sensorValid)) && (!setpointMovedFlag) ) ) { /* not closed loop control */ dm_disconnect(connect->dmiCommandedThrust); if (joystickMode == SPEED) /* use dynamic model thrust */ /* commands */ connectDmi(connect->dmiModelThrust, connect->dmiCommandedThrust, connect->modelErrMsg); else /* joystickMode == THRUST, */ /* use joystick thrust */ /* commands */ connectDmi(connect->dmiThrustCommand, connect->dmiCommandedThrust, connect->thrustErrMsg); } } /* connectOpenLoop */ /******************************************************************************/ /* Function : connectDmi */ /* Purpose : Connects source to destination data manager item. */ /* Inputs : Source and destination data manager items, and error message. */ /* Outputs : None. */ /******************************************************************************/ Void connectDmi(DM_Item source, DM_Item destination, char *errorMsg) { Errno status; /* error code */ status = dm_connect(source, destination); if (status == EDM_ISCONNECTED) { status = dm_disconnect(destination); status = dm_connect(source, destination); } if (checkConnect(status, errorMsg) == ERROR) { controlShutDown("tconfigureMode"); } } /* connectDmi */