Index: /trunk/FACT++/src/drivectrl.cc =================================================================== --- /trunk/FACT++/src/drivectrl.cc (revision 14594) +++ /trunk/FACT++/src/drivectrl.cc (revision 14595) @@ -69,5 +69,5 @@ } - virtual void UpdateTracking(const Time &, const array &) + virtual void UpdateTracking(const Time &, const array &) { } @@ -118,4 +118,22 @@ return sgn=='-' ? -ret : ret; } + + double GetDevAbs(double nomzd, double meszd, double devaz) + { + nomzd *= M_PI/180; + meszd *= M_PI/180; + devaz *= M_PI/180; + + const double x = sin(meszd) * sin(nomzd) * cos(dvaz); + const double y = cos(meszd) * cos(nomzd); + + return acos(x + y) * 180/M_PI; + } + + uint16_t fDeviationLimit; + uint16_t fDeviationCounter; + uint16_t fDeviationMax; + + uint64_t fTrackingCounter; protected: @@ -322,12 +340,12 @@ stream >> mjd; - const double zd1 = ReadAngle(stream); - const double az1 = ReadAngle(stream); - const double zd2 = ReadAngle(stream); - const double az2 = ReadAngle(stream); + const double zd1 = ReadAngle(stream); // Nominal (zd/az asynchronous, dev synchronous, mjd synchronous with zd) + const double az1 = ReadAngle(stream); // Nominal (zd/az asynchronous, dev synchronous, mjd synchronous with z) + const double zd2 = ReadAngle(stream); // Masured (zd/az synchronous, dev asynchronous, mjd asynchronous) + const double az2 = ReadAngle(stream); // Measurd (zd/az synchronous, dev asynchronous, mjd asynchronous) double zd_err, az_err; - stream >> zd_err; - stream >> az_err; + stream >> zd_err; // Deviation = Nominal - Measured + stream >> az_err; // Deviation = Nominal - Measured uint16_t armed, stgmd; @@ -363,4 +381,5 @@ // kMoving, // kTracking, + // kOnTrack, // pdo3: @@ -381,4 +400,29 @@ // charging: 4ef04ef + // Convert to deg + zd_err /= 3600; + az_err /= 3600; + + // Calculate absolut deviation on the sky + const double dev = GetDevAbs(zd1, zd1-zd_err, az_err)*3600; + +Deviation = Nominal - Measured + + // If any other state than tracking or a deviation + // larger than 60, reset the counter + if (fState!=State::kTracking || dev>fDeviationLimit) + fTrackingCounter = 0; + else + fTrackingCounter++; + + // If in tracking, at least five consecutive reports (5s) + // must be below 60arcsec deviation, this is considered OnTrack + if (fState==State::kTracking && fTrackingCounter>=fDeviationCounter) + fState = State::kOnTrack; + + // Having th state as Tracking will reset the counter + if (fState==State::kOnTrack && dev>fDeviationMax) + fState = State::kTracking; + const array state = {{ uint8_t(pdo3>>16), uint8_t(pdo3), uint8_t(pdo3>>24) }}; UpdateStatus(t1, state); @@ -387,9 +431,10 @@ UpdatePointing(t1, point); - const array track = + const array track = {{ ra, dec, ha, zd1, az1, - zd_err/3600, az_err/3600 + zd_err, az_err, + dev }}; if (mjd>0) @@ -517,5 +562,5 @@ public: ConnectionDrive(ba::io_service& ioservice, MessageImp &imp) : Connection(ioservice, imp()), - fState(-1), fIsVerbose(true), fKeepAlive(ioservice) + fState(-1), fIsVerbose(true), fDeviationLimit(120), fDeviationCounter(5), fDeviationMax(240), fTrackingCounter(0), fKeepAlive(ioservice) { SetLogStream(&imp); @@ -527,4 +572,10 @@ } + void SetDeviationCondition(uint16_t limit, uint16_t counter, uint16_t max) + { + fDeviationLimit = limit; + fDeviationCounter = counter; + fDeviationMax = max; + } int GetState() const { @@ -558,5 +609,5 @@ } - void UpdateTracking(const Time &t,const array &arr) + void UpdateTracking(const Time &t,const array &arr) { fDimTracking.setData(arr); @@ -587,5 +638,5 @@ "|Zd[deg]:Zenith distance (encoder readout)" "|Az[deg]:Azimuth angle (encoder readout)"), - fDimTracking("DRIVE_CONTROL/TRACKING_POSITION", "D:1;D:1;D:1;D:1;D:1;D:1;D:1", + fDimTracking("DRIVE_CONTROL/TRACKING_POSITION", "D:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1", "|Ra[h]:Command right ascension" "|Dec[deg]:Command declination" @@ -594,5 +645,6 @@ "|Az[deg]:Nominal azimuth angle" "|dZd[deg]:Control deviation Zd" - "|dAz[deg]:Control deviation Az"), + "|dAz[deg]:Control deviation Az" + "|dev[arcsec]:Absolute control deviation"), fDimSource("DRIVE_CONTROL/SOURCE_POSITION", "D:1;D:1;D:1;D:1;D:1;D:1;C:31", "|Ra_src[h]:Source right ascension" @@ -895,4 +947,24 @@ } + int StartTrackWobble(const char *ptr, size_t size, const double &offset=0, const double &angle=0) + { + const char *last = ptr+size; + + try + { + const sources::const_iterator it = GetSourceFromDB(ptr, last); + + const string &name = it->first; + const Source &src = it->second; + + return StartWobble(src.ra, src.dec, offset, angle, name); + } + catch (const uint32_t &e) + { + return e; + } + + } + int Track(const EventImp &evt) { @@ -907,19 +979,45 @@ const double *dat = evt.Ptr(); const char *ptr = evt.Ptr(16); - const char *last = ptr+evt.GetSize()-16; - - try - { - const sources::const_iterator it = GetSourceFromDB(ptr, last); - - const string &name = it->first; - const Source &src = it->second; - - return StartWobble(src.ra, src.dec, dat[0], dat[1], name); - } - catch (const uint32_t &e) - { - return e; - } + const size_t size = evt.GetSize()-16; + + return StartTrackWobble(ptr, size, dat[0], dat[1]); + } + + int TrackOn(const EventImp &evt) + { + if (evt.GetSize()==0) + { + ostringstream msg; + msg << "TrackOn - Received event has " << evt.GetSize() << " bytes, but expected at least 1."; + T::Fatal(msg); + return T::kSM_FatalError; + } + + return StartTrackWobble(evt.Ptr(), evt.GetSize()); + } + + + int TakeTPoint(const EventImp &evt) + { + if (evt.GetSize()<=4) + { + ostringstream msg; + msg << "TakePoint - Received event has " << evt.GetSize() << " bytes, but expected at least 5."; + T::Fatal(msg); + return T::kSM_FatalError; + } + + const float mag = evt.Get(); + const char *ptr = evt.Ptr(4); + const size_t size = evt.GetSize()-4; + + string src(ptr, size); + + while (src.find_first_of(' ')) + src.erase(src.find_first_of(' '), 1); + + SendCommand("TPOIN "+src+" "+to_string(mag), false);; + + return T::GetCurrentState(); } @@ -1120,23 +1218,26 @@ T::AddStateName(State::kConnected, "Connected", - "Cosy connected, drive stopped"); + "Cosy connected, drive stopped"); T::AddStateName(State::kNotReady, "NotReady", - "Drive system not ready for movement"); + "Drive system not ready for movement"); T::AddStateName(State::kLocked, "Locked", - "Drive system is locked (will not accept commands)"); + "Drive system is locked (will not accept commands)"); T::AddStateName(State::kReady, "Ready", - "Drive system ready for movement"); + "Drive system ready for movement"); T::AddStateName(State::kArmed, "Armed", - "Cosy armed, drive stopped"); + "Cosy armed, drive stopped"); T::AddStateName(State::kMoving, "Moving", - "Telescope moving"); + "Telescope moving"); T::AddStateName(State::kTracking, "Tracking", - "Telescope tracking"); + "Telescope is in tracking mode"); + + T::AddStateName(State::kOnTrack, "OnTrack", + "Telescope tracking stable"); // State::kIdle @@ -1171,5 +1272,5 @@ "|Az[deg]:Azimuth"); - T::AddEvent("TRACK", "D:2", State::kArmed, State::kTracking) // ->RADEC/GRB + T::AddEvent("TRACK", "D:2", State::kArmed, State::kTracking, State::kOnTrack) // ->RADEC/GRB (bind(&StateMachineDrive::SendCoordinates, this, placeholders::_1, kTrackSlow)) ("Move the telescope to the given sky coordinates and start tracking them" @@ -1177,5 +1278,5 @@ "|Dec[deg]:Declination"); - T::AddEvent("WOBBLE", "D:4", State::kArmed, State::kTracking) // ->RADEC/GRB + T::AddEvent("WOBBLE", "D:4", State::kArmed, State::kTracking, State::kOnTrack) // ->RADEC/GRB (bind(&StateMachineDrive::Wobble, this, placeholders::_1)) ("Move the telescope to the given wobble position around the given sky coordinates and start tracking them" @@ -1185,5 +1286,5 @@ "|Angle[deg]:Wobble angle"); - T::AddEvent("TRACK_SOURCE", "D:2;C", State::kArmed, State::kTracking) // ->RADEC/GRB + T::AddEvent("TRACK_SOURCE", "D:2;C", State::kArmed, State::kTracking, State::kOnTrack) // ->RADEC/GRB (bind(&StateMachineDrive::Track, this, placeholders::_1)) ("Move the telescope to the given wobble position around the given source and start tracking" @@ -1192,5 +1293,5 @@ "|Name[string]:Source name"); - T::AddEvent("TRACK_WOBBLE", "S:1;C", State::kArmed, State::kTracking) // ->RADEC/GRB + T::AddEvent("TRACK_WOBBLE", "S:1;C", State::kArmed, State::kTracking, State::kOnTrack) // ->RADEC/GRB (bind(&StateMachineDrive::TrackWobble, this, placeholders::_1)) ("Move the telescope to the given wobble position around the given source and start tracking" @@ -1198,31 +1299,42 @@ "|Name[string]:Source name"); + T::AddEvent("TRACK_ON", "S:1;C", State::kArmed, State::kTracking, State::kOnTrack) // ->RADEC/GRB + (bind(&StateMachineDrive::TrackOn, this, placeholders::_1)) + ("Move the telescope to the given position and start tracking" + "|Name[string]:Source name"); + T::AddEvent("RESUME", StateMachineImp::kSM_Error) (bind(&StateMachineDrive::Resume, this)) ("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."); - T::AddEvent("MOON", State::kArmed, State::kTracking) + T::AddEvent("MOON", State::kArmed, State::kTracking, State::kOnTrack) (bind(&StateMachineDrive::SendCommand, this, "MOON 0 0", true)) ("Start tracking the moon"); - T::AddEvent("VENUS", State::kArmed, State::kTracking) + T::AddEvent("VENUS", State::kArmed, State::kTracking, State::kOnTrack) (bind(&StateMachineDrive::SendCommand, this, "CELEST 2 0 0", true)) ("Start tracking Venus"); - T::AddEvent("MARS", State::kArmed, State::kTracking) + T::AddEvent("MARS", State::kArmed, State::kTracking, State::kOnTrack) (bind(&StateMachineDrive::SendCommand, this, "CELEST 4 0 0", true)) ("Start tracking Mars"); - T::AddEvent("JUPITER", State::kArmed, State::kTracking) + T::AddEvent("JUPITER", State::kArmed, State::kTracking, State::kOnTrack) (bind(&StateMachineDrive::SendCommand, this, "CELEST 5 0 0", true)) ("Start tracking Jupiter"); - T::AddEvent("SATURN", State::kArmed, State::kTracking) + T::AddEvent("SATURN", State::kArmed, State::kTracking, State::kOnTrack) (bind(&StateMachineDrive::SendCommand, this, "CELEST 6 0 0", true)) ("Start tracking Saturn"); - T::AddEvent("PARK", State::kArmed, State::kMoving, State::kTracking, 0x100) + T::AddEvent("PARK", State::kArmed, State::kMoving, State::kTracking, State::kOnTrack, 0x100) (bind(&StateMachineDrive::SendCommand, this, "PREPS Park", false)) ("Park the telescope"); T::AddEvent("TAKE_TPOINT") - (bind(&StateMachineDrive::SendCommand, this, "TPOIN FACT 0", true)) + (bind(&StateMachineDrive::SendCommand, this, "TPOIN FACT 0", false)) ("Take a TPoint"); + + T::AddEvent("TPOINT", "F:1;C") + (bind(&StateMachineDrive::TakeTPoint, this, placeholders::_1)) + ("Take a TPoint (given values will be written to the TPoint files)" + "|mag[float]:Magnitude of the star" + "|name[string]:Name of the star"); T::AddEvent("SET_LED_BRIGHTNESS", "I:2") @@ -1365,4 +1477,8 @@ } + fDrive.SetDeviationCondition(conf.Get("deviation-limit"), + conf.Get("deviation-count"), + conf.Get("deviation-max")); + fAutoResume = conf.Get("auto-resume"); @@ -1401,12 +1517,17 @@ void SetupConfiguration(Configuration &conf) { + const string def = "localhost:7404"; + po::options_description control("Drive control options"); control.add_options() - ("no-dim,d", po_switch(), "Disable dim services") - ("addr,a", var("localhost:7404"), "Network address of Cosy") - ("quiet,q", po_bool(true), "Disable printing contents of all received messages (except dynamic data) in clear text.") - ("source-database", var(), "Database link as in\n\tuser:password@server[:port]/database.") - ("source", vars(), "Additional source entry in the form \"name,hh:mm:ss,dd:mm:ss\"") - ("auto-resume", po_bool(false), "Enable auto result during tracking") + ("no-dim,d", po_switch(), "Disable dim services") + ("addr,a", var(def), "Network address of cosy") + ("quiet,q", po_bool(true), "Disable printing contents of all received messages (except dynamic data) in clear text.") + ("source-database", var(), "Database link as in\n\tuser:password@server[:port]/database.") + ("source", vars(), "Additional source entry in the form \"name,hh:mm:ss,dd:mm:ss\"") + ("deviation-limit", var(60), "Deviation limit in arcsec to get 'OnTrack'") + ("deviation-count", var(3), "Minimum number of reported deviation below deviation-limit to get 'OnTrack'") + ("deviation-max", var(120), "Maximum deviation in arcsec allowed to keep status 'OnTrack'") + ("auto-resume", po_bool(false), "Enable auto result during tracking if connection is lost") ;