Changeset 14595 for trunk/FACT++


Ignore:
Timestamp:
11/09/12 19:56:10 (12 years ago)
Author:
tbretz
Message:
Added a command to track OnSource; added absolut deviation to service; added new stat OnTrack and its infrastructure.
File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/FACT++/src/drivectrl.cc

    r14510 r14595  
    6969    }
    7070
    71     virtual void UpdateTracking(const Time &, const array<double, 7> &)
     71    virtual void UpdateTracking(const Time &, const array<double, 8> &)
    7272    {
    7373    }
    … …  
    118118        return sgn=='-' ? -ret : ret;
    119119    }
     120
     121    double GetDevAbs(double nomzd, double meszd, double devaz)
     122    {
     123        nomzd *= M_PI/180;
     124        meszd *= M_PI/180;
     125        devaz *= M_PI/180;
     126
     127        const double x = sin(meszd) * sin(nomzd) * cos(dvaz);
     128        const double y = cos(meszd) * cos(nomzd);
     129
     130        return acos(x + y) * 180/M_PI;
     131    }
     132
     133    uint16_t fDeviationLimit;
     134    uint16_t fDeviationCounter;
     135    uint16_t fDeviationMax;
     136
     137    uint64_t fTrackingCounter;
    120138
    121139protected:
    … …  
    322340            stream >> mjd;
    323341
    324             const double zd1 = ReadAngle(stream);
    325             const double az1 = ReadAngle(stream);
    326             const double zd2 = ReadAngle(stream);
    327             const double az2 = ReadAngle(stream);
     342            const double zd1 = ReadAngle(stream);  // Nominal (zd/az asynchronous, dev  synchronous, mjd  synchronous with zd)
     343            const double az1 = ReadAngle(stream);  // Nominal (zd/az asynchronous, dev  synchronous, mjd  synchronous with z)
     344            const double zd2 = ReadAngle(stream);  // Masured (zd/az  synchronous, dev asynchronous, mjd asynchronous)
     345            const double az2 = ReadAngle(stream);  // Measurd (zd/az  synchronous, dev asynchronous, mjd asynchronous)
    328346
    329347            double zd_err, az_err;
    330             stream >> zd_err;
    331             stream >> az_err;
     348            stream >> zd_err;                      // Deviation = Nominal - Measured
     349            stream >> az_err;                      // Deviation = Nominal - Measured
    332350
    333351            uint16_t armed, stgmd;
    … …  
    363381            // kMoving,
    364382            // kTracking,
     383            // kOnTrack,
    365384
    366385            // pdo3:
    … …  
    381400            // charging:  4ef04ef
    382401
     402            // Convert to deg
     403            zd_err /= 3600;
     404            az_err /= 3600;
     405
     406            // Calculate absolut deviation on the sky
     407            const double dev = GetDevAbs(zd1, zd1-zd_err, az_err)*3600;
     408
     409Deviation = Nominal - Measured
     410
     411            // If any other state than tracking or a deviation
     412            // larger than 60, reset the counter
     413            if (fState!=State::kTracking || dev>fDeviationLimit)
     414                fTrackingCounter = 0;
     415            else
     416                fTrackingCounter++;
     417
     418            // If in tracking, at least five consecutive reports (5s)
     419            // must be below 60arcsec deviation, this is considered OnTrack
     420            if (fState==State::kTracking && fTrackingCounter>=fDeviationCounter)
     421                fState = State::kOnTrack;
     422
     423            // Having th state as Tracking will reset the counter
     424            if (fState==State::kOnTrack && dev>fDeviationMax)
     425                fState = State::kTracking;
     426
    383427            const array<uint8_t, 3> state = {{ uint8_t(pdo3>>16), uint8_t(pdo3), uint8_t(pdo3>>24) }};
    384428            UpdateStatus(t1, state);
    … …  
    387431            UpdatePointing(t1, point);
    388432
    389             const array<double, 7> track =
     433            const array<double, 8> track =
    390434            {{
    391435                ra, dec, ha,
    392436                zd1, az1,
    393                 zd_err/3600, az_err/3600
     437                zd_err, az_err,
     438                dev
    394439            }};
    395440            if (mjd>0)
    … …  
    517562public:
    518563    ConnectionDrive(ba::io_service& ioservice, MessageImp &imp) : Connection(ioservice, imp()),
    519         fState(-1), fIsVerbose(true), fKeepAlive(ioservice)
     564        fState(-1), fIsVerbose(true), fDeviationLimit(120), fDeviationCounter(5), fDeviationMax(240), fTrackingCounter(0), fKeepAlive(ioservice)
    520565    {
    521566        SetLogStream(&imp);
    … …  
    527572    }
    528573
     574    void SetDeviationCondition(uint16_t limit, uint16_t counter, uint16_t max)
     575    {
     576        fDeviationLimit   = limit;
     577        fDeviationCounter = counter;
     578        fDeviationMax     = max;
     579    }
    529580    int GetState() const
    530581    {
    … …  
    558609    }
    559610
    560     void UpdateTracking(const Time &t,const array<double, 7> &arr)
     611    void UpdateTracking(const Time &t,const array<double, 8> &arr)
    561612    {
    562613        fDimTracking.setData(arr);
    … …  
    587638                     "|Zd[deg]:Zenith distance (encoder readout)"
    588639                     "|Az[deg]:Azimuth angle (encoder readout)"),
    589         fDimTracking("DRIVE_CONTROL/TRACKING_POSITION", "D:1;D:1;D:1;D:1;D:1;D:1;D:1",
     640        fDimTracking("DRIVE_CONTROL/TRACKING_POSITION", "D:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1",
    590641                     "|Ra[h]:Command right ascension"
    591642                     "|Dec[deg]:Command declination"
    … …  
    594645                     "|Az[deg]:Nominal azimuth angle"
    595646                     "|dZd[deg]:Control deviation Zd"
    596                      "|dAz[deg]:Control deviation Az"),
     647                     "|dAz[deg]:Control deviation Az"
     648                     "|dev[arcsec]:Absolute control deviation"),
    597649        fDimSource("DRIVE_CONTROL/SOURCE_POSITION", "D:1;D:1;D:1;D:1;D:1;D:1;C:31",
    598650                     "|Ra_src[h]:Source right ascension"
    … …  
    895947    }
    896948
     949    int StartTrackWobble(const char *ptr, size_t size, const double &offset=0, const double &angle=0)
     950    {
     951        const char *last = ptr+size;
     952
     953        try
     954        {
     955            const sources::const_iterator it = GetSourceFromDB(ptr, last);
     956
     957            const string &name = it->first;
     958            const Source &src  = it->second;
     959
     960            return StartWobble(src.ra, src.dec, offset, angle, name);
     961        }
     962        catch (const uint32_t &e)
     963        {
     964            return e;
     965        }
     966
     967    }
     968
    897969    int Track(const EventImp &evt)
    898970    {
    … …  
    907979        const double *dat  = evt.Ptr<double>();
    908980        const char   *ptr  = evt.Ptr<char>(16);
    909         const char   *last = ptr+evt.GetSize()-16;
    910 
    911         try
    912         {
    913             const sources::const_iterator it = GetSourceFromDB(ptr, last);
    914 
    915             const string &name = it->first;
    916             const Source &src  = it->second;
    917 
    918             return StartWobble(src.ra, src.dec, dat[0], dat[1], name);
    919         }
    920         catch (const uint32_t &e)
    921         {
    922             return e;
    923         }
     981        const size_t  size = evt.GetSize()-16;
     982
     983        return StartTrackWobble(ptr, size, dat[0], dat[1]);
     984    }
     985
     986    int TrackOn(const EventImp &evt)
     987    {
     988        if (evt.GetSize()==0)
     989        {
     990            ostringstream msg;
     991            msg << "TrackOn - Received event has " << evt.GetSize() << " bytes, but expected at least 1.";
     992            T::Fatal(msg);
     993            return T::kSM_FatalError;
     994        }
     995
     996        return StartTrackWobble(evt.Ptr<char>(), evt.GetSize());
     997    }
     998
     999
     1000    int TakeTPoint(const EventImp &evt)
     1001    {
     1002        if (evt.GetSize()<=4)
     1003        {
     1004            ostringstream msg;
     1005            msg << "TakePoint - Received event has " << evt.GetSize() << " bytes, but expected at least 5.";
     1006            T::Fatal(msg);
     1007            return T::kSM_FatalError;
     1008        }
     1009
     1010        const float  mag  = evt.Get<float>();
     1011        const char  *ptr  = evt.Ptr<char>(4);
     1012        const size_t size = evt.GetSize()-4;
     1013
     1014        string src(ptr, size);
     1015
     1016        while (src.find_first_of(' '))
     1017            src.erase(src.find_first_of(' '), 1);
     1018
     1019        SendCommand("TPOIN "+src+" "+to_string(mag), false);;
     1020
     1021        return T::GetCurrentState();
    9241022    }
    9251023
    … …  
    11201218
    11211219        T::AddStateName(State::kConnected, "Connected",
    1122                      "Cosy connected, drive stopped");
     1220                        "Cosy connected, drive stopped");
    11231221
    11241222        T::AddStateName(State::kNotReady, "NotReady",
    1125                      "Drive system not ready for movement");
     1223                        "Drive system not ready for movement");
    11261224
    11271225        T::AddStateName(State::kLocked, "Locked",
    1128                      "Drive system is locked (will not accept commands)");
     1226                        "Drive system is locked (will not accept commands)");
    11291227
    11301228        T::AddStateName(State::kReady, "Ready",
    1131                      "Drive system ready for movement");
     1229                        "Drive system ready for movement");
    11321230
    11331231        T::AddStateName(State::kArmed, "Armed",
    1134                      "Cosy armed, drive stopped");
     1232                        "Cosy armed, drive stopped");
    11351233
    11361234        T::AddStateName(State::kMoving, "Moving",
    1137                      "Telescope moving");
     1235                        "Telescope moving");
    11381236
    11391237        T::AddStateName(State::kTracking, "Tracking",
    1140                      "Telescope tracking");
     1238                        "Telescope is in tracking mode");
     1239
     1240        T::AddStateName(State::kOnTrack, "OnTrack",
     1241                        "Telescope tracking stable");
    11411242
    11421243        // State::kIdle
    … …  
    11711272             "|Az[deg]:Azimuth");
    11721273
    1173         T::AddEvent("TRACK", "D:2", State::kArmed, State::kTracking)   // ->RADEC/GRB
     1274        T::AddEvent("TRACK", "D:2", State::kArmed, State::kTracking, State::kOnTrack)   // ->RADEC/GRB
    11741275            (bind(&StateMachineDrive::SendCoordinates, this, placeholders::_1, kTrackSlow))
    11751276            ("Move the telescope to the given sky coordinates and start tracking them"
    … …  
    11771278             "|Dec[deg]:Declination");
    11781279
    1179         T::AddEvent("WOBBLE", "D:4", State::kArmed, State::kTracking)   // ->RADEC/GRB
     1280        T::AddEvent("WOBBLE", "D:4", State::kArmed, State::kTracking, State::kOnTrack)   // ->RADEC/GRB
    11801281            (bind(&StateMachineDrive::Wobble, this, placeholders::_1))
    11811282            ("Move the telescope to the given wobble position around the given sky coordinates and start tracking them"
    … …  
    11851286             "|Angle[deg]:Wobble angle");
    11861287
    1187         T::AddEvent("TRACK_SOURCE", "D:2;C", State::kArmed, State::kTracking)   // ->RADEC/GRB
     1288        T::AddEvent("TRACK_SOURCE", "D:2;C", State::kArmed, State::kTracking, State::kOnTrack)   // ->RADEC/GRB
    11881289            (bind(&StateMachineDrive::Track, this, placeholders::_1))
    11891290            ("Move the telescope to the given wobble position around the given source and start tracking"
    … …  
    11921293             "|Name[string]:Source name");
    11931294
    1194         T::AddEvent("TRACK_WOBBLE", "S:1;C", State::kArmed, State::kTracking)   // ->RADEC/GRB
     1295        T::AddEvent("TRACK_WOBBLE", "S:1;C", State::kArmed, State::kTracking, State::kOnTrack)   // ->RADEC/GRB
    11951296            (bind(&StateMachineDrive::TrackWobble, this, placeholders::_1))
    11961297            ("Move the telescope to the given wobble position around the given source and start tracking"
    … …  
    11981299             "|Name[string]:Source name");
    11991300
     1301        T::AddEvent("TRACK_ON", "S:1;C", State::kArmed, State::kTracking, State::kOnTrack)   // ->RADEC/GRB
     1302            (bind(&StateMachineDrive::TrackOn, this, placeholders::_1))
     1303            ("Move the telescope to the given position and start tracking"
     1304             "|Name[string]:Source name");
     1305
    12001306        T::AddEvent("RESUME", StateMachineImp::kSM_Error)
    12011307            (bind(&StateMachineDrive::Resume, this))
    12021308            ("If drive is in Error state, this can b used to resume the last tracking command, if the last command sent to cosy was a tracking command.");
    12031309
    1204         T::AddEvent("MOON", State::kArmed, State::kTracking)
     1310        T::AddEvent("MOON", State::kArmed, State::kTracking, State::kOnTrack)
    12051311            (bind(&StateMachineDrive::SendCommand, this, "MOON 0 0", true))
    12061312            ("Start tracking the moon");
    1207         T::AddEvent("VENUS", State::kArmed, State::kTracking)
     1313        T::AddEvent("VENUS", State::kArmed, State::kTracking, State::kOnTrack)
    12081314            (bind(&StateMachineDrive::SendCommand, this, "CELEST 2 0 0", true))
    12091315            ("Start tracking Venus");
    1210         T::AddEvent("MARS", State::kArmed, State::kTracking)
     1316        T::AddEvent("MARS", State::kArmed, State::kTracking, State::kOnTrack)
    12111317            (bind(&StateMachineDrive::SendCommand, this, "CELEST 4 0 0", true))
    12121318            ("Start tracking Mars");
    1213         T::AddEvent("JUPITER", State::kArmed, State::kTracking)
     1319        T::AddEvent("JUPITER", State::kArmed, State::kTracking, State::kOnTrack)
    12141320            (bind(&StateMachineDrive::SendCommand, this, "CELEST 5 0 0", true))
    12151321            ("Start tracking Jupiter");
    1216         T::AddEvent("SATURN", State::kArmed, State::kTracking)
     1322        T::AddEvent("SATURN", State::kArmed, State::kTracking, State::kOnTrack)
    12171323            (bind(&StateMachineDrive::SendCommand, this, "CELEST 6 0 0", true))
    12181324            ("Start tracking Saturn");
    12191325
    1220         T::AddEvent("PARK", State::kArmed, State::kMoving, State::kTracking, 0x100)
     1326        T::AddEvent("PARK", State::kArmed, State::kMoving, State::kTracking, State::kOnTrack, 0x100)
    12211327            (bind(&StateMachineDrive::SendCommand, this, "PREPS Park", false))
    12221328            ("Park the telescope");
    12231329
    12241330        T::AddEvent("TAKE_TPOINT")
    1225             (bind(&StateMachineDrive::SendCommand, this, "TPOIN FACT 0", true))
     1331            (bind(&StateMachineDrive::SendCommand, this, "TPOIN FACT 0", false))
    12261332            ("Take a TPoint");
     1333
     1334        T::AddEvent("TPOINT", "F:1;C")
     1335            (bind(&StateMachineDrive::TakeTPoint, this, placeholders::_1))
     1336            ("Take a TPoint (given values will be written to the TPoint files)"
     1337             "|mag[float]:Magnitude of the star"
     1338             "|name[string]:Name of the star");
    12271339
    12281340        T::AddEvent("SET_LED_BRIGHTNESS", "I:2")
    … …  
    13651477        }
    13661478
     1479        fDrive.SetDeviationCondition(conf.Get<uint16_t>("deviation-limit"),
     1480                                     conf.Get<uint16_t>("deviation-count"),
     1481                                     conf.Get<uint16_t>("deviation-max"));
     1482
    13671483        fAutoResume = conf.Get<bool>("auto-resume");
    13681484
    … …  
    14011517void SetupConfiguration(Configuration &conf)
    14021518{
     1519    const string def = "localhost:7404";
     1520
    14031521    po::options_description control("Drive control options");
    14041522    control.add_options()
    1405         ("no-dim,d",  po_switch(),    "Disable dim services")
    1406         ("addr,a",  var<string>("localhost:7404"),  "Network address of Cosy")
    1407         ("quiet,q", po_bool(true),  "Disable printing contents of all received messages (except dynamic data) in clear text.")
    1408         ("source-database", var<string>(), "Database link as in\n\tuser:password@server[:port]/database.")
    1409         ("source", vars<string>(), "Additional source entry in the form \"name,hh:mm:ss,dd:mm:ss\"")
    1410         ("auto-resume", po_bool(false), "Enable auto result during tracking")
     1523        ("no-dim,d",        po_switch(),        "Disable dim services")
     1524        ("addr,a",          var<string>(def),   "Network address of cosy")
     1525        ("quiet,q",         po_bool(true),      "Disable printing contents of all received messages (except dynamic data) in clear text.")
     1526        ("source-database", var<string>(),      "Database link as in\n\tuser:password@server[:port]/database.")
     1527        ("source",          vars<string>(),     "Additional source entry in the form \"name,hh:mm:ss,dd:mm:ss\"")
     1528        ("deviation-limit", var<uint16_t>(60),  "Deviation limit in arcsec to get 'OnTrack'")
     1529        ("deviation-count", var<uint16_t>(3),   "Minimum number of reported deviation below deviation-limit to get 'OnTrack'")
     1530        ("deviation-max",   var<uint16_t>(120), "Maximum deviation in arcsec allowed to keep status 'OnTrack'")
     1531        ("auto-resume",     po_bool(false),     "Enable auto result during tracking if connection is lost")
    14111532        ;
    14121533
Note: See TracChangeset for help on using the changeset viewer.