/******************************************************************************/ /* Copyright 1992 MBARI */ /******************************************************************************/ /* Summary : Thruster Control Module for the Tiburon Control System */ /* Filename : thruster.c */ /* Author : Janice Tarrant */ /* Project : Tiburon */ /* Version : Version 1.0 */ /* Created : 06/18/92 */ /* Modified : 07/07/92 */ /* Archived : */ /******************************************************************************/ /* Modification History : */ /* $Header: thruster.c,v 1.1 97/12/04 15:28:11 oreilly Exp $ * $Log: thruster.c,v $ * Revision 1.1 97/12/04 15:28:11 15:28:11 oreilly (Thomas C. O'Reilly) * Initial revision * * */ /******************************************************************************/ #include /* Vxworks system 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 /* thruster definitions */ #include "control.h" /* open loop control system definitions */ #define MAX_THRUSTER_POWER 5000 /* maximum power (W) per thruster */ /* piecewise linearization */ #define THRUST2 125 /* thrust offset */ #define THRUST3 311 #define THRUST4 498 #define THRUST5 685 #define THRUST6 872 #define THRUST7 1121 #define POWER2 205 /* power offset */ #define POWER3 703 #define POWER4 1435 #define POWER5 2353 #define POWER6 3378 #define POWER7 4902 #define THRUST_POWER_FACTOR1 1.64 /* thrust to power conversion factor */ #define THRUST_POWER_FACTOR2 2.6774 #define THRUST_POWER_FACTOR3 3.9144 #define THRUST_POWER_FACTOR4 4.9091 #define THRUST_POWER_FACTOR5 5.4813 #define THRUST_POWER_FACTOR6 6.1205 /* function prototypes */ #if __STDC__ LOCAL Errno initThrusterControl(dmOpId dmOp, controlDMItems *controlDmi, Errno *thrustStatus, Errno thrustsStatus, Errno *enableStatus, Errno *powerStatus, SEM_ID controlSem, SEM_ID powerAvailSem); #endif /******************************************************************************/ /* Function : thrusterControl */ /* Purpose : Uses a piecewise linear approximation of the thruster thrust */ /* vs velocity and power vs velocity curves to determine control */ /* thrust power requirements. The power requested is compared */ /* with the power allocated and power and thrust for each thruster */ /* are recalculated if neccessary. */ /* Inputs : Initialization flag and control data manager items structure */ /* pointer. */ /* Outputs : None. */ /******************************************************************************/ Void thrusterControl(controlDMItems *controlDmi) { extern FILE *fpw; /* file ptr for data logging */ Errno thrustStatus[NUM_THRUSTERS];/* error codes */ Errno thrustsStatus; Errno enableStatus[NUM_THRUSTERS]; Errno powerStatus[NUM_THRUSTERS]; SEM_ID controlSem; /* thruster control semaphore */ SEM_ID powerAvailSem; /* power available semaphore */ MBool init; /* initialization flag */ Flt32 thrust[NUM_THRUSTERS]; /* control thrust */ motorEnableState motorEnable[NUM_THRUSTERS];/* motor enable */ Flt32 power[NUM_THRUSTERS]; /* control power */ Flt32 powerSum; /* total power required */ Nat16 powerRequest; /* power requested */ Nat16 powerAllocated; /* power allocated */ MBool powerReduced; /* power reduced flag */ Flt32 powerFactor; /* power reduction factor */ MBool motorPowerReduced; /* motor power reduced flag */ Int16 motorPower[NUM_THRUSTERS]; /* motor power */ Int16 motorThrust[NUM_THRUSTERS];/* motor control thrust */ thrusterId thruster; /* thruster counter */ DM_Time sampleTime; /* control torque sample time */ /* taskSuspend(taskIdSelf());*/ /* initialize task wakeup semaphore */ if ((controlSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { logMsg("Control System FAILURE :\ncould not initialize thruster control semaphore\n"); controlShutDown("tthrusterControl"); } /* initialize power available semaphore */ if ((powerAvailSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { logMsg("Control System FAILURE :\ncould not initialize power available semaphore\n"); controlShutDown("tthrusterControl"); } /* start consumer for control thrust, motor enable, power allocated and power */ /* reduced flag */ if (initThrusterControl(CONSUMER, controlDmi, thrustStatus, thrustsStatus, enableStatus, powerStatus, controlSem, powerAvailSem) == ERROR) controlShutDown("tthrusterControl"); /* start provider for motor control thrust, power, mode and power reduced */ /* flag, and power requested */ if (initThrusterControl(PROVIDER, controlDmi, thrustStatus, thrustsStatus, enableStatus, powerStatus, controlSem, powerAvailSem) == ERROR) controlShutDown("tthrusterControl"); /* initialize motor power reduced flag */ motorPowerReduced = FALSE; /* initialization flag, first time through loop check data manager status */ init = TRUE; FOREVER { /* wakeup when thrusts available */ semTake(controlSem, WAIT_FOREVER); /* get control thrust and power available */ for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++) { thrustStatus[thruster] = dm_read(controlDmi->controlThrust[thruster], (Void *) &thrust[thruster], sizeof(thrust[thruster]), (DM_Time *) NULL); enableStatus[thruster] = dm_read(controlDmi->motorEnable[thruster], (Void *) &motorEnable[thruster], sizeof(motorEnable[thruster]), (DM_Time *) NULL); } if (init) /* verify initial reads only */ if (initThrusterControl(READER, controlDmi, thrustStatus, thrustsStatus, enableStatus, powerStatus, controlSem, powerAvailSem) == ERROR) controlShutDown("tthrusterControl"); /* determine control power request */ powerSum = 0; for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++) { if (fabs(thrust[thruster]) <= THRUST2) power[thruster] = THRUST_POWER_FACTOR1 * fabs(thrust[thruster]); else if (fabs(thrust[thruster]) <= THRUST3) power[thruster] = POWER2 + THRUST_POWER_FACTOR2 * (fabs(thrust[thruster]) - THRUST2); else if (fabs(thrust[thruster]) <= THRUST4) power[thruster] = POWER3 + THRUST_POWER_FACTOR3 * (fabs(thrust[thruster]) - THRUST3); else if (fabs(thrust[thruster]) <= THRUST5) power[thruster] = POWER4 + THRUST_POWER_FACTOR4 * (fabs(thrust[thruster]) - THRUST4); else if (fabs(thrust[thruster]) <= THRUST6) power[thruster] = POWER5 + THRUST_POWER_FACTOR5 * (fabs(thrust[thruster]) - THRUST5); else if (fabs(thrust[thruster]) <= THRUST7) power[thruster] = POWER6 + THRUST_POWER_FACTOR6 * (fabs(thrust[thruster]) - THRUST6); else power[thruster] = POWER7; if (power[thruster] > MAX_THRUSTER_POWER) power[thruster] = MAX_THRUSTER_POWER; else if (power[thruster] < 0.0) power[thruster] = 0.0; powerSum += power[thruster]; } /* round up power request */ powerRequest = (powerSum > 0) ? (Int16) (powerSum + 1.0) : 0; #if 0 fprintf(fpw,"power ph %10.4f sh %10.4f fl %10.4f\n", power[0], power[1], power[2]); fprintf(fpw,"power al %10.4f pv %10.4f sv %10.4f\n", power[3], power[4], power[5]); #endif /* request power from power manager */ gettimeofday(&sampleTime, (struct timezone *) NULL); powerStatus[0] = dm_write(controlDmi->powerRequest, (Void *) &powerRequest, sizeof(powerRequest), &sampleTime); if (init) /* verify initial write only */ if (checkWrite(powerStatus[0] , "Thruster Control - power request") == ERROR) controlShutDown("tthrusterControl"); /* wait until power is available */ semTake(powerAvailSem, WAIT_FOREVER); /* get power allocated and power reduced flag */ powerStatus[0] = dm_read(controlDmi->powerAllocated, (Void *) &powerAllocated, sizeof(powerAllocated), (DM_Time *) NULL); if (init) /* verify initial read only */ if (checkRead(powerStatus[0], "Thruster Control - power allocated", TRUE) == ERROR) controlShutDown("tthrusterControl"); powerStatus[0] = dm_read(controlDmi->powerReduced, (Void *) &powerReduced, sizeof(powerReduced), (DM_Time *) NULL); if (init) /* verify initial read only */ if (checkRead(powerStatus[0], "Thruster Control - power reduced flag", TRUE) == ERROR) controlShutDown("tthrusterControl"); /* check power requested with power allocated and, reduce power and thrust if */ /* necessary */ if (powerRequest > powerAllocated) { powerFactor = ((Flt32) powerAllocated) / ((Flt32) powerRequest); for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++) { motorPower[thruster] = (Int16) (powerFactor * power[thruster]); if (motorPower[thruster] <= POWER2) motorThrust[thruster] = (Int16) (motorPower[thruster] / THRUST_POWER_FACTOR1); else if (motorPower[thruster] <= POWER3) motorThrust[thruster] = THRUST2 + (Int16) ((motorPower[thruster] - POWER2) / THRUST_POWER_FACTOR2); else if (motorPower[thruster] <= POWER4) motorThrust[thruster] = THRUST3 + (Int16) ((motorPower[thruster] - POWER3) / THRUST_POWER_FACTOR3); else if (motorPower[thruster] <= POWER5) motorThrust[thruster] = THRUST4 + (Int16) ((motorPower[thruster] - POWER4) / THRUST_POWER_FACTOR4); else if (motorPower[thruster] <= POWER6) motorThrust[thruster] = THRUST5 + (Int16) ((motorPower[thruster] - POWER5) / THRUST_POWER_FACTOR5); else if (motorPower[thruster] <= POWER7) motorThrust[thruster] = THRUST6 + (Int16) ((motorPower[thruster] - POWER6) / THRUST_POWER_FACTOR6); else motorThrust[thruster] = THRUST7; if (motorThrust[thruster] > MAX_THRUST) motorThrust[thruster] = MAX_THRUST; if (thrust[thruster] < 0.0) motorThrust[thruster] = - motorThrust[thruster]; } #if 0 fprintf(fpw,"motorPower ph %5hd sh %5hd fl %5hd\n", motorPower[0], motorPower[1], motorPower[2]); fprintf(fpw,"motorPower al %5hd pv %5hd sv %5hd\n", motorPower[3], motorPower[4], motorPower[5]); fprintf(fpw,"motorThrust ph %5hd sh %5hd fl %5hd\n", motorThrust[0], motorThrust[1], motorThrust[2]); fprintf(fpw,"motorThrust al %5hd pv %5hd sv %5hd\n", motorThrust[3], motorThrust[4], motorThrust[5]); #endif } else for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++) { motorPower[thruster] = (Int16) power[thruster]; motorThrust[thruster] = (Int16) thrust[thruster]; } /* reset motor power flag */ gettimeofday(&sampleTime, (struct timezone *) NULL); if ((powerReduced) && (!motorPowerReduced)) { motorPowerReduced = TRUE; dm_write(controlDmi->motorPowerReduced, (Void *) &motorPowerReduced, sizeof(motorPowerReduced), &sampleTime); } else if ((!powerReduced) && (motorPowerReduced)) { motorPowerReduced = FALSE; dm_write(controlDmi->motorPowerReduced, (Void *) &motorPowerReduced, sizeof(motorPowerReduced), &sampleTime); } /* update motor control thrust and power */ for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++) if (motorEnable[thruster] == ENABLE_MOTOR) { thrustStatus[thruster] = dm_write(controlDmi->motorControlThrust[thruster], (Void *) &motorThrust[thruster], sizeof(motorThrust[thruster]), &sampleTime); powerStatus[thruster] = dm_write(controlDmi->motorControlPower[thruster], (Void *) &motorPower[thruster], sizeof(motorPower[thruster]), &sampleTime); } thrustsStatus = dm_write(controlDmi->motorControlThrusts, (Void *) &motorThrust, sizeof(motorThrust), &sampleTime); checkWrite(thrustsStatus, "Thruster Control - motor control thrusts"); if (init) /* verify initial writes only */ if (initThrusterControl(WRITER, controlDmi, thrustStatus, thrustsStatus, enableStatus, powerStatus, controlSem, powerAvailSem) == ERROR) controlShutDown("tthrusterControl"); if (init) init = FALSE; } /* FOREVER */ } /* thrusterControl */ /******************************************************************************/ /* Function : initThrusterControl */ /* Purpose : Performs thruster control data manager items initialization and */ /* checks. */ /* Inputs : Data manager operation identifier, control data manager items */ /* structure pointer, read/write status, wakeup semaphore and */ /* power available semaphore. */ /* Outputs : Returns OK or ERROR. */ /******************************************************************************/ LOCAL Errno initThrusterControl(dmOpId dmOp, controlDMItems *controlDmi, Errno *thrustStatus, Errno thrustsStatus, Errno *enableStatus, Errno *powerStatus, SEM_ID controlSem, SEM_ID powerAvailSem) { thrusterId thruster; /* thruster counter */ switch (dmOp) { /* start consumer items and check status */ case CONSUMER: for (thruster = PORT_HORIZ; thruster <= PORT_VERT; thruster++) thrustStatus[thruster] = dm_start_consumer(controlDmi->controlThrust[thruster], CONTROL_PERIOD, SEM_NULL); thrustStatus[STBD_VERT] = dm_start_consumer(controlDmi->controlThrust[STBD_VERT], CONTROL_PERIOD, controlSem); if (checkConsumer(thrustStatus[PORT_HORIZ], "Thruster Control - port-horizontal control thrust", FALSE) == ERROR) return(ERROR); if (checkConsumer(thrustStatus[STBD_HORIZ], "Thruster Control - starboard-horizontal control thrust", FALSE) == ERROR) return(ERROR); if (checkConsumer(thrustStatus[FWD_LATERAL], "Thruster Control - forward-lateral control thrust", FALSE) == ERROR) return(ERROR); if (checkConsumer(thrustStatus[AFT_LATERAL], "Thruster Control - aft-lateral control thrust", FALSE) == ERROR) return(ERROR); if (checkConsumer(thrustStatus[PORT_VERT], "Thruster Control - port-vertical control thrust", FALSE) == ERROR) return(ERROR); if (checkConsumer(thrustStatus[STBD_VERT], "Thruster Control - starboard-vertical control thrust", FALSE) == ERROR) return(ERROR); for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++) enableStatus[thruster] = dm_start_consumer(controlDmi->motorEnable[thruster], DM_STATIC, SEM_NULL); if (checkConsumer(enableStatus[PORT_HORIZ], "Thruster Control - port-horizontal motor enable", FALSE) == ERROR) return(ERROR); if (checkConsumer(enableStatus[STBD_HORIZ], "Thruster Control - starboard-horizontal motor enable", FALSE) == ERROR) return(ERROR); if (checkConsumer(enableStatus[FWD_LATERAL], "Thruster Control - forward-lateral motor enable", FALSE) == ERROR) return(ERROR); if (checkConsumer(enableStatus[AFT_LATERAL], "Thruster Control - aft-lateral motor enable", FALSE) == ERROR) return(ERROR); if (checkConsumer(enableStatus[PORT_VERT], "Thruster Control - port-vertical motor enable", FALSE) == ERROR) return(ERROR); if (checkConsumer(enableStatus[STBD_VERT], "Thruster Control - starboard-vertical motor enable", FALSE) == ERROR) return(ERROR); powerStatus[0] = dm_start_consumer(controlDmi->powerAllocated, DM_STATIC, powerAvailSem); if (checkConsumer(powerStatus[0], "Thruster Control - power allocated", FALSE) == ERROR) return(ERROR); powerStatus[0] = dm_start_consumer(controlDmi->powerReduced, DM_STATIC, SEM_NULL); if (checkConsumer(powerStatus[0], "Thruster Control - power reduced flag", FALSE) == ERROR) return(ERROR); break; /* start provider items and check status */ case PROVIDER: for (thruster = PORT_HORIZ; thruster <= STBD_VERT; thruster++) { thrustStatus[thruster] = dm_start_provider(controlDmi->motorControlThrust[thruster], CONTROL_PERIOD); powerStatus[thruster] = dm_start_provider(controlDmi->motorControlPower[thruster], CONTROL_PERIOD); } if (checkProvider(thrustStatus[PORT_HORIZ], "Thruster Control - port-horizontal motor control thrust") == ERROR) return(ERROR); if (checkProvider(thrustStatus[STBD_HORIZ], "Thruster Control - starboard-horizontal motor control thrust") == ERROR) return(ERROR); if (checkProvider(thrustStatus[FWD_LATERAL], "Thruster Control - forward-lateral motor control thrust") == ERROR) return(ERROR); if (checkProvider(thrustStatus[AFT_LATERAL], "Thruster Control - aft-lateral motor control thrust") == ERROR) return(ERROR); if (checkProvider(thrustStatus[PORT_VERT], "Thruster Control - port-vertical motor control thrust") == ERROR) return(ERROR); if (checkProvider(thrustStatus[STBD_VERT], "Thruster Control - starboard-vertical motor control thrust") == ERROR) return(ERROR); if (checkProvider(powerStatus[PORT_HORIZ], "Thruster Control - port-horizontal motor control power") == ERROR) return(ERROR); if (checkProvider(powerStatus[STBD_HORIZ], "Thruster Control - starboard-horizontal motor control power") == ERROR) return(ERROR); if (checkProvider(powerStatus[FWD_LATERAL], "Thruster Control - forward-lateral motor control power") == ERROR) return(ERROR); if (checkProvider(powerStatus[AFT_LATERAL], "Thruster Control - aft-lateral motor control power") == ERROR) return(ERROR); if (checkProvider(powerStatus[PORT_VERT], "Thruster Control - port-vertical motor control power") == ERROR) return(ERROR); if (checkProvider(powerStatus[STBD_VERT], "Thruster Control - starboard-vertical motor control power") == ERROR) return(ERROR); thrustStatus[0] = dm_start_provider(controlDmi->motorControlThrusts, CONTROL_PERIOD); if (checkProvider(thrustStatus[0], "Thruster Control - motor control thrusts") == ERROR) return(ERROR); powerStatus[0] = dm_start_provider(controlDmi->motorPowerReduced, DM_STATIC); if (checkProvider(powerStatus[0], "Thruster Control - motor power reduced") == ERROR) return(ERROR); powerStatus[0] = dm_start_provider(controlDmi->powerRequest, CONTROL_PERIOD); if (checkProvider(powerStatus[0], "Thruster Control - power request") == ERROR) return(ERROR); break; /* check read status */ case READER: if (checkRead(thrustStatus[PORT_HORIZ], "Thruster Control - port-horizontal control thrust", TRUE) == ERROR) return(ERROR); if (checkRead(thrustStatus[STBD_HORIZ], "Thruster Control - starboard-horizontal control thrust", TRUE) == ERROR) return(ERROR); if (checkRead(thrustStatus[FWD_LATERAL], "Thruster Control - forward-lateral control thrust", TRUE) == ERROR) return(ERROR); if (checkRead(thrustStatus[AFT_LATERAL], "Thruster Control - aft-lateral control thrust", TRUE) == ERROR) return(ERROR); if (checkRead(thrustStatus[PORT_VERT], "Thruster Control - port-vertical control thrust", TRUE) == ERROR) return(ERROR); if (checkRead(thrustStatus[STBD_VERT], "Thruster Control - starboard-vertical control thrust", TRUE) == ERROR) return(ERROR); if (checkRead(enableStatus[PORT_HORIZ], "Thruster Control - port-horizontal motor enable", TRUE) == ERROR) return(ERROR); if (checkRead(enableStatus[STBD_HORIZ], "Thruster Control - starboard-horizontal motor enable", TRUE) == ERROR) return(ERROR); if (checkRead(enableStatus[FWD_LATERAL], "Thruster Control - forward-lateral motor enable", TRUE) == ERROR) return(ERROR); if (checkRead(enableStatus[AFT_LATERAL], "Thruster Control - aft-lateral motor enable", TRUE) == ERROR) return(ERROR); if (checkRead(enableStatus[PORT_VERT], "Thruster Control - port-vertical motor enable", TRUE) == ERROR) return(ERROR); if (checkRead(enableStatus[STBD_VERT], "Thruster Control - starboard-vertical motor enable", TRUE) == ERROR) return(ERROR); break; /* check write status */ case WRITER: if (checkWrite(thrustStatus[PORT_HORIZ], "Thruster Control - port-horizontal motor control thrust") == ERROR) return(ERROR); if (checkWrite(thrustStatus[STBD_HORIZ], "Thruster Control - starboard-horizontal motor control thrust") == ERROR) return(ERROR); if (checkWrite(thrustStatus[FWD_LATERAL], "Thruster Control - forward-lateral motor control thrust") == ERROR) return(ERROR); if (checkWrite(thrustStatus[AFT_LATERAL], "Thruster Control - aft-lateral motor control thrust") == ERROR) return(ERROR); if (checkWrite(thrustStatus[PORT_VERT], "Thruster Control - port-vertical motor control thrust") == ERROR) return(ERROR); if (checkWrite(thrustStatus[STBD_VERT], "Thruster Control - starboard-vertical motor control thrust") == ERROR) return(ERROR); if (checkWrite(thrustsStatus, "Thruster Control - motor control thrusts") == ERROR) return(ERROR); if (checkWrite(powerStatus[PORT_HORIZ], "Thruster Control - port-horizontal motor control power") == ERROR) return(ERROR); if (checkWrite(powerStatus[STBD_HORIZ], "Thruster Control - starboard-horizontal motor control power") == ERROR) return(ERROR); if (checkWrite(powerStatus[FWD_LATERAL], "Thruster Control - forward-lateral motor control power") == ERROR) return(ERROR); if (checkWrite(powerStatus[AFT_LATERAL], "Thruster Control - aft-lateral motor control power") == ERROR) return(ERROR); if (checkWrite(powerStatus[PORT_VERT], "Thruster Control - port-vertical motor control power") == ERROR) return(ERROR); if (checkWrite(powerStatus[STBD_VERT], "Thruster Control - starboard-vertical motor control power") == ERROR) return(ERROR); break; } return(OK); } /* initThrusterControl*/