linuxdvb rotor: reshuffle code again
This commit is contained in:
Jaroslav Kysela
parent
080dfb4ef9
commit
cfc78aba5f
1 changed files with 119 additions and 119 deletions
|
|
@ -131,6 +131,116 @@ double to_rev( double val )
|
|||
return val - floor(val / 360.0) * 360;
|
||||
}
|
||||
|
||||
static
|
||||
void sat_azimuth_and_elevation
|
||||
( double site_lat, double site_lon, double site_alt, double sat_lon,
|
||||
double *azimuth, double *elevation )
|
||||
{
|
||||
const double f = 1.00 / 298.257; // Earth flattening factor
|
||||
const double r_sat = 42164.57; // Distance from earth centre to satellite
|
||||
const double r_eq = 6378.14; // Earth radius
|
||||
|
||||
const double a0 = 0.58804392;
|
||||
const double a1 = -0.17941557;
|
||||
const double a2 = 0.29906946E-1;
|
||||
const double a3 = -0.25187400E-2;
|
||||
const double a4 = 0.82622101E-4;
|
||||
|
||||
double sin_site_lat = sin(to_radians(site_lat));
|
||||
double cos_site_lat = cos(to_radians(site_lat));
|
||||
double Rstation = r_eq / sqrt(1.00 - f*(2.00-f)*sin_site_lat*sin_site_lat);
|
||||
double Ra = (Rstation+site_alt)*cos_site_lat;
|
||||
double Rz = Rstation*(1.00-f)*(1.00-f)*sin_site_lat;
|
||||
double alfa_rx = r_sat*cos(to_radians(sat_lon-site_lon)) - Ra;
|
||||
double alfa_ry = r_sat*sin(to_radians(sat_lon-site_lon));
|
||||
double alfa_rz = -Rz;
|
||||
double alfa_r_north = -alfa_rx*sin_site_lat + alfa_rz*cos_site_lat;
|
||||
|
||||
double alfa_r_zenith = alfa_rx*cos_site_lat + alfa_rz*sin_site_lat;
|
||||
double El_geometric = to_degrees(atan(alfa_r_zenith/sqrt(alfa_r_north*alfa_r_north+alfa_ry*alfa_ry)));
|
||||
double x = fabs(El_geometric+0.589);
|
||||
double refraction = fabs(a0+a1*x+a2*x*x+a3*x*x*x+a4*x*x*x*x);
|
||||
|
||||
*azimuth = 0.00;
|
||||
if (alfa_r_north < 0)
|
||||
*azimuth = 180+to_degrees(atan(alfa_ry/alfa_r_north));
|
||||
else
|
||||
*azimuth = to_rev(360+to_degrees(atan(alfa_ry/alfa_r_north)));
|
||||
|
||||
*elevation = 0.00;
|
||||
if (El_geometric > 10.2)
|
||||
*elevation = El_geometric+0.01617*(cos(to_radians(fabs(El_geometric)))/
|
||||
sin(to_radians(fabs(El_geometric))));
|
||||
else
|
||||
*elevation = El_geometric+refraction;
|
||||
if (alfa_r_zenith < -3000)
|
||||
*elevation = -99;
|
||||
}
|
||||
|
||||
/*
|
||||
* Site Latitude
|
||||
* Site Longtitude
|
||||
* Site Altitude
|
||||
* Satellite Longtitute
|
||||
*/
|
||||
static double
|
||||
sat_angle( linuxdvb_rotor_t *lr, linuxdvb_satconf_ele_t *ls )
|
||||
{
|
||||
linuxdvb_satconf_t *lsp = ls->lse_parent;
|
||||
double site_lat = lsp->ls_site_lat;
|
||||
double site_lon = lsp->ls_site_lon;
|
||||
double site_alt = lsp->ls_site_altitude;
|
||||
double sat_lon = lr->lr_sat_lon;
|
||||
|
||||
if (lsp->ls_site_lat_south)
|
||||
site_lat = -site_lat;
|
||||
if (lsp->ls_site_lon_west)
|
||||
site_lon = 360 - site_lon;
|
||||
if (sat_lon < 0)
|
||||
sat_lon = 360 - sat_lon;
|
||||
|
||||
double azimuth, elevation;
|
||||
|
||||
sat_azimuth_and_elevation(site_lat, site_lon, site_alt, sat_lon,
|
||||
&azimuth, &elevation);
|
||||
|
||||
tvhtrace("diseqc", "rotor angle azimuth %.4f elevation %.4f", azimuth, elevation);
|
||||
|
||||
double rad_azimuth = to_radians(azimuth);
|
||||
double rad_elevation = to_radians(elevation);
|
||||
double rad_site_lat = to_radians(site_lat);
|
||||
double cos_elevation = cos(rad_elevation);
|
||||
double a, b, value;
|
||||
|
||||
a = -cos_elevation * sin(rad_azimuth);
|
||||
b = sin(rad_elevation) * cos(rad_site_lat) -
|
||||
cos_elevation * sin(rad_site_lat) * cos(rad_azimuth);
|
||||
|
||||
value = 180 + to_degrees(atan(a/b));
|
||||
|
||||
if (azimuth > 270) {
|
||||
value = value + 180;
|
||||
if (value > 360)
|
||||
value = 360 - (value-360);
|
||||
}
|
||||
if (azimuth < 90)
|
||||
value = 180 - value;
|
||||
|
||||
int ret;
|
||||
|
||||
if (site_lat >= 0) {
|
||||
ret = round(fabs(180 - value) * 10.0);
|
||||
if (value >= 180)
|
||||
ret = -(ret);
|
||||
} else if (value < 180) {
|
||||
ret = -round(fabs(value) * 10.0);
|
||||
} else {
|
||||
ret = round(fabs(360 - value) * 10.0);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* **************************************************************************
|
||||
* Class methods
|
||||
* *************************************************************************/
|
||||
|
|
@ -147,9 +257,10 @@ linuxdvb_rotor_grace
|
|||
return ls->ls_max_rotor_move;
|
||||
|
||||
newpos = pos_to_integer(lr->lr_sat_lon);
|
||||
|
||||
tunit = 10000; /* 1/1000 sec per one degree */
|
||||
|
||||
delta = abs(deltaI32(ls->ls_last_orbital_pos, newpos));
|
||||
delta = abs(deltaI32(ls->ls_last_orbital_pos, newpos));
|
||||
|
||||
/* ignore very small movements like 0.8W and 1W */
|
||||
if (delta <= 2)
|
||||
|
|
@ -203,92 +314,6 @@ linuxdvb_rotor_gotox_tune
|
|||
return linuxdvb_rotor_grace((linuxdvb_diseqc_t*)lr,lm);
|
||||
}
|
||||
|
||||
static
|
||||
void usals_sat_azimuth_and_elevation
|
||||
( double site_lat, double site_lon, double site_alt, double sat_lon,
|
||||
double *azimuth, double *elevation )
|
||||
{
|
||||
const double f = 1.00 / 298.257; // Earth flattening factor
|
||||
const double r_sat = 42164.57; // Distance from earth centre to satellite
|
||||
const double r_eq = 6378.14; // Earth radius
|
||||
|
||||
const double a0 = 0.58804392;
|
||||
const double a1 = -0.17941557;
|
||||
const double a2 = 0.29906946E-1;
|
||||
const double a3 = -0.25187400E-2;
|
||||
const double a4 = 0.82622101E-4;
|
||||
|
||||
double sin_site_lat = sin(to_radians(site_lat));
|
||||
double cos_site_lat = cos(to_radians(site_lat));
|
||||
double Rstation = r_eq / sqrt(1.00 - f*(2.00-f)*sin_site_lat*sin_site_lat);
|
||||
double Ra = (Rstation+site_alt)*cos_site_lat;
|
||||
double Rz = Rstation*(1.00-f)*(1.00-f)*sin_site_lat;
|
||||
double alfa_rx = r_sat*cos(to_radians(sat_lon-site_lon)) - Ra;
|
||||
double alfa_ry = r_sat*sin(to_radians(sat_lon-site_lon));
|
||||
double alfa_rz = -Rz;
|
||||
double alfa_r_north = -alfa_rx*sin_site_lat + alfa_rz*cos_site_lat;
|
||||
|
||||
double alfa_r_zenith = alfa_rx*cos_site_lat + alfa_rz*sin_site_lat;
|
||||
double El_geometric = to_degrees(atan(alfa_r_zenith/sqrt(alfa_r_north*alfa_r_north+alfa_ry*alfa_ry)));
|
||||
double x = fabs(El_geometric+0.589);
|
||||
double refraction = fabs(a0+a1*x+a2*x*x+a3*x*x*x+a4*x*x*x*x);
|
||||
|
||||
*azimuth = 0.00;
|
||||
if (alfa_r_north < 0)
|
||||
*azimuth = 180+to_degrees(atan(alfa_ry/alfa_r_north));
|
||||
else
|
||||
*azimuth = to_rev(360+to_degrees(atan(alfa_ry/alfa_r_north)));
|
||||
|
||||
*elevation = 0.00;
|
||||
if (El_geometric > 10.2)
|
||||
*elevation = El_geometric+0.01617*(cos(to_radians(fabs(El_geometric)))/
|
||||
sin(to_radians(fabs(El_geometric))));
|
||||
else
|
||||
*elevation = El_geometric+refraction;
|
||||
if (alfa_r_zenith < -3000)
|
||||
*elevation = -99;
|
||||
}
|
||||
|
||||
/*
|
||||
* Site Latitude
|
||||
* Site Longtitude
|
||||
* Site Altitude
|
||||
* Satellite Longtitute
|
||||
*/
|
||||
static double
|
||||
usals_sat_angle( double site_lat, double site_lon,
|
||||
double site_alt, double sat_lon )
|
||||
{
|
||||
double azimuth, elevation;
|
||||
|
||||
usals_sat_azimuth_and_elevation(site_lat, site_lon, site_alt, sat_lon,
|
||||
&azimuth, &elevation);
|
||||
|
||||
tvhtrace("diseqc", "rotor angle azimuth %.4f elevation %.4f", azimuth, elevation);
|
||||
|
||||
double rad_azimuth = to_radians(azimuth);
|
||||
double rad_elevation = to_radians(elevation);
|
||||
double rad_site_lat = to_radians(site_lat);
|
||||
double cos_elevation = cos(rad_elevation);
|
||||
double a, b, value;
|
||||
|
||||
a = -cos_elevation * sin(rad_azimuth);
|
||||
b = sin(rad_elevation) * cos(rad_site_lat) -
|
||||
cos_elevation * sin(rad_site_lat) * cos(rad_azimuth);
|
||||
|
||||
value = 180 + to_degrees(atan(a/b));
|
||||
|
||||
if (azimuth > 270) {
|
||||
value = value + 180;
|
||||
if (value > 360)
|
||||
value = 360 - (value-360);
|
||||
}
|
||||
if (azimuth < 90)
|
||||
value = 180 - value;
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
/* USALS */
|
||||
static int
|
||||
linuxdvb_rotor_usals_tune
|
||||
|
|
@ -298,40 +323,18 @@ linuxdvb_rotor_usals_tune
|
|||
{ 0x00, 0x02, 0x03, 0x05, 0x06, 0x08, 0x0A, 0x0B, 0x0D, 0x0E };
|
||||
|
||||
linuxdvb_satconf_t *lsp = ls->lse_parent;
|
||||
int i, angle = sat_angle(lr, ls);
|
||||
uint32_t cmd = 0xE000;
|
||||
|
||||
double site_lat = lsp->ls_site_lat;
|
||||
double site_lon = lsp->ls_site_lon;
|
||||
double sat_lon = lr->lr_sat_lon;
|
||||
double motor_angle;
|
||||
uint32_t tmp, cmd;
|
||||
int i;
|
||||
|
||||
if (lsp->ls_site_lat_south)
|
||||
site_lat = -site_lat;
|
||||
if (lsp->ls_site_lon_west)
|
||||
site_lon = 360 - site_lon;
|
||||
if (sat_lon < 0)
|
||||
sat_lon = 360 - sat_lon;
|
||||
|
||||
motor_angle = usals_sat_angle(site_lat, site_lon,
|
||||
lsp->ls_site_altitude,
|
||||
sat_lon);
|
||||
|
||||
if (site_lat >= 0) {
|
||||
tmp = round(fabs(180 - motor_angle) * 10.0);
|
||||
cmd = ((tmp / 10) * 0x10 + xtable[tmp % 10]) |
|
||||
(motor_angle < 180 ? 0xE000 : 0xD000);
|
||||
} else if (motor_angle < 180) {
|
||||
tmp = round(fabs(motor_angle) * 10.0);
|
||||
cmd = ((tmp / 10) * 0x10 + xtable[tmp % 10]) | 0xD000;
|
||||
} else {
|
||||
tmp = round(fabs(360 - motor_angle) * 10.0);
|
||||
cmd = ((tmp / 10) * 0x10 + xtable[tmp % 10]) | 0xE000;
|
||||
if (angle < 0) {
|
||||
angle = -(angle);
|
||||
cmd = 0xD000;
|
||||
}
|
||||
cmd |= (angle / 10) * 0x10 + xtable[angle % 10];
|
||||
|
||||
tvhdebug("diseqc", "rotor USALS goto %0.1f%c (motor %0.1f %sclockwise)",
|
||||
fabs(lr->lr_sat_lon), (lr->lr_sat_lon > 0.0) ? 'E' : 'W',
|
||||
((double)tmp / 10.0), (cmd & 0xF000) == 0xD000 ? "counter-" : "");
|
||||
((double)angle / 10.0), (cmd & 0xF000) == 0xD000 ? "counter-" : "");
|
||||
|
||||
for (i = 0; i <= lsp->ls_diseqc_repeats; i++) {
|
||||
if (linuxdvb_diseqc_send(fd, 0xE0, 0x31, 0x6E, 2,
|
||||
|
|
@ -343,9 +346,6 @@ linuxdvb_rotor_usals_tune
|
|||
}
|
||||
|
||||
return linuxdvb_rotor_grace((linuxdvb_diseqc_t*)lr,lm);
|
||||
|
||||
#undef TO_RAD
|
||||
#undef TO_DEG
|
||||
}
|
||||
|
||||
static int
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue