computeSlew.py

Go to the documentation of this file.

from __future__ import division, absolute_import

import collections

import coordConv
from twistedActor import log

import tcc.base
from tcc.util import getAxisNames
from .computeObj import computeObj
from .checkSlewPath import checkSlewPath
from .oneSlewIter import oneSlewIter

ShortSlewTime = 5.0 # replace this with a suitable test of tune parameters;
    # a short slew's end-time < slew end time + track adv time + perhaps extra for time to send all slew mnodes

class SlewData(object):
    """!Data about a slew
    """
    def __init__(self, pathList, begTime, endTime, numIter):
        """!Construct a SlewData

        @param[in] pathList  a list of pvtList, one per axis;
            pvtList will be () in two cases:
            - axes for which a gentle halt was wanted, but a valid slew could not be computed
              (in this case you should command an immediate halt)
            - axes that were not drifting
        @param[in] begTime  starting time of slew
        @param[in] endTime  end time of slew, ignoring fudge factors such as tune.slewFudge
            (the fudged end time is obj.slewEndTime)
        @param[in] numIter  number of iterations for slew to converge
        """
        self.pathList = pathList
        self.begTime = begTime
        self.endTime = endTime
        self.numIter = numIter

_ValidStartingStateSet = set((
    tcc.base.AxisState_NotAvailable,
    tcc.base.AxisState_Halted,
    tcc.base.AxisState_Drifting,
))

class FullSlewData(object):
    """!Contains full data about one slew iteration

    Intended for debugging problems with slew iteration
    """
    def __init__(self, iterNum, obj, begTime, minDuration, estEndTime, pathList, endTime, duration):
        self.iterNum = iterNum
        self.obj = tcc.base.Obj(obj)
        self.begTime = begTime
        self.minDuration = minDuration
        self.estEndTime = estEndTime
        self.pathList = pathList[:]
        self.endTime = endTime
        self.duration = duration

    def __repr__(self):
        outStrList = (
            repr(self.obj),
            repr(self.obj.site),
            "begTime = %r" % (self.begTime,),
            "minDuration = %r" % (self.minDuration,),
            "estEndTime = %r" % (self.estEndTime,),
            "pathList = %r" % (self.pathList,),
            "endTime = %r" % (self.endTime,),
            "duration = %r" % (self.duration,),
        )
        return "FullSlewData for iter %s:\n%s" % (self.iterNum, "\n* ".join(outStrList))

