Changeset 15363 for trunk


Ignore:
Timestamp:
04/18/13 10:21:36 (12 years ago)
Author:
tbretz
Message:
Shift sunrise in a way that it does not consume 100% CPU at day time; move some code to memeber functions
File:
1 edited

Legend:

Unmodified
Added
Removed

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

    r15211 r15363  
    129129    uint64_t fTrackingCounter;
    130130
     131    void ProcessDriveStatus(const string &line)
     132    {
     133        Message(line);
     134    }
     135
     136    bool ProcessStargReport(const string &line)
     137    {
     138        istringstream stream(line);
     139
     140        // 0: Error
     141        // 1: Standby
     142        // 2: Monitoring
     143        uint16_t status1;
     144        stream >> status1;
     145        /*const Time t1 = */ReadTime(stream);
     146
     147        uint16_t status2;
     148        stream >> status2;
     149        /*const Time t2 = */ReadTime(stream);
     150
     151        double misszd, missaz;
     152        stream >> misszd >> missaz;
     153
     154        const double zd = ReadAngle(stream);
     155        const double az = ReadAngle(stream);
     156
     157        double cx, cy;
     158        stream >> cx >> cy;
     159
     160        int ncor;
     161        stream >> ncor;
     162
     163        double bright, mjd;
     164        stream >> bright >> mjd;
     165
     166        int nled, nring, nstars;
     167        stream >> nled >> nring >> nstars;
     168
     169        if (stream.fail())
     170            return false;
     171
     172        DimStarguider data;
     173
     174        data.fMissZd = misszd;
     175        data.fMissAz = missaz;
     176        data.fNominalZd = zd;
     177        data.fNominalAz = az;
     178        data.fCenterX = cx;
     179        data.fCenterY = cy;
     180        data.fNumCorrelated = ncor;
     181        data.fBrightness = bright;
     182        data.fNumLeds = nled;
     183        data.fNumRings = nring;
     184        data.fNumStars = nstars;
     185
     186        UpdateStarguider(Time(mjd), data);
     187
     188        return true;
     189    }
     190
     191    bool ProcessTpointReport(const string &line)
     192    {
     193        istringstream stream(line);
     194
     195        uint16_t status1;
     196        stream >> status1;
     197        const Time t1 = ReadTime(stream);
     198
     199        uint16_t status2;
     200        stream >> status2;
     201        /*const Time t2 =*/ ReadTime(stream);
     202
     203        char type;
     204        stream >> type;
     205        if (type != 'T')
     206            return false;
     207
     208        double az1, alt1, az2, alt2, ra, dec, dzd, daz;
     209        stream >> az1 >> alt1 >> az2 >> alt2 >> ra >> dec >> dzd >> daz;
     210
     211        // c: center, s:start
     212        double mjd, cmag, smag, cx, cy, sx, sy;
     213        stream >> mjd >> cmag >> smag >> cx >> cy >> sx >> sy;
     214
     215        int nled, nring, nstar, ncor;
     216        stream >> nled >> nring >> nstar >> ncor;
     217
     218        double bright, mag;
     219        stream >> bright >> mag;
     220
     221        string name;
     222        stream >> name;
     223
     224        if (stream.fail())
     225            return false;
     226
     227        DimTPoint tpoint;
     228
     229        tpoint.fRa         = ra;
     230        tpoint.fDec        = dec;
     231
     232        tpoint.fNominalZd  = 90-alt1-dzd;
     233        tpoint.fNominalAz  = 90-az1 +daz;
     234
     235        tpoint.fPointingZd = 90-alt1;
     236        tpoint.fPointingAz = az1;
     237
     238        tpoint.fFeedbackZd = 90-alt2;
     239        tpoint.fFeedbackAz = az2;
     240
     241        tpoint.fNumLeds    = nled;
     242        tpoint.fNumRings   = nring;
     243
     244        tpoint.fCenterX    = cx;
     245        tpoint.fCenterY    = cy;
     246        tpoint.fCenterMag  = cmag;
     247
     248        tpoint.fStarX      = sx;
     249        tpoint.fStarY      = sy;
     250        tpoint.fStarMag    = smag;
     251
     252        tpoint.fRealMag    = mag;
     253
     254        UpdateTPoint(t1, tpoint, name);
     255
     256        return true;
     257    }
     258
     259    bool ProcessDriveReport(const string &line)
     260    {
     261        // DRIVE-REPORT M1
     262        // 01 2011 05 14 11 31 19 038
     263        // 02 1858 11 17 00 00 00 000
     264        // + 000 00 000 + 000 00 000
     265        // + 000 00 000
     266        // 55695.480081
     267        // + 000 00 000 + 000 00 000
     268        // + 000 00 000 + 000 00 000
     269        // 0000.000 0000.000
     270        // 0 2
     271
     272        // status
     273        // year month day hour minute seconds millisec
     274        // year month day hour minute seconds millisec
     275        // ra(+ h m s) dec(+ d m s) ha(+ h m s)
     276        // mjd
     277        // zd(+ d m s) az(+ d m s)
     278        // zd(+ d m s) az(+ d m s)
     279        // zd_err az_err
     280        // armed(0=unlocked, 1=locked)
     281        // stgmd(0=none, 1=starguider, 2=starguider off)
     282        istringstream stream(line);
     283
     284        uint16_t status1;
     285        stream >> status1;
     286        const Time t1 = ReadTime(stream);
     287
     288        uint16_t status2;
     289        stream >> status2;
     290        /*const Time t2 =*/ ReadTime(stream);
     291
     292        const double ra  = ReadAngle(stream);
     293        const double dec = ReadAngle(stream);
     294        const double ha  = ReadAngle(stream);
     295
     296        double mjd;
     297        stream >> mjd;
     298
     299        const double zd1 = ReadAngle(stream);  // Nominal (zd/az asynchronous, dev  synchronous, mjd  synchronous with zd)
     300        const double az1 = ReadAngle(stream);  // Nominal (zd/az asynchronous, dev  synchronous, mjd  synchronous with z)
     301        const double zd2 = ReadAngle(stream);  // Masured (zd/az  synchronous, dev asynchronous, mjd asynchronous)
     302        const double az2 = ReadAngle(stream);  // Measurd (zd/az  synchronous, dev asynchronous, mjd asynchronous)
     303
     304        double zd_err, az_err;
     305        stream >> zd_err;                      // Deviation = Nominal - Measured
     306        stream >> az_err;                      // Deviation = Nominal - Measured
     307
     308        uint16_t armed, stgmd;
     309        stream >> armed;
     310        stream >> stgmd;
     311
     312        uint32_t pdo3;
     313        stream >> hex >> pdo3;
     314
     315        if (stream.fail())
     316            return false;
     317
     318        // Status 0: Error
     319        // Status 1: Stopped
     320        // Status 3: Stopping || Moving
     321        // Status 4: Tracking
     322        if (status1==0)
     323            status1 = StateMachineImp::kSM_Error - Drive::State::kNotReady;
     324
     325        const bool ready = (pdo3&0xef00ef)==0xef00ef;
     326        if (!ready)
     327            fState = Drive::State::kNotReady;
     328        else
     329            fState = status1==1 ?
     330                Drive::State::kReady+armed :
     331                Drive::State::kNotReady+status1;
     332
     333        // kDisconnected = 1,
     334        // kConnected,
     335        // kNotReady,
     336        // kReady,
     337        // kArmed,
     338        // kMoving,
     339        // kTracking,
     340        // kOnTrack,
     341
     342        // pdo3:
     343        //   1 Ab
     344        //   2 1
     345        //   4 Emergency
     346        //   8 OverVolt
     347        //  10 Move (Drehen-soll)
     348        //  20 Af
     349        //  40 1
     350        //  80 Power on Az
     351        // ------------------
     352        // 100 NOT UPS Alarm
     353        // 200 UPS on Battery
     354        // 400 UPS charging
     355
     356        // Power cut: 2ef02ef
     357        // charging:  4ef04ef
     358
     359        // Convert to deg
     360        zd_err /= 3600;
     361        az_err /= 3600;
     362
     363        // Calculate absolut deviation on the sky
     364        const double dev = GetDevAbs(zd1, zd1-zd_err, az_err)*3600;
     365
     366        // If any other state than tracking or a deviation
     367        // larger than 60, reset the counter
     368        if (fState!=State::kTracking || dev>fDeviationLimit)
     369            fTrackingCounter = 0;
     370        else
     371            fTrackingCounter++;
     372
     373        // If in tracking, at least five consecutive reports (5s)
     374        // must be below 60arcsec deviation, this is considered OnTrack
     375        if (fState==State::kTracking && fTrackingCounter>=fDeviationCounter)
     376            fState = State::kOnTrack;
     377
     378        // Having th state as Tracking will reset the counter
     379        if (fState==State::kOnTrack && dev>fDeviationMax)
     380            fState = State::kTracking;
     381
     382        // 206 206         ce ce       pwr vlt emcy fs        |  pwr vlt emcy fs
     383        // 239 239         ef ef       pwr vlt emcy fs bb rf  |  pwr vlt emcy fs bb rf
     384        // 111  78         6f 4e           vlt emcy fs bb rf  |          emcy fs bb
     385
     386        /*
     387        fArmed     = data[3]&0x01; // armed status
     388        fPosActive = data[3]&0x02; // positioning active
     389        fRpmActive = data[3]&0x04; // RPM mode switched on
     390                  // data[3]&0x08; //  - unused -
     391                  // data[3]&0x10; //  - unused -
     392                  // data[3]&0x20; //  - unused -
     393        //fInControl = data[3]&0x40; // motor uncontrolled
     394                  // data[3]&0x80; // axis resetted (after errclr, motor stop, motor on)
     395
     396        fStatus = data[3];
     397    }
     398
     399    const LWORD_t stat = data[0] | (data[1]<<8);
     400    if (fStatusPdo3!=stat)
     401    {
     402        gLog << inf << MTime(-1) << ": " << GetNodeName() << " - PDO3(0x" << hex << (int)stat << dec << ") = ";
     403        const Bool_t ready  = stat&0x001;
     404        const Bool_t fuse   = stat&0x002;
     405        const Bool_t emcy   = stat&0x004;
     406        const Bool_t vltg   = stat&0x008;
     407        const Bool_t mode   = stat&0x010;
     408        const Bool_t rf     = stat&0x020;
     409        const Bool_t brake  = stat&0x040;
     410        const Bool_t power  = stat&0x080;
     411        const Bool_t alarm  = stat&0x100;  // UPS Alarm      (FACT only)
     412        const Bool_t batt   = stat&0x200;  // UPS on battery (FACT only)
     413        const Bool_t charge = stat&0x400;  // UPS charging   (FACT only)
     414        if (ready)  gLog << "DKC-Ready ";
     415        if (fuse)   gLog << "FuseOk ";
     416        if (emcy)   gLog << "EmcyOk ";
     417        if (vltg)   gLog << "OvervoltOk ";
     418        if (mode)   gLog << "SwitchToManualMode ";
     419        if (rf)     gLog << "RF ";
     420        if (brake)  gLog << "BrakeOpen ";
     421        if (power)  gLog << "PowerOn ";
     422        if (alarm)  gLog << "UPS-PowerLoss ";
     423        if (batt)   gLog << "UPS-OnBattery ";
     424        if (charge) gLog << "UPS-Charging ";
     425        gLog << endl;
     426
     427        fStatusPdo3 = stat;
     428        }*/
     429
     430        // ((stat1&0xffff)<<16)|(stat2&0xffff)
     431                                                                              // no alarm, no batt, no charge
     432        const array<uint8_t, 3> state = {{ uint8_t(pdo3>>16), uint8_t(pdo3), uint8_t(pdo3>>24) }};
     433        UpdateStatus(t1, state);
     434
     435        const array<double, 2> point = {{ zd2, az2 }};
     436        UpdatePointing(t1, point);
     437
     438        const array<double, 8> track =
     439        {{
     440            ra, dec, ha,
     441            zd1, az1,
     442            zd_err, az_err,
     443            dev
     444        }};
     445        if (mjd>0)
     446            UpdateTracking(Time(mjd), track);
     447
     448        // ---- DIM ----> t1 as event time
     449        //                status1
     450        //                mjd
     451        //                ra/dec/ha
     452        //                zd/az (nominal)
     453        //                zd/az (current)
     454        //                err(zd/az)
     455        //                [armed] [stgmd]
     456
     457        // Maybe:
     458        // POINTING_POSITION --> t1, zd/az (current), [armed, stgmd, status1]
     459        //
     460        // if (mjd>0)
     461        // TRACKING_POSITION --> mjd, zd/az (nominal), err(zd/az)
     462        //                       ra/dec, ha(not well defined),
     463        //                       [Nominal + Error == Current]
     464
     465        // MJD is the time which corresponds to the nominal position
     466        // t1  is the time which corresponds to the current position/HA
     467
     468        return true;
     469    }
     470
    131471protected:
    132472    void HandleReceivedReport(const boost::system::error_code& err, size_t bytes_received)
    … …  
    164504        if (line.substr(0, 13)=="DRIVE-STATUS ")
    165505        {
    166             Message(line.substr(70));
     506            ProcessDriveStatus(line.substr(70));
    167507            return;
    168508        }
    … …  
    170510        if (line.substr(0, 13)=="STARG-REPORT ")
    171511        {
    172             istringstream stream(line.substr(16));
    173 
    174             // 0: Error
    175             // 1: Standby
    176             // 2: Monitoring
    177             uint16_t status1;
    178             stream >> status1;
    179             /*const Time t1 = */ReadTime(stream);
    180 
    181             uint16_t status2;
    182             stream >> status2;
    183             /*const Time t2 = */ReadTime(stream);
    184 
    185             double misszd, missaz;
    186             stream >> misszd >> missaz;
    187 
    188             const double zd = ReadAngle(stream);
    189             const double az = ReadAngle(stream);
    190 
    191             double cx, cy;
    192             stream >> cx >> cy;
    193 
    194             int ncor;
    195             stream >> ncor;
    196 
    197             double bright, mjd;
    198             stream >> bright >> mjd;
    199 
    200             int nled, nring, nstars;
    201             stream >> nled >> nring >> nstars;
    202 
    203             if (stream.fail())
    204                 return;
    205 
    206             DimStarguider data;
    207 
    208             data.fMissZd = misszd;
    209             data.fMissAz = missaz;
    210             data.fNominalZd = zd;
    211             data.fNominalAz = az;
    212             data.fCenterX = cx;
    213             data.fCenterY = cy;
    214             data.fNumCorrelated = ncor;
    215             data.fBrightness = bright;
    216             data.fNumLeds = nled;
    217             data.fNumRings = nring;
    218             data.fNumStars = nstars;
    219 
    220             UpdateStarguider(Time(mjd), data);
    221 
     512            ProcessStargReport(line.substr(16));
    222513            return;
    223 
    224514        }
    225515
    226516        if (line.substr(0, 14)=="TPOINT-REPORT ")
    227517        {
    228             istringstream stream(line.substr(17));
    229 
    230             uint16_t status1;
    231             stream >> status1;
    232             const Time t1 = ReadTime(stream);
    233 
    234             uint16_t status2;
    235             stream >> status2;
    236             /*const Time t2 =*/ ReadTime(stream);
    237 
    238             char type;
    239             stream >> type;
    240             if (type != 'T')
    241                 return;
    242 
    243             double az1, alt1, az2, alt2, ra, dec, dzd, daz;
    244             stream >> az1 >> alt1 >> az2 >> alt2 >> ra >> dec >> dzd >> daz;
    245 
    246             // c: center, s:start
    247             double mjd, cmag, smag, cx, cy, sx, sy;
    248             stream >> mjd >> cmag >> smag >> cx >> cy >> sx >> sy;
    249 
    250             int nled, nring, nstar, ncor;
    251             stream >> nled >> nring >> nstar >> ncor;
    252 
    253             double bright, mag;
    254             stream >> bright >> mag;
    255 
    256             string name;
    257             stream >> name;
    258 
    259             if (stream.fail())
    260                 return;
    261 
    262             DimTPoint tpoint;
    263 
    264             tpoint.fRa         = ra;
    265             tpoint.fDec        = dec;
    266 
    267             tpoint.fNominalZd  = 90-alt1-dzd;
    268             tpoint.fNominalAz  = 90-az1 +daz;
    269 
    270             tpoint.fPointingZd = 90-alt1;
    271             tpoint.fPointingAz = az1;
    272  
    273             tpoint.fFeedbackZd = 90-alt2;
    274             tpoint.fFeedbackAz = az2;
    275 
    276             tpoint.fNumLeds    = nled;
    277             tpoint.fNumRings   = nring;
    278 
    279             tpoint.fCenterX    = cx;
    280             tpoint.fCenterY    = cy;
    281             tpoint.fCenterMag  = cmag;
    282 
    283             tpoint.fStarX      = sx;
    284             tpoint.fStarY      = sy;
    285             tpoint.fStarMag    = smag;
    286 
    287             tpoint.fRealMag    = mag;
    288 
    289             UpdateTPoint(t1, tpoint, name);
    290 
     518            ProcessTpointReport(line.substr(17));
    291519            return;
    292520        }
    … …  
    294522        if (line.substr(0, 13)=="DRIVE-REPORT ")
    295523        {
    296             // DRIVE-REPORT M1
    297             // 01 2011 05 14 11 31 19 038
    298             // 02 1858 11 17 00 00 00 000
    299             // + 000 00 000 + 000 00 000
    300             // + 000 00 000
    301             // 55695.480081
    302             // + 000 00 000 + 000 00 000
    303             // + 000 00 000 + 000 00 000
    304             // 0000.000 0000.000
    305             // 0 2
    306 
    307             // status
    308             // year month day hour minute seconds millisec
    309             // year month day hour minute seconds millisec
    310             // ra(+ h m s) dec(+ d m s) ha(+ h m s)
    311             // mjd
    312             // zd(+ d m s) az(+ d m s)
    313             // zd(+ d m s) az(+ d m s)
    314             // zd_err az_err
    315             // armed(0=unlocked, 1=locked)
    316             // stgmd(0=none, 1=starguider, 2=starguider off)
    317             istringstream stream(line.substr(16));
    318 
    319             uint16_t status1;
    320             stream >> status1;
    321             const Time t1 = ReadTime(stream);
    322 
    323             uint16_t status2;
    324             stream >> status2;
    325             /*const Time t2 =*/ ReadTime(stream);
    326 
    327             const double ra  = ReadAngle(stream);
    328             const double dec = ReadAngle(stream);
    329             const double ha  = ReadAngle(stream);
    330 
    331             double mjd;
    332             stream >> mjd;
    333 
    334             const double zd1 = ReadAngle(stream);  // Nominal (zd/az asynchronous, dev  synchronous, mjd  synchronous with zd)
    335             const double az1 = ReadAngle(stream);  // Nominal (zd/az asynchronous, dev  synchronous, mjd  synchronous with z)
    336             const double zd2 = ReadAngle(stream);  // Masured (zd/az  synchronous, dev asynchronous, mjd asynchronous)
    337             const double az2 = ReadAngle(stream);  // Measurd (zd/az  synchronous, dev asynchronous, mjd asynchronous)
    338 
    339             double zd_err, az_err;
    340             stream >> zd_err;                      // Deviation = Nominal - Measured
    341             stream >> az_err;                      // Deviation = Nominal - Measured
    342 
    343             uint16_t armed, stgmd;
    344             stream >> armed;
    345             stream >> stgmd;
    346 
    347             uint32_t pdo3;
    348             stream >> hex >> pdo3;
    349 
    350             if (stream.fail())
    351                 return;
    352 
    353             // Status 0: Error
    354             // Status 1: Stopped
    355             // Status 3: Stopping || Moving
    356             // Status 4: Tracking
    357             if (status1==0)
    358                 status1 = StateMachineImp::kSM_Error - Drive::State::kNotReady;
    359 
    360             const bool ready = (pdo3&0xef00ef)==0xef00ef;
    361             if (!ready)
    362                 fState = Drive::State::kNotReady;
    363             else   
    364                 fState = status1==1 ?
    365                     Drive::State::kReady+armed :
    366                     Drive::State::kNotReady+status1;
    367 
    368             // kDisconnected = 1,
    369             // kConnected,
    370             // kNotReady,
    371             // kReady,
    372             // kArmed,
    373             // kMoving,
    374             // kTracking,
    375             // kOnTrack,
    376 
    377             // pdo3:
    378             //   1 Ab
    379             //   2 1
    380             //   4 Emergency
    381             //   8 OverVolt
    382             //  10 Move (Drehen-soll)
    383             //  20 Af
    384             //  40 1
    385             //  80 Power on Az
    386             // ------------------
    387             // 100 NOT UPS Alarm
    388             // 200 UPS on Battery
    389             // 400 UPS charging
    390 
    391             // Power cut: 2ef02ef
    392             // charging:  4ef04ef
    393 
    394             // Convert to deg
    395             zd_err /= 3600;
    396             az_err /= 3600;
    397 
    398             // Calculate absolut deviation on the sky
    399             const double dev = GetDevAbs(zd1, zd1-zd_err, az_err)*3600;
    400 
    401             // If any other state than tracking or a deviation
    402             // larger than 60, reset the counter
    403             if (fState!=State::kTracking || dev>fDeviationLimit)
    404                 fTrackingCounter = 0;
    405             else
    406                 fTrackingCounter++;
    407 
    408             // If in tracking, at least five consecutive reports (5s)
    409             // must be below 60arcsec deviation, this is considered OnTrack
    410             if (fState==State::kTracking && fTrackingCounter>=fDeviationCounter)
    411                 fState = State::kOnTrack;
    412 
    413             // Having th state as Tracking will reset the counter
    414             if (fState==State::kOnTrack && dev>fDeviationMax)
    415                 fState = State::kTracking;
    416 
    417             const array<uint8_t, 3> state = {{ uint8_t(pdo3>>16), uint8_t(pdo3), uint8_t(pdo3>>24) }};
    418             UpdateStatus(t1, state);
    419 
    420             const array<double, 2> point = {{ zd2, az2 }};
    421             UpdatePointing(t1, point);
    422 
    423             const array<double, 8> track =
    424             {{
    425                 ra, dec, ha,
    426                 zd1, az1,
    427                 zd_err, az_err,
    428                 dev
    429             }};
    430             if (mjd>0)
    431                 UpdateTracking(Time(mjd), track);
    432 
    433             // ---- DIM ----> t1 as event time
    434             //                status1
    435             //                mjd
    436             //                ra/dec/ha
    437             //                zd/az (nominal)
    438             //                zd/az (current)
    439             //                err(zd/az)
    440             //                [armed] [stgmd]
    441 
    442             // Maybe:
    443             // POINTING_POSITION --> t1, zd/az (current), [armed, stgmd, status1]
    444             //
    445             // if (mjd>0)
    446             // TRACKING_POSITION --> mjd, zd/az (nominal), err(zd/az)
    447             //                       ra/dec, ha(not well defined),
    448             //                       [Nominal + Error == Current]
    449 
    450             // MJD is the time which corresponds to the nominal position
    451             // t1  is the time which corresponds to the current position/HA
    452 
     524            ProcessDriveReport(line.substr(16));
    453525            return;
    454526        }
    … …  
    11191191
    11201192    Time fSunRise;
    1121 
     1193    /*
    11221194    int ShiftSunRise()
    11231195    {
    … …  
    11341206
    11351207        return Drive::State::kLocked;
    1136     }
     1208    }*/
    11371209
    11381210    int Execute()
    … …  
    11451217        poll_one();
    11461218
     1219        /*
    11471220        if (T::GetCurrentState()==Drive::State::kLocked)
    11481221            return ShiftSunRise();
    … …  
    11521225            if (Time()>fSunRise)
    11531226                return Park();
    1154         }
     1227        }*/
     1228
     1229        if (Time()>fSunRise)
     1230        {
     1231            if (T::GetCurrentState()>Drive::State::kLocked)
     1232                return Park();
     1233
     1234            if (T::GetCurrentState()==Drive::State::kLocked)
     1235            {
     1236                fSunRise = Time().GetNextSunRise();
     1237
     1238                ostringstream msg;
     1239                msg << "Next sun-rise will be at " << fSunRise;
     1240                T::Info(msg);
     1241
     1242                return Drive::State::kLocked;
     1243            }
     1244        }
     1245
     1246        if (T::GetCurrentState()==Drive::State::kLocked)
     1247            return Drive::State::kLocked;
    11551248
    11561249        const int state = fDrive.GetState();
Note: See TracChangeset for help on using the changeset viewer.