basicMountFromObs.cc

Go to the documentation of this file.

#include <cmath>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <stdexcept>
#include <boost/tr1/array.hpp> // so array works with old and new compilers
#include "slalib.h"
#include "tcs.h"
#include "tcssys.h" // for MAXAUX,though it sneaks in via earth.h
#include "coordConv/coordConv.h"
#include "tcc/basics.h"
#include "tcc/telConst.h"
#include "tcc/tcsShim.h"
#include "tcc/tcsAstrom.h"
#include "tcc/tcsPOrig.h"
#include "tcc/tcsSite.h"
#include "tcc/tcsTScope.h"
#include "tcc/basicMountFromObs.h"

const float DELTA_ANGLE_DEG = 1.0e-7 / coordConv::RadPerDeg;
const double MAX_MOUNT_ERR_RAD = 5e-8; // maximum mount error (rad) when iterating computeMount

namespace {

    double computeObjAzInstAng(
        double objInstX,
        double objInstY,
        tcc::Inst const &inst,
        TSCOPE &tscope,
        ASTROM &ast,
        double ga,
        double gb,
        double roll,
        double pitch,
        double rot
    ) {
        tcc::TcsPOrig tcsPOrig0(objInstX - DELTA_ANGLE_DEG, objInstY, inst);
        tcc::TcsPOrig tcsPOrig1(objInstX + DELTA_ANGLE_DEG, objInstY, inst);

        // Scale the pointing-origin into radians.
        double porX0, porY0, porX1, porY1;        
        tcsQpor(&tcsPOrig0.porig, &tscope, &porX0, &porY0);
        tcsQpor(&tcsPOrig1.porig, &tscope, &porX1, &porY1);

        // Transform both points into the coordinate system specified in ast
        double astEquat0, astPolar0, astEquat1, astPolar1;
        tcsVTsky(roll, pitch, &ast, rot, porX0, porY0, &tscope, ga, gb, &astEquat0, &astPolar0);

        tcsVTsky(roll, pitch, &ast, rot, porX1, porY1, &tscope, ga, gb, &astEquat1, &astPolar1);
        
        return -90 - (slaDbear(astEquat0, astPolar0, astEquat1, astPolar1) / coordConv::RadPerDeg);
    }

    /***
    Call TCSpk routines to compute mount position
    
    @param[out] roll  roll angle (azimuth) (radians)
    @param[out] pitch  pitch angle (elevation) (radians)
    @param[out] rot  rotator angle (radians)
    @param[in] tcsTScope  telescope info
    @param[in] tcsAstrom  astrometry info at desired date
    @param[in] targ  target info at desired date
    @param[in] porig  pointing origin at desired date
    @param[in] fldor  field orientation at desired date
    @param[in] ga  guiding correction in azimuth (radians)
    @param[in] gb  guiding correction in elevation (radians)
    @param[in] predRoll  predicted roll (radians)
    @param[in] predPitch  predicted pitch (radians)
    @param[in] predRot  predicted rotator angle (radians)
    */
    void computeMount(
        double &roll, double &pitch, double &rot,
        tcc::TcsTScope &tcsTScope,
        tcc::TcsAstrom &tcsAstrom,
        TARG &targ,
        PORIG &porig,
        FLDOR &fldor,
        double ga, double gb,
        double predRoll, double predPitch, double predRot
    ) {
        double duma, dumb;  // roll and pitch for a case that doesn't apply
        double aimx, aimy, aimz;    // aim vector
        double pox = porig.p[0] / tcsTScope.tel.fl; // pointing origin x in units of focal length
        double poy = porig.p[1] / tcsTScope.tel.fl; // pointing origin y in units of focal length
        int stat = tcsTrack(
            targ.op0[0], targ.op0[1],
            &tcsAstrom.m_ast,
            predRot, predRoll, predPitch,
            pox, poy,
            &tcsTScope.tel,
            ga, gb,
            &aimx, &aimy, &aimz,
            &roll, &pitch, &duma, &dumb);
        if (stat != 0) {
            if (stat == 1) {
                throw std::runtime_error("Too near pole (should not happen)");
            } else {
                std::cerr << "*** tcsTrack error: could not compute roll and pitch; here are the inputs and outputs:" << std::endl;
                std::cerr << "targ.op0=" << targ.op0[0] << ", " << targ.op0[1] << std::endl;
                std::cerr << "tcsAstrom (only .m_ast is used)=" << tcsAstrom << std::endl;
                std::cerr << "predRot=" << predRot << std::endl;
                std::cerr << "predRoll=" << predPitch << std::endl;
                std::cerr << "pox=" << pox << std::endl;
                std::cerr << "poy=" << poy << std::endl;
                std::cerr << "tcsTScope (only .tel is used)=" << tcsTScope << std::endl;
                std::cerr << "ga=" << ga << std::endl;
                std::cerr << "gb=" << gb << std::endl;
                throw std::runtime_error("Could not compute roll and pitch");
            }
        }
    
        stat = tcsRotator(
            aimx, aimy, aimz,
            predRot, roll, pitch,
            tcc::tcspk::JBP,
            pox, poy,
            &tcsTScope.tel,
            ga, gb,
            &fldor,
            &tcsAstrom.r_ast,
            &rot);
        if (stat != 0) {
            throw std::runtime_error("Could not compute rotator angle");
        }
    }

}

