Changeset 15363 for trunk/FACT++/src

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:

• trunk/FACT++/src/drivectrl.cc (modified) (8 diffs)

trunk/FACT++/src/drivectrl.cc

@@ -129 +129 @@
     uint64_t fTrackingCounter;
 
+    void ProcessDriveStatus(const string &line)
+    {
+        Message(line);
+    }
+
+    bool ProcessStargReport(const string &line)
+    {
+        istringstream stream(line);
+
+        // 0: Error
+        // 1: Standby
+        // 2: Monitoring
+        uint16_t status1;
+        stream >> status1;
+        /*const Time t1 = */ReadTime(stream);
+
+        uint16_t status2;
+        stream >> status2;
+        /*const Time t2 = */ReadTime(stream);
+
+        double misszd, missaz;
+        stream >> misszd >> missaz;
+
+        const double zd = ReadAngle(stream);
+        const double az = ReadAngle(stream);
+
+        double cx, cy;
+        stream >> cx >> cy;
+
+        int ncor;
+        stream >> ncor;
+
+        double bright, mjd;
+        stream >> bright >> mjd;
+
+        int nled, nring, nstars;
+        stream >> nled >> nring >> nstars;
+
+        if (stream.fail())
+            return false;
+
+        DimStarguider data;
+
+        data.fMissZd = misszd;
+        data.fMissAz = missaz;
+        data.fNominalZd = zd;
+        data.fNominalAz = az;
+        data.fCenterX = cx;
+        data.fCenterY = cy;
+        data.fNumCorrelated = ncor;
+        data.fBrightness = bright;
+        data.fNumLeds = nled;
+        data.fNumRings = nring;
+        data.fNumStars = nstars;
+
+        UpdateStarguider(Time(mjd), data);
+
+        return true;
+    }
+
+    bool ProcessTpointReport(const string &line)
+    {
+        istringstream stream(line);
+
+        uint16_t status1;
+        stream >> status1;
+        const Time t1 = ReadTime(stream);
+
+        uint16_t status2;
+        stream >> status2;
+        /*const Time t2 =*/ ReadTime(stream);
+
+        char type;
+        stream >> type;
+        if (type != 'T')
+            return false;
+
+        double az1, alt1, az2, alt2, ra, dec, dzd, daz;
+        stream >> az1 >> alt1 >> az2 >> alt2 >> ra >> dec >> dzd >> daz;
+
+        // c: center, s:start
+        double mjd, cmag, smag, cx, cy, sx, sy;
+        stream >> mjd >> cmag >> smag >> cx >> cy >> sx >> sy;
+
+        int nled, nring, nstar, ncor;
+        stream >> nled >> nring >> nstar >> ncor;
+
+        double bright, mag;
+        stream >> bright >> mag;
+
+        string name;
+        stream >> name;
+
+        if (stream.fail())
+            return false;
+
+        DimTPoint tpoint;
+
+        tpoint.fRa         = ra;
+        tpoint.fDec        = dec;
+
+        tpoint.fNominalZd  = 90-alt1-dzd;
+        tpoint.fNominalAz  = 90-az1 +daz;
+
+        tpoint.fPointingZd = 90-alt1;
+        tpoint.fPointingAz = az1;
+
+        tpoint.fFeedbackZd = 90-alt2;
+        tpoint.fFeedbackAz = az2;
+
+        tpoint.fNumLeds    = nled;
+        tpoint.fNumRings   = nring;
+
+        tpoint.fCenterX    = cx;
+        tpoint.fCenterY    = cy;
+        tpoint.fCenterMag  = cmag;
+
+        tpoint.fStarX      = sx;
+        tpoint.fStarY      = sy;
+        tpoint.fStarMag    = smag;
+
+        tpoint.fRealMag    = mag;
+
+        UpdateTPoint(t1, tpoint, name);
+
+        return true;
+    }
+
+    bool ProcessDriveReport(const string &line)
+    {
+        // DRIVE-REPORT M1
+        // 01 2011 05 14 11 31 19 038
+        // 02 1858 11 17 00 00 00 000
+        // + 000 00 000 + 000 00 000
+        // + 000 00 000
+        // 55695.480081
+        // + 000 00 000 + 000 00 000
+        // + 000 00 000 + 000 00 000
+        // 0000.000 0000.000
+        // 0 2
+
+        // status
+        // year month day hour minute seconds millisec
+        // year month day hour minute seconds millisec
+        // ra(+ h m s) dec(+ d m s) ha(+ h m s)
+        // mjd
+        // zd(+ d m s) az(+ d m s)
+        // zd(+ d m s) az(+ d m s)
+        // zd_err az_err
+        // armed(0=unlocked, 1=locked)
+        // stgmd(0=none, 1=starguider, 2=starguider off)
+        istringstream stream(line);
+
+        uint16_t status1;
+        stream >> status1;
+        const Time t1 = ReadTime(stream);
+
+        uint16_t status2;
+        stream >> status2;
+        /*const Time t2 =*/ ReadTime(stream);
+
+        const double ra  = ReadAngle(stream);
+        const double dec = ReadAngle(stream);
+        const double ha  = ReadAngle(stream);
+
+        double mjd;
+        stream >> mjd;
+
+        const double zd1 = ReadAngle(stream);  // Nominal (zd/az asynchronous, dev  synchronous, mjd  synchronous with zd)
+        const double az1 = ReadAngle(stream);  // Nominal (zd/az asynchronous, dev  synchronous, mjd  synchronous with z)
+        const double zd2 = ReadAngle(stream);  // Masured (zd/az  synchronous, dev asynchronous, mjd asynchronous)
+        const double az2 = ReadAngle(stream);  // Measurd (zd/az  synchronous, dev asynchronous, mjd asynchronous)
+
+        double zd_err, az_err;
+        stream >> zd_err;                      // Deviation = Nominal - Measured
+        stream >> az_err;                      // Deviation = Nominal - Measured
+
+        uint16_t armed, stgmd;
+        stream >> armed;
+        stream >> stgmd;
+
+        uint32_t pdo3;
+        stream >> hex >> pdo3;
+
+        if (stream.fail())
+            return false;
+
+        // Status 0: Error
+        // Status 1: Stopped
+        // Status 3: Stopping || Moving
+        // Status 4: Tracking
+        if (status1==0)
+            status1 = StateMachineImp::kSM_Error - Drive::State::kNotReady;
+
+        const bool ready = (pdo3&0xef00ef)==0xef00ef;
+        if (!ready)
+            fState = Drive::State::kNotReady;
+        else
+            fState = status1==1 ?
+                Drive::State::kReady+armed :
+                Drive::State::kNotReady+status1;
+
+        // kDisconnected = 1,
+        // kConnected,
+        // kNotReady,
+        // kReady,
+        // kArmed,
+        // kMoving,
+        // kTracking,
+        // kOnTrack,
+
+        // pdo3:
+        //   1 Ab
+        //   2 1
+        //   4 Emergency
+        //   8 OverVolt
+        //  10 Move (Drehen-soll)
+        //  20 Af
+        //  40 1
+        //  80 Power on Az
+        // ------------------
+        // 100 NOT UPS Alarm
+        // 200 UPS on Battery
+        // 400 UPS charging
+
+        // Power cut: 2ef02ef
+        // charging:  4ef04ef
+
+        // Convert to deg
+        zd_err /= 3600;
+        az_err /= 3600;
+
+        // Calculate absolut deviation on the sky
+        const double dev = GetDevAbs(zd1, zd1-zd_err, az_err)*3600;
+
+        // If any other state than tracking or a deviation
+        // larger than 60, reset the counter
+        if (fState!=State::kTracking || dev>fDeviationLimit)
+            fTrackingCounter = 0;
+        else
+            fTrackingCounter++;
+
+        // If in tracking, at least five consecutive reports (5s)
+        // must be below 60arcsec deviation, this is considered OnTrack
+        if (fState==State::kTracking && fTrackingCounter>=fDeviationCounter)
+            fState = State::kOnTrack;
+
+        // Having th state as Tracking will reset the counter
+        if (fState==State::kOnTrack && dev>fDeviationMax)
+            fState = State::kTracking;
+
+        // 206 206         ce ce       pwr vlt emcy fs        |  pwr vlt emcy fs
+        // 239 239         ef ef       pwr vlt emcy fs bb rf  |  pwr vlt emcy fs bb rf
+        // 111  78         6f 4e           vlt emcy fs bb rf  |          emcy fs bb
+
+        /*
+        fArmed     = data[3]&0x01; // armed status
+        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];
+    }
+
+    const LWORD_t stat = data[0] | (data[1]<<8);
+    if (fStatusPdo3!=stat)
+    {
+        gLog << inf << MTime(-1) << ": " << GetNodeName() << " - PDO3(0x" << hex << (int)stat << dec << ") = ";
+        const Bool_t ready  = stat&0x001;
+        const Bool_t fuse   = stat&0x002;
+        const Bool_t emcy   = stat&0x004;
+        const Bool_t vltg   = stat&0x008;
+        const Bool_t mode   = stat&0x010;
+        const Bool_t rf     = stat&0x020;
+        const Bool_t brake  = stat&0x040;
+        const Bool_t power  = stat&0x080;
+        const Bool_t alarm  = stat&0x100;  // UPS Alarm      (FACT only)
+        const Bool_t batt   = stat&0x200;  // UPS on battery (FACT only)
+        const Bool_t charge = stat&0x400;  // UPS charging   (FACT only)
+        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 ";
+        if (alarm)  gLog << "UPS-PowerLoss ";
+        if (batt)   gLog << "UPS-OnBattery ";
+        if (charge) gLog << "UPS-Charging ";
+        gLog << endl;
+
+        fStatusPdo3 = stat;
+        }*/
+
+        // ((stat1&0xffff)<<16)|(stat2&0xffff)
+                                                                              // no alarm, no batt, no charge
+        const array<uint8_t, 3> state = {{ uint8_t(pdo3>>16), uint8_t(pdo3), uint8_t(pdo3>>24) }};
+        UpdateStatus(t1, state);
+
+        const array<double, 2> point = {{ zd2, az2 }};
+        UpdatePointing(t1, point);
+
+        const array<double, 8> track =
+        {{
+            ra, dec, ha,
+            zd1, az1,
+            zd_err, az_err,
+            dev
+        }};
+        if (mjd>0)
+            UpdateTracking(Time(mjd), track);
+
+        // ---- DIM ----> t1 as event time
+        //                status1
+        //                mjd
+        //                ra/dec/ha
+        //                zd/az (nominal)
+        //                zd/az (current)
+        //                err(zd/az)
+        //                [armed] [stgmd]
+
+        // Maybe:
+        // POINTING_POSITION --> t1, zd/az (current), [armed, stgmd, status1]
+        //
+        // if (mjd>0)
+        // TRACKING_POSITION --> mjd, zd/az (nominal), err(zd/az)
+        //                       ra/dec, ha(not well defined),
+        //                       [Nominal + Error == Current]
+
+        // MJD is the time which corresponds to the nominal position
+        // t1  is the time which corresponds to the current position/HA
+
+        return true;
+    }
+
 protected:
     void HandleReceivedReport(const boost::system::error_code& err, size_t bytes_received)
