Index: /trunk/MagicSoft/Cosy/Changelog =================================================================== --- /trunk/MagicSoft/Cosy/Changelog (revision 9131) +++ /trunk/MagicSoft/Cosy/Changelog (revision 9132) @@ -1,3 +1,54 @@ -*-*- END -*-*- + + 2008/08/26 Thomas Bretz (La Palma) + + * cosy.cc: + - changed ceco to 151.72.130.60 + + * candrv/canopen.[h,cc]: + - added HasConnection + + * candrv/network.cc: + - check for connection before trying to reboot zombies + + * candrv/nodedrv.cc: + - some improvement to output + + * devdrv/DevdrvLinkDef.h, devdrv/Makefile: + - added Dkc + + * gui/MGCosy.[h,cc]: + - change to layout + - display DKC information + - display cabinet information + + * gui/MGSkyPosition.[h,cc]: + - for convinience added Polaris to display + + * main/MCosy.[h,cc]: + - removed shaftencoder and macs + - removed a lot of old obsolete code + - added Dkc + - set new velocity and acceleration values + - removed a lot of expert stuff + - removed ambiguity between feedback 1 and 2 + + * main/MSlewing.[h,cc]: + - replaced Macs by Dkc + - fixed to use new units + - scale velocity even with different maximum speeds + - changed from relative positionuing to absolute positioning + - implemented a new correction for the non-uniformity of the el-axis + + * main/MStarguider.cc: + - changed usage of MString + - use GetSePos now instead of GetTrackingPosRaw + + * main/MTracking.[h,cc]: + - replaced Macs by Dkc + - removed ambiguity of feedback 1 and feedback 2 + - adapted definition of control deviation + + 2008/07/13 Thomas Bretz Index: /trunk/MagicSoft/Cosy/candrv/canopen.cc =================================================================== --- /trunk/MagicSoft/Cosy/candrv/canopen.cc (revision 9131) +++ /trunk/MagicSoft/Cosy/candrv/canopen.cc (revision 9132) @@ -78,4 +78,9 @@ if (fInterface) fInterface->Stop(); +} + +Bool_t CanOpen::HasConnection() const +{ + return fInterface ? fInterface->HasConnection() : kFALSE; } Index: /trunk/MagicSoft/Cosy/candrv/canopen.h =================================================================== --- /trunk/MagicSoft/Cosy/candrv/canopen.h (revision 9131) +++ /trunk/MagicSoft/Cosy/candrv/canopen.h (revision 9132) @@ -101,4 +101,5 @@ void SetInterface(Interface *f) { fInterface=f; } // Set Handle to the interface + Bool_t HasConnection() const; // Send a Process Data Object (PDO) to the CanOpen bus Index: /trunk/MagicSoft/Cosy/candrv/network.cc =================================================================== --- /trunk/MagicSoft/Cosy/candrv/network.cc (revision 9131) +++ /trunk/MagicSoft/Cosy/candrv/network.cc (revision 9132) @@ -356,7 +356,13 @@ bool Network::RebootZombies() { + if (!HasConnection()) + { + gLog << warn << "- No connection to network." << endl; + return false; + } + bool rc = true; - gLog << "- Trying to reboot all Zombies..." << endl; + gLog << inf2 << "- Trying to reboot all Zombies..." << endl; for (int i=0; i<32; i++) if (fNodes[i]) @@ -364,5 +370,5 @@ if (!fNodes[i]->Reboot()) { - gLog << "- Failed to reboot " << fNodes[i]->GetNodeName() << "." << endl; + gLog << err << "- Failed to reboot " << fNodes[i]->GetNodeName() << "." << endl; rc = false; } Index: /trunk/MagicSoft/Cosy/candrv/nodedrv.cc =================================================================== --- /trunk/MagicSoft/Cosy/candrv/nodedrv.cc (revision 9131) +++ /trunk/MagicSoft/Cosy/candrv/nodedrv.cc (revision 9132) @@ -427,7 +427,13 @@ return true; - gLog << inf2 << " + " << GetNodeName() << ": NodeDrv::WaitForSdo: 0x" << hex << idx << "/" << dec << (int)subidx << " --> ZOMBIE! " << MTime(-1) << endl; + gLog << inf2 << " + " << GetNodeName() << ": NodeDrv::WaitForSdo: 0x" << hex << idx << "/" << dec << (int)subidx << " "; if (zombie) + { + gLog << "--> ZOMBIE! " << endl; SetZombie(); + } + + gLog << MTime(-1) << endl; + return false; } Index: /trunk/MagicSoft/Cosy/cosy.cc =================================================================== --- /trunk/MagicSoft/Cosy/cosy.cc (revision 9131) +++ /trunk/MagicSoft/Cosy/cosy.cc (revision 9132) @@ -92,5 +92,5 @@ const Bool_t kDebugThreads = arg.HasOnlyAndRemove("--debug-threads"); const TString sps = arg.GetStringAndRemove("--sps=", "sps"); - const TString ceco = arg.GetStringAndRemove("--cc=", "ceco"); // ceco + const TString ceco = arg.GetStringAndRemove("--cc=", "161.72.130.60"); // ceco const Int_t ceco_tx = arg.GetIntAndRemove("--cc-tx=", 7314);//7304); const Int_t ceco_rx = arg.GetIntAndRemove("--cc-rx=", 7414);//7404); @@ -197,4 +197,5 @@ MDriveCom *com = new MDriveCom(ceco, ceco_tx, ceco_rx); + com->SetTelescope(2); MCosy *cosy = new MCosy(env, com, pointing); Index: /trunk/MagicSoft/Cosy/devdrv/DevdrvLinkDef.h =================================================================== --- /trunk/MagicSoft/Cosy/devdrv/DevdrvLinkDef.h (revision 9131) +++ /trunk/MagicSoft/Cosy/devdrv/DevdrvLinkDef.h (revision 9132) @@ -5,4 +5,5 @@ #pragma link off all functions; +#pragma link C++ class Dkc+; #pragma link C++ class Macs+; #pragma link C++ class ShaftEncoder+; Index: /trunk/MagicSoft/Cosy/devdrv/Makefile =================================================================== --- /trunk/MagicSoft/Cosy/devdrv/Makefile (revision 9131) +++ /trunk/MagicSoft/Cosy/devdrv/Makefile (revision 9132) @@ -17,5 +17,5 @@ #------------------------------------------------------------------------------ -SRCFILES = shaftencoder.cc macs.cc +SRCFILES = shaftencoder.cc macs.cc dkc.cc ############################################################ Index: /trunk/MagicSoft/Cosy/devdrv/dkc.cc =================================================================== --- /trunk/MagicSoft/Cosy/devdrv/dkc.cc (revision 9132) +++ /trunk/MagicSoft/Cosy/devdrv/dkc.cc (revision 9132) @@ -0,0 +1,830 @@ +#include "dkc.h" + +#include // timeval->tv_sec + +#include + +#include "network.h" + +#include "MLogManip.h" + +#include "MString.h" + +ClassImp(Dkc); + +using namespace std; + +#define EXPERT + +Dkc::Dkc(const BYTE_t nodeid, const char *name) + : NodeDrv(nodeid, name), fMacId(2*nodeid+1), + fPdoPos1(0), fPdoPos2(0), fPosActive(0), fRpmActive(0), + fStatusDKC(0), fStatusPdo3(0xff), fArmed(false), + fReport(NULL),fLabel(NULL) +{ +} + +Dkc::~Dkc() +{ +} + +TString Dkc::EvalStatusDKC(UInt_t stat) const +{ + switch (stat) + { + case 0: return "offline"; + case 0xa000: + case 0xa001: + case 0xa002: + case 0xa003: return MString::Format("Communication phase %d", stat&0xf); + case 0xa010: return "Drive HALT"; + case 0xa012: return "Control and power section ready for operation"; + case 0xa013: return "Ready for power on"; + case 0xa100: return "Drive in Torque mode"; + case 0xa101: return "Drive in Velocity mode"; + case 0xa102: return "Position control mode with encoder 1"; + case 0xa103: return "Position control mode with encoder 2"; + case 0xa104: return "Position control mode with encoder 1, lagless"; + case 0xa105: return "Position control mode with encoder 2, lagless"; + case 0xa106: return "Drive controlled interpolated positioning with encoder 1"; + case 0xa107: return "Drive controlled interpolated positioning with encoder 2"; + case 0xa108: return "Drive controlled interpolated positioning with encoder 1, lagless"; + case 0xa109: return "Drive controlled interpolated positioning with encoder 2, lagless"; + //case 0xa146: return "Drive controlled interpolated relative positioning with encoder 1"; + //case 0xa147: return "Drive controlled interpolated relative positioning with encoder 2"; + //case 0xa148: return "Drive controlled interpolated relative positioning lagless with encoder 1"; + //case 0xa149: return "Drive controlled interpolated relative positioning lagless with encoder 2"; + case 0xa150: return "Drive controlled positioning with encoder 1"; + case 0xa151: return "Drive controlled positioning with encoder 1, lagless"; + case 0xa152: return "Drive controlled positioning with encoder 2"; + case 0xa153: return "Drive controlled positioning with encoder 2, lagless"; + case 0xa208: return "Jog mode positive"; + case 0xa218: return "Jog mode negative"; + case 0xa400: return "Automatic drive check and adjustment"; + case 0xa401: return "Drive decelerating to standstill"; + case 0xa800: return "Unknown operation mode"; + case 0xc217: return "Motor encoder reading error"; + case 0xc218: return "Shaft encoder reading error"; + case 0xc220: return "Motor encoder initialization error"; + case 0xc221: return "Shaft encoder initialization error"; + case 0xc300: return "Command: set absolute measure"; + case 0xc400: return "Switching to parameter mode"; + case 0xc401: return "Drive active, switching mode not allowed"; + case 0xc500: return "Error reset"; + case 0xc600: return "Drive controlled homi*ng procedure "; + case 0xe225: return "Motor overload"; + case 0xe249: return "Positioning command velocity exceeds limit bipolar"; + case 0xe250: return "Drive overtemp warning"; + case 0xe251: return "Motor overtemp warning"; + case 0xe252: return "Bleeder overtemp warning"; + case 0xe257: return "Continous current limit active"; + case 0xe264: return "Target position out of numerical range"; + case 0xe829: return "Positive position limit exceeded"; + case 0xe830: return "Negative position limit exceeded"; + case 0xe831: return "Position limit reached during jog"; + case 0xe834: return "Emergency-Stop"; + case 0xe842: return "Both end-switches activated"; + case 0xe843: return "Positive end-switch activated"; + case 0xe844: return "Negative end-switch activated"; + case 0xf218: return "Amplifier overtemp shutdown"; + case 0xf219: return "Motor overtemp shutdown"; + case 0xf220: return "Bleeder overload shutdown"; + case 0xf221: return "Motor temperature surveillance defective"; + case 0xf224: return "Maximum breaking time exceeded"; + case 0xf228: return "Excessive control deviation"; + case 0xf250: return "Overflow of target position preset memory"; + case 0xf269: return "Error during release of the motor holding brake"; + case 0xf276: return "Absolute encoder out of allowed window"; + case 0xf409: return "Bus error on Profibus interface"; + case 0xf434: return "Emergency-Stop"; + case 0xf629: return "Positive position limit exceeded"; + case 0xf630: return "Negative position limit exceeded"; + case 0xf634: return "Emergency-Stop"; + case 0xf643: return "Positive end-switch activated"; + case 0xf644: return "Negative end-switch activated"; + case 0xf870: return "24V DC error"; + case 0xf878: return "Velocity loop error"; + } + return "unknown"; +} + +TString Dkc::GetStatus(LWORD_t val) const +{ + const Int_t errnum = val&0xffff; + const Int_t errinf = val>>16; + + if (errnum!=0xff) + return ""; + + // DKC offline. This is a fatal error + if (errinf==0) + return "offline."; + + TString str; + + const Int_t type = errinf&0xf000; + switch (type) + { + case 0xf000: str += "ERROR"; break; + case 0xe000: str += "WARNING"; break; + case 0xa000: str += "Status"; break; + case 0xc000: + case 0xd000: str += "Message"; break; + default: str += "Unknown"; break; + } + + str += " ("; + str += MString::Format("%X", errinf); + str += "): "; + str += EvalStatusDKC(errinf); + str += (type==0xf000 || type==0xe000 ? "!" : "."); + + return str; +} + +Bool_t Dkc::PrintStatus(LWORD_t val) const +{ + const Int_t errnum = val&0xffff; + const Int_t errinf = val>>16; + + if (errnum!=0xff) + return errnum==0; + + gLog << all << MTime(-1) << ": " << GetNodeName() << " " << GetStatus(val) << endl; + + // errinf==0: DKC offline. This is a fatal error + return errinf==0 ? kFALSE : (errinf&0xf000)!=0xf000; +} + +void Dkc::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, const timeval_t &tv) +{ + // cout << "SdoRx: Idx=0x"<< hex << idx << "/" << (int)subidx; + // cout << ", val=0x" << val << endl; + switch (idx) + { + case 0x1000: + if (subidx==1) + { + gLog << inf2 << "- " << GetNodeName() << ": Node is" << (val?" ":" not ") << "armed." << endl; + fArmed = val==1; + } + return; + + case 0x1003: + // FIXME, see Init + if (subidx!=2) + return; + gLog << inf2 << "- " << GetNodeName() << ": Error[0]=" << hex << val << dec << endl; + CheckErrorDKC(val); + return; + + case 0x100a: + gLog << inf2 << "- " << GetNodeName() << ": Using Software Version V" << dec << (int)(val>>16) << "." << (int)(val&0xff) << endl; + fSoftVersion = val; + return; + + case 0x100b: + // Do not display, this is used for CheckConnection + // lout << "Node ID: " << dec << val << endl; + return; + + case 0x100c: + gLog << inf2 << "- " << GetNodeName() << ": Guard time:" << dec << val << endl; + return; + + case 0x100d: + gLog << inf2 << "- " << GetNodeName() << ": Life time factor:" << dec << val << endl; + return; + + case 0x2002: + gLog << inf2 << GetNodeName() << ": Current velocity: " << dec << val << endl; + fVel = val; + return; + + case 0x6004: + if (subidx==0) + { + fPdoPos1 = (LWORDS_t)val; + fPdoTime1.Set(tv); + fHasChangedPos1 = true; + } + if (subidx==1) + { + fPdoPos2 = (LWORDS_t)val; + fPdoTime2.Set(tv); + fHasChangedPos2 = true; + } + return; + + case 0x6002: + gLog << inf2 << "- " << GetNodeName() << ": Velocity resolution = " << dec << val << " (1rpm)" << endl; + fVelRes = val; + return; + + case 0x6003: + gLog << inf2 << "- " << GetNodeName() << ": Maximum velocity = " << dec << val << " (100%)" << endl; + fVelMax = val; + return; + + case 0x6501: + gLog << inf2 << "- " << GetNodeName() << ": Position resolution = " << dec << val << " ticks/revolution" << endl; + fPosRes = val; + return; + } + + NodeDrv::HandleSDO(idx, subidx, val, tv); + + // cout << "Dkc: SDO, idx=0x"<< hex << idx << "/" << (int)subidx; + // cout << ", val=0x"< ZOMBIE!" << endl; + SetZombie(); + } + if (IsZombieNode()) + return; + */ + + SetRpmMode(FALSE); + + ReqPosRes(); // Init fVelRes + ReqVelRes(); // Init fVelResVelRes + ReqVelMax(); // Init fVelMax + +#ifdef EXPERT + StartNode(); +#endif + + gLog << inf2 << "- " << GetNodeName() << ": Checking armed status." << endl; + RequestSDO(0x1000, 1); + WaitForSdo(0x1000, 1); +} + +void Dkc::StopDevice() +{ + // + // FIXME: This isn't called if the initialization isn't done completely! + // + + SetRpmMode(FALSE); +} + +void Dkc::ReqPos1() +{ + gLog << inf2 << "- " << GetNodeName() << ": Requesting position feedback 1." << endl; + RequestSDO(0x6004, 0); + WaitForSdo(0x6004, 0); +} + +void Dkc::ReqPos2() +{ + gLog << inf2 << "- " << GetNodeName() << ": Requesting position feedback 2." << endl; + RequestSDO(0x6004, 1); + WaitForSdo(0x6004, 1); +} + +void Dkc::ReqVel() +{ + gLog << inf2 << "- " << GetNodeName() << ": Requesting Velocity." << endl; + RequestSDO(0x2002); + WaitForSdo(0x2002); +} + +void Dkc::SetVelocity(LWORD_t vel) +{ + gLog << dbg << "- Setting velocity to: " << vel << endl; + SendSDO(0x2002, vel); // velocity + WaitForSdo(0x2002, 0); +} + +void Dkc::SetVelocityRel(Double_t vel) +{ + gLog << dbg << "- Setting velocity to: " << vel*100 << "%" << endl; + SendSDO(0x2002, (LWORD_t)(vel*fVelMax+0.5)); // velocity + WaitForSdo(0x2002, 0); +} + +void Dkc::SetAcceleration(LWORD_t acc) +{ + gLog << dbg << "- Setting acceleration to: " << acc << endl; + SendSDO(0x2003, 0, acc); // acceleration + WaitForSdo(0x2003, 0); +} + +void Dkc::SetDeceleration(LWORD_t dec) +{ + gLog << dbg << "- Setting deceleration to: " << dec << endl; + SendSDO(0x2003, 1, dec); + WaitForSdo(0x2003, 1); +} + +void Dkc::SetRpmMode(BYTE_t mode) +{ + // + // SetRpmMode(FALSE) stop the motor, but lets the position control unit on + // + SendSDO(0x3006, 0, mode ? string('s','t','r','t') : string('s','t','o','p')); + WaitForSdo(0x3006, 0); +} + +void Dkc::SetRpmVelocity(LWORDS_t cvel) +{ + SendSDO(0x3006, 1, (LWORD_t)cvel); + WaitForSdo(0x3006, 1); +} + +void Dkc::StartRelPos(LWORDS_t pos) +{ + if (!fArmed) + { + gLog << err << GetNodeName() << ": ERROR - Moving without being armed is not allowed." << endl; + SetZombie(); + return; + } + + gLog << dbg << GetNodeName() << ": Starting relative positioning by " << (LWORDS_t)pos << " ticks." << endl; + SendSDO(0x6004, 1, (LWORD_t)pos); + fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO +} + +void Dkc::StartAbsPos(LWORDS_t pos) +{ + if (!fArmed) + { + gLog << err << GetNodeName() << ": ERROR - Moving without being armed is not allowed." << endl; + SetZombie(); + return; + } + + gLog << dbg << GetNodeName() << ": Starting absolute positioning to " << (LWORDS_t)pos << " ticks." << endl; + SendSDO(0x6004, 0, (LWORD_t)pos); + fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO +} + +void Dkc::StartAbsPosRev(Double_t pos) +{ + if (!fArmed) + { + gLog << err << GetNodeName() << ": ERROR - Moving without being armed is not allowed." << endl; + SetZombie(); + return; + } + + const LWORD_t p = (LWORD_t)(pos*fPosRes+.5); + + gLog << dbg << GetNodeName() << ": Starting absolute positioning to " << p << " ticks." << endl; + SendSDO(0x6004, 0, p); + fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO +} + +void Dkc::StartVelSync() +{ + // + // The syncronization mode is disabled by a 'MOTOR STOP' + // or by a positioning command (POSA, ...) + // + gLog << inf2 << "- " << GetNodeName() << ": Starting RPM Sync Mode." << endl; + SendSDO(0x3007, 0, string('s', 'y', 'n', 'c')); + WaitForSdo(0x3007, 0); +} + +void Dkc::StartPosSync() +{ + // + // The syncronization mode is disabled by a 'MOTOR STOP' + // or by a positioning command (POSA, ...) + // + gLog << inf2 << "- " << GetNodeName() << ": Starting Position Sync Mode." << endl; + SendSDO(0x3007, 1, string('s', 'y', 'n', 'c')); + WaitForSdo(0x3007, 1); +} + +void Dkc::SendMsg(BYTE_t data[6]) +{ + GetNetwork()->SendCanFrame(fMacId, 0, 0, data[0], data[1], data[2], data[3], data[4], data[5]); +} + +void Dkc::SendMsg(BYTE_t d0, BYTE_t d1, BYTE_t d2, + BYTE_t d3, BYTE_t d4, BYTE_t d5) +{ + GetNetwork()->SendCanFrame(fMacId, 0, 0, d0, d1, d2, d3, d4, d5); +} + +void Dkc::HandlePDO1(const BYTE_t *data, const timeval_t &tv) +{ + // FIXME!!!! Only 0x4000 should do this to be + // CanOpen conform + //HandleNodeguard(tv); + + fPdoPos1 = (data[3]<<24) | (data[2]<<16) | (data[1]<<8) | data[0]; + fPdoTime1.Set(tv); + fHasChangedPos1 = true; + + fPdoPos2 = (data[7]<<24) | (data[6]<<16) | (data[5]<<8) | data[4]; + fPdoTime2.Set(tv); + fHasChangedPos2 = true; + + if (fReport) + { + fReport->Lock("ShaftEncoder::HandlePDOType0"); + *fReport << "FEEDBACK " << (int)GetId() << " " << fPdoTime1 << " PDO0 " << GetNodeName() << " " << fPdoPos1 << " " << fPdoPos2 << endl; + fReport->UnLock("ShaftEncoder::HandlePDOType0"); + } +} + +void Dkc::HandlePDO3(const BYTE_t *data, const timeval_t &tv) +{ + fArmed = data[2]&0x01; + + if (fStatus!=data[3]) + { + // data[3]&0x01; // motor not moving + fPosActive = data[3]&0x02; // positioning active + fRpmActive = data[3]&0x04; // RPM mode switched on + // data[3]&0x08; // - unused - + // data[3]&0x10; // - unused - + // data[3]&0x20; // - unused - + //fInControl = data[3]&0x40; // motor uncontrolled + // data[3]&0x80; // axis resetted (after errclr, motor stop, motor on) + + fStatus = data[3]; + } + + if (fStatusPdo3!=data[0]) + { + gLog << inf << MTime(-1) << ": " << GetNodeName() << " - PDO1 = "; + const Bool_t ready = data[0]&0x01; + const Bool_t fuse = data[0]&0x02; + const Bool_t emcy = data[0]&0x04; + const Bool_t vltg = data[0]&0x08; + const Bool_t mode = data[0]&0x10; + const Bool_t rf = data[0]&0x20; + const Bool_t brake = data[0]&0x40; + const Bool_t power = data[0]&0x80; + if (ready) gLog << "DKC-Ready "; + if (fuse) gLog << "FuseOk "; + if (emcy) gLog << "EmcyOk "; + if (vltg) gLog << "OvervoltOk "; + if (mode) gLog << "SwitchToManualMode "; + if (rf) gLog << "RF "; + if (brake) gLog << "BrakeOpen "; + if (power) gLog << "PowerOn "; + gLog << endl; + + fStatusPdo3 = data[0]; + } +} + +void Dkc::CheckErrorDKC(LWORD_t val) +{ + fStatusDKC = val; + + const Bool_t rc = PrintStatus(val); + SetError(rc ? 0 : val); + if (!rc) + SetZombie(); +} + +void Dkc::HandlePDO2(const BYTE_t *data, const timeval_t &tv) +{ + LWORDS_t errnum = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3]; + LWORDS_t errinf = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7]; + + // Check if the DKC changed its status message + if (errnum==0xff && (errinf&0xf000)<=0xe000) + { + CheckErrorDKC(errnum, errinf); + return; + } + + // Check if MACS report error occursion. + // errnum==0 gives a sudden information that something happened. Now the + // microcontroller is running inside its interrupt procedure which + // stopped the normal program. The interrupt procedure should try to clear + // the error state of the hardware. This should never create a new error! + // + if (!errnum) + { + gLog << err << "- " << GetNodeName() << ": reports Error occursion." << endl; + gLog << "Dkc::HandlePDO2: " << GetNodeName() << " --> ZOMBIE!" << endl; + SetZombie(); + SetError(-1); + return; + } + + // + // Now the error is handled by the hardware now it is the software part + // to react on it. The Error flag now is set to the correct value. + // + if (GetError()>0) + { + gLog << warn << GetNodeName() << ": WARNING! Previous error #" << GetError() << " unhandled (not cleared) by software." << endl; + + // + // If the error is unhadled and/or not cleared, don't try it again. + // + if (GetError()==errnum) + return; + } + + SetError(errnum); + + gLog << err << GetNodeName() << " reports: "; + switch (errnum) + { + case 0xff: + EvalStatus(errnum, errinf); + return; + + default: + gLog << "Error Nr. " << dec << errnum << ", " << errinf << endl; + } +} + +// FIXME? Handling of fIsZombie? +void Dkc::HandleError() +{ + // + // If there is no error we must not handle anything + // + if (!HasError()) + return; + + // + // If the program got into the: HandleError state before the hardware + // has finished handeling the error we have to wait for the hardware + // handeling the error + // + // FIXME: Timeout??? + // +// while (GetError()<0) +// usleep(1); + + // + // After this software and hardware should be in a state so that + // we can go on working 'as usual' Eg. Initialize a Display Update + // + gLog << inf << GetNodeName() << " Handling Error #" << dec << GetError() << endl; + switch (GetError()) + { + case 0xff: + gLog << err << "DKC error! Go and check what is going on!" << endl; + return; + + default: + gLog << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl; + + } +} + +void Dkc::DisplayVal() +{ + const LWORDS_t pos = GetPdoPos2(); + if (IsZombieNode()) + { + if (fLabel) + fLabel->SetText(new TGString("")); + fUpdPos = ~pos; + return; + } + + char text[21]=""; + if (pos!=fUpdPos && fLabel) + { + sprintf(text, "%ld", pos); + fLabel->SetText(new TGString(text)); + fUpdPos = pos; + } +} Index: /trunk/MagicSoft/Cosy/devdrv/dkc.h =================================================================== --- /trunk/MagicSoft/Cosy/devdrv/dkc.h (revision 9132) +++ /trunk/MagicSoft/Cosy/devdrv/dkc.h (revision 9132) @@ -0,0 +1,149 @@ +#ifndef COSY_Dkc +#define COSY_Dkc + +#ifndef COSY_NodeDrv +#include "nodedrv.h" +#endif + +#ifndef MARS_MTime +#include "MTime.h" +#endif + +class TGLabel; + +class Dkc : public NodeDrv +{ +private: + BYTE_t fMacId; + + LWORD_t fSoftVersion; + + LWORD_t fPosRes; + LWORD_t fVelMax; + LWORD_t fVelRes; + LWORDS_t fVel; + + LWORDS_t fPdoPos1; + LWORDS_t fPdoPos2; + MTime fPdoTime1; + MTime fPdoTime2; + + BYTE_t fPosActive; + BYTE_t fRpmActive; + BYTE_t fStatus; + LWORD_t fStatusDKC; + BYTE_t fStatusPdo3; + + bool fHasChangedPos1; //! + bool fHasChangedPos2; //! + + bool fArmed; + + MLog *fReport; + + TGLabel *fLabel; // + LWORDS_t fUpdPos; // ticks + + void Init(); + void StopDevice(); + + TString EvalStatusDKC(UInt_t stat) const; + Bool_t PrintStatus(LWORD_t val) const; + Bool_t EvalStatus(UInt_t errnum, UInt_t errinf) const + { + return PrintStatus(errnum|(errinf<<16)); + } + TString GetStatus(LWORD_t val) const; + void CheckErrorDKC(LWORD_t val); + void CheckErrorDKC(UInt_t errnum, UInt_t errinf) + { + CheckErrorDKC(errnum|(errinf<<16)); + } + + + void HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, const timeval_t &tv); + void HandleSDOOK(WORD_t idx, BYTE_t subidx, LWORD_t data, const timeval_t &tv); + + void HandlePDO1(const BYTE_t *data, const timeval_t &tv); + void HandlePDO2(const BYTE_t *data, const timeval_t &tv); + void HandlePDO3(const BYTE_t *data, const timeval_t &tv); + + void CheckConnection(); + +public: + Dkc(const BYTE_t nodeid, const char *name=NULL); + virtual ~Dkc(); + + static LWORD_t string(BYTE_t b0=0, BYTE_t b1=0, BYTE_t b2=0, BYTE_t b3=0) + { + return (LWORD_t)(b0<<24 | b1<<16 | b2<<8 | b3); + } + + void SendMsg(BYTE_t data[6]); + void SendMsg(BYTE_t d0=0, BYTE_t d1=0, BYTE_t d2=0, + BYTE_t d3=0, BYTE_t d4=0, BYTE_t d5=0); + + void ReqPos1(); + void ReqPos2(); + void ReqVel(); + void ReqVelRes(); + void ReqPosRes(); + void ReqVelMax(); + void SetAcceleration(LWORD_t acc); + void SetDeceleration(LWORD_t dec); + void SetVelocity(LWORD_t vel); + void SetVelocityRel(Double_t vel); + void SetRpmMode(BYTE_t mode=TRUE); + void SetRpmVelocity(LWORDS_t cvel); + void SetPDO1On(BYTE_t flag=TRUE); + + void StartVelSync(); + void StartPosSync(); + + void StartRelPos(LWORDS_t pos); + void StartAbsPos(LWORDS_t pos); + void StartAbsPosRev(Double_t pos); + + void StartNode(); + + bool IsArmed() const { return fArmed; } + void Arm(); + void Disarm(); + + int IsPositioning() const { return fPosActive; } + BYTE_t GetStatus() const { return fStatus; } + BYTE_t GetStatusPdo3() const { return fStatusPdo3; } + + LWORDS_t GetPdoPos1() const { return fPdoPos1; } + LWORDS_t GetPdoPos2() const { return fPdoPos2; } + + LWORDS_t GetVel() const { return fVel; } + LWORD_t GetVelMax() const { return fVelMax; } // Velocity units (would be 100 for %) + Double_t GetVelMaxRev() const { return (Double_t)fVelMax/fVelRes; } // Velocity units (would be 100 for %) + LWORD_t GetVelRes() const { return fVelRes; } // Velocity units (would be 100 for %) + LWORD_t GetPosRes() const { return fPosRes; } // Velocity units (would be 100 for %) + + bool IsOperative() const { return fStatusPdo3==0xef; } + + void HandleError(); + + Double_t GetMjdPos1() const { return fPdoTime1.GetMjd(); } + Double_t GetMjdPos2() const { return fPdoTime2.GetMjd(); } + + bool HasChangedPos1() const { return fHasChangedPos1; } + bool HasChangedPos2() const { return fHasChangedPos2; } + + void ResetHasChangedPos1() { fHasChangedPos1 = false; } + void ResetHasChangedPos2() { fHasChangedPos2 = false; } + + TString GetStatusDKC() const { return GetStatus(fStatusDKC); } + + void SetReport(MLog *log) { fReport = log; } + void SetDisplay(TGLabel *label) { fLabel = label; } + + void DisplayVal(); + + ClassDef(Dkc, 0) +}; + +#endif Index: /trunk/MagicSoft/Cosy/gui/MGCosy.cc =================================================================== --- /trunk/MagicSoft/Cosy/gui/MGCosy.cc (revision 9131) +++ /trunk/MagicSoft/Cosy/gui/MGCosy.cc (revision 9132) @@ -131,5 +131,5 @@ { const int x = 180; - const int y = 25; + const int y = 20; TGLabel *l; @@ -150,72 +150,47 @@ fList->Add(fMjd); - l = new TGLabel(f, "SE-Az:"); + l = new TGLabel(f, "Encoder-Az:"); + l->Move(x-60, y+57); + fList->Add(l); + + l = new TGLabel(f, "Encoder-Zd:"); l->Move(x-60, y+40); fList->Add(l); - l = new TGLabel(f, "SE-Zd1:"); - l->Move(x-60, y+57); - fList->Add(l); - - l = new TGLabel(f, "SE-Zd2:"); - l->Move(x-60, y+74); - fList->Add(l); - fWeather = new TGLabel(f, ""); - fWeather->MoveResize(10, y+280, 280, 16); + fWeather->MoveResize(10, y+272, 280, 16); fList->Add(fWeather); - fLabel1 = new TGLabel(f, "00000000"); // Max: 16384 - fLabel2 = new TGLabel(f, "00000000"); - fLabel3 = new TGLabel(f, "00000000"); + fLabel1 = new TGLabel(f, "0000000000"); + fLabel2 = new TGLabel(f, "0000000000"); fLabel1->SetTextJustify(kTextRight); fLabel2->SetTextJustify(kTextRight); - fLabel3->SetTextJustify(kTextRight); - fLabel1->Move(x, y+40); - fLabel2->Move(x, y+57); - fLabel3->Move(x, y+74); + fLabel1->Move(x+30, y+57); + fLabel2->Move(x+30, y+40); fList->Add(fLabel1); fList->Add(fLabel2); - fList->Add(fLabel3); -/* - l = new TGLabel(f, "Offset-Zd:"); - l->Move(x-60, y+102); + + l = new TGLabel(f, "Ra (estimated):"); + l->Move(x-60, y+57+23); fList->Add(l); - l = new TGLabel(f, "Offset-Az:"); - l->Move(x-60, y+119); + l = new TGLabel(f, "Ra (nominal):"); + l->Move(x-60, y+57+40); fList->Add(l); - fOffsetZd = new TGLabel(f, "000000000"); - fOffsetAz = new TGLabel(f, "000000000"); - fOffsetZd->SetTextJustify(kTextRight); - fOffsetAz->SetTextJustify(kTextRight); - fOffsetZd->Move(x+15, y+102); - fOffsetAz->Move(x+15, y+119); - fList->Add(fOffsetZd); - fList->Add(fOffsetAz); -*/ - l = new TGLabel(f, "Ra (estimated):"); - l->Move(x-60, y+142-20); + l = new TGLabel(f, "Dec (estimated):"); + l->Move(x-60, y+57+40+23); fList->Add(l); - l = new TGLabel(f, "Ra (nominal):"); - l->Move(x-60, y+159-20); + l = new TGLabel(f, "Dec (nominal):"); + l->Move(x-60, y+57+40+40); fList->Add(l); - l = new TGLabel(f, "Dec (estimated):"); - l->Move(x-60, y+182-20); + l = new TGLabel(f, "Zd (nominal):"); + l->Move(x-60, y+57+80+23); fList->Add(l); - l = new TGLabel(f, "Dec (nominal):"); - l->Move(x-60, y+199-20); - fList->Add(l); - - l = new TGLabel(f, "Zd (nominal):"); - l->Move(x-60, y+222); - fList->Add(l); - l = new TGLabel(f, "Az (nominal):"); - l->Move(x-60, y+239); + l->Move(x-60, y+57+80+40); fList->Add(l); @@ -232,10 +207,10 @@ fZdSoll->SetTextJustify(kTextRight); fAzSoll->SetTextJustify(kTextRight); - fRaEst->Move(x+30, y+142-20); - fRaSoll->Move(x+30, y+159-20); - fDecEst->Move(x+30, y+182-20); - fDecSoll->Move(x+30, y+199-20); - fZdSoll->Move(x+30, y+222); - fAzSoll->Move(x+30, y+239); + fRaEst->Move(x+30, y+57+23); + fRaSoll->Move(x+30, y+57+40); + fDecEst->Move(x+30, y+57+40+23); + fDecSoll->Move(x+30, y+57+40+40); + fZdSoll->Move(x+30, y+57+80+23); + fAzSoll->Move(x+30, y+57+80+40); fList->Add(fRaEst); fList->Add(fDecEst); @@ -251,10 +226,19 @@ fStopping = new TGLabel(f, "Stopping"); fStopped = new TGLabel(f, "Stopped"); - fAvailMac1 = new TGLabel(f, "- MACS1 -"); - fAvailMac2 = new TGLabel(f, "- MACS2 -"); - //fAvailMac3 = new TGLabel(f, "- MAC3 -"); - fAvailSe1 = new TGLabel(f, "-SE/Zd1-"); - fAvailSe2 = new TGLabel(f, "-SE/Zd2-"); - fAvailSe3 = new TGLabel(f, "- SE/Az -"); + //fAvailDkc1 = new TGLabel(f, "- DKC/Zd -"); + //fAvailDkc2 = new TGLabel(f, "- DKC/Az -"); + fAvailTcp = new TGLabel(f, "- TCP/IP -"); + + fDkcReady1 = new TGLabel(f, "bb"); + fDkcReady2 = new TGLabel(f, "bb"); + fDkcEmergency = new TGLabel(f, "Emergency"); + fDkcPcMode = new TGLabel(f, "PC mode"); + fDkcRF1 = new TGLabel(f, "AF"); + fDkcRF2 = new TGLabel(f, "AF"); + fDkcBrake = new TGLabel(f, "Zd Brake"); + fDkcPower = new TGLabel(f, "Power on"); + + fMsgAz = new TGLabel(f, " xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx"); + fMsgZd = new TGLabel(f, " xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx"); ULong_t color; @@ -264,10 +248,13 @@ gClient->GetColorByName("Red", color); fError->SetBackgroundColor(color); - fAvailMac1->SetBackgroundColor(color); - fAvailMac2->SetBackgroundColor(color); - //fAvailMac3->SetBackgroundColor(color); - fAvailSe1->SetBackgroundColor(color); - fAvailSe2->SetBackgroundColor(color); - fAvailSe3->SetBackgroundColor(color); + fDkcReady1->SetBackgroundColor(color); + fDkcReady2->SetBackgroundColor(color); + fDkcEmergency->SetBackgroundColor(color); + fDkcPcMode->SetBackgroundColor(color); + fDkcRF1->SetBackgroundColor(color); + fDkcRF2->SetBackgroundColor(color); + fDkcBrake->SetBackgroundColor(color); + fDkcPower->SetBackgroundColor(color); + fAvailTcp->SetBackgroundColor(color); gClient->GetColorByName("LightBlue", color); fMoving->SetBackgroundColor(color); @@ -279,16 +266,32 @@ fStopped->SetBackgroundColor(color); - fArmed ->MoveResize(10, 25, 70, 20); - fError ->MoveResize(10, 25+25, 70, 20); - fMoving ->MoveResize(10, 25+25+20, 70, 20); - fTracking->MoveResize(10, 25+25+40, 70, 20); - fStopping->MoveResize(10, 25+25+60, 70, 20); - fStopped ->MoveResize(10, 25+25+80, 70, 20); - fAvailMac1->MoveResize(10, 25+25+120, 70, 20); - fAvailMac2->MoveResize(10, 25+25+140, 70, 20); - //fAvailMac3->Move(10, 25+160); - fAvailSe1->MoveResize(10, 25+25+200, 70, 20); - fAvailSe2->MoveResize(10, 25+25+220, 70, 20); - fAvailSe3->MoveResize(10, 25+25+180, 70, 20); + gClient->GetColorByName("White", color); + fMsgAz->SetBackgroundColor(color); + fMsgZd->SetBackgroundColor(color); + + fArmed ->MoveResize(10, 20, 70, 20); + fError ->MoveResize(10, 20+25, 70, 20); + fMoving ->MoveResize(10, 20+25+20, 70, 20); + fTracking->MoveResize(10, 20+25+40, 70, 20); + fStopping->MoveResize(10, 20+25+60, 70, 20); + fStopped ->MoveResize(10, 20+25+80, 70, 20); + + fAvailTcp ->MoveResize( 10, 80+20+102, 75, 20); + + fDkcEmergency->MoveResize( 10, 80+20+124, 75, 20); + fDkcPcMode ->MoveResize( 89, 80+20+124, 75, 20); + fDkcPower ->MoveResize(168, 80+20+124, 75, 20); + fDkcBrake ->MoveResize(247, 80+20+124, 75, 20); + + fDkcReady1->MoveResize( 10, 248+2, 16, 16); + fDkcReady2->MoveResize( 10, 268+0, 16, 16); + fDkcRF1 ->MoveResize( 10+18, 248+2, 18, 16); + fDkcRF2 ->MoveResize( 10+18, 268+0, 18, 16); + + fMsgAz->MoveResize(10+38, 248+2, 310-34, 16); // x-95 + fMsgZd->MoveResize(10+38, 268+0, 310-34, 16); // x-95 + + fMsgAz->SetTextJustify(kTextLeft); + fMsgZd->SetTextJustify(kTextLeft); fList->Add(fArmed); @@ -298,10 +301,19 @@ fList->Add(fStopping); fList->Add(fStopped); - fList->Add(fAvailMac1); - fList->Add(fAvailMac2); - //fList->Add(fAvailMac3); - fList->Add(fAvailSe1); - fList->Add(fAvailSe2); - fList->Add(fAvailSe3); + //fList->Add(fAvailDkc1); + //fList->Add(fAvailDkc2); + fList->Add(fAvailTcp); + + fList->Add(fDkcReady1 ); + fList->Add(fDkcReady2 ); + fList->Add(fDkcEmergency); + fList->Add(fDkcPcMode ); + fList->Add(fDkcRF1 ); + fList->Add(fDkcRF2 ); + fList->Add(fDkcBrake ); + fList->Add(fDkcPower ); + + fList->Add(fMsgZd); + fList->Add(fMsgAz); } @@ -716,6 +728,6 @@ } -MGCosy::MGCosy(MObservatory::LocationName_t key, MsgQueue *q, const TGWindow *p, UInt_t w, UInt_t h) - : TGMainFrame(p, w, h), fObservatory(key), fQueue(q) +MGCosy::MGCosy(MObservatory::LocationName_t key, MsgQueue *q) + : TGMainFrame(gClient->GetRoot(), 1, 1), fObservatory(key), fQueue(q) { // This means that all objects added with AddFrame are deleted @@ -806,9 +818,10 @@ CreateLabel(frame); + // // Map the window, set up the layout, etc. // const Int_t ww = 1010; - const Int_t hh = 700; + const Int_t hh = 680; SetWMSizeHints(ww, hh, ww, hh, 1, 1); // set the smallest and biggest size of the Main frame @@ -1038,6 +1051,6 @@ } -void MGCosy::Update(ZdAz pos, ZdAz acc, ZdAz vel, /*ZdAz off,*/ RaDec radec, - ZdAz soll, UInt_t stat, UInt_t stat2, Bool_t armed) +void MGCosy::Update(ZdAz pos, ZdAz acc, /*ZdAz vel, ZdAz off,*/ RaDec radec, + ZdAz soll, UInt_t stat, UInt_t pdo3, UInt_t stat2, Bool_t armed, const TString &statzd, const TString &stataz) { // acc [rad] @@ -1060,12 +1073,23 @@ EnableLabel(fAzSoll, stat&MDriveCom::kMoving); - SetLabelColor(fAvailMac1, stat2&0x01); - SetLabelColor(fAvailMac2, stat2&0x02); - //SetLabelColor(fAvailMac3, stat2&0x04); - SetLabelColor(fAvailSe1, stat2&0x08); - SetLabelColor(fAvailSe2, stat2&0x10); - SetLabelColor(fAvailSe3, stat2&0x20); - - EnableLabel(fArmed, armed);//stat2&0x40); + SetLabelColor(fAvailTcp, stat2); + + if (!stat2) + pdo3 = 0; + + SetLabelColor(fDkcReady1, pdo3&0x0001); + SetLabelColor(fDkcReady2, pdo3&0x0100); + SetLabelColor(fDkcEmergency, (pdo3&0x0404)==0x0404); + SetLabelColor(fDkcPcMode, (pdo3&0x1010)!=0x1010 && pdo3); + SetLabelColor(fDkcRF1, pdo3&0x0020); + SetLabelColor(fDkcRF2, pdo3&0x2000); + SetLabelColor(fDkcBrake, (pdo3&0x4040)==0x4040); + SetLabelColor(fDkcPower, (pdo3&0x8080)==0x8080); + + EnableLabel(fArmed, armed); + + fMsgZd->SetText(new TGString(TString(" Zd: ")+statzd)); + fMsgAz->SetText(new TGString(TString(" Az: ")+stataz)); + if (stat&MDriveCom::kTracking) Index: /trunk/MagicSoft/Cosy/gui/MGCosy.h =================================================================== --- /trunk/MagicSoft/Cosy/gui/MGCosy.h (revision 9131) +++ /trunk/MagicSoft/Cosy/gui/MGCosy.h (revision 9132) @@ -49,5 +49,5 @@ TGLabel *fLabel1; TGLabel *fLabel2; - TGLabel *fLabel3; + // TGLabel *fLabel3; // TGLabel *fOffsetZd; @@ -62,4 +62,7 @@ TGLabel *fZdSoll; TGLabel *fAzSoll; + + TGLabel *fMsgZd; + TGLabel *fMsgAz; TGLabel *fUtc; @@ -77,5 +80,4 @@ MGSkyPosition *fSkyPosition; MGAccuracy *fAccuracy; - //MGAccuracyTime *fAccuracyTime; MGVelocity *fVelocity; MGVelocity *fOffset; @@ -93,10 +95,16 @@ TGLabel *fStopping; TGLabel *fStopped; - TGLabel *fAvailMac1; - TGLabel *fAvailMac2; - TGLabel *fAvailMac3; - TGLabel *fAvailSe1; - TGLabel *fAvailSe2; - TGLabel *fAvailSe3; + //TGLabel *fAvailDkc1; + //TGLabel *fAvailDkc2; + TGLabel *fAvailTcp; + + TGLabel *fDkcReady1; + TGLabel *fDkcReady2; + TGLabel *fDkcEmergency; + TGLabel *fDkcPcMode; + TGLabel *fDkcRF1; + TGLabel *fDkcRF2; + TGLabel *fDkcBrake; + TGLabel *fDkcPower; TGTextView *fLog; @@ -125,5 +133,5 @@ public: - MGCosy(MObservatory::LocationName_t key, MsgQueue *q, const TGWindow *p, UInt_t w, UInt_t h); + MGCosy(MObservatory::LocationName_t key, MsgQueue *q); ~MGCosy(); @@ -132,10 +140,10 @@ TGLabel *GetLabel1() const { return fLabel1; } TGLabel *GetLabel2() const { return fLabel2; } - TGLabel *GetLabel3() const { return fLabel3; } + //TGLabel *GetLabel3() const { return fLabel3; } TGTextView *GetLog() const { return fLog; } MGImage *GetImage() { return fImage; } - void Update(ZdAz pos, ZdAz acc, ZdAz vel, /*ZdAz off,*/ RaDec radec, ZdAz soll, UInt_t stat, UInt_t stat2, Bool_t armed); + void Update(ZdAz pos, ZdAz acc, /*ZdAz vel, ZdAz off,*/ RaDec radec, ZdAz soll, UInt_t stat, UInt_t pdo3, UInt_t stat2, Bool_t armed, const TString &statzd, const TString &stataz); void UpdateWeather(const MCeCoCom &com); Index: /trunk/MagicSoft/Cosy/gui/MGSkyPosition.cc =================================================================== --- /trunk/MagicSoft/Cosy/gui/MGSkyPosition.cc (revision 9131) +++ /trunk/MagicSoft/Cosy/gui/MGSkyPosition.cc (revision 9132) @@ -74,5 +74,5 @@ InitArc(fSaturn, 1001, kYellow/*17*/, 16); - for (int i=0; i<14; i++) + for (int i=0; i<15; i++) { fStars[i] = new TArc(0, 0, 1); @@ -115,5 +115,5 @@ } -static const RaDec stars[14] = +static const RaDec stars[15] = { // 32349 Sirius -1.44 1.45 2.64 0.009 A0m @@ -138,10 +138,12 @@ (RaDec(MAstro::Hms2Rad( 5, 16, 0), MAstro::Dms2Rad( 46, 00, 0))), (RaDec(MAstro::Hms2Rad( 7, 39, 0), MAstro::Dms2Rad( 5, 14, 0))), - (RaDec(MAstro::Hms2Rad( 5, 55, 0), MAstro::Dms2Rad( 7, 24, 0))) + (RaDec(MAstro::Hms2Rad( 5, 55, 0), MAstro::Dms2Rad( 7, 24, 0))), + // Polaris 2.02 + (RaDec(MAstro::Hms2Rad( 2, 31, 49), MAstro::Dms2Rad( 89, 15, 51))), }; void MGSkyPosition::UpdateStars() { - for (int i=0; i<14; i++) + for (int i=0; i<15; i++) SetDot(fStars[i], stars[i], 0); } @@ -370,4 +372,6 @@ fSlaStar->SetMjd(mjd); + UpdateStars(); + UpdatePlanet(kESun, fSun); UpdatePlanet(kEMoon, fMoon); @@ -377,6 +381,4 @@ UpdatePlanet(kESaturn, fSaturn); - UpdateStars(); - RaDec radec = fSlaStar->CalcRaDecFast(pos*TMath::DegToRad()); UpdatePosition(radec, pos.Zd(), pos.Az()); Index: /trunk/MagicSoft/Cosy/gui/MGSkyPosition.h =================================================================== --- /trunk/MagicSoft/Cosy/gui/MGSkyPosition.h (revision 9131) +++ /trunk/MagicSoft/Cosy/gui/MGSkyPosition.h (revision 9132) @@ -35,5 +35,5 @@ TArc *fSaturn; - TArc *fStars[14]; + TArc *fStars[15]; TArc *fSaturnRing; Index: /trunk/MagicSoft/Cosy/main/MCosy.cc =================================================================== --- /trunk/MagicSoft/Cosy/main/MCosy.cc (revision 9131) +++ /trunk/MagicSoft/Cosy/main/MCosy.cc (revision 9132) @@ -7,9 +7,4 @@ #include #include - -#include -#include -#include -#include //#include "MLog.h" @@ -26,6 +21,5 @@ #include "MTracking.h" -#include "macs.h" -#include "shaftencoder.h" +#include "dkc.h" ClassImp(MCosy); @@ -34,9 +28,4 @@ typedef struct tm tm_t; - -/* +===================================+ - FIXME: What if fMac3 (Sync) died? - +===================================+ -*/ //#define EXPERT @@ -114,12 +103,4 @@ } */ - -MTTalk::~MTTalk() -{ - gLog << inf2 << "~MTTalk::CancelThread" << std::endl; - CancelThread(); - gLog << inf2 << "~MTTalk::MTTalk" << std::endl; -} - // -------------------------------------------------------------------------- // @@ -136,54 +117,8 @@ ZdAz MCosy::GetSePos() const { - const int pa = fAz->GetPos(); - if (fZd1->IsZombieNode() && fZd2->IsZombieNode()) - return ZdAz(0, pa); - - // - // Get the values (FIXME!) - // - //int p1 = (fZd1->GetPos()+fZd1->GetPhysRes()/2)%fZd1->GetPhysRes(); - //int p2 = -(fZd2->GetPos()+fZd2->GetPhysRes()/2)%fZd2->GetPhysRes(); - - int p1 = fZd1->GetPos();//+fZd1->GetPhysRes()/2)%fZd1->GetPhysRes(); - int p2 = -fZd2->GetPos();//+fZd2->GetPhysRes()/2)%fZd2->GetPhysRes(); - - if (fZd1->IsZombieNode()) - return ZdAz(p2, pa); - if (fZd2->IsZombieNode()) - return ZdAz(p1, pa); - - // - // interpolate shaft encoder positions - // - float p = (float)(p1+p2)/2; - - return ZdAz(p, pa); -} - -// -------------------------------------------------------------------------- -// -// reads the Rotary encoder positions from the last request of the Macs. -// -// The positions are returned as a ZdAz object. Use RequestRePos to request -// the current positions first. -// -ZdAz MCosy::GetRePos() -{ - return ZdAz(fMac2->GetPos(), fMac1->GetPos()); -} - -// -------------------------------------------------------------------------- -// -// reads the Rotary encoder positions from the Macs. -// -// The positions are returned as a ZdAz object. The positions are the ones -// which are send as PDOs to the computer. This is done at a given -// frequency. Which means, that this positions are not ought to be -// up-to-date. -// -ZdAz MCosy::GetRePosPdo() -{ - return ZdAz(fMac2->GetPdoPos(), fMac1->GetPdoPos()); + const Double_t pa = fMac1 ? (Double_t)fMac1->GetPdoPos2()/fMac1->GetPosRes() : 0; + const Double_t p1 = fMac2 ? (Double_t)fMac2->GetPdoPos2()/fMac2->GetPosRes() : 0; + + return ZdAz(p1, pa); } @@ -323,5 +258,5 @@ } - const ZdAz se = GetSePos()*TMath::TwoPi()/kResSE; // [rad] + const ZdAz se = GetSePos()*TMath::TwoPi(); // [rad] const ZdAz unbendedse = fBending.CorrectBack(se)*TMath::RadToDeg(); // ist pointing @@ -350,4 +285,5 @@ } +/* Double_t MCosy::Starguider(Double_t mjd, ZdAz &dest) const { @@ -365,12 +301,4 @@ ZdAz point=AlignTrackingPos(ZdAz(zd, az)/TMath::RadToDeg()); - /* - if (!AlignTrackingPos(ZdAz(zd, az), point)) - { - cout << "Starguider position couldn't be aligned..." << endl; - return -1; - }*/ - - // FIXME: Check Range missing! const ZdAz diff = (dest-point)*TMath::RadToDeg(); @@ -389,4 +317,5 @@ return (mjd-mjd0) * (24*60*60); // [s] } +*/ // -------------------------------------------------------------------------- @@ -405,32 +334,9 @@ MSlewing point(this); -//#ifdef EXPERT -// point.SetPointAccDec(0.25, 0.25); -// point.SetPointVelocity(0.4); -//#else - -// original settings -// - point.SetPointAccDec(0.2, 0.1); - point.SetPointVelocity(0.1); - - -//#endif - -// original + // Default: point.SetPointAcc(0.03, 0.01); + point.SetPointAcc(0.03, 0.01); + point.SetPointVelocity(0.3); + return point.SetPosition(dst, track); - -// test -// return point.SetPosition(dst, kTRUE); -} - -void MCosy::SetTrackingPosRE(ZdAz za) -{ - za /= kGearTot; // [U_tel] - za *= TMath::TwoPi(); // [rad] - //cout << "RE1: " << za.Zd()*180/3.1415 << " " << za.Az()*180/3.1415 << endl; - fTrackingPosRaw = za*TMath::RadToDeg(); - fTrackingPos = fBending.CorrectBack(za)*TMath::RadToDeg(); - //cout << "RE2: " << fTrackingPos.Zd() << " " << fTrackingPos.Az() << endl; } @@ -439,21 +345,8 @@ MTracking track(this); track.SetOut(fOutRep); -//#ifdef EXPERT -// track.SetPointAccDec(0.4, 0.4); -// track.SetPointVelocity(0.2); // fast: 0.6, slow: 0.2 -//#else - - - - track.SetPointAccDec(0.2, 0.1); - track.SetPointVelocity(0.1); - - // track.SetPointAccDec(0.4, 0.4); - // track.SetPointVelocity(0.4); - - - -//#endif - track.SetTrackAccDec(0.1, 0.1); + + track.SetPointAcc(0.03, 0.01); + track.SetPointVelocity(0.3); + track.SetTrackAcc(0.01, 0.01); track.TrackPosition(dst); @@ -462,14 +355,12 @@ void MCosy::TrackPositionGRB(const RaDec &dst) // ra, dec [rad] { + TrackPosition(dst); + return; + MTracking track(this); track.SetOut(fOutRep); -//#ifdef EXPERT -// track.SetPointAccDec(0.4, 0.4); -// track.SetPointVelocity(0.2); // fast: 0.6, slow: 0.2 -//#else - track.SetPointAccDec(0.4, 0.4); - track.SetPointVelocity(0.3); -//#endif - track.SetTrackAccDec(0.1, 0.1); + track.SetPointAcc(0.09, 0.03); + track.SetPointVelocity(1.0); + track.SetTrackAcc(0.01, 0.01); track.TrackPosition(dst); @@ -493,29 +384,14 @@ // set deceleration to 50% // - gLog << inf2 << "Stopping movement (dec=30%)..." << endl; + gLog << inf2 << "Stopping movement..." << endl; if (fMac1 && fMac2) { -#ifdef EXPERT - fMac1->SetDeceleration(TMath::Nint(0.5*fMac1->GetVelRes())); - fMac2->SetDeceleration(TMath::Nint(0.5*fMac2->GetVelRes())); -#else - fMac1->SetDeceleration(TMath::Nint(0.3*fMac1->GetVelRes())); - fMac2->SetDeceleration(TMath::Nint(0.3*fMac2->GetVelRes())); -#endif + // FIXME: Makes sense? + fMac1->SetDeceleration(TMath::Nint(0.03*1000000000)); + fMac2->SetDeceleration(TMath::Nint(0.09*1000000000)); + fMac1->SetRpmMode(FALSE); fMac2->SetRpmMode(FALSE); } - -/* - fMac1->SetDeceleration(0.3*fMac1->GetVelRes()); - fMac2->SetDeceleration(0.3*fMac2->GetVelRes()); - - fMac2->SendSDO(0x3000, Macs::string('s','t','o','p')); - fMac1->SendSDO(0x3000, Macs::string('s','t','o','p')); - fMac2->WaitForSdo(0x3000, 0); - fMac1->WaitForSdo(0x3000, 0); - fMac1->SetRpmMode(FALSE); - fMac2->SetRpmMode(FALSE); - */ // @@ -538,6 +414,9 @@ bool MCosy::CheckNetwork() { - //return kTRUE; - //CheckConnections(); + if (!HasConnection()) + { + gLog << warn << "- No connection to network." << endl; + return false; + } CheckForError(); @@ -557,6 +436,4 @@ fMac1->HandleError(); fMac2->HandleError(); - //if (fMac3) - // fMac3->HandleError(); if (HasError() || HasZombie()) return false; @@ -564,5 +441,6 @@ CheckForError(); - return true; + + return fMac1->IsOperative() && fMac2->IsOperative(); } @@ -581,47 +459,5 @@ StopMovement(); return 0; -/* - case WM_PRESET: - cout << "WM_Preset: start." << endl; - if (!CheckNetwork()) - return (void*)0xebb0; - fZd1->SetPreset(); - fZd2->SetPreset(); - fAz->SetPreset(); - cout << "WM_Preset: done. (return 0xaffe)" << endl; - return (void*)0xaffe; -*/ - /* - case WM_CALIB: - { - cout << "WM_Calib: start." << endl; - if (!CheckNetwork()) - return (void*)0xebb0; - - SlaStars sla(fObservatory); - sla.Now(); - - RaDec rd = *((RaDec*)mp); - - //RaDec rd(37.94, 89.2644); // POLARIS - //RaDec rd(213.915417, 19.1825); // ARCTURUS - - cout << "Calibrating to: " << rd.Ra()*24/360 << "h " << rd.Dec() << "°" << endl; - - ZdAz za=sla.CalcZdAz(rd*TMath::DegToRad())*16384.0/k2Pi; - - cout << "Calc Zd: " << za.Zd() << " Az: " << za.Az() << endl; - - ZdAz sepos = GetSePos(); - cout << "Got Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl; - - fZd1->SetPreset(za.Zd()); - fZd2->SetPreset(-za.Zd()); - fAz->SetPreset(za.Az()); - - cout << "WM_Calib: done. (return 0xaffe)" << endl; - } - return (void*)0xaffe; - */ + case WM_TPOINT: { @@ -655,5 +491,5 @@ *fOutTp << setprecision(7) << za.Az() << " " << za.Alt() << " "; - ZdAz sepos = GetSePos()*TMath::TwoPi()/kResSE; + ZdAz sepos = GetSePos()*TMath::TwoPi(); za.Set(TMath::Pi()/2-sepos.Zd(), sepos.Az()); za *= TMath::RadToDeg(); @@ -681,6 +517,6 @@ ZdAz dest = *((ZdAz*)mp) * TMath::DegToRad(); - if (!SetPosition(dest, kTRUE)) - return 0x1234; + //if (!SetPosition(dest, kTRUE)) + // return 0x1234; SlaStars sla(fObservatory); @@ -777,5 +613,5 @@ //cout << "WM_Track: done. (return 0x8888)" << endl; return 0x7878; - +/* case WM_TESTSE: //cout << "WM_TestSe: start." << endl; @@ -795,5 +631,5 @@ //cout << "WM_Disply: done. (return 0xd1e1)" << endl; return 0xd1e1; - + */ case WM_TRACK: case WM_GRB: @@ -832,36 +668,4 @@ return 0xbe0e; - case WM_HOME: - //cout << "WM_Home: START" << endl; - if (!CheckNetwork()) - return 0xebb0; - else - { - cout << "HOME NOT ALLOWED... for Magic." << endl; - /* - cout << "Going Home..." << endl; - TEnv env(".cosyrc"); - - SetStatus(MDriveCom::kMoving); - - fMac1->SetHome(250000, env.GetValue("Az_MaxTime2ReachHome[s]", 100)); - fMac2->SetHome(250000, env.GetValue("Zd_MaxTime2ReachHome[s]", 100)); - - gLog << "SETHOME DONE" << endl; - - SetStatus(HasError() ? MDriveCom::kError : MDriveCom::kStopped); - - fAz->SetPreset(); - fZd1->SetPreset(); - fZd2->SetPreset(); - - fMac1->ReqPos(); - fMac2->ReqPos(); - fMac3->StopMotor(); - */ - } - //cout << "WM_Home: done. (return 0x403e)" << endl; - return 0x403e; - case WM_CALCALTAZ: { @@ -879,5 +683,5 @@ cout << "Zd/Az target: " << a1.Zd()*TMath::RadToDeg() << "° " << a1.Az()*TMath::RadToDeg() << "°" << endl; - if (fZd1 && fZd2 && fAz) + if (fMac1 && fMac2) a1 = AlignTrackingPos(a1); @@ -886,5 +690,5 @@ a1 *= TMath::RadToDeg(); - const ZdAz a2 = a1*kResSE/360; + const ZdAz a2 = a1/360; cout << "Zd/Az bended: " << a1.Zd() << "° " << a1.Az() << "°" << endl; @@ -895,5 +699,5 @@ case WM_ENDSWITCH: { - ZdAz pos = GetSePos()*TMath::TwoPi()/kResSE; + ZdAz pos = GetSePos()*TMath::TwoPi(); pos = fBending.SubtractOffsets(pos)*TMath::RadToDeg(); @@ -919,10 +723,4 @@ } -Int_t MTTalk::Thread() -{ - fCosy->TalkThread(); - return 0; -} - void MCosy::ReadConfig(MEnv &env) { @@ -947,297 +745,13 @@ gLog << " * Min': " << fMin.Zd()*TMath::RadToDeg() << "deg " << fMin.Az()*TMath::RadToDeg() << "deg" << endl; gLog << " * Max': " << fMax.Zd()*TMath::RadToDeg() << "deg " << fMax.Az()*TMath::RadToDeg() << "deg" << endl; - - kGear.Set(env.GetValue("Zd_GearRatio[U_mot/U_tel]", 1000.0), - env.GetValue("Az_GearRatio[U_mot/U_tel]", 1000.0)); - - Bool_t mac1 = fMac1 && !fMac1->IsZombieNode(); - Bool_t mac2 = fMac2 && !fMac2->IsZombieNode(); - //Bool_t mac3 = fMac3 && !fMac3->IsZombieNode(); - - Bool_t zd1 = fZd1 && !fZd1->IsZombieNode(); - Bool_t zd2 = fZd2 && !fZd2->IsZombieNode(); - Bool_t az = fAz && !fAz->IsZombieNode(); - - Double_t x = 0; - Double_t y = 0; -// y = mac1 ? fMac1->GetRes() : (mac3 ? fMac3->GetRes() : env.GetValue("Az_ResRE[re/U_mot]", 1500)); - y = mac1 ? fMac1->GetRes() : env.GetValue("Az_ResRE[re/U_mot]", 1500); - x = mac2 ? fMac2->GetRes() : env.GetValue("Zd_ResRE[re/U_mot]", 1500); - kResRE.Set(x,y); - - gLog << " * Az RE resolution: " << x << " re/U_mot" << endl; - gLog << " * Zd RE resolution: " << y << " re/U_mot" << endl; - - x = zd1 ? fZd1->GetPhysRes() : (zd2 ? fZd2->GetPhysRes() : env.GetValue("Zd_ResSE[se/U_tel]", 16384)); - y = az ? fAz->GetPhysRes() : env.GetValue("Az_ResSE[se/U_tel]", 16384); - kResSE.Set(x,y); - - gLog << " * Az SE resolution: " << x << " se/U_tel" << endl; - gLog << " * Zd SE resolution: " << y << " se/U_tel" << endl; - - /* - kResRE.Y(0); - if (fMac1 && !fMac1->IsZombieNode()) - kResRE.Y(fMac1->GetRes()); - else - y = fMac3 && !fMac3->IsZombieNode() ? fMac3->GetRes() : env.GetValue("Az_ResRE[re/U_mot]", 1500); - if (fMac3 && !fMac3->IsZombieNode()) - kResRE.Y(fMac3->GetRes()); - else - kResRE.Y(env.GetValue("Az_ResRE[re/U_mot]", 1500)); - - kResRE.X(0); - if (fMac2 && !fMac2->IsZombieNode()) - kResRE.X(fMac2->GetRes()); - else - kResRE.X(env.GetValue("Zd_ResRE[re/U_mot]", 1500)); - - kResSE.X(0); - if (fZd1 && !fZd1->IsZombieNode()) - kResSE.X(fZd1->GetPhysRes()); - else - if (fZd2 && !fZd2->IsZombieNode()) - kResSE.X(fZd2->GetPhysRes()); - else - kResSE.X(env.GetValue("Zd_ResSE[se/U_mot]", 16384)); - - kResSE.Y(0); - if (fAz && !fAz->IsZombieNode()) - kResSE.Y(fAz->GetPhysRes()); - else - kResSE.Y(env.GetValue("Az_ResSE[se/U_mot]", 16384)); - */ - - // believing the Macs manual '*4' shouldn't be necessary, but it is. - // Because the a RE is 4 quad counts. - // Calculating speeds we have to convert back to qc - kResRE *= 4; - kGearTot = Mul(kResRE, kGear); -// kGearTot = kResRE*kGear; - -// gLog << all; -// gLog << " * Setting Gear Ratios:" << endl; - // gLog << " --------------------" << endl; - gLog << " * X: " << kGear.X() << "*" << kResRE.X()/4 << "/" << kResSE.X() << "=4*" << kGearTot.X() << "/" << kResSE.X() << endl; - gLog << " * Y: " << kGear.Y() << "*" << kResRE.Y()/4 << "/" << kResSE.Y() << "=4*" << kGearTot.Y() << "/" << kResSE.Y() << endl; -} -/* -void MCosy::InitSync() -{ - if (!fMac3) - { - gLog << "Unable to Init Sync! Mac3 not available." << endl; - return; - } - - const int res = fMac3->GetVelRes(); - - fMac3->SetVelocity(0.3*res); - fMac3->SetAcceleration(0.2*res); - fMac3->SetDeceleration(0.2*res); - fMac3->StartPosSync(); -} -*/ -void MCosy::TalkThreadSeTest() -{ -// if (fZd1->IsZombieNode() || fZd2->IsZombieNode()) - // return; - - if (fHist) - { - gLog << err << "You are much too fast... try again." << endl; - return; - } - - fHist = new TH2F("Diff", "Difference of SE values", - 201, fMin.Zd(), fMax.Zd(), 41, -10.5, 10.5); - fHist->SetXTitle("ZA [\\circ]"); - fHist->SetYTitle("\\Delta SE"); - - Double_t offset = 0; - - int cnt = 0; - - gLog << inf2 << "Starting Shaftencoder Test..." << endl; - - gLog << err << "Build in values!!!" << endl; - - while (fBackground==kBgdSeTest) - { - fZd1->ResetPosHasChanged(); - fZd2->ResetPosHasChanged(); - - while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() && - fBackground==kBgdSeTest) - { - usleep(1); - TThread::CancelPoint(); - } - - const Double_t pos[3] = { - (fZd1->GetPos()+8192)%16384, - (fZd2->GetPos()+8192)%16384, - fAz->GetPos() }; - - // - // Estimate Offset from the first ten positions - // - if (cnt++<10) - { - offset += pos[0]+pos[1]; - continue; - } - if (cnt==11) - { - offset /= 10; - cnt++; - } - - Double_t apos = (pos[0]-pos[1])/2 * TMath::TwoPi() / kResSE.X(); - - ZdAz bend = fBending.CorrectBack(ZdAz(apos, pos[2]))*TMath::RadToDeg(); - fHist->Fill(bend.Zd(), pos[0]+pos[1]-offset); - } - - gLog << inf2 << "Shaftencoder Test Stopped... displaying Histogram." << endl; - - fBackground=kBgdSeTestDispl; -} - -void MCosy::TalkThreadGear() -{ -// if (fZd1->IsZombieNode() || fZd2->IsZombieNode()) - // return; - - if (fHist) - { - gLog << err << "You are much too fast... try again." << endl; - return; - } - - fHist = new TH3F("Gear", "Gear Ratio Re/Se", - (int)((fMax.Zd()-fMin.Zd())/2.5+1), fMin.Zd(), fMax.Zd(), - (int)((fMax.Az()-fMin.Az())/2.5+1), fMin.Az(), fMax.Az(), - 61, 349.5, 500.5); - - fHist->SetXTitle("Zd [\\circ]"); - fHist->SetYTitle("Az [\\circ]"); - fHist->SetZTitle("Re/Se"); - - gLog << inf2 << "Starting Gear determination..." << endl; - - ZdAz se0 = GetSePos(); - ZdAz re0 = GetRePosPdo(); - - while (fBackground==kBgdGear) - { - fZd1->ResetPosHasChanged(); - fZd2->ResetPosHasChanged(); - fAz->ResetPosHasChanged(); - - while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() && - !fAz->PosHasChanged() && fBackground==kBgdGear) - { - usleep(1); - TThread::CancelPoint(); - } - - ZdAz se = GetSePos(); - ZdAz re = GetRePosPdo(); - - ZdAz dse = se-se0; - ZdAz dre = re-re0; - - if (fabs(dse.Zd())*144>kResSE.X()) // Each 2.5deg (144) - { - se0.Zd(se.Zd()); - re0.Zd(re.Zd()); - - se -= dse/2; - - ZdAz bend = fBending.CorrectBack(se*TMath::TwoPi()/kResSE)*TMath::RadToDeg(); - ((TH3*)fHist)->Fill(bend.Zd(), bend.Az(), dre.Zd()/dse.Zd()); - } - - if (fabs(dse.Az())*144>kResSE.Y()) // Each 2.5deg (144) - { - se0.Az(se.Az()); - re0.Az(re.Az()); - - se -= dse/2; - - ZdAz bend = fBending.CorrectBack(se*TMath::TwoPi()/kResSE)*TMath::RadToDeg(); - ((TH3*)fHist)->Fill(bend.Az(), bend.Az(), dre.Az()/dse.Az()); - } - } - gLog << inf2 << "Gear Test Stopped... displaying Histogram." << endl; - - fBackground=kBgdGearDispl; -} - -void MCosy::TalkThread() -{ - /* ========== FIXME? ============= - if (fMac1->IsZombieNode() || fMac2->IsZombieNode()) - return; - */ - - if (fMac1 && fMac2) - { - fMac1->ReqPos(); - fMac2->ReqPos(); - } - - //InitSync(); - - /*** FOR DEMO MODE ***/ - if (!fZd1 || !fZd2 || !fAz) - return; - /*** FOR DEMO MODE ***/ - - // - // Start the Network - // - while (1) - { - // - // wait until a tracking session is started - // - while (fBackground==kBgdNone) - { - usleep(1); - TThread::CancelPoint(); - } - - switch (fBackground) - { - case kBgdNone: - continue; -/*#ifndef NEWALGO - case kBgdTracking: - TalkThreadTracking(); - continue; -#endif*/ - case kBgdSeTest: - TalkThreadSeTest(); - continue; - - case kBgdGear: - TalkThreadGear(); - continue; - - default: - continue; - } - } } ZdAz MCosy::GetPointingPos() const { - if (fZd1->IsZombieNode() || fZd2->IsZombieNode() || fAz->IsZombieNode()) + if (fMac1->IsZombieNode() || fMac2->IsZombieNode()) return ZdAz(0, 0); // GetPointingPos [deg] - const ZdAz seist = GetSePos()*TMath::TwoPi()/kResSE; // [rad] + const ZdAz seist = GetSePos()*TMath::TwoPi(); // [rad] return fBending.CorrectBack(seist)*TMath::RadToDeg(); } @@ -1258,21 +772,16 @@ // Update Gui, foremer MTGui. // - if (fZd1) - fZd1->DisplayVal(); - if (fZd2) - fZd2->DisplayVal(); - if (fAz) - fAz->DisplayVal(); - + if (fMac1) + fMac1->DisplayVal(); + if (fMac2) + fMac2->DisplayVal(); + + /* Byte_t avail = 0; avail |= (fMac1 && !fMac1->IsZombieNode()) ? 0x01 : 0; avail |= (fMac2 && !fMac2->IsZombieNode()) ? 0x02 : 0; - //avail |= (fMac3 && !fMac3->IsZombieNode()) ? 0x04 : 0; - avail |= (fZd1 && !fZd1->IsZombieNode()) ? 0x08 : 0; - avail |= (fZd2 && !fZd2->IsZombieNode()) ? 0x10 : 0; - avail |= (fAz && !fAz->IsZombieNode()) ? 0x20 : 0; // avail |= (!(fStatus&MDriveCom::kError) && 1 ? 0x40 : 0; - + */ Bool_t armed = kTRUE; @@ -1283,25 +792,26 @@ SetStatus(MDriveCom::kError); - - ZdAz bendist = fStatus&MDriveCom::kTracking ? fTrackingPos : GetPointingPos(); + const TString stataz = fMac1 ? fMac1->GetStatusDKC() : ""; + const TString statzd = fMac2 ? fMac2->GetStatusDKC() : ""; + + const UInt_t stat1 = fMac1 ? fMac1->GetStatusPdo3() : 0; + const UInt_t stat2 = fMac2 ? fMac2->GetStatusPdo3() : 0; + + ZdAz bendist = GetPointingPos(); //cout << (fStatus&MDriveCom::kTracking?"TRA: ":"POS: ") << bendist.Zd() << " " << bendist.Az() << endl; - fCom->SendReport(fStatus, fRaDec, fZdAzSoll, bendist, fTrackingError, armed); + static MTimeout tout(0); + if (tout.HasTimedOut()) + { + tout.Start(999); + fCom->SendReport(fStatus, fRaDec, fZdAzSoll, bendist, fTrackingError, armed); + } fWin->UpdateWeather(*fCom); - fWin->Update(bendist, fTrackingError, fVelocity, /*fOffset,*/ - fRaDec, fZdAzSoll, fStatus, avail, armed); + fWin->Update(bendist, fTrackingError, /*fVelocity, fOffset,*/ + fRaDec, fZdAzSoll, fStatus, (stat1<<8)|stat2, HasConnection(), armed, statzd, stataz); gLog.UpdateGui(); - - const Bool_t trigger = fTriggerDisplay; - fTriggerDisplay = kFALSE; - - if (fBackground==kBgdSeTestDispl || (trigger&&fBackground==kBgdSeTest)) - DisplayHistTestSe(!trigger); - - if (fBackground==kBgdGearDispl || (trigger&&fBackground==kBgdGear)) - DisplayHistGear(!trigger); if (fMutexGui.UnLock()==13) @@ -1309,121 +819,4 @@ return kTRUE; -} - -void MCosy::DisplayHistTestSe(Bool_t del) -{ - gLog << inf2 << "Displaying histogram..." << endl; - - TH2F &hist = *(TH2F*)fHist; - - if (del) - { - fHist = NULL; - fBackground = kBgdNone; - } - - 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("Zd [\\circ]"); - for (int i=0; iGetBinError(i)); - p2.SetLineColor(kRed); - p2.SetStats(0); - p2.DrawCopy(); - - if (del) - delete &hist; -} - -void MCosy::DisplayHistGear(Bool_t del) -{ - gLog << inf2 << "Displaying histogram..." << endl; - - TH3F &hist = *(TH3F*)fHist; - - if (del) - { - fHist = NULL; - fBackground = kBgdNone; - } - - TCanvas *c=new TCanvas("c1", "", 1000, 1000); - c->Divide(2,2); - - // ---------- - - c->cd(1); - TH2D &h1=*(TH2D*)hist.Project3D("zx"); // Zd - h1.SetTitle(" Gear Ratio Zenith Distance [re/se] "); - h1.SetXTitle("Zd [\\circ]"); - h1.Draw(); - h1.SetBit(kCanDelete); - - TProfile *p1 = h1.ProfileX("_pfx", -1, 9999, "s"); - p1->SetLineColor(kBlue); - p1->Draw("same"); - p1->SetBit(kCanDelete); - - // ---------- - - c->cd(2); - TH2D &h2=*(TH2D*)hist.Project3D("zy"); // Az - h2.SetTitle(" Gear Ratio Azimuth [re/se] "); - h2.SetXTitle("Zd [\\circ]"); - h2.Draw(); - h2.SetBit(kCanDelete); - - TProfile *p2 = h2.ProfileX("_pfx", -1, 9999, "s"); - p2->SetLineColor(kBlue); - p2->Draw("same"); - p2->SetBit(kCanDelete); - - // ---------- - - c->cd(3); - - TAxis &axe1 = *h1.GetXaxis(); - - TH1F f1("spreadzd", " Spread Zenith Distance ", - axe1.GetNbins(), axe1.GetXmin(), axe1.GetXmax()); - f1.SetXTitle("Zd [\\circ]"); - for (int i=0; iGetBinError(i)); - f1.SetLineColor(kRed); - f1.SetStats(0); - f1.DrawCopy(); - - c->cd(4); - - // ---------- - - TAxis &axe2 = *h2.GetXaxis(); - - TH1F f2("spreadaz", " Spread Azimuth ", - axe2.GetNbins(), axe2.GetXmin(), axe2.GetXmax()); - f2.SetXTitle("Az [\\circ]"); - for (int i=0; iGetBinError(i)); - f2.SetLineColor(kRed); - f2.SetStats(0); - f2.DrawCopy(); - - // ---------- - - if (del) - delete &hist; } @@ -1445,7 +838,4 @@ ReadConfig(env); - gLog << inf << "- Starting TX Thread." << endl; - fTTalk = new MTTalk(this); - gLog << inf << "- Starting GUI update." << endl; fUpdateGui->TurnOn(); @@ -1466,8 +856,4 @@ gLog << inf << "- GUI Update stopped." << endl; - gLog << inf << "- Stopping TX Thread." << endl; - delete fTTalk; - gLog << inf << "- TX Thread stopped." << endl; - gLog << inf << "- Stopping CAN network." << endl; Network::Stop(); @@ -1483,6 +869,5 @@ // Disable the synchronization by using a negative CAN Id for id2. // -void MCosy::Constructor(Int_t id1, Int_t id2, Int_t id3, - Int_t id4, Int_t id5, Int_t id6) +void MCosy::Constructor(Int_t id1, Int_t id2) { // @@ -1491,21 +876,10 @@ gLog << inf << "- Setting up network." << endl; - fMac1=new Macs(id1, "Mac/Az"); - fMac2=new Macs(id3, "Mac/Zd"); - //if (id2>=0) - // fMac3=new Macs(id2, "Mac/Az-Sync"); - - fZd1=new ShaftEncoder(id4, "SE/Zd1"); - fZd2=new ShaftEncoder(id5, "SE/Zd2"); - fAz =new ShaftEncoder(id6, "SE/Az"); - - fZd1->SetReport(fOutRep); - fZd2->SetReport(fOutRep); - fAz->SetReport(fOutRep); - - fAz->SetMotor(fMac1); - fZd1->SetMotor(fMac2); - fZd2->SetMotor(fMac2); - + fMac1=new Dkc(id1, "DKC/Az"); + fMac2=new Dkc(id2, "DKC/Zd"); + + fMac1->SetReport(fOutRep); + fMac2->SetReport(fOutRep); + gLog << inf << "- Connecting devices to network." << endl; @@ -1515,9 +889,4 @@ SetNode(fMac1); SetNode(fMac2); - //if (id2>=0) - // SetNode(fMac3); - SetNode(fZd1); - SetNode(fZd2); - SetNode(fAz); // @@ -1528,8 +897,9 @@ gLog << all << "- Starting GUI." << endl; - fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1); -} + fWin=new MGCosy(fObservatory, this); +} + /* -void MCosy::ConstructorSE(Int_t id4, Int_t id5, Int_t id6) +void MCosy::ConstructorDemo() { // @@ -1537,17 +907,4 @@ // gLog << "- Setting up network." << endl; - - fZd1=new ShaftEncoder(id4, "SE/Zd1", gLog); - fZd2=new ShaftEncoder(id5, "SE/Zd2", gLog); - fAz =new ShaftEncoder(id6, "SE/Az", gLog); - - gLog << "- Connecting devices to network." << endl; - - // - // Connect the devices to the network - // - SetNode(fZd1); - SetNode(fZd2); - SetNode(fAz); // @@ -1560,21 +917,4 @@ fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1); } - -void MCosy::ConstructorDemo() -{ - // - // Create Nodes - // - gLog << "- Setting up network." << endl; - - // - // Create Gui Event timer and Gui - // - gLog << "- Initializing GUI Timer." << endl; - fUpdateGui = new TTimer(this, 100); // 100ms - - gLog << "- Starting GUI." << endl; - fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1); -} */ @@ -1614,14 +954,8 @@ MCosy::MCosy(MEnv &env, MDriveCom *com, const char *pointing) : Network(), fObservatory(MObservatory::kMagic1), fStarguider(NULL), -fZd1(0), fZd2(0), fAz(0), fMac1(0), fMac2(0), /*fMac3(0),*/ -fBackground(kBgdNone), fTriggerDisplay(kFALSE), fStatus(MDriveCom::kStopped), -fOutTp(0), fOutRep(0) -{ - const Int_t id1 = env.GetValue("Az_Id-MAC1", 1); //1 - const Int_t id2 = env.GetValue("Az_Id-MAC2", 2); //2 - const Int_t id3 = env.GetValue("Zd_Id-MAC", 3); //3 - const Int_t id4 = env.GetValue("Zd_Id-SE1", 4); //4 - const Int_t id5 = env.GetValue("Zd_Id-SE2", 5); //5 - const Int_t id6 = env.GetValue("Az_Id-SE", 6); //6 +fMac1(0), fMac2(0), fStatus(MDriveCom::kStopped), fOutTp(0), fOutRep(0) +{ + const Int_t id1 = env.GetValue("Az_Id", 1); + const Int_t id2 = env.GetValue("Zd_Id", 3); TString name = GetFileName("rep", "cosy", "rep"); @@ -1644,5 +978,5 @@ else gLog << err << "ERROR - Reading pointing model from " << pointing << endl; - Constructor(id1, id2, id3, id4, id5, id6);/* + Constructor(id1, id2);/* break; case 1: @@ -1659,7 +993,6 @@ gLog.SetOutputGui(fWin->GetLog(), kTRUE); - fZd1->SetDisplay(fWin->GetLabel2()); - fZd2->SetDisplay(fWin->GetLabel3()); - fAz->SetDisplay(fWin->GetLabel1()); + fMac2->SetDisplay(fWin->GetLabel2()); + fMac1->SetDisplay(fWin->GetLabel1()); fCom = com;//new MDriveCom(this, addr, tx, rx, fOutRep); @@ -1716,15 +1049,9 @@ gLog << inf2 << "Deleting Nodes." << endl; - fZd1->SetReport(0); - fZd2->SetReport(0); - fAz->SetReport(0); - - delete fAz; - delete fZd1; - delete fZd2; + fMac1->SetReport(0); + fMac2->SetReport(0); + delete fMac1; delete fMac2; - //if (fMac3) - // delete fMac3; gLog << inf2 << "Deleting MGCosy." << endl; Index: /trunk/MagicSoft/Cosy/main/MCosy.h =================================================================== --- /trunk/MagicSoft/Cosy/main/MCosy.h (revision 9131) +++ /trunk/MagicSoft/Cosy/main/MCosy.h (revision 9132) @@ -45,32 +45,16 @@ #define WM_ARM 0x1013 -class ShaftEncoder; -class Macs; +class Dkc; + class MGCosy; class MCosy; -class TH1; class MStarguider; class MDriveCom; class MEnv; -class MTTalk : public MThread -{ -private: - MCosy *fCosy; +class TTimer; - Int_t Thread(); - -public: - MTTalk(MCosy *cosy) : MThread("MTTalk"), fCosy(cosy) - { - RunThread(); - } - ~MTTalk(); -}; - -class TTimer; class MCosy : public Network, public MsgQueue, public TObject { - friend class MTTalk; friend class MSlewing; friend class MTracking; @@ -81,11 +65,6 @@ MStarguider *fStarguider; - ShaftEncoder *fZd1; - ShaftEncoder *fZd2; - ShaftEncoder *fAz; - - Macs *fMac1; - Macs *fMac2; - //Macs *fMac3; + Dkc *fMac1; + Dkc *fMac2; MGCosy *fWin; @@ -93,22 +72,6 @@ TTimer *fUpdateGui; - MTTalk *fTTalk; // should be outsourced, like the starguider. - // with a generic interface to both... - ZdAz fTrackingPos; // [deg] Current tracking position - ZdAz fTrackingPosRaw; // [deg] Raw tracking position TMutex fMutexGui; - - enum BackgroundAction_t - { - kBgdNone, - //kBgdTracking, - kBgdSeTest, - kBgdSeTestDispl, - kBgdGear, - kBgdGearDispl - }; - - BackgroundAction_t fBackground; // Flag for present backgroundthread ZdAz fTrackingError; // [rad] Tracking Offset between SE and calc-pos @@ -116,15 +79,6 @@ RaDec fRaDec; // Position to track ZdAz fAccuracy; // Actual accuracy of Tracking - ZdAz fVelocity; // Actual velocity of Tracking ZdAz fMin; ZdAz fMax; - - TH1 *fHist; - Bool_t fTriggerDisplay; - - XY kResSE; // describing the resolution of the system [se/U_tel] - XY kResRE; // describing the resolution of the system [re/U_mot] - XY kGear; // describing the resolution of the system [U_mot/U_tel] - XY kGearTot; // describing the resolution of the system [re/U_tel] MPointing fBending; @@ -142,21 +96,6 @@ UInt_t GetStatus() const { return fStatus; } - ZdAz GetRePos(); - ZdAz GetRePosPdo(); - ZdAz GetSePos() const; // [se] - // FIXME: Must depend on the Shaftencoder mounted - ZdAz GetSePosRad() const { return GetSePos()*TMath::TwoPi()/16384; } // [rad] - - void InitSync(); - - void TalkThread(); - void TalkThreadSeTest(); - void TalkThreadGear(); - - void DisplayHistTestSe(Bool_t del=kTRUE); - void DisplayHistGear(Bool_t del=kTRUE); - int SetPosition(const ZdAz &dst, Bool_t track=kFALSE); - void TrackPosition(const RaDec &dst); // ra, dec [rad] + void TrackPosition(const RaDec &dst); // ra, dec [rad] void TrackPositionGRB(const RaDec &dst); // ra, dec [rad] @@ -170,5 +109,5 @@ void WaitForEndMovement(); - void Constructor(Int_t id1, Int_t id2, Int_t id3, Int_t id4, Int_t id5, Int_t id6); + void Constructor(Int_t id1, Int_t id2); //void ConstructorSE(Int_t id1, Int_t id2, Int_t id3); //void ConstructorDemo(); @@ -198,15 +137,5 @@ MGCosy *GetWin() { return fWin; } - ZdAz GetTrackingPosDeg() const { return fTrackingPos; }; - ZdAz GetTrackingPosRaw() const { return fTrackingPosRaw; }; - void SetTrackingPosRE(ZdAz za); - - AltAz GetAltAzDeg() const - { - ZdAz sepos = GetSePos()*TMath::TwoPi()/kResSE; - AltAz za1(TMath::Pi()/2-sepos.Zd(), sepos.Az()); - za1 *= kRad2Deg; - return za1; - } + ZdAz GetSePos() const; // [revolutions] MLog *GetOutRep() { return fOutRep; } @@ -214,7 +143,4 @@ MDriveCom *GetDriveCom() { return fCom; } - // static ZdAz CorrectTarget(const ZdAz &src, const ZdAz &dst); - // static ZdAz RaDec2ZdAz(const double mjd, const RaDec &pos, const RaDec &pm=RaDec(0,0)); - ClassDef(MCosy, 0) }; Index: /trunk/MagicSoft/Cosy/main/MSlewing.cc =================================================================== --- /trunk/MagicSoft/Cosy/main/MSlewing.cc (revision 9131) +++ /trunk/MagicSoft/Cosy/main/MSlewing.cc (revision 9132) @@ -5,5 +5,5 @@ #include "MCosy.h" -#include "macs.h" +#include "dkc.h" #include "MDriveCom.h" @@ -17,4 +17,11 @@ //#define EXPERT #undef EXPERT + +bool MSlewing::SetAcc(Dkc *mac, Float_t acc) +{ + // FIXME: Get acceleration scale from DKC! + mac->SetAcceleration(TMath::Nint(acc*1000000000)); + return !mac->IsZombieNode(); +} // -------------------------------------------------------------------------- @@ -28,27 +35,24 @@ // target position at the same time. // -void MSlewing::SetPosVelocity(const Float_t ratio, Float_t vel) -{ - // - // Set velocities - // - const int vr = fCosy->fMac1->GetVelRes(); - vel *= vr; - - if (ratio<1) - { - fCosy->fMac1->SetVelocity(TMath::Nint(vel)); - fCosy->fMac2->SetVelocity(TMath::Nint(vel*ratio)); +void MSlewing::SetPosVelocity(const ZdAz &res, Float_t vel) +{ + const Double_t taz = TMath::Abs(res.Az())/fCosy->fMac1->GetVelMax(); + const Double_t tzd = TMath::Abs(res.Zd())/fCosy->fMac2->GetVelMax(); + + if (tzd > taz) + { + fCosy->fMac1->SetVelocityRel(vel*TMath::Abs(res.Az()/res.Zd())); + fCosy->fMac2->SetVelocityRel(vel); } else { - fCosy->fMac1->SetVelocity(TMath::Nint(vel/ratio)); - fCosy->fMac2->SetVelocity(TMath::Nint(vel)); - } -} - -// -------------------------------------------------------------------------- -// -// Does a relative positioning. + fCosy->fMac1->SetVelocityRel(vel); + fCosy->fMac2->SetVelocityRel(vel*TMath::Abs(res.Zd()/res.Az())); + } +} + +// -------------------------------------------------------------------------- +// +// Does an absolute positioning. // // The steps to move are given in a ZdAz object relative to the current @@ -57,5 +61,5 @@ // if Axis 2==kTRUE. The function waits for the movement to be finished. // -void MSlewing::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2) +void MSlewing::DoAbsPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2) { if (fCosy->HasZombie()) @@ -64,12 +68,18 @@ fCosy->SetStatus(MDriveCom::kMoving); - if (axe1) fCosy->fMac2->StartRelPos(TMath::Nint(rd.Zd())); - if (axe2) fCosy->fMac1->StartRelPos(TMath::Nint(rd.Az())); + if (axe1) fCosy->fMac2->StartAbsPosRev(rd.Zd()); + if (axe2) fCosy->fMac1->StartAbsPosRev(rd.Az()); + + if (axe1) fCosy->fMac2->WaitForSdo(0x6004, 0); + if (axe2) fCosy->fMac1->WaitForSdo(0x6004, 0); + + // FIXME: We need a delay here to account for the delay of the + // toggle bit in the SPS. We need a more precise return value from + // the SPS: + usleep(150000); + #ifdef EXPERT cout << "Waiting for positioning..." << flush; #endif - if (axe1) fCosy->fMac2->WaitForSdo(0x6004, 1); - if (axe2) fCosy->fMac1->WaitForSdo(0x6004, 1); - fCosy->WaitForEndMovement(); #ifdef EXPERT @@ -78,15 +88,24 @@ } -bool MSlewing::SetAccDec(Macs *mac, Float_t acc, Float_t dec) -{ - const int vr = mac->GetVelRes(); - mac->SetAcceleration(TMath::Nint(acc*vr)); - mac->SetDeceleration(TMath::Nint(dec*vr)); - return !mac->IsZombieNode(); -} - bool MSlewing::Break() { return fCosy->Break() || fCosy->HasError() || fCosy->HasZombie(); +} + + +// -------------------------------------------------------------------------- +// +// Caluclate the difference between feedback 1 and feedback 2 at +// the given zenith angle (feedback 2) +// +Double_t MSlewing::GetDiff(const ZdAz &za) const +{ + const Double_t zd = za.Zd(); //[revolutions] + + const Double_t sh = -1.21 *(TMath::SinH(0.916*zd*TMath::TwoPi())-1); + const Double_t cs = 0.667 *TMath::Cos(1.735*(zd-0.236)*TMath::TwoPi()); + const Double_t of = 0.6497; + + return (sh+cs+of)/360; //[revolutions] } @@ -104,21 +123,5 @@ int MSlewing::SetPosition(const ZdAz &dst, Bool_t track) // [rad] { - const ZdAz d = dst*kRad2Deg; - - MTime t(-1); - gLog << all << t << " - Target Position: " << d.Zd() << "deg, " << d.Az() << "deg (Zd/Az)" << endl; - - // - // Calculate new target position (shortest distance to go) - // - //const ZdAz src = fCosy->GetSePos(); // [se] - - // - // Make sure that the motors are in sync mode (necessary if the - // MACS has been rebooted from a Zombie state. - // - //InitSync(); - //if (fMac3->IsZombieNode()) - // return false; + gLog << all << MTime(-1) << " - Target Position: " << dst.Zd()*TMath::RadToDeg() << "deg, " << dst.Az()*TMath::RadToDeg() << "deg (Zd/Az)" << endl; // @@ -128,73 +131,40 @@ // const ZdAz dest = CorrectTarget(src, dst); // - ZdAz bend = fCosy->fBending(dst); // [rad] - - const ZdAz dest = bend*fCosy->kResSE/TMath::TwoPi(); // [se] + const ZdAz bend = fCosy->fBending(dst); // [rad] + const ZdAz dest = bend/TMath::TwoPi(); // [revolutions] if (!fCosy->CheckRange(bend)) return kFALSE; - bend *= kRad2Deg; fCosy->fZdAzSoll = dst; - //cout << "Source Zd: " << src.Zd() << "se Az:" << src.Az() << "se" << endl; - //cout << "Destination Zd: " << Rad2SE(dst.Zd()) << "se Az:" << Rad2SE(dst.Az()) << "se" << endl; - //cout << "Bend'd Dest Zd: " << dest.Zd() << "se Az:" << dest.Az() << "se" << endl; - //cout << "Bend'd Dest Zd: " << bend.Zd() << "deg Az:" << bend.Az() << "deg" << endl; - - // - // Set velocities - // - //const int vr = fCosy->fMac1->GetVelRes(); - - const Float_t rad2se = fCosy->kResSE.X()/TMath::TwoPi(); - int i; - for (i=0; i<(track?1:10) && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/; i++) - { - + for (i=0; i<(track?1:10) && !Break(); i++) + { gLog << inf2 << "- Step #" << i << endl; - // Get Shaft Encoder Positions - const ZdAz p=fCosy->GetSePos(); - - // calculate control deviation - ZdAz rd = dest-p; // [se] - ZdAz cd = rd; // [se] - // Correct for having two SE available -// FIMXE.... -// cd.Zd(cd.Zd()*2); - // Round to check whether we are as near as possible - // to the value we expect - - cout << cd.Zd() << " " << cd.Az() << endl; - - cd /= fCosy->kResSE; // Scale to Revolutions + // Get feedback 2 + const ZdAz sepos = fCosy->GetSePos(); + + // Calculate residual to move deviation + const ZdAz res = dest-sepos; // [revolutions] gLog << inf2 << "- Shaftencoders show a residual deviation of dZd="; - gLog << MString::Format("%.2f", cd.Zd()*360*60) << "' and dAz="; - gLog << MString::Format("%.2f", cd.Az()*360*60) << "'" << endl; - - cout << cd.Zd() << " " << cd.Az() << endl; - - cd *= 1./fMaxResidual; // Scale to units of the maximum residual - - cout << fCosy->kResSE.X() << " " << 1./fMaxResidual << endl; - cout << cd.Zd() << " " << cd.Az() << endl; - + gLog << MString::Format("%.2f", res.Zd()*360*60) << "' and dAz="; + gLog << MString::Format("%.2f", res.Az()*360*60) << "'" << endl; + + // Check which axis should still be moved + ZdAz cd = res; // [revolutions] + cd *= 1./fMaxResidual; // Scale to units of the maximum residual cd.Abs(); - // FIXME:: Do NOT check Shaftencoder values!!! // Check if there is a control deviation on the axis const Bool_t cdzd = cd.Zd()>0.5 ? kTRUE : kFALSE; const Bool_t cdaz = cd.Az()>0.5 ? kTRUE : kFALSE; - cout << (int)cdzd << " " << (int)cdaz << endl; - // check if we reached the correct position already if (!cdzd && !cdaz) { - t.Now(); - gLog << all << t << " - Positioning done in " << i << (i==1?" step.":" steps.") << endl; + gLog << all << MTime(-1) << " - Positioning done in " << i << (i==1?" step.":" steps.") << endl; fCosy->SetStatus(MDriveCom::kStopped); fCosy->fCom->SendStatus("Target position reached."); @@ -204,68 +174,24 @@ // ============================================== // Estimate the noncircularity of the zd axis -// const ZdAz ist = dst-rd*TMath::TwoPi()/fCosy->kResSE; - -// const double p1 = ist.Zd()-19.0605/kRad2Deg; -// const double p2 = dst.Zd()-19.0605/kRad2Deg; - - // const double f1 = (-26.0101*sin(p1)+443.761*ist.Zd())*rad2se; - // const double f2 = (-26.0101*sin(p2)+443.761*dst.Zd())*rad2se; - // =========================================== - - gLog << warn << "WARNING - The center of the elevation axis is taken as center of the drive bow" << endl; - - - // change units from se to re -// rd *= fCosy->kGearTot/fCosy->kResSE; // [re] - rd *= Div(fCosy->kGearTot,fCosy->kResSE); // [re] -// rd.Zd(f2-f1); - - // Initialize Velocities so that we reach both positions - // at the same time - if (i) - { - //cout << "--- LO-SPEED ---" << endl; - SetAccDec(fCosy->fMac1, 0.05, 0.05); - SetAccDec(fCosy->fMac2, 0.05, 0.05); - - SetPosVelocity(1.0, 0.05); - } - else - { - // 15 motor revolutions - const Double_t y = 15*fCosy->kGearTot.Y()/fCosy->kResSE.Y(); - -// lout << "MSlewing::SetPosition y: " << y << " rd.Az(): " << rd.Az() << endl; -// lout << "MSlewing::SetPosition fCosy->kGearTot.Y(): " << fCosy->kGearTot.Y() << " fCosy->kResSE.Y(): " << fCosy->kResSE.Y() << endl; - - if (rd.Az()>-y && rd.Az()fMac1, 0.05, 0.05); - else - SetAccDec(fCosy->fMac1, fAcc, fDec); - - SetAccDec(fCosy->fMac2, fAcc, fDec); - - SetPosVelocity(fabs(rd.Ratio()), fVel); - } - - rd.Round(); - - // FIXME? Check for Error or Zombie? - - // cout << " + " << (int)cdzd << " " << (int)cdaz << endl; - // cout << " + APOS: Zd=" << setw(6) << p.Zd() << "se Az=" << setw(6) << p.Az() << "se" << endl; - // cout << " + dZd=" << setw(6) << cd.Zd() << "se dAz=" << setw(6) << cd.Az() << "se" << endl; - // cout << " + dZd=" << setw(6) << rd.Zd() << "re dAz=" << setw(6) << rd.Az() << "re" << endl; - // cout << " + Ratio: Zd=" << setw(6) << kGearRatio.X() << "se Az=" << setw(6) << kGearRatio.Y() << "se" << endl; - - // repositioning (relative) - gLog << inf2 << "- Do Relative Positioning..." << endl; - DoRelPos(rd, cdzd, cdaz); - gLog << inf2 << "- Relative Positioning Done" << endl; - } - if (i==1 && track && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/) - { - t.Now(); - gLog << all << t << " - Positioning done." << endl; + const Double_t add = GetDiff(sepos)-GetDiff(dest); + + const ZdAz dest2(dest.Zd()+add, dest.Az()); + const ZdAz res2 = dest-sepos; + // ================================================= + + //gLog << warn << "WARNING - The center of the elevation axis is taken as center of the drive bow" << endl; + + SetAcc(fCosy->fMac1, fAcc.Az()); + SetAcc(fCosy->fMac2, fAcc.Zd()); + + SetPosVelocity(res2, fVel); + + gLog << inf2 << "- Do absolute positioning..." << endl; + DoAbsPos(dest2, cdzd, cdaz); + gLog << inf2 << "- Absolute Positioning Done" << endl; + } + if (i==1 && track && !Break()) + { + gLog << all << MTime(-1) << " - Positioning done." << endl; fCosy->SetStatus(MDriveCom::kStopped); fCosy->fCom->SendStatus("Tracking preposition reached."); @@ -278,6 +204,5 @@ fCosy->SetStatus(MDriveCom::kStopped); - t.Now(); - gLog << warn << t << " - Warning: Requested position not reached (i=" << i << ")" << endl; + gLog << warn << MTime(-1) << " - Warning: Requested position not reached (i=" << i << ")" << endl; fCosy->fCom->SendStatus("Target position missed!"); Index: /trunk/MagicSoft/Cosy/main/MSlewing.h =================================================================== --- /trunk/MagicSoft/Cosy/main/MSlewing.h (revision 9131) +++ /trunk/MagicSoft/Cosy/main/MSlewing.h (revision 9132) @@ -6,7 +6,10 @@ #endif -class Macs; +class Dkc; class MCosy; -class ZdAz; + +#ifndef MARS_MPointing +#include "MPointing.h" +#endif class MSlewing @@ -15,21 +18,24 @@ MCosy *fCosy; - bool SetAccDec(Macs *mac, Float_t acc, Float_t dec); + bool SetAcc(Dkc *mac, Float_t acc); bool Break(); private: Float_t fVel; - Float_t fAcc; - Float_t fDec; + ZdAz fAcc; Float_t fMaxResidual; // [rev] Maximum residual deviation to stop pointin void DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2); - void SetPosVelocity(const Float_t ratio, Float_t vel); + void DoAbsPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2); + void SetPosVelocity(const ZdAz &res, Float_t vel); + + Double_t GetDiff(const ZdAz &za) const; public: - MSlewing(MCosy *cosy) : fCosy(cosy), fVel(0.1), fAcc(0.2), fDec(0.1), fMaxResidual(1./16384) { } + MSlewing(MCosy *cosy) : fCosy(cosy), fVel(0.1), fAcc(0,0), fMaxResidual(1./16384) { } + virtual ~MSlewing() { } - void SetPointAccDec(Float_t acc, Float_t dec) { fAcc = acc; fDec = dec; } + void SetPointAcc(Float_t zd, Float_t az) { fAcc.Zd(zd); fAcc.Az(az); } void SetPointVelocity(Float_t vel) { fVel = vel; } int SetPosition(const ZdAz &dst, Bool_t track=kFALSE); Index: /trunk/MagicSoft/Cosy/main/MStarguider.cc =================================================================== --- /trunk/MagicSoft/Cosy/main/MStarguider.cc (revision 9131) +++ /trunk/MagicSoft/Cosy/main/MStarguider.cc (revision 9132) @@ -523,4 +523,5 @@ fCaos = new MCaos; fCaos->ReadResources(); + fCaos->SetRadii(237.9, 239.9); fStargCaos = new MCaos; @@ -1666,5 +1667,6 @@ // From the Shaftencoders we get the current 'pointing' position // as it is seen by the drive system (system pointing position) - const ZdAz za1 = fCosy->GetTrackingPosRaw(); + // FIXME???? + const ZdAz za1 = fCosy->GetSePos()*TMath::TwoPi(); // Write real pointing position @@ -1740,6 +1742,5 @@ if (d>1) { - MString txt; - txt.Print("%dfps", (int)((n-n0)/d+.5)); + const TString txt = MString::Format("%dfps", (int)((n-n0)/d+.5)); fFps->SetText(txt); fTime = t; @@ -1787,5 +1788,5 @@ pos = fCosy->GetPointingPos(); center = fCaos->Run(img, printl, printr, pos, t, 19, 3.0); - cout << "Caos Filter Camera center position: " << center.GetX() << " " << center.GetY() << endl; + cout << "Caos Filter Camera center position: " << center.GetX() << " " << center.GetY() << " (R=" << center.GetR() << ")" << endl; } @@ -1995,6 +1996,5 @@ fSkyBright->SetBackgroundColor(color); - MString txt; - txt.Print("Sky Brightness: %.1f", bright); + const MString txt = MString::Format("Sky Brightness: %.1f", bright); fSkyBright->SetText(txt); @@ -2085,8 +2085,8 @@ if (aa.Az()<0) aa.Az(aa.Az()+360); - txt.Print("(%d, %d) %.1fd/%.1fd", fDx, fDy, -aa.Alt(), aa.Az()-180); + txt.Form("(%d, %d) %.1fd/%.1fd", fDx, fDy, -aa.Alt(), aa.Az()-180); } else - txt.Print("(%d, %d)", fDx, fDy); + txt.Form("(%d, %d)", fDx, fDy); fPosZoom->SetText(txt); } @@ -2124,5 +2124,4 @@ MAstro::Deg2Dms(deg, sgn, d, m, s); - MString txt; - str += txt.Print("%c %03d %02d %03d ", sgn, d, m, s); -} + str += MString::Format("%c %03d %02d %03d ", sgn, d, m, s); +} Index: /trunk/MagicSoft/Cosy/main/MTracking.cc =================================================================== --- /trunk/MagicSoft/Cosy/main/MTracking.cc (revision 9131) +++ /trunk/MagicSoft/Cosy/main/MTracking.cc (revision 9132) @@ -3,5 +3,5 @@ #include "MLogManip.h" -#include "macs.h" +#include "dkc.h" #include "shaftencoder.h" @@ -20,45 +20,4 @@ // -------------------------------------------------------------------------- // -// request the current positions from the rotary encoders. -// use GetRePos to get the psotions. If the request fails the function -// returns kFALSE, otherwise kTRUE -// -bool MTracking::RequestRePos() -{ - for (int i=0; i<2; i++) - { - // - // Send request - // - fCosy->fMac2->RequestSDO(0x6004); - fCosy->fMac1->RequestSDO(0x6004); - - // - // Wait until the objects are received. - // - fCosy->fMac2->WaitForSdo(0x6004, 0, 500, i>0); - fCosy->fMac1->WaitForSdo(0x6004, 0, 500, i>0); - - // - // If waiting was not interrupted everything is ok. return. - // - if (!Break()) - return true; - - fCosy->PrintError(); - } - - // - // If the waiting was interrupted due to a network error, - // print some logging message. - // - if (fCosy->HasError()) - gLog << err << "ERROR - while requesting re pos from Macs (SDO #6004)" << endl; - - return false; -} - -// -------------------------------------------------------------------------- -// // Sets the tracking velocity // @@ -68,26 +27,26 @@ Bool_t MTracking::SetVelocity(const ZdAz &v) { - for (int i=0; i<2; i++) - { - // - // Send the new velocities for both axes. - // - fCosy->fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd()); // SetRpmVelocity [re/min] - fCosy->fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az()); // SetRpmVelocity [re/min] - - // - // Wait for the objects to be acknoledged. - // - fCosy->fMac2->WaitForSdo(0x3006, 1, 500, i>0); - fCosy->fMac1->WaitForSdo(0x3006, 1, 500, i>0); - - // - // If the waiting for the objects wasn't interrupted return kTRUE - // - if (!Break()) - return kTRUE; - - fCosy->PrintError(); - } + const Double_t vrzd = fCosy->fMac2->GetVelRes(); + const Double_t vraz = fCosy->fMac1->GetVelRes(); + + // + // Send the new velocities for both axes. + // + fCosy->fMac2->SendSDO(0x3006, 1, (LWORD_t)(v.Zd()*vrzd)); // SetRpmVelocity [re/min] + fCosy->fMac1->SendSDO(0x3006, 1, (LWORD_t)(v.Az()*vraz)); // SetRpmVelocity [re/min] + + // + // Wait for the objects to be acknoledged. + // + fCosy->fMac2->WaitForSdo(0x3006, 1, 100); + fCosy->fMac1->WaitForSdo(0x3006, 1, 100); + + // + // If the waiting for the objects wasn't interrupted return kTRUE + // + if (!Break()) + return kTRUE; + + fCosy->PrintError(); // @@ -117,8 +76,8 @@ // Start revolution mode // - if (!SetAccDec(fCosy->fMac2, fTrackAcc, fTrackDec)) + if (!SetAcc(fCosy->fMac2, fTrackAcc.Zd())) return false; - if (!SetAccDec(fCosy->fMac1, fTrackAcc, fTrackDec)) + if (!SetAcc(fCosy->fMac1, fTrackAcc.Az())) return false; @@ -135,4 +94,5 @@ return true; } + /* void MTracking::StopTracking() @@ -177,4 +137,5 @@ } */ + // -------------------------------------------------------------------------- // @@ -185,6 +146,14 @@ // velocities are limited to the maximum velocity. // -Bool_t MTracking::LimitSpeed(ZdAz *vt, const SlaStars &sla) const -{ +Bool_t MTracking::LimitSpeed(const ZdAz &vt, const SlaStars &sla) const +{ + // vt [deg/min] + + // We can set a maximum speed here + // And we can limit the change of the speed (which is done + // by acceleration in the drive anyway) + + return kTRUE; +/* // vt[re/min] @@ -237,92 +206,5 @@ } return rc; -} - -// -------------------------------------------------------------------- -// -// Return pointing position of the telescope based on the -// Shaftencoders with interpolation with motor encoders. -// -// GetPointingPos [re] -// -ZdAz MTracking::GetPointingPosRE(Bool_t pdo) const -{ - // Conversion factor from se to re - //const XY re = fCosy->kGearTot/fCosy->kResSE; //[re/se] - const XY re = Div(fCosy->kGearTot, fCosy->kResSE); //[re/se] - - // Check wether moving direction has changed - const bool bool1 = fCosy->fZd1->DirHasChanged(); - const bool bool2 = fCosy->fZd2->DirHasChanged(); - - // If both directions have changed reset the flags - if (bool1 && bool2) - { - fCosy->fZd1->ResetDirHasChanged(); - fCosy->fZd2->ResetDirHasChanged(); - } - - // Get shaftencoder positions - // Ignore the shaftencoder which has not yet changed its value - const Int_t pzd1 = fCosy->fZd1->GetPosDirCorrected(); - const Int_t pzd2 = fCosy->fZd2->GetPosDirCorrected(); - const Int_t paz = fCosy->fAz->GetPos(); - - const Int_t res1 = fCosy->fZd1->GetPhysRes(); - const Int_t res2 = fCosy->fZd2->GetPhysRes(); - - // Get current shaftencoder position of the telescope - Double_t seposzd1 = ((pzd1+res1/2)%res1)*re.X(); - Double_t seposzd2 = ((pzd2+res2/2)%res2)*re.X(); - //Double_t seposzd1 = pzd1*re.X(); - //Double_t seposzd2 = pzd2*re.X(); - Double_t seposaz = paz *re.Y(); - - // distance between (To+dt) and To [re] - // position time difference < 5usec - // fRePos does the synchronization between the - // Shaft- and the rotary encoders - const ZdAz repos = pdo ? fCosy->GetRePosPdo() : fCosy->GetRePos(); - - // Get rotary encoder positions - // Get stored offset if one SE has not changed its direction yet - const Int_t offset1 = fCosy->fZd1->GetOffsetDirCorrected(); - const Int_t offset2 = fCosy->fZd2->GetOffsetDirCorrected(); - - // Calculate the part of one SE which the motors moved - // since the last SE has changed its value - const Double_t offzd1 = repos.Zd() - offset1; - const Double_t offzd2 = repos.Zd() - offset2; - const Double_t offaz = repos.Az() - fCosy->fAz->GetOffset(); - - // Correct for the direction in which the motor is moving - // (in which the shaftencoders should change its values) - if (offaz<0) - seposaz += re.Y(); - if (offzd1<0) - seposzd1 += re.X(); - if (offzd2<0) - seposzd2 += re.X(); - - // If the correction exceeds one shaftencoder step stop interpolation - // of shaftencoder positions using rotary encoder values. - //ofstream fout("offsets.log", ios::app); - //fout << MTime(-1) << " " << offaz << " " << offzd1 << " " << offzd2 << endl; - /* - if (TMath::Abs(offaz)>re.Y()) - offaz = TMath::Sign(re.Y(), offaz); - if (TMath::Abs(offzd1)>re.X()) - offzd1 = TMath::Sign(re.X(), offzd1); - if (TMath::Abs(offzd2)>re.X()) - offzd2 = TMath::Sign(re.X(), offzd2); - */ - - // and interpolate the shaftencoder steps using the motor - // encoder positon (Be carefull the minus-sign is important) - seposzd1 += offzd1; - seposzd2 -= offzd2; - seposaz += offaz; - - return ZdAz((seposzd1-seposzd2)/2, seposaz); + */ } @@ -359,19 +241,4 @@ } - // - // calculate offset from present se position - // - //const ZdAz sepos = fCosy->GetSePos()*fCosy->kGearTot/fCosy->kResSE; //[re] - if (!RequestRePos()) - return; - - // Estimate Offset before starting to track - ZdAz repos = fCosy->GetRePos(); - fCosy->fZd1->SetOffset(TMath::Nint(repos.Zd())); - fCosy->fZd2->SetOffset(TMath::Nint(repos.Zd())); - fCosy->fAz->SetOffset(TMath::Nint(repos.Az())); - - fCosy->SetTrackingPosRE(GetPointingPosRE()); - // Initialize Tracker (slalib or starguider) fCosy->fRaDec = dst; @@ -402,59 +269,44 @@ // *OLD*const float dt = 1; // 1 second const float dt = 5;//3; // 2 second - while (!Break()) - { - // - // Request Target position for Now+dt - // - sla.Now(dt); - - // - // Request nominal position for this time in the future (To+dt) - // + while (!Break()/* && !fCosy->HasError() && !fCosy->HasZombie()*/) + { + /* + sla.Now(1); const ZdAz pointing = sla.CalcZdAz(fCosy->fRaDec); // [rad] ZdAz dest = fCosy->AlignTrackingPos(pointing); // fix ambiguity - - //ZdAz vcalc = sla.GetApproxVel(fCosy->fRaDec); - //vcalc *= fCosy->kGearRatio2*4./60.; // [re/min] - - float dtime = -1; - //if (kFALSE /*fUseStarguider*/) - // dtime = Starguider(sla.GetMjd(), dest); - - ZdAz repos; - if (dtime<0) - { - dest = fCosy->fBending(dest); // [rad] - if (!fCosy->CheckRange(dest)) - break; - - // Destination position at t+dt in re-units - dest *= fCosy->kGearTot/TMath::TwoPi(); // [re] - - // Request absolute position of rotary encoder from Macs - // Such that the RE position used in GetPointingPos is - // as up-to-date as possible. -// DO I NEED THIS OR IS THE PDOPOS ENOUGH? - if (!RequestRePos()) - break; - - // *NEW* offset handling - // Get current position of the telescope and - // forward this position to MCosy - ZdAz sepos = GetPointingPosRE(); //[re] - fCosy->SetTrackingPosRE(sepos); - - // distance between (To+dt) and To [re] - // position time difference < 5usec - // fRePos does the synchronization between the - // Shaft- and the rotary encoders - repos = fCosy->GetRePos(); - - // Now calculate the distance to move from now - // to a time in t+dt. - dest -= sepos; - - dtime = dt; - } + dest = fCosy->fBending(dest); // [rad] + if (!fCosy->CheckRange(dest)) + break; + dest *= 1./TMath::TwoPi(); //[rev] + fCosy->fMac2->SendSDO(0x6004, 0, (LWORD_t)(dest.Zd()*fCosy->fMac2->GetPosRes()+.5), false); + fCosy->fMac1->SendSDO(0x6004, 0, (LWORD_t)(dest.Az()*fCosy->fMac1->GetPosRes()+.5), false); + usleep(100000); // 1s + continue; + */ + + // + // Request Target position for Now+dt + // + sla.Now(dt); + + // + // Request nominal position for this time in the future (To+dt) + // + const ZdAz pointing = sla.CalcZdAz(fCosy->fRaDec); // [rad] + ZdAz dest = fCosy->AlignTrackingPos(pointing); // fix ambiguity + + //ZdAz repos; + dest = fCosy->fBending(dest); // [rad] + if (!fCosy->CheckRange(dest)) + break; + + // Destination position at t+dt in re-units + dest *= TMath::RadToDeg(); // [re] + + const ZdAz sepos = fCosy->GetSePos()*360; // [deg] + + // Now calculate the distance to move from now + // to a time in t+dt. + dest -= sepos; // @@ -462,18 +314,13 @@ // correct for the duration of RaDec2AltAz // - /* --- OLD --- */ - ZdAz v = dest*60.0/dtime; //[re/min] - /* --- NEW --- seems to work worse! */ - //const Double_t dtaz = sla.GetTime() - fCosy->fMac1->GetPosTime(); - //const Double_t dtzd = sla.GetTime() - fCosy->fMac2->GetPosTime(); - // - //ZdAz v = dest*60.0; - //v.Zd(v.Zd()/dtzd); - //v.Az(v.Az()/dtaz); - /* --- END --- */ - - //*fCosy->fOutRep << "> Dt: " << dtaz << " " << dtzd << endl; - - if (LimitSpeed(&v, sla)) + const ZdAz v = dest * 60/dt; // [deg/min] + const ZdAz vt = v/360; // [rpm] + + //Double_t kpZd = TMath::Abs(fCosy->fTrackingError.Zd()*TMath::RadToDeg()*60*4); + //Double_t kpAz = TMath::Abs(fCosy->fTrackingError.Az()*TMath::RadToDeg()*60*12); + //v.Zd(v.Zd()*(1+TMath::Min(0.3, kpZd))); + //v.Az(v.Az()*(1+TMath::Min(0.3, kpAz))); + + if (LimitSpeed(v, sla)) { gLog << dbg << "vt: " << v.Zd() << " " << v.Az() << "re/min" << endl; @@ -482,18 +329,11 @@ // - // calculate real velocity of future [re/min] - // believing the Macs manual '/4' shouldn't be necessary, but it is. - // - ZdAz vt = v; //[re'/min] - //lout << " " << vt.Zd() << " " << vt.Az() << " "; - vt.Round(); - //lout << " " << vt.Zd() << " " << vt.Az() << endl; - - // // check if the drive is fast enough to follow the star // - if (vt.Zd()>.9*fCosy->fMac1->GetVelRes() || vt.Az()>.9*fCosy->fMac2->GetVelRes()) + if (TMath::Abs(vt.Zd())>0.5*fCosy->fMac2->GetVelMaxRev() || + TMath::Abs(vt.Az())>0.5*fCosy->fMac1->GetVelMaxRev()) { - gLog << err << "ERROR - Tracking speed faster than 90% of possible maximum velocity." << endl; + gLog << err << "ERROR - Tracking speed faster than 50% of possible maximum velocity." << endl; + gLog << "Zd: " << vt.Zd() << " Az: " << vt.Az() << endl; break; } @@ -503,21 +343,6 @@ // Maybe we should attenuate the changes // - //*fCosy->fOutRep << "> SetVelocity1: " << vt.Zd() << " " << vt.Az() << endl; if (!SetVelocity(vt)) break; - //*fCosy->fOutRep << "> SetVelocity2 " << endl; - - // - // Now do 'unnecessary' things (timing) - // - fCosy->fVelocity = vt/fCosy->kGear; // [U_mot/min] - // *OLD* fVelocity = vt/kGearRatio2*4; - - if (fOut) - { - fOut->Lock("MTracking::TrackPosition"); - *fOut << "RE-REPORT 00 " << MTime(-1) << " " << repos.Zd() << " " << repos.Az() <<" " << vt.Zd() << " " << vt.Az() << endl; - fOut->UnLock("MTracking::TrackPosition"); - } // @@ -547,10 +372,4 @@ Int_t MTracking::Thread() { - if (fCosy->fZd1->IsZombieNode() && fCosy->fZd2->IsZombieNode()) - return 1; - - if (fCosy->fAz->IsZombieNode()) - return 2; - if (!fCosy->fMac1 || !fCosy->fMac2) return 3; @@ -558,11 +377,7 @@ gLog << inf2 << "- Tracking Thread started (" << MTime(-1) << ")" << endl; - //const XY re2se = fCosy->kGearTot/fCosy->kResSE; //[re/se] - SlaStars sla(fCosy->fObservatory); sla.Now(); - //ZdAz time; - ZdAz soll = sla.CalcZdAz(fCosy->fRaDec); // [rad] @@ -571,9 +386,5 @@ // bool phca1=false; - bool phca2=false; bool phcaz=false; - - //ZdAz wasse = fCosy->GetSePos(); - //ZdAz oldse = fCosy->GetSePos(); while (1) @@ -585,42 +396,27 @@ // Check for changes of the shaftencoder values - //*fCosy->fOutRep << "> ResetPosHasChanged" << endl; - fCosy->fZd1->ResetPosHasChanged(); - fCosy->fZd2->ResetPosHasChanged(); - fCosy->fAz->ResetPosHasChanged(); - //*fCosy->fOutRep << "> Check for PosHasChanged" << endl; + fCosy->fMac1->ResetHasChangedPos2(); + fCosy->fMac2->ResetHasChangedPos2(); do { - phca1 = fCosy->fZd1->PosHasChanged(); - phca2 = fCosy->fZd2->PosHasChanged(); - phcaz = fCosy->fAz->PosHasChanged(); + phcaz = fCosy->fMac1->HasChangedPos2(); + phca1 = fCosy->fMac2->HasChangedPos2(); usleep(1); + TThread::CancelPoint(); - } while (!phca1 && !phca2 && !phcaz); - - // Get time from last shaftencoder position change (position: ist) - // FIXME: Is this correct? - // time.Az(fCosy->fMac1->GetMjd()); - // time.Zd(fCosy->fMac2->GetMjd()); - - //Double_t mjd1 = fCosy->fZd1->GetMjd(); - //Double_t mjd2 = fCosy->fZd2->GetMjd(); - //Double_t mjd0 = fCosy->fAz->GetMjd(); - - Double_t mjdaz = fCosy->fMac1->GetPdoMjd();//mjd0; - Double_t mjdzd = fCosy->fMac2->GetPdoMjd();//TMath::Max(mjd1, mjd2); - - // get current position of shaftencoders (interpolated - // using motor encoders) - const ZdAz istse = GetPointingPosRE(kTRUE)/fCosy->kGearTot*TMath::TwoPi(); - //const ZdAz istse = fCosy->GetSePosPdo(); + } while (!phca1 && !phcaz); + + // get current position and corresponding time of shaftencoders + const ZdAz istse = fCosy->GetSePos()*TMath::TwoPi(); // [deg] + + const Double_t mjdaz = fCosy->fMac1->GetMjdPos2(); + const Double_t mjdzd = fCosy->fMac2->GetMjdPos2(); + // calculate offset for both axis (only one is needed) - // *NEW* offset handling - //.const ZdAz offset = istre; //(istse*re2se - istre)*weight + fRePos*(weight-1); // if Shaftencoder changed position, calculate nominal position - if (phca1 || phca2) + if (phca1) { ZdAz dummy = sla.CalcZdAz(fCosy->fRaDec, mjdzd); @@ -644,14 +440,4 @@ TThread::CancelPoint(); } - - - //fCosy->fZdAzSoll = soll; -/* - // Calculate the aligned tracking position from 'soll'-position - if (phca1 || phca2) - fCosy->fTrackingError.Zd(soll.Zd()-istse.Zd()); - if (phcaz) - fCosy->fTrackingError.Az(soll.Az()-istse.Az()); - */ } Index: /trunk/MagicSoft/Cosy/main/MTracking.h =================================================================== --- /trunk/MagicSoft/Cosy/main/MTracking.h (revision 9131) +++ /trunk/MagicSoft/Cosy/main/MTracking.h (revision 9132) @@ -17,33 +17,19 @@ { private: - Float_t fTrackAcc; - Float_t fTrackDec; - - //Int_t fSePosZd1; - //Int_t fSePosZd2; - //Int_t fSePosAz; - - //Int_t fRePosZd1; - //Int_t fRePosZd2; - //Int_t fRePosAz; - - //ZdAz fRePos; // Offset between se and re coordinate system [re] - //ZdAz fSePos; // Shaftencoder Position + ZdAz fTrackAcc; MLog *fOut; - bool RequestRePos(); bool SetVelocity(const ZdAz &v); - bool LimitSpeed(ZdAz *vt, const SlaStars &sla) const; + bool LimitSpeed(const ZdAz &vt, const SlaStars &sla) const; bool InitTracking(); - ZdAz GetPointingPosRE(Bool_t pdo=kFALSE) const; Int_t Thread(); public: - MTracking(MCosy *cosy) : MSlewing(cosy), MThread("MTracking"), fTrackAcc(0.1), fTrackDec(0.1), fOut(0) { } + MTracking(MCosy *cosy) : MSlewing(cosy), MThread("MTracking"), fTrackAcc(0, 0), fOut(0) { } void TrackPosition(const RaDec &dst); // ra, dec [rad] - void SetTrackAccDec(Float_t acc, Float_t dec) { fTrackAcc=0.1; fTrackDec=0.1; } + void SetTrackAcc(Float_t zd, Float_t az) { fTrackAcc.Zd(zd); fTrackAcc.Az(az); } void SetOut(MLog *fout) { fOut = fout; }