Changeset 18363


Ignore:
Timestamp:
11/07/15 18:30:03 (9 years ago)
Author:
tbretz
Message:
Removed some obsolete comments.
File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/FACT++/src/drivectrl.cc

    r18362 r18363  
    194194
    195195public:
    196     /*
    197     Source    source;        // Informations about source to track
    198     Planets_t planet;        // Id of the planet if tracking a planet
    199     double    start;         // Starting time of wobble observation      [mjd]
    200     double    orbit_period;  // Time for one revolution (0:off)          [day]
    201     double    wobble_offset; // Distance of wobble position              [rad]
    202     double    wobble_angle;  // Starting phi angle of wobble observation [rad]
    203 
    204     // Source to track with all constraints
    205     //RaDec     source;        // Catalog coordinates (J2000, FK5)     [rad/rad] source position
    206 
    207     // Pointing direction of the opticl axis of the telescope
    208     RaDec     pointing;      // Catalog coordinates (J2000, FK5)        [rad/rad] pointing position
    209     RaDecHa   apparent;      // Apparent position on the sky            [rad/rad]
    210     ZdAz      sky;           // Apparent position on the sky            [rad/rad]
    211     Encoder   mount;         // Encoder position corresponding to 'sky' [deg/deg]
    212     double    mjd;
    213     */
    214196
    215197    void Load(const string &name)
    … …  
    286268        return (M_PI/2-alt < 0 ? -val : val);
    287269    }
    288 /*
    289     AltAz CalcAnAw(const AltAz &p) const
    290     {
    291         // Corrections for AN and AW without approximations
    292         // as done by Patrick Wallace. The approximation cannot
    293         // be used for MAGIC because the correction angle
    294         // AW (~1.5deg) is not small enough.
    295 
    296         const double theta0 = M_PI/2-p.alt;
    297         const double phi0   = p.az;
    298 
    299         const double st = sin(theta0);
    300         const double ct = cos(theta0);
    301         const double sp = sin(phi0);
    302         const double cp = cos(phi0);
    303 
    304         double x = st * cp;
    305         double y = st * sp;
    306         double z = ct;
    307 
    308         // RotateY
    309         const double sy = sin(fAn);
    310         const double cy = cos(fAn);
    311         const double zz = z;
    312         z = cy*zz - sy*x;
    313         x = sy*zz + cy*x;
    314 
    315         // RotateX
    316         const double sx = sin(fAw);
    317         const double cx = cos(fAw);
    318         const double yy = y;
    319         y = cx*yy - sx*z;
    320         z = sx*yy + cx*z;
    321 
    322         const double xy     = hypot(x,  y);
    323         const double theta1 = atan2(xy, z);
    324         const double phi1   = atan2(y,  x);
    325 
    326         double dalt = theta0 - theta1;
    327         double daz  = phi1   - phi0;
    328 
    329         if (daz>M_PI)
    330             daz -= 2*M_PI;
    331         if (daz<-M_PI)
    332             daz += 2*M_PI;
    333 
    334         return AltAz(dalt, daz);
    335     }
    336 */
     270
    337271    Encoder SkyToMount(AltAz p)
    338272    {
    … …  
    541475        return out;
    542476    }
    543 
    544     //PointingModel(double ra, double dec) : planet(kENone), pointing(ra, dec) { }
    545     //PointingModel(const Nova::ZdAzPosn &za) : planet(kENone), sky(za.zd* M_PI/180,za.az* M_PI/180) { SkyToMount(); }
    546     //PointingModel(const Nova::EquPosn  &rd) : planet(kENone), pointing(rd.ra* M_PI/180, rd.dec* M_PI/180) { }
    547477};
    548478
    549479// ------------------------------------------------------------------------
    550 
    551 /*
    552 #define kEMERGENCY     0x1
    553 #define kPDO1_TX       0x3
    554 #define kPDO2_TX       0x5
    555 #define kPDO3_TX       0x7
    556 #define kPDO4_TX       0x9
    557 #define kSDO_RX        0xb  // this is used to set data of the shaft encoder
    558 #define kSDO_TX        0xc  // this is used to request data from the shaft encoder
    559 #define kNodeguard     0xe
    560 
    561 // NMT: no answer to NMT command
    562 // cob-id=0, command (byte), id (byte)
    563 #define kNMT_START     0x01 // change to operational state (start)
    564 #define kNMT_STOP      0x02 // change to prepared    state (stop)
    565 #define kNMT_PREOP     0x80 // enter pre operational state
    566 #define kNMT_RESET     0x81 // reset node (set parameter to power on values)
    567 #define kNMT_REINIT    0x82 // reset communication of node (set communication parameters to power on values)
    568 
    569 // command for SDOs
    570 #define kSDO_LEN4      0x3
    571 #define kSDO_LEN2      0xb
    572 #define kSDO_LEN1      0xf
    573 
    574 #define kSDO_RXm4      0x22  // this is used with SDO_TX to send a maximum of 4 bytes
    575 #define kSDO_RX4       0x20|kSDO_LEN4  // this is used with SDO_TX to send 4 bytes
    576 #define kSDO_RX2       0x20|kSDO_LEN2  // this is used with SDO_TX to send 2 bytes
    577 #define kSDO_RX1       0x20|kSDO_LEN1  // this is used with SDO_TX to send 1 byte
    578 #define kSDO_RX_DATA   0x40            // this is used to request parameters from the encoder
    579 #define kSDO_TX4       0x40|kSDO_LEN4  // answer to 0x40 with 4 bytes of data
    580 #define kSDO_TX3       0x40|kSDO_LEN2  // answer to 0x40 with 2 bytes of data
    581 #define kSDO_TX1       0x40|kSDO_LEN1  // answer to 0x40 with 1 byte  of data
    582 #define kSDO_TX_OK     0x60            // answer to a SDO_TX message
    583 #define kSDO_TX_ERROR  0x80            // error message (instead of 0x60)
    584 */
    585 
    586480
    587481
    … …  
    640534        kTxSdo1      = 0x20|0xf,
    641535    };
    642 
    643     //const Int_t id1 = env.GetValue("Az_Id", 1);
    644     //const Int_t id2 = env.GetValue("Zd_Id", 3);
    645536
    646537    void SendCanFrame(uint16_t cobid,
    … …  
    11501041
    11511042        const uint16_t desc  = fData[1]<<8 | fData[2];
    1152         //const uint8_t  rtr   = (desc>>4)&1;
    1153         //const uint8_t  len   =  desc&0xf;
    11541043        const uint16_t cobid = desc>>5;
    11551044
    … …  
    20901979    }
    20911980
    2092 /*
    2093     int StartWobble(const double &srcra,  const double &srcdec,
    2094                     const double &woboff, const double &wobang,
    2095                     const string name="")
    2096     {
    2097 
    2098 
    2099         const double ra  = srcra *M_PI/ 12;
    2100         const double dec = srcdec*M_PI/180;
    2101         const double off = woboff*M_PI/180;
    2102         const double dir = wobang*M_PI/180;
    2103 
    2104         if (off==0)
    2105         {
    2106             const array<double, 6> dim = {{ srcra, srcdec, srcra, srcdec, 0, 0 }};
    2107             UpdateSource(dim, src.name);
    2108             return StartTracking(src);//srcra, srcdec);
    2109         }
    2110 
    2111         const double cosdir = cos(dir);
    2112         const double sindir = sin(dir);
    2113         const double cosoff = cos(off);
    2114         const double sinoff = sin(off);
    2115         const double cosdec = cos(dec);
    2116         const double sindec = sin(dec);
    2117 
    2118         const double sintheta = sindec*cosoff + cosdec*sinoff*cosdir;
    2119         if (sintheta >= 1)
    2120         {
    2121             T::Error("cos(Zd) > 1");
    2122             return T::GetCurrentState();
    2123         }
    2124 
    2125         const double costheta = sqrt(1 - sintheta*sintheta);
    2126 
    2127         const double cosdeltara = (cosoff - sindec*sintheta)/(cosdec*costheta);
    2128         const double sindeltara = sindir*sinoff/costheta;
    2129 
    2130         const double ndec = asin(sintheta)*180/M_PI;
    2131         const double nra  = (atan2(sindeltara, cosdeltara) + ra)*12/M_PI;
    2132 
    2133         const array<double, 6> dim = {{ srcra, srcdec, nra, ndec, woboff, wobang }};
    2134         UpdateSource(dim, src.name);
    2135 
    2136         return StartTracking(nra, ndec);
    2137     }
    2138 */
    2139 
    21401981    int Wobble(const EventImp &evt)
    21411982    {
    … …  
    25102351        T::Out() << Time().GetAsStr("    %H:%M:%S.%f") << " - Position    [deg]    " << pos.zd*180/M_PI << " " << pos.az*180/M_PI << endl;
    25112352    }
    2512 /*
    2513     void TrackingLoop2(const boost::system::error_code &error=boost::system::error_code())
    2514     {
    2515         if (error==ba::error::basic_errors::operation_aborted)
    2516             return;
    2517 
    2518         if (error)
    2519         {
    2520             ostringstream str;
    2521             str << "TrackingLoop: " << error.message() << " (" << error << ")";// << endl;
    2522             T::Error(str);
    2523             return;
    2524         }
    2525 
    2526         if (T::GetCurrentState()!=State::kTracking &&
    2527             T::GetCurrentState()!=State::kOnTrack)
    2528             return;
    2529 
    2530         //
    2531         // Update speed as often as possible.
    2532         // make sure, that dt is around 10 times larger than the
    2533         // update time
    2534         //
    2535         // The loop should not be executed faster than the ramp of
    2536         // a change in the velocity can be followed.
    2537         //
    2538         fTrackingLoop.expires_from_now(boost::posix_time::milliseconds(2000/8));
    2539 
    2540         // We want to reach the theoretical position exactly in about 7.6s
    2541         const float dt = 2.0;
    2542 
    2543         // Get current position and corresponding time
    2544         const double  mjd   = fDrive.GetSeTime();
    2545         //const Encoder sepos = fDrive.GetSePos();  // [rev]
    2546 
    2547         // Request Target position for Now+dt
    2548         //fPointing.mount = sepos*360; // [deg] ref pos for alignment
    2549 
    2550         fPointing.Update(mjd+dt/24/3600, fWeather, fWeatherTimeout);
    2551         if (!CheckRange(fPointing.sky))
    2552         {
    2553             StopMovement();
    2554             T::HandleNewState(State::kAllowedRangeExceeded, 0, "by TrackingLoop");
    2555             return;
    2556         }
    2557 
    2558         // Destination position at t+dt in re-units
    2559         const Encoder dest = fPointing.mount*(1./360);  // [rev]
    2560 
    2561         // Current position
    2562         const Encoder sepos = fDrive.GetSePos();  // [rev]
    2563 
    2564         // Now calculate the distance to move from now
    2565         // to a time in t+dt.
    2566         const Encoder dist = dest-sepos; // [rev]
    2567 
    2568         // This is to avoid that we are always too slow
    2569         // On average 30ms have been passed since the last report was
    2570         // received, it is fair to assume that on average als0 30ms
    2571         // will pass until this report arrives. Without that correction
    2572         // we were always too fast   (60ms = 3% von 2000ms)
    2573         const double deltat = 60./1000/60;//(Time().Mjd() - mjd)/1440; //[min]
    2574 
    2575         // Assume a mainly linear movements. We can estimate the position
    2576         // at which we are now
    2577         static Velocity last = 0;
    2578 
    2579         // Velocity to go [rev/min] to reach the right position at time t+dt
    2580         // correct for the duration of RaDec2AltAz
    2581         const Velocity vt  = (dist - last*deltat) / (dt/60 - deltat);    //[rev/min]
    2582 
    2583         last = vt;
    2584 
    2585         if (fDrive.GetVerbosity()>1)
    2586         {
    2587              T::Out() << "Destination  [deg]  " << dest.zd *360 << " " << dest.az *360 << endl;
    2588              T::Out() << "Position     [deg]  " << sepos.zd*360 << " " << sepos.az*360 << endl;
    2589              T::Out() << "Distance  [arcmin]  " << dist.zd *360*60 << " " << dist.az *360*60 << endl;
    2590              T::Out() << "Velocity     [rpm]  " << vt.zd    << " " << vt.az    << endl;
    2591              T::Out() << "Delta T (enc) [ms]  " << fabs(mjd-fDrive.fPdoTime2[0].Mjd())*24*3600*1000 << endl;
    2592              T::Out() << "Delta T (now) [ms]  " << (Time().Mjd()-mjd)*24*3600*1000 << endl;
    2593         }
    2594 
    2595         // Tracking loop every 250ms
    2596         // Vorsteuerung 2s
    2597         // Delta T (enc) 5ms, every 5th, 25ms
    2598         // Delta T (now) equal dist 5ms-35 plus equal dist 25-55 (0.2%-2% of 2s)
    2599 
    2600         //
    2601         // FIXME: check if the drive is fast enough to follow the star
    2602         //
    2603         // Velocity units (would be 100 for %)
    2604 
    2605         fDrive.SetTrackingVelocity(vt);
    2606 
    2607         fTrackingLoop.async_wait(boost::bind(&StateMachineDrive::TrackingLoop,
    2608                                              this, ba::placeholders::error));
    2609     }
    2610 */
     2353
    26112354    void TrackingLoop(const boost::system::error_code &error=boost::system::error_code())
    26122355    {
    … …  
    27682511        if (rc>0)
    27692512            return rc;
    2770         /*
    2771 
    2772         if (!fDrive.IsConnected())
    2773             return State::kDisconnected;
    2774 
    2775         if (!fDrive.IsOnline())
    2776             return State::kUnavailable;
    2777 
    2778         // FIXME: This can prevent parking in case e.g.
    2779         // of e8029 Position limit exceeded
    2780         if (fDrive.HasError())
    2781         {
    2782             if (T::GetCurrentState()==State::kOnTrack  ||
    2783                 T::GetCurrentState()==State::kTracking ||
    2784                 T::GetCurrentState()==State::kMoving   ||
    2785                 T::GetCurrentState()==State::kParking)
    2786                 return StopMovement();
    2787 
    2788             if (T::GetCurrentState()==State::kStopping && fDrive.IsMoving())
    2789                 return State::kStopping;
    2790 
    2791             return StateMachineImp::kSM_Error;
    2792         }
    2793 
    2794         // This can happen if one of the drives is not in RF.
    2795         // Usually this only happens when the drive is not yet in RF
    2796         // or an error was just cleared. Usually there is no way that
    2797         // a drive goes below the RF state during operation without
    2798         // a warning or error message.
    2799         if (fDrive.IsOnline() && !fDrive.IsReady())
    2800             return State::kAvailable;
    2801 
    2802         if (fDrive.IsOnline() && fDrive.IsBlocked())
    2803             return State::kBlocked;
    2804 
    2805         // This is the case as soon as the init commands were send
    2806         // after a connection to the SPS was established
    2807         if (fDrive.IsOnline() && fDrive.IsReady() && !fDrive.IsInitialized())
    2808             return State::kArmed;
    2809 */
     2513
    28102514        // Once every second
    28112515        static time_t lastTime = 0;
    … …  
    29302634            (bind(&StateMachineDrive::HandleTPoint, this, placeholders::_1));
    29312635
    2932         /*
    2933             kDisconnected = 1,
    2934             kConnected,
    2935             kLocked,
    2936             kUnavailable,    // IndraDrives not connected
    2937             kAvailable,      // IndraDrives connected, but not in Af
    2938             kBlocked,        // Drive blocked by manual operation of emergency button
    2939             kArmed,          // IndraDrives Af, not yet initialized
    2940             kInitialized,    // IndraDrives Af, initialized
    2941             kStopping,
    2942             kParking,
    2943             kMoving,
    2944             kTracking,
    2945             kOnTrack,
    2946 
    2947           */
    29482636        // State names
    29492637        T::AddStateName(State::kDisconnected, "Disconnected",
    … …  
    30522740            ("Move the telescope to the given position and start tracking"
    30532741             "|Name[string]:Source name");
    3054 /*
    3055         T::AddEvent("RESUME", StateMachineImp::kSM_Error)
    3056             (bind(&StateMachineDrive::Resume, this))
    3057             ("If drive is in Error state, this can b used to resume the last tracking command, if the last command sent to cosy was a tracking command.");
    3058 */
     2742
    30592743        T::AddEvent("MOON", State::kInitialized, State::kTracking, State::kOnTrack)
    30602744            (bind(&StateMachineDrive::TrackCelest, this, kEMoon))
Note: See TracChangeset for help on using the changeset viewer.