/******************************************************************************/ /* Copyright 1992 MBARI */ /******************************************************************************/ /* Summary : Dynamic Model Module for Tiuron Control System */ /* Filename : model.c */ /* Author : Janice Tarrant */ /* Project : Tiuron */ /* Version : Version 1.0 */ /* Created : 06/18/92 */ /* Modified : 06/14/94 */ /* Archived : */ /******************************************************************************/ /* Modification History : */ /* $Header: model.c,v 1.1 97/12/04 15:28:04 oreilly Exp $ * $Log: model.c,v $ * Revision 1.1 97/12/04 15:28:04 15:28:04 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" /* control system definitions */ #include "interface.h" /* command interface definitions */ #include "model.h" /* dynamic model definitions */ /* function prototypes */ #if __STDC__ LOCAL Errno initDynamicModel(dmOpId dmOp, controlDMItems *controlDmi, Errno xstatus, Errno ystatus, Errno zStatus, Errno headingStatus, Errno hcfStatus, SEM_ID modelSem); void checkModelThrust(Flt32 *thrust, Flt32 thrustMax); #endif /******************************************************************************/ /* Function : dynamicModel */ /* Purpose : Uses a simple vehicle dynamic model to determine required thrust*/ /* for velocity commands. */ /* Inputs : Control data manager items structure pointer. */ /* Outputs : None. */ /******************************************************************************/ Void dynamicModel(controlDMItems *controlDmi) { extern FILE *fpw; /* file ptr for data logging */ Errno xStatus, yStatus, zStatus; /* error codes */ Errno headingStatus; Errno hcfStatus; SEM_ID modelSem; /* dynamic model semaphore */ MBool init; /* initialization flag */ Flt32 xVelocity, yVelocity, zVelocity; /* velocity commands */ Flt32 headingVelocity; Flt32 xThrust, yThrust, zThrust; /* thrust commands */ Flt32 headingThrust; Flt32 thrustHCF; /* heading conversion factor */ thrusterId thruster; /* thruster counter */ DM_Time sampleTime; /* thrust command sample time */ /* initialize task wakeup semaphore */ if ((modelSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { logMsg("Control System FAILURE :\ncould not initialize dynamic model semaphore\n"); controlShutDown("tdynamicModel"); } /* start consumer for velocity commands */ if (initDynamicModel(CONSUMER, controlDmi, xStatus, yStatus, zStatus, headingStatus, hcfStatus, modelSem) == ERROR) controlShutDown("tdynamicModel"); /* start provider for thrust commands and thrust heading conversion factor */ if (initDynamicModel(PROVIDER, controlDmi, xStatus, yStatus, zStatus, headingStatus, hcfStatus, modelSem) == ERROR) controlShutDown("tdynamicModel"); /* initialization flag, first time through loop check data manager status */ init = TRUE; FOREVER { /* wakeup when commands available */ semTake(modelSem, WAIT_FOREVER); /* get velocity commands */ xStatus = dm_read(controlDmi->velocityCommandX, (Void *) &xVelocity, sizeof(xVelocity), (DM_Time *)NULL); yStatus = dm_read(controlDmi->velocityCommandY, (Void *) &yVelocity, sizeof(yVelocity), (DM_Time *)NULL); zStatus = dm_read(controlDmi->velocityCommandZ, (Void *) &zVelocity, sizeof(zVelocity), (DM_Time *)NULL); headingStatus = dm_read(controlDmi->velocityCommandHeading, (Void *) &headingVelocity, sizeof(headingVelocity), (DM_Time *)NULL); if (init) /* verify initial reads only */ if (initDynamicModel(READER, controlDmi, xStatus, yStatus, zStatus, headingStatus, hcfStatus, modelSem) == ERROR) controlShutDown("tdynamicModel"); /* convert command velocities to thrust using a square-law drag approximation */ xThrust = (xVelocity >= 0.0) ? HORIZ_THRUST_FACTOR * xVelocity * xVelocity : -HORIZ_THRUST_FACTOR * xVelocity * xVelocity; yThrust = (yVelocity >= 0.0) ? LAT_THRUST_FACTOR * yVelocity * yVelocity : -LAT_THRUST_FACTOR * yVelocity * yVelocity; zThrust = (zVelocity >= 0.0) ? VERT_THRUST_FACTOR * zVelocity * zVelocity : -VERT_THRUST_FACTOR * zVelocity * zVelocity; headingThrust = (headingVelocity >= 0.0) ? HEADING_THRUST_FACTOR * headingVelocity * headingVelocity : -HEADING_THRUST_FACTOR * headingVelocity * headingVelocity; /* adjust thrust by feed forward compensation based on thrust loss with */ /* advance velocity */ xThrust = xThrust / (1.0 - LOSS_FACTOR * fabs(xVelocity)); yThrust = yThrust / (1.0 - LOSS_FACTOR * fabs(yVelocity)); zThrust = zThrust / (1.0 - LOSS_FACTOR * fabs(zVelocity)); headingThrust = headingThrust / (1.0 - LOSS_FACTOR * LAT_RADIAL * fabs(headingVelocity)); /* check for out of range thrusts */ checkModelThrust(&xThrust, X_THRUST_MAX); checkModelThrust(&yThrust, Y_THRUST_MAX); checkModelThrust(&zThrust, Z_THRUST_MAX); checkModelThrust(&headingThrust, HEADING_THRUST_MAX); #if 0 fprintf(fpw,"model thrust x %10.4f y %10.4f z %10.4f h %10.4f\n", xThrust, yThrust, zThrust, headingThrust); #endif /* calculate lateral to horizontal thrusters heading conversion factor */ /* negate laterals feed forward compensation, multiply by ratio of thruster */ /* radii, adjust by horizontals feed forward compensation */ thrustHCF = (1 - LOSS_FACTOR * LAT_RADIAL * fabs(headingVelocity)) * (LAT_RADIAL / HORIZ_RADIAL) / (1 - LOSS_FACTOR * HORIZ_RADIAL * fabs(headingVelocity)); /* update thrust commands and thrust heading conversion factor */ gettimeofday(&sampleTime, (struct timezone *) NULL); xStatus = dm_write(controlDmi->modelThrustX, (Void *) &xThrust, sizeof(xThrust), &sampleTime); yStatus = dm_write(controlDmi->modelThrustY, (Void *) &yThrust, sizeof(yThrust), &sampleTime); zStatus = dm_write(controlDmi->modelThrustZ, (Void *) &zThrust, sizeof(zThrust), &sampleTime); headingStatus = dm_write(controlDmi->modelThrustHeading, (Void *) &headingThrust, sizeof(headingThrust), &sampleTime); hcfStatus = dm_write(controlDmi->modelThrustHCF, (Void *) &thrustHCF, sizeof(thrustHCF), &sampleTime); if (init) /* verify initial writes only */ if (initDynamicModel(WRITER, controlDmi, xStatus, yStatus, zStatus, headingStatus, hcfStatus, modelSem) == ERROR) controlShutDown("tdynamicModel"); if (init) init = FALSE; } /* FOREVER */ } /* dynamicModel */ /******************************************************************************/ /* Function : initDynamicModel */ /* Purpose : Performs dynamic model 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 initDynamicModel(dmOpId dmOp, controlDMItems *controlDmi, Errno xStatus, Errno yStatus, Errno zStatus, Errno headingStatus, Errno hcfStatus, SEM_ID modelSem) { Errno status; /* error code */ thrusterId thruster; /* thruster counter */ switch (dmOp) { /* start consumer items and check status */ case CONSUMER: status = dm_start_consumer(controlDmi->velocityCommandX, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Dynamic Model - velocity command x", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->velocityCommandY, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Dynamic Model - velocity command y", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->velocityCommandZ, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Dynamic Model - velocity command z", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->velocityCommandHeading, CONTROL_PERIOD, modelSem); if (checkConsumer(status, "Dynamic Model - velocity command heading", FALSE) == ERROR) return(ERROR); break; /* start provider items and check status */ case PROVIDER: status = dm_start_provider(controlDmi->modelThrustX, CONTROL_PERIOD); if (checkProvider(status, "Dynamic Model - thrust x") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->modelThrustY, CONTROL_PERIOD); if (checkProvider(status, "Dynamic Model - thrust y") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->modelThrustZ, CONTROL_PERIOD); if (checkProvider(status, "Dynamic Model - thrust z") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->modelThrustHeading, CONTROL_PERIOD); if (checkProvider(status, "Dynamic Model - thrust heading") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->modelThrustHCF, CONTROL_PERIOD); if (checkProvider(status, "Dynamic Model - thrust heading conversion factor") == ERROR) return(ERROR); break; /* check read status */ case READER: if (checkRead(xStatus, "Dynamic Model - velocity command x", TRUE) == ERROR) return(ERROR); if (checkRead(yStatus, "Dynamic Model - velocity command y", TRUE) == ERROR) return(ERROR); if (checkRead(zStatus, "Dynamic Model - velocity command z", TRUE) == ERROR) return(ERROR); if (checkRead(headingStatus, "Dynamic Model - velocity command heading", TRUE) == ERROR) return(ERROR); break; /* check write status */ case WRITER: if (checkWrite(xStatus, "Dynamic Model - thrust x") == ERROR) return(ERROR); if (checkWrite(yStatus, "Dynamic Model - thrust y") == ERROR) return(ERROR); if (checkWrite(zStatus, "Dynamic Model - thrust z") == ERROR) return(ERROR); if (checkWrite(headingStatus, "Dynamic Model - thrust heading") == ERROR) return(ERROR); if (checkWrite(hcfStatus, "Dynamic Model - thrust heading conversion factor") == ERROR) return(ERROR); break; } return(OK); } /* initDynamicModel */ /******************************************************************************/ /* Function : checkModelThrust */ /* Purpose : Checks control thrust range. */ /* Inputs : Thrust pointer and maximum thrust. */ /* Outputs : None. */ /******************************************************************************/ void checkModelThrust(Flt32 *thrust, Flt32 thrustMax) { if (*thrust > thrustMax) *thrust = thrustMax; else if (*thrust < -thrustMax) *thrust = -thrustMax; } /* checkModelThrust */