| 1 | #include "MTracking.h"
|
|---|
| 2 |
|
|---|
| 3 | #include "macs.h"
|
|---|
| 4 | #include "shaftencoder.h"
|
|---|
| 5 |
|
|---|
| 6 | #include "MCosy.h"
|
|---|
| 7 | #include "SlaStars.h"
|
|---|
| 8 |
|
|---|
| 9 | #include "MDriveCom.h"
|
|---|
| 10 |
|
|---|
| 11 | ClassImp(MTracking);
|
|---|
| 12 |
|
|---|
| 13 | //#define EXPERT
|
|---|
| 14 | #undef EXPERT
|
|---|
| 15 |
|
|---|
| 16 | // --------------------------------------------------------------------------
|
|---|
| 17 | //
|
|---|
| 18 | // Initializes Tracking mode
|
|---|
| 19 | //
|
|---|
| 20 | // Initializes the accelerations of both axes with 90% of the maximum
|
|---|
| 21 | // acceleration. Set the status for moving and tracking and starts thr
|
|---|
| 22 | // revolution mode.
|
|---|
| 23 | //
|
|---|
| 24 | bool MTracking::InitTracking()
|
|---|
| 25 | {
|
|---|
| 26 | // FIXME? Handling of Zombie OK?
|
|---|
| 27 | if (fCosy->fMac1->IsZombieNode() || fCosy->fMac2->IsZombieNode())
|
|---|
| 28 | return false;
|
|---|
| 29 |
|
|---|
| 30 | //
|
|---|
| 31 | // Start revolution mode
|
|---|
| 32 | //
|
|---|
| 33 | if (!SetAccDec(fCosy->fMac2, fTrackAcc, fTrackDec))
|
|---|
| 34 | return false;
|
|---|
| 35 |
|
|---|
| 36 | if (!SetAccDec(fCosy->fMac1, fTrackAcc, fTrackDec))
|
|---|
| 37 | return false;
|
|---|
| 38 |
|
|---|
| 39 | fCosy->SetStatus(MDriveCom::kMoving | MDriveCom::kTracking);
|
|---|
| 40 |
|
|---|
| 41 | fCosy->fMac2->SetRpmMode(TRUE);
|
|---|
| 42 | if (fCosy->fMac2->IsZombieNode())
|
|---|
| 43 | return false;
|
|---|
| 44 |
|
|---|
| 45 | fCosy->fMac1->SetRpmMode(TRUE);
|
|---|
| 46 | if (fCosy->fMac1->IsZombieNode())
|
|---|
| 47 | return false;
|
|---|
| 48 |
|
|---|
| 49 | return true;
|
|---|
| 50 | }
|
|---|
| 51 |
|
|---|
| 52 | // --------------------------------------------------------------------------
|
|---|
| 53 | //
|
|---|
| 54 | // Limits the speed.
|
|---|
| 55 | //
|
|---|
| 56 | // This function should work as a limiter. If a tracking error is too large
|
|---|
| 57 | // to be corrected fast enough we would get enormous velocities. These
|
|---|
| 58 | // velocities are limited to the maximum velocity.
|
|---|
| 59 | //
|
|---|
| 60 | Bool_t MTracking::LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const
|
|---|
| 61 | {
|
|---|
| 62 | Bool_t rc = kFALSE;
|
|---|
| 63 |
|
|---|
| 64 | //
|
|---|
| 65 | // How to limit the speed. If the wind comes and blowes
|
|---|
| 66 | // we cannot forbid changing of the sign. But on the other hand
|
|---|
| 67 | // we don't want fast changes!
|
|---|
| 68 | //
|
|---|
| 69 | ULong_t vrzd = fCosy->fMac1->GetVelRes();
|
|---|
| 70 | ULong_t vraz = fCosy->fMac2->GetVelRes();
|
|---|
| 71 |
|
|---|
| 72 | #define sgn(x) (x<0?-1:1)
|
|---|
| 73 |
|
|---|
| 74 | //
|
|---|
| 75 | // When speed changes sign, the maximum allowed speed
|
|---|
| 76 | // is 25% of the |v|
|
|---|
| 77 | //
|
|---|
| 78 | //const Float_t limit = 0.25;
|
|---|
| 79 |
|
|---|
| 80 | //
|
|---|
| 81 | // The maximum allowed speed while tracking is 10%
|
|---|
| 82 | //
|
|---|
| 83 | const Float_t maxtrack = 0.1;
|
|---|
| 84 |
|
|---|
| 85 | if (fabs(vt->Az()) > maxtrack*vraz)
|
|---|
| 86 | {
|
|---|
| 87 | lout << "Warning: Azimuth speed limit (" << maxtrack*100 << "%) exceeded (" << fabs(vt->Az()) << " > " << maxtrack*vraz << ")... limited." << endl;
|
|---|
| 88 | vt->Az(maxtrack*vraz*sgn(vcalc.Az()));
|
|---|
| 89 | rc=kTRUE;
|
|---|
| 90 | }
|
|---|
| 91 | if (fabs(vt->Zd()) > maxtrack*vrzd)
|
|---|
| 92 | {
|
|---|
| 93 | lout << "Warning: Altitude speed limit (" << maxtrack*100 << "%) exceeded (" << fabs(vt->Zd()) <<" > " << maxtrack*vrzd << ")... limited." << endl;
|
|---|
| 94 | vt->Zd(maxtrack*vrzd*sgn(vcalc.Zd()));
|
|---|
| 95 | rc=kTRUE;
|
|---|
| 96 | }
|
|---|
| 97 | return rc;
|
|---|
| 98 | }
|
|---|
| 99 |
|
|---|
| 100 | // --------------------------------------------------------------------------
|
|---|
| 101 | //
|
|---|
| 102 | // Sets the tracking velocity
|
|---|
| 103 | //
|
|---|
| 104 | // The velocities are given in a ZdAz object in re/min. Return kTRUE
|
|---|
| 105 | // in case of success, kFALSE in case of failure.
|
|---|
| 106 | //
|
|---|
| 107 | Bool_t MTracking::SetVelocity(const ZdAz &v)
|
|---|
| 108 | {
|
|---|
| 109 | //
|
|---|
| 110 | // Send the new velocities for both axes.
|
|---|
| 111 | //
|
|---|
| 112 | fCosy->fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd()); // SetRpmVelocity [re/min]
|
|---|
| 113 | fCosy->fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az()); // SetRpmVelocity [re/min]
|
|---|
| 114 |
|
|---|
| 115 | //
|
|---|
| 116 | // Wait for the objects to be acknoledged.
|
|---|
| 117 | //
|
|---|
| 118 | fCosy->fMac2->WaitForSdo(0x3006, 1);
|
|---|
| 119 | fCosy->fMac1->WaitForSdo(0x3006, 1);
|
|---|
| 120 |
|
|---|
| 121 | //
|
|---|
| 122 | // If the waiting for the objects wasn't interrupted return kTRUE
|
|---|
| 123 | //
|
|---|
| 124 | if (!Break())
|
|---|
| 125 | return kTRUE;
|
|---|
| 126 |
|
|---|
| 127 | //
|
|---|
| 128 | // print a message if the interruption was due to a Can-node Error
|
|---|
| 129 | //
|
|---|
| 130 | if (fCosy->HasError())
|
|---|
| 131 | lout << "Error while setting tracking velocity (SDO #3006)" << endl;
|
|---|
| 132 |
|
|---|
| 133 | return kFALSE;
|
|---|
| 134 | }
|
|---|
| 135 |
|
|---|
| 136 | void MTracking::TrackPosition(const RaDec &dst) // ra, dec [rad]
|
|---|
| 137 | {
|
|---|
| 138 | SlaStars sla(fCosy->fObservatory);
|
|---|
| 139 |
|
|---|
| 140 | //
|
|---|
| 141 | // Position to actual position
|
|---|
| 142 | //
|
|---|
| 143 | sla.Now();
|
|---|
| 144 | ZdAz dest = sla.CalcZdAz(dst);
|
|---|
| 145 |
|
|---|
| 146 | lout << sla.GetTime() << ": Track Position " << dst.Ra()*kRad2Deg/15 << "h, " << dst.Dec()*kRad2Deg <<"deg" << endl;
|
|---|
| 147 |
|
|---|
| 148 | // az between -180 and 180
|
|---|
| 149 | if (dst.Dec()>sla.GetPhi() && dest.Az()<0)
|
|---|
| 150 | {
|
|---|
| 151 | // align az between (roughly) 60 and 320
|
|---|
| 152 | lout << "Adding 360deg to Azimuth " << dest.Az()*kRad2Deg << endl;
|
|---|
| 153 | dest.Az(dest.Az() + TMath::Pi()*2);
|
|---|
| 154 | }
|
|---|
| 155 | /*
|
|---|
| 156 | // FIXME: Determin tracking start point by star culmination
|
|---|
| 157 | if (dest.Az()<-TMath::Pi()/2)
|
|---|
| 158 | {
|
|---|
| 159 | lout << "Adding 360deg to Azimuth " << dest.Az()*kRad2Deg << endl;
|
|---|
| 160 | dest.Az(dest.Az() + TMath::Pi()*2);
|
|---|
| 161 | }
|
|---|
| 162 |
|
|---|
| 163 | if (dest.Az()>3*TMath::Pi()/2)
|
|---|
| 164 | {
|
|---|
| 165 | lout << "Substracting 360deg to Azimuth " << dest.Az()*kRad2Deg << endl;
|
|---|
| 166 | dest.Az(dest.Az() -TMath::Pi()*2);
|
|---|
| 167 | }
|
|---|
| 168 | */
|
|---|
| 169 | if (!SetPosition(dest, kTRUE))
|
|---|
| 170 | //if (!SetPosition(dest, kFALSE))
|
|---|
| 171 | {
|
|---|
| 172 | lout << "Error: Cannot start tracking, positioning failed." << endl;
|
|---|
| 173 | return;
|
|---|
| 174 | }
|
|---|
| 175 |
|
|---|
| 176 | //
|
|---|
| 177 | // calculate offset from present se position
|
|---|
| 178 | //
|
|---|
| 179 | const ZdAz sepos = fCosy->GetSePos()*fCosy->kGearRatio;
|
|---|
| 180 |
|
|---|
| 181 | if (!fCosy->RequestRePos())
|
|---|
| 182 | return;
|
|---|
| 183 |
|
|---|
| 184 | //
|
|---|
| 185 | // Estimate Offset before starting to track
|
|---|
| 186 | //
|
|---|
| 187 | fCosy->fOffset = sepos-fCosy->GetRePos();
|
|---|
| 188 |
|
|---|
| 189 | /*
|
|---|
| 190 | cout << "Sepos: " << sepos.Zd() << "re, " << sepos.Az() << "re" << endl;
|
|---|
| 191 | cout << "Repos: " << repos.Zd() << "re, " << repos.Az() << "re" << endl;
|
|---|
| 192 | cout << "Offset: " << fOffset.Zd() << "re, " << fOffset.Az() << "re" << endl;
|
|---|
| 193 | */
|
|---|
| 194 |
|
|---|
| 195 | //
|
|---|
| 196 | // Init accelerations and Rpm Mode
|
|---|
| 197 | //
|
|---|
| 198 | if (!InitTracking())
|
|---|
| 199 | {
|
|---|
| 200 | fCosy->StopMovement();
|
|---|
| 201 | return;
|
|---|
| 202 | }
|
|---|
| 203 |
|
|---|
| 204 | XY xy(Rad2Deg(dst.Ra())*24/360, Rad2Deg(dst.Dec()));
|
|---|
| 205 |
|
|---|
| 206 | sla.Now();
|
|---|
| 207 | // lout << sla.GetTime() << " - Start tracking:";
|
|---|
| 208 | // lout << " Ra: " << xy.X() << "h " << "Dec: " << xy.Y() << "\xb0" << endl;
|
|---|
| 209 |
|
|---|
| 210 | /*#ifdef EXPERT
|
|---|
| 211 | ofstream fout("coordinates.txt");
|
|---|
| 212 | fout << xy;
|
|---|
| 213 | fout.close();
|
|---|
| 214 | #endif
|
|---|
| 215 | */ //
|
|---|
| 216 | // Initialize Tracker (slalib or starguider)
|
|---|
| 217 | //
|
|---|
| 218 | fCosy->fRaDec = dst;
|
|---|
| 219 |
|
|---|
| 220 | // StartThread
|
|---|
| 221 | Start();
|
|---|
| 222 |
|
|---|
| 223 | ZdAz pos = sla.CalcZdAz(fCosy->fRaDec);
|
|---|
| 224 |
|
|---|
| 225 | lout << sla.GetTime() << " - Start Tracking: Ra=" <<xy.X() << "h Dec=";
|
|---|
| 226 | lout << xy.Y() << "\xb0 @ Zd=" << pos.Zd()*kRad2Deg <<"deg Az=" << pos.Az()*kRad2Deg <<"deg" << endl;
|
|---|
| 227 |
|
|---|
| 228 | //--- ofstream fout("log/cosy.pos");
|
|---|
| 229 | //--- fout << "Tracking:";
|
|---|
| 230 | //--- fout << " Ra: " << Rad2Deg(dst.Ra()) << "\x9c ";
|
|---|
| 231 | //--- fout << "Dec: " << Rad2Deg(dst.Dec()) << "\x9c" << endl << endl;
|
|---|
| 232 | //--- fout << " Mjd/10ms V/re/min/4" << endl;
|
|---|
| 233 |
|
|---|
| 234 | //
|
|---|
| 235 | // We want to reach the theoretical position exactly in about 0.5s
|
|---|
| 236 | //
|
|---|
| 237 | // *OLD*const float dt = 1; // 1 second
|
|---|
| 238 | const float dt = 5;//3; // 2 second
|
|---|
| 239 | while (!Break())
|
|---|
| 240 | {
|
|---|
| 241 | //
|
|---|
| 242 | // Request Target position for this moment
|
|---|
| 243 | //
|
|---|
| 244 | sla.Now(dt);
|
|---|
| 245 |
|
|---|
| 246 | //
|
|---|
| 247 | // Request theoretical Position for a time in the future (To+dt) from CPU
|
|---|
| 248 | //
|
|---|
| 249 | const ZdAz pointing = sla.CalcZdAz(fCosy->fRaDec); // soll pointing [rad]
|
|---|
| 250 |
|
|---|
| 251 | //lout << sla.GetTime() << pointing.Zd()*kRad2Deg << " " << pointing.Az()*kRad2Deg << endl;
|
|---|
| 252 | /*
|
|---|
| 253 | ZdAz dest;
|
|---|
| 254 | if (!AlignTrackingPos(pointing, dest))
|
|---|
| 255 | break;
|
|---|
| 256 | */
|
|---|
| 257 | ZdAz dest = fCosy->AlignTrackingPos(pointing);
|
|---|
| 258 |
|
|---|
| 259 | // lout << "DEST: " << dest.Zd()*kRad2Deg << " " <<dest.Az()*kRad2Deg << endl;
|
|---|
| 260 |
|
|---|
| 261 | ZdAz vcalc = sla.GetApproxVel(fCosy->fRaDec) * fCosy->kGearRatio2*4./60.;
|
|---|
| 262 | //lout << "Vcalc: " << dest.Zd() << " " << dest.Az() << endl;
|
|---|
| 263 | vcalc *= fCosy->kGearRatio2*4./60.; // [re/min]
|
|---|
| 264 |
|
|---|
| 265 | float dtime = -1;
|
|---|
| 266 | //if (kFALSE /*fUseStarguider*/)
|
|---|
| 267 | // dtime = Starguider(sla.GetMjd(), dest);
|
|---|
| 268 |
|
|---|
| 269 | if (dtime<0)
|
|---|
| 270 | {
|
|---|
| 271 | dest = fCosy->fBending(dest); // [rad]
|
|---|
| 272 |
|
|---|
| 273 | //lout << "DEST-BEND: " << dest.Zd()*kRad2Deg << " " <<dest.Az()*kRad2Deg << endl;
|
|---|
| 274 |
|
|---|
| 275 | if (!fCosy->CheckRange(dest))
|
|---|
| 276 | break;
|
|---|
| 277 |
|
|---|
| 278 | dest *= 16384/TMath::Pi()/2; // [se]
|
|---|
| 279 | dest *= fCosy->kGearRatio; // [re]
|
|---|
| 280 |
|
|---|
| 281 | *fCosy->fOutRep << "> ReqRePos1 " << endl;
|
|---|
| 282 |
|
|---|
| 283 | //
|
|---|
| 284 | // Request absolute position of rotary encoder from Macs
|
|---|
| 285 | //
|
|---|
| 286 | if (!fCosy->RequestRePos())
|
|---|
| 287 | break;
|
|---|
| 288 |
|
|---|
| 289 | *fCosy->fOutRep << "> ReqRePos2 " << endl;
|
|---|
| 290 |
|
|---|
| 291 | //
|
|---|
| 292 | // distance between (To+dt) and To [re]
|
|---|
| 293 | // position time difference < 5usec
|
|---|
| 294 | // fOffset does the synchronization between the
|
|---|
| 295 | // Shaft- and the rotary encoders
|
|---|
| 296 | dest -= fCosy->GetRePos() + fCosy->fOffset;
|
|---|
| 297 |
|
|---|
| 298 | dtime = dt;
|
|---|
| 299 |
|
|---|
| 300 | ZdAz repos = fCosy->GetRePos();
|
|---|
| 301 | // lout << "Repos: " << repos.Zd()/kGearRatio.X() << " " << repos.Az()*kGearRatio.Y() << endl;
|
|---|
| 302 | // repos /= kGearRatio;
|
|---|
| 303 | repos /= 16384/TMath::Pi()/2;
|
|---|
| 304 | repos *= kRad2Deg;
|
|---|
| 305 | }
|
|---|
| 306 |
|
|---|
| 307 | //
|
|---|
| 308 | // Velocity to go [re/min] to reach the right position at time t+dt
|
|---|
| 309 | // correct for the duration of RaDec2AltAz
|
|---|
| 310 | //
|
|---|
| 311 | const ZdAz v = dest*60.0/(dtime/*-(fMac2->GetTime()-sla)*/);
|
|---|
| 312 |
|
|---|
| 313 | //
|
|---|
| 314 | // calculate real velocity of future [re/min]
|
|---|
| 315 | // believing the Macs manual '/4' shouldn't be necessary, but it is.
|
|---|
| 316 | //
|
|---|
| 317 | ZdAz vt = v/4;
|
|---|
| 318 | if (LimitSpeed(&vt, vcalc))
|
|---|
| 319 | {
|
|---|
| 320 | lout << "Vcalc: " << vcalc.Zd() << " " << vcalc.Az() << "re/min" <<endl;
|
|---|
| 321 | lout << "vt: " << vt.Zd() << " " << vt.Az() << "re/min" << endl;
|
|---|
| 322 | lout << "Dest: " << dest.Zd() << " " << dest.Az() << endl;
|
|---|
| 323 | }
|
|---|
| 324 | vt.Round();
|
|---|
| 325 |
|
|---|
| 326 | //
|
|---|
| 327 | // check if the drive is fast enough to follow the star
|
|---|
| 328 | //
|
|---|
| 329 | if (vt.Zd()>.9*fCosy->fMac1->GetVelRes() || vt.Az()>.9*fCosy->fMac2->GetVelRes())
|
|---|
| 330 | {
|
|---|
| 331 | lout << "Error: Tracking speed faster than 90% of possible maximum velocity." << endl;
|
|---|
| 332 | break;
|
|---|
| 333 | }
|
|---|
| 334 |
|
|---|
| 335 | //
|
|---|
| 336 | // Set theoretical velocity (as early after calculation as possible)
|
|---|
| 337 | // Maybe we should attenuate the changes
|
|---|
| 338 | //
|
|---|
| 339 | *fCosy->fOutRep << "> SetVelocity1 " << endl;
|
|---|
| 340 | if (!SetVelocity(vt))
|
|---|
| 341 | break;
|
|---|
| 342 | *fCosy->fOutRep << "> SetVelocity2 " << endl;
|
|---|
| 343 |
|
|---|
| 344 | //
|
|---|
| 345 | // Now do 'unnecessary' things
|
|---|
| 346 | //
|
|---|
| 347 | fCosy->fVelocity = vt/fCosy->kGearRatio2*4;
|
|---|
| 348 |
|
|---|
| 349 | //--- const double mjd = fMac2->GetMjd();
|
|---|
| 350 | //--- fout << setprecision(15) << setw(17) << mjd*60.*60.*24. << " ";
|
|---|
| 351 | //--- fout << setw(4) << vt.Zd() << " ";
|
|---|
| 352 | //--- fout << setw(4) << vt.Az() << endl;
|
|---|
| 353 | //
|
|---|
| 354 | // FIXME? Calculate an accuracy for the tracking system?
|
|---|
| 355 | // How good do we reach the calculated position in 'real'
|
|---|
| 356 | // re valus?
|
|---|
| 357 | //
|
|---|
| 358 |
|
|---|
| 359 |
|
|---|
| 360 | //
|
|---|
| 361 | // Update speed as often as possible.
|
|---|
| 362 | // make sure, that dt is around 10 times larger than the
|
|---|
| 363 | // update time
|
|---|
| 364 | //
|
|---|
| 365 | //
|
|---|
| 366 | // The loop should not be executed faster than the ramp of
|
|---|
| 367 | // a change in the velocity can be followed.
|
|---|
| 368 | // (This is important on fast machines >500MHz)
|
|---|
| 369 | //
|
|---|
| 370 | /*
|
|---|
| 371 | MTimeout t(1000);
|
|---|
| 372 | while (!t.HasTimedOut())
|
|---|
| 373 | usleep(1);
|
|---|
| 374 | */
|
|---|
| 375 | usleep(1000000); // 1s
|
|---|
| 376 | cout << "." << flush;
|
|---|
| 377 | //usleep(50000); // 0.05s
|
|---|
| 378 | }
|
|---|
| 379 |
|
|---|
| 380 | sla.Now();
|
|---|
| 381 |
|
|---|
| 382 | // StopThread
|
|---|
| 383 | Stop();
|
|---|
| 384 |
|
|---|
| 385 | fCosy->StopMovement();
|
|---|
| 386 |
|
|---|
| 387 | lout << sla.GetTime() << " - Tracking stopped." << endl;
|
|---|
| 388 | }
|
|---|
| 389 |
|
|---|
| 390 | void *MTracking::Thread()
|
|---|
| 391 | {
|
|---|
| 392 | if (fCosy->fZd1->IsZombieNode() && fCosy->fZd2->IsZombieNode())
|
|---|
| 393 | return (void*)1;
|
|---|
| 394 |
|
|---|
| 395 | if (fCosy->fAz->IsZombieNode())
|
|---|
| 396 | return (void*)2;
|
|---|
| 397 |
|
|---|
| 398 | if (!fCosy->fMac1 || !fCosy->fMac2)
|
|---|
| 399 | return (void*)3;
|
|---|
| 400 |
|
|---|
| 401 | lout << "- Tracking Thread started..." << endl;
|
|---|
| 402 |
|
|---|
| 403 | SlaStars sla(fCosy->fObservatory);
|
|---|
| 404 | sla.Now();
|
|---|
| 405 |
|
|---|
| 406 | ZdAz old;
|
|---|
| 407 | ZdAz ist = fCosy->GetSePos(); // [se]
|
|---|
| 408 |
|
|---|
| 409 | ZdAz time;
|
|---|
| 410 |
|
|---|
| 411 | ZdAz sollzd = sla.CalcZdAz(fCosy->fRaDec); // [rad]
|
|---|
| 412 | ZdAz sollaz = sollzd; // [rad]
|
|---|
| 413 |
|
|---|
| 414 | //
|
|---|
| 415 | // only update fTrackingError while tracking
|
|---|
| 416 | //
|
|---|
| 417 | bool phca1=false;
|
|---|
| 418 | bool phca2=false;
|
|---|
| 419 | bool phcaz=false;
|
|---|
| 420 |
|
|---|
| 421 | while (!HasStopFlag())
|
|---|
| 422 | {
|
|---|
| 423 | //
|
|---|
| 424 | // Make changes (eg wind) smoother - attenuation of control function
|
|---|
| 425 | //
|
|---|
| 426 | const float weight = 1.; //0.3;
|
|---|
| 427 |
|
|---|
| 428 | //
|
|---|
| 429 | // This is the time constant which defines how fast
|
|---|
| 430 | // you correct for external influences (like wind)
|
|---|
| 431 | //
|
|---|
| 432 | *fCosy->fOutRep << "> ResetPosHasChanged" << endl;
|
|---|
| 433 | fCosy->fZd1->ResetPosHasChanged();
|
|---|
| 434 | fCosy->fZd2->ResetPosHasChanged();
|
|---|
| 435 | fCosy->fAz->ResetPosHasChanged();
|
|---|
| 436 | *fCosy->fOutRep << "> Check for PosHasChanged" << endl;
|
|---|
| 437 | do
|
|---|
| 438 | {
|
|---|
| 439 | phca1 = fCosy->fZd1->PosHasChanged();
|
|---|
| 440 | phca2 = fCosy->fZd2->PosHasChanged();
|
|---|
| 441 | phcaz = fCosy->fAz->PosHasChanged();
|
|---|
| 442 | usleep(1);
|
|---|
| 443 | } while (!phca1 && !phca2 && !phcaz && !HasStopFlag());
|
|---|
| 444 |
|
|---|
| 445 | //---usleep(100000); // 0.1s
|
|---|
| 446 |
|
|---|
| 447 | *fCosy->fOutRep << "> Do Calculation" << endl;
|
|---|
| 448 |
|
|---|
| 449 | //
|
|---|
| 450 | // get position, where we are
|
|---|
| 451 | //
|
|---|
| 452 | old = ist;
|
|---|
| 453 | ist = fCosy->GetSePos(); // [se]
|
|---|
| 454 |
|
|---|
| 455 | //
|
|---|
| 456 | // if the position didn't change continue
|
|---|
| 457 | //
|
|---|
| 458 | /*---
|
|---|
| 459 | if ((int)ist.Zd() == (int)old.Zd() &&
|
|---|
| 460 | (int)ist.Az() == (int)old.Az())
|
|---|
| 461 | continue;
|
|---|
| 462 | */
|
|---|
| 463 | ZdAz istre = fCosy->GetRePosPdo();
|
|---|
| 464 |
|
|---|
| 465 | //
|
|---|
| 466 | // Get time from last shaftencoder position change (position: ist)
|
|---|
| 467 | // FIXME: I cannot take the avarage
|
|---|
| 468 | //
|
|---|
| 469 | // FIXME
|
|---|
| 470 | //time.Zd(fZd1->GetMjd());
|
|---|
| 471 | /* OLD* */
|
|---|
| 472 | if (fCosy->fZd1->GetMjd()>fCosy->fZd2->GetMjd())
|
|---|
| 473 | time.Zd(fCosy->fZd1->GetMjd());
|
|---|
| 474 | else
|
|---|
| 475 | time.Zd(fCosy->fZd2->GetMjd());
|
|---|
| 476 |
|
|---|
| 477 | //time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0);
|
|---|
| 478 | time.Az(fCosy->fAz->GetMjd());
|
|---|
| 479 |
|
|---|
| 480 | //
|
|---|
| 481 | // if Shaftencoder changed position
|
|---|
| 482 | // calculate were we should be
|
|---|
| 483 | //
|
|---|
| 484 | if (phca1 || phca2 /*(int)ist.Zd() != (int)old.Zd()*/)
|
|---|
| 485 | {
|
|---|
| 486 | sollzd = sla.CalcZdAz(fCosy->fRaDec, time.Zd()); // [rad]
|
|---|
| 487 | /*
|
|---|
| 488 | ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
|
|---|
| 489 | sollzd = CorrectTarget(ist, dummy); // [se]
|
|---|
| 490 | */
|
|---|
| 491 | fCosy->fOffset.Zd(fCosy->fOffset.Zd()*(1.-weight)+(ist.Zd()*fCosy->kGearRatio.X()-istre.Zd())*weight);
|
|---|
| 492 | }
|
|---|
| 493 |
|
|---|
| 494 | if (phcaz /*(int)ist.Az() != (int)old.Az()*/)
|
|---|
| 495 | {
|
|---|
| 496 | sollaz = sla.CalcZdAz(fCosy->fRaDec, time.Az()); // [rad]
|
|---|
| 497 | /*
|
|---|
| 498 | ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
|
|---|
| 499 | sollaz = CorrectTarget(ist, dummy); // [se]
|
|---|
| 500 | */
|
|---|
| 501 | fCosy->fOffset.Az(fCosy->fOffset.Az()*(1.-weight)+(ist.Az()*fCosy->kGearRatio.Y()-istre.Az())*weight);
|
|---|
| 502 | }
|
|---|
| 503 |
|
|---|
| 504 | ZdAz soll(sollzd.Zd(), sollaz.Az()); // [rad]
|
|---|
| 505 |
|
|---|
| 506 | fCosy->fZdAzSoll = fCosy->AlignTrackingPos(soll);
|
|---|
| 507 |
|
|---|
| 508 | ist *= TMath::Pi()*2/16384;
|
|---|
| 509 | soll = fCosy->fBending(fCosy->fZdAzSoll);
|
|---|
| 510 | fCosy->fTrackingError.Set(ist.Zd()-soll.Zd(), ist.Az()-soll.Az());
|
|---|
| 511 |
|
|---|
| 512 | //--- fout << setprecision(15) << setw(17) << time.Zd()*60.*60.*24. << " ";
|
|---|
| 513 | //--- fout << setprecision(5) << setw(7) << fTrackingError.Zd() << " ";
|
|---|
| 514 | //--- fout << setprecision(15) << setw(17) << time.Az()*60.*60.*24. << " ";
|
|---|
| 515 | //--- fout << setprecision(5) << setw(7) << fTrackingError.Az() << endl;
|
|---|
| 516 | }
|
|---|
| 517 |
|
|---|
| 518 | lout << "- Tracking Thread done." << endl;
|
|---|
| 519 |
|
|---|
| 520 | return 0;
|
|---|
| 521 | }
|
|---|
