1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
|
/*
* 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
* by Albert Danis.
*
* $Id: positioner.c 4.0 2015/02/14 11:54:31 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 812 // 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 Alpha = 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(Alpha), cos(Alpha) - cos(Lat) * SAT_EARTH_RATIO)));
}
int cPositioner::CalcLongitude(int HourAngle)
{
double Lat = RAD(Setup.SiteLat);
double Lon = RAD(Setup.SiteLon);
double Delta = RAD(HourAngle);
double Alpha = Delta - asin(sin(M_PI - Delta) * cos(Lat) * SAT_EARTH_RATIO);
int Sign = Setup.SiteLat >= 0 ? 1 : -1;
return NormalizeAngle(round(DEG(Lon - Sign * Alpha)));
}
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 = 0;
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;
}
|
