/******************************************************************************/ /* Copyright 1992 MBARI */ /******************************************************************************/ /* Summary : Control System Module Definitions for Tiburon */ /* Filename : control.h */ /* Author : Janice Tarrant */ /* Project : Tiburon */ /* Version : Version 1.0 */ /* Created : 06/18/92 */ /* Modified : 10/14/96 */ /* Archived : */ /******************************************************************************/ /* Modification History : */ /* $Header: control.h,v 1.1 97/12/04 15:28:13 oreilly Exp $ * $Log: control.h,v $ * Revision 1.1 97/12/04 15:28:13 15:28:13 oreilly (Thomas C. O'Reilly) * Initial revision * * */ /******************************************************************************/ #ifndef ROV_CONTROL_H #define ROV_CONTROL_H #if 0 /* flag for simulation specific code*/ #define ROV_SIMULATION #endif #define CONTROL_PERIOD 67000 /* control period (microsec) */ /* (15 Hz rate) */ #define NUM_THRUSTERS 6 /* number of thrusters */ #define DOF 4 /* number of degrees of freedom */ #define PID_GAINS 3 /* proportional, integral and */ /* derivative gains */ #define KALMAN_STATES 6 /* mumber of Kalman filter states */ #define CONTROL_PRIORITY 62 /* control task priority */ #define CONFIGURE_MODE_PRIORITY 63 /* configure mode task priority */ #define SENSOR_FILTER_PRIORITY 70 /* sensor filter task priority */ #define INTERFACE_COMMAND_PRIORITY 69 /* interface command task priority */ #define TRAJECTORY_PLAN_PRIORITY 68 /* trajectory plan task priority */ #define DYNAMIC_MODEL_PRIORITY 67 /* dynamic model task priority */ #define SERVO_CONTROL_PRIORITY 66 /* servo control task priority */ #define THRUSTER_MAP_PRIORITY 65 /* thruster map task priority */ #define THRUSTER_CONTROL_PRIORITY 64 /* thruster control task priority */ #define CONFIGURE_MODE_STACKSIZE 5000 /* configure mode task stacksize */ #define SENSOR_FILTER_STACKSIZE 6000 /* sensor filter task stacksize */ #define INTERFACE_COMMAND_STACKSIZE 6000 /* interface command task stacksize */ #define TRAJECTORY_PLAN_STACKSIZE 5000 /* trajectory plan task stacksize */ #define DYNAMIC_MODEL_STACKSIZE 5000 /* dynamic model task stacksize */ #define SERVO_CONTROL_STACKSIZE 5000 /* servo control task stacksize */ #define THRUSTER_MAP_STACKSIZE 6000 /* thruster map task stacksize */ #define THRUSTER_CONTROL_STACKSIZE 6000 /* thruster control task stacksize */ #define DATA_NULL ((Void *) 0) /* null data array */ #define PI 3.14159265358979323846 /* pi */ #define PI_2 1.570796326794897 /* pi/2 */ #define MAX_THRUST 1245 /* maximum thrust (N) per thruster */ /*#define MAX_THRUST 1120*/ /* reduced maximum thrust (N) */ /* data manager operation identifiers */ typedef enum { CONSUMER, PROVIDER, READER, WRITER } dmOpId; /* joystick command modes */ typedef enum { SPEED, THRUST } joystickCmdMode; /* dof and connection indices */ typedef enum { X_INDEX, Y_INDEX, Z_INDEX, HEADING_INDEX, HCF_INDEX } indices; /* gain indices */ typedef enum { X_GAIN, Y_GAIN, DEPTH_GAIN, ALTITUDE_GAIN, HEADING_GAIN } gainDof; /* data manager item handles */ typedef struct { DM_Item joystickX; /* joystick x command value */ DM_Item joystickY; /* joystick y command value */ DM_Item joystickZ; /* joystick z command value */ DM_Item joystickHeading; /* joystick heading command value */ DM_Item joystickGainSwitch; /* joystick gain switch */ DM_Item joystickGain; /* joystick gain */ DM_Item joystickMode; /* joystick command mode */ DM_Item sharpsX; /* sharps x measurement */ DM_Item sharpsY; /* sharps y measurement */ DM_Item sharpsZ; /* sharps z measurement */ DM_Item sharpsRateX; /* sharps x rate measurement */ DM_Item sharpsRateY; /* sharps y rate measurement */ DM_Item sharpsRateZ; /* sharps z rate measurement */ DM_Item gyroRadians; /* gyro heading measurement */ DM_Item gyroRateRadians; /* gyro heading rate measurement */ DM_Item depth; /* depth measurement */ DM_Item altitude; /* altitude measurement */ DM_Item kalmanPosition; /* Kalman filter position */ DM_Item kalmanAttitude; /* Kalman filter attitude */ DM_Item kalmanHeading; /* Kalman filter heading */ DM_Item kalmanHeadingDegrees; /* Kalman filter heading degrees */ DM_Item kalmanRateHeading; /* Kalman filter heading rate */ DM_Item sharpsDataValid; /* sharps data valid flag */ DM_Item gyroDataValid; /* gyro data valid flag */ DM_Item depthDataValid; /* depth data valid flag */ DM_Item altimeterDataValid; /* altimeter data valid flag */ DM_Item kalmanPositionStatus; /* Kalman filter position status */ DM_Item kalmanAttitudeStatus; /* Kalman filter attitude status */ DM_Item kalmanHeadingDataValid; /* Kalman filter heading data valid flag*/ DM_Item sensorX; /* sensor x measurement */ DM_Item sensorY; /* sensor y measurement */ DM_Item sensorSharpsZ; /* sensor z measurement */ DM_Item sensorDepth; /* sensor depth measurement */ DM_Item sensorAltitude; /* sensor altitude measurement */ DM_Item sensorZ; /* sensor z (depth/alt/sharps) msmt */ DM_Item sensorHeading; /* sensor heading measurement */ DM_Item sensorRateX; /* sensor x rate measurement */ DM_Item sensorRateY; /* sensor y rate measurement */ DM_Item sensorRateSharpsZ; /* sensor z rate measurement */ DM_Item sensorRateDepth; /* sensor depth rate measurement */ DM_Item sensorRateAltitude; /* sensor altitude rate measurement */ DM_Item sensorRateZ; /* sensor z (depth/alt/sharps) rate msmt*/ DM_Item sensorRateHeading; /* sensor heading rate measurement */ DM_Item turnsCount; /* rov heading turns count */ DM_Item turnsSum; /* rov heading turns sum */ DM_Item initialTurnsSum; /* initial rov heading turns sum */ DM_Item turnsSetpoint; /* rov heading turns setpoint */ DM_Item resetTurns; /* rov heading reset turns counter */ DM_Item autoPosition; /* auto position (x, y) flag */ DM_Item autoDepth; /* auto depth flag */ DM_Item autoAltitude; /* auto altitude flag */ DM_Item autoZ; /* auto z (depth/altitude) flag */ DM_Item autoHeading; /* auto heading flag */ DM_Item adaptiveZServo; /* adaptive z servo flag */ DM_Item kalmanFilter; /* kalman filter flag */ DM_Item headingValid; /* heading valid flag */ DM_Item headingRateValid; /* heading rate valid flag */ DM_Item depthValid; /* depth valid */ DM_Item altitudeValid; /* altitude valid */ DM_Item positionValid; /* position valid */ DM_Item rateValid; /* rate valid */ DM_Item autoHeadingError; /* auto heading error */ DM_Item autoDepthError; /* auto depth error */ DM_Item autoAltitudeError; /* auto altitude error */ DM_Item autoPositionError; /* auto position error */ DM_Item autoZError; /* auto z (depth/altitude) error */ DM_Item setpointX; /* x trajectory setpoint */ DM_Item setpointY; /* y trajectory setpoint */ DM_Item setpointDepth; /* depth trajectory setpoint */ DM_Item setpointAltitude; /* altitude trajectory setpoint */ DM_Item setpointZ; /* z trajectory setpoint */ DM_Item setpointGyro; /* gyro trajectory setpoint */ DM_Item setpointKalmanHeading; /* Kalman heading trajectory setpoint, */ /* and in degrees */ DM_Item setpointKalmanHeadingDegrees; DM_Item setpointHeading; /* heading trajectory setpoint */ DM_Item setpointErrorHeading; /* heading error setpoint */ DM_Item setpointMoved[DOF]; /* trajectory setpoint moved flag */ DM_Item velocityCommandX; /* joystick x velocity command */ DM_Item velocityCommandY; /* joystick y velocity command */ DM_Item velocityCommandZ; /* joystick z velocity command */ DM_Item velocityCommandHeading; /* joystick heading velocity command */ DM_Item thrustCommandX; /* joystick x thrust command */ DM_Item thrustCommandY; /* joystick y thrust command */ DM_Item thrustCommandZ; /* joystick z thrust command */ DM_Item thrustCommandHeading; /* joystick heading thrust command */ DM_Item thrustCommandHCF; /* lat/horiz heading conversion factor */ DM_Item modelThrustX; /* dynamic model x thrust command */ DM_Item modelThrustY; /* dynamic model y thrust command */ DM_Item modelThrustZ; /* dynamic model z thrust command */ DM_Item modelThrustHeading; /* dynamic model heading thrust command */ DM_Item modelThrustHCF; /* lat/horiz heading conversion factor */ DM_Item proportionalGain[DOF]; /* control servo proportional gains */ DM_Item integralGain[DOF]; /* control servo integral gains */ DM_Item derivativeGain[DOF]; /* control servo derivative gains */ DM_Item depthGain[PID_GAINS]; /* control servo depth PID gains */ DM_Item altitudeGain[PID_GAINS]; /* control servo altitude PID gains */ DM_Item zIntegral; /* z integration term */ DM_Item servoThrust[DOF]; /* control servo thrust command */ DM_Item servoThrustHCF; /* lat/horiz heading conversion factor */ DM_Item commandedThrust[DOF]; /* vehicle commanded thrust */ DM_Item commandedThrustHCF; /* lat/horiz heading conversion factor */ DM_Item headingDisableTransfer; /* transfer heading on disable flag */ DM_Item xThrustThreshold; /* thrust threshold for heading transfer*/ DM_Item controlThrust[NUM_THRUSTERS]; /* thruster control thrust */ DM_Item powerRequest; /* power requested */ DM_Item powerAllocated; /* power allocated */ DM_Item powerReduced; /* power reduced flag */ DM_Item motorPowerReduced; /*thruster motor power reduced */ DM_Item motorEnable[NUM_THRUSTERS]; /*thruster motor enable */ DM_Item motorStall[NUM_THRUSTERS]; /*thruster motor stall alarm */ DM_Item motorControlPower[NUM_THRUSTERS]; /*thruster motor control power */ DM_Item motorControlThrust[NUM_THRUSTERS];/*thruster motor control thrust */ DM_Item motorControlMode[NUM_THRUSTERS]; /*thruster motor control mode */ DM_Item motorControlThrusts; /* thruster motor control thrusts */ DM_Item shipHeading; /* Ship's heading (for umbilical turns calculation) */ DM_Item launchAngle; /* Angle between ROV and ship heading at launch */ } controlDMItems; /* function prototypes */ #if __STDC__ Void sensorFilter(controlDMItems *controlDmi); Void trajectoryPlan(controlDMItems *controlDmi); Void configureMode(controlDMItems *controlDmi); Void interfaceCommand(controlDMItems *controlDmi); Void dynamicModel(controlDMItems *controlDmi); Void servoControl(controlDMItems * controlDmi); Void thrusterMap(controlDMItems *controlDmi); Void thrusterControl(controlDMItems *controlDmi); Void controlShutDown(char *taskName); Errno checkCreate(Errno status, char *itemName, MBool logWarning); Errno checkLookup(DM_Item item, char *itemName); Errno checkProvider(Errno status, char *errorMsg); Errno checkConsumer(Errno status, char *errorMsg, MBool logWarning); Errno checkRead(Errno status, char *errorMsg, MBool logWarning); Errno checkWrite(Errno status, char *errorMsg); Errno checkConnect(Errno status, char *itemName); Errno checkGroup(Errno status, char *errorMsg); Errno checkItemStatus(Errno status, char *errorMsg); #endif #endif