#include "microTask.h" /* Microcontroller Task IO definitions */ #define NUM_TASKS 3 /* Number of co-operating tasks */ /****************************************************************************/ /* 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) { microIOControl microIOCtrl; /* Microcontroller IO Task control */ 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 */ Initialize Data Manager Items... . . . Start providers, consumers etc ... . . . ************* REMEMBER TO USE VX_FP_TASK IF NEEDED FOR THE SPAWNS ********** /* Start Thruster Data Input Task */ if ((microIOCtrl.childTasks[OUT_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 */ /* Start Thruster Data Output Task */ if ((microIOCtrl.childTasks[IN_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) setSerialLinkState(µIOCtrl, LINK_DOWN); else /* Wake up output task to configure micro*/ semGive(microIOCtrl.childTasks[OUT_TASK].taskSyncSem); } /* 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) { Initialize Data Manager Items... . . . Start providers, consumers etc ... . . . 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 */ Update data Manager Items . . . 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) { #if INCLUDE_MOTOR_VEL_BUF DWord velBufEnableBit; DWord velBufReadBit; Int16 velBufData[MOTOR_DAS_BUF_SIZE]; Int16 torqueBufData[MOTOR_DAS_BUF_SIZE]; #endif /* This task consumes motor power switch */ dm_start_consumer(microIOCtrl->powerSwitchDm, DM_STATIC, dmUpdateSem); Initialize Data Manager Items... . . . Start providers, consumers etc... . . . Create and initialize groups... dm_group_add_item(dmGroup, microIOCtrl->powerSwitchDm, &motorSwitchBit); FOREVER { /* Mark micro configuration as invalid */ microIOCtrl->microConfigured = FALSE; /* Wait for micro reset via SRQ Task */ semTake(microIOCtrl->childTasks[OUT_TASK].taskSyncSem, WAIT_FOREVER); Initialize Data Manager Items to reflect actual state in micro... . . . Download parameters to configure micro... . . . /* Flag that micro has been configured */ microIOCtrl->microConfigured = TRUE; /* Wake up input task to read from micro */ semGive(microIOCtrl->childTasks[IN_TASK].taskSyncSem); The keep alive mode only needs to be changed for devices that need to detect loss of communication with the GMS, like motors, pan/tilts, etc /* 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]); /* Clear data manager item changed bits */ thrustGroupBits = dm_get_group_changes(dmGroup); while (microIOCtrl->serialLinkState == LINK_UP) { /* Wait for any data items to change */ /* 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) && (powerSwitchState(microIOCtrl) != SWITCH_ON) ) { if (motorEnabledState(dmItems) == ENABLE_MOTOR) initMotorEnableDm(dmItems->motorEnable, 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 */ Process data manager item changes and output commands to micro . . . /* 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() */ Change function type to Word, not Mbool Change returns need to change LOCAL Word processThrusterSRQ(Reg thrusterDmItems *dmItems, microIOControl *microIOCtrl) /****************************************************************************/ /* 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 */ /****************************************************************************/ LOCAL 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) == OK) 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; } /* 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 */ /****************************************************************************/ LOCAL 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");