/******************************************************************************/ /* Copyright 1992 MBARI */ /******************************************************************************/ /* Summary : User Interface Command Module for Tiburon Control System */ /* Filename : interface.c */ /* Author : Janice Tarrant */ /* Project : Tiburon */ /* Version : Version 1.0 */ /* Created : 06/18/92 */ /* Modified : 06/10/93 */ /* Archived : */ /******************************************************************************/ /* Modification History : */ /* $Header: interface.c,v 1.1 97/12/04 15:28:02 oreilly Exp $ * $Log: interface.c,v $ * Revision 1.1 97/12/04 15:28:02 15:28:02 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 system data manager definitions */ #include /* thruster definitions */ #include "control.h" /* control system definitions */ #include "interface.h" /* interface command definitions */ #include "model.h" /* dynamic model definitions */ /* function prototypes */ #if __STDC__ LOCAL Errno initInterfaceCommand(dmOpId dmOp, controlDMItems *controlDmi, Errno xstatus, Errno ystatus, Errno zStatus, Errno headingStatus, SEM_ID commandSem); LOCAL Void thrustCommandTF(Flt32 factor1[], Flt32 factor2[], Flt32 factor3[], Flt32 offset2[], Flt32 offset3[], Flt32 deflection1[], Flt32 deflection2[], Flt32 *thrustHCF); LOCAL Void checkVelocityCommand(Flt32 *velocity, char *dof, Flt32 velocityMax); LOCAL Void checkThrustCommand(Flt32 *thrust, char *dof, Flt32 thrustMax); #endif /******************************************************************************/ /* Function : interfaceCommand */ /* Purpose : Interprets user interface commands, scales them to velocities */ /* or thrusts. */ /* Inputs : Control data manager items structure pointer. */ /* Outputs : None. */ /******************************************************************************/ Void interfaceCommand(controlDMItems *controlDmi) { extern FILE *fpw; /* file ptr for data logging */ Errno gainStatus; /* error codes */ Errno modeStatus; Errno xStatus, yStatus, zStatus; Errno headingStatus; Errno hcfStatus; SEM_ID commandSem; /* interface command semaphore*/ MBool init; /* initialization flag */ MBool gainSwitch; /* joystick gain switch */ Nat16 joystickGain; /* joystick gain (percent) */ joystickCmdMode joystickMode; /* joystick command mode */ Int16 xValue, yValue, zValue; /* joystick command values */ Int16 headingValue; Flt32 factor1[DOF], factor2[DOF]; /* thrust command constants */ Flt32 factor3[DOF]; Flt32 offset2[DOF], offset3[DOF]; Flt32 deflection1[DOF]; Flt32 deflection2[DOF]; Flt32 commandGain; /* command gain (fraction) */ Flt32 xVelocity, yVelocity; /* velocity commands */ Flt32 zVelocity, headingVelocity; Flt32 deflection[DOF]; /* joystick deflection */ Int16 dof; /* degree of freedom counter */ Flt32 thrust[DOF]; /* thrust commands */ Flt32 thrustHCF; /* heading conversion factor */ DM_Time sampleTime; /* command sample time */ /* initialize task wakeup semaphore */ if ((commandSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { logMsg("Control System FAILURE :\ncould not initialize interface command semaphore\n"); controlShutDown("tinterfaceCommand"); } /* start consumer for command gain switch, gain, mode and inputs */ if (initInterfaceCommand(CONSUMER, controlDmi, xStatus, yStatus, zStatus, headingStatus, commandSem) == ERROR) controlShutDown("tinterfaceCommand"); /* start provider for velocity and thrust commands, and thrust command */ /* heading conversion factor */ if (initInterfaceCommand(PROVIDER, controlDmi, xStatus, yStatus, zStatus, headingStatus, commandSem) == ERROR) controlShutDown("tinterfaceCommand"); /* initialize thrust command transfer function constants */ thrustCommandTF(factor1, factor2, factor3, offset2, offset3, deflection1, deflection2, &thrustHCF); /* initialization flag, first time through loop check data manager status */ init = TRUE; FOREVER { /* wakeup when commands available */ semTake(commandSem, WAIT_FOREVER); /* get joystick gain switch and gain, and convert from percentage to fraction */ gainStatus = dm_read(controlDmi->joystickGainSwitch, (Void *) &gainSwitch, sizeof(gainSwitch), (DM_Time *) NULL); if (init) /* verify initial reads only */ if (checkRead(gainStatus, "Interface Command - joystick gain switch", TRUE) == ERROR) gainSwitch = FALSE; /* on error, set switch off */ if (gainSwitch) /* use joystick gain */ { gainStatus = dm_read(controlDmi->joystickGain, (Void *) &joystickGain, sizeof(joystickGain), (DM_Time *) NULL); if (init) /* verify initial reads only */ if (checkRead(gainStatus, "Interface Command - joystick gain", TRUE) == ERROR) joystickGain = 100; /* on error, set default gain */ commandGain = joystickGain / 100.0; } else /* use default gain */ commandGain = 1.0; /* get joystick command mode */ modeStatus = dm_read(controlDmi->joystickMode, (Void *) &joystickMode, sizeof(joystickMode), (DM_Time *) NULL); if (init) /* verify initial reads only */ if (checkRead(modeStatus, "Interface Command - joystick mode", TRUE) == ERROR) joystickMode = SPEED; /* on error, set default mode */ /* get command inputs */ xStatus = dm_read(controlDmi->joystickX, (Void *) &xValue, sizeof(xValue), (DM_Time *) NULL); yStatus = dm_read(controlDmi->joystickY, (Void *) &yValue, sizeof(yValue), (DM_Time *) NULL); zStatus = dm_read(controlDmi->joystickZ, (Void *) &zValue, sizeof(zValue), (DM_Time *) NULL); headingStatus = dm_read(controlDmi->joystickHeading, (Void *) &headingValue, sizeof(headingValue), (DM_Time *) NULL); #ifdef ROV_SIMULATION if (xValue == 9999) { fclose(fpw); controlShutDown("tinterfaceCommand"); } #endif #if 0 fprintf(fpw,"joystick %d %d %d %d\n", xValue, yValue, zValue, headingValue); #endif if (init) /* verify initial reads only */ if (initInterfaceCommand(READER, controlDmi, xStatus, yStatus, zStatus, headingStatus, commandSem) == ERROR) controlShutDown("tinterfaceCommand"); switch (joystickMode) { case SPEED: /* convert command values to */ /* velocity */ xVelocity = commandGain * xValue * X_VELOCITY_MAX / X_SCALE; yVelocity = commandGain * yValue * Y_VELOCITY_MAX / Y_SCALE; zVelocity = commandGain * zValue * Z_VELOCITY_MAX / Z_SCALE; headingVelocity = commandGain * headingValue * HEADING_VELOCITY_MAX / HEADING_SCALE; /* check for out of range velocities */ checkVelocityCommand(&xVelocity, "x", X_VELOCITY_MAX); checkVelocityCommand(&yVelocity, "y", Y_VELOCITY_MAX); checkVelocityCommand(&zVelocity, "z", Z_VELOCITY_MAX); checkVelocityCommand(&headingVelocity, "heading", HEADING_VELOCITY_MAX); #if 0 fprintf(fpw,"velocity command x %10.4f y %10.4f z %10.4f h %10.4f\n", xVelocity, yVelocity, zVelocity, headingVelocity); #endif /* update velocity commands */ gettimeofday(&sampleTime, (struct timezone *) NULL); xStatus = dm_write(controlDmi->velocityCommandX, (Void *) &xVelocity, sizeof(xVelocity), &sampleTime); yStatus = dm_write(controlDmi->velocityCommandY, (Void *) &yVelocity, sizeof(yVelocity), &sampleTime); zStatus = dm_write(controlDmi->velocityCommandZ, (Void *) &zVelocity, sizeof(zVelocity), &sampleTime); headingStatus = dm_write(controlDmi->velocityCommandHeading, (Void *) &headingVelocity, sizeof(headingVelocity), &sampleTime); if (init) /* verify initial writes only */ if (initInterfaceCommand(WRITER, controlDmi, xStatus, yStatus, zStatus, headingStatus, commandSem) == ERROR) controlShutDown("tinterfaceCommand"); break; case THRUST: /* convert command values to */ /* thrust */ deflection[X_INDEX] = commandGain * xValue / X_SCALE; deflection[Y_INDEX] = commandGain * yValue / Y_SCALE; deflection[Z_INDEX] = commandGain * zValue / Z_SCALE; deflection[HEADING_INDEX] = commandGain * headingValue / HEADING_SCALE; for (dof = 0; dof < DOF; dof++) if (fabs(deflection[dof]) < deflection1[dof]) thrust[dof] = factor1[dof] * deflection[dof]; else if (fabs(deflection[dof]) < deflection2[dof]) thrust[dof] = (deflection[dof] > 0.0) ? factor2[dof] * deflection[dof] + offset2[dof] : factor2[dof] * deflection[dof] - offset2[dof]; else thrust[dof] = (deflection[dof] > 0.0) ? factor3[dof] * deflection[dof] + offset3[dof] : factor3[dof] * deflection[dof] - offset3[dof]; /* check for out of range thrusts */ checkThrustCommand(&thrust[X_INDEX], "x", X_THRUST_MAX); checkThrustCommand(&thrust[Y_INDEX], "y", Y_THRUST_MAX); checkThrustCommand(&thrust[Z_INDEX], "z", Z_THRUST_MAX); checkThrustCommand(&thrust[HEADING_INDEX], "heading", HEADING_THRUST_MAX); #if 0 fprintf(fpw,"thrust command x %10.4f y %10.4f z %10.4f h %10.4f\n", thrust[0], thrust[1], thrust[2], thrust[3]); #endif /* update thrust commands and thrust heading conversion factor */ gettimeofday(&sampleTime, (struct timezone *) NULL); xStatus = dm_write(controlDmi->thrustCommandX, (Void *) &thrust[X_INDEX], sizeof(thrust[X_INDEX]), &sampleTime); yStatus = dm_write(controlDmi->thrustCommandY, (Void *) &thrust[Y_INDEX], sizeof(thrust[Y_INDEX]), &sampleTime); zStatus = dm_write(controlDmi->thrustCommandZ, (Void *) &thrust[Z_INDEX], sizeof(thrust[Z_INDEX]), &sampleTime); headingStatus = dm_write(controlDmi->thrustCommandHeading, (Void *) &thrust[HEADING_INDEX], sizeof(thrust[HEADING_INDEX]), &sampleTime); hcfStatus = dm_write(controlDmi->thrustCommandHCF, (Void *) &thrustHCF, sizeof(thrustHCF), &sampleTime); if (init) /* verify initial writes only */ { if (initInterfaceCommand(WRITER, controlDmi, xStatus, yStatus, zStatus, headingStatus, commandSem) == ERROR) controlShutDown("tinterfaceCommand"); if (checkWrite(hcfStatus, "Interface Command - command heading conversion factor") == ERROR) controlShutDown("tinterfaceCommand"); } break; } /* case */ if (init) init = FALSE; } /* FOREVER */ } /* interfaceCommand */ /******************************************************************************/ /* Function : initInterfaceCommand */ /* Purpose : Performs interface command 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 initInterfaceCommand(dmOpId dmOp, controlDMItems *controlDmi, Errno xStatus, Errno yStatus, Errno zStatus, Errno headingStatus, SEM_ID commandSem) { Errno status; /* error code */ switch (dmOp) { /* start consumer items and check status */ case CONSUMER: status = dm_start_consumer(controlDmi->joystickGainSwitch, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Interface Command - joystick gain switch", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->joystickGain, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Interface Command - joystick gain", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->joystickMode, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Interface Command - joystick mode", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->joystickX, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Interface Command - joystick x", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->joystickY, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Interface Command - joystick y", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->joystickZ, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Interface Command - joystick z", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->joystickHeading, CONTROL_PERIOD, commandSem); if (checkConsumer(status, "Interface Command - joystick heading", FALSE) == ERROR) return(ERROR); break; /* start provider items and check status */ case PROVIDER: status = dm_start_provider(controlDmi->velocityCommandX, CONTROL_PERIOD); if (checkProvider(status, "Interface Command - velocity command x") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->velocityCommandY, CONTROL_PERIOD); if (checkProvider(status, "Interface Command - velocity command y") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->velocityCommandZ, CONTROL_PERIOD); if (checkProvider(status, "Interface Command - velocity command z") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->velocityCommandHeading, CONTROL_PERIOD); if (checkProvider(status, "Interface Command - velocity command heading") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->thrustCommandX, CONTROL_PERIOD); if (checkProvider(status, "Interface Command - thrust command x") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->thrustCommandY, CONTROL_PERIOD); if (checkProvider(status, "Interface Command - thrust command y") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->thrustCommandZ, CONTROL_PERIOD); if (checkProvider(status, "Interface Command - thrust command z") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->thrustCommandHeading, CONTROL_PERIOD); if (checkProvider(status, "Interface Command - thrust command heading") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->thrustCommandHCF, CONTROL_PERIOD); if (checkProvider(status, "Interface Command - thrust command heading conversion factor") == ERROR) return(ERROR); break; /* check read status */ case READER: if (checkRead(xStatus, "Interface Command - joystick x", TRUE) == ERROR) return(ERROR); if (checkRead(yStatus, "Interface Command - joystick y", TRUE) == ERROR) return(ERROR); if (checkRead(zStatus, "Interface Command - joystick z", TRUE) == ERROR) return(ERROR); if (checkRead(headingStatus, "Interface Command - joystick heading", TRUE) == ERROR) return(ERROR); break; /* check write status */ case WRITER: if (checkWrite(xStatus, "Interface Command - command x") == ERROR) return(ERROR); if (checkWrite(yStatus, "Interface Command - command y") == ERROR) return(ERROR); if (checkWrite(zStatus, "Interface Command - command z") == ERROR) return(ERROR); if (checkWrite(headingStatus, "Interface Command - command heading") == ERROR) return(ERROR); break; } return(OK); } /* initInterfaceCommand */ /******************************************************************************/ /* Function : thrustCommandTF */ /* Purpose : Calculates the constants for the joystick to thrust command */ /* transfer function. */ /* Inputs : Thrust command constant pointers and thrust heading conversion */ /* factor. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void thrustCommandTF(Flt32 factor1[], Flt32 factor2[], Flt32 factor3[], Flt32 offset2[], Flt32 offset3[], Flt32 deflection1[], Flt32 deflection2[], Flt32 *thrustHCF) { factor1[X_INDEX] = (X_THRUST1 * X_THRUST_MAX) / X_DEFLECTION1; factor1[Y_INDEX] = (Y_THRUST1 * Y_THRUST_MAX) / Y_DEFLECTION1; factor1[Z_INDEX] = (Z_THRUST1 * Z_THRUST_MAX) / Z_DEFLECTION1; factor1[HEADING_INDEX] = (HEADING_THRUST1 * HEADING_THRUST_MAX) / HEADING_DEFLECTION1; /* printf("factor1 %f %f %f %f\n",factor1[X_INDEX],factor1[Y_INDEX],factor1[Z_INDEX],factor1[HEADING_INDEX]);*/ factor2[X_INDEX] = (X_THRUST2 - X_THRUST1) * X_THRUST_MAX / (X_DEFLECTION2 - X_DEFLECTION1); factor2[Y_INDEX] = (Y_THRUST2 - Y_THRUST1) * Y_THRUST_MAX / (Y_DEFLECTION2 - Y_DEFLECTION1); factor2[Z_INDEX] = (Z_THRUST2 - Z_THRUST1) * Z_THRUST_MAX / (Z_DEFLECTION2 - Z_DEFLECTION1); factor2[HEADING_INDEX] = (HEADING_THRUST2 - HEADING_THRUST1) * HEADING_THRUST_MAX / (HEADING_DEFLECTION2 - HEADING_DEFLECTION1); /* printf("factor2 %f %f %f %f\n",factor2[X_INDEX],factor2[Y_INDEX],factor2[Z_INDEX],factor2[HEADING_INDEX]);*/ factor3[X_INDEX] = (1.0 - X_THRUST2) * X_THRUST_MAX / (1.0 - X_DEFLECTION2); factor3[Y_INDEX] = (1.0 - Y_THRUST2) * Y_THRUST_MAX / (1.0 - Y_DEFLECTION2); factor3[Z_INDEX] = (1.0 - Z_THRUST2) * Z_THRUST_MAX / (1.0 - Z_DEFLECTION2); factor3[HEADING_INDEX] = (1.0 - HEADING_THRUST2) * HEADING_THRUST_MAX / (1.0 - HEADING_DEFLECTION2); /* printf("factor3 %f %f %f %f\n",factor3[X_INDEX],factor3[1],factor3[2],factor3[HEADING_INDEX]);*/ offset2[X_INDEX] = X_THRUST1 * X_THRUST_MAX - factor2[X_INDEX] * X_DEFLECTION1; offset2[Y_INDEX] = Y_THRUST1 * Y_THRUST_MAX - factor2[Y_INDEX] * Y_DEFLECTION1; offset2[Z_INDEX] = Z_THRUST1 * Z_THRUST_MAX - factor2[Z_INDEX] * Z_DEFLECTION1; offset2[HEADING_INDEX] = HEADING_THRUST1 * HEADING_THRUST_MAX - factor2[HEADING_INDEX] * HEADING_DEFLECTION1; /* printf("offset2 %f %f %f %f\n",offset2[X_INDEX],offset2[Y_INDEX],offset2[Z_INDEX],offset2[HEADING_INDEX]);*/ offset3[X_INDEX] = X_THRUST2 * X_THRUST_MAX - factor3[X_INDEX] * X_DEFLECTION2; offset3[Y_INDEX] = Y_THRUST2 * Y_THRUST_MAX - factor3[Y_INDEX] * Y_DEFLECTION2; offset3[Z_INDEX] = Z_THRUST2 * Z_THRUST_MAX - factor3[Z_INDEX] * Z_DEFLECTION2; offset3[HEADING_INDEX] = HEADING_THRUST2 * HEADING_THRUST_MAX - factor3[HEADING_INDEX] * HEADING_DEFLECTION2; /* printf("offset3 %f %f %f %f\n",offset3[X_INDEX],offset3[Y_INDEX],offset3[Z_INDEX],offset3[HEADING_INDEX]);*/ deflection1[X_INDEX] = X_DEFLECTION1; deflection1[Y_INDEX] = Y_DEFLECTION1; deflection1[Z_INDEX] = Z_DEFLECTION1; deflection1[HEADING_INDEX] = HEADING_DEFLECTION1; deflection2[X_INDEX] = X_DEFLECTION2; deflection2[Y_INDEX] = Y_DEFLECTION2; deflection2[Z_INDEX] = Z_DEFLECTION2; deflection2[HEADING_INDEX] = HEADING_DEFLECTION2; /* thrust command lateral to horizontal thrusters heading conversion factor */ /* (ratio of thruster radii) */ *thrustHCF = LAT_RADIAL / HORIZ_RADIAL; } /* thrustCommandTF */ /******************************************************************************/ /* Function : checkVelocityCommand */ /* Purpose : Checks velocity command range. */ /* Inputs : Velocity pointer, degree of freedom string and maximum velocity.*/ /* Outputs : None. */ /******************************************************************************/ LOCAL Void checkVelocityCommand(Flt32 *velocity, char *dof, Flt32 velocityMax) { Flt32 dv; /* delta velocity */ if (*velocity > velocityMax) /* if velocity > max, */ { /* set to max */ dv = *velocity - velocityMax; *velocity = velocityMax; if (dv > (COMMAND_ERROR * velocityMax)) /* if delta large, log warning*/ logMsg("Control System WARNING : Interface Command - %s\nvelocity command exceeded maximum value\n", dof); } else if (*velocity < -velocityMax) /* if velocity < min, */ { /* set to min */ dv = (-velocityMax - *velocity); *velocity = -velocityMax; if (dv > (COMMAND_ERROR * velocityMax)) /* if delta large, log warning*/ logMsg("Control System WARNING : Interface Command - %s\nvelocity command exceeded maximum value\n", dof); } } /* checkVelocityCommand */ /******************************************************************************/ /* Function : checkThrustCommand */ /* Purpose : Checks thrust command range. */ /* Inputs : Thrust pointer, degree of freedom string and maximum thrust. */ /* Outputs : None. */ /******************************************************************************/ LOCAL Void checkThrustCommand(Flt32 *thrust, char *dof, Flt32 thrustMax) { Flt32 dT; /* delta thrust */ if (*thrust > thrustMax) /* if thrust > max, set to max*/ { dT = *thrust - thrustMax; *thrust = thrustMax; if (dT > (COMMAND_ERROR * thrustMax)) /* if delta large, log warning*/ logMsg("Control System WARNING : Interface Command - %s\nthrust command exceeded maximum value\n", dof); } else if (*thrust < -thrustMax) /* if thrust < min, set to min*/ { dT = (-thrustMax - *thrust); *thrust = -thrustMax; if (dT > (COMMAND_ERROR * thrustMax)) /* if delta large, log warning*/ logMsg("Control System WARNING : Interface Command - %s\nthrust command exceeded maximum value\n", dof); } } /* checkThrustCommand */