Added basic support for positioners to control steerable satellite dishes

1 files changed, 171 insertions, 29 deletions

diff --git a/diseqc.c b/diseqc.c
index 0db87616..86ee61fe 100644
--- a/diseqc.c
+++ b/diseqc.c
@@ -4,24 +4,37 @@
  * See the main source file 'vdr.c' for copyright information and
  * how to reach the author.
  *
- * $Id: diseqc.c 3.1 2013/05/02 09:30:33 kls Exp $
+ * $Id: diseqc.c 3.2 2013/08/21 09:26:11 kls Exp $
  */
 
 #include "diseqc.h"
 #include <ctype.h>
+#include <linux/dvb/frontend.h>
+#include <sys/ioctl.h>
 #include "sources.h"
 #include "thread.h"
 
-static bool ParseDeviceNumbers(const char *s, int &Devices)
+#define ALL_DEVICES (~0) // all bits set to '1'
+#define MAX_DEVICES 32   // each bit in a 32-bit integer represents one device
+
+static int CurrentDevices = 0;
+
+static bool IsDeviceNumbers(const char *s)
+{
+  return *s && s[strlen(s) - 1] == ':';
+}
+
+static bool ParseDeviceNumbers(const char *s)
 {
-  if (*s && s[strlen(s) - 1] == ':') {
+  if (IsDeviceNumbers(s)) {
+     CurrentDevices = 0;
      const char *p = s;
      while (*p && *p != ':') {
            char *t = NULL;
            int d = strtol(p, &t, 10);
            p = t;
-           if (0 < d && d < 31)
-              Devices |= (1 << d - 1);
+           if (0 < d && d <= MAX_DEVICES)
+              CurrentDevices |= (1 << d - 1);
            else {
               esyslog("ERROR: invalid device number %d in '%s'", d, s);
               return false;
@@ -31,6 +44,107 @@ static bool ParseDeviceNumbers(const char *s, int &Devices)
   return true;
 }
 
+// --- cDiseqcPositioner -----------------------------------------------------
+
+// See http://www.eutelsat.com/files/live/sites/eutelsatv2/files/contributed/satellites/pdf/Diseqc/associated%20docs/positioner_appli_notice.pdf
+
+cDiseqcPositioner::cDiseqcPositioner(void)
+{
+  SetCapabilities(pcCanDrive |
+                  pcCanStep |
+                  pcCanHalt |
+                  pcCanSetLimits |
+                  pcCanDisableLimits |
+                  pcCanEnableLimits |
+                  pcCanStorePosition |
+                  pcCanRecalcPositions |
+                  pcCanGotoPosition |
+                  pcCanGotoAngle
+                  );
+}
+
+void cDiseqcPositioner::SendDiseqc(uint8_t *Codes, int NumCodes)
+{
+  struct dvb_diseqc_master_cmd cmd;
+  NumCodes = min(NumCodes, int(sizeof(cmd.msg) - 2));
+  cmd.msg_len = 0;
+  cmd.msg[cmd.msg_len++] = 0xE0;
+  cmd.msg[cmd.msg_len++] = 0x31;
+  for (int i = 0; i < NumCodes; i++)
+      cmd.msg[cmd.msg_len++] = Codes[i];
+  CHECK(ioctl(Frontend(), FE_DISEQC_SEND_MASTER_CMD, &cmd));
+}
+
+void cDiseqcPositioner::Drive(ePositionerDirection Direction)
+{
+  uint8_t Code[] = { uint8_t(Direction == pdLeft ? 0x68 : 0x69), 0x00 };
+  SendDiseqc(Code, 2);
+}
+
+void cDiseqcPositioner::Step(ePositionerDirection Direction, uint Steps)
+{
+  if (Steps == 0)
+     return;
+  uint8_t Code[] = { uint8_t(Direction == pdLeft ? 0x68 : 0x69), 0xFF };
+  Code[1] -= min(Steps, uint(0x7F)) - 1;
+  SendDiseqc(Code, 2);
+}
+
+void cDiseqcPositioner::Halt(void)
+{
+  uint8_t Code[] = { 0x60 };
+  SendDiseqc(Code, 1);
+}
+
+void cDiseqcPositioner::SetLimit(ePositionerDirection Direction)
+{
+  uint8_t Code[] = { uint8_t(Direction == pdLeft ? 0x66 : 0x67) };
+  SendDiseqc(Code, 1);
+}
+
+void cDiseqcPositioner::DisableLimits(void)
+{
+  uint8_t Code[] = { 0x63 };
+  SendDiseqc(Code, 1);
+}
+
+void cDiseqcPositioner::EnableLimits(void)
+{
+  uint8_t Code[] = { 0x6A, 0x00 };
+  SendDiseqc(Code, 2);
+}
+
+void cDiseqcPositioner::StorePosition(uint Number)
+{
+  uint8_t Code[] = { 0x6A, uint8_t(Number) };
+  SendDiseqc(Code, 2);
+}
+
+void cDiseqcPositioner::RecalcPositions(uint Number)
+{
+  uint8_t Code[] = { 0x6F, uint8_t(Number), 0x00, 0x00 };
+  SendDiseqc(Code, 4);
+}
+
+void cDiseqcPositioner::GotoPosition(uint Number, int Longitude)
+{
+  uint8_t Code[] = { 0x6B, uint8_t(Number) };
+  SendDiseqc(Code, 2);
+  cPositioner::GotoPosition(Number, Longitude);
+}
+
+void cDiseqcPositioner::GotoAngle(int Longitude)
+{
+  uint8_t Code[] = { 0x6E, 0x00, 0x00 };
+  int Angle = CalcHourAngle(Longitude);
+  int a = abs(Angle);
+  Code[1] = a / 10 / 16;
+  Code[2] = a / 10 % 16 * 16 + a % 10 * 16 / 10;
+  Code[1] |= (Angle < 0) ? 0xE0 : 0xD0;
+  SendDiseqc(Code, 3);
+  cPositioner::GotoAngle(Longitude);
+}
+
 // --- cScr ------------------------------------------------------------------
 
 cScr::cScr(void)
@@ -44,10 +158,9 @@ cScr::cScr(void)
 
 bool cScr::Parse(const char *s)
 {
-  if (!ParseDeviceNumbers(s, devices))
-     return false;
-  if (devices)
-     return true;
+  if (IsDeviceNumbers(s))
+     return ParseDeviceNumbers(s);
+  devices = CurrentDevices;
   bool result = false;
   int fields = sscanf(s, "%d %u %d", &channel, &userBand, &pin);
   if (fields == 2 || fields == 3) {
@@ -68,16 +181,17 @@ bool cScr::Parse(const char *s)
 
 cScrs Scrs;
 
+bool cScrs::Load(const char *FileName, bool AllowComments, bool MustExist)
+{
+  CurrentDevices = ALL_DEVICES;
+  return cConfig<cScr>::Load(FileName, AllowComments, MustExist);
+}
+
 cScr *cScrs::GetUnused(int Device)
 {
   cMutexLock MutexLock(&mutex);
-  int Devices = 0;
   for (cScr *p = First(); p; p = Next(p)) {
-      if (p->Devices()) {
-         Devices = p->Devices();
-         continue;
-         }
-      if (Devices && !(Devices & (1 << Device - 1)))
+      if (!IsBitSet(p->Devices(), Device - 1))
          continue;
       if (!p->Used()) {
         p->SetUsed(true);
@@ -96,6 +210,7 @@ cDiseqc::cDiseqc(void)
   slof = 0;
   polarization = 0;
   lof = 0;
+  position = -1;
   scrBank = -1;
   commands = NULL;
   parsing = false;
@@ -108,10 +223,9 @@ cDiseqc::~cDiseqc()
 
 bool cDiseqc::Parse(const char *s)
 {
-  if (!ParseDeviceNumbers(s, devices))
-     return false;
-  if (devices)
-     return true;
+  if (IsDeviceNumbers(s))
+     return ParseDeviceNumbers(s);
+  devices = CurrentDevices;
   bool result = false;
   char *sourcebuf = NULL;
   int fields = sscanf(s, "%a[^ ] %d %c %d %a[^\n]", &sourcebuf, &slof, &polarization, &lof, &commands);
@@ -178,6 +292,28 @@ const char *cDiseqc::Wait(const char *s) const
   return NULL;
 }
 
+const char *cDiseqc::GetPosition(const char *s) const
+{
+  if (!*s || !isdigit(*s)) {
+     position = 0;
+     return s;
+     }
+  char *p = NULL;
+  errno = 0;
+  int n = strtol(s, &p, 10);
+  if (!errno && p != s && n >= 0 && n < 0xFF) {
+     if (parsing) {
+        if (position < 0)
+           position = n;
+        else
+           esyslog("ERROR: more than one position in '%s'", s - 1);
+        }
+     return p;
+     }
+  esyslog("ERROR: invalid satellite position in '%s'", s - 1);
+  return NULL;
+}
+
 const char *cDiseqc::GetScrBank(const char *s) const
 {
   char *p = NULL;
@@ -250,8 +386,12 @@ cDiseqc::eDiseqcActions cDiseqc::Execute(const char **CurrentAction, uchar *Code
           case 'V': return daVoltage18;
           case 'A': return daMiniA;
           case 'B': return daMiniB;
-          case 'W': *CurrentAction = Wait(*CurrentAction); break;
-          case 'S': *CurrentAction = GetScrBank(*CurrentAction); break;
+          case 'W': *CurrentAction = Wait(*CurrentAction); return daWait;
+          case 'P': *CurrentAction = GetPosition(*CurrentAction);
+                    if (Setup.UsePositioner)
+                       return position ? daPositionN : daPositionA;
+                    break;
+          case 'S': *CurrentAction = GetScrBank(*CurrentAction); return daScr;
           case '[': *CurrentAction = GetCodes(*CurrentAction, Codes, MaxCodes);
                     if (*CurrentAction) {
                        if (Scr && Frequency) {
@@ -261,7 +401,8 @@ cDiseqc::eDiseqcActions cDiseqc::Execute(const char **CurrentAction, uchar *Code
                        return daCodes;
                        }
                     break;
-          default: return daNone;
+          default:  esyslog("ERROR: unknown diseqc code '%c'", *(*CurrentAction - 1));
+                    return daNone;
           }
         }
   return daNone;
@@ -271,17 +412,18 @@ cDiseqc::eDiseqcActions cDiseqc::Execute(const char **CurrentAction, uchar *Code
 
 cDiseqcs Diseqcs;
 
+bool cDiseqcs::Load(const char *FileName, bool AllowComments, bool MustExist)
+{
+  CurrentDevices = ALL_DEVICES;
+  return cConfig<cDiseqc>::Load(FileName, AllowComments, MustExist);
+}
+
 const cDiseqc *cDiseqcs::Get(int Device, int Source, int Frequency, char Polarization, const cScr **Scr) const
 {
-  int Devices = 0;
   for (const cDiseqc *p = First(); p; p = Next(p)) {
-      if (p->Devices()) {
-         Devices = p->Devices();
-         continue;
-         }
-      if (Devices && !(Devices & (1 << Device - 1)))
+      if (!IsBitSet(p->Devices(), Device - 1))
          continue;
-      if (p->Source() == Source && p->Slof() > Frequency && p->Polarization() == toupper(Polarization)) {
+      if (cSource::Matches(p->Source(), Source) && p->Slof() > Frequency && p->Polarization() == toupper(Polarization)) {
          if (p->IsScr() && Scr && !*Scr) {
             *Scr = Scrs.GetUnused(Device);
             if (*Scr)