/****************************************************************************/ /* Copyright 1992 - 1996 MBARI */ /****************************************************************************/ /* Summary : Moog Thruster Motor Controller Interface Module for VxWorks */ /* Filename : thruster.c */ /* Author : Andrew Pearce */ /* Project : Tiburon ROV */ /* Version : Version 1.0 */ /* Created : 05/19/92 */ /* Modified : 07/29/96 */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /* $Header: thruster.c,v 1.2 93/07/01 14:39:11 pean Exp $ * $Log: thruster.c,v $ * Revision 1.2 93/07/01 14:39:11 14:39:11 pean (Andrew Pearce) * *** empty log message *** * * Revision 1.1 92/06/29 10:16:44 10:16:44 pean (Andrew Pearce 408-647-3746) * Initial revision * */ /****************************************************************************/ #include /* vxWorks system declarations */ #include /* vxWorks semaphore functions */ #include /* vxWorks task library functions */ #include /* vxWorks time and date functions */ #include /* vxWorks string function library */ #include /* vxWorks Message Queue Library */ #include /* MBARI style guide type declarations */ #include /* Miscellaneous constants */ #include /* MBARI Rov Application Priorities */ #include /* MBARI time and date functions */ #include /* Data Manager declarations */ #include /* Data Manager error declarations */ #include "powerAlloc.h" /* Device Power Requirements */ #include "powerDM.h" /* Power System Data Manager Items */ #include "powerTypes.h" /* Power management Data Types */ #include "console.h" /* Topside Console Data Manager Items */ #include "sio32.h" /* Sio32 hardware and driver information*/ #include "sio32Server.h" /* Sio32 server communication services */ #include "sio32Client.h" /* Sio32 client communication services */ #include "controlDM.h" /* Control System Data Manager Items */ #include "applic.h" /* Microcontroller applications */ #include "microdef.h" /* Microcontroller definitions */ #include "microcmd.h" /* Microcontroller commands */ #include "microCmd.h" /* Microcontroller serial I/O functions */ #include "microTask.h" /* Microcontroller Task IO definitions */ #include "moogdef.h" /* Moog motor software definitions */ #include "moogcmd.h" /* Moog motor controller commands */ #include "moog.h" /* Moog motor communication functions */ #include "microDm.h" /* Microcontroller Data Manager Items */ #include "thrusterDM.h" /* Thruster Data Manager Item Names */ #define VEL_DM_PERIOD 50000 /* Velocity provider peroid usecs */ #define TEMP_DM_PERIOD 5000000 /* Temperature sensor provider peroid */ #define THRUST_IN_STACKSIZE 4000 /* Thruster Input Task Stack Size+TCB */ #define THRUST_OUT_STACKSIZE 7000 /* Thruster Output Task Stack Size+TCB */ #define MOTOR_STALL_ALARM_LVL 10 /* Consecutive motor stall counts */ #define THRUSTER_SEM_TIMEOUT 500000 /* .5 sec timeout If telmetery is down */ /* stop thrusters turning */ /* Motor will stop if KA packets stop */ #define KEEP_ALIVE_COUNT 3 /* Stop KA Packets after 1 second */ #define KEEP_ALIVE_TICKS KEEP_ALIVE_TIME #define NUM_TASKS 3 /* Number of co-operating tasks */ /****************************************************************************/ const char *thrusterName[] = /* Thruster motor names */ { "Port Horizontal", "Stbd Horizontal", "Forward Lateral", "Aft Lateral", "Port Vertical", "Stbd Vertical" }; const char *thrustChanName[] = /* sio32 serial channel names */ { "/thrust/port_horiz", "/thrust/stbd_horiz", "/thrust/fwd_lat", "/thrust/aft_lat", "/thrust/port_vert", "/thrust/stbd_vert" }; const char *thrustDmPrefix[] = /* Data Manager Item Name Prefix */ { PORT_HORIZ_THRUSTER_DM, STBD_HORIZ_THRUSTER_DM, FWD_LATERAL_THRUSTER_DM, AFT_LATERAL_THRUSTER_DM, PORT_VERT_THRUSTER_DM, STBD_VERT_THRUSTER_DM }; const char *thrusterPowerDmName[] = { THRUSTER_PORT_HORIZ_SWITCH_STATUS_DM, THRUSTER_STBD_HORIZ_SWITCH_STATUS_DM, THRUSTER_FWD_LAT_SWITCH_STATUS_DM, THRUSTER_AFT_LAT_SWITCH_STATUS_DM, THRUSTER_PORT_VERT_SWITCH_STATUS_DM, THRUSTER_STBD_VERT_SWITCH_STATUS_DM }; const char *thrusterReducedPowerConfirmDmName[] = { THRUSTER_PORT_HORIZ_PWR_CONF_DM, THRUSTER_STBD_HORIZ_PWR_CONF_DM, THRUSTER_FWD_LAT_PWR_CONF_DM, THRUSTER_AFT_LAT_PWR_CONF_DM, THRUSTER_PORT_VERT_PWR_CONF_DM, THRUSTER_STBD_VERT_PWR_CONF_DM }; typedef struct /* Thruster Data Manager Item Handles */ { microDmItems microDm; /* Microcontroller Data Manager Items */ DM_Item waterAlarm; /* Water Alarm Detected TRUE/FALSE */ DM_Item waterAlarmThresh; /* Water Alarm Threshold Level K Ohms */ DM_Item microTemperature; /* Temperature sensor value deg C */ DM_Item temperatureAlarm; /* Temperature Alarm Detected TRUE/FALSE*/ DM_Item tempWarnThresh; /* Temperature Warning Threshold */ DM_Item tempAlarmThresh; /* Temperature Alarm Threshold */ DM_Item writeNVRam; /* Write Non-Volatile RAM Command */ DM_Item NVRamCycles; /* Number of writes to Non-Volatile RAM */ DM_Item NVRamDateTime; /* Date & Time NV RAM was last written */ DM_Item NVRamInitialized; /* Non-volatile RAM was re-initialized */ DM_Item motorSerialNo; /* Motor Serial Number String */ DM_Item motorStatus; /* Motor Status from Moog Controller */ DM_Item motorEnableReq; /* Motor Enable/Disable Request */ DM_Item motorEnableStatus; /* Motor Enable Status */ DM_Item motorVelocity; /* Motor measured velocity RPM */ DM_Item motorTorqueOffset; /* Motor Torque Offset Setting PWM Bits */ DM_Item motorCtrlMode; /* Motor control mode */ DM_Item motorCmdThrust; /* Motor Commanded Thrust */ DM_Item motorCmdPower; /* Motor Commanded Power */ DM_Item motorCurrentGain; /* Motor current gain parameter */ DM_Item motorTorqueGain; /* Motor torque gain parameter */ DM_Item motorVelocityGains; /* Motor velocity gain paramaters */ DM_Item motorLinearizedGains; /* Motor linearize gain parameters */ DM_Item motorModelParamK4; /* Motor model parameter K4 */ DM_Item motorRegenLimitMode; /* Motor regeneration limit mode enable */ DM_Item motorRegenLimit; /* Motor regenegation torque limit value*/ DM_Item motorRedPwrConfirmDm; /* Motor Power Reduce Confirmation */ DM_Item motorReducePowerDm; /* Motor Power Reduce Request DM Item */ #ifdef INCLUDE_MOTOR_VEL_BUF DM_Item motorEnableVelBuf; /* Motor Enable Velocity Buffer Data */ DM_Item motorReadVelBuf; /* Motor Read Velocity Buffer Data */ DM_Item motorVelBufData; /* Motor Shaft Velocity Buffer Data */ DM_Item motorTorqueBufData; /* Motor Torque Command Buffer Data */ #endif DM_Item motorRpmCalibrate; /* Motor Velocity Calibration */ DM_Item motorDisableAlarm; /* Serial link failure, so motor disable*/ DM_Item motorStallAlarm; /* Software Motor Stall Detected Alarm */ } thrusterDmItems; /************************* Forward Declarations *****************************/ Void thrusterInTask( Reg thrusterDmItems *dmItems, microIOControl *microIOCtrl); Void thrusterOutTask(thrusterId thruster, Reg thrusterDmItems *dmItems, microIOControl *microIOCtrl); MLocal Void checkMotorReducedPower( DM_Item motorReducePowerDm, DM_Item motorReduceConfirmDm ); /****************************************************************************/ /* Function : updateMicroTempDm */ /* Purpose : Reads micro temperature sensor value & updates dmgr item */ /* Inputs : Sio32 chan & temperature sensor data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS updateMicroTempDm( sio32Chan *sio32DataChan, DM_Item microTempDm ) { Int16 temperature; /* Micro Temperature Sensor Value */ /* Read temperature sensor value */ if (motorGetTemperature(sio32DataChan, &temperature) != OK) return(ERROR); /* Write temperature to Data Manager */ return (writeDmItem(microTempDm, &temperature, sizeof(temperature))); } /* updateMicroTempDm() */ /****************************************************************************/ /* Function : updateTempAlarmDm */ /* Purpose : Reads micro temperature alarm status & updates dmgr item */ /* Inputs : Sio32 chan & temperature alarm data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS updateTempAlarmDm( sio32Chan *sio32DataChan, DM_Item tempAlarmDm ) { microAlarm tempAlarmStatus; /* Micro temperature alarm status */ /* Read temperature alarm status */ if (motorGetTempAlarm(sio32DataChan, &tempAlarmStatus) != OK) return(ERROR); /* Write temperature alarm to Dmgr */ return(writeDmItem(tempAlarmDm, &tempAlarmStatus, sizeof(tempAlarmStatus))); } /* updateTempAlarmDm() */ /****************************************************************************/ /* Function : updateTempAlarm */ /* Purpose : Update temperature alarm DM item. Temperature updated also */ /* so alarm status reflects state of temperature */ /* Inputs : Temp alarm status, serial chan, temp & temp alarm DM items */ /* Outputs : None, but data manager items change */ /****************************************************************************/ MLocal Void updateTempAlarm(sio32Chan *sio32DataChan, microAlarm tempAlarm, DM_Item tempAlarmDm, Reg DM_Item temperatureDm) { /* Use urgent calls as task is not provider */ dm_urgent_open(temperatureDm, DM_STATIC); /* Read temperature and update Dmgr item */ updateMicroTempDm(sio32DataChan, temperatureDm); dm_urgent_close(temperatureDm); /* Write temperature alarm to Dmgr */ writeDmItem(tempAlarmDm, &tempAlarm, sizeof(tempAlarm)); } /* updateTempAlarm() */ /****************************************************************************/ /* Function : updateWaterAlarmDm */ /* Purpose : Reads micro Water Alarm status & updates dmgr item */ /* Inputs : Sio32 chan & water alarm status data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS updateWaterAlarmDm( sio32Chan *sio32DataChan, DM_Item waterAlarmDm ) { Word waterAlarmStatus; /* Microcontroller Water Alarm Status */ /* Read Water Alarm Status */ if (motorGetWaterAlarm(sio32DataChan, &waterAlarmStatus) != OK) return(ERROR); /* Map from a word to a boolean */ if (waterAlarmStatus) waterAlarmStatus = (Word) TRUE; waterAlarmStatus = (waterAlarmStatus ? TRUE : FALSE); /* Write water alarm status to Dmgr */ return (writeDmItem(waterAlarmDm, &waterAlarmStatus, sizeof(waterAlarmStatus))); } /* updateWaterAlarmDm() */ /****************************************************************************/ /* Function : initWaterAlarmThreshDm */ /* Purpose : Read Water Alarm Threshold and update data manager items */ /* Inputs : Sio32 channel, water alarm threshold dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS initWaterAlarmThreshDm ( sio32Chan *sio32DataChan, DM_Item waterThreshDm ) { Int16 waterAlarmThresh; /* Water Alarm Threshold from motor */ /* Get Water Alarm Thresh from motor*/ if (motorGetWaterAlarmThresh(sio32DataChan, &waterAlarmThresh) != OK) return (ERROR); /* We MUST use urgent open here as */ /* another task is normal provider */ return(urgentWriteDmItem(waterThreshDm, &waterAlarmThresh, sizeof(waterAlarmThresh))); } /* initWaterAlarmThreshDm() */ /****************************************************************************/ /* Function : writeWaterAlarmThresh */ /* Purpose : Get water alarm threshold from DM & send to moto */ /* Inputs : Sio32 channel, water alarm threshold dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS writeWaterAlarmThresh( sio32Chan *sio32DataChan, DM_Item waterAlarmThreshDm ) { Int16 waterAlarmThresh; /* Water Alarm Threshold for motor */ /* Read Alarm Threshold from DM */ if (dm_read( waterAlarmThreshDm, (char *) &waterAlarmThresh, sizeof(waterAlarmThresh), (DM_Time *) NULL) == SUCCESS) /* Send values to motor micro */ if (motorSetWaterAlarmThresh( sio32DataChan, waterAlarmThresh) == OK) return (OK); return(ERROR); } /* writeWaterAlarmThresh() */ /****************************************************************************/ /* Function : updateMotorSerNoDm */ /* Purpose : Reads motor serial number string & updates dmgr item */ /* Inputs : Sio32 chan & motor serial number data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS updateMotorSerNoDm( sio32Chan *sio32DataChan, DM_Item motorSerNoDm ) { Char serialNo[MOTOR_SERNO_LEN]; /* Motor Serial Number String */ /* Read serial number from motor micro */ if (motorSerialNo(sio32DataChan, (Char *) serialNo ) != OK) return(ERROR); /* Write serial number to Data Manager */ return(writeDmItem(motorSerNoDm, serialNo, strlen(serialNo) + 1)); } /* updateMotorSerNoDm() */ /****************************************************************************/ /* Function : writeMotorSerialNo */ /* Purpose : Get new serial number string from DM and send to motor */ /* Inputs : Sio32 channel, Serial Number DM item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS writeMotorSerialNo( sio32Chan *sio32DataChan, DM_Item serialNoDm ) { Char serialNo[MOTOR_SERNO_LEN + 1]; /* Read Serial Number from dmgr */ if (dm_read( serialNoDm, (char *) &serialNo, MOTOR_SERNO_LEN + 1, (DM_Time *) NULL) == SUCCESS ) /* Send Serial No to motor micro */ if (motorSetSerialNo( sio32DataChan, serialNo) == OK) return (OK); return(ERROR); } /* writeMotorSerialNo() */ /****************************************************************************/ /* Function : updateMotorStatusDm */ /* Purpose : Reads motor status word and updates data manager item */ /* Inputs : Sio32 chan & motor status data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS updateMotorStatusDm( sio32Chan *sio32DataChan, DM_Item motorStatusDm ) { motorFaultStatus motorStatus; /* Motor Status Word */ /* Read motor status from motor micro */ if (motorGetMotorStatus(sio32DataChan, &motorStatus) != OK) return(ERROR); /* Write motor status to Data Manager */ return (writeDmItem(motorStatusDm, &motorStatus, sizeof( motorStatus))); } /* updateMotorStatusDm() */ /****************************************************************************/ /* Function : updateMotorVelocityDm */ /* Purpose : Reads motor velocity and updates data manager item */ /* Inputs : Sio32 chan & motor velocity data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS updateMotorVelocityDm( sio32Chan *sio32DataChan, DM_Item motorVelocityDm ) { Int16 velocity; /* Motor Actual Velocity Value */ /* Read motor actual velocity from micro*/ if (motorGetVelocity(sio32DataChan, &velocity) != OK) return(ERROR); /* Write motor velocity to Data Manager */ return (writeDmItem(motorVelocityDm, &velocity, sizeof(velocity))); } /* updateMotorStatusDm() */ /****************************************************************************/ /* Function : initMotorEnableDm */ /* Purpose : Uses urgent calls to set Motor Enable Dmgr item = FALSE */ /* Inputs : Motor velocity data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal Void initMotorEnableDm( DM_Item motorEnableDm, motorEnableState motorEnable ) { /* Use urgent calls as task is not provider */ /* Set Motor Enable Status to FALSE */ urgentWriteDmItem(motorEnableDm, &motorEnable, sizeof(motorEnable)); } /* initMotorEnableDm() */ /****************************************************************************/ /* Function : writeMotorEnable */ /* Purpose : Read Motor Enable Status from Dmgr & send command to motor */ /* Inputs : Sio32 chan & motor enable data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS writeMotorEnable( sio32Chan *sio32DataChan, DM_Item motorEnableDm, DM_Item motorEnableStatus ) { motorEnableState motorEnable; /* Motor Enable State */ /* Read Motor Enable Status from Dmgr */ if (dm_read( motorEnableDm, (char *) &motorEnable, sizeof(motorEnable), (DM_Time *)NULL) == SUCCESS ) { /* Write 0 torque command to motor */ motorSetCmdThrustPower( sio32DataChan, 0, 0); /* Write Motor Enable Command to motor */ if (motorSetMotorEnable( sio32DataChan, (motorEnable == ENABLE_MOTOR ? TRUE : FALSE) ) == OK) { if (motorEnable == ENABLE_MOTOR) taskDelay(sysClkRateGet() / 10); /* Wait 100 msec */ initMotorEnableDm( motorEnableStatus, motorEnable ); return (OK); } /* if */ } /* if */ return(ERROR); } /* writeMotorEnable() */ /****************************************************************************/ /* Function : initMotorRegenModeDm */ /* Purpose : Uses urgent calls to set Motor Regen Mode Dmgr item */ /* Inputs : Motor regen limit mode Data Manager Item Handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS initMotorRegenModeDm(sio32Chan *sio32DataChan, DM_Item RegenModeDm, MBool regenMode ) { /* Write Motor Regen Mode Command to motor */ if (motorSetRegenMode( sio32DataChan, regenMode) == OK) return (OK); /* Use urgent calls as task is not provider */ /* Set Motor Enable Status to FALSE */ return (urgentWriteDmItem(RegenModeDm, ®enMode, sizeof(regenMode))); } /* initMotorRegenModeDm() */ /****************************************************************************/ /* Function : writeMotorRegenMode */ /* Purpose : Read Motor Regen Mode from Dmgr & send command to motor */ /* Inputs : Sio32 chan & motor regen mode data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS writeMotorRegenMode( sio32Chan *sio32DataChan, DM_Item motorRegenModeDm ) { MBool regenMode; /* Motor Regen Mode Status */ /* Read Motor Regen Mode from Dmgr */ if (dm_read( motorRegenModeDm, (char *) ®enMode, sizeof(regenMode), (DM_Time *)NULL) == SUCCESS ) /* Send Motor Regen Mode Cmd to motor */ if (motorSetRegenMode( sio32DataChan, regenMode) == OK) return (OK); return(ERROR); } /* writeMotorRegenMode() */ /****************************************************************************/ /* Function : initMotorRegenLimitDm */ /* Purpose : Read Motor Regen Limit from motor & update data mgr item */ /* Inputs : Sio32 channel, regen limit dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS initMotorRegenLimitDm( sio32Chan *sio32DataChan, DM_Item regenLimitDm ) { Nat16 regenLimit; /* Regen Limit from motor */ /* Get regen limit from motor */ if (motorGetRegenLimit( sio32DataChan, ®enLimit ) != OK) return (ERROR); /* We MUST use urgent open here as */ /* another task is normal provider */ return(urgentWriteDmItem(regenLimitDm, ®enLimit, sizeof(regenLimit))); } /* initMotorRegenLimitDm() */ /****************************************************************************/ /* Function : writeMotorRegenLimit */ /* Purpose : Get regen limit from dm item and send to motor */ /* Inputs : Sio32 channel, regen limit dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS writeMotorRegenLimit( sio32Chan *sio32DataChan, DM_Item regenLimitDm ) { Int16 regenLimit; /* Regen Limit dm value */ /* Read regen limit from dmgr */ if (dm_read( regenLimitDm, (char *) ®enLimit, sizeof(regenLimit), (DM_Time *)NULL) == SUCCESS ) /* Send value to motor micro */ if (motorSetRegenLimit( sio32DataChan, regenLimit) == OK) return (OK); return(ERROR); } /* writeRegenLimit() */ /****************************************************************************/ /* Function : initMotorCtrlModeDm */ /* Purpose : Uses urgent calls to set Motor Control Mode Data Manager */ /* item to the value obtained from the motor */ /* Inputs : Sio32 chan & motor control mode data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS initMotorCtrlModeDm( sio32Chan *sio32DataChan, DM_Item motorCtrlModeDm ) { motorCtrlMode controlMode; /* Motor Control Mode */ /* Read motor control mode */ if (motorGetCtrlMode( sio32DataChan, &controlMode ) != OK) return (ERROR); /* Use urgent calls as task is not provider */ /* Write Motor Control Mode to Data Manager */ urgentWriteDmItem(motorCtrlModeDm, &controlMode, sizeof(controlMode)); return(OK); } /* initMotorCtrlModeDm() */ /****************************************************************************/ /* Function : writeMotorCtrlMode */ /* Purpose : Read Motor Enable Status from Dmgr & send command to motor */ /* Inputs : Sio32 chan & motor enable data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS writeMotorCtrlMode( sio32Chan *sio32DataChan, DM_Item motorCtrlModeDm ) { motorCtrlMode controlMode; /* Motor Control Mode */ /* Read Motor Control Mode from Dmgr */ if (dm_read( motorCtrlModeDm, (char *) &controlMode, sizeof(controlMode), (DM_Time *)NULL) == SUCCESS ) /* Write Motor Control Mode to motor */ if (motorSetCtrlMode( sio32DataChan, controlMode) == OK) return (OK); return(ERROR); } /* writeMotorCtrlMode() */ /****************************************************************************/ /* Function : initMotorCmdThrustDm */ /* Purpose : Uses urgent calls to set Commanded Thrust Data Mananger */ /* item to the value obtained from the motor */ /* Inputs : Sio32 chan & motor enable data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS initMotorCmdThrustDm( sio32Chan *sio32DataChan, DM_Item motorCmdThrustDm ) { Int16 motorCmdThrust = 0; /* Commanded Thrust Value */ /* Use urgent calls as task is not provider */ /* Write Commanded Thrust to Data Manager */ return (urgentWriteDmItem(motorCmdThrustDm, &motorCmdThrust, sizeof(motorCmdThrust))); } /* initMotorCmdThrustDm() */ /****************************************************************************/ /* Function : writeMotorCmdThrust */ /* Purpose : Read Commanded Thrust from Dmgr & send command to motor */ /* Inputs : Sio32 chan & motor commanded thrust Dmgr item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS writeMotorCmdThrust( sio32Chan *sio32DataChan, DM_Item motorCmdThrustDm ) { Int16 thrustNewtons; /* Commanded Thrust from control system */ Int16 thrustCmd; /* Thrust value to send to thruster motor */ /* Read Commanded Thrust value from Dmgr */ if (dm_read( motorCmdThrustDm, (char *) &thrustNewtons, sizeof(thrustNewtons), (DM_Time *)NULL) == SUCCESS ) /* Write torque command to motor */ if (motorSetCmdThrust( sio32DataChan, thrustNewtons ) == OK) return (OK); return(ERROR); } /* writeMotorCmdThrust() */ /****************************************************************************/ /* Function : initMotorCmdPowerDm */ /* Purpose : Uses urgent calls to set Commanded Power Data Mananger */ /* Inputs : Sio32 chan & motor commanded power data manager item handle*/ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS initMotorCmdPowerDm( sio32Chan *sio32DataChan, DM_Item motorCmdPowerDm ) { Int16 motorCmdPower = 0; /* Commanded Power Value */ /* Use urgent calls as task is not provider */ /* Write Commanded Power to Data Manager */ return (urgentWriteDmItem(motorCmdPowerDm, &motorCmdPower, sizeof(motorCmdPower))); } /* initMotorCmdPowerDm() */ /****************************************************************************/ /* Function : writeMotorCmdPower */ /* Purpose : Read Commanded Power from Dmgr & send command to motor */ /* Inputs : Sio32 chan & motor commanded power Dmgr item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS writeMotorCmdPower( sio32Chan *sio32DataChan, DM_Item motorCmdPowerDm ) { Int16 motorCmdPower; /* Motor Commanded Power Value */ /* Read Commanded Power value from Dmgr */ if (dm_read( motorCmdPowerDm, (char *) &motorCmdPower, sizeof(motorCmdPower), (DM_Time *)NULL) == SUCCESS ) /* Write torque command to motor */ if (motorSetCmdPower( sio32DataChan, motorCmdPower) == OK) return (OK); return(ERROR); } /* writeMotorCmdPower() */ /****************************************************************************/ /* Function : writeMotorCmdThrustPower */ /* Purpose : Read Commanded Thrust & Power from Dmgr & send to motor */ /* Inputs : Sio32 chan & commanded thrust & power Dmgr item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS writeMotorCmdThrustPower( sio32Chan *sio32DataChan, DM_Item motorCmdThrustDm, DM_Item motorCmdPowerDm, DM_Item motorReducePowerDm, DM_Item motorRedPwrConfirmDm ) { Int16 thrustNewtons; /* Commanded Thrust from control system */ Int16 motorCmdThrust; /* Motor Commanded Thrust Value */ Int16 motorCmdPower; /* Motor Commanded Power Value */ /* Read Commanded Thrust value from Dmgr */ if (dm_read( motorCmdThrustDm, (char *) &thrustNewtons, sizeof(thrustNewtons), (DM_Time *) NULL) == SUCCESS ) { /* Read Commanded Power value from Dmgr */ if (dm_read( motorCmdPowerDm, (char *) &motorCmdPower, sizeof(motorCmdPower), (DM_Time *) NULL) == SUCCESS ) /* Write torque command to motor */ if (motorSetCmdThrustPower( sio32DataChan, thrustNewtons, motorCmdPower) == OK) { /* Confirm motor power reduction */ checkMotorReducedPower( motorReducePowerDm, motorRedPwrConfirmDm ); return (OK); } /* if */ } /* if */ return(ERROR); } /* writeMotorCmdThrustPower() */ /****************************************************************************/ /* Function : initMotorTorqueOffsetDm */ /* Purpose : Read Torque Offset from motor & update data manager item */ /* Inputs : Sio32 channel, torque offset dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS initMotorTorqueOffsetDm( sio32Chan *sio32DataChan, DM_Item torqueOffsetDm ) { Int16 torqueOffset; /* Torque Offset from motor */ /* Get torque offset from motor */ if (motorGetTorqueOffset( sio32DataChan, &torqueOffset ) != OK) return (ERROR); /* We MUST use urgent open here as */ /* another task is normal provider */ return(urgentWriteDmItem(torqueOffsetDm, &torqueOffset, sizeof(torqueOffset))); } /* initMotorTorqueOffsetDm() */ /****************************************************************************/ /* Function : writeTorqueOffset */ /* Purpose : Get torque offset from dm item and send to motor */ /* Inputs : Sio32 channel, torque offset dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS writeTorqueOffset( sio32Chan *sio32DataChan, DM_Item torqueOffsetDm ) { Int16 torqueOffset; /* Torque offset dm value */ /* Read torque offset from dmgr */ if (dm_read( torqueOffsetDm, (char *) &torqueOffset, sizeof(torqueOffset), (DM_Time *)NULL) == SUCCESS ) /* Send value to motor micro */ if (motorSetTorqueOffset( sio32DataChan, torqueOffset) == OK) return (OK); return(ERROR); } /* writeTorqueOffset() */ /****************************************************************************/ /* Function : initMotorCurrentGainDm */ /* Purpose : Read Current mode gain from motor & update data mgr item */ /* Inputs : Sio32 channel, current mode gain dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS initMotorCurrentGainDm( sio32Chan *sio32DataChan, DM_Item currentGainDm ) { Int16 currentGain; /* Current Gain from motor */ /* Get Current Gain from motor */ if (motorGetCurrentGain( sio32DataChan, ¤tGain ) != OK) return (ERROR); /* We MUST use urgent open here as */ /* another task is normal provider */ return (urgentWriteDmItem(currentGainDm, ¤tGain, sizeof(currentGain))); } /* initMotorCurrentGain() */ /****************************************************************************/ /* Function : writeMotorCurrentGain */ /* Purpose : Get current gain from dm item and send to motor */ /* Inputs : Sio32 channel, current gain dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS writeMotorCurrentGain( sio32Chan *sio32DataChan, DM_Item currentGainDm ) { Int16 currentGain; /* Current gain dm value */ /* Read current gain from dmgr */ if (dm_read( currentGainDm, (char *) ¤tGain, sizeof(currentGain), (DM_Time *)NULL) == SUCCESS ) /* Send value to motor micro */ if (motorSetCurrentGain( sio32DataChan, currentGain) == OK) return (OK); return(ERROR); } /* writeMotorCurrentGain() */ /****************************************************************************/ /* Function : initMotorTorqueGainDm */ /* Purpose : Read Torque mode gain from motor & update data mgr item */ /* Inputs : Sio32 channel, torque mode gain dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS initMotorTorqueGainDm( sio32Chan *sio32DataChan, DM_Item torqueGainDm ) { Int16 torqueGain; /* Torque Gain from motor */ /* Get Torque Gain from motor */ if (motorGetTorqueGain( sio32DataChan, &torqueGain ) != OK) return (ERROR); /* We MUST use urgent open here as */ /* another task is normal provider */ return (urgentWriteDmItem(torqueGainDm, &torqueGain, sizeof(torqueGain))); } /* initMotorTorqueGainDm() */ /****************************************************************************/ /* Function : writeMotorTorqueGain */ /* Purpose : Get torque gain from dm item and send to motor */ /* Inputs : Sio32 channel, torque gain dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS writeMotorTorqueGain( sio32Chan *sio32DataChan, DM_Item torqueGainDm ) { Int16 torqueGain; /* Torque gain dm value */ /* Read torque gain from dmgr */ if (dm_read( torqueGainDm, (char *) &torqueGain, sizeof(torqueGain), (DM_Time *)NULL) == SUCCESS ) /* Send value to motor micro */ if (motorSetTorqueGain( sio32DataChan, torqueGain) == OK) return (OK); return(ERROR); } /* writeMotorTorqueGain() */ /****************************************************************************/ /* Function : initMotorVelocityGainDm */ /* Purpose : Read Velocity mode gains from motor & update data mgr item */ /* Inputs : Sio32 channel, velocity mode gain dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS initMotorVelocityGainsDm( sio32Chan *sio32DataChan, DM_Item velocityGainsDm ) { velocity_gains gains; /* Velocity Gains from motor */ /* Get velocity Gains from motor */ if (motorGetVelocityGains( sio32DataChan, &gains ) != OK) return (ERROR); /* We MUST use urgent open here as */ /* another task is normal provider */ return (urgentWriteDmItem( velocityGainsDm, gains, sizeof(velocity_gains))); } /* initMotorVelocityGainsDm() */ /****************************************************************************/ /* Function : writeMotorVelocityGains */ /* Purpose : Get velocity gains from dm item and send to motor */ /* Inputs : Sio32 channel, velocity gain dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS writeMotorVelocityGains( sio32Chan *sio32DataChan, DM_Item velocityGainsDm ) { velocity_gains gains; /* Velocity Gains from motor */ /* Read velocity gains from dmgr */ if (dm_read( velocityGainsDm, (char *) &gains, sizeof(gains), (DM_Time *)NULL) == SUCCESS ) /* Send values to motor micro */ if (motorSetVelocityGains( sio32DataChan, gains) == OK) return (OK); return(ERROR); } /* writeMotorVelocityGains() */ /****************************************************************************/ /* Function : initMotorLinearizedGainsDm */ /* Purpose : Read Linearized mode gains from motor & update data mgr */ /* Inputs : Sio32 channel, linearized mode gains dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS initMotorLinearizedGainsDm(sio32Chan *sio32DataChan, DM_Item linearizedGainsDm) { linearized_gains gains; /* Linearized Gains from motor */ /* Get Linearized Gains from motor */ if (motorGetLinearizedGains( sio32DataChan, &gains ) != OK) return (ERROR); /* We MUST use urgent open here as */ /* another task is normal provider */ return (urgentWriteDmItem(linearizedGainsDm, gains, sizeof(linearized_gains))); } /* initMotorLinearizedGainsDm() */ /****************************************************************************/ /* Function : writeMotorLinearizedGains */ /* Purpose : Get linearized gains from dm item and send to motor */ /* Inputs : Sio32 channel, linearized gains dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS writeMotorLinearizedGains( sio32Chan *sio32DataChan, DM_Item linearizedGainsDm ) { linearized_gains gains; /* Linearized Gains from motor */ /* Read Linearized Gains from dmgr */ if (dm_read( linearizedGainsDm, (char *) &gains, sizeof(gains), (DM_Time *)NULL) == SUCCESS ) /* Send values to motor micro */ if (motorSetLinearizedGains( sio32DataChan, gains) == OK) return (OK); return(ERROR); } /* writeMotorLinearizedGains() */ /****************************************************************************/ /* Function : initMotorModelParamK4 */ /* Purpose : Read Motor Dynamic Model param K4 & update data mgr item */ /* Inputs : Sio32 channel, dynamic model dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS initMotorModelParamK4Dm( sio32Chan *sio32DataChan, DM_Item modelParamK4Dm ) { Int16 modelParamK4; /* Motor Model Param K4 from motor */ /* Get model param k4 from motor */ if (motorGetModelParamK4( sio32DataChan, &modelParamK4 ) != OK) return (ERROR); /* We MUST use urgent open here as */ /* another task is normal provider */ return (urgentWriteDmItem(modelParamK4Dm, &modelParamK4, sizeof(modelParamK4))); } /* initMotorModelParamK4Dm() */ /****************************************************************************/ /* Function : writeModelParamK4 */ /* Purpose : Get torque offset from dm item and send to motor */ /* Inputs : Sio32 channel, torque offset dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS writeModelParamK4( sio32Chan *sio32DataChan, DM_Item modelParamK4Dm ) { Int16 modelParamK4; /* Motor Model Param K4 from motor */ /* Read model param from dmgr */ if (dm_read( modelParamK4Dm, (char *) &modelParamK4, sizeof(modelParamK4), (DM_Time *)NULL) == SUCCESS ) /* Send value to motor micro */ if (motorSetModelParamK4( sio32DataChan, modelParamK4) == OK) return (OK); return(ERROR); } /* writeModelParamK4() */ /****************************************************************************/ /* Function : initTempAlarmThreshDm */ /* Purpose : Read Temp Alarm Thresholds and update data manager items */ /* Inputs : Sio32 channel, temp alarm threshold dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS initTempAlarmThreshDm ( sio32Chan *sio32DataChan, DM_Item warnThreshDm, DM_Item alarmThreshDm ) { Int16 warnThresh, alarmThresh; /* Temp Alarm Threshold from motor */ /* Get Temp Alarm Thresh from motor*/ if (motorGetTempAlarmThresh(sio32DataChan, &warnThresh, &alarmThresh) != OK) return (ERROR); /* We MUST use urgent open here as */ /* another task is normal provider */ urgentWriteDmItem(warnThreshDm, &warnThresh, sizeof(warnThresh)); urgentWriteDmItem(alarmThreshDm, &alarmThresh, sizeof(alarmThresh)); return(OK); } /* initTempAlarmThreshDm() */ /****************************************************************************/ /* Function : writeTempAlarmThresh */ /* Purpose : Get temperature alarm thresholds from DM & send to motor */ /* Inputs : Sio32 channel, minimum velocity dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS writeTempAlarmThresh( sio32Chan *sio32DataChan, DM_Item alarmThreshDm, DM_Item warnThreshDm ) { Int16 warnThresh, alarmThresh; /* Temp Alarm Threshold from motor */ /* Read Alarm Thresholds from dmgr */ if (dm_read( warnThreshDm, (char *) &warnThresh, sizeof(warnThresh), (DM_Time *) NULL) == SUCCESS) if (dm_read( alarmThreshDm, (char *) &alarmThresh, sizeof(alarmThresh), (DM_Time *) NULL) == SUCCESS) /* Send values to motor micro */ if (motorSetTempAlarmThresh( sio32DataChan, warnThresh, alarmThresh) == OK) return (OK); return(ERROR); } /* writeTempAlarmThresh() */ /****************************************************************************/ /* Function : checkMotorPowerReduced */ /* Purpose : Check Reduced Power Flag and confirm power reduction */ /* Inputs : Reduced Power & Confirmation Data manager Items */ /* Outputs : None */ /****************************************************************************/ Void checkMotorReducedPower(DM_Item motorReducePowerDm, DM_Item motorReduceConfirmDm) { MBool reducedFlag; /* Read Motor Power Reduced Flag from DM*/ if (dm_read( motorReducePowerDm, (char *) &reducedFlag, sizeof(reducedFlag), (DM_Time *)NULL) == SUCCESS ) if (reducedFlag) writeDmItem( motorReduceConfirmDm, &reducedFlag, sizeof(reducedFlag) ); } /* checkMotorReducedPower() */ /****************************************************************************/ /* Function : updateMotorNVRamDm */ /* Purpose : Read NV RAM Write Count and Date/Time string from micro */ /* Inputs : Sio32 channel, Write Count and Date/Time dm item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS updateMotorNVRamDm( sio32Chan *sio32DataChan, DM_Item NVRamCyclesDm, DM_Item NVRamDateDm ) { Nat16 NVRamCycles; /* NV RAM Write Count */ char NVRamDateTime[NV_RAM_DATE_LEN]; /* Modification Date & Time */ /* Get Write Count from micro */ if ((motorGetNVRamCycles( sio32DataChan, &NVRamCycles ) != OK) || (writeDmItem(NVRamCyclesDm, &NVRamCycles, sizeof(NVRamCycles)) != OK)) { logMsg("Error updating NV Ram Cycles\n"); return (ERROR); } /* if */ /* Get Write Date & time from micro */ if ((motorGetNVRamDate( sio32DataChan, NVRamDateTime ) != OK) || (writeDmItem(NVRamDateDm, NVRamDateTime, strlen(NVRamDateTime)) != OK)) { logMsg("Error updating NV Ram Date/Time\n"); return (ERROR); } /* if */ return(OK); } /* updateMotorNVRamDm() */ MLocal motorEnableState motorEnabledState(thrusterDmItems *dmItems) { motorEnableState motorEnable; /* Motor Enable State */ /* Check that the motor is enabled */ if (dm_read( dmItems->motorEnableStatus, (char *) &motorEnable, sizeof(motorEnable), (DM_Time *)NULL) == SUCCESS) return (motorEnable); return (MOTOR_DISABLE); } /* motorEnabledState() */ MLocal Void checkForMotorStall(thrusterDmItems *dmItems, Nat16 *motorStallCount) { Int16 velocity; Int16 thrust; /* Check that the motor is enabled */ if (motorEnabledState(dmItems) != ENABLE_MOTOR) return; /* Motor Disabled so exit */ if (dm_read( dmItems->motorVelocity, (char *) &velocity, sizeof(velocity), (DM_Time *)NULL) == SUCCESS ) { if (dm_read( dmItems->motorCmdThrust, (char *) &thrust, sizeof(thrust), (DM_Time *)NULL) == SUCCESS ) { if ((thrust != 0) && (velocity == 0)) { *motorStallCount++; if (*motorStallCount > MOTOR_STALL_ALARM_LVL) { *motorStallCount = 0; writeDmItem(dmItems->motorStallAlarm, NULL, 0); } /* if */ } /* if */ else *motorStallCount = 0; } /* if */ } /* if */ } /* checkForMotorStall() */ /*****************************************************************************/ /* Function : processThrusterSRQ */ /* Purpose : Decode Service Request and take appropriate action */ /* Inputs : SRQ code, array of DM items, serial channel */ /* Outputs : Returns SRQ received & many DM items are changed */ /*****************************************************************************/ MLocal Word processThrusterSRQ(Reg thrusterDmItems *dmItems, microIOControl *microIOCtrl) { Word status; /* Micro Alarm Status Word */ Word srqRequest; /* Service Request Value */ motorFaultStatus motorStatus; /* Motor Status Code from SRQ */ Byte buffer[SRQ_PACKET_LEN]; /* Buffer for micro communications data */ if (readSRQPacket(SERIAL_CHAN, buffer, SRQ_PACKET_LEN, WAIT_FOREVER) == ERROR) return (ERROR); /* Service Request received so process it*/ srqRequest = wordFromBuf(buffer, 0); switch (srqRequest) /* Decode Service Request from micro */ { case MICRO_RESET_SRQ: /* Microcontroller Reset Occurred */ /* Post Reset Event DM Item */ if (microIOCtrl->announceResets) microResetEventDm(dmItems->microDm.resetEvent); break; case MOTOR_DISABLE_SRQ: logMsg("Motor Disable\n"); writeDmItem(dmItems->motorDisableAlarm, NULL, 0); break; case WATER_ALARM_ON_SRQ: /* Water Alarm Active */ writeBooleanDmItem(dmItems->waterAlarm, TRUE); break; case WATER_ALARM_OFF_SRQ: /* Water Alarm Inactive */ writeBooleanDmItem(dmItems->waterAlarm, FALSE); break; case TEMP_ALARM_ON: /* Temperature Alarm */ wordsFromBuf(buffer, 2, &srqRequest, &status); /* Update Temperature & Alarm Status*/ updateTempAlarm(SERIAL_CHAN, (microAlarm) status, dmItems->temperatureAlarm, dmItems->microTemperature); break; case MOTOR_NVRAM_INIT_SRQ: /* Motor NV RAM was initialized */ writeBooleanDmItem(dmItems->NVRamInitialized, FALSE); break; /* *** Motor Controller Faults */ case MOTOR_CTRL_VOLTS_SRQ: /* Motor Controller Voltage Alarm */ case MOTOR_CTRL_TEMP_SRQ: /* Motor Controller Temperature */ case MOTOR_STALL_SRQ: /* Motor Stalled Alarm */ case MOTOR_RESOLVER_SRQ: /* Motor Resolver Alarm */ case MOTOR_CURRENT_SRQ: /* Motor Over Current Alarm */ case MOTOR_STATUS_SRQ: /* Motor Status Unknown */ case MOTOR_NO_FAULT_SRQ: /* No Motor Faults Detected */ case MOTOR_FAULT_SRQ: /* Undefined Motor Fault Detected */ /* Ignore alarms if motor is off */ if (microDevPowerSwitchState(microIOCtrl) == SWITCH_ON) { /* Motor is DISABLED if fault occurs*/ /* So update motorEnable DM item */ initMotorEnableDm( dmItems->motorEnableStatus, MOOG_MOTOR_FAULT ); /* Map SRQ code to Motor Status */ motorStatus = SRQToMotorStatus( srqRequest ); /* Update Motor Status DM Item */ writeDmItem(dmItems->motorStatus, &motorStatus, sizeof(motorStatus) ); } /* if */ break; } /* switch */ return(srqRequest); /* Return SRQ received to caller */ } /* processThrusterSRQ() */ /****************************************************************************/ /* Function : thrusterDmItemInit */ /* Porpose : Initial communications with micro following a RESET event */ /* : Reads various data items from micro and updates DM items */ /* Inputs : Pointer to structure of DM item handles, serial channel */ /* Outputs : Returns OK or ERROR if a transaction fails */ /****************************************************************************/ MLocal STATUS thrusterDmItemInit(thrusterDmItems* dmItems, sio32Chan* thrusterDataChan) { Int16 tries; /* Send Command Enable Command to IBC Micro */ for (tries = 0; ((microCmdEnable(thrusterDataChan) == ERROR) && tries < MICRO_ENABLE_TRYS); tries++) taskDelay(sysClkRateGet() / 5); if (tries == MICRO_ENABLE_TRYS) { logMsg("Thruster I/F: Error Enabling Motor Microcontroller\n"); return (ERROR); } /* if */ /* Set Micro Identification initial value */ if (updateMicroIdentDm(thrusterDataChan, dmItems->microDm.idString) == ERROR) logMsg("Thruster I/F: Error reading Identification String\n"); /* Set Micro Serial Number initial value */ if (updateSerialNoDm(thrusterDataChan, dmItems->microDm.serialNo) == ERROR) logMsg("Thruster I/F: Error reading Micro Serial Number\n"); /* Set Software Revision initial value */ if (updateSoftwareRevDm(thrusterDataChan, dmItems->microDm.softwareRev) == ERROR) logMsg("Thruster I/F: Error reading Software Revision\n"); /* Set Micro Ram Test Result value */ if (updateMicroRamTestDm(thrusterDataChan, dmItems->microDm.ramTest) == ERROR) logMsg("Thruster I/F: Error checking micro RAM Test\n"); /* Set Micro Temperature Alarm initial value */ if (updateTempAlarmDm(thrusterDataChan, dmItems->temperatureAlarm) == ERROR) logMsg("Thruster I/F: Error checking temperature alarm status\n"); /* Set Micro Water Alarm initial value */ if (updateWaterAlarmDm(thrusterDataChan, dmItems->waterAlarm) == ERROR) logMsg("Thruster I/F: Error checking water alarm status\n"); /* Set Motor Serial Number initial value */ if (updateMotorSerNoDm(thrusterDataChan, dmItems->motorSerialNo) == ERROR) logMsg("Thruster I/F: Error reading Motor Serial Number\n"); /* Set Moog Motor Status initial value */ if (updateMotorStatusDm(thrusterDataChan, dmItems->motorStatus) == ERROR) logMsg("Thruster I/F: Error checking motor status\n"); return (OK); } /* thrusterDmItemInit() */ /****************************************************************************/ /* Function : createThrusterDmItems */ /* Purpose : Create Data Mgr Items specific to thruster motor applic */ /* Inputs : Pointer to structure of DM item handles, Motor DM prefix, */ /* Thruster Motor Name */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS createThrusterDmItems( thrusterDmItems *dmItems, const char* thrusterDmPrefix, const char* thrusterName, const char* reducedPwrConfirmDmName ) { dmItemList thrustDmInit[] = /* Data Manager Item Table for init */ { { &dmItems->microDm.resetEvent, "Reset Event", MICRO_RESET_EVENT_DM, DM_EMPTY, 1 }, { &dmItems->microDm.resetCmd, "Reset Command", MICRO_RESET_CMD_DM, DM_EMPTY, 1 }, { &dmItems->microDm.idString, "Identification", MICRO_IDENT_DM, DM_CHAR, MICRO_ID_LEN }, { &dmItems->microDm.serialNo, "Micro Serial Number", MICRO_SERIAL_NO_DM, DM_CHAR, MICRO_SERNO_LEN }, { &dmItems->microDm.softwareRev, "Software Revision", SOFTWARE_REV_DM, DM_CHAR, SOFTWARE_REV_LEN }, { &dmItems->microDm.ramTest, "Micro RAM Test", RAM_TEST_STATUS_DM, DM_MBOOL, 1 }, { &dmItems->microTemperature, "Micro Temperature", TEMPERATURE_DM, DM_INT16, 1 }, { &dmItems->temperatureAlarm, "Temperature Alarm", TEMPERATURE_ALARM_DM, DM_ENUM, 1 }, { &dmItems->tempWarnThresh, "Temperature Warn Threshold", TEMP_WARN_THRESH_DM, DM_INT16, 1 }, { &dmItems->tempAlarmThresh, "Temperature Alarm Threshold", TEMP_ALARM_THRESH_DM, DM_INT16, 1 }, { &dmItems->waterAlarm, "Water Alarm", WATER_ALARM_DM, DM_MBOOL, 1 }, { &dmItems->waterAlarmThresh, "Water Alarm Threshold", WATER_ALARM_THRESH_DM, DM_INT16, 1 }, { &dmItems->motorSerialNo, "Motor Serial Number", MOTOR_SERIAL_NO_DM, DM_CHAR, MOTOR_SERNO_LEN }, { &dmItems->motorStatus, "Motor Status", MOTOR_STATUS_DM, DM_ENUM, 1 }, { &dmItems->motorEnableReq, "Motor Enable Request", MOTOR_ENABLE_DM, DM_ENUM, 1 }, { &dmItems->motorEnableStatus, "Motor Enable Status", MOTOR_ENABLE_STATUS_DM, DM_ENUM, 1 }, { &dmItems->motorCtrlMode, "Motor Control Mode", MOTOR_CTRL_MODE_DM, DM_ENUM, 1 }, { &dmItems->motorCmdThrust, "Motor Thrust Command", MOTOR_CMD_THRUST_DM, DM_INT16, 1 }, { &dmItems->motorCmdPower, "Motor Power Command", MOTOR_CMD_POWER_DM, DM_INT16, 1 }, { &dmItems->motorCurrentGain, "Motor Current Gain Param", MOTOR_CURRENT_GAIN_DM, DM_INT16, 1 }, { &dmItems->motorTorqueGain, "Motor Torque Gain Param", MOTOR_TORQUE_GAIN_DM, DM_INT16, 1 }, { &dmItems->motorVelocityGains, "Motor Velocity Gain Params", MOTOR_VELOCITY_GAINS_DM, DM_INT16, VELOCITY_GAIN_SIZE }, { &dmItems->motorLinearizedGains,"Motor Linearized Gain Params", MOTOR_LINEARIZED_GAINS_DM, DM_INT16, LINEARIZED_GAIN_SIZE}, { &dmItems->motorModelParamK4, "Motor Model K4 Param", MOTOR_MODEL_PARAM_K4_DM, DM_INT16, 1 }, { &dmItems->motorRegenLimitMode, "Motor Renegeration Limit Mode", MOTOR_REGEN_LIMIT_MODE_DM, DM_MBOOL, 1 }, { &dmItems->motorRegenLimit, "Motor Renegeration Limit", MOTOR_REGEN_LIMIT_DM, DM_NAT16, 1 }, { &dmItems->motorVelocity, "Motor Velocity", MOTOR_VELOCITY_DM, DM_INT16, 1 }, { &dmItems->motorTorqueOffset, "Motor Torque Offset", MOTOR_TORQUE_OFFSET_DM, DM_INT16, 1 }, { &dmItems->writeNVRam, "NV RAM Write", MICRO_WRITE_NVRAM_DM, DM_EMPTY, 1 }, { &dmItems->NVRamCycles, "NV RAM Cycles", MICRO_NVRAM_CYCLES_DM, DM_NAT16, 1 }, { &dmItems->NVRamDateTime, "NV RAM Date/Time", MICRO_NVRAM_DATE_DM, DM_CHAR, NV_RAM_DATE_LEN }, { &dmItems->NVRamInitialized, "NV RAM Initialize", MICRO_NVRAM_INITIALIZED_DM, DM_EMPTY, 1 }, { &dmItems->motorRpmCalibrate, "Velocity Calibrate", MOTOR_RPM_CAL_DM, DM_EMPTY, 1 }, #ifdef INCLUDE_MOTOR_VEL_BUF { &dmItems->motorEnableVelBuf, "Velocity Buffer Enable", MOTOR_VEL_BUF_MODE_DM, DM_EMPTY, 1 }, { &dmItems->motorReadVelBuf, "Velocity Buffer Read", MOTOR_VEL_BUF_READ_DM, DM_EMPTY, 1 }, { &dmItems->motorVelBufData, "Velocity Buffer Data", MOTOR_VEL_BUF_DATA_DM, DM_INT16, MOTOR_DAS_BUF_SIZE }, { &dmItems->motorTorqueBufData, "Torque Buffer Data", MOTOR_TORQUE_BUF_DATA_DM, DM_INT16, MOTOR_DAS_BUF_SIZE }, #endif { &dmItems->motorDisableAlarm, "Motor Disable Alarm", MOTOR_DISABLE_ALARM_DM, DM_EMPTY, 1 }, { &dmItems->motorStallAlarm, "Motor Software Stall Alarm", MOTOR_STALL_ALARM_DM, DM_EMPTY, 1 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; /* Create Thruster Power Reduce DM Item */ if ((dmItems->motorReducePowerDm = initMicroDmItem("", MOTOR_PWR_RED_DM, DM_MBOOL, 1)) == ERROR) return (ERROR); if ((dmItems->motorRedPwrConfirmDm = initMicroDmItem("", (char *) reducedPwrConfirmDmName, DM_MBOOL, 1)) == ERROR) return (ERROR); return (initMicroDmItemList(thrustDmInit, thrusterDmPrefix, thrusterName)); } /* createThrusterDmItems() */ /****************************************************************************/ /* Function : thrusterTask */ /* Purpose : Initializes thruster motor communications. Provides */ /* interface between motor and Data Manager items. */ /* Inputs : Thruster identifier & SIO32 serial channel number to motor */ /* These params physically map a motor to a serial channel */ /* Outputs : Normally runs forever, but returns ERROR on fatal error */ /****************************************************************************/ STATUS thrusterTask(thrusterId thruster, Nat16 serialChannel) { thrusterDmItems dmItems; /* structure containing dm item handles */ microIOControl microIOCtrl; /* Microcontroller IO Task control */ char taskName[16]; /* Task name for thruster in/out tasks */ /* Initialize serial channel and task */ /* synchronization resources */ if (initMicroTaskIO( µIOCtrl, NUM_TASKS, serialChannel, thrustDmPrefix[thruster], thrustChanName[thruster], thrusterPowerDmName[thruster]) == ERROR) { logMsg("Error initializing IO for %s thruster", thrusterName[thruster]); return(ERROR); } /* if */ /* Clear list of Data Manager Items */ bzero((char *) &dmItems, sizeof(dmItems)); /* Create Thruster Application DM Items */ if (createThrusterDmItems(&dmItems, thrustDmPrefix[thruster], thrusterName[thruster], thrusterReducedPowerConfirmDmName[thruster]) == ERROR) { /* Error occurred, print message & exit */ sio32ChanDestroy( serialChannel, µIOCtrl.sio32DataChan ); return(ERROR); } /* if */ /* Start Provider for static dat, Alarms*/ dm_start_provider(dmItems.microDm.idString, DM_STATIC); dm_start_provider(dmItems.microDm.serialNo, DM_STATIC); dm_start_provider(dmItems.microDm.softwareRev, DM_STATIC); dm_start_provider(dmItems.microDm.resetEvent, DM_STATIC); dm_start_provider(dmItems.microDm.ramTest, DM_STATIC); dm_start_provider(dmItems.NVRamInitialized, DM_STATIC); dm_start_provider(dmItems.temperatureAlarm, DM_STATIC); dm_start_provider(dmItems.waterAlarm, DM_STATIC); dm_start_provider(dmItems.motorSerialNo, DM_STATIC); dm_start_provider(dmItems.motorStatus, DM_STATIC); dm_start_provider(dmItems.motorDisableAlarm, DM_STATIC); /* Create Thruster Input Task name */ sprintf(taskName, "thrIn%d", thruster); /* Start Thruster Data Input Task */ if ((microIOCtrl.childTasks[IN_TASK].taskId = taskSpawn( taskName, THRUST_IN_PRIORITY, 0, THRUST_IN_STACKSIZE, (FUNCPTR) thrusterInTask, (int) &dmItems, (int) µIOCtrl, 0, 0, 0, 0, 0, 0, 0, 0)) == ERROR) { sio32ChanDestroy( serialChannel, µIOCtrl.sio32DataChan ); logMsg("Thruster I/F: Error spawning data input task\n"); return(ERROR); } /* if */ /* Create Thruster Output Task name */ sprintf(taskName, "thrOut%d", thruster); /* Start Thruster Data Output Task */ if ((microIOCtrl.childTasks[OUT_TASK].taskId = taskSpawn("thrustOut", THRUST_OUT_PRIORITY, 0, THRUST_OUT_STACKSIZE, (FUNCPTR) thrusterOutTask, (int) thruster, (int) &dmItems, (int) µIOCtrl, 0, 0, 0, 0, 0, 0, 0)) == ERROR) { /* Delete Thruster Input Task due to Error*/ taskDelete(microIOCtrl.childTasks[OUT_TASK].taskId); /* Close serial communication channel */ sio32ChanDestroy( serialChannel, µIOCtrl.sio32DataChan ); logMsg("Thruster I/F: Error spawning data output task\n"); return(ERROR); } /* if */ FOREVER { /* Check if Thruster is switched on */ if ((microDevPowerSwitchState(µIOCtrl) != SWITCH_ON) || ((microDevPowerSwitchState(µIOCtrl) == SWITCH_ON) && (microIOCtrl.resetEventCount != 0))) /* Process SRQ's from motor micro */ while(processThrusterSRQ(&dmItems, µIOCtrl) != MICRO_RESET_SRQ); microIOCtrl.resetEventCount++; /* Count reset events ffrom micro */ /* Reset event received from Micro so */ /* Mark serial coms link as up */ setSerialLinkState(µIOCtrl, LINK_UP); /* Initialize state of DM and micro */ if (thrusterDmItemInit(&dmItems, µIOCtrl.sio32DataChan) == OK) /* Wake up output task to configure micro*/ semGive(microIOCtrl.childTasks[OUT_TASK].taskSyncSem); else setSerialLinkState(µIOCtrl, LINK_DOWN); } /* FOREVER */ /* Delete Child Tasks */ taskDelete(microIOCtrl.childTasks[IN_TASK].taskId); taskDelete(microIOCtrl.childTasks[OUT_TASK].taskId); /* Close serial communication channel */ sio32ChanDestroy( serialChannel, µIOCtrl.sio32DataChan ); /* dm_stop_provider, dm_stop_consumer, etc are NOT */ /* required when the task exits as the Data Manager */ /* does this automatically when the task is deleted */ /* via taskDeleteHookAdd() and dm_task_exit functions */ return(OK); /* That's all folks */ } /* thrusterTask() */ /****************************************************************************/ /* Function : thrusterInTask */ /* Purpose : Thruster Input Task. Reads data from motor & and writes DM */ /* Inputs : Array of Data Manager Item handles & micro O Ctrl structure*/ /* Outputs : Normally runs forever, but exits if error occurs */ /****************************************************************************/ Void thrusterInTask(Reg thrusterDmItems *dmItems, microIOControl *microIOCtrl) { SEM_ID wakeupSem; /* Used wake up this task periodically */ Int32 updates = 0; /* Counts dm updates for rate calculation*/ Nat16 motorStallCount = 0; /* Counts motor stall events */ /* Wakeup semaphore for provider rate */ if ((wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL ) { logMsg("Thruster I/F: Error creating Input Task Resources\n"); return; } /* if */ /* Provider Task for non-static data items*/ dm_start_provider(dmItems->motorVelocity, VEL_DM_PERIOD); dm_start_provider(dmItems->microTemperature, TEMP_DM_PERIOD); dm_start_provider(dmItems->motorStallAlarm, DM_STATIC); dm_start_consumer(dmItems->motorEnableStatus, DM_STATIC, SEM_NULL); dm_start_consumer(dmItems->motorVelocity, DM_STATIC, SEM_NULL); dm_start_consumer(dmItems->motorCmdThrust, DM_STATIC, SEM_NULL); FOREVER { /* Wait for Motor Coms to be initialized */ semTake(microIOCtrl->childTasks[IN_TASK].taskSyncSem, WAIT_FOREVER); while (microIOCtrl->serialLinkState == LINK_UP) { taskSafe(); /* Prevent task from being deleted during */ /* serial IO and Data Manager updates */ /* Read motor velocity */ updateMotorVelocityDm(SERIAL_CHAN, dmItems->motorVelocity); /* update data manager items we provide */ if ((++updates % (TEMP_DM_PERIOD / VEL_DM_PERIOD)) == 0) { updates = 0; updateMicroTempDm(SERIAL_CHAN, dmItems->microTemperature); } /* if */ /* Software motor stall detection */ checkForMotorStall(dmItems, &motorStallCount); taskUnsafe(); /* Task may now be deleted safely */ /* Pend until timer wakes us up */ /* Update Rate is not guaranteed due to */ /* the possibility of task preemption */ semTake(wakeupSem, sysClkRateGet() / (USECS_PER_SEC / VEL_DM_PERIOD)); } /* while */ } /* FOREVER */ semDelete(wakeupSem); /* semaphore resources */ /* dm_stop_provider, dm_stop_consumer, dm_delete_group*/ /* etc are NOT required when the task exits as the */ /* Data Manager does this automatically when the task */ /* is deleted via the taskDeleteHookAdd() and */ /* dm_task_exit functions. */ } /* thrusterInTask() *//* That's all folks ! */ /****************************************************************************/ /* Function : thrusterOutTask */ /* Purpose : Thruster Output Task. Read DM items and write them to motor*/ /* Inputs : Array of Data Manager Item handles & serial chan structure */ /* Outputs : Normally runs forever, but exits if error occurs */ /****************************************************************************/ Void thrusterOutTask(thrusterId thruster, Reg thrusterDmItems *dmItems, microIOControl *microIOCtrl) { DM_Group dmGroup; /* Thruster Data Data Manager Group */ SEM_ID dmUpdateSem; /* Wakes up task when item values changes */ DWord thrustGroupBits; /* Holds dm_group_changes bit vector */ DWord motorSwitchBit; /* Motor Power Switch iten changed bit */ DWord resetEventBit; /* Microcontroller reset event bit */ DWord resetCmdBit; /* Microcontroller reset item changed bit */ DWord serialNoBit; /* Micro serial number item changed bit */ DWord motorSerialNoBit; /* Motor serial number item changed bit */ DWord motorEnableBit; /* Motor Enable item changed bit */ DWord motorCtrlModeBit; /* Motor Control Mode item changed bit */ DWord thrustCommandBit; /* Motor commanded thrust item changed bit*/ DWord powerCommandBit; /* Motor commanded power item changed bit */ DWord torqueOffsetBit; /* Motor torque offset item changed bit */ DWord modelParamBit; /* Motor dymanic model parameter K4 bit */ DWord currentGainBit; /* Motor Current Mode Gain changed bit */ DWord torqueGainBit; /* Motor Torque Mode Gain changed bit */ DWord velocityGainsBit; /* Motor Velocity Mode Gains changed bit */ DWord linearGainsBit; /* Motor Linearized Mode Gains changed bit*/ DWord waterAlarmBit; /* Water alarm threshold item changed bit */ DWord tempWarnBit; /* Temp warn threshold item changed bit */ DWord tempAlarmBit; /* Temp alarm threshold item changed bit */ DWord NVRamWriteBit; /* NV RAM write command item changed bit */ DWord regenModeBit; /* Regeneration mode item changed bit */ DWord regenLimitBit; /* Regeneration value item changed bit */ DWord rpmCalibrateBit; /* Velocity Calibration item changed bit */ #if INCLUDE_MOTOR_VEL_BUF DWord velBufEnableBit; DWord velBufReadBit; Int16 velBufData[MOTOR_DAS_BUF_SIZE]; Int16 torqueBufData[MOTOR_DAS_BUF_SIZE]; #endif /* Create wakeup sem for DM item updates */ if ((dmUpdateSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL ) { logMsg("Thruster I/F: Error initializing Output Task Resources\n"); return; } /* if */ /* This task consumes motor power switch */ dm_start_consumer(microIOCtrl->powerSwitchDm, DM_STATIC, dmUpdateSem); /* Declare this task a consumer of items */ /* which are written to the motor */ dm_start_consumer(dmItems->microDm.resetEvent, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->microDm.resetCmd, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->microDm.serialNo, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->motorSerialNo, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->motorEnableReq, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->motorEnableStatus, DM_STATIC, SEM_NULL); dm_start_consumer(dmItems->motorCtrlMode, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->motorCmdThrust, DM_ASYNC, dmUpdateSem); dm_start_consumer(dmItems->motorCmdPower, DM_ASYNC, dmUpdateSem); dm_start_consumer(dmItems->motorCurrentGain, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->motorTorqueGain, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->motorVelocityGains, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->motorLinearizedGains, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->motorTorqueOffset, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->motorModelParamK4, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->waterAlarmThresh, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->tempWarnThresh, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->tempAlarmThresh, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->writeNVRam, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->motorRegenLimitMode, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->motorRegenLimit, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->motorRpmCalibrate, DM_STATIC, dmUpdateSem); /* We consumer the reduce power request */ dm_start_consumer(dmItems->motorReducePowerDm, DM_STATIC, SEM_NULL ); /* and provide the confirmation data item */ dm_start_provider(dmItems->motorRedPwrConfirmDm, DM_STATIC); dm_start_provider(dmItems->motorEnableStatus, DM_STATIC); /* This task also provides some items */ dm_start_provider(dmItems->NVRamCycles, DM_STATIC); dm_start_provider(dmItems->NVRamDateTime, DM_STATIC); #ifdef INCLUDE_MOTOR_VEL_BUF dm_start_consumer(dmItems->motorEnableVelBuf, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->motorReadVelBuf, DM_STATIC, dmUpdateSem); dm_start_provider(dmItems->motorVelBufData, DM_STATIC); dm_start_provider(dmItems->motorTorqueBufData, DM_STATIC); #endif dmGroup = dm_create_group(); /* Create Data Manager Group for items */ dm_group_add_item(dmGroup, microIOCtrl->powerSwitchDm, &motorSwitchBit); dm_group_add_item(dmGroup, dmItems->microDm.resetEvent, &resetEventBit); dm_group_add_item(dmGroup, dmItems->microDm.resetCmd, &resetCmdBit); dm_group_add_item(dmGroup, dmItems->microDm.serialNo, &serialNoBit); dm_group_add_item(dmGroup, dmItems->motorSerialNo, &motorSerialNoBit); dm_group_add_item(dmGroup, dmItems->motorEnableReq, &motorEnableBit); dm_group_add_item(dmGroup, dmItems->motorCtrlMode, &motorCtrlModeBit); dm_group_add_item(dmGroup, dmItems->motorCmdThrust, &thrustCommandBit); dm_group_add_item(dmGroup, dmItems->motorCmdPower, &powerCommandBit); dm_group_add_item(dmGroup, dmItems->motorCurrentGain, ¤tGainBit); dm_group_add_item(dmGroup, dmItems->motorTorqueGain, &torqueGainBit); dm_group_add_item(dmGroup, dmItems->motorVelocityGains, &velocityGainsBit); dm_group_add_item(dmGroup, dmItems->motorLinearizedGains,&linearGainsBit); dm_group_add_item(dmGroup, dmItems->motorTorqueOffset, &torqueOffsetBit); dm_group_add_item(dmGroup, dmItems->motorModelParamK4, &modelParamBit); dm_group_add_item(dmGroup, dmItems->waterAlarmThresh, &waterAlarmBit); dm_group_add_item(dmGroup, dmItems->tempWarnThresh, &tempWarnBit); dm_group_add_item(dmGroup, dmItems->tempAlarmThresh, &tempAlarmBit); dm_group_add_item(dmGroup, dmItems->writeNVRam, &NVRamWriteBit); dm_group_add_item(dmGroup, dmItems->motorRegenLimitMode, ®enModeBit); dm_group_add_item(dmGroup, dmItems->motorRegenLimit, ®enLimitBit); dm_group_add_item(dmGroup, dmItems->motorRpmCalibrate, &rpmCalibrateBit); #if INCLUDE_MOTOR_VEL_BUF dm_group_add_item(dmGroup, dmItems->motorEnableVelBuf, &velBufEnableBit); dm_group_add_item(dmGroup, dmItems->motorReadVelBuf, &velBufReadBit); #endif FOREVER { /* Mark micro configuration as invalid */ microIOCtrl->microConfigured = FALSE; /* Wait for micro reset via SRQ Task */ semTake(microIOCtrl->childTasks[OUT_TASK].taskSyncSem, WAIT_FOREVER); /* Reset received so set Data Manager to */ /* reflect motor state for cruitial items */ initMotorEnableDm( dmItems->motorEnableStatus, DISABLE_MOTOR ); initMotorCtrlModeDm( SERIAL_CHAN, dmItems->motorCtrlMode ); initMotorCmdThrustDm( SERIAL_CHAN, dmItems->motorCmdThrust ); initMotorCmdPowerDm ( SERIAL_CHAN, dmItems->motorCmdPower ); initMotorCurrentGainDm( SERIAL_CHAN, dmItems->motorCurrentGain ); initMotorTorqueGainDm( SERIAL_CHAN, dmItems->motorTorqueGain ); initMotorVelocityGainsDm( SERIAL_CHAN, dmItems->motorVelocityGains); initMotorLinearizedGainsDm( SERIAL_CHAN, dmItems->motorLinearizedGains ); initMotorTorqueOffsetDm( SERIAL_CHAN, dmItems->motorTorqueOffset ); initMotorModelParamK4Dm( SERIAL_CHAN, dmItems->motorModelParamK4 ); initWaterAlarmThreshDm( SERIAL_CHAN, dmItems->waterAlarmThresh); initTempAlarmThreshDm( SERIAL_CHAN, dmItems->tempWarnThresh, dmItems->tempAlarmThresh ); initMotorRegenModeDm( SERIAL_CHAN, dmItems->motorRegenLimitMode, FALSE); initMotorRegenLimitDm( SERIAL_CHAN, dmItems->motorRegenLimit ); /* Set initial values of these items */ updateMotorNVRamDm( SERIAL_CHAN, dmItems->NVRamCycles, dmItems->NVRamDateTime ); /* Clear data manager item changed bits */ thrustGroupBits = dm_get_group_changes(dmGroup); /* Flag that micro has been configured */ microIOCtrl->microConfigured = TRUE; /* Wake up input task to read from micro */ semGive(microIOCtrl->childTasks[IN_TASK].taskSyncSem); /* Change Serial I/O Keep Alive Mode so */ /* motor disables if communication stops */ if (sio32ChanSetKeepAliveMode(microIOCtrl->sio32DataChan.channel, KEEP_ALIVE_TICKS, KEEP_ALIVE_COUNT) == ERROR) logMsg("Error setting Keep Alive Mode for %s thruster", thrusterName[thruster]); while (microIOCtrl->serialLinkState == LINK_UP) { /* Wait for any data items to change */ /* Check for DM updates send zero command */ /* if no update for .5 sec */ while (semTake(dmUpdateSem, sysClkRateGet() / (USECS_PER_SEC / THRUSTER_SEM_TIMEOUT)) == ERROR) { /* Check if Data is updating */ /* Check that the motor is enabled */ if (motorEnabledState(dmItems) == ENABLE_MOTOR) { /* Write 0 torque command to motor */ motorSetCmdThrustPower( SERIAL_CHAN, 0, 0 ); logMsg("Thruster I/F: sending zero thrust command\n"); } /* if */ } /* if */ /* Ask Data Manager which items changed */ thrustGroupBits = dm_get_group_changes(dmGroup); /* Check if micro Reset Event occurred */ if (thrustGroupBits & resetEventBit) break; /* Exit loop and re-initialize DM items */ /* Check if motor power was switch off */ if ( (thrustGroupBits & motorSwitchBit) && (microDevPowerSwitchState(microIOCtrl) != SWITCH_ON) ) { initMotorEnableDm(dmItems->motorEnableStatus, DISABLE_MOTOR); /* Motor was switched off so link is down*/ setSerialLinkState(microIOCtrl, LINK_DOWN); if (sio32ChanSetKeepAliveMode( microIOCtrl->sio32DataChan.channel, 0, 0) == ERROR) logMsg("Error clearing Keep Alive Mode for %s thruster", thrusterName[thruster]); break; /* Exit loop and wait for power up */ } /* if */ taskSafe(); /* Prevent task from being deleted during */ /* serial IO and Data Manager updates */ /* Send Regeneration Mode command to motor*/ if (thrustGroupBits & regenModeBit) writeMotorRegenMode(SERIAL_CHAN, dmItems->motorRegenLimitMode); /* Send Micro Serial Number String */ if (thrustGroupBits & serialNoBit) writeMicroSerialNo(SERIAL_CHAN, dmItems->microDm.serialNo); /* Send Motor Serial Number String */ if (thrustGroupBits & motorSerialNoBit) writeMotorSerialNo(SERIAL_CHAN, dmItems->motorSerialNo); /* Send Regen Limit command to motor */ if (thrustGroupBits & regenLimitBit) writeMotorRegenLimit(SERIAL_CHAN, dmItems->motorRegenLimitMode); /* Send Enable/Disable command to motor */ if (thrustGroupBits & motorEnableBit) writeMotorEnable(SERIAL_CHAN, dmItems->motorEnableReq, dmItems->motorEnableStatus); /* Send Motor Control Mode to motor */ if (thrustGroupBits & motorCtrlModeBit) writeMotorCtrlMode(SERIAL_CHAN, dmItems->motorCtrlMode); /* Send Set Torque Offset command to motor*/ if (thrustGroupBits & torqueOffsetBit) writeTorqueOffset(SERIAL_CHAN, dmItems->motorTorqueOffset); /* Send Dynamic Model params to motor */ if (thrustGroupBits & modelParamBit) writeModelParamK4(SERIAL_CHAN, dmItems->motorModelParamK4); /* Send Current mode gains to motor */ if (thrustGroupBits & currentGainBit) writeMotorCurrentGain(SERIAL_CHAN, dmItems->motorCurrentGain); /* Send Torque mode gains to motor */ if (thrustGroupBits & torqueGainBit) writeMotorTorqueGain(SERIAL_CHAN, dmItems->motorTorqueGain); /* Send Velocity mode gains to motor */ if (thrustGroupBits & velocityGainsBit) writeMotorVelocityGains(SERIAL_CHAN, dmItems->motorVelocityGains); /* Send Linearized mode gains to motor */ if (thrustGroupBits & linearGainsBit) writeMotorLinearizedGains(SERIAL_CHAN, dmItems->motorLinearizedGains); /* Send Thrust & Power command to motor */ if ((thrustGroupBits & thrustCommandBit) || (thrustGroupBits & powerCommandBit) ) { writeMotorCmdThrustPower(SERIAL_CHAN, dmItems->motorCmdThrust, dmItems->motorCmdPower, dmItems->motorReducePowerDm, dmItems->motorRedPwrConfirmDm); } /* if */ /* Send Set Water Alarm Threshold cmd */ if (thrustGroupBits & waterAlarmBit) writeWaterAlarmThresh(SERIAL_CHAN, dmItems->waterAlarmThresh); /* Send Set Temp Alarm Thresholds cmd */ if ((thrustGroupBits & tempWarnBit) || (thrustGroupBits & tempAlarmBit)) writeTempAlarmThresh(SERIAL_CHAN,dmItems->tempWarnThresh, dmItems->tempAlarmThresh); if (thrustGroupBits & NVRamWriteBit) { /* Send Write NV Ram command to micro and */ motorWriteNVRam( SERIAL_CHAN ); /* update Data Manager Items for NV Ram */ /* Write cycles and write Date/Time */ updateMotorNVRamDm( SERIAL_CHAN, dmItems->NVRamCycles, dmItems->NVRamDateTime ); } /* if */ if (thrustGroupBits & rpmCalibrateBit) motorRpmCalibrate(SERIAL_CHAN); #if INCLUDE_MOTOR_VEL_BUF if (thrustGroupBits & velBufEnableBit) motorEnableVelBuf(SERIAL_CHAN); if (thrustGroupBits & velBufReadBit) { motorRedVelBuf(SERIAL_CHAN, velBufData, torqueBufData); writeDmItem(dmItems->motorVelBufData, velBufData, sizeof(Int16) * MOTOR_DAS_BUF_SIZE); writeDmItem(dmItems->motorTorqueBufData, torqueBufData, sizeof(Int16) * MOTOR_DAS_BUF_SIZE); } /* if */ #endif /* Check if Reset Command request occurred*/ if (thrustGroupBits & resetCmdBit) { /* Mark serial link as down */ setSerialLinkState(microIOCtrl, LINK_DOWN); /* Set keep alive mode to continuous so */ /* we can reset the link to the micro */ if (sio32ChanSetKeepAliveMode( microIOCtrl->sio32DataChan.channel, 0, 0) == ERROR) logMsg("Error clearing Keep Alive Mode for %s thruster", thrusterName[thruster]); /* Manual reset so ammounce it */ microIOCtrl->announceResets = TRUE; /* Send Reset Command to Microcontroller */ microResetCmd(SERIAL_CHAN); break; /* Exit loop and wait for RESET */ } /* if */ taskUnsafe(); /* Task may now be safely deleted */ } /* while (LINK_UP) */ } /* FOREVER */ /* dm_stop_provider, dm_stop_consumer, dm_delete_group*/ /* etc are NOT required when the task exits as the */ /* Data Manager does this automatically when the task */ /* is deleted via the taskDeleteHookAdd() and */ /* dm_task_exit functions. */ /* That's all folks ! */ } /* thrusterOutTask() */