/******************************************************************************/ /* Copyright 1992 MBARI */ /******************************************************************************/ /* Summary : Initialization Module for Tiburon Control System */ /* Filename : control.c */ /* Author : Janice Tarrant */ /* Project : Tiburon */ /* Version : Version 1.0 */ /* Created : 06/18/92 */ /* Modified : 10/14/96 */ /* Archived : */ /******************************************************************************/ /* Modification History : */ /* $Header: control.c,v 1.1 97/12/04 15:28:00 oreilly Exp $ * $Log: control.c,v $ * Revision 1.1 97/12/04 15:28:00 15:28:00 oreilly (Thomas C. O'Reilly) * Initial revision * * */ /******************************************************************************/ #include /* VxWorks system declarations */ #include /* VxWorks standard I/O library */ #include /* VxWorks watchdog timer library */ #include /* VxWorks semaphore library */ #include /* VxWorks system time declarations */ #include /* VxWorks string library */ #include /* VxWorks task library */ #include /* MBARI style guide type declarations */ #include /* MBARI time declarations */ #include /* power manager data manager definitions */ #include /* data manager declarations */ #include /* data manager error declarations */ #include /* control system data manager definitions */ #include /* SHARPS data manager definitions */ #include /* thruster definitions */ #include /* sensor definitions */ #include /* motion pak definitions */ #include /* kalman filter definitions */ #include /* VME IBC definitions */ #include /* vehicle turns definitions */ #include "control.h" /* control system definitions */ #include "servo.h" /* servo control definitions */ #include "westernFlyer.h" /* function prototypes */ LOCAL Errno controlDMInit(controlDMItems *controlDmi); controlDMItems controlDmi; /* data manager items structure */ /* not in final version */ FILE *fpw; /* file pointer for data logging */ /******************************************************************************/ /* Function : controlTask */ /* Purpose : Initializes control system and spawns eight tasks : */ /* Configure Mode - configures data manager items connections, */ /* Sensor Filter - determines sensor measurements, /* Trajectory Plan - provides commanded trajectory setpoints, */ /* Interface Command - interprets operator commands as velocities */ /* or thrusts, */ /* Dynamic Model - converts velocities to thrusts, */ /* Servo Control - provides PD control thrusts, */ /* Thruster Map - maps vehicle thrusts to thruster thrusts, */ /* Thruster Control - generates motor control thrusts. */ /* Inputs : None. */ /* Outputs : Returns ERROR on fatal error. */ /******************************************************************************/ STATUS controlTask(Void) { Int32 sensorFilterId; /* sensor filter task id */ Int32 trajectoryPlanId; /* trajectory plan task id */ Int32 configureModeId; /* configure mode task id */ Int32 interfaceCommandId; /* interface command task id */ Int32 dynamicModelId; /* dynamic model task id */ Int32 servoControlId; /* servo control task id */ Int32 thrusterMapId; /* thruster map task id */ Int32 thrusterControlId; /* thruster control task id */ /* set control task priority */ taskPrioritySet(taskIdSelf(), CONTROL_PRIORITY); /* open control data file */ #ifdef ROV_SIMULATION if ((fpw = fopen("/users/vxworks/control.out", "w")) == NULL) { logMsg("error opening control.out\n"); return(ERROR); } #endif /* create data manager items */ if (controlDMInit(&controlDmi) == ERROR) return(ERROR); #if 1 /* configure command and control mode connections */ if ((configureModeId = taskSpawn("tconfigureMode", CONFIGURE_MODE_PRIORITY, VX_DEALLOC_STACK | VX_FP_TASK, CONFIGURE_MODE_STACKSIZE, (FUNCPTR) configureMode, (int) &controlDmi, 0, 0, 0, 0, 0, 0, 0, 0, 0)) == ERROR) { logMsg("Control System FAILURE :\ncould not spawn Configure Mode task\n"); controlShutDown("controlTask"); } #endif /* filter sensor measurements */ if ((sensorFilterId = taskSpawn("tsensorFilter", SENSOR_FILTER_PRIORITY, VX_DEALLOC_STACK | VX_FP_TASK, SENSOR_FILTER_STACKSIZE, (FUNCPTR) sensorFilter, (int) &controlDmi, 0, 0, 0, 0, 0, 0, 0, 0, 0)) == ERROR) { logMsg("Control System FAILURE :\ncould not spawn Sensor Filter task\n"); controlShutDown("controlTask"); } /* determine trajectory */ if ((trajectoryPlanId = taskSpawn("ttrajectoryPlan", TRAJECTORY_PLAN_PRIORITY, VX_DEALLOC_STACK | VX_FP_TASK, TRAJECTORY_PLAN_STACKSIZE, (FUNCPTR) trajectoryPlan, (int) &controlDmi, 0, 0, 0, 0, 0, 0, 0, 0, 0)) == ERROR) { logMsg("Control System FAILURE :\ncould not spawn Trajectory Plan task\n"); controlShutDown("controlTask"); } /* determine user interface commands */ if ((interfaceCommandId = taskSpawn("tinterfaceCommand", INTERFACE_COMMAND_PRIORITY, VX_DEALLOC_STACK | VX_FP_TASK, INTERFACE_COMMAND_STACKSIZE, (FUNCPTR) interfaceCommand, (int) &controlDmi, 0, 0, 0, 0, 0, 0, 0, 0, 0)) == ERROR) { logMsg("Control System FAILURE :\ncould not spawn Interface Command task\n"); controlShutDown("controlTask"); } /* determine thrust commands */ if ((dynamicModelId = taskSpawn("tdynamicModel", DYNAMIC_MODEL_PRIORITY, VX_DEALLOC_STACK | VX_FP_TASK, DYNAMIC_MODEL_STACKSIZE, (FUNCPTR) dynamicModel, (int) &controlDmi, 0, 0, 0, 0, 0, 0, 0, 0, 0)) == ERROR) { logMsg("Control System FAILURE :\ncould not spawn Dynamic Model task\n"); controlShutDown("controlTask"); } /* determine servo control */ if ((servoControlId = taskSpawn("tservoControl", SERVO_CONTROL_PRIORITY, VX_DEALLOC_STACK | VX_FP_TASK, SERVO_CONTROL_STACKSIZE, (FUNCPTR) servoControl, (int) &controlDmi, 0, 0, 0, 0, 0, 0, 0, 0, 0)) == ERROR) { logMsg("Control System FAILURE :\ncould not spawn Servo Control task\n"); controlShutDown("controlTask"); } /* determine control thrust */ if ((thrusterMapId = taskSpawn("tthrusterMap", THRUSTER_MAP_PRIORITY, VX_DEALLOC_STACK | VX_FP_TASK, THRUSTER_MAP_STACKSIZE, (FUNCPTR) thrusterMap, (int) &controlDmi, 0, 0, 0, 0, 0, 0, 0, 0, 0)) == ERROR) { logMsg("Control System FAILURE :\ncould not spawn Thruster Map task\n"); controlShutDown("controlTask"); } /* determine thruster control thrust */ if ((thrusterControlId = taskSpawn("tthrusterControl", THRUSTER_CONTROL_PRIORITY, VX_DEALLOC_STACK | VX_FP_TASK, THRUSTER_CONTROL_STACKSIZE, (FUNCPTR) thrusterControl, (int) &controlDmi, 0, 0, 0, 0, 0, 0, 0, 0, 0)) == ERROR) { logMsg("Control System FAILURE :\ncould not spawn Thruster Control task\n"); controlShutDown("controlTask"); } return; } /* controlTask */ /******************************************************************************/ /* Function : controlDMInit */ /* Purpose : Creates control data manager items. */ /* Inputs : Control data manager items structure pointer. */ /* Outputs : Returns data manager error code. */ /******************************************************************************/ LOCAL Errno controlDMInit(controlDMItems *controlDmi) { char *thrusterDMPrefix[] = /* data manager thruster item name */ { PORT_HORIZ_THRUSTER_DM, /* prefix */ STBD_HORIZ_THRUSTER_DM, FWD_LATERAL_THRUSTER_DM, AFT_LATERAL_THRUSTER_DM, PORT_VERT_THRUSTER_DM, STBD_VERT_THRUSTER_DM }; /* data manager thruster item name */ char *thrusterDMName; /* motor enable item name */ MLocal char motorEnableName[NUM_THRUSTERS][MAX_NMLEN]; /* motor stall alarm item name */ MLocal char motorStallName[NUM_THRUSTERS][MAX_NMLEN]; /* motor power item name */ MLocal char motorPowerName[NUM_THRUSTERS][MAX_NMLEN]; /* motor thrust item name */ MLocal char motorThrustName[NUM_THRUSTERS][MAX_NMLEN]; /* data manager sensor item name */ /* prefix */ char sensorDMPrefix[] = SENSOR_DM_PREFIX; /* data manager sensor item name */ char *sensorDMName; /* gyro radians item name */ MLocal char gyroRadiansName[MAX_NMLEN]; /* gyro rate radians item name */ MLocal char gyroRateRadiansName[MAX_NMLEN]; /* gyro data valid item name */ MLocal char gyroDataValidName[MAX_NMLEN]; /* gain messages */ MLocal char *depthGainMsg[PID_GAINS] = { "Control System - proportional depth gain", "Control System - integral depth gain", "Control System - derivative depth gain" }; MLocal char *altitudeGainMsg[PID_GAINS] = { "Control System - proportional altitude gain", "Control System - integral altitude gain", "Control System - derivative altitude gain" }; Nat32 proportionalGains[] = /* default proportional gains */ { X_PROPORTIONAL_GAIN, Y_PROPORTIONAL_GAIN, DEPTH_PROPORTIONAL_GAIN, ALTITUDE_PROPORTIONAL_GAIN, HEADING_PROPORTIONAL_GAIN }; Nat32 integralGains[] = /* default integral gains */ { X_INTEGRAL_GAIN, Y_INTEGRAL_GAIN, DEPTH_INTEGRAL_GAIN, ALTITUDE_INTEGRAL_GAIN, HEADING_INTEGRAL_GAIN }; Nat32 derivativeGains[] = /* default derivative gains */ { X_DERIVATIVE_GAIN, Y_DERIVATIVE_GAIN, DEPTH_DERIVATIVE_GAIN, ALTITUDE_DERIVATIVE_GAIN, HEADING_DERIVATIVE_GAIN }; Nat32 depthGains[] = /* default depth gains */ { DEPTH_PROPORTIONAL_GAIN, DEPTH_INTEGRAL_GAIN, DEPTH_DERIVATIVE_GAIN }; Nat32 altitudeGains[] = /* default altitude gains */ { ALTITUDE_PROPORTIONAL_GAIN, ALTITUDE_INTEGRAL_GAIN, ALTITUDE_DERIVATIVE_GAIN }; MBool adaptiveZ = FALSE; /* adaptive z servo flag */ MBool kalmanFilter = FALSE; /* kalman filter flag */ MBool headingDisableTransfer /* transfer heading on disable flag */ = TRUE; Int16 xThrustThreshold /* thrust threshold for heading */ = MAX_THRUST; /* transfer */ thrusterId thruster; /* thruster initialization counter */ Int16 item; /* data manager item counter */ Int16 gain; /* gain counter */ DM_Num dof; /* degree of freedom counter */ Errno status; /* data manager error code */ DM_Time sampleTime; /* sample time */ struct /* data manager items create values */ { char *name; /* data manager array name */ DM_Type type; /* data manager item type */ DM_Num num; /* number of data manager items */ DM_Item *item; /* data manager item handle pointer */ } dmiCreate[] = { { JOYSTICK_X_VALUE_DM, DM_INT16, 1, &controlDmi->joystickX }, { JOYSTICK_Y_VALUE_DM, DM_INT16, 1, &controlDmi->joystickY }, { JOYSTICK_Z_VALUE_DM, DM_INT16, 1, &controlDmi->joystickZ }, { JOYSTICK_HEADING_VALUE_DM, DM_INT16, 1, &controlDmi->joystickHeading }, { JOYSTICK_GAIN_SWITCH_DM, DM_MBOOL, 1, &controlDmi->joystickGainSwitch }, { JOYSTICK_GAIN_DM, DM_NAT16, 1, &controlDmi->joystickGain }, { JOYSTICK_MODE_DM, DM_ENUM, 1, &controlDmi->joystickMode }, { SHARPS_X_DM, DM_FLT32, 1, &controlDmi->sharpsX }, { SHARPS_Y_DM, DM_FLT32, 1, &controlDmi->sharpsY }, { SHARPS_Z_DM, DM_FLT32, 1, &controlDmi->sharpsZ}, { SHARPS_RATE_X_DM, DM_FLT32, 1, &controlDmi->sharpsRateX }, { SHARPS_RATE_Y_DM, DM_FLT32, 1, &controlDmi->sharpsRateY }, { SHARPS_RATE_Z_DM, DM_FLT32, 1, &controlDmi->sharpsRateZ }, { gyroRadiansName, DM_FLT32, 1, &controlDmi->gyroRadians }, { gyroRateRadiansName, DM_FLT32, 1, &controlDmi->gyroRateRadians }, { VEHICLE_DEPTH_DM, DM_FLT64, 1, &controlDmi->depth }, { VEHICLE_ALTITUDE_DM, DM_FLT32, 1, &controlDmi->altitude }, { KALMAN_HEADING_DM, DM_FLT32, 1, &controlDmi->kalmanHeading }, { KALMAN_HEADING_DEGREES_DM, DM_FLT32, 1, &controlDmi->kalmanHeadingDegrees }, { KALMAN_HEADING_RATE_DM, DM_FLT32, 1, &controlDmi->kalmanRateHeading }, { SHARPS_DATA_VALID_DM, DM_MBOOL, 1, &controlDmi->sharpsDataValid }, { gyroDataValidName, DM_MBOOL, 1, &controlDmi->gyroDataValid }, { VEHICLE_DEPTH_DATA_VALID_DM, DM_MBOOL, 1, &controlDmi->depthDataValid }, { VEHICLE_ALTITUDE_DATA_VALID_DM, DM_MBOOL, 1, &controlDmi->altimeterDataValid }, { KALMAN_HEADING_DATA_VALID_DM, DM_MBOOL, 1, &controlDmi->kalmanHeadingDataValid }, { SENSOR_X_DM, DM_FLT32, 1, &controlDmi->sensorX }, { SENSOR_Y_DM, DM_FLT32, 1, &controlDmi->sensorY }, { SENSOR_SHARPS_Z_DM, DM_FLT32, 1, &controlDmi->sensorSharpsZ }, { SENSOR_DEPTH_DM, DM_FLT32, 1, &controlDmi->sensorDepth }, { SENSOR_ALTITUDE_DM, DM_FLT32, 1, &controlDmi->sensorAltitude }, { SENSOR_Z_DM, DM_FLT32, 1, &controlDmi->sensorZ }, { SENSOR_HEADING_DM, DM_FLT32, 1, &controlDmi->sensorHeading }, { SENSOR_RATE_X_DM, DM_FLT32, 1, &controlDmi->sensorRateX }, { SENSOR_RATE_Y_DM, DM_FLT32, 1, &controlDmi->sensorRateY }, { SENSOR_RATE_SHARPS_Z_DM, DM_FLT32, 1, &controlDmi->sensorRateSharpsZ }, { SENSOR_RATE_DEPTH_DM, DM_FLT32, 1, &controlDmi->sensorRateDepth }, { SENSOR_RATE_ALTITUDE_DM, DM_FLT32, 1, &controlDmi->sensorRateAltitude }, { SENSOR_RATE_Z_DM, DM_FLT32, 1, &controlDmi->sensorRateZ }, { SENSOR_RATE_HEADING_DM, DM_FLT32, 1, &controlDmi->sensorRateHeading }, { VEHICLE_TURNS_COUNT_DM, DM_INT16, 1, &controlDmi->turnsCount }, { VEHICLE_TURNS_SUM_DM, DM_FLT32, 1, &controlDmi->turnsSum }, { INITIAL_VEHICLE_TURNS_SUM_DM, DM_FLT32, 1, &controlDmi->initialTurnsSum }, { ROV_SHIP_LAUNCH_ANGLE_DM, DM_FLT32, 1, &controlDmi->launchAngle }, { VEHICLE_TURNS_SETPOINT_DM, DM_FLT32, 1, &controlDmi->turnsSetpoint }, { RESET_VEHICLE_TURNS_COUNT_DM, DM_MBOOL, 1, &controlDmi->resetTurns }, { AUTO_POSITION_DM, DM_MBOOL, 1, &controlDmi->autoPosition }, { AUTO_DEPTH_DM, DM_MBOOL, 1, &controlDmi->autoDepth }, { AUTO_ALTITUDE_DM, DM_MBOOL, 1, &controlDmi->autoAltitude }, { AUTO_Z_DM, DM_MBOOL, 1, &controlDmi->autoZ }, { AUTO_HEADING_DM, DM_MBOOL, 1, &controlDmi->autoHeading }, { HEADING_VALID_DM, DM_MBOOL, 1, &controlDmi->headingValid }, { HEADING_RATE_VALID_DM, DM_MBOOL, 1, &controlDmi->headingRateValid }, { DEPTH_VALID_DM, DM_MBOOL, 1, &controlDmi->depthValid }, { ALTITUDE_VALID_DM, DM_MBOOL, 1, &controlDmi->altitudeValid }, { POSITION_VALID_DM, DM_MBOOL, 1, &controlDmi->positionValid }, { RATE_VALID_DM, DM_MBOOL, 1, &controlDmi->rateValid }, { AUTO_HEADING_ERROR_DM, DM_MBOOL, 1, &controlDmi->autoHeadingError }, { AUTO_DEPTH_ERROR_DM, DM_MBOOL, 1, &controlDmi->autoDepthError }, { AUTO_ALTITUDE_ERROR_DM, DM_MBOOL, 1, &controlDmi->autoAltitudeError }, { AUTO_POSITION_ERROR_DM, DM_MBOOL, 1, &controlDmi->autoPositionError }, { AUTO_Z_ERROR_DM, DM_MBOOL, 1, &controlDmi->autoZError }, { SETPOINT_X_DM, DM_FLT32, 1, &controlDmi->setpointX }, { SETPOINT_Y_DM, DM_FLT32, 1, &controlDmi->setpointY }, { SETPOINT_DEPTH_DM, DM_FLT32, 1, &controlDmi->setpointDepth }, { SETPOINT_ALTITUDE_DM, DM_FLT32, 1, &controlDmi->setpointAltitude }, { SETPOINT_Z_DM, DM_FLT32, 1, &controlDmi->setpointZ }, { SETPOINT_GYRO_DM, DM_FLT32, 1, &controlDmi->setpointGyro }, { SETPOINT_KALMAN_HEADING_DM, DM_FLT32, 1, &controlDmi->setpointKalmanHeading }, { SETPOINT_KALMAN_HEADING_DEGREES_DM, DM_FLT32, 1, &controlDmi->setpointKalmanHeadingDegrees }, { SETPOINT_HEADING_DM, DM_FLT32, 1, &controlDmi->setpointHeading }, { SETPOINT_ERROR_HEADING_DM, DM_FLT32, 1, &controlDmi->setpointErrorHeading }, { SETPOINT_MOVED_DM, DM_MBOOL, DOF, controlDmi->setpointMoved }, { VELOCITY_X_COMMAND_DM, DM_FLT32, 1, &controlDmi->velocityCommandX }, { VELOCITY_Y_COMMAND_DM, DM_FLT32, 1, &controlDmi->velocityCommandY }, { VELOCITY_Z_COMMAND_DM, DM_FLT32, 1, &controlDmi->velocityCommandZ }, { VELOCITY_HEADING_COMMAND_DM, DM_FLT32, 1, &controlDmi->velocityCommandHeading }, { THRUST_X_COMMAND_DM, DM_FLT32, 1, &controlDmi->thrustCommandX }, { THRUST_Y_COMMAND_DM, DM_FLT32, 1, &controlDmi->thrustCommandY }, { THRUST_Z_COMMAND_DM, DM_FLT32, 1, &controlDmi->thrustCommandZ }, { THRUST_HEADING_COMMAND_DM, DM_FLT32, 1, &controlDmi->thrustCommandHeading }, { THRUST_HEADING_CONV_FACTOR_DM, DM_FLT32, 1, &controlDmi->thrustCommandHCF }, { MODEL_X_THRUST_DM, DM_FLT32, 1, &controlDmi->modelThrustX }, { MODEL_Y_THRUST_DM, DM_FLT32, 1, &controlDmi->modelThrustY }, { MODEL_Z_THRUST_DM, DM_FLT32, 1, &controlDmi->modelThrustZ }, { MODEL_HEADING_THRUST_DM, DM_FLT32, 1, &controlDmi->modelThrustHeading }, { MODEL_HEADING_CONV_FACTOR_DM, DM_FLT32, 1, &controlDmi->modelThrustHCF }, { PROPORTIONAL_GAIN_DM, DM_NAT32, DOF, controlDmi->proportionalGain }, { INTEGRAL_GAIN_DM, DM_NAT32, DOF, controlDmi->integralGain }, { DERIVATIVE_GAIN_DM, DM_NAT32, DOF, controlDmi->derivativeGain }, { DEPTH_GAIN_DM, DM_NAT32, PID_GAINS, controlDmi->depthGain }, { ALTITUDE_GAIN_DM, DM_NAT32, PID_GAINS, controlDmi->altitudeGain }, { Z_INTEGRAL_DM, DM_FLT32, 1, &controlDmi->zIntegral }, { SERVO_THRUST_DM, DM_FLT32, DOF, controlDmi->servoThrust }, { SERVO_HEADING_CONV_FACTOR_DM, DM_FLT32, 1, &controlDmi->servoThrustHCF }, { COMMANDED_THRUST_DM, DM_FLT32, DOF, controlDmi->commandedThrust }, { COMMANDED_HEADING_CONV_FACTOR_DM, DM_FLT32, 1, &controlDmi->commandedThrustHCF }, { CONTROL_THRUST_DM, DM_FLT32, NUM_THRUSTERS, controlDmi->controlThrust }, { THRUSTERS_PWR_REQ_DM, DM_NAT16, 1, &controlDmi->powerRequest }, { THRUSTERS_PWR_ALLOC_DM, DM_NAT16, 1, &controlDmi->powerAllocated }, { THRUSTERS_PWR_RED_DM, DM_MBOOL, 1, &controlDmi->powerReduced }, { MOTOR_PWR_RED_DM, DM_MBOOL, 1, &controlDmi->motorPowerReduced }, { motorEnableName[PORT_HORIZ], DM_ENUM, 1, &controlDmi->motorEnable[PORT_HORIZ] }, { motorEnableName[STBD_HORIZ], DM_ENUM, 1, &controlDmi->motorEnable[STBD_HORIZ] }, { motorEnableName[FWD_LATERAL], DM_ENUM, 1, &controlDmi->motorEnable[FWD_LATERAL] }, { motorEnableName[AFT_LATERAL], DM_ENUM, 1, &controlDmi->motorEnable[AFT_LATERAL] }, { motorEnableName[PORT_VERT], DM_ENUM, 1, &controlDmi->motorEnable[PORT_VERT] }, { motorEnableName[STBD_VERT], DM_ENUM, 1, &controlDmi->motorEnable[STBD_VERT] }, { motorStallName[PORT_HORIZ], DM_EMPTY, 1, &controlDmi->motorStall[PORT_HORIZ] }, { motorStallName[STBD_HORIZ], DM_EMPTY, 1, &controlDmi->motorStall[STBD_HORIZ] }, { motorStallName[FWD_LATERAL], DM_EMPTY, 1, &controlDmi->motorStall[FWD_LATERAL] }, { motorStallName[AFT_LATERAL], DM_EMPTY, 1, &controlDmi->motorStall[AFT_LATERAL] }, { motorStallName[PORT_VERT], DM_EMPTY, 1, &controlDmi->motorStall[PORT_VERT] }, { motorStallName[STBD_VERT], DM_EMPTY, 1, &controlDmi->motorStall[STBD_VERT] }, { motorPowerName[PORT_HORIZ], DM_INT16, 1, &controlDmi->motorControlPower[PORT_HORIZ] }, { motorPowerName[STBD_HORIZ], DM_INT16, 1, &controlDmi->motorControlPower[STBD_HORIZ] }, { motorPowerName[FWD_LATERAL], DM_INT16, 1, &controlDmi->motorControlPower[FWD_LATERAL] }, { motorPowerName[AFT_LATERAL], DM_INT16, 1, &controlDmi->motorControlPower[AFT_LATERAL] }, { motorPowerName[PORT_VERT], DM_INT16, 1, &controlDmi->motorControlPower[PORT_VERT] }, { motorPowerName[STBD_VERT], DM_INT16, 1, &controlDmi->motorControlPower[STBD_VERT] }, { motorThrustName[PORT_HORIZ], DM_INT16, 1, &controlDmi->motorControlThrust[PORT_HORIZ] }, { motorThrustName[STBD_HORIZ], DM_INT16, 1, &controlDmi->motorControlThrust[STBD_HORIZ] }, { motorThrustName[FWD_LATERAL], DM_INT16, 1, &controlDmi->motorControlThrust[FWD_LATERAL] }, { motorThrustName[AFT_LATERAL], DM_INT16, 1, &controlDmi->motorControlThrust[AFT_LATERAL] }, { motorThrustName[PORT_VERT], DM_INT16, 1, &controlDmi->motorControlThrust[PORT_VERT] }, { motorThrustName[STBD_VERT], DM_INT16, 1, &controlDmi->motorControlThrust[STBD_VERT] }, { WF_DM_PREFIX GYRO_DEGREES_DM, DM_FLT32, 1, &controlDmi->shipHeading }, { NULL, DM_ENDT, 0, NO_ITEM } }; DM_Define dmiInit[] = /* initial control gains */ { { ADAPTIVE_Z_SERVO_DM, 1, DM_MBOOL, DMT_NULL, &adaptiveZ }, { KALMAN_FILTER_DM, 1, DM_MBOOL, DMT_NULL, &kalmanFilter }, { HEADING_DISABLE_TRANSFER_DM, 1, DM_MBOOL, DMT_NULL, &headingDisableTransfer }, { X_THRUST_THRESHOLD_DM, 1, DM_INT16, DMT_NULL, &xThrustThreshold }, { NULL, 0, DM_ENDT, DMT_NULL, NULL } }; /* create sensor data manager item names */ 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); /* create motor enable data manager item name */ thrusterDMName = MOTOR_ENABLE_STATUS_DM; for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++) { strcpy(motorEnableName[thruster], thrusterDMPrefix[thruster]); strcat(motorEnableName[thruster], thrusterDMName); } /* create motor stall alarm data manager item name */ thrusterDMName = MOTOR_STALL_ALARM_DM; for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++) { strcpy(motorStallName[thruster], thrusterDMPrefix[thruster]); strcat(motorStallName[thruster], thrusterDMName); } /* create motor control power data manager item name */ thrusterDMName = MOTOR_CMD_POWER_DM; for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++) { strcpy(motorPowerName[thruster], thrusterDMPrefix[thruster]); strcat(motorPowerName[thruster], thrusterDMName); } /* create motor control thrust data manager item name */ thrusterDMName = MOTOR_CMD_THRUST_DM; for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++) { strcpy(motorThrustName[thruster], thrusterDMPrefix[thruster]); strcat(motorThrustName[thruster], thrusterDMName); } /* 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, FALSE) == ERROR) return(ERROR); } status = dm_create(MOTOR_CONTROL_THRUSTS_DM, 1, &controlDmi->motorControlThrusts, DM_INT16, NUM_THRUSTERS, DM_ENDT); if (checkCreate(status, MOTOR_CONTROL_THRUSTS_DM, FALSE) == ERROR) return(ERROR); status = dm_create(POSITION_DM, 1, &controlDmi->kalmanPosition, DM_FLT32, KALMAN_STATES, DM_ENDT); if (checkCreate(status, POSITION_DM, FALSE) == ERROR) return(ERROR); status = dm_create(POSITION_STATUS_DM, 1, &controlDmi->kalmanPositionStatus, DM_ENUM, KALMAN_STATES, DM_ENDT); if (checkCreate(status, POSITION_STATUS_DM, FALSE) == ERROR) return(ERROR); status = dm_create(ATTITUDE_DM, 1, &controlDmi->kalmanAttitude, DM_FLT32, KALMAN_STATES, DM_ENDT); if (checkCreate(status, ATTITUDE_DM, FALSE) == ERROR) return(ERROR); status = dm_create(ATTITUDE_STATUS_DM, 1, &controlDmi->kalmanAttitudeStatus, DM_ENUM, KALMAN_STATES, DM_ENDT); if (checkCreate(status, ATTITUDE_STATUS_DM, FALSE) == ERROR) return(ERROR); /* initialize adaptive z servo flag, kalman filter flag, heading disable */ /* transfer flag and x thrust threshold data manager items */ status = dm_create_items(dmiInit); if (checkCreate(status,"INITIAL_CONTROL_VALUES", FALSE) == ERROR) return(ERROR); /* get adaptive z servo flag, kalman filter flag, heading disable transfer */ /* flag and x thrust threshold data manager item handles */ if ((controlDmi->adaptiveZServo = dm_lookup(ADAPTIVE_Z_SERVO_DM, 0)) == NO_ITEM) { logMsg("Control System FAILURE : adaptive z servo data manager item does not exist\n"); return(ERROR); } if ((controlDmi->kalmanFilter = dm_lookup(KALMAN_FILTER_DM, 0)) == NO_ITEM) { logMsg("Control System FAILURE : kalman filter data manager item does not exist\n"); return(ERROR); } if ((controlDmi->headingDisableTransfer = dm_lookup(HEADING_DISABLE_TRANSFER_DM, 0)) == NO_ITEM) { logMsg("Control System FAILURE : heading disable transfer data manager item does not exist\n"); return(ERROR); } if ((controlDmi->xThrustThreshold = dm_lookup(X_THRUST_THRESHOLD_DM, 0)) == NO_ITEM) { logMsg("Control System FAILURE : x thrust threshold data manager item does not exist\n"); return(ERROR); } /* start provider for and write initial values of all control gains */ gettimeofday(&sampleTime, (struct timezone *) NULL); status = dm_start_provider(controlDmi->proportionalGain[X_INDEX], DM_ASYNC); if (checkProvider(status, "Control System - x proportional gain") != ERROR) { status = dm_write(controlDmi->proportionalGain[X_INDEX], (Void *) &proportionalGains[X_GAIN], sizeof(proportionalGains[X_GAIN]), &sampleTime); if (checkWrite(status, "Control System - x proportional gain") == ERROR) logMsg("Control System WARNING : could not initialize x proportional gain\n"); } dm_stop_provider(controlDmi->proportionalGain[X_INDEX]); status = dm_start_provider(controlDmi->proportionalGain[Y_INDEX], DM_ASYNC); if (checkProvider(status, "Control System - y proportional gain") != ERROR) { status = dm_write(controlDmi->proportionalGain[Y_INDEX], (Void *) &proportionalGains[Y_GAIN], sizeof(proportionalGains[Y_GAIN]), &sampleTime); if (checkWrite(status, "Control System - y proportional gain") == ERROR) logMsg("Control System WARNING : could not initialize y proportional gain\n"); } dm_stop_provider(controlDmi->proportionalGain[Y_INDEX]); status = dm_start_provider(controlDmi->proportionalGain[HEADING_INDEX], DM_ASYNC); if (checkProvider(status, "Control System - heading proportional gain") != ERROR) { status = dm_write(controlDmi->proportionalGain[HEADING_INDEX], (Void *) &proportionalGains[HEADING_GAIN], sizeof(proportionalGains[HEADING_GAIN]), &sampleTime); if (checkWrite(status, "Control System - heading proportional gain") == ERROR) logMsg("Control System WARNING : could not initialize heading proportional gain\n"); } dm_stop_provider(controlDmi->proportionalGain[HEADING_INDEX]); status = dm_start_provider(controlDmi->integralGain[X_INDEX], DM_ASYNC); if (checkProvider(status, "Control System - x integral gain") != ERROR) { status = dm_write(controlDmi->integralGain[X_INDEX], (Void *) &integralGains[X_GAIN], sizeof(integralGains[X_GAIN]), &sampleTime); if (checkWrite(status, "Control System - x integral gain") == ERROR) logMsg("Control System WARNING : could not initialize x integral gain\n"); } dm_stop_provider(controlDmi->integralGain[X_INDEX]); status = dm_start_provider(controlDmi->integralGain[Y_INDEX], DM_ASYNC); if (checkProvider(status, "Control System - y integral gain") != ERROR) { status = dm_write(controlDmi->integralGain[Y_INDEX], (Void *) &integralGains[Y_GAIN], sizeof(integralGains[Y_GAIN]), &sampleTime); if (checkWrite(status, "Control System - y integral gain") == ERROR) logMsg("Control System WARNING : could not initialize y integral gain\n"); } dm_stop_provider(controlDmi->integralGain[Y_INDEX]); status = dm_start_provider(controlDmi->integralGain[HEADING_INDEX], DM_ASYNC); if (checkProvider(status, "Control System - heading integral gain") != ERROR) { status = dm_write(controlDmi->integralGain[HEADING_INDEX], (Void *) &integralGains[HEADING_GAIN], sizeof(integralGains[HEADING_GAIN]), &sampleTime); if (checkWrite(status, "Control System - heading integral gain") == ERROR) logMsg("Control System WARNING : could not initialize heading integral gain\n"); } dm_stop_provider(controlDmi->integralGain[HEADING_INDEX]); status = dm_start_provider(controlDmi->derivativeGain[X_INDEX], DM_ASYNC); if (checkProvider(status, "Control System - x derivative gain") != ERROR) { status = dm_write(controlDmi->derivativeGain[X_INDEX], (Void *) &derivativeGains[X_GAIN], sizeof(derivativeGains[X_GAIN]), &sampleTime); if (checkWrite(status, "Control System - x derivative gain") == ERROR) logMsg("Control System WARNING : could not initialize x derivative gain\n"); } dm_stop_provider(controlDmi->derivativeGain[X_INDEX]); status = dm_start_provider(controlDmi->derivativeGain[Y_INDEX], DM_ASYNC); if (checkProvider(status, "Control System - y derivative gain") != ERROR) { status = dm_write(controlDmi->derivativeGain[Y_INDEX], (Void *) &derivativeGains[Y_GAIN], sizeof(derivativeGains[Y_GAIN]), &sampleTime); if (checkWrite(status, "Control System - y derivative gain") == ERROR) logMsg("Control System WARNING : could not initialize y derivative gain\n"); } dm_stop_provider(controlDmi->derivativeGain[Y_INDEX]); status = dm_start_provider(controlDmi->derivativeGain[HEADING_INDEX], DM_ASYNC); if (checkProvider(status, "Control System - heading derivative gain") != ERROR) { status = dm_write(controlDmi->derivativeGain[HEADING_INDEX], (Void *) &derivativeGains[HEADING_GAIN], sizeof(derivativeGains[HEADING_GAIN]), &sampleTime); if (checkWrite(status, "Control System - heading derivative gain") == ERROR) logMsg("Control System WARNING : could not initialize heading derivative gain\n"); } dm_stop_provider(controlDmi->derivativeGain[HEADING_INDEX]); for (gain = 0; gain < PID_GAINS; gain++) { status = dm_start_provider(controlDmi->depthGain[gain], DM_ASYNC); if (checkProvider(status, depthGainMsg[gain]) != ERROR) { status = dm_write(controlDmi->depthGain[gain], (Void *) &depthGains[gain], sizeof(depthGains[gain]), &sampleTime); if (checkWrite(status, depthGainMsg[gain]) == ERROR) logMsg("Control System WARNING : could not initialize depth gain %d\n", gain); } dm_stop_provider(controlDmi->depthGain[gain]); status = dm_start_provider(controlDmi->altitudeGain[gain], DM_ASYNC); if (checkProvider(status, altitudeGainMsg[gain]) != ERROR) { status = dm_write(controlDmi->altitudeGain[gain], (Void *) &altitudeGains[gain], sizeof(altitudeGains[gain]), &sampleTime); if (checkWrite(status, altitudeGainMsg[gain]) == ERROR) logMsg("Control System WARNING : could not initialize heading altitude gain\n"); } dm_stop_provider(controlDmi->altitudeGain[gain]); } return(OK); } /* controlDMInit */ /******************************************************************************/ /* Function : controlShutDown */ /* Purpose : Deletes all tasks, generally if control system failure is */ /* detected. */ /* Inputs : Calling task name. */ /* Outputs : None. */ /******************************************************************************/ Void controlShutDown(char *taskName) { Int32 taskId; /* task id */ /* close data file */ /* not in final version */ /* fclose(fpw);*/ /* delete all tasks except calling task */ if (strcmp(taskName, "tconfigureMode") != 0) if ((taskId = taskNameToId("tconfigureMode")) != ERROR) taskDelete(taskId); if (strcmp(taskName, "tsensorFilter") != 0) if ((taskId = taskNameToId("tsensorFilter")) != ERROR) taskDelete(taskId); if (strcmp(taskName, "ttrajectoryPlan") != 0) if ((taskId = taskNameToId("ttrajectoryPlan")) != ERROR) taskDelete(taskId); if (strcmp(taskName, "tinterfaceCommand") != 0) if ((taskId = taskNameToId("tinterfaceCommand")) != ERROR) taskDelete(taskId); if (strcmp(taskName, "tdynamicModel") != 0) if ((taskId = taskNameToId("tdynamicModel")) != ERROR) taskDelete(taskId); if (strcmp(taskName, "tservoControl") != 0) if ((taskId = taskNameToId("tservoControl")) != ERROR) taskDelete(taskId); if (strcmp(taskName, "tthrusterMap") != 0) if ((taskId = taskNameToId("tthrusterMap")) != ERROR) taskDelete(taskId); if (strcmp(taskName, "tthrusterControl") != 0) if ((taskId = taskNameToId("tthrusterControl")) != ERROR) taskDelete(taskId); /* delete calling task */ if ((taskId = taskNameToId(taskName)) != ERROR) taskDelete(taskId); } /* controlShutDown */