Index: /trunk/FACT++/src/drivectrl.cc =================================================================== --- /trunk/FACT++/src/drivectrl.cc (revision 12769) +++ /trunk/FACT++/src/drivectrl.cc (revision 12770) @@ -136,4 +136,11 @@ } +public: + virtual void UpdateSource() + { + } + virtual void UpdateSource(const array &) + { + } protected: @@ -587,12 +594,11 @@ { private: - DimDescribedService fDimPointing; DimDescribedService fDimTracking; + DimDescribedService fDimSource; DimDescribedService fDimTPoint; DimDescribedService fDimStatus; - virtual void UpdatePointing(const Time &t, - const array &arr) + void UpdatePointing(const Time &t, const array &arr) { fDimPointing.setData(arr); @@ -600,19 +606,18 @@ } - virtual void UpdateTracking(const Time &t, - const array &arr) + void UpdateTracking(const Time &t,const array &arr) { fDimTracking.setData(arr); fDimTracking.Update(t); } - virtual void UpdateStatus(const Time &t, - const array &arr) + + void UpdateStatus(const Time &t, const array &arr) { fDimStatus.setData(arr); fDimStatus.Update(t); } - virtual void UpdateTPoint(const Time &t, - const Drive::DimTPoint &data, - const string &name) + + void UpdateTPoint(const Time &t, const Drive::DimTPoint &data, + const string &name) { vector dim(sizeof(data)+name.length()+1); @@ -630,5 +635,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;D:1", "|Ra[h]:Command right ascension" "|Dec[deg]:Command declination" @@ -638,4 +643,11 @@ "|dZd[deg]:Control deviation Zd" "|dAz[deg]:Control deviation Az"), + fDimSource("DRIVE_CONTROL/SOURCE_POSITION", "D:1;D:1;D:1;D:1;D:1;D:1", + "|Ra_cmd[h]:Command right ascension" + "|Dec_cmd[deg]:Command declination" + "|Ra_src[h]:Source right ascension" + "|Dec_src[deg]:Source declination" + "|Offfset[deg]:Wobble offset" + "|Angle[deg]:Wobble angle"), fDimTPoint("DRIVE_CONTROL/TPOINT", "D:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1;S:1;D:1;D:1;D:1;D:1;D:1;D:1;D:1;C", "|Ra[h]:Command right ascension" @@ -661,4 +673,17 @@ } + void UpdateSource() + { + const array arr = {{ 0, 0, 0, 0, 0, 0 }}; + fDimSource.setQuality(0); + fDimSource.Update(arr); + } + + void UpdateSource(const array &arr) + { + fDimSource.setQuality(1); + fDimSource.Update(arr); + } + // A B [C] [D] E [F] G H [I] J K [L] M N O P Q R [S] T U V W [X] Y Z }; @@ -739,8 +764,12 @@ } - int SendCommand(const string &str) + int SendCommand(const string &str, bool upd=true) { fDrive.PostMessage(str); T::Message("Sending: "+str); + + if (upd) + fDrive.UpdateSource(); + return T::GetCurrentState(); } @@ -762,7 +791,64 @@ } + if (type!=kPoint) + { + const array dim = {{ dat[0], dat[1], dat[0], dat[1], 0, 0 }}; + fDrive.UpdateSource(dim); + } + command += AngleToStr(dat[0]) + ' ' + AngleToStr(dat[1]); - - return SendCommand(command); + return SendCommand(command, type==kPoint); + } + + int Wobble(const EventImp &evt) + { + if (!CheckEventSize(evt.GetSize(), "Wobble", 32)) + return T::kSM_FatalError; + + const double *dat = evt.Ptr(); + + const double ra = dat[0]*M_PI/12; + const double dec = dat[1]*M_PI/180; + const double off = dat[2]*M_PI/180; + const double dir = dat[3]*M_PI/180; + + const double cosdir = cos(dir); + const double sindir = sin(dir); + const double cosoff = cos(off); + const double sinoff = sin(off); + const double cosdec = cos(dec); + const double sindec = sin(dec); + + if (off==0) + { + const array dim = {{ ra, dec, ra, dec, 0, 0 }}; + fDrive.UpdateSource(dim); + + string command = "RADEC "; + command += AngleToStr(dat[0]) + ' ' + AngleToStr(dat[1]); + return SendCommand(command, false); + } + + const double costheta = cosdec*cosoff + sindec*sinoff*cosdir; + if (costheta >= 1) + { + T::Error("cos(Zd) > 1"); + return T::GetCurrentState(); + } + + const double sintheta = sqrt(1 - costheta*costheta); + + const double cosdeltara = (cosoff - cosdec*costheta)/(sindec*sintheta); + const double sindeltara = sindir*sinoff/sintheta; + + const double ndec = acos(costheta)*180/M_PI; + const double nra = (atan2(sindeltara, cosdeltara) + ra)*12/M_PI; + + const array dim = {{ ra, dec, nra, ndec, off, dir }}; + fDrive.UpdateSource(dim); + + string command = "RADEC "; + command += AngleToStr(nra) + ' ' + AngleToStr(ndec); + return SendCommand(command, false); } @@ -776,5 +862,5 @@ ostringstream cmd; cmd << "LEDS " << led[0] << " " << led[1]; - return SendCommand(cmd.str()); + return SendCommand(cmd.str(), false); } @@ -906,22 +992,30 @@ "|Dec[deg]:Declination"); + T::AddEvent("WOBBLE", "D:4", kStateArmed) // ->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" + "|Ra[h]:Right ascension" + "|Dec[deg]:Declination" + "|Offset[deg]:Wobble offset" + "|Angle[deg]:Wobble angle"); + T::AddEvent("MOON", kStateArmed) - (bind(&StateMachineDrive::SendCommand, this, "MOON 0 0")) + (bind(&StateMachineDrive::SendCommand, this, "MOON 0 0", true)) ("Start tracking the moon"); T::AddEvent("VENUS", kStateArmed) - (bind(&StateMachineDrive::SendCommand, this, "CELEST 2 0 0")) + (bind(&StateMachineDrive::SendCommand, this, "CELEST 2 0 0", true)) ("Start tracking Venus"); T::AddEvent("MARS", kStateArmed) - (bind(&StateMachineDrive::SendCommand, this, "CELEST 4 0 0")) + (bind(&StateMachineDrive::SendCommand, this, "CELEST 4 0 0", true)) ("Start tracking Mars"); T::AddEvent("JUPITER", kStateArmed) - (bind(&StateMachineDrive::SendCommand, this, "CELEST 5 0 0")) + (bind(&StateMachineDrive::SendCommand, this, "CELEST 5 0 0", true)) ("Start tracking Jupiter"); T::AddEvent("SATURN", kStateArmed) - (bind(&StateMachineDrive::SendCommand, this, "CELEST 6 0 0")) + (bind(&StateMachineDrive::SendCommand, this, "CELEST 6 0 0", true)) ("Start tracking Saturn"); T::AddEvent("TAKE_TPOINT") - (bind(&StateMachineDrive::SendCommand, this, "TPOIN FACT 0")) + (bind(&StateMachineDrive::SendCommand, this, "TPOIN FACT 0", true)) ("Take a TPoint"); @@ -933,9 +1027,9 @@ T::AddEvent("LEDS_OFF") - (bind(&StateMachineDrive::SendCommand, this, "LEDS 0 0")) + (bind(&StateMachineDrive::SendCommand, this, "LEDS 0 0", false)) ("Switch off TPoint LEDs"); T::AddEvent("STOP") - (bind(&StateMachineDrive::SendCommand, this, "STOP!")) + (bind(&StateMachineDrive::SendCommand, this, "STOP!", true)) ("Stop any kind of movement.");