namespace tcc {

    void basicMountFromObs(
        Obj &obj,
        Inst const &inst,
        TelMod const &telMod,
        double tai
    ) {
        FOPT JF = FIELDO;   // field optimization: SLITO = 1D (slit), FIELDO or else = 2D (field)
                            // the value doesn't matter because coordConv handles refraction

        // create TCSpk structs and fill them
        TcsSite tcsSite;
        tcsSite.update(tai, obj.site);

        // make non-const references to const inputs so I can call non-const-correct TCSpk code
        TPMOD &tmod = const_cast<TPMOD&>(telMod._model);

        TcsTScope tcsTScope(tcsSite, inst.instPos);

        TcsAstrom tcsAstrom(obj.site.wavelen);

        coordConv::PVT netGuideOff[3];
        for (int i = 0; i < 3; ++i) {
            netGuideOff[i] = obj.guideOff.at(i) + obj.calibOff.at(i);
        }

        bool computeOrientFromRotPhys = false;
        if (!inst.hasRotator()) {
            // no instrument rotator; rotPhys = inst.rot_fixed_phys
            obj.rotPhys = coordConv::PVT(inst.rot_fixed_phys, 0.0, tai);
            obj.targetMount.at(2).invalidate(tai);
            computeOrientFromRotPhys = true;
        } else if (obj.rotType == RotType_Mount) {
            // mount rotation; set rot mount to rot user and compute rot phys
            coordConv::PVT const rotMount = obj.rotUser.copy(tai);
            obj.targetMount.at(2) = rotMount;
            obj.rotPhys = wrapCtr((rotMount - inst.rot_offset) / inst.rot_scale);
            computeOrientFromRotPhys = true;
        } else if (obj.rotType == RotType_None) {
            // user specifies no rotation; do the best you can
            // (can't set rotPhys to NaN because TCSpk needs something)
            if (obj.actMount[2].isfinite()) {
                obj.rotPhys = (obj.actMount[2] - inst.rot_offset) / inst.rot_scale;
            } else {
                obj.rotPhys = coordConv::PVT(inst.rot_fixed_phys, 0.0, tai);
            }
            obj.targetMount.at(2).invalidate(tai);
            computeOrientFromRotPhys = true;
        }

        // Compute mount position at time tai and tai + DeltaTForVel (a small delta-time)
        FLDOR fldor;    // TCSpk field origin
        TARG targ;      // TCSpk target
        double stat;    // return code from TCSpk routines
        double dumv;    // dummy velocity argument
        double rollArr[2], pitchArr[2], rotPhysArr[2], objAzInstAngArr[2];
        for (int tInd = 0; tInd < 2; ++tInd) {
            double tempTAI = tai + (static_cast<double>(tInd) * DeltaTForVel);
            double tempTAIDays = tempTAI / coordConv::SecPerDay;

            tcsAstrom.initTarget(targ, obj.obsPos, tempTAI);

            /*
            IPA is the position angle: angle of instrument with respect to observed coords
            (because Az/Alt is the coordinate system used as input to TCSpk):
            0 if instrument y axis = alt axis; pi/2 if instrument y axis = az axis
            i.e. 0 if az axis = instrument x axis, pi/2 if az axis = instrument y axis
            The corresponding TCC angle is obj.objAzInstAng: 0 if az axis = instrument x axis; 90 if az axis = inst Y
            IPA = obj.rotUser * RadPerDeg
            
            IAA = angle of instrument Y axis in rotator coordinate frame:
            0 if instrument Y is along rotator Y; pi/2 if instrument Y is along rotator X.
            The corresponding TCC angle is -inst.rot_inst_ang: 0 if rot X = inst X; 90 if rot X = inst Y
            */
            double iaa = inst.rot_inst_ang * coordConv::RadPerDeg;
            double ipa;
            tcspk::tcsFromPVT(ipa, dumv, obj.objAzInstAng, tempTAI);

            stat = tcsIfld(
                iaa,        /* IAA */
                ipa,        /* IPA */
                JF,         /* field-optimized */
                &fldor);
            if (stat != 0) {
                std::ostringstream os;
                os << "tcsIfld error: " << stat;
                throw std::runtime_error(os.str());
            }
          
            /*
            Compute porXY: pointing origin in the rotator frame (rad)
            from objInstXY: pointing origin in the instrument frame (deg)
            rot_inst_xy is the position of the center of the rotator frame in the instrument frame (deg)
            rot_inst_ang is the angle of the rotator frame with respect to the instrument frame (deg)
            */

            TcsPOrig tcsPOrig(
                obj.objInstXY.at(0).getPos(tempTAI),
                obj.objInstXY.at(1).getPos(tempTAI),
                inst);

            tcspk::tcsFromPVT(ipa, dumv, obj.rotUser, tempTAI);    // instrument position-angle
        
            // ga and gb (TCKSpk guide correction roll and pitch) have the opposite sign of netGuideOff
            double ga, gb;
            tcspk::tcsFromPVT(ga, dumv, -netGuideOff[0], tempTAI);
            tcspk::tcsFromPVT(gb, dumv, -netGuideOff[1], tempTAI);

            double predRot = 0;
            if ((tInd == 0) || computeOrientFromRotPhys) {
                // initial estimate of rot is based on rotPhys,
                // or if computeOrientFromRotPhys then rot is precisely computed from rotPhys
                double predRotPhys = inst.rot_fixed_phys;
                if (obj.rotPhys.isfinite()) {
                    predRotPhys = obj.rotPhys.getPos(tempTAI);
                } else if (obj.actMount[2].isfinite()) {
                    predRotPhys = (obj.actMount[2].getPos(tempTAI) - inst.rot_offset) / inst.rot_scale;
                }
                predRot = tcspk::rotTcsFromPhys(predRotPhys, inst.instPos);
            }

            double predPitch, predRoll;
            if (tInd == 0) {
                // initial estimated roll, pitch are the last commanded values
                // but next time use the previous computed values
                // TO DO: base on actual mount if cmd mount unknown; meanwhile use 0
                tcspk::tcsFromPVT(predRoll, dumv, obj.targetMount.at(0), tempTAI);
                tcspk::tcsFromPVT(predPitch, dumv, obj.targetMount.at(1), tempTAI);

                // call tcsMedium just once (but after targ is set)
                // tel, m_ast and r_ast are updated; everything else is an input
            
                double *auxPtr = const_cast<double *>(tcsSite.aux.data());

                stat = tcsMedium(tempTAIDays, predRoll, predPitch, tcspk::JBP, auxPtr,
                    &tcsSite.tsite, &tmod, &targ, &tcsTScope.tel, &tcsAstrom.m_ast, &tcsAstrom.r_ast);
                if (stat != 0) {
                    std::ostringstream os;
                    os << "tcsMedium error: " << stat;
                    throw std::runtime_error(os.str());
                }
            }
            
            // we probably only need to iterate for the first of the two nearby times,
            // but the code is simpler and safer this way
            
            int const MaxIter = 10;
            double roll, pitch, rot;
            bool didConverge = false;
            for (int iter=0; iter < MaxIter; ++iter) {
                // iterate to get a good estimate of roll, pitch and (especially) rot
                computeMount(roll, pitch, rot,
                    tcsTScope, tcsAstrom, targ, tcsPOrig.porig, fldor, ga, gb, predRoll, predPitch, predRot);
                
                // std::cout << "computeMount iter=" << iter
                //     << "; rotErrAS=" << slaDrange(rot - predRot) * 3600.0 / coordConv::RadPerDeg
                //     << "; rollErrAS=" << slaDrange(roll - predRoll) * 3600.0 / coordConv::RadPerDeg
                //     << "; pitchErrAS=" << slaDrange(pitch - predPitch) * 3600.0 / coordConv::RadPerDeg
                //     << "; maxErrAS=" << MAX_MOUNT_ERR_RAD * 3600.0 / coordConv::RadPerDeg
                //     << std::endl;
                if (computeOrientFromRotPhys) {
                    // rotator angle is known
                    rot = predRot;
                }

                if (std::fabs(slaDrange(rot - predRot)) < MAX_MOUNT_ERR_RAD
                    && std::fabs(slaDrange(roll - predRoll)) < MAX_MOUNT_ERR_RAD
                    && std::fabs(slaDrange(pitch - predPitch)) < MAX_MOUNT_ERR_RAD) {
                    didConverge = true;
                    // std::cout << "computeMount iteration converged in " << iter + 1 << " iterations" << std::endl;
                    break;
                }
                predRoll = roll;
                predPitch = pitch;
                predRot = rot;
            }
            if (!didConverge) {
                std::ostringstream os;
                os << "computeMount iteration did not converge in " << MaxIter << " iterations"
                    << "; predRot=" << predRot << "; rot=" << rot
                    << "; predRoll=" << predRoll << "; roll=" << roll
                    << "; predPitch=" << predPitch << "; pitch=" << pitch;
                throw std::runtime_error(os.str());
            }
            rollArr[tInd] = roll;
            pitchArr[tInd] = pitch;

            // std::cout << std::setprecision(12)
            //     << "after computeMount: ind=" << tInd
            //     << "; taiDays=" << tempTAIDays
            //     << "; ga=" << ga
            //     << "; gb=" << gb
            //     << "; roll=" << rollArr[tInd]
            //     << "; pitch=" << pitchArr[tInd]
            //     << "; rota=" << rot
            //     << std::endl;
            // std::cout << tcsSite.tsite;
            // std::cout << tcsTScope.tel;
            // std::cout << targ;
            // std::cout << tcsPOrig;
            // std::cout << fldor;
            // std::cout << "m_ast=" << tcsAstrom.m_ast;
            // std::cout << "r_ast=" << tcsAstrom.r_ast;
            
            if (computeOrientFromRotPhys) {
                // The rotation angle computed above cannot be used because the rotator is not tracking
                // the sky or horizon. Compute orientation angles from obj.rotPhys, instead.
                rotPhysArr[tInd] = obj.rotPhys.getPos(tempTAI);
                objAzInstAngArr[tInd] = computeObjAzInstAng(
                    obj.objInstXY.at(0).getPos(tempTAI),
                    obj.objInstXY.at(1).getPos(tempTAI),
                    inst,
                    tcsTScope.tel,
                    tcsAstrom.m_ast,
                    ga, gb,
                    rollArr[tInd], pitchArr[tInd], predRot);
            } else {
                // TCSpk has computed the rotator angle we need
                rotPhysArr[tInd] = tcspk::rotPhysFromTcs(rot, inst.instPos);
            }
        }

    //             for (int tInd = 0; tInd < 2; ++tInd) {
    //                 std::cout << "at end of mountFromObs: ind=" << tInd << "; roll=" << rollArr[tInd] << "; pitch="
    //                     << pitchArr[tInd] << "; tcsRotPhys=" << rotPhysArr[tInd] << std::endl;
    //             }

        // update object block, without wrap; note that roll (mount azimuth)
        // already uses the TCC's convention of 0 south, pi/2 east, so do NOT set the isAz flag true
        tcspk::pvtFromTCS(obj.targetMount.at(0), rollArr, tai, DeltaTForVel);
        tcspk::pvtFromTCS(obj.targetMount.at(1), pitchArr, tai, DeltaTForVel);

        if (computeOrientFromRotPhys) {
            // use objAzInstAngArr to compute objAzInstAng and objUserInstAng
            // note that obj.rotPhys and obj.targetMount[2] are already set
            obj.objAzInstAng.setFromPair(objAzInstAngArr, tai, DeltaTForVel, true);
            obj.objUserInstAng = coordConv::wrapCtr(obj.objUserObjAzAng + obj.objAzInstAng);

            // diagnostic print for a mysterious problem
            if (std::abs(obj.objUserInstAng.vel) > 1.0) {
                std::cerr << "basicMountFromObs warning: obj.objUserInstAng=" << obj.objUserInstAng
                    << "; obj.objUserObjAzAng=" << obj.objUserObjAzAng
                    << "; obj.objAzInstAng=" << obj.objAzInstAng
                    << "; objAzInstAngArr=" << objAzInstAngArr[0] << ", " << objAzInstAngArr[1]
                    << std::endl;
            }
        } else if (obj.rotType == RotType_None) {
            // no rotation desired; unknown orientation
            obj.rotPhys.invalidate(tai);
            obj.targetMount.at(2).invalidate(tai);
            obj.objAzInstAng.invalidate(tai);
            obj.objUserInstAng.invalidate(tai);
        } else if (obj.rotType == RotType_Mount) {
            throw std::runtime_error("Bug: computeOrientFromRotPhys false for RotType_Mount");
        } else {
            // rotate with sky or horizon; rot phys = rotator angle computed by TCSpk
            obj.rotPhys.setFromPair(rotPhysArr, tai, DeltaTForVel, true);
            // include guide correction when computing rotator mount (az and alt guide correction is handled as ga, gb above)
            obj.targetMount.at(2) = (obj.rotPhys * inst.rot_scale) + inst.rot_offset + netGuideOff[2].getPos(tai);
        }

        /*
        Compute additional orientation angles: rotAzObjAzAng, rotAzRotAng and spiderInstAng
        
        spiderInstAng is defined as the angle from instrument x to increasing azimuth at the center of the rotator
        spiderInstAng = rotAzInstAng-at-center-of-rot
        
        by offsetting observed coordinates from object (at boresight) to center of rotator
        along a vector rotObjInstXY in the focal plane, we get:
        distance = length of rotObjInstXY vector
        fromDir is the direction of offset in az/alt frame at object
        fromDir = rotObjInstDir - objAzInstAng
        toDir is the direction of the offset in the az/alt frame at the center of the rotator
        toDir   = rotObjInstDir - rotAzInstAng-at-center-of-rot
                = rotObjInstDir - spiderInstAng
        spiderInst = rotObjInstDir - toDir
        
        rotAzRotAng = rotAzInstAng-at-center-of-rot - rotInstAng
                    = spiderInstAng - rotInstAng
                     = (rotObjInstDir - toDir)  - rotInstAng
                     = (rotObjInstDir - rotInstAng) - toDir
        
        rotAzObjAzAng = rotAzInstAng-at-center-of-rot - objAzInstAng
                      = spiderInstAng - objAzInstAng
                      = (rotObjInstDir - toDir) - objAzInstAng
                      = (rotObjInstDir - objAzInstAng) - toDir
                      = fromDir - toDir

        note: the offset also computes the az/alt at the center of the instrument, but we don't need it so don't save it
        */
        std::tr1::array<coordConv::PVT, 2> objRotInstXY; 
        for (int i = 0; i < 2; ++i) {
            objRotInstXY[i] = obj.objInstXY[i] - inst.rot_inst_xy[i];
        }
        coordConv::PVT rotObjInstDir, rotObjDist;  // direction is from rotator center to object, in instrument frame
        coordConv::polarFromXY(rotObjDist, rotObjInstDir, -objRotInstXY.at(0), -objRotInstXY.at(1), tai);
        coordConv::PVT fromDir = rotObjInstDir - obj.objAzInstAng;
        coordConv::PVT toDir;
        coordConv::PVTCoord obsInstPos = obj.obsPos.offset(toDir, fromDir, rotObjDist);
        obj.rotAzObjAzAng = coordConv::wrapCtr(fromDir - toDir);
        obj.spiderInstAng = coordConv::wrapCtr(rotObjInstDir - toDir);
        obj.rotAzRotAng = coordConv::wrapCtr(obj.spiderInstAng - inst.rot_inst_ang);
    }

}