def computeSlew(minEndTime, obj, earth, inst, telMod, axeLim, tune):
    """!Compute a slew to a given user-specified position, by iteration

    Before calling this:
    - Update obj at some time near "now" including applying doRestart and wrap preferences.
    - Send the DRIFT command to all axes that are moving (whether you want to halt them or slew them)
      and update obj.actMount and obj.axisCmdState accordingly.

    This routine then does the following for each axis:
    - If obj.axisCmdState==AxisState_Drifting and obj.targetMount[axis].isfinite():
      compute a slew to the new position. Raise an exception if such a slew cannot be computed.
    - If obj.axisCmdState==AxisState_Drifting and not obj.targetMount.isfinite():
      try to compute a slew to a halt. If that fails, set SlewData.pathList[axis]=();
      the caller then must command that axis to halt!
    - For other axes, leave the axis alone.

    @param[in,out] obj      object block:
        input: object block computed at approximately the current time (see above)
        output:
        - the object block is be recomputed
        - obj.targetMount is set to the final pvt for each axis that is slewing or halting;
            it is unchanged for axes that are already halted
        - obj.axisCmdState is set to AxisState_Halting or AxisState_Slewing for axes that are drifting
        - obj.axisErrCode is updated appropriately for axes that are drifting
        - obj.slewEndTime is set
    @param[in] minEndTime   earliest date at which slew may finish (TAI, MJD sec)
    @param[in] earth        earth orientation data, a tcc.base.Earth
    @param[in] inst         instrument-position information block, a tcc.base.Inst
    @param[in] telMod       telescope model, a tcc.base.TelMod
    @param[in] axeLim       axes limits block, a tcc.base.AxeLim
    @param[in] tune         axes motion tuning constants block, a tc.base.Tune

    @return slewData: a SlewData

    @throw RuntimeError if any of the following are true:
    - obj.axisCmdState is not one of AxisState_NotAvailable,  AxisState_Halted, or AxisState_Drifting for any axis
    - obj.actMount is not finite for any axis whose axisCmdState = AxisState_Drifting
    - a valid slew cannot be computed for an axis being slewed (not halted)

    @warning
    - If an axis is to be halted, and a valid slew cannot be computed,
        then slewData.pathList[axis] = (). The caller must stop the axis!

    Special cases:
    - If no axes are to be slewed, pathList[axis] will be an empty list for all axes, but begTime
      and endTime will be correct -- set as for the shortest possible slew.

    Details:
      The slew path is computed by iteration, as follows:
      - The end time of the slew is estimated (based on many factors, see code)
      - the target mount coordinates (p,v,t) are computed at that instant
      - A slew is computed which meets the target path (using linear extrapolation);
        this also gives us the true end time
      - The estimated and true end times are compared;
        if they are too far apart, the cycle is repeated

      There are several parameters in the Tune block which control this
      iteration process. See TuneBlkDef for more info.

      Types of motion:
      - Slew: move to a requested position via a computed, smooth path.
        Used for axes with a requested target position.
      - Controlled stop: move to the current pos. via a computed, smooth path.
        Used for axes with no position requested (e.g. coord. sys. = "none").

    History:
    2013-12-09 ROwen  converted from computeSlew.for
    """
    if any(cmdState not in _ValidStartingStateSet for cmdState in obj.axisCmdState):
        raise RuntimeError("obj.axisCmdState = %s; must be NotAvailable, Halted or Drifting for all axes" % (obj.axisCmdState,))

    oldMount = [coordConv.PVT(mt) for mt in obj.actMount]

    # doAxis: axis is to be slewed or halted
    doAxis = [cmdState == tcc.base.AxisState_Drifting for cmdState in obj.axisCmdState]
    # doSlewAxis: axis is to be slewed to a new position (not halted)
    doSlewAxis = [doAx and obj.axisErrCode[axis] == tcc.base.AxisErr_OK for axis, doAx in enumerate(doAxis)]
    if obj.userSys.getName() == "none":
        for i in range(2):
            doSlewAxis[i] = False
    if obj.rotType == tcc.base.RotType_None:
        doSlewAxis[2] = False
    # doHaltAxis: axis is to be halted, by slewing if possible, else abruptly
    doHaltAxis = [doAxis[axis] and not doSlewAxis[axis] for axis in range(tcc.base.NAxes)]

    badAxisList = [doAxis[axis] and not obj.actMount[axis].isfinite() for axis in range(tcc.base.NAxes)]
    if any(badAxisList):
        raise RuntimeError("Axis drifting but actMount unknown for axes: %s" % (getAxisNames(badAxisList),))

    # compute current mount position to use as the final position for axes wanting a controlled stop
    # (the result will be invalid for axes that aren't drifting, but so what?)
    currTAI = tcc.base.tai()
    stopPos = [actMount.getPos(currTAI) for actMount in obj.actMount]

    # Iterate the slew computation until endTime converges.
    iterNum = 0        # incremented at the beginning of the loop
    endTime = None
    estEndTime = None    # gurantees non-convergence at the first test
    duration = 0.0

    # a list of slew data -- in case slew iterations fail it can be printed
    fullSlewDataQueue = collections.deque(maxlen=5)
    while estEndTime is None or abs(endTime - estEndTime) > tune.slewConvTime:
        iterNum += 1
        if iterNum > tune.slewMaxIter:
            errMsg = "Slew computation did not converge in %s iterations" % (tune.slewMaxIter,)
            log.error(errMsg)
            for fullSlewData in fullSlewDataQueue:
                print fullSlewData
            raise RuntimeError(errMsg)

        # Compute the begin-time and estimated end-time for this iteration:
        # begin-time = current time + time needed to start sending the resulting slew to the axes
        # min. duration must be large enough that the slew will not end
        #   before "minEndTime" and we have time to send the slew commands
        # estimated end-time = begin-time + duration computed in prev. iter.
        #   (or substitute min. duration if larger)
        begTime = tcc.base.tai() + tune.slewAdvTime
        minDuration = max(tune.slewMinDuration, minEndTime - begTime)
        estEndTime = begTime + max(duration, minDuration)

        # Compute object block at estimated end-time of slew
        #
        # The object block has already been computed once with doRestart and user-specified
        # wrap applied, so for these iterations do not use user-specified wrap
        # (to avoid dithering near a wrap transition) and do further attempt to restart axes.
        computeObj(
            obj = obj,
            doWrap = False,
            doRestart = [False]*tcc.base.NAxes,
            reportLim = False,
            earth = earth,
            inst = inst,
            telMod = telMod,
            axeLim = axeLim,
            tai = estEndTime,
        )

        # set any axes which are to be halted in a controlled fashion to the proper stopped position
        for axis in range(tcc.base.NAxes):
            if doHaltAxis[axis]:
                obj.targetMount[axis] = coordConv.PVT(stopPos[axis], 0, estEndTime)

        # compute the slew (yielding the true end-time for this iteration), and compute the resulting duration
        pathList, endTime = oneSlewIter(
            doAxis = doAxis,
            oldMount = obj.actMount,
            newMount = obj.targetMount,
            begTime = begTime,
            minTime = minDuration,
            axeLim = axeLim,
        )
        if endTime <= 0.0:
            raise RuntimeError("Slew iteration failed: no axes were slewable")
        duration = endTime - begTime

        fullSlewDataQueue.append(FullSlewData(
            iterNum = iterNum,
            obj = obj,
            begTime = begTime,
            minDuration = minDuration,
            estEndTime = estEndTime,
            pathList = pathList,
            endTime = endTime,
            duration = duration,
        ))

    # update obj at actual end time, or if slew is really short, at first post-slew tracking time
    if duration <= ShortSlewTime:
        # Slew is so short I'm worried about getting the first tracking update out on time,
        # so add a PVT node for the first tracking update after the slew
        extraNodeTime = endTime + tune.trackInterval
        computeObj(
            obj = obj,
            doWrap = False,
            doRestart = [False]*tcc.base.NAxes,
            reportLim = False,
            earth = earth,
            inst = inst,
            telMod = telMod,
            axeLim = axeLim,
            tai = extraNodeTime,
        )
        for axis in range(tcc.base.NAxes):
            pvtList = pathList[axis]
            if len(pvtList) > 0 and obj.targetMount[axis].isfinite() and pvtList[-1].isfinite():
                pvtList.append(obj.targetMount[axis])
    else:
        computeObj(
            obj = obj,
            doWrap = False,
            doRestart = [False]*tcc.base.NAxes,
            reportLim = False,
            earth = earth,
            inst = inst,
            telMod = telMod,
            axeLim = axeLim,
            tai = endTime,
        )
    obj.slewEndTime = endTime + tune.slewFudge

    # test the position limits of the resulting slew
    # (but not velocity, acceleration or jerk, because they are constrained as part of generating the slew)
    # handle the results in the final sanity check, since bad limits are acceptable if halting the axis
    limErrCodeList = [tcc.base.AxisErr_OK]*tcc.base.NAxes
    checkSlewPath(limErrCodeList, doAxis, pathList, doPos=True, doVel=False, doAccel=False, doJerk=False, axeLim=axeLim, inst=inst)
    badLimitList = [limErrCode != tcc.base.AxisErr_OK for limErrCode in limErrCodeList]

    # set obj.axisCmdState and make sure obj.axisErrCode not OK if halting
    # and perform a final sanity-check
    badAxisList = [False]*tcc.base.NAxes
    for axis in range(tcc.base.NAxes):
        if doSlewAxis[axis]:
            obj.axisCmdState[axis] = tcc.base.AxisState_Slewing
            if badLimitList[axis] or obj.axisErrCode[axis] != tcc.base.AxisErr_OK:
                # this axis didn' work out
                badAxisList[axis] = True
        elif doHaltAxis[axis]:
            obj.axisCmdState[axis] = tcc.base.AxisState_Halting
            if obj.axisErrCode[axis] == tcc.base.AxisErr_OK:
                logMsg = "doHalt Error axis %i\ncomputeSlew(minEndTime=%s, obj, earth, inst, telMod, axeLim, tune)\nDumping desObj block:\n%s\nDumping inst block:\n%s\nDumping tune block:\n%s\nDumping axeLim block:\n%s"%(axis, str(minEndTime), str(obj), str(inst), str(tune), str(axeLim))
                log.error(logMsg)
                obj.axisErrCode[axis] = tcc.base.AxisErr_CannotCompute
            if badLimitList[axis]:
                # cannot slew this axis gently to a halt, so command an immediate stop
                pathList[axis] = ()
        else:
            # restore original mount position and make sure error is set
            obj.actMount[axis] = oldMount[axis]
            if obj.axisErrCode[axis] == tcc.base.AxisErr_OK:
                logMsg = "noHalt Error axis %i\ncomputeSlew(minEndTime=%s, obj, earth, inst, telMod, axeLim, tune)\nDumping desObj block:\n%s\nDumping inst block:\n%s\nDumping tune block:\n%s\nDumping axeLim block:\n%s"%(axis, str(minEndTime), str(obj), str(inst), str(tune), str(axeLim))
                log.error(logMsg)
                obj.axisErrCode[axis] = tcc.base.AxisErr_CannotCompute

    if any(badAxisList):
        raise RuntimeError("Could not compute a slew for axes: %s" % (getAxisNames(badAxisList),))

    return SlewData(
        pathList = pathList,
        begTime = begTime,
        endTime = endTime,
        numIter = iterNum,
    )