Added basic support for positioners to control steerable satellite dishes

1 files changed, 140 insertions, 0 deletions

diff --git a/positioner.c b/positioner.c
new file mode 100644
index 00000000..7adcc735
--- /dev/null
+++ b/positioner.c
@@ -0,0 +1,140 @@
+/*
+ * positioner.c: Steerable dish positioning
+ *
+ * See the main source file 'vdr.c' for copyright information and
+ * how to reach the author.
+ *
+ * For an explanation (in German) of the theory behind the calculations see
+ * http://www.vdr-portal.de/board17-developer/board97-vdr-core/p1154305-grundlagen-und-winkelberechnungen-f%C3%BCr-h-h-diseqc-motor-antennenanlagen
+ *
+ * $Id: positioner.c 3.1 2013/08/21 11:02:52 kls Exp $
+ */
+
+#include "positioner.h"
+#include <math.h>
+#include "config.h"
+
+#define SAT_EARTH_RATIO    0.1513 // the Earth's radius, divided by the distance from the Earth's center to the satellite
+#define SAT_VISIBILITY_LAT 810    // the absolute latitude beyond which no satellite can be seen (degrees * 10)
+
+#define RAD(x) ((x) * M_PI / 1800)
+#define DEG(x) ((x) * 1800 / M_PI)
+
+cPositioner *cPositioner::positioner = NULL;
+
+cPositioner::cPositioner(void)
+{
+  capabilities = pcCanNothing;
+  frontend = -1;
+  targetLongitude = lastLongitude = Setup.PositionerLastLon;
+  targetHourAngle = lastHourAngle = CalcHourAngle(lastLongitude);
+  swingTime = 0;
+  delete positioner;
+  positioner = this;
+}
+
+cPositioner::~cPositioner()
+{
+  positioner = NULL;
+}
+
+int cPositioner::NormalizeAngle(int Angle)
+{
+  while (Angle < -1800)
+        Angle += 3600;
+  while (Angle > 1800)
+        Angle -= 3600;
+  return Angle;
+}
+
+int cPositioner::CalcHourAngle(int Longitude)
+{
+  double Delta = RAD(Longitude - Setup.SiteLon);
+  double Lat = RAD(Setup.SiteLat);
+  int Sign = Setup.SiteLat >= 0 ? -1 : 1; // angles to the right are positive, angles to the left are negative
+  return Sign * round(DEG(atan2(sin(Delta), cos(Delta) - cos(Lat) * SAT_EARTH_RATIO)));
+}
+
+int cPositioner::CalcLongitude(int HourAngle)
+{
+  double Lat = RAD(Setup.SiteLat);
+  double Lon = RAD(Setup.SiteLon);
+  double Alpha = RAD(HourAngle);
+  double Delta = Alpha - asin(sin(M_PI - Alpha) * cos(Lat) * SAT_EARTH_RATIO);
+  int Sign = Setup.SiteLat >= 0 ? 1 : -1;
+  return NormalizeAngle(round(DEG(Lon - Sign * Delta)));
+}
+
+int cPositioner::HorizonLongitude(ePositionerDirection Direction)
+{
+  double Delta;
+  if (abs(Setup.SiteLat) < SAT_VISIBILITY_LAT)
+     Delta = acos(SAT_EARTH_RATIO / cos(RAD(Setup.SiteLat)));
+  else
+     Delta = RAD(145);
+  if ((Setup.SiteLat >= 0) != (Direction == pdLeft))
+     Delta = -Delta;
+  return NormalizeAngle(round(DEG(RAD(Setup.SiteLon) + Delta)));
+}
+
+int cPositioner::HardLimitLongitude(ePositionerDirection Direction) const
+{
+  return CalcLongitude(Direction == pdLeft ? -Setup.PositionerSwing : Setup.PositionerSwing);
+}
+
+void cPositioner::StartMovementTimer(int Longitude)
+{
+  if (Setup.PositionerSpeed <= 0)
+     return;
+  cMutexLock MutexLock(&mutex);
+  lastLongitude = CurrentLongitude(); // in case the dish was already in motion
+  targetLongitude = Longitude;
+  lastHourAngle = CalcHourAngle(lastLongitude);
+  targetHourAngle = CalcHourAngle(targetLongitude);
+  swingTime = abs(targetHourAngle - lastHourAngle) * 1000 / Setup.PositionerSpeed; // time (ms) it takes to move the dish from lastHourAngle to targetHourAngle
+  movementStart.Set();
+  Setup.PositionerLastLon = targetLongitude;
+}
+
+void cPositioner::GotoPosition(uint Number, int Longitude)
+{
+  if (Longitude != targetLongitude)
+     dsyslog("moving positioner to position %d, longitude %d", Number, Longitude);
+  StartMovementTimer(Longitude);
+}
+
+void cPositioner::GotoAngle(int Longitude)
+{
+  if (Longitude != targetLongitude)
+     dsyslog("moving positioner to longitude %d", Longitude);
+  StartMovementTimer(Longitude);
+}
+
+int cPositioner::CurrentLongitude(void) const
+{
+  cMutexLock MutexLock(&mutex);
+  if (targetLongitude != lastLongitude) {
+     int Elapsed = movementStart.Elapsed(); // it's important to make this 'int', otherwise the expression below yields funny results
+     if (swingTime <= Elapsed)
+        lastLongitude = targetLongitude;
+     else
+        return CalcLongitude(lastHourAngle + (targetHourAngle - lastHourAngle) * Elapsed / swingTime);
+     }
+  return lastLongitude;
+}
+
+bool cPositioner::IsMoving(void) const
+{
+  cMutexLock MutexLock(&mutex);
+  return CurrentLongitude() != targetLongitude;
+}
+
+cPositioner *cPositioner::GetPositioner(void)
+{
+  return positioner;
+}
+
+void cPositioner::DestroyPositioner(void)
+{
+  delete positioner;
+}