/****************************************************************************/ /* Copyright 1992 - 1998 MBARI */ /****************************************************************************/ /* Summary : Semi Power Moog Thruster Motor Controller Interface Module */ /* Filename : thruster.c */ /* Author : Andrew Pearce */ /* Project : Tiburon ROV */ /* Version : Version 1.0 */ /* Created : 05/19/92 */ /* Modified : 11/25/98 */ /* 12/22/98: rsm. Increased MAX_TORQUE_CMD TO 2860 centi-Newton */ /* meters. Note that MAX_MOTOR_TORQUE serves the same purpose, */ /* and in fact divides out in the calc of "temp" in */ /* velocityMode(). Thus 2860 adds foward loop gain of 28.6/22. */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /* $Header: thruster.c,v 13.2 93/07/01 14:39:11 pean Exp $ * $Log: thruster.c,v $ * Initial revision * */ /****************************************************************************/ #include /* vxWorks system declarations */ #include /* vxWorks standard IO library functions*/ #include /* vxWorks string library functions */ #include /* vxWorks character functions */ #include /* vxWorks semaphore functions */ #include /* vxWorks task library functions */ #include /* vxWorks time and date functions */ #include /* vxWorks Message Queue Library */ #include /* vxWorks log message library funcs */ #include /* vxWorks ticker functions */ #include /* vxWorks CPU specific system functions*/ #include /* vxWorks math functions */ #include /* MBARI style guide type declarations */ #include /* Miscellaneous constants */ #include /* MBARI Rov Application Priorities */ #include "sio32Drv.h" /* Sio32 hardware and driver information*/ #include "sio32Server.h" /* Sio32 server communication services */ #include "sio32Client.h" /* Sio32 client communication services */ #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 "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 "moogcmd.h" /* Moog motor controller commands */ #include "microDm.h" /* Microcontroller Data Manager Items */ #include "thrusterDM.h" /* Thruster Data Manager Item Names */ #include "semiPower.h" /* Semipower motor controller functions */ #include "semiPowerVx.h" /* semiPower vxWorks Library definitions*/ #define VEL_DM_PERIOD 50000 /* Velocity provider peroid usecs */ #define TELEM_DM_PERIOD 1000000 /* Telemetry data 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 */ #define MAX_TORQUE_LIMIT (Int32) 2500 /* Maximum motor torque 25 NM * 100 */ /*#define MAX_TORQUE_CMD (Int32) 2200 Maximum motor torque 22 NM * 100 */ #define MAX_TORQUE_CMD (Int32) 2860 /* Max motor trq 22 NM*100*1.3 */ /* #define MAX_MOTOR_RPM (Int32) 2038 Motor RPM expected at max power */ #define MAX_MOTOR_RPM (Int32) 1630 /* Motor RPM expected at max power */ #define MAX_MOTOR_THRUST (Int32) 1245 /* Maximum Motor Thrust Command */ #define MAX_MOTOR_TORQUE 22 /* Max motor torque in Newton.metres*/ #define VELOCITY_DEADBAND 10 /* Velocity deadband region RPM */ #define MOTOR_TORQUE_DEADBAND 4 /* Motor torque deadband in bits */ #define NEWTONS_TO_MOTOR_UNITS(newtons) (Int16) (( (Int32) newtons * MAX_TORQUE_CMD) / MAX_MOTOR_THRUST) #define VEL_FILTER_TIME_CONST 20 /* 20 Milliseconds = 0.02 seconds */ #define TEMP_WARN_THRESH 45 /* Temperature Warning Threshold deg*/ #define TEMP_ALARM_THRESH 50 /* Temperature Alarm Threshold degs */ #define TEMPERATURE_HYSTERESIS 5 /* Temperature Alarm Hystesis degs */ #define HUMIDITY_WARN_THRESH 25 /* Set humidity warning to 10% RH */ #define HUMIDITY_ALARM_THRESH 35 /* Set humidity alarm to 20% RH */ #define HUMIDITY_HYSTERESIS 2 /* Humidity Alarm hysteresis 2% */ /****************************************************************************/ const char *thrusterName[] = /* Thruster motor names */ { "Port Horizontal", "Stbd Horizontal", "Forward Lateral", "Aft Lateral", "Port Vertical", "Stbd Vertical", "Motor Test" }; 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, MOTOR_TEST_PREFIX_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, NO_ITEM }; typedef enum { INPUT_VOLTAGE, MECHANICAL_POWER, MOTOR_CURRENT, MOTOR_TORQUE, ELAPSED_HOURS, TEMPERATURE, HUMIDITY, END_OF_TELEM_LIST } telemItemList; typedef struct /* Thruster Data Manager Item Handles */ { DM_Item temperature; /* Temperature sensor value (deg C) */ DM_Item temperatureAlarm; /* Temperature Alarm Detected microAlarm*/ DM_Item tempWarnThresh; /* Temperature Warning Threshold */ DM_Item tempAlarmThresh; /* Temperature Alarm Threshold */ DM_Item humidity; /* Humidity sensor value (%RH) */ DM_Item humidityAlarm; /* Humidity Alarm Detected microAlarm */ DM_Item humidityWarnThresh; /* Humidity Warning Threshold */ DM_Item humidityAlarmThresh; /* Humidity Alarm Threshold */ DM_Item inputVoltage; /* Controller Input Voltage (volts) */ DM_Item outputPower; /* Controller Output Power (watts) */ DM_Item motorCurrent; /* Controller Motor Current (mA) */ DM_Item elapsedHours; /* Controller Elapsed Hour Log (hours) */ DM_Item dynamicBrake; /* Controller Dynamic Brake */ DM_Item readFaultQueue; /* Command to read fault queue */ DM_Item clearFaultQueue; /* Command to clear fault queue */ DM_Item faultQueue; /* Controller Fault Queue */ DM_Item motorFaultCode; /* Controller Fault Code */ DM_Item motorEnableReq; /* Motor Enable/Disable Request */ DM_Item motorEnableStatus; /* Motor Enable Status */ DM_Item motorVelocity; /* Motor measured velocity RPM */ DM_Item motorTorque; /* Motor measured torque Nm */ DM_Item motorCtrlMode; /* Motor control mode */ DM_Item motorCmdThrust; /* Motor Commanded Thrust */ DM_Item motorCtrlTorque; /* Output from controller to motor */ DM_Item motorCtrlParam; /* Controller system debug parameter */ DM_Item motorCurrentGain; /* Motor current gain parameter */ DM_Item motorVelocityGains; /* Motor velocity gain paramaters */ DM_Item motorModelParamK4; /* Motor model parameter K4 */ DM_Item motorPowerSwitchDm; /* Motor Power Switch */ DM_Item motorRedPwrConfirmDm; /* Motor Power Reduce Confirmation */ DM_Item motorReducePowerDm; /* Motor Power Reduce Request DM Item */ DM_Item motorStallAlarm; /* Software Motor Stall Detected Alarm */ DM_Item motorCmdString; /* Manual serial command string */ DM_Item motorCmdReply; /* Manual serial command reply */ } thrusterDmItems; typedef struct { motorCtrlMode controlMode; /* Motor control mode */ Int16 desiredThrust; /* Desired motor thrust value */ Int16 motorVelocity; /* Actual motor velocity */ Int16 filtAcceleration; /* Filtered motor acceleration at K */ Int16 lastVelocity; /* Motor velocity at K-1 */ Int16 lastVelDeriv; /* Motor velocity derivative at K-1 */ Int16 motorCtrlTorque; /* Controller torque command */ Int32 motorCtrlParam; alarmParam temperatureAlarm; /* Temperature Alarm */ alarmParam humidityAlarm; /* humidityAlarm */ Int16 currentGain; /* Motor Current mode gain parameter*/ velocity_gains velocityGains; /* Motor Velocity mode gain params */ Int16 modelParamOneOnK4; /* Thruster dynamic model param K4 */ Word driveModeCmd; } motorCtrlStruct; /************************* Forward Declarations *****************************/ Void thrusterInTask(Reg thrusterDmItems *dmItems, semiPowerSerial *serialIO, motorCtrlStruct *motorParams, SEM_ID outTaskSyncSem); Void thrusterOutTask(Reg thrusterDmItems *dmItems, semiPowerSerial *serialIO, motorCtrlStruct *motorParams, SEM_ID outTaskSyncSem); MLocal Void initMotorEnableDm( DM_Item motorEnableDm, motorEnableState motorEnable ); MLocal Void initMotorFaultCodeDm( DM_Item motorFaultCodeDm, semiPowerFaultCode faultCode ); /****************************************************************************/ /* Function : updateAlarmDm */ /* Purpose : Write to micro alarm status data manager item */ /* Inputs : Micro alarm data manager item handle, alarm value */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS updateAlarmDm(DM_Item alarmDm, microAlarm alarmStatus) { /* Write alarm to Dmgr */ return(writeDmItem(alarmDm, &alarmStatus, sizeof(alarmStatus))); } /* updateAlarmDm() */ /****************************************************************************/ /* Function : alarmThresholdCheck */ /* Purpose : Compares Write to micro alarm status data manager item */ /* Inputs : Micro alarm data manager item handle, alarm value */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal MBool alarmThresholdCheck( alarmParam *alarm, Int16 hysteresis ) { microAlarm status = alarm->alarmStatus; switch (status) { case MICRO_ALARM_OK: if (alarm->value >= alarm->alarmThresh ) alarm->alarmStatus = MICRO_ALARM; else if (alarm->value >= alarm->warnThresh) alarm->alarmStatus = MICRO_WARNING; break; case MICRO_WARNING: if (alarm->value >= alarm->alarmThresh) alarm->alarmStatus = MICRO_ALARM; else if (alarm->value < (alarm->warnThresh - hysteresis) ) alarm->alarmStatus = MICRO_ALARM_OK; break; case MICRO_ALARM: if (alarm->value < alarm->warnThresh) alarm->alarmStatus = MICRO_ALARM_OK; else if (alarm->value < (alarm->alarmThresh - hysteresis) ) alarm->alarmStatus = MICRO_WARNING; break; } /* select */ return( (status != alarm->alarmStatus) ? TRUE : FALSE); /* Return TRUE if alarm status changed */ } /* alarmThresholdCheck() */ MLocal Void checkTemperatureAlarm(thrusterDmItems *dmItems, motorCtrlStruct *motorParams) { if (dm_read( dmItems->temperature, (char *) &motorParams->temperatureAlarm.value, sizeof(motorParams->temperatureAlarm.value), (DM_Time *)NULL) == SUCCESS ) if (alarmThresholdCheck( &motorParams->temperatureAlarm, TEMPERATURE_HYSTERESIS) == TRUE) writeDmItem(dmItems->temperatureAlarm, &motorParams->temperatureAlarm.alarmStatus, sizeof(motorParams->temperatureAlarm.alarmStatus)); } /* checkTemperatureAlarm() */ MLocal Void checkHumidityAlarm(thrusterDmItems *dmItems, motorCtrlStruct *motorParams) { if (dm_read( dmItems->humidity, (char *) &motorParams->humidityAlarm.value, sizeof(motorParams->humidityAlarm.value), (DM_Time *)NULL) == SUCCESS ) if (alarmThresholdCheck( &motorParams->humidityAlarm, HUMIDITY_HYSTERESIS ) == TRUE) writeDmItem(dmItems->humidityAlarm, &motorParams->humidityAlarm.alarmStatus, sizeof(motorParams->humidityAlarm.alarmStatus)); } /* checkHumidityAlarm() */ /****************************************************************************/ /* Function : updateSemiPowerTempDm */ /* Purpose : Reads semipower temperature sensor value & update dmgr item*/ /* Inputs : Sio32 chan & temperature sensor data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS updateSemiPowerTempDm( semiPowerSerial *sio32DataChan, DM_Item tempDm ) { Flt32 degs; /* Degrees from Semi Power Controller */ Int16 temperature; /* Temperature Sensor Value */ /* Read temperature sensor value */ if (semiPowerHeatsinkTemp( sio32DataChan, DRIVE_ID, °s ) != OK) return (ERROR); temperature = (Int16) degs; /* Round to nearest interger */ /* Write temperature to Data Manager */ return (writeDmItem(tempDm, &temperature, sizeof(temperature))); } /* updateSemiPowerTempDm() */ /****************************************************************************/ /* Function : updateSemiPowerHumidityDm */ /* Purpose : Reads semipower humidity sensor value & update dmgr item */ /* Inputs : Sio32 chan & humidity sensor data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS updateSemiPowerHumidityDm( semiPowerSerial *sio32DataChan, DM_Item humidityDm ) { Nat16 humidity; /* Humidity Sensor Value */ /* Read humidity sensor value */ if (semiPowerHumidity( sio32DataChan, DRIVE_ID, &humidity ) != OK) return (ERROR); /* Write humidity to Data Manager */ return (writeDmItem(humidityDm, &humidity, sizeof(humidity))); } /* updateSemiPowerHumidityDm() */ /****************************************************************************/ /* Function : updateSemiPowerOutputPowerDm */ /* Purpose : Reads semipower input power and update dmgr item */ /* Inputs : Sio32 chan & power data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS updateSemiPowerOutputPowerDm( semiPowerSerial *sio32DataChan, DM_Item powerDm ) { Int16 watts; /* Input Power */ /* Read output power value */ if (semiPowerOutputPower( sio32DataChan, DRIVE_ID, &watts ) != OK) return (ERROR); /* Write input power to Data Manager */ return (writeDmItem(powerDm, &watts, sizeof(watts))); } /* updateSemiPowerOutputPowerDm() */ /****************************************************************************/ /* Function : updateSemiPowerInputVoltageDm */ /* Purpose : Reads semipower input voltage and update dmgr item */ /* Inputs : Sio32 chan & voltage data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS updateSemiPowerInputVoltageDm( semiPowerSerial *sio32DataChan, DM_Item voltageDm ) { Int16 voltage; /* Input Voltage */ /* Read input voltage value */ if (semiPowerInputVoltage( sio32DataChan, DRIVE_ID, &voltage ) != OK) return (ERROR); /* Write input voltage to Data Manager */ return (writeDmItem(voltageDm, &voltage, sizeof(voltage))); } /* updateSemiPowerInputVoltageDm() */ /****************************************************************************/ /* Function : updateSemiPowerMotorCurrentDm */ /* Purpose : Reads semiPower motor current and update dmgr item */ /* Inputs : Sio32 chan & voltage data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS updateSemiPowerMotorCurrentDm( semiPowerSerial *sio32DataChan, DM_Item currentDm ) { Int16 current; /* Motor Current */ /* Read motor current value */ if (semiPowerMotorCurrent( sio32DataChan, DRIVE_ID, ¤t ) != OK) return (ERROR); /* Write input current to Data Manager */ return (writeDmItem(currentDm, ¤t, sizeof(current))); } /* updateSemiPowerMotorCurrentDm() */ /****************************************************************************/ /* Function : updateSemiPowerMotorTorqueDm */ /* Purpose : Reads semiPower motor torque and update dmgr item */ /* Inputs : Sio32 chan & torque data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS updateSemiPowerMotorTorqueDm( semiPowerSerial *sio32DataChan, DM_Item torqueDm ) { Int16 torque; /* Motor Torque */ /* Read motor torque value */ if (semiPowerMotorTorque( sio32DataChan, DRIVE_ID, &torque ) != OK) return (ERROR); /* Write motor torque to Data Manager */ return (writeDmItem(torqueDm, &torque, sizeof(torque))); } /* updateSemiPowerMotorTorqueDm() */ /****************************************************************************/ /* Function : updateSemiPowerDynamicBrakeDm */ /* Purpose : Reads semipower dynamic brake value and update dmgr item */ /* Inputs : Sio32 chan & dynamic brake data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS updateSemiPowerDynamicBrakeDm( semiPowerSerial *sio32DataChan, DM_Item dynamicBrakeDm ) { Int16 db; /* Dynamic Brake Value */ /* Read dynamic brake accumulator value */ if (semiPowerDynamicBrake( sio32DataChan, DRIVE_ID, &db ) != OK) return (ERROR); /* Write DB value to Data Manager */ return (writeDmItem(dynamicBrakeDm, &db, sizeof(db))); } /* updateSemiPowerDynamicBrakeDm() */ /****************************************************************************/ /* Function : updateSemiPowerElapsedHoursDm */ /* Purpose : Reads semipower elapsed hours and update dmgr item */ /* Inputs : Sio32 chan & elapsed hours data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS updateSemiPowerElapsedHoursDm( semiPowerSerial *sio32DataChan, DM_Item elapsedHoursDm ) { Int16 hours; /* Elapsed Hours Value */ /* Read elapsed hours */ if (semiPowerElapsedHours( sio32DataChan, DRIVE_ID, &hours ) != OK) return (ERROR); /* Write Elapsed Hours to Data Manager */ return (writeDmItem(elapsedHoursDm, &hours, sizeof(hours))); } /* updateSemiPowerElapsedHoursDm() */ /****************************************************************************/ /* Function : updateSemiPowerFaultQueueDm */ /* Purpose : Reads semipower fault queue and update dmgr item */ /* Inputs : Sio32 chan & fault queue data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS updateSemiPowerFaultQueueDm( semiPowerSerial *sio32DataChan, DM_Item faultQueueDm ) { struct faultQueueEntry faultQueue[FAULT_QUEUE_LENGTH]; /* Read fault queue */ if (semiPowerReadFaultQueue( sio32DataChan, DRIVE_ID, faultQueue ) != OK) return (ERROR); /* Write fault queue to Data Manager */ return (writeDmItem(faultQueueDm, faultQueue, sizeof(faultQueue))); } /* updateSemiPowerFaultQueueDm() */ Void semiPowerFaultHandler( semiPowerSerial *serialIO, Byte driveAdr, semiPowerFaultCode faultCode, thrusterDmItems *dmItems ) { Word driveStatus; logMsg("Motor Fault handler 0x%x\n", faultCode); initMotorEnableDm( dmItems->motorEnableStatus, DISABLE_MOTOR ); /* Read Drive Status to Clear Motor Fault */ semiPowerClearDriveFault( serialIO, driveAdr, &driveStatus ); initMotorFaultCodeDm( dmItems->motorFaultCode, faultCode ); } /* semiPowerFaultHandler() */ /****************************************************************************/ /* Function : initMotorVelocityDm */ /* Purpose : Sets motor velocoity data manager item */ /* Inputs : velocity data manager item handle and value */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS initMotorVelocityDm( DM_Item motorVelocityDm, motorCtrlStruct *motorParams, Int16 velocity ) { motorParams->motorVelocity = velocity; urgentWriteDmItem(motorVelocityDm, &velocity, sizeof(velocity)); } /* initMotorVelocityDm() */ /****************************************************************************/ /* 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( semiPowerSerial *sio32DataChan, DM_Item motorVelocityDm, motorCtrlStruct *motorParams ) { Int16 velocity; /* Motor Actual Velocity Value */ /* Read actual motor velocity */ if (semiPowerMotorVelocity(sio32DataChan, DRIVE_ID, &velocity) != OK) velocity = 0; motorParams->motorVelocity = velocity; /* Write motor velocity to Data Manager */ return (writeDmItem(motorVelocityDm, &velocity, sizeof(velocity))); } /* updateMotorStatusDm() */ /****************************************************************************/ /* Function : motorSetCmdTorque */ /* Purpose : Sets moog motor commanded torque on microcontroller */ /* Inputs : Sio32 channel, thrust values */ /* Outputs : Return OK or ERROR */ /****************************************************************************/ MLocal Void motorSetCmdTorque( Reg semiPowerSerial *serialIO, Reg Int16 cmdTorque ) { if (cmdTorque > MAX_TORQUE_LIMIT) cmdTorque = MAX_TORQUE_LIMIT; else if (cmdTorque < -MAX_TORQUE_LIMIT) cmdTorque = -MAX_TORQUE_LIMIT; semiPowerSetMotorTorque( serialIO, DRIVE_ID, cmdTorque ); } /* motorSetCmdTorque() */ /****************************************************************************/ /* Function : initMotorEnableDm */ /* Purpose : Uses urgent calls to set Motor Enable Dmgr item = FALSE */ /* Inputs : Motor enable 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 : initMotorFaultCodeDm */ /* Purpose : Uses urgent calls to set Motor Fault Code Dmgr item */ /* Inputs : Motor fault code data manager item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal Void initMotorFaultCodeDm( DM_Item motorFaultCodeDm, semiPowerFaultCode faultCode ) { /* Use urgent calls as task is not provider */ urgentWriteDmItem(motorFaultCodeDm, &faultCode, sizeof(faultCode)); } /* initMotorFaultCodeDm() */ /****************************************************************************/ /* Function : readMotorEnable */ /* 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 readMotorEnable( semiPowerSerial *sio32DataChan, DM_Item motorEnableDm, DM_Item motorEnableStatus, motorCtrlStruct *motorParams ) { 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 */ if (motorParams->controlMode == SEMIVELOCITY) semiPowerSetMotorSpeed( sio32DataChan, DRIVE_ID, 0, &motorParams->driveModeCmd ); else /* Send desired torque to motor */ motorSetCmdTorque(sio32DataChan, 0); /* Write Motor Enable Command to motor */ if (semiPowerSetMotorEnable( sio32DataChan, DRIVE_ID, (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); } /* readMotorEnable() */ /****************************************************************************/ /* Function : readMotorCtrlMode */ /* 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 readMotorCtrlMode(semiPowerSerial *sio32DataChan, thrusterDmItems *dmItems, motorCtrlStruct *motorParams) { motorCtrlMode controlMode; /* Motor Control Mode */ /* Read Motor Control Mode from Dmgr */ if (dm_read( dmItems->motorCtrlMode, (char *) &controlMode, sizeof(controlMode), (DM_Time *)NULL) != SUCCESS ) return(ERROR); if (controlMode == SEMIVELOCITY) /* Switch Semi-Power ctrl mode. rsm */ semiPowerSetDriveModeBit( sio32DataChan, DRIVE_ID, speedRegulatorMode, &motorParams->driveModeCmd ); else semiPowerSetDriveModeBit( sio32DataChan, DRIVE_ID, torqueRegulatorMode, &motorParams->driveModeCmd ); semiPowerSetDriveModeBit( sio32DataChan, DRIVE_ID, speedRegulatorMode, &motorParams->driveModeCmd ); semiPowerSendDriveMode( sio32DataChan, DRIVE_ID, motorParams->driveModeCmd ); motorParams->controlMode = controlMode; return (OK); } /* readMotorCtrlMode() */ /****************************************************************************/ /* Function : readMotorCmdThrust */ /* Purpose : Read Commanded Thrust from Dmgr & update motor params */ /* Inputs : Sio32 chan & motor commanded thrust Dmgr item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS readMotorCmdThrust(DM_Item motorCmdThrustDm, motorCtrlStruct *motorParams ) { Int16 thrustNewtons; /* Commanded Thrust from control system */ /* Read Commanded Thrust value from Dmgr */ if (dm_read( motorCmdThrustDm, (char *) &thrustNewtons, sizeof(thrustNewtons), (DM_Time *)NULL) != SUCCESS ) return (ERROR); motorParams->desiredThrust = thrustNewtons; return (OK); } /* readMotorCmdThrust() */ /****************************************************************************/ /* Function : readMotorCurrentGain */ /* 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 readMotorCurrentGain(thrusterDmItems *dmItems, motorCtrlStruct *motorParams ) { Int16 currentGain; /* Current gain dm value */ /* Read current gain from dmgr */ if (dm_read( dmItems->motorCurrentGain, (char *) ¤tGain, sizeof(currentGain), (DM_Time *)NULL) != SUCCESS ) return (ERROR); motorParams->currentGain = currentGain; return (OK); } /* readMotorCurrentGain() */ /****************************************************************************/ /* Function : readMotorVelocityGains */ /* 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 readMotorVelocityGains(thrusterDmItems *dmItems, motorCtrlStruct *motorParams) { velocity_gains gains; /* Velocity Gains from motor */ /* Read velocity gains from dmgr */ if (dm_read( dmItems->motorVelocityGains, (char *) &gains, sizeof(gains), (DM_Time *)NULL) != SUCCESS ) return (OK); /* Send values to motor micro */ motorParams->velocityGains[VEL_PROP_GAIN] = gains[VEL_PROP_GAIN]; motorParams->velocityGains[VEL_DERIV_GAIN] = gains[VEL_DERIV_GAIN]; motorParams->velocityGains[VEL_POWER_GAIN] = gains[VEL_POWER_GAIN]; return(OK); } /* readMotorVelocityGains() */ /****************************************************************************/ /* Function : readModelParamK4 */ /* 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 readModelParamK4(DM_Item modelParamK4Dm, motorCtrlStruct *motorParams) { 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 ) return (ERROR); motorParams->modelParamOneOnK4 = modelParamK4; return(OK); } /* readModelParamK4() */ /****************************************************************************/ /* 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 */ /****************************************************************************/ MLocal STATUS initTempAlarmThreshDm(thrusterDmItems *dmItems, motorCtrlStruct *motorParams) { /* We MUST use urgent open here as */ /* another task is normal provider */ urgentWriteDmItem(dmItems->tempWarnThresh, &(motorParams->temperatureAlarm.warnThresh), sizeof(motorParams->temperatureAlarm.warnThresh)); urgentWriteDmItem(dmItems->tempAlarmThresh, &(motorParams->temperatureAlarm.alarmThresh), sizeof(motorParams->temperatureAlarm.alarmThresh)); return(OK); } /* initTempAlarmThreshDm() */ /****************************************************************************/ /* Function : readTempAlarmThresh */ /* Purpose : Get temperature alarm thresholds from DM & send to motor */ /* Inputs : Sio32 channel, minimum velocity dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS readTempAlarmThresh(thrusterDmItems *dmItems, motorCtrlStruct *motorParams) { Int16 warnThresh, alarmThresh; /* Temp Alarm Threshold from motor */ /* Read Alarm Thresholds from dmgr */ if (dm_read( dmItems->tempWarnThresh, (char *) &warnThresh, sizeof(warnThresh), (DM_Time *) NULL) == SUCCESS) { if (dm_read( dmItems->tempAlarmThresh, (char *) &alarmThresh, sizeof(alarmThresh), (DM_Time *) NULL) == SUCCESS) { motorParams->temperatureAlarm.warnThresh = warnThresh; motorParams->temperatureAlarm.alarmThresh = alarmThresh; return (OK); } /* if */ } /* if */ return(ERROR); } /* readTempAlarmThresh() */ /****************************************************************************/ /* Function : initHumidityAlarmThreshDm */ /* Purpose : Read Humidity Alarm Thresholds and update Dmgr items */ /* Inputs : Sio32 channel, temp alarm threshold dm item handle */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS initHumidityAlarmThreshDm(thrusterDmItems *dmItems, motorCtrlStruct *motorParams) { /* We MUST use urgent open here as */ /* another task is normal provider */ urgentWriteDmItem(dmItems->humidityWarnThresh, &motorParams->humidityAlarm.warnThresh, sizeof(motorParams->humidityAlarm.warnThresh)); urgentWriteDmItem(dmItems->humidityAlarmThresh, &motorParams->humidityAlarm.alarmThresh, sizeof(motorParams->humidityAlarm.alarmThresh)); return(OK); } /* initHumidityAlarmThreshDm() */ /****************************************************************************/ /* Function : readHumidityAlarmThresh */ /* Purpose : Get humdidity alarm thresholds from DM & send to motor */ /* Inputs : Data Manager Items, motor parameters */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal STATUS readHumidityAlarmThresh(thrusterDmItems *dmItems, motorCtrlStruct *motorParams) { Int16 warnThresh, alarmThresh; /* Humidity Alarm Thresholds */ /* Read Alarm Thresholds from dmgr */ if (dm_read( dmItems->humidityWarnThresh, (char *) &warnThresh, sizeof(warnThresh), (DM_Time *) NULL) == SUCCESS) { if (dm_read( dmItems->humidityAlarmThresh, (char *) &alarmThresh, sizeof(alarmThresh), (DM_Time *) NULL) == SUCCESS) { motorParams->humidityAlarm.warnThresh = warnThresh; motorParams->humidityAlarm.alarmThresh = alarmThresh; return (OK); } /* if */ } /* if */ return(ERROR); } /* readHumidityAlarmThresh() */ /****************************************************************************/ /* Function : checkMotorPowerReduced */ /* Purpose : Check Reduced Power Flag and confirm power reduction */ /* Inputs : Reduced Power & Confirmation Data manager Items */ /* Outputs : None */ /****************************************************************************/ Void checkMotorReducedPower(thrusterDmItems *dmItems) { MBool reduced; /* Read Motor Power Reduced Flag from DM*/ if (dm_read(dmItems->motorReducePowerDm, (char *) &reduced, sizeof(reduced), (DM_Time *)NULL) == SUCCESS ) if (reduced) writeDmItem(dmItems->motorRedPwrConfirmDm, &reduced, sizeof(reduced)); } /* checkMotorReducedPower() */ 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 (DISABLE_MOTOR); } /* 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 : sendMotorCmdString */ /* Purpose : Send Motor Command String to Motor Controller */ /* Inputs : Sio32 chan & data manager item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ MLocal Void sendMotorCmdString( semiPowerSerial *serialIO, thrusterDmItems *dmItems ) { Word value; semiPowerCmd motorCmd; char *pos, temp[32]; bzero((char *) &motorCmd, sizeof(motorCmd)); if (dm_read( dmItems->motorCmdString, (char *) &motorCmd, sizeof(motorCmd), (DM_Time *)NULL) == SUCCESS ) { if (strlen(motorCmd.value)) /* Check for a write command */ { /* If value is hex, convert to int */ if ((pos = strstr(motorCmd.value, "0x")) != NULL) { value = strtol(pos + 2, (char **) &temp, 16); semiPowerWriteCmd(serialIO, DRIVE_ID, motorCmd.paramID, value); } else { for (pos = (char *) &motorCmd.paramID; *pos != '\0'; pos++) *pos = toupper(*pos); /* convert string to upper case */ value = atoi(motorCmd.value); semiPowerWriteCmd(serialIO, DRIVE_ID, motorCmd.paramID, value); } /* else */ } /* if */ /* read back value from controller */ if (semiPowerReadCmd(serialIO, DRIVE_ID, motorCmd.paramID, &value) == OK) writeDmItem(dmItems->motorCmdReply, &value, sizeof(value)); } /* if */ } /* sendMotorCmdString() */ /****************************************************************************/ /* 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* motorPowerSwitchDmName, const char* reducedPwrConfirmDmName ) { char dmItemName[DM_ITEM_NAME_LEN]; /* Complete item name string*/ dmItemList thrustDmInit[] = /* Data Manager Item Table for init */ { { &dmItems->temperature, "Controller Temperature", HEATSINK_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->humidity, "Controller Humidity", HUMIDITY_DM, DM_NAT16, 1 }, { &dmItems->humidityAlarm, "Humidity Alarm", HUMIDITY_ALARM_DM, DM_ENUM, 1 }, { &dmItems->humidityWarnThresh, "Humidity Warn Threshold", HUMIDITY_WARN_THRESH_DM, DM_INT16, 1 }, { &dmItems->humidityAlarmThresh, "Humidity Alarm Threshold", HUMIDITY_ALARM_THRESH_DM, DM_INT16, 1 }, { &dmItems->outputPower, "Controller Output Power", OUTPUT_POWER_DM, DM_INT16, 1 }, { &dmItems->inputVoltage, "Controller Input Voltage", INPUT_VOLTAGE_DM, DM_INT16, 1 }, { &dmItems->motorCurrent, "Controller Motor Current", MOTOR_CURRENT_DM, DM_NAT16, 1 }, { &dmItems->elapsedHours, "Controller Elapsed Hours", ELAPSED_HOURS_DM, DM_INT16, 1 }, { &dmItems->clearFaultQueue, "Clear Fault Queue", CLEAR_FAULT_QUEUE_DM, DM_EMPTY, 1 }, { &dmItems->readFaultQueue, "Read Fault Queue", READ_FAULT_QUEUE_DM, DM_EMPTY, 1 }, { &dmItems->motorFaultCode, "Motor Fault Code", MOTOR_FAULT_CODE_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->motorCurrentGain, "Motor Current Gain Param", MOTOR_CURRENT_GAIN_DM, DM_INT16, 1 }, { &dmItems->motorVelocityGains, "Motor Velocity Gain Params", MOTOR_VELOCITY_GAINS_DM, DM_INT16, VELOCITY_GAIN_SIZE }, { &dmItems->motorModelParamK4, "Motor Model K4 Param", MOTOR_MODEL_PARAM_K4_DM, DM_INT16, 1 }, { &dmItems->motorVelocity, "Motor Velocity", MOTOR_VELOCITY_DM, DM_INT16, 1 }, { &dmItems->motorTorque, "Motor Torque", MOTOR_TORQUE_DM, DM_INT16, 1 }, { &dmItems->motorCtrlTorque, "Motor Controller Torque", MOTOR_CTRL_TORQUE_DM, DM_INT16, 1 }, { &dmItems->motorCtrlParam, "Motor Controller Param", MOTOR_CTRL_PARAM_DM, DM_INT32, 1 }, { &dmItems->motorStallAlarm, "Motor Software Stall Alarm", MOTOR_STALL_ALARM_DM, DM_EMPTY, 1 }, { &dmItems->motorCmdReply, "Motor Command Reply", MOTOR_MANUAL_CMD_REPLY_DM, DM_INT16, 1 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; if ((dmItems->motorPowerSwitchDm = initMicroDmItem("", (char *) motorPowerSwitchDmName, DM_ENUM, 1)) == ERROR) return(ERROR); /* 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); strcpy(dmItemName, thrusterDmPrefix); strcat(dmItemName, MOTOR_MANUAL_CMD_DM); if (dm_create(dmItemName, 1, &dmItems->motorCmdString, DM_CHAR, MOTOR_PARAM_ID_LEN, DM_CHAR, MOTOR_VALUE_LEN, DM_ENDT) == ERROR) logMsg("Error creating motor command string Data Manager Item\n"); strcpy(dmItemName, thrusterDmPrefix); strcat(dmItemName, FAULT_QUEUE_STRUCT_DM); if (dm_create(dmItemName, 1, &dmItems->faultQueue, DM_NAT16, 1, DM_NAT32, 1, DM_NAT16, 1, DM_NAT16, 1, DM_INT16, 1, DM_NAT16, 1, DM_NAT16, 1, DM_NAT32, 1, DM_NAT16, 1, DM_NAT16, 1, DM_NAT16, 1, DM_INT16, 1, DM_NAT16, 1, DM_NAT32, 1, DM_NAT16, 1, DM_NAT16, 1, DM_INT16, 1, DM_NAT16, 1, DM_NAT16, 1, DM_NAT32, 1, DM_NAT16, 1, DM_NAT16, 1, DM_INT16, 1, DM_NAT16, 1, DM_ENDT) == ERROR) logMsg("Error creating motor command string Data Manager Item\n"); return (initMicroDmItemList(thrustDmInit, thrusterDmPrefix, thrusterName)); } /* createThrusterDmItems() */ MLocal Void initMotorParams( motorCtrlStruct *motorParams ) { motorParams->controlMode = SEMIVELOCITY; motorParams->desiredThrust = 0; motorParams->motorVelocity = 0; motorParams->lastVelocity = 0; motorParams->lastVelDeriv = 0; motorParams->filtAcceleration = 0; motorParams->currentGain = 1; motorParams->velocityGains[VEL_PROP_GAIN] = 300; motorParams->velocityGains[VEL_DERIV_GAIN] = 4000; motorParams->velocityGains[VEL_POWER_GAIN] = 57; motorParams->modelParamOneOnK4 = 2400; /* 1.0/K4 */ motorParams->temperatureAlarm.alarmStatus = MICRO_ALARM_OK; motorParams->temperatureAlarm.warnThresh = TEMP_WARN_THRESH; motorParams->temperatureAlarm.alarmThresh = TEMP_ALARM_THRESH; motorParams->humidityAlarm.alarmStatus = MICRO_ALARM_OK; motorParams->humidityAlarm.warnThresh = HUMIDITY_WARN_THRESH; motorParams->humidityAlarm.alarmThresh = HUMIDITY_ALARM_THRESH; motorParams->driveModeCmd = 0xffff; } /* initMotorParams() */ MLocal STATUS initMotorParamsDm(thrusterDmItems *dmItems, semiPowerSerial *serialIO, motorCtrlStruct *motorParams) { initMotorEnableDm( dmItems->motorEnableStatus, DISABLE_MOTOR ); initMotorFaultCodeDm( dmItems->motorFaultCode, motorNoFault_FC ); urgentWriteDmItem( dmItems->motorCtrlMode, &motorParams->controlMode, sizeof(motorParams->controlMode)); /* Use urgent calls as task is not provider */ urgentWriteDmItem(dmItems->motorCmdThrust, &motorParams->desiredThrust, sizeof(motorParams->desiredThrust)); urgentWriteDmItem(dmItems->motorCurrentGain, &motorParams->currentGain, sizeof(motorParams->currentGain)); urgentWriteDmItem(dmItems->motorVelocityGains, &motorParams->velocityGains, sizeof(motorParams->velocityGains)); urgentWriteDmItem(dmItems->motorModelParamK4, &motorParams->modelParamOneOnK4, sizeof(motorParams->modelParamOneOnK4)); /* Read temperature and update Dmgr item */ updateSemiPowerTempDm(serialIO, dmItems->temperature); initTempAlarmThreshDm(dmItems, motorParams); /* Set temperature alarm Dmgr item */ updateAlarmDm(dmItems->temperatureAlarm, MICRO_ALARM_OK); /* Read humidity and update Dmgr item */ updateSemiPowerHumidityDm( serialIO, dmItems->humidity ); initHumidityAlarmThreshDm(dmItems, motorParams); /* Set humidity alarm Dmgr item */ updateAlarmDm(dmItems->humidityAlarm, MICRO_ALARM_OK); /* Read dynamic brake and udpate Dmgr item */ updateSemiPowerDynamicBrakeDm(serialIO, dmItems->dynamicBrake); return (OK); } /* initMotorParamsDm() */ /****************************************************************************/ /* Function : calcMotorAcceleration */ /* Purpose : Calculate filtered motor acceleration from velocity */ /* Inputs : New motor velocity value */ /* Outputs : lastVelocity, lastVelDeriv, */ /****************************************************************************/ MLocal Void calcMotorAcceleration(Int16 velocity, motorCtrlStruct *motorParams) { Int32 velDeriv; Int32 timeConstant = (VEL_DM_PERIOD / 1000); velDeriv = (velocity - motorParams->lastVelocity) / timeConstant; motorParams->filtAcceleration = (motorParams->filtAcceleration * (2 * VEL_FILTER_TIME_CONST - timeConstant) + (velocity - motorParams->lastVelocity) + timeConstant * motorParams->lastVelDeriv) / (VEL_FILTER_TIME_CONST + timeConstant); motorParams->lastVelocity = velocity; motorParams->lastVelDeriv = velDeriv; } /* calcMotorAcceleration() */ /****************************************************************************/ /* Function : motorDeadbandCompensate */ /* Purpose : Adds an offset to non-zero torque commands for deadband */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Int16 motorDeadbandCompensate( Int16 torque, Int16 command ) { /* Compensate for thruster deadband */ /* By adding an offset to non zero commands */ if (command > 0) torque += MOTOR_TORQUE_DEADBAND; else if (command < 0) torque -= MOTOR_TORQUE_DEADBAND; return (torque); } /* motorDeadbandCompensate */ /****************************************************************************/ /* Function : currentMode */ /* Purpose : Motor Current Command Mode algorithm */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void currentMode( semiPowerSerial *serialIO, motorCtrlStruct *motorParams ) { Int16 torque; torque = motorParams->currentGain * NEWTONS_TO_MOTOR_UNITS (motorParams->desiredThrust); torque = motorDeadbandCompensate(torque, motorParams->desiredThrust); motorParams->motorCtrlTorque = torque; motorSetCmdTorque(serialIO, torque); /* Send desired torque to motor */ } /* currentMode() */ /****************************************************************************/ /* Function : semiVelocityMode */ /* Purpose : Semi-Power Speed Regulator Control rsm */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void semiVelocityMode( semiPowerSerial *serialIO, motorCtrlStruct *motorParams ) { Int16 omegad, sgnThrust, oneOnK4; double thrust; /* ** The objective here is to command the speed setpoint to the ** steady-state value that corresponds to the desired thrust. This ** relation is omegad = sgn(Thrust) * sqrt(Thrust/k4) ** ** Notes: ** 1) oneOnK4 should be refined, based on the results of our tank tests. ** 2) Also try computing omegad (desiredVelocity) as in velocityMode. */ thrust = (double) motorParams->desiredThrust; oneOnK4 = motorParams->modelParamOneOnK4; if (thrust > 0) /* Compute sgn(thrust) */ sgnThrust = 1; else sgnThrust = -1; omegad = (Int16) ( sgnThrust * sqrt( sgnThrust*thrust*oneOnK4 ) ); if (omegad > MAX_MOTOR_RPM) omegad = MAX_MOTOR_RPM; else if (omegad < -MAX_MOTOR_RPM) omegad = -MAX_MOTOR_RPM; semiPowerSetMotorSpeed( serialIO, DRIVE_ID, omegad, &motorParams->driveModeCmd ); } /* semiVelocityMode() */ /****************************************************************************/ /* Function : velocityMode */ /* Purpose : Motor Velocity Command Mode algorithm */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void velocityMode( semiPowerSerial *serialIO, motorCtrlStruct *motorParams ) { Int16 torque; /* Torque value calculated */ Int16 velocity; /* Motor velocity adjusted for divide */ Int16 absVelocity; /* Absolute value of velocity */ Int32 desiredVelocity; /* Desired velocity - calc from thrust */ Int32 temp; /* Temporary stores intermediate result */ /* Copy motor velocity feedback */ velocity = motorParams->motorVelocity; if (velocity < 0) /* Derive absolute value of velocity */ absVelocity = -velocity; else absVelocity = velocity; /* Adjust to prevent divide by zero */ if ( (velocity > -VELOCITY_DEADBAND) && (velocity < VELOCITY_DEADBAND) ) { velocity = 0; absVelocity = 1; } /* if */ /* Calculate filtered motor acceleration*/ calcMotorAcceleration(velocity, motorParams); motorParams->motorCtrlParam = motorParams->filtAcceleration; /* Calc desired velocity from thrust */ desiredVelocity = (Int32) (( (Int32) motorParams->desiredThrust * (Int32) motorParams->modelParamOneOnK4) / (Int32) absVelocity); if (desiredVelocity > MAX_MOTOR_RPM) desiredVelocity = MAX_MOTOR_RPM; else if (desiredVelocity < -MAX_MOTOR_RPM) desiredVelocity = -MAX_MOTOR_RPM; temp = (((Int32) (desiredVelocity - (Int32) velocity) * MAX_TORQUE_CMD / (Int32) motorParams->velocityGains[VEL_PROP_GAIN]) - ((Int32) motorParams->filtAcceleration * MAX_TORQUE_CMD / (Int32) motorParams->velocityGains[VEL_DERIV_GAIN]) + ((Int32) motorParams->desiredThrust * MAX_TORQUE_CMD / (Int32) motorParams->velocityGains[VEL_POWER_GAIN]) ); torque = (Int16) (temp / (Int32) MAX_MOTOR_TORQUE); torque = motorDeadbandCompensate(torque, motorParams->desiredThrust); if (torque > MAX_TORQUE_LIMIT) torque = MAX_TORQUE_LIMIT; else if (torque < -MAX_TORQUE_LIMIT) torque = -MAX_TORQUE_LIMIT; motorParams->motorCtrlTorque = torque; motorSetCmdTorque(serialIO, torque); /* Send desired torque to motor */ } /* velocityMode() */ /****************************************************************************/ /* Function : motorControlLoop */ /* Purpose : Motor Control Loop. Call algorithm based on control mode */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void motorControlLoop( semiPowerSerial *serialIO, motorCtrlStruct *motorParams ) { switch (motorParams->controlMode) { case CURRENT: /* Current Control Mode */ currentMode(serialIO, motorParams); break; case VELOCITY: /* Velocity Control Mode */ velocityMode(serialIO, motorParams); break; case SEMIVELOCITY: /* Semi-Power Speed Control Mode rsm */ semiVelocityMode(serialIO, motorParams); break; default: /* Unrecognized mode so disable motor */ semiPowerSetMotorEnable( serialIO, DRIVE_ID, FALSE ); motorSetCmdTorque(serialIO, 0); /* Set motor torque to 0 */ } /* switch */ } /* motorControlLoop() */ MLocal switchStatus motorPowerSwitchState(thrusterDmItems *dmItems) { switchStatus powerSwitch; if (dm_read(dmItems->motorPowerSwitchDm, &powerSwitch, sizeof(powerSwitch), (DM_Time *) NULL) == SUCCESS ) return (powerSwitch); else return (SWITCH_OFF); } /* microDevPowerSwitchStatus() */ /****************************************************************************/ /* Function : thrusterTask */ /* Purpose : Initializes thruster motor communications. Thruster Data */ /* inout task */ /* 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 */ motorCtrlStruct motorParams; SEM_ID outTaskSyncSem; semiPowerSerial serialIO; Int32 outTaskId; /* Output task ID for taskDelete */ char outTaskName[16]; /* Task name for thruster out tasks */ taskPrioritySet(taskIdSelf(), THRUST_IN_PRIORITY); bzero((char *) &serialIO, sizeof(serialIO)); /* Initialize micro serial communication*/ if (sio32RS485ChanInit(serialChannel, (char *) thrustChanName[thruster], 19200, 8, 1, "NONE", &serialIO.sio32) == ERROR) { logMsg("Error initializing serial IO for %s thruster", thrusterName[thruster]); return (ERROR); } /* if */ /* Disable serial I/O Keep Alive Traffic*/ sio32ChanSetKeepAliveMode(serialChannel, WAIT_FOREVER, 0); /* Clear list of Data Manager Items */ bzero((char *) &dmItems, sizeof(dmItems)); semiPowerFaultHandlerHookAdd(&serialIO, (FUNCPTR) semiPowerFaultHandler, (Int32) &dmItems); /* Create Thruster Application DM Items */ if (createThrusterDmItems(&dmItems, thrustDmPrefix[thruster], thrusterName[thruster], thrusterPowerDmName[thruster], thrusterReducedPowerConfirmDmName[thruster]) == ERROR) { /* Error occurred, print message & exit */ sio32ChanDestroy( serialChannel, &serialIO.sio32 ); return(ERROR); } /* if */ /* Create Thruster Output Task Name */ sprintf(outTaskName, "thrOut%d", thruster); /* Create task synchronization sem */ if ((outTaskSyncSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { sio32ChanDestroy( serialChannel, &serialIO.sio32 ); return (ERROR); } /* if */ if ((outTaskId = /* Start Thruster Data Output Task */ taskSpawn(outTaskName, THRUST_OUT_PRIORITY, 0, THRUST_OUT_STACKSIZE, (FUNCPTR) thrusterOutTask, (int) &dmItems, (int) &serialIO, (int) &motorParams, (int) outTaskSyncSem, 0, 0, 0, 0, 0, 0)) == ERROR) { /* Close serial communication channel */ sio32ChanDestroy( serialChannel, &serialIO.sio32 ); logMsg("Thruster I/F: Error spawning data output task\n"); return(ERROR); } /* if */ thrusterInTask(&dmItems, &serialIO, &motorParams, outTaskSyncSem); /* Close serial communication channel */ sio32ChanDestroy( serialChannel, &serialIO.sio32 ); taskDelete(outTaskId); /* 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 function */ 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 Ctrl structure */ /* Outputs : Normally runs forever, but exits if error occurs */ /****************************************************************************/ Void thrusterInTask(Reg thrusterDmItems *dmItems, semiPowerSerial *serialIO, motorCtrlStruct *motorParams, SEM_ID outTaskSyncSem) { SEM_ID wakeupSem; /* Wakeup semaphore for sampling */ Int32 updates = 0; /* Counts dm updates for rate calculation*/ Int32 ticks, delay; Int32 updatePeriod; Nat16 motorStallCount = 0; /* Counts motor stall events */ telemItemList telemItem = INPUT_VOLTAGE; /* 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 items */ dm_start_provider(dmItems->motorVelocity, VEL_DM_PERIOD); dm_start_provider(dmItems->motorTorque, VEL_DM_PERIOD); dm_start_provider(dmItems->motorCtrlTorque, VEL_DM_PERIOD); dm_start_provider(dmItems->motorCtrlParam, VEL_DM_PERIOD); dm_start_provider(dmItems->temperature, TELEM_DM_PERIOD); dm_start_provider(dmItems->humidity, TELEM_DM_PERIOD); dm_start_provider(dmItems->outputPower, TELEM_DM_PERIOD); dm_start_provider(dmItems->inputVoltage, TELEM_DM_PERIOD); dm_start_provider(dmItems->motorCurrent, TELEM_DM_PERIOD); dm_start_provider(dmItems->dynamicBrake, TELEM_DM_PERIOD); dm_start_provider(dmItems->elapsedHours, TELEM_DM_PERIOD); dm_start_provider(dmItems->motorRedPwrConfirmDm, DM_STATIC); dm_start_provider(dmItems->motorStallAlarm, DM_STATIC); dm_start_provider(dmItems->temperatureAlarm, DM_STATIC); dm_start_provider(dmItems->humidityAlarm, DM_STATIC); dm_start_consumer(dmItems->motorEnableStatus, DM_STATIC, SEM_NULL); dm_start_consumer(dmItems->motorReducePowerDm, DM_STATIC, SEM_NULL); dm_start_consumer(dmItems->motorVelocity, DM_STATIC, SEM_NULL); dm_start_consumer(dmItems->motorCmdThrust, DM_ASYNC, SEM_NULL); dm_start_consumer(dmItems->temperature, TELEM_DM_PERIOD, SEM_NULL); dm_start_consumer(dmItems->humidity, TELEM_DM_PERIOD, SEM_NULL); dm_start_consumer(dmItems->motorPowerSwitchDm, DM_STATIC, wakeupSem); initMotorParams(motorParams); /* Set motor params to default values */ FOREVER { initMotorEnableDm( dmItems->motorEnableStatus, DISABLE_MOTOR ); initMotorVelocityDm(dmItems->motorVelocity, motorParams, 0); /* Wait for motor to power up */ while (motorPowerSwitchState(dmItems) != SWITCH_ON) semTake(wakeupSem, WAIT_FOREVER); initMotorParams(motorParams); /* Set motor params to default values */ /* Configure motor controller parameters*/ semiPowerMotorInit( serialIO, DRIVE_ID, &motorParams->driveModeCmd ); /* Create Thruster Output Task Name */ initMotorParamsDm(dmItems, serialIO, motorParams); ticks = tickGet(); updatePeriod = (sysClkRateGet() * VEL_DM_PERIOD) / USECS_PER_SEC; semGive(outTaskSyncSem); /* wait up out task */ FOREVER { taskSafe(); /* Stop task from being deleted during */ /* serial IO and Data Manager updates */ /* Read actual motor velocity */ updateMotorVelocityDm(serialIO, dmItems->motorVelocity, motorParams); /* Read commanded thrust */ readMotorCmdThrust(dmItems->motorCmdThrust, motorParams); /* Run motor controller algorithm */ motorControlLoop( serialIO, motorParams ); writeDmItem(dmItems->motorCtrlTorque, &motorParams->motorCtrlTorque, sizeof(motorParams->motorCtrlTorque)); writeDmItem(dmItems->motorCtrlParam, &motorParams->motorCtrlParam, sizeof(motorParams->motorCtrlParam)); /* Confirm power reduction for power mgr*/ checkMotorReducedPower(dmItems); /* update telemetry data items */ if ((updates % (TELEM_DM_PERIOD / VEL_DM_PERIOD)) != 0) updates++; else { switch(((Int32) telemItem)++) /* Select item to read at this tick */ { case INPUT_VOLTAGE: updateSemiPowerInputVoltageDm( serialIO, dmItems->inputVoltage ); break; case MECHANICAL_POWER: updateSemiPowerOutputPowerDm( serialIO, dmItems->outputPower ); break; case MOTOR_CURRENT: updateSemiPowerMotorCurrentDm( serialIO, dmItems->motorCurrent ); break; case MOTOR_TORQUE: updateSemiPowerMotorTorqueDm( serialIO, dmItems->motorTorque ); break; case ELAPSED_HOURS: updateSemiPowerElapsedHoursDm( serialIO, dmItems->elapsedHours ); break; case TEMPERATURE: updateSemiPowerTempDm(serialIO, dmItems->temperature); checkTemperatureAlarm(dmItems, motorParams); break; case HUMIDITY: updateSemiPowerHumidityDm(serialIO, dmItems->humidity); checkHumidityAlarm(dmItems, motorParams); break; case END_OF_TELEM_LIST: telemItem = INPUT_VOLTAGE; updates = 1; break; } /* switch */ } /* if */ /* Software motor stall detection */ checkForMotorStall(dmItems, &motorStallCount); taskUnsafe(); /* Task may now be deleted safely */ if (motorPowerSwitchState(dmItems) != SWITCH_ON) break; /* Exit loop and wait for power up */ /* Update Rate is not guaranteed due to */ /* the possibility of task preemption */ delay = updatePeriod - (tickGet() - ticks); if (delay > 0) semTake(wakeupSem, delay); ticks = tickGet(); } /* FOREVER */ } /* 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(Reg thrusterDmItems *dmItems, semiPowerSerial *serialIO, motorCtrlStruct *motorParams, SEM_ID outTaskSyncSem) { DM_Group dmGroup; /* Thruster Data Data Manager Group */ SEM_ID dmUpdateSem; /* Wakes up task when item values changes*/ DWord changedBits; /* Holds dm_group_changes bit vector */ DWord motorEnableBit; /* Motor Enable item changed bit */ DWord motorCtrlModeBit; /* Motor Control Mode item changed bit */ DWord currentGainBit; /* Motor Current Mode Gain changed bit */ DWord velocityGainsBit; /* Motor Velocity Mode Gains changed bit */ DWord modelK4ParamBit; /* Motor dymanic model parameter K4 bit */ DWord tempWarnBit; /* Temp warn threshold item changed bit */ DWord tempAlarmBit; /* Temp alarm threshold item changed bit */ DWord humidityWarnBit; /* Humidity warn threshold changed bit */ DWord humidityAlarmBit; /* Humidity alarm threshold changed bit */ DWord readFaultQueueBit; /* Read fault queue command change bit */ DWord clearFaultQueueBit; /* Clear fault queue command change bit */ DWord motorCmdStringBit; /* Motor Command String changed bit */ /* 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 */ /* Declare this task a consumer of items */ /* which are written to the motor */ dm_start_provider(dmItems->motorEnableStatus, DM_STATIC); dm_start_consumer(dmItems->motorEnableReq, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->motorCtrlMode, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->motorCurrentGain, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->motorVelocityGains, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->motorModelParamK4, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->tempWarnThresh, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->tempAlarmThresh, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->humidityWarnThresh, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->humidityAlarmThresh, DM_STATIC, dmUpdateSem); dm_start_provider(dmItems->faultQueue, DM_STATIC); dm_start_consumer(dmItems->readFaultQueue, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->clearFaultQueue, DM_STATIC, dmUpdateSem); dm_start_provider(dmItems->motorCmdReply, DM_STATIC); dm_start_consumer(dmItems->motorCmdString, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->motorPowerSwitchDm, DM_STATIC, dmUpdateSem); dmGroup = dm_create_group(); /* Create Data Manager Group for items */ dm_group_add_item(dmGroup, dmItems->motorEnableReq, &motorEnableBit); dm_group_add_item(dmGroup, dmItems->motorCtrlMode, &motorCtrlModeBit); dm_group_add_item(dmGroup, dmItems->motorCurrentGain, ¤tGainBit); dm_group_add_item(dmGroup, dmItems->motorVelocityGains, &velocityGainsBit); dm_group_add_item(dmGroup, dmItems->motorModelParamK4, &modelK4ParamBit); dm_group_add_item(dmGroup, dmItems->tempWarnThresh, &tempWarnBit); dm_group_add_item(dmGroup, dmItems->tempAlarmThresh, &tempAlarmBit); dm_group_add_item(dmGroup, dmItems->humidityWarnThresh, &humidityWarnBit); dm_group_add_item(dmGroup, dmItems->humidityAlarmThresh, &humidityAlarmBit); dm_group_add_item(dmGroup, dmItems->readFaultQueue, &readFaultQueueBit); dm_group_add_item(dmGroup, dmItems->clearFaultQueue, &clearFaultQueueBit); dm_group_add_item(dmGroup, dmItems->motorCmdString, &motorCmdStringBit); FOREVER { /* Wait for In Task to Wake us up */ semTake(outTaskSyncSem, WAIT_FOREVER); /* Clear data manager item changed bits */ changedBits = dm_get_group_changes(dmGroup); while (motorPowerSwitchState(dmItems) == SWITCH_ON) { /* 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 */ logMsg("Thruster I/F: sending zero thrust command\n"); if (motorParams->controlMode == SEMIVELOCITY) semiPowerSetMotorSpeed( serialIO, DRIVE_ID, 0, &motorParams->driveModeCmd ); else /* Send desired torque to motor */ motorSetCmdTorque(serialIO, 0); } /* if */ } /* while */ /* Ask Data Manager which items changed */ changedBits = dm_get_group_changes(dmGroup); taskSafe(); /* Prevent task from being deleted during*/ /* serial IO and Data Manager updates */ /* Send Enable/Disable command to motor */ if (changedBits & motorEnableBit) readMotorEnable(serialIO, dmItems->motorEnableReq, dmItems->motorEnableStatus, motorParams); /* Send Motor Control Mode to motor */ if (changedBits & motorCtrlModeBit) readMotorCtrlMode(serialIO, dmItems, motorParams); /* Send Current mode gains to motor */ if (changedBits & currentGainBit) readMotorCurrentGain(dmItems, motorParams); /* Send Velocity mode gains to motor */ if (changedBits & velocityGainsBit) readMotorVelocityGains(dmItems, motorParams); /* Send Dynamic Model params to motor */ if (changedBits & modelK4ParamBit) readModelParamK4(dmItems->motorModelParamK4, motorParams); /* Set Temp Alarm Thresholds */ if ((changedBits & tempWarnBit) || (changedBits & tempAlarmBit)) readTempAlarmThresh(dmItems, motorParams); /* Set Humidity Alarm Thresholds */ if ((changedBits & humidityWarnBit) || (changedBits & humidityAlarmBit)) readHumidityAlarmThresh(dmItems, motorParams); /* Send Command String to motor */ if (changedBits & motorCmdStringBit) sendMotorCmdString(serialIO, dmItems); /* Read Fault Queue */ if (changedBits & readFaultQueueBit) updateSemiPowerFaultQueueDm( serialIO, dmItems->faultQueue ); /* Send Clear Fault Queue Command */ if (changedBits & clearFaultQueueBit) { semiPowerClearFaultQueue( serialIO, DRIVE_ID ); updateSemiPowerFaultQueueDm( serialIO, dmItems->faultQueue ); } /* if */ taskUnsafe(); /* Task may now be safely deleted */ } /* while */ } /* 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 */ /* m_task_exit functions. */ } /* thrusterOutTask */ /* That's all folks ! */