/******************************************************************************/ /* Copyright 1992 MBARI */ /******************************************************************************/ /* Summary : Servo Control Module for Tiburon Control System */ /* Filename : servo.c */ /* Author : Janice Tarrant */ /* Project : Tiburon */ /* Version : Version 1.0 */ /* Created : 10/12/92 */ /* Modified : 11/26/94 */ /* Archived : */ /******************************************************************************/ /* Modification History : */ /* $Header: servo.c,v 1.1 97/12/04 15:28:07 oreilly Exp $ * $Log: servo.c,v $ * Revision 1.1 97/12/04 15:28:07 15:28:07 oreilly (Thomas C. O'Reilly) * Initial revision * * */ /******************************************************************************/ #include /* VxWorks systems declarations */ #include /* VxWorks standard I/O library */ #include /* VxWorks floating point math 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.h" /* control system definitions */ #include "servo.h" /* servo control definitions */ #include "model.h" /* dynamic model definitions */ #include "interface.h" /* interface command definitions */ /* functiov prototypes */ #if __STDC__ LOCAL Errno initServoControl(dmOpId dmOp, controlDMItems *controlDmi, Errno setpointStatus[], Errno positionStatus[], Errno rateStatus[], Errno thrustStatus[], Errno hcfStatus, Errno altitudeStatus, Errno dhStatus, SEM_ID servoSem); LOCAL Errno initServoGroup(controlDMItems *controlDmi, DM_Group *servoGroup, DWord proportionalGainBit[], DWord integralGainBit[], DWord derivativeGainBit[], DWord *adaptiveZBit); LOCAL Void initGains(controlDMItems *controlDmi, Nat32 proportionalGain[], Nat32 integralGain[], Nat32 derivativeGain[]); LOCAL Void getServoChanges(DWord servoGroupBits, DWord proportionalGainBit[], DWord integralGainBit[], DWord derivativeGainBit[], DWord adaptiveZBit, controlDMItems *controlDmi, Nat32 proportionalGain[], Nat32 integralGain[], Nat32 derivativeGain[], MBool *adaptiveZ); LOCAL Void checkControlThrust(Flt32 *thrust, Flt32 thrustMax); #endif /******************************************************************************/ /* Function : servoControl */ /* Purpose : Provides simple PID (proportional-integral-derivative) */ /* controllers for position, depth and heading. */ /* Inputs : Control data manager items structure pointer. */ /* Outputs : None. */ /******************************************************************************/ void servoControl(controlDMItems *controlDmi) { extern FILE *fpw; /* file ptr for data logging */ Errno setpointStatus[DOF]; /* error codes */ Errno positionStatus[DOF]; Errno rateStatus[DOF]; Errno thrustStatus[DOF]; Errno hcfStatus; Errno altitudeStatus; Errno dhStatus; Errno adaptiveZStatus; SEM_ID servoSem; /* trajectory plan semaphore */ MBool init; /* initialization flag */ MBool adaptiveZ; /* adaptive z servo flag */ DM_Group servoGroup; /* servo changes data manager group */ DWord servoGroupBits; /* servo group change bit vector */ DWord proportionalGainBit[DOF]; /* proportional gain id bits */ DWord integralGainBit[DOF]; /* integral gain id bits */ DWord derivativeGainBit[DOF]; /* derivative gain id bits */ DWord adaptiveZBit; /* adaptive z id bit */ Flt32 setpoint[DOF]; /* trajectory setpoints */ Flt32 position[DOF]; /* position measurements */ Flt32 rate[DOF]; /* velocity measurements */ Flt32 prevPositionZ = 0.0; /* previous z position measurement */ Flt32 prevSetpointZ = 0.0; /* previous z setpoint */ Flt32 zIntegral = 0.0; /* z position integral */ MBool autoAltitude; /* auto altitude flag */ Flt32 cosH, sinH; /* cosine and sine of heading */ Flt32 thrustXGlobal; /* x thrust in vehicle coordinates */ Flt32 thrustYGlobal; /* y thrust in vehicle coordinates */ Flt32 deltaHeading; /* heading error */ Flt32 deltaHeadingDegrees; /* heading error (degrees) */ Nat32 proportionalGain[DOF]; /* proportional gain */ Nat32 integralGain[DOF]; /* integral gain */ Nat32 derivativeGain[DOF]; /* derivative gain */ Flt32 bHat, bHatDot; /* adaptive buoyancy term */ Flt32 bHatPrev = 0.0; /* previous buoyancy term */ Flt32 thrust[DOF]; /* servo control thrust */ Flt32 thrustHCF; /* thrust heading conversion factor */ Int16 dof; /* degree of freedom counter */ DM_Time sampleTime; /* control thrust sample time */ /* initialize task wakeup semaphore */ if ((servoSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { logMsg("Control System FAILURE :\ncould not initialize servo controller semaphore\n"); controlShutDown("tservoControl"); } /* start consumer for trajectory setpoints, sensor position and velocity */ /* measurements, auto altitude, control gains and adaptive z flag */ if (initServoControl(CONSUMER, controlDmi, setpointStatus, positionStatus, rateStatus, thrustStatus, hcfStatus, altitudeStatus, dhStatus, servoSem) == ERROR) controlShutDown("tservoControl"); /* start provider for control thrusts and thrust heading conversion factor */ if (initServoControl(PROVIDER, controlDmi, setpointStatus, positionStatus, rateStatus, thrustStatus, hcfStatus, altitudeStatus, dhStatus, servoSem) == ERROR) controlShutDown("tservoControl"); /* initialize control gains and adaptive z flag data manager group */ if (initServoGroup(controlDmi, &servoGroup, proportionalGainBit, integralGainBit, derivativeGainBit, &adaptiveZBit) == ERROR) controlShutDown("tservoControl"); /* get initial control gains */ initGains(controlDmi, proportionalGain, integralGain, derivativeGain); /* get initial adaptive z servo flag */ adaptiveZStatus = dm_read(controlDmi->adaptiveZServo, (Void *) &adaptiveZ, sizeof(adaptiveZ), (DM_Time *) NULL); if (checkRead(adaptiveZStatus, "Servo Control - adaptive z", TRUE) == ERROR) controlShutDown("tservoControl"); /* initialize control thrust lateral to horizontal thrusters heading */ /* conversion factor (ratio of thruster radii) */ thrustHCF = LAT_RADIAL / HORIZ_RADIAL; /* initialization flag, first time through loop check data manager status */ init = TRUE; FOREVER { /* wakeup when setpoints available */ semTake(servoSem, WAIT_FOREVER); /* get setpoints, sensor measurements and auto altitude flag */ setpointStatus[X_INDEX] = dm_read(controlDmi->setpointX, (Void *) &setpoint[X_INDEX], sizeof(setpoint[X_INDEX]), (DM_Time *) NULL); setpointStatus[Y_INDEX] = dm_read(controlDmi->setpointY, (Void *) &setpoint[Y_INDEX], sizeof(setpoint[Y_INDEX]), (DM_Time *) NULL); setpointStatus[Z_INDEX] = dm_read(controlDmi->setpointZ, (Void *) &setpoint[Z_INDEX], sizeof(setpoint[Z_INDEX]), (DM_Time *) NULL); setpointStatus[HEADING_INDEX] = dm_read(controlDmi->setpointHeading, (Void *) &setpoint[HEADING_INDEX], sizeof(setpoint[HEADING_INDEX]), (DM_Time *) NULL); positionStatus[X_INDEX] = dm_read(controlDmi->sensorX, (Void *) &position[X_INDEX], sizeof(position[X_INDEX]), (DM_Time *) NULL); positionStatus[Y_INDEX] = dm_read(controlDmi->sensorY, (Void *) &position[Y_INDEX], sizeof(position[Y_INDEX]), (DM_Time *) NULL); positionStatus[Z_INDEX] = dm_read(controlDmi->sensorZ, (Void *) &position[Z_INDEX], sizeof(position[Z_INDEX]), (DM_Time *) NULL); positionStatus[HEADING_INDEX] = dm_read(controlDmi->sensorHeading, (Void *) &position[HEADING_INDEX], sizeof(position[HEADING_INDEX]), (DM_Time *) NULL); rateStatus[X_INDEX] = dm_read(controlDmi->sensorRateX, (Void *) &rate[X_INDEX], sizeof(rate[X_INDEX]), (DM_Time *) NULL); rateStatus[Y_INDEX] = dm_read(controlDmi->sensorRateY, (Void *) &rate[Y_INDEX], sizeof(rate[Y_INDEX]), (DM_Time *) NULL); rateStatus[Z_INDEX] = dm_read(controlDmi->sensorRateZ, (Void *) &rate[Z_INDEX], sizeof(rate[Z_INDEX]), (DM_Time *) NULL); rateStatus[HEADING_INDEX] = dm_read(controlDmi->sensorRateHeading, (Void *) &rate[HEADING_INDEX], sizeof(rate[HEADING_INDEX]), (DM_Time *) NULL); altitudeStatus = dm_read(controlDmi->autoAltitude, (Void *) &autoAltitude, sizeof(autoAltitude), (DM_Time *) NULL); if (init) /* verify initial reads only */ if (initServoControl(READER, controlDmi, setpointStatus, positionStatus, rateStatus, thrustStatus, hcfStatus, altitudeStatus, dhStatus, servoSem) == ERROR) controlShutDown("tservoControl"); /* get control gains and adaptive z flag, only if they have changed */ servoGroupBits = dm_get_group_changes(servoGroup); if (servoGroupBits != 0) getServoChanges(servoGroupBits, proportionalGainBit, integralGainBit, derivativeGainBit, adaptiveZBit, controlDmi, proportionalGain, integralGain, derivativeGain, &adaptiveZ); /* determine control thrusts */ /* convert global x and y coordinates to vehicle coordinates */ cosH = cos(position[HEADING_INDEX]); sinH = sin(position[HEADING_INDEX]); thrustXGlobal = proportionalGain[X_INDEX] * (setpoint[X_INDEX] - position[X_INDEX]) - derivativeGain[X_INDEX] * rate[X_INDEX]; thrustYGlobal = proportionalGain[Y_INDEX] * (setpoint[Y_INDEX] - position[Y_INDEX]) - derivativeGain[Y_INDEX] * rate[Y_INDEX]; thrust[X_INDEX] = thrustXGlobal * cosH + thrustYGlobal * sinH; thrust[Y_INDEX] = thrustYGlobal * cosH - thrustXGlobal * sinH; /* setpoint moving, reset integral */ if (fabs(setpoint[Z_INDEX] - prevSetpointZ) > EPSILON) zIntegral = 0.0; /* calculate integral, but don't sum */ else /* floating point zeros */ if (fabs(setpoint[Z_INDEX] - position[Z_INDEX]) > EPSILON) zIntegral += (setpoint[Z_INDEX] - 0.5 * (position[Z_INDEX] + prevPositionZ)) * CONTROL_DT; prevPositionZ = position[Z_INDEX]; prevSetpointZ = setpoint[Z_INDEX]; thrust[Z_INDEX] = proportionalGain[Z_INDEX] * (setpoint[Z_INDEX] - position[Z_INDEX]) + integralGain[Z_INDEX] * zIntegral - derivativeGain[Z_INDEX] * rate[Z_INDEX]; if (adaptiveZ) /* adaptive z servo */ { bHatDot = -ADAPTIVE_GAIN * (rate[Z_INDEX] - LAMBDA * (setpoint[Z_INDEX] - position[Z_INDEX])); bHat = bHatPrev + bHatDot * CONTROL_DT; bHatPrev = bHat; thrust[Z_INDEX] -= bHat * ACCEL_G; } if (autoAltitude) thrust[Z_INDEX] = -thrust[Z_INDEX]; deltaHeading = setpoint[HEADING_INDEX] - position[HEADING_INDEX]; if (deltaHeading > PI) deltaHeading -= 2.0 * PI; else if (deltaHeading < -PI) deltaHeading += 2.0 * PI; deltaHeadingDegrees = deltaHeading * 180.0 / PI; thrust[HEADING_INDEX] = proportionalGain[HEADING_INDEX] * deltaHeading - derivativeGain[HEADING_INDEX] * rate[HEADING_INDEX]; /* check for out of range thrusts */ checkControlThrust(&thrust[X_INDEX], X_THRUST_MAX); checkControlThrust(&thrust[Y_INDEX], Y_THRUST_MAX); checkControlThrust(&thrust[Z_INDEX], Z_THRUST_MAX); checkControlThrust(&thrust[HEADING_INDEX], HEADING_THRUST_MAX); #if 0 fprintf(fpw,"servo x %10.4f y %10.4f z %10.4f h %10.4f\n", thrust[X_INDEX], thrust[Y_INDEX], thrust[Z_INDEX], thrust[HEADING_INDEX]); #endif /* update control thrusts and thrust heading conversion factor */ gettimeofday(&sampleTime, (struct timezone *) NULL); for (dof = 0; dof < DOF; dof++) thrustStatus[dof] = dm_write(controlDmi->servoThrust[dof], (Void *) &thrust[dof], sizeof(thrust[dof]), &sampleTime); hcfStatus = dm_write(controlDmi->servoThrustHCF, (Void *) &thrustHCF, sizeof(thrustHCF), &sampleTime); dhStatus = dm_write(controlDmi->setpointErrorHeading, (Void *) &deltaHeadingDegrees, sizeof(deltaHeadingDegrees), &sampleTime); dm_write(controlDmi->zIntegral, (Void *) &zIntegral, sizeof(zIntegral), &sampleTime); if (init) /* verify initial writes only */ if (initServoControl(WRITER, controlDmi, setpointStatus, positionStatus, rateStatus, thrustStatus, hcfStatus, altitudeStatus, dhStatus, servoSem) == ERROR) controlShutDown("tservoControl"); if (init) init = FALSE; } /* FOREVER */ } /* servoControl */ /******************************************************************************/ /* Function : initServoControl */ /* Purpose : Performs servo control data manager items initialization */ /* and checks. */ /* Inputs : Data manager operation identifier, control data manager items */ /* structure pointer, read/write status, wakeup semaphore. */ /* Outputs : Returns OK or ERROR. */ /******************************************************************************/ LOCAL Errno initServoControl(dmOpId dmOp, controlDMItems *controlDmi, Errno setpointStatus[], Errno positionStatus[], Errno rateStatus[], Errno thrustStatus[], Errno hcfStatus, Errno altitudeStatus, Errno dhStatus, SEM_ID servoSem) { Errno status; /* error codes */ switch (dmOp) { /* start consumer items and check status */ case CONSUMER: status = dm_start_consumer(controlDmi->setpointX, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Servo Control - setpoint x", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->setpointY, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Servo Control - setpoint y", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->setpointZ, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Servo Control - setpoint z", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->setpointHeading, CONTROL_PERIOD, servoSem); if (checkConsumer(status, "Servo Control - setpoint heading", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sensorX, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Servo Control - sensor x", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sensorY, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Servo Control - sensor y", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sensorZ, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Servo Control - sensor z", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sensorHeading, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Servo Control - sensor heading", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sensorRateX, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Servo Control - sensor rate x", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sensorRateY, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Servo Control - sensor rate y", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sensorRateZ, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Servo Control - sensor rate z", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sensorRateHeading, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Servo Control - sensor rate heading", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->autoAltitude, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Servo Control - auto altitude", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->proportionalGain[X_INDEX], DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Servo Control - x proportional gain", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->proportionalGain[Y_INDEX], DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Servo Control - y proportional gain", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->proportionalGain[Z_INDEX], DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Servo Control - z proportional gain", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->proportionalGain[HEADING_INDEX], DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Servo Control - heading proportional gain", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->integralGain[X_INDEX], DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Servo Control - x integral gain", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->integralGain[Y_INDEX], DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Servo Control - y integral gain", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->integralGain[Z_INDEX], DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Servo Control - z integral gain", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->integralGain[HEADING_INDEX], DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Servo Control - heading integral gain", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->derivativeGain[X_INDEX], DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Servo Control - x derivative gain", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->derivativeGain[Y_INDEX], DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Servo Control - y derivative gain", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->derivativeGain[Z_INDEX], DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Servo Control - z derivative gain", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->derivativeGain[HEADING_INDEX], DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Servo Control - heading derivative gain", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->adaptiveZServo, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Servo Control - adaptive z servo", FALSE) == ERROR) return(ERROR); break; /* start provider items and check status */ case PROVIDER: status = dm_start_provider(controlDmi->servoThrust[X_INDEX], CONTROL_PERIOD); if (checkProvider(status, "Servo Control - x thrust") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->servoThrust[Y_INDEX], CONTROL_PERIOD); if (checkProvider(status, "Servo Control - y thrust") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->servoThrust[Z_INDEX], CONTROL_PERIOD); if (checkProvider(status, "Servo Control - z thrust") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->servoThrust[HEADING_INDEX], CONTROL_PERIOD); if (checkProvider(status, "Servo Control - heading thrust") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->servoThrustHCF, CONTROL_PERIOD); if (checkProvider(status, "Servo Control - heading thrust conversion factor ") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->setpointErrorHeading, CONTROL_PERIOD); if (checkProvider(status, "Servo Control - heading error ") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->zIntegral, CONTROL_PERIOD); if (checkProvider(status, "Servo Control - z integral") == ERROR) return(ERROR); break; /* check read status */ case READER: if (checkRead(setpointStatus[X_INDEX], "Servo Control - setpoint x", TRUE) == ERROR) return(ERROR); if (checkRead(setpointStatus[Y_INDEX], "Servo Control - setpoint y", TRUE) == ERROR) return(ERROR); if (checkRead(setpointStatus[Z_INDEX], "Servo Control - setpoint z", TRUE) == ERROR) return(ERROR); if (checkRead(setpointStatus[HEADING_INDEX], "Servo Control - setpoint heading", TRUE) == ERROR) return(ERROR); if (checkRead(positionStatus[X_INDEX], "Servo Control - sensor x", TRUE) == ERROR) return(ERROR); if (checkRead(positionStatus[Y_INDEX], "Servo Control - sensor y", TRUE) == ERROR) return(ERROR); if (checkRead(positionStatus[Z_INDEX], "Servo Control - sensor z", TRUE) == ERROR) return(ERROR); if (checkRead(positionStatus[HEADING_INDEX], "Servo Control - sensor heading", TRUE) == ERROR) return(ERROR); if (checkRead(rateStatus[X_INDEX], "Servo Control- sensor rate x", TRUE) == ERROR) return(ERROR); if (checkRead(rateStatus[Y_INDEX], "Servo Control- sensor rate y", TRUE) == ERROR) return(ERROR); if (checkRead(rateStatus[Z_INDEX], "Servo Control- sensor rate z", TRUE) == ERROR) return(ERROR); if (checkRead(rateStatus[HEADING_INDEX], "Servo Control- sensor rate heading", TRUE) == ERROR) return(ERROR); if (checkRead(altitudeStatus, "Servo Control- auto altitude", TRUE) == ERROR) return(ERROR); break; /* check write status */ case WRITER: if (checkWrite(thrustStatus[X_INDEX], "Servo Control - x thrust") == ERROR) return(ERROR); if (checkWrite(thrustStatus[Y_INDEX], "Servo Control - y thrust") == ERROR) return(ERROR); if (checkWrite(thrustStatus[Z_INDEX], "Servo Control - z thrust") == ERROR) return(ERROR); if (checkWrite(thrustStatus[HEADING_INDEX], "Servo Control - heading thrust") == ERROR) return(ERROR); if (checkWrite(hcfStatus, "Servo Control - heading thrust conversion factor") == ERROR) return(ERROR); if (checkWrite(dhStatus, "Servo Control - heading error") == ERROR) return(ERROR); break; } return(OK); } /* initServoControl */ /******************************************************************************/ /* Function : initServoGroup */ /* Purpose : Initializes control gains and adaptive z servo flag data manager*/ /* group and group id bits. */ /* Inputs : Control data manager items structure pointer, servo group */ /* pointer, proportional gain, integral derivative gain and */ /* adaptive z bits. */ /* Outputs : Returns OK or ERROR. */ /******************************************************************************/ LOCAL Errno initServoGroup(controlDMItems *controlDmi, DM_Group *servoGroup, DWord proportionalGainBit[], DWord integralGainBit[], DWord derivativeGainBit[], DWord *adaptiveZBit) { Errno status; /* error code */ /* create data manager group */ *servoGroup = dm_create_group(); /* add data manager items to group */ status = dm_group_add_item(*servoGroup, controlDmi->proportionalGain[X_INDEX], &proportionalGainBit[X_INDEX]); if (checkGroup(status, "Servo Control - x proportional gain") == ERROR) return(ERROR); status = dm_group_add_item(*servoGroup, controlDmi->proportionalGain[Y_INDEX], &proportionalGainBit[Y_INDEX]); if (checkGroup(status, "Servo Control - y proportional gain") == ERROR) return(ERROR); status = dm_group_add_item(*servoGroup, controlDmi->proportionalGain[Z_INDEX], &proportionalGainBit[Z_INDEX]); if (checkGroup(status, "Servo Control - z proportional gain") == ERROR) return(ERROR); status = dm_group_add_item(*servoGroup, controlDmi->proportionalGain[HEADING_INDEX], &proportionalGainBit[HEADING_INDEX]); if (checkGroup(status, "Servo Control - heading proportional gain") == ERROR) return(ERROR); status = dm_group_add_item(*servoGroup, controlDmi->integralGain[X_INDEX], &integralGainBit[X_INDEX]); if (checkGroup(status, "Servo Control - x integral gain") == ERROR) return(ERROR); status = dm_group_add_item(*servoGroup, controlDmi->integralGain[Y_INDEX], &integralGainBit[Y_INDEX]); if (checkGroup(status, "Servo Control - y integral gain") == ERROR) return(ERROR); status = dm_group_add_item(*servoGroup, controlDmi->integralGain[Z_INDEX], &integralGainBit[Z_INDEX]); if (checkGroup(status, "Servo Control - z integral gain") == ERROR) return(ERROR); status = dm_group_add_item(*servoGroup, controlDmi->integralGain[HEADING_INDEX], &integralGainBit[HEADING_INDEX]); if (checkGroup(status, "Servo Control - heading integral gain") == ERROR) return(ERROR); status = dm_group_add_item(*servoGroup, controlDmi->derivativeGain[X_INDEX], &derivativeGainBit[X_INDEX]); if (checkGroup(status, "Servo Control - x derivative gain") == ERROR) return(ERROR); status = dm_group_add_item(*servoGroup, controlDmi->derivativeGain[Y_INDEX], &derivativeGainBit[Y_INDEX]); if (checkGroup(status, "Servo Control - y derivative gain") == ERROR) return(ERROR); status = dm_group_add_item(*servoGroup, controlDmi->derivativeGain[Z_INDEX], &derivativeGainBit[Z_INDEX]); if (checkGroup(status, "Servo Control - z derivative gain") == ERROR) return(ERROR); status = dm_group_add_item(*servoGroup, controlDmi->derivativeGain[HEADING_INDEX], &derivativeGainBit[HEADING_INDEX]); if (checkGroup(status, "Servo Control - heading derivative gain") == ERROR) return(ERROR); status = dm_group_add_item(*servoGroup, controlDmi->adaptiveZServo, adaptiveZBit); if (checkGroup(status, "Servo Control - adaptive z servo") == ERROR) return(ERROR); } /* initServoGroup */ /******************************************************************************/ /* Function : initGains */ /* Purpose : Gets initial proportional and derivative control gains. */ /* Inputs : Control data manager items structure pointer, proportional gain */ /* integral gain, and derivative gain pointers. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void initGains(controlDMItems *controlDmi, Nat32 proportionalGain[], Nat32 integralGain[], Nat32 derivativeGain[]) { Errno status; /* error code */ /* read data manager values for gains, on error, set to default values */ status = dm_read(controlDmi->proportionalGain[X_INDEX], (Void *) &proportionalGain[X_INDEX], sizeof(proportionalGain[X_INDEX]), (DM_Time *) NULL); if (checkRead(status, "Servo Control - x proportional gain", TRUE) == ERROR) proportionalGain[X_INDEX] = X_PROPORTIONAL_GAIN; status = dm_read(controlDmi->proportionalGain[Y_INDEX], (Void *) &proportionalGain[Y_INDEX], sizeof(proportionalGain[Y_INDEX]), (DM_Time *) NULL); if (checkRead(status, "Servo Control - y proportional gain", TRUE) == ERROR) proportionalGain[Y_INDEX] = Y_PROPORTIONAL_GAIN; status = dm_read(controlDmi->proportionalGain[Z_INDEX], (Void *) &proportionalGain[Z_INDEX], sizeof(proportionalGain[Z_INDEX]), (DM_Time *) NULL); if (checkRead(status, "Servo Control - z proportional gain", TRUE) == ERROR) proportionalGain[Z_INDEX] = DEPTH_PROPORTIONAL_GAIN; status = dm_read(controlDmi->proportionalGain[HEADING_INDEX], (Void *) &proportionalGain[HEADING_INDEX], sizeof(proportionalGain[HEADING_INDEX]), (DM_Time *) NULL); if (checkRead(status, "Servo Control - heading proportional gain", TRUE) == ERROR) proportionalGain[HEADING_INDEX] = HEADING_PROPORTIONAL_GAIN; status = dm_read(controlDmi->integralGain[X_INDEX], (Void *) &integralGain[X_INDEX], sizeof(integralGain[X_INDEX]), (DM_Time *) NULL); if (checkRead(status, "Servo Control - x integral gain", TRUE) == ERROR) integralGain[X_INDEX] = X_INTEGRAL_GAIN; status = dm_read(controlDmi->integralGain[Y_INDEX], (Void *) &integralGain[Y_INDEX], sizeof(integralGain[Y_INDEX]), (DM_Time *) NULL); if (checkRead(status, "Servo Control - y integral gain", TRUE) == ERROR) integralGain[Y_INDEX] = Y_INTEGRAL_GAIN; status = dm_read(controlDmi->integralGain[Z_INDEX], (Void *) &integralGain[Z_INDEX], sizeof(integralGain[Z_INDEX]), (DM_Time *) NULL); if (checkRead(status, "Servo Control - z integral gain", TRUE) == ERROR) integralGain[Z_INDEX] = DEPTH_INTEGRAL_GAIN; status = dm_read(controlDmi->integralGain[HEADING_INDEX], (Void *) &integralGain[HEADING_INDEX], sizeof(integralGain[HEADING_INDEX]), (DM_Time *) NULL); if (checkRead(status, "Servo Control - heading integral gain", TRUE) == ERROR) integralGain[HEADING_INDEX] = HEADING_INTEGRAL_GAIN; status = dm_read(controlDmi->derivativeGain[X_INDEX], (Void *) &derivativeGain[X_INDEX], sizeof(derivativeGain[X_INDEX]), (DM_Time *) NULL); if (checkRead(status, "Servo Control - x derivative gain", TRUE) == ERROR) derivativeGain[X_INDEX] = X_DERIVATIVE_GAIN; status = dm_read(controlDmi->derivativeGain[Y_INDEX], (Void *) &derivativeGain[Y_INDEX], sizeof(derivativeGain[Y_INDEX]), (DM_Time *) NULL); if (checkRead(status, "Servo Control - y derivative gain", TRUE) == ERROR) derivativeGain[Y_INDEX] = Y_DERIVATIVE_GAIN; status = dm_read(controlDmi->derivativeGain[Z_INDEX], (Void *) &derivativeGain[Z_INDEX], sizeof(derivativeGain[Z_INDEX]), (DM_Time *) NULL); if (checkRead(status, "Servo Control - z derivative gain", TRUE) == ERROR) derivativeGain[Z_INDEX] = DEPTH_DERIVATIVE_GAIN; status = dm_read(controlDmi->derivativeGain[HEADING_INDEX], (Void *) &derivativeGain[HEADING_INDEX], sizeof(derivativeGain[HEADING_INDEX]), (DM_Time *) NULL); if (checkRead(status, "Servo Control - heading derivative gain", TRUE) == ERROR) derivativeGain[HEADING_INDEX] = HEADING_DERIVATIVE_GAIN; } /* initGains */ /******************************************************************************/ /* Function : getServoChanges */ /* Purpose : Gets proportional, integral and derivative control gains, and */ /* the adaptive z servo flag. */ /* Inputs : Servo group bits, proportional gain bits, integral gain bits, */ /* derivative gain bits, adaptive z bits, control data manager */ /* items, proportional gains, integral gains, derivative gains and */ /* adaptive z servo flag. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void getServoChanges(DWord servoGroupBits, DWord proportionalGainBit[], DWord integralGainBit[], DWord derivativeGainBit[], DWord adaptiveZBit, controlDMItems *controlDmi, Nat32 proportionalGain[], Nat32 integralGain[], Nat32 derivativeGain[], MBool *adaptiveZ) { Int16 dof; /* degree of freedom counter */ /* if control gain has changed, read new value */ for (dof = 0; dof < DOF; dof++) { if (servoGroupBits & proportionalGainBit[dof]) { dm_read(controlDmi->proportionalGain[dof], (Void *) &proportionalGain[dof], sizeof(proportionalGain[dof]), (DM_Time *) NULL); logMsg("Control System ACKNOWLEDGE : proportional gain %d new value %ld\n", dof, proportionalGain[dof]); } if (servoGroupBits & integralGainBit[dof]) { dm_read(controlDmi->integralGain[dof], (Void *) &integralGain[dof], sizeof(integralGain[dof]), (DM_Time *) NULL); logMsg("Control System ACKNOWLEDGE : integral gain %d new value %ld\n", dof, integralGain[dof]); } if (servoGroupBits & derivativeGainBit[dof]) { dm_read(controlDmi->derivativeGain[dof], (Void *) &derivativeGain[dof], sizeof(derivativeGain[dof]), (DM_Time *) NULL); logMsg("Control System ACKNOWLEDGE : derivative gain %d new value %ld\n", dof, derivativeGain[dof]); } } /* if adaptive z servo flag has changed, read new value */ if (servoGroupBits & adaptiveZBit) dm_read(controlDmi->adaptiveZServo, (Void *) adaptiveZ, sizeof(*adaptiveZ), (DM_Time *) NULL); } /* getServoChanges */ /******************************************************************************/ /* Function : checkControlThrust */ /* Purpose : Checks control thrust range. */ /* Inputs : Thrust pointer and maximum thrust. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void checkControlThrust(Flt32 *thrust, Flt32 thrustMax) { if (*thrust > thrustMax) *thrust = thrustMax; else if (*thrust < -thrustMax) *thrust = -thrustMax; } /* checkControlThrust */