@@ -164 +504 @@
         if (line.substr(0, 13)=="DRIVE-STATUS ")
         {
-            Message(line.substr(70));
+            ProcessDriveStatus(line.substr(70));
             return;
         }
@@ -170 +510 @@
         if (line.substr(0, 13)=="STARG-REPORT ")
         {
-            istringstream stream(line.substr(16));
-
-            // 0: Error
-            // 1: Standby
-            // 2: Monitoring
-            uint16_t status1;
-            stream >> status1;
-            /*const Time t1 = */ReadTime(stream);
-
-            uint16_t status2;
-            stream >> status2;
-            /*const Time t2 = */ReadTime(stream);
-
-            double misszd, missaz;
-            stream >> misszd >> missaz;
-
-            const double zd = ReadAngle(stream);
-            const double az = ReadAngle(stream);
-
-            double cx, cy;
-            stream >> cx >> cy;
-
-            int ncor;
-            stream >> ncor;
-
-            double bright, mjd;
-            stream >> bright >> mjd;
-
-            int nled, nring, nstars;
-            stream >> nled >> nring >> nstars;
-
-            if (stream.fail())
-                return;
-
-            DimStarguider data;
-
-            data.fMissZd = misszd;
-            data.fMissAz = missaz;
-            data.fNominalZd = zd;
-            data.fNominalAz = az;
-            data.fCenterX = cx;
-            data.fCenterY = cy;
-            data.fNumCorrelated = ncor;
-            data.fBrightness = bright;
-            data.fNumLeds = nled;
-            data.fNumRings = nring;
-            data.fNumStars = nstars;
-
-            UpdateStarguider(Time(mjd), data);
-
+            ProcessStargReport(line.substr(16));
             return;
-
         }
 
         if (line.substr(0, 14)=="TPOINT-REPORT ")
         {
-            istringstream stream(line.substr(17));
-
-            uint16_t status1;
-            stream >> status1;
-            const Time t1 = ReadTime(stream);
-
-            uint16_t status2;
-            stream >> status2;
-            /*const Time t2 =*/ ReadTime(stream);
-
-            char type;
-            stream >> type;
-            if (type != 'T')
-                return;
-
-            double az1, alt1, az2, alt2, ra, dec, dzd, daz;
-            stream >> az1 >> alt1 >> az2 >> alt2 >> ra >> dec >> dzd >> daz;
-
-            // c: center, s:start
-            double mjd, cmag, smag, cx, cy, sx, sy;
-            stream >> mjd >> cmag >> smag >> cx >> cy >> sx >> sy;
-
-            int nled, nring, nstar, ncor;
-            stream >> nled >> nring >> nstar >> ncor;
-
-            double bright, mag;
-            stream >> bright >> mag;
-
-            string name;
-            stream >> name;
-
-            if (stream.fail())
-                return;
-
-            DimTPoint tpoint;
-
-            tpoint.fRa         = ra;
-            tpoint.fDec        = dec;
-
-            tpoint.fNominalZd  = 90-alt1-dzd;
-            tpoint.fNominalAz  = 90-az1 +daz;
-
-            tpoint.fPointingZd = 90-alt1;
-            tpoint.fPointingAz = az1;
- 
-            tpoint.fFeedbackZd = 90-alt2;
-            tpoint.fFeedbackAz = az2;
-
-            tpoint.fNumLeds    = nled;
-            tpoint.fNumRings   = nring;
-
-            tpoint.fCenterX    = cx;
-            tpoint.fCenterY    = cy;
-            tpoint.fCenterMag  = cmag;
-
-            tpoint.fStarX      = sx;
-            tpoint.fStarY      = sy;
-            tpoint.fStarMag    = smag;
-
-            tpoint.fRealMag    = mag;
-
-            UpdateTPoint(t1, tpoint, name);
-
+            ProcessTpointReport(line.substr(17));
             return;
         }
