Index: trunk/MagicSoft/Cosy/main/MCosy.cc =================================================================== --- trunk/MagicSoft/Cosy/main/MCosy.cc (revision 1757) +++ trunk/MagicSoft/Cosy/main/MCosy.cc (revision 1758) @@ -10,4 +10,8 @@ #include #include + +#include +#include +#include #include "MGCosy.h" @@ -147,4 +151,7 @@ // are avaraged. The values are returned as a ZdAz object. // +// If one of the two shaftencoders on the elevation axis is missing +// the other one's position is returned. +// // The positions are alway up-to-date because the shaftencoders are // sending all changes immediatly. @@ -152,20 +159,28 @@ ZdAz MCosy::GetSePos() { + const int pa = fAz->GetPos(); + if (fZd1->IsZombieNode() && fZd2->IsZombieNode()) + return ZdAz(0, pa); + // // Get the values // - const int p0 = fZd1->GetPos(); - const int p1 = fZd2->GetPos(); - const int p2 = fAz->GetPos(); + const int p1 = fZd1->GetPos(); + const int p2 = -fZd2->GetPos(); + + if (fZd1->IsZombieNode()) + return ZdAz(p2, pa); + if (fZd2->IsZombieNode()) + return ZdAz(p1, pa); // // interpolate shaft encoder positions // - const float p = (float)(p0-p1)/2; + float p = (float)(p1+p2)/2; // // calculate 'regelabweichung' // - return ZdAz(p, p2); + return ZdAz(p, pa); } @@ -655,10 +670,10 @@ void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad] { - SlaStars sla; + SlaStars sla(fObservatory); // // Position to actual position // - sla.SetMjd2Now(); + sla.Now(); ZdAz dest = sla.CalcZdAz(dst); @@ -707,5 +722,5 @@ // fRaDec = dst; - fTracking = kTRUE; + fBackground = kBgdTracking; //--- ofstream fout("log/cosy.pos"); @@ -729,5 +744,5 @@ // Request theoretical Position for a time in the future (To+dt) from CPU // - sla.SetMjd(sla.CalcMjd()+dt/(60*60*24)); + sla.SetMjd(sla.GetMjd()+dt/(60*60*24)); ZdAz dummy = fBending(sla.CalcZdAz(fRaDec)); dest = CorrectTarget(GetSePos(), dummy); // [se] @@ -817,5 +832,5 @@ } - fTracking = kFALSE; + fBackground = kBgdNone; StopMovement(); lout << "Tracking stopped." << endl; @@ -915,6 +930,6 @@ return (void*)0xebb0; - SlaStars sla; - sla.SetMjd2Now(); + SlaStars sla(fObservatory); + sla.Now(); RaDec rd = *((RaDec*)mp); @@ -943,6 +958,6 @@ { cout << "WM_TPoint: start." << endl; - SlaStars sla; - sla.SetMjd2Now(); + SlaStars sla(fObservatory); + sla.Now(); RaDec rd = *((RaDec*)mp); @@ -976,4 +991,10 @@ cout << "WM_Position: done. (return 0x7777)" << endl; return (void*)0x7777; + + case WM_TESTSE: + cout << "WM_TestSe: start." << endl; + fBackground = mp ? kBgdSeTest : kBgdNone; + cout << "WM_TestSe: done. (return 0x1e51)" << endl; + return (void*)0x1e51; case WM_TRACK: @@ -1040,6 +1061,6 @@ cout << endl; - SlaStars sla; - sla.SetMjd2Now(); + SlaStars sla(fObservatory); + sla.Now(); XY xy = *((XY*)mp); @@ -1160,4 +1181,171 @@ } +void MCosy::TalkThreadTracking() +{ + if (fZd1->IsZombieNode() || fZd2->IsZombieNode() || fAz->IsZombieNode()) + return; + if (!fMac1 || !fMac2) + return; + + SlaStars sla(fObservatory); + + ZdAz old; + ZdAz ist; + + ZdAz sollzd; + ZdAz sollaz; + + ZdAz istre = -fOffset; // [re] + ZdAz time; + + // + // only update fTrackingError while tracking + // + bool phca1=false; + bool phca2=false; + bool phcaz=false; + + while (fBackground==kBgdTracking) + { + // + // Make changes (eg wind) smoother - attenuation of control function + // + const float weight = 1.; //0.3; + + // + // This is the time constant which defines how fast + // you correct for external influences (like wind) + // + fZd1->ResetPosHasChanged(); + fZd2->ResetPosHasChanged(); + fAz->ResetPosHasChanged(); + do + { + phca1 = fZd1->PosHasChanged(); + phca2 = fZd2->PosHasChanged(); + phcaz = fAz->PosHasChanged(); + usleep(1); + } while (!phca1 && !phca2 && !phcaz && fBackground==kBgdTracking); + + //---usleep(100000); // 0.1s + + // + // get position, where we are + // + old = ist; + ist = GetSePos(); // [se] + + // + // if the position didn't change continue + // + /*--- + if ((int)ist.Zd() == (int)old.Zd() && + (int)ist.Az() == (int)old.Az()) + continue; + */ + istre = GetRePosPdo(); + + // + // Get time from last shaftencoder position change (position: ist) + // FIXME: I cannot take the avarage + // + if (fZd1->GetMjd()>fZd2->GetMjd()) + time.Zd(fZd1->GetMjd()); + else + time.Zd(fZd2->GetMjd()); + //time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0); + time.Az(fAz->GetMjd()); + + // + // if Shaftencoder changed position + // calculate were we should be + // + if (phca1 || phca2 /*(int)ist.Zd() != (int)old.Zd()*/) + { + sla.SetMjd(time.Zd()); + + ZdAz dummy = fBending(sla.CalcZdAz(fRaDec)); + sollzd = CorrectTarget(ist, dummy); // [se] + + fOffset.Zd(fOffset.Zd()*(1.-weight)+(ist.Zd()*kGearRatio.X()-istre.Zd())*weight); + } + + if (phcaz /*(int)ist.Az() != (int)old.Az()*/) + { + sla.SetMjd(time.Az()); + + ZdAz dummy = fBending(sla.CalcZdAz(fRaDec)); + sollaz = CorrectTarget(ist, dummy); // [se] + + fOffset.Az(fOffset.Az()*(1.-weight)+(ist.Az()*kGearRatio.Y()-istre.Az())*weight); + } + + ZdAz soll(sollzd.Zd(), sollaz.Az()); + fZdAzSoll = fBending.CorrectBack(soll*2*TMath::Pi()/16384); + + fTrackingError.Set((ist.Zd()-sollzd.Zd())*kGearRatio.X(), + (ist.Az()-sollaz.Az())*kGearRatio.Y()); + + //--- fout << setprecision(15) << setw(17) << time.Zd()*60.*60.*24. << " "; + //--- fout << setprecision(5) << setw(7) << fTrackingError.Zd() << " "; + //--- fout << setprecision(15) << setw(17) << time.Az()*60.*60.*24. << " "; + //--- fout << setprecision(5) << setw(7) << fTrackingError.Az() << endl; + } + //--- fout << endl << endl; +} + +void MCosy::TalkThreadSeTest() +{ +// if (fZd1->IsZombieNode() || fZd2->IsZombieNode()) + // return; + + if (fHistTestSe) + { + lout << "You are much too fast... try again." << endl; + return; + } + + fHistTestSe = new TH2F("Diff", "Difference of SE values", + 201, fMin.Zd(), fMax.Zd(), 101, -50, 50); + fHistTestSe->SetXTitle("ZA [\\circ]"); + fHistTestSe->SetYTitle("\\Delta SE"); + + Double_t offset = 0; + + int cnt = 0; + + lout << "Starting Shaftencoder Test..." << endl; + + while (fBackground==kBgdSeTest) + { + fZd1->ResetPosHasChanged(); + fZd2->ResetPosHasChanged(); + + while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() && + fBackground==kBgdSeTest) + usleep(1); + + const Double_t pos[3] = { fZd1->GetPos(), fZd2->GetPos(), fAz->GetPos() }; + + // + // Estimate Offset from the first ten positions + // + if (cnt++<10) + offset += pos[0]+pos[1]; + if (cnt++==10) + offset /= 10; + if (cnt<11) + continue; + + Double_t apos = (pos[0]-pos[1])/2 * TMath::Pi()*2 / 16384; + + ZdAz bend = fBending(ZdAz(apos, pos[2]))*kRad2Deg; + + fHistTestSe->Fill(bend.Zd(), pos[0]+pos[1]-offset); + } + + lout << "Shaftencoder Test Stopped... displaying Histogram." << endl; +} + void MCosy::TalkThread() { @@ -1180,25 +1368,7 @@ /*** FOR DEMO MODE ***/ - if (fZd1->IsZombieNode() || fZd2->IsZombieNode() || fAz->IsZombieNode()) - return; - - if (!fMac1 || !fMac2) - return; - // // Start the Network // - /* - TEnv env(".cosyrc"); - - cout << "Setting up software endswitch..." << flush; - fMac1->SetNegEndswitch(Deg2AzRE(env.GetValue("Az_Min[Deg]", -1.0))); - fMac1->SetPosEndswitch(Deg2AzRE(env.GetValue("Az_Max[Deg]", +1.0))); - - fMac2->SetNegEndswitch(Deg2ZdRE(env.GetValue("Zd_Min[Deg]", -1.0))); - fMac2->SetPosEndswitch(Deg2ZdRE(env.GetValue("Zd_Max[Deg]", +1.0))); - cout << "done." << endl; - */ - SlaStars sla; while (1) { @@ -1206,118 +1376,20 @@ // wait until a tracking session is started // - while (!fTracking) + while (fBackground==kBgdNone) usleep(1); -//--- ofstream fout("log/cosy.err"); -//--- fout << "Tracking:"; -//--- fout << " Ra: " << Rad2Deg(fRaDec.Ra()) << "\x9c "; -//--- fout << "Dec: " << Rad2Deg(fRaDec.Dec()) << "\x9c" << endl << endl; -//--- fout << " MjdZd/10ms ErrZd/re"; -//--- fout << " MjdAz/10ms ErrAd/re" << endl; - - ZdAz old; - ZdAz ist; - - ZdAz sollzd; - ZdAz sollaz; - - ZdAz istre = -fOffset; // [re] - ZdAz time; - - // - // only update fTrackingError while tracking - // - bool phca1=false; - bool phca2=false; - bool phcaz=false; - - while (fTracking) + switch (fBackground) { - // - // Make changes (eg wind) smoother - attenuation of control function - // - const float weight = 1.; //0.3; - - // - // This is the time constant which defines how fast - // you correct for external influences (like wind) - // - fZd1->ResetPosHasChanged(); - fZd2->ResetPosHasChanged(); - fAz->ResetPosHasChanged(); - do - { - phca1 = fZd1->PosHasChanged(); - phca2 = fZd2->PosHasChanged(); - phcaz = fAz->PosHasChanged(); - usleep(1); - } while (!phca1 && !phca2 && !phcaz && fTracking); - - //---usleep(100000); // 0.1s - - // - // get position, where we are - // - old = ist; - ist = GetSePos(); // [se] - - // - // if the position didn't change continue - // - /*--- - if ((int)ist.Zd() == (int)old.Zd() && - (int)ist.Az() == (int)old.Az()) - continue; - */ - istre = GetRePosPdo(); - - // - // Get time from last shaftencoder position change (position: ist) - // FIXME: I cannot take the avarage - // - if (fZd1->GetMjd()>fZd2->GetMjd()) - time.Zd(fZd1->GetMjd()); - else - time.Zd(fZd2->GetMjd()); - //time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0); - time.Az(fAz->GetMjd()); - - // - // if Shaftencoder changed position - // calculate were we should be - // - if (phca1 || phca2 /*(int)ist.Zd() != (int)old.Zd()*/) - { - sla.SetMjd(time.Zd()); - - ZdAz dummy = fBending(sla.CalcZdAz(fRaDec)); - sollzd = CorrectTarget(ist, dummy); // [se] - - fOffset.Zd(fOffset.Zd()*(1.-weight)+(ist.Zd()*kGearRatio.X()-istre.Zd())*weight); - } - - if (phcaz /*(int)ist.Az() != (int)old.Az()*/) - { - sla.SetMjd(time.Az()); - - ZdAz dummy = fBending(sla.CalcZdAz(fRaDec)); - sollaz = CorrectTarget(ist, dummy); // [se] - - fOffset.Az(fOffset.Az()*(1.-weight)+(ist.Az()*kGearRatio.Y()-istre.Az())*weight); - } - - ZdAz soll(sollzd.Zd(), sollaz.Az()); - fZdAzSoll = fBending.CorrectBack(soll*2*TMath::Pi()/16384); - - fTrackingError.Set((ist.Zd()-sollzd.Zd())*kGearRatio.X(), - (ist.Az()-sollaz.Az())*kGearRatio.Y()); - -//--- fout << setprecision(15) << setw(17) << time.Zd()*60.*60.*24. << " "; -//--- fout << setprecision(5) << setw(7) << fTrackingError.Zd() << " "; -//--- fout << setprecision(15) << setw(17) << time.Az()*60.*60.*24. << " "; -//--- fout << setprecision(5) << setw(7) << fTrackingError.Az() << endl; + case kBgdNone: + continue; + + case kBgdTracking: + TalkThreadTracking(); + continue; + + case kBgdSeTest: + TalkThreadSeTest(); + continue; } - -//--- fout << endl << endl; } } @@ -1355,5 +1427,4 @@ fVelocity, fOffset, fRaDec, fZdAzSoll, fStatus, avail); - /* cout << (int)(fMac1->GetStatus()&Macs::kOutOfControl) << " "; @@ -1362,7 +1433,41 @@ */ + if (fBackground==kBgdSeTest || fHistTestSe==NULL) + return kTRUE; + + DisplayHistTestSe(); + return kTRUE; } +void MCosy::DisplayHistTestSe() +{ + TH2F &hist = *fHistTestSe; + fHistTestSe = NULL; + + TCanvas *c=new TCanvas("c1", "", 1000, 1000); + c->Divide(1,2); + + c->cd(1); + TH2 *h=(TH2*)hist.DrawCopy(); + + TProfile *p = h->ProfileX("_pfx", -1, 9999, "s"); + p->SetLineColor(kBlue); + p->Draw("same"); + p->SetBit(kCanDelete); + + c->cd(2); + + TH1F p2("spread", "Spread of the differences", hist.GetNbinsX(), hist.GetBinLowEdge(1), + hist.GetBinLowEdge(hist.GetNbinsX()+1)); + p2.SetXTitle("ZA [\\circ]"); + for (int i=0; iGetBinError(i)); + p2.SetLineColor(kRed); + p2.SetStats(0); + p2.DrawCopy(); + + delete &hist; +} // -------------------------------------------------------------------------- @@ -1450,5 +1555,5 @@ lout << "- Starting GUI." << endl; - fWin=new MGCosy(this, gClient->GetRoot(), 1, 1); + fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1); } @@ -1480,5 +1585,5 @@ lout << "- Starting GUI." << endl; - fWin=new MGCosy(this, gClient->GetRoot(), 1, 1); + fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1); } @@ -1497,9 +1602,9 @@ lout << "- Starting GUI." << endl; - fWin=new MGCosy(this, gClient->GetRoot(), 1, 1); + fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1); } MCosy::MCosy(int mode, const char *dev, const int baud, MLog &out) -: Network(dev, baud, out), fZd1(0), fZd2(0), fAz(0), fMac1(0), fMac2(0), fMac3(0), fTracking(kFALSE) +: Network(dev, baud, out), fObservatory(MObservatory::kMagic1), fZd1(0), fZd2(0), fAz(0), fMac1(0), fMac2(0), fMac3(0), fBackground(kBgdNone) { TEnv env(".cosyrc"); Index: trunk/MagicSoft/Cosy/main/MCosy.h =================================================================== --- trunk/MagicSoft/Cosy/main/MCosy.h (revision 1757) +++ trunk/MagicSoft/Cosy/main/MCosy.h (revision 1758) @@ -7,4 +7,8 @@ #include "MThread.h" #include "MBending.h" + +#ifndef MARS_Mobservatory +#include "MObservatory.h" +#endif #define kDEG ((char)0x9c) // Linux 'ΓΈ' @@ -22,4 +26,5 @@ #define WM_CALCALTAZ 0x1009 #define WM_HOME 0x100a +#define WM_TESTSE 0x100b @@ -28,4 +33,5 @@ class MGCosy; class MCosy; +class TH2F; class MTTalk : public MThread @@ -46,5 +52,4 @@ }; - class TTimer; class MCosy : public Network, public MsgQueue, public TObject @@ -62,4 +67,6 @@ }; + const MObservatory::LocationName_t fObservatory; + ShaftEncoder *fZd1; ShaftEncoder *fZd2; @@ -76,13 +83,23 @@ // with a generic interface to both... + enum BackgroundAction_t + { + kBgdNone, + kBgdTracking, + kBgdSeTest + }; + + BackgroundAction_t fBackground; // Flag for present backgroundthread + ZdAz fTrackingError; // Tracking Offset between SE and calc-pos [re] ZdAz fOffset; // Offset between se and re coordinate system [re] ZdAz fZdAzSoll; // Soll position when moving RaDec fRaDec; // Position to track - int fTracking; // Flag for present tracking action ZdAz fAccuracy; // Actual accuracy of Tracking ZdAz fVelocity; // Actual velocity of Tracking ZdAz fMin; ZdAz fMax; + + TH2F *fHistTestSe; XY kGearRatio; // describing the gear of the system [re/se] @@ -119,4 +136,8 @@ void TalkThread(); + void TalkThreadTracking(); + void TalkThreadSeTest(); + + void DisplayHistTestSe(); int SetPosition(const ZdAz &dst); Index: trunk/MagicSoft/Cosy/main/Makefile =================================================================== --- trunk/MagicSoft/Cosy/main/Makefile (revision 1757) +++ trunk/MagicSoft/Cosy/main/Makefile (revision 1758) @@ -33,6 +33,5 @@ SRCFILES = MCosy.cc \ - MBending.cc \ - MStarguider.cc + MBending.cc SRCS = $(SRCFILES)