Changeset 12532

Timestamp:

11/16/11 00:27:47 (13 years ago)

Author:

tbretz

Message:

Added a drive status dim service.

File:

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

trunk/FACT++/src/drivectrl.cc

@@ -124 +124 @@
     }
 
+    virtual void UpdateStatus(const Time &, const array<uint8_t, 3> &)
+    {
+    }
+
     virtual void UpdateStarguider(const Time &, const Drive::DimStarguider &)
     {
@@ -131 +135 @@
     {
     }
+
 
 protected:
@@ -189 +194 @@
         StartReadReport();
 
+        if (line.substr(0, 13)=="DRIVE-STATUS ")
+        {
+            Message(line.substr(70));
+            return;
+        }
+
         if (line.substr(0, 13)=="STARG-REPORT ")
         {
@@ -366 +377 @@
             stream >> stgmd;
 
+            uint32_t pdo3;
+            stream >> hex >> pdo3;
+
             if (stream.fail())
                 return;
@@ -375 +389 @@
             if (status1==0)
                 status1 = 99;
-            fState = status1==1 ? armed+1 : status1;
+
+            const bool ready = (pdo3&0xef00ef)==0xef00ef;
+            if (!ready)
+                fState = 0;
+            else   
+                fState = status1==1 ? armed+1 : status1;
+
+            //  1: Not ready
+            //  2: Not armed
+            //  3: Armed
+            //  4: Moving
+            //  5: Tracking
+            // 99: Error
+
+
+
+            // 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
+
+            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 }};
@@ -509 +557 @@
 public:
     ConnectionDrive(ba::io_service& ioservice, MessageImp &imp) : Connection(ioservice, imp()),
-        fState(0), fIsVerbose(true), fKeepAlive(ioservice)
+        fState(-1), fIsVerbose(true), fKeepAlive(ioservice)
     {
         SetLogStream(&imp);
@@ -519 +567 @@
     }
 
-    int GetState() const { return IsConnected() ? fState+1 : 1; }
+    int GetState() const
+    {
+        if (!IsConnected())
+            return 1;
+        if (IsConnected() && fState<0)
+            return 2;
+        return fState+3;
+    }
 };
 
@@ -535 +590 @@
     DimDescribedService fDimTracking;
     DimDescribedService fDimTPoint;
+    DimDescribedService fDimStatus;
 
     virtual void UpdatePointing(const Time &t,
@@ -548 +604 @@
         fDimTracking.setData(arr);
         fDimTracking.Update(t);
+    }
+    virtual void UpdateStatus(const Time &t,
+                              const array<uint8_t, 3> &arr)
+    {
+        fDimStatus.setData(arr);
+        fDimStatus.Update(t);
     }
     virtual void UpdateTPoint(const Time &t,
@@ -592 +654 @@
                    "|Ys[pix]:Y position of star in CCD camera frame"
                    "|Ms[mag]:Artifical magnitude of star (calculated form image))"
-                   "|Mc[mag]:Catalog magnitude of star")
+                   "|Mc[mag]:Catalog magnitude of star"),
+        fDimStatus("DRIVE_CONTROL/STATUS", "C:2;C:1", "")
 
     {
@@ -623 +686 @@
         kStateDisconnected = 1,
         kStateConnected,
+        kStateNotReady,
+        kStateReady,
         kStateArmed,
         kStateMoving,
@@ -676 +741 @@
     {
         fDrive.PostMessage(str);
+        T::Message("Sending: "+str);
         return T::GetCurrentState();
     }
@@ -786 +852 @@
         AddStateName(kStateConnected, "Connected",
                      "Cosy connected, drive stopped");
+
+        AddStateName(kStateNotReady, "NotReady",
+                     "Drive system not ready for movement");
+
+        AddStateName(kStateReady, "Ready",
+                     "Drive system ready for movement");
 
         AddStateName(kStateArmed, "Armed",