@@ -294 +522 @@
         if (line.substr(0, 13)=="DRIVE-REPORT ")
         {
-            // DRIVE-REPORT M1
-            // 01 2011 05 14 11 31 19 038
-            // 02 1858 11 17 00 00 00 000
-            // + 000 00 000 + 000 00 000
-            // + 000 00 000
-            // 55695.480081
-            // + 000 00 000 + 000 00 000
-            // + 000 00 000 + 000 00 000
-            // 0000.000 0000.000
-            // 0 2
-
-            // status
-            // year month day hour minute seconds millisec
-            // year month day hour minute seconds millisec
-            // ra(+ h m s) dec(+ d m s) ha(+ h m s)
-            // mjd
-            // zd(+ d m s) az(+ d m s)
-            // zd(+ d m s) az(+ d m s)
-            // zd_err az_err
-            // armed(0=unlocked, 1=locked)
-            // stgmd(0=none, 1=starguider, 2=starguider off)
-            istringstream stream(line.substr(16));
-
-            uint16_t status1;
-            stream >> status1;
-            const Time t1 = ReadTime(stream);
-
-            uint16_t status2;
-            stream >> status2;
-            /*const Time t2 =*/ ReadTime(stream);
-
-            const double ra  = ReadAngle(stream);
-            const double dec = ReadAngle(stream);
-            const double ha  = ReadAngle(stream);
-
-            double mjd;
-            stream >> mjd;
-
-            const double zd1 = ReadAngle(stream);  // Nominal (zd/az asynchronous, dev  synchronous, mjd  synchronous with zd)
-            const double az1 = ReadAngle(stream);  // Nominal (zd/az asynchronous, dev  synchronous, mjd  synchronous with z)
-            const double zd2 = ReadAngle(stream);  // Masured (zd/az  synchronous, dev asynchronous, mjd asynchronous)
-            const double az2 = ReadAngle(stream);  // Measurd (zd/az  synchronous, dev asynchronous, mjd asynchronous)
-
-            double zd_err, az_err;
-            stream >> zd_err;                      // Deviation = Nominal - Measured
-            stream >> az_err;                      // Deviation = Nominal - Measured
-
-            uint16_t armed, stgmd;
-            stream >> armed;
-            stream >> stgmd;
-
-            uint32_t pdo3;
-            stream >> hex >> pdo3;
-
-            if (stream.fail())
-                return;
-
-            // Status 0: Error
-            // Status 1: Stopped
-            // Status 3: Stopping || Moving
-            // Status 4: Tracking
-            if (status1==0)
-                status1 = StateMachineImp::kSM_Error - Drive::State::kNotReady;
-
-            const bool ready = (pdo3&0xef00ef)==0xef00ef;
-            if (!ready)
-                fState = Drive::State::kNotReady;
-            else   
-                fState = status1==1 ?
-                    Drive::State::kReady+armed :
-                    Drive::State::kNotReady+status1;
-
-            // kDisconnected = 1,
-            // kConnected,
-            // kNotReady,
-            // kReady,
-            // kArmed,
-            // kMoving,
-            // kTracking,
-            // kOnTrack,
-
-            // pdo3:
-            //   1 Ab
-            //   2 1
-            //   4 Emergency
-            //   8 OverVolt
-            //  10 Move (Drehen-soll)
-            //  20 Af
-            //  40 1
-            //  80 Power on Az
-            // ------------------
-            // 100 NOT UPS Alarm
-            // 200 UPS on Battery
-            // 400 UPS charging
-
-            // Power cut: 2ef02ef
-            // charging:  4ef04ef
-
-            // Convert to deg
-            zd_err /= 3600;
-            az_err /= 3600;
-
-            // Calculate absolut deviation on the sky
-            const double dev = GetDevAbs(zd1, zd1-zd_err, az_err)*3600;
-
-            // If any other state than tracking or a deviation
-            // larger than 60, reset the counter
-            if (fState!=State::kTracking || dev>fDeviationLimit)
-                fTrackingCounter = 0;
-            else
-                fTrackingCounter++;
-
-            // If in tracking, at least five consecutive reports (5s)
-            // must be below 60arcsec deviation, this is considered OnTrack
-            if (fState==State::kTracking && fTrackingCounter>=fDeviationCounter)
-                fState = State::kOnTrack;
-
-            // Having th state as Tracking will reset the counter
-            if (fState==State::kOnTrack && dev>fDeviationMax)
-                fState = State::kTracking;
-
-            const array<uint8_t, 3> state = {{ uint8_t(pdo3>>16), uint8_t(pdo3), uint8_t(pdo3>>24) }};
-            UpdateStatus(t1, state);
-
-            const array<double, 2> point = {{ zd2, az2 }};
-            UpdatePointing(t1, point);
-
-            const array<double, 8> track =
-            {{
-                ra, dec, ha,
-                zd1, az1,
-                zd_err, az_err,
-                dev
-            }};
-            if (mjd>0)
-                UpdateTracking(Time(mjd), track);
-
-            // ---- DIM ----> t1 as event time
-            //                status1
-            //                mjd
-            //                ra/dec/ha
-            //                zd/az (nominal)
-            //                zd/az (current)
-            //                err(zd/az)
-            //                [armed] [stgmd]
-
-            // Maybe:
-            // POINTING_POSITION --> t1, zd/az (current), [armed, stgmd, status1]
-            //
-            // if (mjd>0)
-            // TRACKING_POSITION --> mjd, zd/az (nominal), err(zd/az)
-            //                       ra/dec, ha(not well defined),
-            //                       [Nominal + Error == Current]
-
-            // MJD is the time which corresponds to the nominal position
-            // t1  is the time which corresponds to the current position/HA
-
+            ProcessDriveReport(line.substr(16));
             return;
         }
@@ -1119 +1191 @@
 
     Time fSunRise;
-
+    /*
     int ShiftSunRise()
     {
@@ -1134 +1206 @@
 
         return Drive::State::kLocked;
-    }
+    }*/
 
     int Execute()
@@ -1145 +1217 @@
         poll_one();
 
+        /*
         if (T::GetCurrentState()==Drive::State::kLocked)
             return ShiftSunRise();
@@ -1152 +1225 @@
             if (Time()>fSunRise)
                 return Park();
-        }
+        }*/
+
+        if (Time()>fSunRise)
+        {
+            if (T::GetCurrentState()>Drive::State::kLocked)
+                return Park();
+
+            if (T::GetCurrentState()==Drive::State::kLocked)
+            {
+                fSunRise = Time().GetNextSunRise();
+
+                ostringstream msg;
+                msg << "Next sun-rise will be at " << fSunRise;
+                T::Info(msg);
+
+                return Drive::State::kLocked;
+            }
+        }
+
+        if (T::GetCurrentState()==Drive::State::kLocked)
+            return Drive::State::kLocked;
 
         const int state = fDrive.GetState();