/******************************************************************************/ /* Copyright 1992 MBARI */ /******************************************************************************/ /* Summary : Trajectory Plan Module for Tiburon Control System */ /* Filename : trajectory.c */ /* Author : Janice Tarrant */ /* Project : Tiburon */ /* Version : Version 1.0 */ /* Created : 10/02/92 */ /* Modified : 10/11/96 */ /* Archived : */ /******************************************************************************/ /* Modification History : */ /* $Header: trajectory.c,v 1.1 97/12/04 15:28:12 oreilly Exp $ * $Log: trajectory.c,v $ * Revision 1.1 97/12/04 15:28:12 15:28:12 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 "trajectory.h" /* trajectory plan definitions */ /* function prototypes */ #if __STDC__ LOCAL Errno initTrajectoryPlan(dmOpId dmOp, controlDMItems *controlDmi, dmStatus *statusPtr, SEM_ID commandSem); LOCAL Errno initSetpoints(controlDMItems *controlDmi, Flt32 setpoint[], MBool setpointMoved[], SEM_ID commandSem); LOCAL Void resetPositionSetpoint(Int16 command[], MBool autoPosition, Flt32 setpoint[], MBool setpointMoved[], autoPositionDmi *apd); LOCAL Void resetSetpoint(Int16 command, MBool autoControl, Flt32 *setpoint, MBool *setpointMoved, DM_Item dmiSensor, DM_Item dmiSetpoint, DM_Item dmiSetpointMoved); #endif /******************************************************************************/ /* Function : trajectoryPlan */ /* Purpose : Provides a commanded trajectory based on joystick controlled */ /* setpoints for auto position, auto depth or auto altitude and */ /* auto heading. */ /* Inputs : Control data manager items structure pointer. */ /* Outputs : None. */ /******************************************************************************/ Void trajectoryPlan(controlDMItems *controlDmi) { extern FILE *fpw; /* file ptr for data logging */ SEM_ID commandSem; /* trajectory plan semaphore */ MBool init; /* initialization flag */ Int16 command[DOF]; /* joystick command values */ MBool autoPosition, autoZ; /* auto control flags */ MBool autoHeading; Flt32 setpoint[NUM_SETPOINTS]; /* command setpoint */ Flt32 kalmanHeadingSetpointDegrees; MBool setpointMoved[DOF]; /* setpoint moved flag */ autoPositionDmi autoPDmi; /* auto position dm items */ dmStatus status; /* data manager status */ DM_Time sampleTime; /* setpoint sample time */ /* initialize task wakeup semaphore */ if ((commandSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { logMsg("Control System FAILURE :\ncould not initialize trajectory plan semaphore\n"); controlShutDown("ttrajectoryPlan"); } /* start consumer for command inputs, control mode and sensor measurements */ if (initTrajectoryPlan(CONSUMER, controlDmi, &status, commandSem) == ERROR) controlShutDown("ttrajectoryPlan"); /* start provider for command setpoints and setpoint moved flags */ if (initTrajectoryPlan(PROVIDER, controlDmi, &status, commandSem) == ERROR) controlShutDown("ttrajectoryPlan"); /* initialize setpoints and setpoint moved flags */ if (initSetpoints(controlDmi, setpoint, setpointMoved, commandSem) == ERROR) controlShutDown("ttrajectoryPlan"); /* initialize auto position dm items */ autoPDmi.dmiSensorX = controlDmi->sensorX; autoPDmi.dmiSensorY = controlDmi->sensorY; autoPDmi.dmiSetpointX = controlDmi->setpointX; autoPDmi.dmiSetpointY = controlDmi->setpointY; autoPDmi.dmiSetpointMovedX = controlDmi->setpointMoved[X_POSITION]; autoPDmi.dmiSetpointMovedY = controlDmi->setpointMoved[Y_POSITION]; init = TRUE; FOREVER { /* wakeup when commands available */ semTake(commandSem, WAIT_FOREVER); /* get command inputs and auto control flags */ status.x = dm_read(controlDmi->joystickX, (Void *) &command[X_POSITION], sizeof(command[X_POSITION]), (DM_Time *) NULL); status.y = dm_read(controlDmi->joystickY, (Void *) &command[Y_POSITION], sizeof(command[Y_POSITION]), (DM_Time *) NULL); status.z = dm_read(controlDmi->joystickZ, (Void *) &command[Z_POSITION], sizeof(command[Z_POSITION]), (DM_Time *) NULL); status.heading = dm_read(controlDmi->joystickHeading, (Void *) &command[HEADING], sizeof(command[HEADING]), (DM_Time *) NULL); status.autoControl[AUTO_POSITION] = dm_read(controlDmi->autoPosition, (Void *) &autoPosition, sizeof(autoPosition), (DM_Time *) NULL); status.autoControl[AUTO_Z] = dm_read(controlDmi->autoZ, (Void *) &autoZ, sizeof(autoZ), (DM_Time *) NULL); status.autoControl[AUTO_HEADING] = dm_read(controlDmi->autoHeading, (Void *) &autoHeading, sizeof(autoHeading), (DM_Time *) NULL); if (init) /* verify initial reads only */ if (initTrajectoryPlan(READER, controlDmi, &status, commandSem) == ERROR) controlShutDown("ttrajectoryPlan"); /* update position setpoint and setpoint moved flag */ resetPositionSetpoint(command, autoPosition, setpoint, setpointMoved, &autoPDmi); /* update depth setpoint and setpoint moved flag */ resetSetpoint(command[Z_POSITION], autoZ, &setpoint[DEPTH_SETPOINT], &setpointMoved[Z_POSITION], controlDmi->sensorDepth, controlDmi->setpointDepth, controlDmi->setpointMoved[Z_POSITION]); /* update altitude setpoint and setpoint moved flag */ resetSetpoint(command[Z_POSITION], autoZ, &setpoint[ALTITUDE_SETPOINT], &setpointMoved[Z_POSITION], controlDmi->sensorAltitude, controlDmi->setpointAltitude, controlDmi->setpointMoved[Z_POSITION]); /* update gyro setpoint and setpoint moved flag */ resetSetpoint(command[HEADING], autoHeading, &setpoint[GYRO_SETPOINT], &setpointMoved[HEADING], controlDmi->gyroRadians, controlDmi->setpointGyro, controlDmi->setpointMoved[HEADING]); /* update Kalman filter heading setpoint and setpoint moved flag */ resetSetpoint(command[HEADING], autoHeading, &setpoint[KALMAN_HEADING_SETPOINT], &setpointMoved[HEADING], controlDmi->kalmanHeading, controlDmi->setpointKalmanHeading, controlDmi->setpointMoved[HEADING]); kalmanHeadingSetpointDegrees = setpoint[KALMAN_HEADING_SETPOINT] * 180.0 / PI; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(controlDmi->setpointKalmanHeadingDegrees, (Void *) &kalmanHeadingSetpointDegrees, sizeof(kalmanHeadingSetpointDegrees), &sampleTime); #if 0 fprintf(fpw, "setpoint x %10.4f y %10.4f d %10.4f a %10.4f h %10.4f\n", setpoint[X_SETPOINT], setpoint[Y_SETPOINT], setpoint[DEPTH_SETPOINT], setpoint[ALTITUDE_SETPOINT], setpoint[HEADING_SETPOINT]); #endif if (init) init = FALSE; } /* FOREVER */ } /* trajectoryPlan */ /******************************************************************************/ /* Function : initTrajectoryPlan */ /* Purpose : Performs trajectory plan 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. */ /******************************************************************************/ Errno initTrajectoryPlan(dmOpId dmOp, controlDMItems *controlDmi, dmStatus *statusPtr, SEM_ID commandSem) { Errno status; /* error codes */ autoCtrlDof dof; /* control dof counter */ switch (dmOp) { /* start consumer items and check status */ case CONSUMER: status = dm_start_consumer(controlDmi->joystickX, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Trajectory Plan - joystick x", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->joystickY, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Trajectory Plan - joystick y", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->joystickZ, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Trajectory Plan - joystick z", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->joystickHeading, CONTROL_PERIOD, commandSem); if (checkConsumer(status, "Trajectory Plan - joystick heading", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->autoPosition, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Trajectory Plan - auto position", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->autoZ, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Trajectory Plan - auto z (depth/altitude)", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->autoHeading, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Trajectory Plan - auto heading", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sensorX, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Trajectory Plan - sensor x", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sensorY, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Trajectory Plan - sensor y", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sensorDepth, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Trajectory Plan - sensor depth", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->sensorAltitude, CONTROL_PERIOD, SEM_NULL); if (checkConsumer(status, "Trajectory Plan - sensor altitude", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->gyroRadians, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Trajectory Plan - gyro", FALSE) == ERROR) return(ERROR); status = dm_start_consumer(controlDmi->kalmanHeading, DM_ASYNC, SEM_NULL); if (checkConsumer(status, "Trajectory Plan - Kalman filter heading", FALSE) == ERROR) return(ERROR); break; /* start provider items and check status */ case PROVIDER: status = dm_start_provider(controlDmi->setpointX, CONTROL_PERIOD); if (checkProvider(status, "Trajectory Plan - setpoint x") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->setpointY, CONTROL_PERIOD); if (checkProvider(status, "Trajectory Plan - setpoint y") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->setpointDepth, CONTROL_PERIOD); if (checkProvider(status, "Trajectory Plan - setpoint depth") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->setpointAltitude, CONTROL_PERIOD); if (checkProvider(status, "Trajectory Plan - setpoint altitude") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->setpointGyro, CONTROL_PERIOD); if (checkProvider(status, "Trajectory Plan - setpoint gyro") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->setpointKalmanHeading, CONTROL_PERIOD); if (checkProvider(status, "Trajectory Plan - setpoint Kalman filter heading") == ERROR) return(ERROR); status = dm_start_provider(controlDmi->setpointKalmanHeadingDegrees, CONTROL_PERIOD); if (checkProvider(status, "Trajectory Plan - setpoint Kalman filter heading (degrees)") == ERROR) return(ERROR); for (dof = X_POSITION; dof <= HEADING; dof++) statusPtr->move[dof] = dm_start_provider(controlDmi->setpointMoved[dof], CONTROL_PERIOD); if (checkProvider(statusPtr->move[X_POSITION], "Trajectory Plan - x position setpoint moved") == ERROR) return(ERROR); if (checkProvider(statusPtr->move[Y_POSITION], "Trajectory Plan - y position setpoint moved") == ERROR) return(ERROR); if (checkProvider(statusPtr->move[Z_POSITION], "Trajectory Plan - z (depth/altitude) setpoint moved") == ERROR) return(ERROR); if (checkProvider(statusPtr->move[HEADING], "Trajectory Plan - heading setpoint moved") == ERROR) return(ERROR); break; /* check read status */ case READER: if (checkRead(statusPtr->x, "Trajectory Plan - joystick x", TRUE) == ERROR) return(ERROR); if (checkRead(statusPtr->y, "Trajectory Plan - joystick y", TRUE) == ERROR) return(ERROR); if (checkRead(statusPtr->z, "Trajectory Plan - joystick z", TRUE) == ERROR) return(ERROR); if (checkRead(statusPtr->heading, "Trajectory Plan - joystick heading", TRUE) == ERROR) return(ERROR); if (checkRead(statusPtr->autoControl[AUTO_POSITION], "Trajectory Plan - auto position", TRUE) == ERROR) return(ERROR); if (checkRead(statusPtr->autoControl[AUTO_Z], "Trajectory Plan - auto z (depth/altitude)", TRUE) == ERROR) return(ERROR); if (checkRead(statusPtr->autoControl[AUTO_HEADING], "Trajectory Plan - auto heading", TRUE) == ERROR) return(ERROR); break; /* check write status */ case WRITER: if (checkWrite(statusPtr->x, "Trajectory Plan - setpoint x") == ERROR) return(ERROR); if (checkWrite(statusPtr->y, "Trajectory Plan - setpoint y") == ERROR) return(ERROR); if (checkWrite(statusPtr->depth, "Trajectory Plan - setpoint depth") == ERROR) return(ERROR); if (checkWrite(statusPtr->altitude, "Trajectory Plan - setpoint altitude") == ERROR) return(ERROR); if (checkWrite(statusPtr->gyro, "Trajectory Plan - setpoint gyro") == ERROR) return(ERROR); if (checkWrite(statusPtr->kalmanHeading, "Trajectory Plan - setpoint Kalman filter heading") == ERROR) return(ERROR); if (checkWrite(statusPtr->kalmanHeadingDegrees, "Trajectory Plan - setpoint Kalman filter heading (degrees)") == ERROR) return(ERROR); if (checkWrite(statusPtr->move[X_POSITION], "Trajectory Plan - x position setpoint move") == ERROR) return(ERROR); if (checkWrite(statusPtr->move[Y_POSITION], "Trajectory Plan - y position setpoint move") == ERROR) return(ERROR); if (checkWrite(statusPtr->move[Z_POSITION], "Trajectory Plan - z (depth/altitude) setpoint move") == ERROR) return(ERROR); if (checkWrite(statusPtr->move[HEADING], "Trajectory Plan - heading setpoint move") == ERROR) return(ERROR); break; } return(OK); } /* initTrajectoryPlan */ /******************************************************************************/ /* Function : initSetpoints */ /* Purpose : Initializes trajectory setpoints and setpoint moved flags. */ /* Inputs : Control data manager items structure pointer, setpoint pointer, */ /* setpoint moved flags array pointer, wakeup semaphore. */ /* Outputs : Returns OK or ERROR. */ /******************************************************************************/ Errno initSetpoints(controlDMItems *controlDmi, Flt32 setpoint[], MBool setpointMoved[], SEM_ID commandSem) { dmStatus status; /* data manager status */ Flt32 xSensor, ySensor; /* sensor measurements */ Flt32 depthSensor, altitudeSensor; Flt32 gyro; Flt32 kalmanHeading; Flt32 kalmanHeadingDegrees; autoCtrlDof dof; /* control dof counter */ DM_Time sampleTime; /* setpoint sample time */ /* get sensor measurements */ status.x = dm_read(controlDmi->sensorX, (Void *) &xSensor, sizeof(xSensor), (DM_Time *) NULL); if (checkRead(status.x, "Trajectory Plan - sensor x", TRUE) == ERROR) return(ERROR); status.y = dm_read(controlDmi->sensorY, (Void *) &ySensor, sizeof(ySensor), (DM_Time *) NULL); if (checkRead(status.y, "Trajectory Plan - sensor y", TRUE) == ERROR) return(ERROR); status.depth = dm_read(controlDmi->sensorDepth, (Void *) &depthSensor, sizeof(depthSensor), (DM_Time *) NULL); if (checkRead(status.depth, "Trajectory Plan - sensor depth", TRUE) == ERROR) return(ERROR); status.altitude = dm_read(controlDmi->sensorAltitude, (Void *) &altitudeSensor, sizeof(altitudeSensor), (DM_Time *) NULL); if (checkRead(status.altitude, "Trajectory Plan - sensor altitude", TRUE) == ERROR) return(ERROR); status.gyro = dm_read(controlDmi->gyroRadians, (Void *) &gyro, sizeof(gyro), (DM_Time *) NULL); if (checkRead(status.gyro, "Trajectory Plan - gyro", TRUE) == ERROR) return(ERROR); status.kalmanHeading = dm_read(controlDmi->kalmanHeading, (Void *) &kalmanHeading, sizeof(kalmanHeading), (DM_Time *) NULL); if (checkRead(status.kalmanHeading, "Trajectory Plan - Kalman filter heading", TRUE) == ERROR) return(ERROR); /* convert the Kalman filter heading to degrees */ kalmanHeadingDegrees = kalmanHeading * 180.0 / PI; /* set initial setpoints to sensor measurements */ setpoint[X_SETPOINT] = xSensor; setpoint[Y_SETPOINT] = ySensor; setpoint[DEPTH_SETPOINT] = depthSensor; setpoint[ALTITUDE_SETPOINT] = altitudeSensor; setpoint[GYRO_SETPOINT] = gyro; setpoint[KALMAN_HEADING_SETPOINT] = kalmanHeading; gettimeofday(&sampleTime, (struct timezone *) NULL); status.x = dm_write(controlDmi->setpointX, (Void *) &setpoint[X_SETPOINT], sizeof(setpoint[X_SETPOINT]), &sampleTime); status.y = dm_write(controlDmi->setpointY, (Void *) &setpoint[Y_SETPOINT], sizeof(setpoint[Y_SETPOINT]), &sampleTime); status.depth = dm_write(controlDmi->setpointDepth, (Void *) &setpoint[DEPTH_SETPOINT], sizeof(setpoint[DEPTH_SETPOINT]), &sampleTime); status.altitude = dm_write(controlDmi->setpointAltitude, (Void *) &setpoint[ALTITUDE_SETPOINT], sizeof(setpoint[ALTITUDE_SETPOINT]), &sampleTime); status.gyro = dm_write(controlDmi->setpointGyro, (Void *) &setpoint[GYRO_SETPOINT], sizeof(setpoint[GYRO_SETPOINT]), &sampleTime); status.kalmanHeading = dm_write(controlDmi->setpointKalmanHeading, (Void *) &setpoint[KALMAN_HEADING_SETPOINT], sizeof(setpoint[KALMAN_HEADING_SETPOINT]), &sampleTime); status.kalmanHeadingDegrees = dm_write(controlDmi->setpointKalmanHeadingDegrees, (Void *) &kalmanHeadingDegrees, sizeof(kalmanHeadingDegrees), &sampleTime); /* set setpoint moved flags */ for (dof = X_POSITION; dof <= HEADING; dof++) { setpointMoved[dof] = FALSE; status.move[dof] = dm_write(controlDmi->setpointMoved[dof], (Void *) &setpointMoved[dof], sizeof(setpointMoved[dof]), &sampleTime); } /* verify initial writes */ if (initTrajectoryPlan(WRITER, controlDmi, &status, commandSem) == ERROR) return(ERROR); return(OK); } /* initSetpoints */ /******************************************************************************/ /* Function : resetPositionSetpoint */ /* Purpose : Updates the position trajectory setpoint and its setpoint moved */ /* flag. The setpoint moved flag is a change of state variable */ /* which tells the configureMode module that it needs to */ /* reconfigure connections between data manager items for open */ /* versus closed loop control. */ /* Inputs : Joystick command; auto control flag; setpoint; setpoint moved */ /* flag; auto position dm items structure pointer. */ /* Outputs : None. */ /******************************************************************************/ Void resetPositionSetpoint(Int16 command[], MBool autoPosition, Flt32 setpoint[], MBool setpointMoved[], autoPositionDmi *apd) { DM_Time sampleTime; /* setpoint sample time */ /* when not in auto control, vehicle commanded to new setpoint, or joystick */ /* command overrides auto control, update trajectory setpoint */ /* when setpoint moved flag changes, update setpoint moved flag */ if ( (!autoPosition) || /* reset setpoint */ ((autoPosition) && (fabs(command[X_POSITION]) > AUTO_CONTROL_OVERRIDE)) || ((autoPosition) && (fabs(command[Y_POSITION]) > AUTO_CONTROL_OVERRIDE)) ) { dm_read(apd->dmiSensorX, (Void *) &setpoint[X_POSITION], sizeof(setpoint[X_POSITION]), (DM_Time *) NULL); dm_read(apd->dmiSensorY, (Void *) &setpoint[Y_POSITION], sizeof(setpoint[Y_POSITION]), (DM_Time *) NULL); gettimeofday(&sampleTime, (struct timezone *) NULL); if (!(setpointMoved[X_POSITION])) /* reset setpoint moved flag */ { setpointMoved[X_POSITION] = TRUE; dm_write(apd->dmiSetpointMovedX, (Void *) &setpointMoved[X_POSITION], sizeof(setpointMoved[X_POSITION]), &sampleTime); } if (!(setpointMoved[Y_POSITION])) /* reset setpoint moved flag */ { setpointMoved[Y_POSITION] = TRUE; dm_write(apd->dmiSetpointMovedY, (Void *) &setpointMoved[Y_POSITION], sizeof(setpointMoved[Y_POSITION]), &sampleTime); } } else { gettimeofday(&sampleTime, (struct timezone *) NULL); if (setpointMoved[X_POSITION]) /* reset setpoint moved flag */ { setpointMoved[X_POSITION] = FALSE; dm_write(apd->dmiSetpointMovedX, (Void *) &setpointMoved[X_POSITION], sizeof(setpointMoved[X_POSITION]), &sampleTime); } if (setpointMoved[Y_POSITION]) /* reset setpoint moved flag */ { setpointMoved[Y_POSITION] = FALSE; dm_write(apd->dmiSetpointMovedY, (Void *) &setpointMoved[Y_POSITION], sizeof(setpointMoved[Y_POSITION]), &sampleTime); } } /* update trajectory setpoint */ gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(apd->dmiSetpointX, (Void *) &setpoint[X_POSITION], sizeof(setpoint[X_POSITION]), &sampleTime); dm_write(apd->dmiSetpointY, (Void *) &setpoint[Y_POSITION], sizeof(setpoint[Y_POSITION]), &sampleTime); } /* resetPositionSetpoint */ /******************************************************************************/ /* Function : resetSetpoint */ /* Purpose : Updates a trajectory setpoint and its setpoint moved flag. The */ /* setpoint moved flag is a change of state variable which tells */ /* the configureMode module that it needs to reconfigure */ /* connections between data manager items for open versus closed */ /* loop control. */ /* Inputs : Joystick command; auto control flag; setpoint pointer; setpoint */ /* moved flag pointer; trajectory sensor, setpoint and set point */ /* moved flag data manager items. */ /* Outputs : None. */ /******************************************************************************/ Void resetSetpoint(Int16 command, MBool autoControl, Flt32 *setpoint, MBool *setpointMoved, DM_Item dmiSensor, DM_Item dmiSetpoint, DM_Item dmiSetpointMoved) { extern FILE *fpw; /* file ptr for data logging */ Flt32 sensor; /* sensor measurement */ DM_Time sampleTime; /* setpoint sample time */ /* when not in auto control, vehicle commanded to new setpoint, or joystick */ /* command overrides auto control, update trajectory setpoint */ /* when setpoint moved flag changes, update setpoint moved flag */ if ( (!autoControl) || /* reset setpoint */ ((autoControl) && (fabs(command) > AUTO_CONTROL_OVERRIDE)) ) { dm_read(dmiSensor, (Void *) &sensor, sizeof(sensor), (DM_Time *) NULL); *setpoint = sensor; if (!(*setpointMoved)) /* reset setpoint moved flag */ { *setpointMoved = TRUE; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(dmiSetpointMoved, (Void *) setpointMoved, sizeof(*setpointMoved), &sampleTime); } } else if (*setpointMoved) /* reset setpoint move flag */ { *setpointMoved = FALSE; gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(dmiSetpointMoved, (Void *) setpointMoved, sizeof(*setpointMoved), &sampleTime); } /* update trajectory setpoint */ gettimeofday(&sampleTime, (struct timezone *) NULL); dm_write(dmiSetpoint, (Void *) setpoint, sizeof(*setpoint), &sampleTime); } /* resetSetpoint */