/****************************************************************************/ /* Copyright 1992 to 1994 MBARI */ /****************************************************************************/ /* Summary : Moog Thruster Motor Controller Command Module for VxWorks */ /* Filename : moog.c */ /* Author : Andrew Pearce */ /* Project : New ROV */ /* Version : Version 1.0 */ /* Created : 05/19/92 */ /* Modified : 09/22/95 */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /* $Header: moog.c,v 1.2 93/07/01 14:39:08 pean Exp $ * $Log: moog.c,v $ * Revision 1.2 93/07/01 14:39:08 14:39:08 pean (Andrew Pearce) * *** empty log message *** * * Revision 1.1 92/06/29 10:18:22 10:18:22 pean (Andrew Pearce 408-647-3746) * Initial revision * */ /****************************************************************************/ #include /* vxWorks system declarations */ #include /* vxWorks semaphore functions */ #include /* vxWorks string library functions */ #include /* vxWorks va_list declarations */ #include /* vxWorks watch dog timer functions */ #include /* vxWorks Message Queue Library */ #include "mbariTypes.h" /* mbari style guide type declarations */ #include "usrTime.h" /* mbari time and date functions */ #include "sio32.h" /* SIO32 hardware and driver information*/ #include "sio32Server.h" /* Sio32 server communication services */ #include "sio32Client.h" /* Sio32 client communication services */ #include "microdef.h" /* Microcontroller definitions */ #include "applic.h" /* Microcontroller applications */ #include "microcmd.h" /* Microcontroller commands */ #include "moogdef.h" /* Moog motor software definitions */ #include "moogcmd.h" /* Moog motorcontroller commands */ #include "moog.h" /* Application Specific Declarations */ /****************************************************************************/ /* Function : SRQToMotorStatus */ /* Purpose : Converts Motor Status Service Requests to Motor Sttaus */ /* Inputs : Service Request Code */ /* Outputs : Returns Motor Status Manifest Constant */ /****************************************************************************/ motorFaultStatus SRQToMotorStatus(Word motorStatusSRQ) { switch (motorStatusSRQ) /* Examine Service Request Code */ { case MOTOR_CTRL_VOLTS_SRQ: /* Motor Controller Voltage Alarm */ return ( MOTOR_CONTROLLER_VOLTS ); case MOTOR_CTRL_TEMP_SRQ: /* Motor Controller Temperature */ return ( MOTOR_CONTROLLER_TEMP ); case MOTOR_STALL_SRQ: /* Motor Stalled Alarm */ return ( MOTOR_STALL ); case MOTOR_RESOLVER_SRQ: /* Motor Resolver Alarm */ return ( MOTOR_RESOLVER ); case MOTOR_CURRENT_SRQ: /* Motor Over Current Alarm */ return ( MOTOR_OVERCURRENT ); case MOTOR_STATUS_SRQ: /* Motor Status Unknown */ return ( STATUS_UNKNOWN ); case MOTOR_NO_FAULT_SRQ: /* No Motor Faults Detected */ return ( MOTOR_NO_FAULT ); case MOTOR_FAULT_SRQ: /* Undefined Motor Fault Detected */ default: return ( MOTOR_FAULT ); } /* switch */ } /* SRQToMotorStatus() */ /****************************************************************************/ /* Function : motorGetTemperature */ /* Purpose : Returns temperature sensor reading from moog microcontroler*/ /* Inputs : Sio32 channel, pointer to temperature value */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorGetTemperature( sio32Chan *sio32DataChan, Int16 *temperature ) { return (microCommand(sio32DataChan, MOOG_APP|GET_TEMPERATURE, temperature)); } /* motorGetTemperature() */ /****************************************************************************/ /* Function : motorGetTempAlarm */ /* Purpose : Returns temperature alarm status from moog microcontroler */ /* Inputs : Sio32 channel, pointer to temperature alarm status */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorGetTempAlarm( sio32Chan *sio32DataChan, microAlarm *tempAlarmStatus ) { Word alarmStatus; /* Get Temperature Alarm Status from micro */ if (microCommand(sio32DataChan, MOOG_APP | GET_TEMP_ALARM_STATUS, &alarmStatus) == ERROR) return (ERROR); *tempAlarmStatus = (microAlarm) alarmStatus; return (OK); } /* motorGetTempAlarm() */ /******************************************************************************/ /* Function : motorSerialNo */ /* Purpose : returns motor serial number string from moog microcontroller */ /* Inputs : Buffer for serial number string */ /* Outputs : Return OK or ERROR */ /******************************************************************************/ STATUS motorSerialNo(sio32Chan *sio32DataChan, Char *serialNo) { if ( microCmdStringReply( sio32DataChan, MOOG_APP | GET_MOTOR_SER_NO, serialNo, MOTOR_SERNO_LEN ) == ERROR ) return (ERROR); return(OK); } /* motorSerialNo() */ /****************************************************************************/ /* Function : motorSetSerialNo */ /* Purpose : Sends Motor Serial Number String to microcontroller */ /* Inputs : None */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorSetSerialNo(sio32Chan *sio32DataChan, Char *serialNo ) { Byte command[MOTOR_SERNO_LEN + sizeof(Word)]; wordToBuf(command, MOOG_APP | SET_MOTOR_SER_NO); /* put cmd into buffer */ bcopy(serialNo, command + sizeof(Word), min(strlen(serialNo), MOTOR_SERNO_LEN)); return (microWriteBufReply(sio32DataChan, min(strlen((char *) serialNo), MOTOR_SERNO_LEN) + sizeof(Word), command)); } /* motorSetSerialNo() */ /****************************************************************************/ /* Function : motorGetMotorStatus */ /* Purpose : Returns motor status from Moog microcontroller */ /* Inputs : Sio32 channel, pointer to motor status value */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorGetMotorStatus( sio32Chan *sio32DataChan, motorFaultStatus *motorStatus ) { STATUS status; Word motorStatusWord; status = microCommand(sio32DataChan, MOOG_APP | GET_MOTOR_STATUS, &motorStatusWord); if (status == OK) *motorStatus = (motorFaultStatus) motorStatusWord; return (status); } /* motorGetMotorStatus() */ /****************************************************************************/ /* Function : motorGetFoldbackStatus */ /* Purpose : Returns motor foldback status from Moog microcontroller */ /* Inputs : Sio32 channel, pointer to motor foldback status */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorGetFoldbackStatus( sio32Chan *sio32DataChan, MBool *motorFoldbackStatus ) { /* Get Foldback Status from Motor */ if (microCommand(sio32DataChan, MOOG_APP | GET_MOTOR_FOLDBACK, motorFoldbackStatus) !=OK) return(ERROR); /* Map to vxWorks TRUE definition */ if (*motorFoldbackStatus) *motorFoldbackStatus = TRUE; return(OK); } /* motorGetFoldbackStatus() */ /****************************************************************************/ /* Function : motorGetVelocity */ /* Purpose : Returns motor velocity from Moog microcontroller */ /* Inputs : Sio32 channel, pointer to motor velocity value */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorGetVelocity( sio32Chan *sio32DataChan, Int16 *motorVelocity ) { return(microCommand(sio32DataChan, MOOG_APP | GET_MOTOR_VELOCITY, motorVelocity)); } /* motorGetVelocity() */ /****************************************************************************/ /* Function : motorSetMotorEnable */ /* Purpose : Sets state of moog motor enable bit on microcontroller */ /* Inputs : Sio32 channel, Desired state TRUE = On, FALSE = Off */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorSetMotorEnable( sio32Chan *sio32DataChan, MBool motorStatus ) { Word cmdStatus; /* Command Result from microcontroller */ /* Send command to microcontroller */ if ( microCommand(sio32DataChan, MOOG_APP | (motorStatus == TRUE ? MOTOR_ENABLE : MOTOR_DISABLE), &cmdStatus) != OK ) return(ERROR); return (OK); /* Check command result */ } /* motorSetMotorEnable() */ /****************************************************************************/ /* Function : motorGetCtrlMode */ /* Purpose : Returns motor control mode from Moog microcontroller */ /* Inputs : Sio32 channel, pointer to control mode value */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorGetCtrlMode( sio32Chan *sio32DataChan, motorCtrlMode *controlMode ) { Word ctrlMode; STATUS status; status = ( microCommand(sio32DataChan, MOOG_APP | GET_MOTOR_CTRL_MODE, &ctrlMode)); *controlMode = (motorCtrlMode) ctrlMode; return (status); } /* motorGetCtrlMode() */ /****************************************************************************/ /* Function : motorSetCtrlMode */ /* Purpose : Sets moog motor control mode to Moog microcontroller */ /* Inputs : Sio32 channel, control mode */ /* Outputs : Return OK or ERROR */ /****************************************************************************/ STATUS motorSetCtrlMode( sio32Chan *sio32DataChan, motorCtrlMode controlMode ) { Word status; /* Command Result from microcontroller */ /* Send command to microcontroller */ if ( sendMicroCommand( sio32DataChan, &status, sizeof(status), 2, MOOG_APP | SET_MOTOR_CTRL_MODE, controlMode ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&status, 0) == CMD_OK ? OK : ERROR); } /* motorSetCtrlMode() */ /****************************************************************************/ /* Function : motorSetCmdThrust */ /* Purpose : Sets moog motor commanded thrust on microcontroller */ /* Inputs : Sio32 channel, thrust value */ /* Outputs : Return OK or ERROR */ /****************************************************************************/ STATUS motorSetCmdThrust( Reg sio32Chan *sio32DataChan, Reg Int16 cmdThrust ) { Word status; /* Command Result from microcontroller */ if (cmdThrust > MAX_CMD_THRUST) cmdThrust = MAX_CMD_THRUST; else if (cmdThrust < -MAX_CMD_THRUST) cmdThrust = -MAX_CMD_THRUST; /* Send command to microcontroller */ if ( sendMicroCommand( sio32DataChan, &status, sizeof(status), 2, MOOG_APP | SET_MOTOR_CMD_TORQUE, cmdThrust ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&status, 0) == CMD_OK ? OK : ERROR); } /* motorSetCmdThrust() */ /****************************************************************************/ /* Function : motorSetCmdThrustPower */ /* Purpose : Sets moog motor commanded thrust & power for velocity mode */ /* on microcontroller */ /* Inputs : Sio32 channel, thrust & power values */ /* Outputs : Return OK or ERROR */ /****************************************************************************/ STATUS motorSetCmdThrustPower( Reg sio32Chan *sio32DataChan, Reg Int16 cmdThrust, Reg Int16 cmdPower ) { Word reply; /* Command Result from microcontroller */ if (cmdThrust > MAX_CMD_THRUST) cmdThrust = MAX_CMD_THRUST; else if (cmdThrust < -MAX_CMD_THRUST) cmdThrust = -MAX_CMD_THRUST; if (sendMicroCommand(sio32DataChan, &reply, sizeof(Word), 3, MOOG_APP | SET_MOTOR_CMD_VELOCITY, cmdThrust, cmdPower) == ERROR) return(ERROR); /* Check command result */ return (wordFromBuf(&reply, 0) == CMD_OK ? OK : ERROR); } /* motorSetCmdThrustPower() */ /****************************************************************************/ /* Function : motorGetTorqueOffset */ /* Purpose : Returns motor torque offset from Moog microcontroller */ /* Inputs : Sio32 channel, pointer to torque offset value */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorGetTorqueOffset( sio32Chan *sio32DataChan, Int16 *torqueOffset ) { return( microCommand(sio32DataChan, MOOG_APP | GET_TORQUE_OFFSET, torqueOffset)); } /* motorGetTorqueOffset() */ /****************************************************************************/ /* Function : motorSetTorqueOffset */ /* Purpose : Sets moog motor torque offset on microcontroller */ /* Inputs : Sio32 channel, torque offset value */ /* Outputs : Return OK or ERROR */ /****************************************************************************/ STATUS motorSetTorqueOffset( Reg sio32Chan *sio32DataChan, Reg Int16 torqueOffset ) { Word status; /* Command Result from microcontroller */ /* Send command to microcontroller */ if ( sendMicroCommand( sio32DataChan, &status, sizeof(status), 2, MOOG_APP | SET_TORQUE_OFFSET, torqueOffset ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&status, 0) == CMD_OK ? OK : ERROR); } /* motorSetTorqueOffset() */ /****************************************************************************/ /* Function : motorGetCurrentGain */ /* Purpose : Returns motor current gain value from Moog microcontroller */ /* Inputs : Sio32 channel, pointer to current gain value */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorGetCurrentGain( sio32Chan *sio32DataChan, Int16 *currentGain ) { return( microCommand(sio32DataChan, MOOG_APP | GET_CURRENT_MODE_GAIN, currentGain)); } /* motorGetCurrentGain() */ /****************************************************************************/ /* Function : motorSetCurrentGain */ /* Purpose : Sets moog motor current mode gain on microcontroller */ /* Inputs : Sio32 channel, current mode gain value */ /* Outputs : Return OK or ERROR */ /****************************************************************************/ STATUS motorSetCurrentGain( Reg sio32Chan *sio32DataChan, Reg Int16 currentGain ) { Word status; /* Command Result from microcontroller */ /* Send command to microcontroller */ if ( sendMicroCommand( sio32DataChan, &status, sizeof(status), 2, MOOG_APP | SET_CURRENT_MODE_GAIN, currentGain ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&status, 0) == CMD_OK ? OK : ERROR); } /* motorSetCurrentGain() */ /****************************************************************************/ /* Function : motorGetTorqueGain */ /* Purpose : Returns motor torque gain value from Moog microcontroller */ /* Inputs : Sio32 channel, pointer to torque gain value */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorGetTorqueGain( sio32Chan *sio32DataChan, Int16 *torqueGain ) { return( microCommand(sio32DataChan, MOOG_APP | GET_TORQUE_MODE_GAIN, torqueGain)); } /* motorGetTorqueGain() */ /****************************************************************************/ /* Function : motorSetTorqueGain */ /* Purpose : Sets moog motor torque mode gain on microcontroller */ /* Inputs : Sio32 channel, torque mode gain value */ /* Outputs : Return OK or ERROR */ /****************************************************************************/ STATUS motorSetTorqueGain( Reg sio32Chan *sio32DataChan, Reg Int16 torqueGain ) { Word status; /* Command Result from microcontroller */ /* Send command to microcontroller */ if ( sendMicroCommand( sio32DataChan, &status, sizeof(status), 2, MOOG_APP | SET_TORQUE_MODE_GAIN, torqueGain ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&status, 0) == CMD_OK ? OK : ERROR); } /* motorSetTorqueGain() */ /****************************************************************************/ /* Function : motorGetVelocityGains */ /* Purpose : Returns motor velocity gains from Moog microcontroller */ /* Inputs : Sio32 channel, pointer to velocity gain values */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorGetVelocityGains( sio32Chan *sio32DataChan, velocity_gains *velocityGains ) { Word reply[4]; /* Reply to command */ if (sendMicroCommand(sio32DataChan, &reply, sizeof(Word) * 4, 1, MOOG_APP | GET_VEL_MODE_GAINS) == ERROR) return(ERROR); /* Check if command result is CMD OK */ if (wordFromBuf(&reply, 0) != CMD_OK) return(ERROR); /* Command failed so return ERROR */ (*velocityGains)[VEL_PROP_GAIN ] = wordFromBuf(reply, sizeof(Word)); (*velocityGains)[VEL_DERIV_GAIN ] = wordFromBuf(reply, sizeof(Word) * 2); (*velocityGains)[VEL_POWER_GAIN ] = wordFromBuf(reply, sizeof(Word) * 3); return (OK); /* Return OK */ } /* motorGetVelocityGains() */ /****************************************************************************/ /* Function : motorSetVelocityGains */ /* Purpose : Sets moog motor velocity mode gain on microcontroller */ /* Inputs : Sio32 channel, velocity mode gain value */ /* Outputs : Return OK or ERROR */ /****************************************************************************/ STATUS motorSetVelocityGains( sio32Chan *sio32DataChan, velocity_gains velocityGains ) { Word reply; /* Command Result from microcontroller */ if (sendMicroCommand(sio32DataChan, &reply, sizeof(Word), 4, MOOG_APP | SET_VEL_MODE_GAINS, velocityGains[VEL_PROP_GAIN], velocityGains[VEL_DERIV_GAIN], velocityGains[VEL_POWER_GAIN]) == ERROR) return(ERROR); /* Check command result */ return (wordFromBuf(&reply, 0) == CMD_OK ? OK : ERROR); } /* motorSetVelocityGains() */ /****************************************************************************/ /* Function : motorGetLinearizedGains */ /* Purpose : Returns motor linearized gains from Moog microcontroller */ /* Inputs : Sio32 channel, pointer to linearized gain values */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorGetLinearizedGains( sio32Chan *sio32DataChan, linearized_gains *linearizedGains ) { Word reply[3]; /* Reply to command */ if (sendMicroCommand(sio32DataChan, &reply, sizeof(Word) * 3, 1, MOOG_APP | GET_LINEAR_MODE_GAINS) == ERROR) return(ERROR); /* Check if command result is CMD OK */ if (wordFromBuf(reply, 0) != CMD_OK) return(ERROR); /* Command failed so return ERROR */ /* Command OK so update Controller Gains*/ (*linearizedGains)[LINEAR_THRUST_GAIN] = wordFromBuf(reply, sizeof(Word)); (*linearizedGains)[LINEAR_VELOCITY_GAIN] = wordFromBuf(reply, sizeof(Word) * 2); return (OK); /* Return OK */ } /* motorGetLinearizedGains() */ /****************************************************************************/ /* Function : motorSetLinearizedGains */ /* Purpose : Sets moog motor linearized mode gains on microcontroller */ /* Inputs : Sio32 channel, linearized mode gain values */ /* Outputs : Return OK or ERROR */ /****************************************************************************/ STATUS motorSetLinearizedGains( Reg sio32Chan *sio32DataChan, Reg linearized_gains linearizedGains ) { Word reply; /* Command Result from microcontroller */ if (sendMicroCommand(sio32DataChan, &reply, sizeof(Word), 3, MOOG_APP | SET_LINEAR_MODE_GAINS, linearizedGains[LINEAR_THRUST_GAIN], linearizedGains[LINEAR_VELOCITY_GAIN]) == ERROR) return(ERROR); /* Check command result */ return (wordFromBuf(&reply, 0) == CMD_OK ? OK : ERROR); } /* motorSetLinearizedGains() */ /****************************************************************************/ /* Function : motorGetModelParamK4 */ /* Purpose : Returns motor model param K4 from Moog microcontroller */ /* Inputs : Sio32 channel, pointer to model parameter value */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorGetModelParamK4( sio32Chan *sio32DataChan, Int16 *modelParamK4 ) { return( microCommand(sio32DataChan, MOOG_APP | GET_DYNAMIC_MODEL_K4, modelParamK4)); } /* motorGetModelParamK4() */ /****************************************************************************/ /* Function : motorSetModelParamK4 */ /* Purpose : Sets moog motor torque offset on microcontroller */ /* Inputs : Sio32 channel, torque offset value */ /* Outputs : Return OK or ERROR */ /****************************************************************************/ STATUS motorSetModelParamK4( Reg sio32Chan *sio32DataChan, Reg Int16 modelParamK4 ) { Word status; /* Command Result from microcontroller */ if (sendMicroCommand(sio32DataChan, &status, sizeof(status), 2, MOOG_APP | SET_DYNAMIC_MODEL_K4, modelParamK4 ) == ERROR) return(ERROR); /* Check command result */ return (wordFromBuf(&status, 0) == CMD_OK ? OK : ERROR); } /* motorSetModelParamK4() */ /****************************************************************************/ /* Function : motorWriteNVRam */ /* Purpose : Sends Write NV RAM command to moog microcontroller */ /* Inputs : Sio32 channel */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorWriteNVRam( sio32Chan *sio32DataChan ) { char dateTime[NV_RAM_DATE_LEN]; /* Data & Time for Write NV Ram cmd */ char command [NV_RAM_DATE_LEN+8];/* Command buffer */ Word cmdStatus; /* Result of microcontroller command*/ STATUS status; /* Return status of read call */ time_t seconds; /* Used to hold time in seconds */ struct tm *localTime; /* Pointer to local time structure */ seconds = time( (time_t *) NULL); /* Get time in seconds from clock */ localTime = localtime(&seconds); /* Convert to local time structure */ /* Build string - mm/dd/yy hh:mm:ss */ sprintf(dateTime, "%02d/%02d/%02d %02d:%02d:%02d", localTime->tm_mon + 1, localTime->tm_mday, localTime->tm_year, localTime->tm_hour, localTime->tm_min, localTime->tm_sec); wordToBuf(command, MOOG_APP | WRITE_NV_RAM); /* put command into buffer */ /* Append date and time to command */ bcopy(dateTime, command + 2, strlen(dateTime)); /* Lock access to serial channel */ if ((status = semTake(sio32DataChan->mutexSem, sio32DataChan->timeout)) == ERROR) return(ERROR); /* Send the command to the micro */ writeServerPacket(sio32DataChan, (Byte *) command, (Word) strlen(dateTime) + 2); taskDelay(sysClkRateGet()); /* Wait 1 second for NV Ram Write */ /* Read back the reply */ if ((status = readServerPacket(sio32DataChan, (Byte *) command, sizeof(Word), sio32DataChan->timeout)) != ERROR) /* do byte ordering & extract value */ cmdStatus = wordFromBuf(command, 0); semGive(sio32DataChan->mutexSem); /* Release access to serial channel */ /* Check command's returned value */ if ((status == ERROR) || (cmdStatus != CMD_OK)) return (ERROR); /* Command failed so return ERROR */ return(OK); /* Command succeeded so return OK */ } /* motorWriteNVRam() */ /****************************************************************************/ /* Function : motorGetNVRamDate */ /* Purpose : Returns date & time NV RAM was written on microcontroller */ /* Inputs : Sio32 channel, Buffer for date and time string */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorGetNVRamDate( sio32Chan *sio32DataChan, char *dateTime ) { if ( microCmdStringReply( sio32DataChan, MOOG_APP | GET_NV_DATE_TIME, dateTime, NV_RAM_DATE_LEN ) == ERROR ) return (ERROR); return(OK); } /* motorGetNVRamDate() */ /****************************************************************************/ /* Function : motorGetNVRamCycles */ /* Purpose : Returns number of times NV RAM Was written on micro */ /* Inputs : Sio32 channel, pointer to */ /* Outputs : Returns write count or ERROR */ /****************************************************************************/ STATUS motorGetNVRamCycles( sio32Chan *sio32DataChan, Word *NVRamWriteCycles ) { return (microCommand(sio32DataChan, MOOG_APP | GET_NV_CYCLES, NVRamWriteCycles)); } /* motorGetNVRamCycles() */ /****************************************************************************/ /* Function : motorSetTempAlarmThresh */ /* Purpose : Sets moog motor minimum velocity on microcontroller */ /* Inputs : Sio32 channel, minumum velocity value */ /* Outputs : Return OK or ERROR */ /****************************************************************************/ STATUS motorSetTempAlarmThresh( Reg sio32Chan *sio32DataChan, Int16 warnThresh, Int16 alarmThresh ) { Word status; /* Command Result from microcontroller */ /* Send command to microcontroller */ if ( sendMicroCommand( sio32DataChan, &status, sizeof(status), 3, MOOG_APP | SET_TEMP_ALARM_THRESH, warnThresh, alarmThresh ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&status, 0) == CMD_OK ? OK : ERROR); } /* motorSetTempAlarmThresh() */ /****************************************************************************/ /* Function : motorGetTempAlarmThresh */ /* Purpose : Returns temperature alarm threshold from microcontroller */ /* Inputs : Sio32 channel, pointer to temp alarm threshold value */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorGetTempAlarmThresh( sio32Chan *sio32DataChan, Int16 *warnThresh, Int16 *alarmThresh ) { Int16 cmdStatus; /* Command Status */ Word reply[3]; /* Command Result from microcontroller */ /* Send command to microcontroller */ if ( sendMicroCommand( sio32DataChan, &reply, sizeof(Word) * 3, 1, MOOG_APP | GET_TEMP_ALARM_THRESH ) == ERROR ) return(ERROR); /* Check if command result is CMD OK */ if (wordFromBuf(&reply, 0) != CMD_OK) return(ERROR); /* Command failed so return ERROR */ /* Command OK so update thresholds */ wordsFromBuf(reply, 3, &cmdStatus, warnThresh, alarmThresh); return (OK); /* Return OK */ } /* motorGetTempAlarmThresh() */ /****************************************************************************/ /* Function : motorSetCmdPower */ /* Purpose : Sets moog motor commanded power to microcontroller */ /* Inputs : Sio32 channel, power command value */ /* Outputs : Return OK or ERROR */ /****************************************************************************/ STATUS motorSetCmdPower( Reg sio32Chan *sio32DataChan, Reg Int16 commandedPower ) { Word status; /* Command Result from microcontroller */ /* Send command to microcontroller */ if ( sendMicroCommand( sio32DataChan, &status, sizeof(status), 2, MOOG_APP | SET_MOTOR_CMD_POWER, commandedPower ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&status, 0) == CMD_OK ? OK : ERROR); } /* motorSetCmdPower() */ /****************************************************************************/ /* Function : motorSetRegenMode */ /* Purpose : Sets state of moog motor regen limit mode on micro */ /* Inputs : Sio32 channel, Desired regen limit state TRUE/FALSE */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorSetRegenMode( sio32Chan *sio32DataChan, MBool regenMode) { Word cmdStatus; /* Command Result from microcontroller */ /* Send command to microcontroller */ if ( microCommand(sio32DataChan, MOOG_APP | (regenMode == TRUE ? MOTOR_REGEN_LIMIT_ON : MOTOR_REGEN_LIMIT_OFF), &cmdStatus) != OK ) return(ERROR); /* Check command result */ return (cmdStatus == CMD_OK ? OK : ERROR); } /* motorSetRegenMode() */ /****************************************************************************/ /* Function : motorGetRegenLimit */ /* Purpose : Returns regeneration limit value from microcontroller */ /* Inputs : Sio32 channel, pointer to regen limit value */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorGetRegenLimit( sio32Chan *sio32DataChan, Int16 *regenLimit ) { return(microCommand(sio32DataChan, MOOG_APP | GET_REGEN_TORQUE_LIMIT, regenLimit)); } /* motorGetRegenLimit() */ /****************************************************************************/ /* Function : motorSetRegenLimit */ /* Purpose : Sets moog motor regeneration limit on microcontroller */ /* Inputs : Sio32 channel, regeneration torque limit */ /* Outputs : Return OK or ERROR */ /****************************************************************************/ STATUS motorSetRegenLimit( Reg sio32Chan *sio32DataChan, Int16 regenLimit ) { Word status; /* Command Result from microcontroller */ /* Send command to microcontroller */ if ( sendMicroCommand( sio32DataChan, &status, sizeof(status), 2, MOOG_APP | SET_REGEN_TORQUE_LIMIT, regenLimit ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&status, 0) == CMD_OK ? OK : ERROR); } /* motorSetRegenLimit() */ /******************************************************************************/ /* Function : motorGetWaterAlarm */ /* Purpose : Read water alarm status from Moog Motor microcontroller */ /* Inputs : Sio32 socket and pointer to water alarm status */ /* Outputs : Returns OK or ERROR */ /******************************************************************************/ STATUS motorGetWaterAlarm( sio32Chan *sio32DataChan, Word *waterAlarmStatus ) { STATUS status; Word waterAlarms; /* Send command to microcontroller */ status = microCommand(sio32DataChan, MOOG_APP | GET_WATER_ALARM, &waterAlarms); if (status != OK) return(status); /* Map to vxWorks TRUE definition */ *waterAlarmStatus = (waterAlarms ? TRUE : FALSE); return(status); } /* motorGetWaterAlarm() */ /****************************************************************************/ /* Function : motorSetWaterAlarmThresh */ /* Purpose : Sets moog motor water alarm threshold on microcontroller */ /* Inputs : Sio32 channel, water alarm threshold value */ /* Outputs : Return OK or ERROR */ /****************************************************************************/ STATUS motorSetWaterAlarmThresh( Reg sio32Chan *sio32DataChan, Int16 waterAlarmThresh ) { Word status; /* Command Result from microcontroller */ /* Send command to microcontroller */ if ( sendMicroCommand( sio32DataChan, &status, sizeof(status), 2, MOOG_APP | SET_WATER_ALARM_THRESH, waterAlarmThresh ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&status, 0) == CMD_OK ? OK : ERROR); } /* motorSetWaterAlarmThresh() */ /****************************************************************************/ /* Function : motorGetWaterAlarmThresh */ /* Purpose : Returns water alarm threshold from microcontroller */ /* Inputs : Sio32 channel, pointer to water alarm threshold value */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorGetWaterAlarmThresh( sio32Chan *sio32DataChan, Int16 *waterAlarmThresh ) { return(microCommand(sio32DataChan, MOOG_APP | GET_WATER_ALARM_THRESH, waterAlarmThresh)); } /* motorGetWaterAlarmThresh() */ /****************************************************************************/ /* Function : motorRpmCalibrate */ /* Purpose : Commands motor to perform Velocity Offset Calibrationr */ /* Inputs : Sio32 channel */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorRpmCalibrate( sio32Chan *sio32DataChan ) { return( microCommand(sio32DataChan, MOOG_APP | CALC_VELOCITY_OFFSET, NULL)); } /* motorRpmCalibrate() */ /****************************************************************************/ /* Function : motorEnableVelBuf */ /* Purpose : Returns motor model param K4 from Moog microcontroller */ /* Inputs : Sio32 channel, pointer to model parameter value */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS motorEnableVelBuf( sio32Chan *sio32DataChan ) { return( microCommand(sio32DataChan, MOOG_APP | ENABLE_DAS_MODE, NULL)); } /* motorEnableVelBuf() */ /****************************************************************************/ STATUS motorReadVelBuf( sio32Chan *sio32DataChan, Int16 velData[], Int16 torqueData[] ) { Int16 pkt, pktNum, status, val; Byte reply[256]; for (pkt = 0; pkt < MOTOR_DAS_BUF_SIZE / DAS_SAMPLES_PER_PKT; pkt++) { /* Send command to microcontroller */ if ( sendMicroCommand( sio32DataChan, reply, 256, 2, MOOG_APP | GET_DAS_MODE_DATA, pkt ) == ERROR ) return(ERROR); status = wordFromBuf(reply, 0); pktNum = wordFromBuf(reply, sizeof(Word)); for (val = 0; val < DAS_SAMPLES_PER_PKT; val++) { velData[pkt * DAS_SAMPLES_PER_PKT + val] = wordFromBuf(reply, sizeof(Word) * (2 + val + val)); torqueData[pkt * DAS_SAMPLES_PER_PKT + val] = wordFromBuf(reply, sizeof(Word) * (3 + val + val)); } /* for */ } /* for */ return(OK); } /* motorReadVelBuf() */