From cfc78aba5fa203c42f5a363847db24eecdf8b23c Mon Sep 17 00:00:00 2001 From: Jaroslav Kysela Date: Mon, 15 Dec 2014 15:21:41 +0100 Subject: [PATCH] linuxdvb rotor: reshuffle code again --- src/input/mpegts/linuxdvb/linuxdvb_rotor.c | 238 ++++++++++----------- 1 file changed, 119 insertions(+), 119 deletions(-) diff --git a/src/input/mpegts/linuxdvb/linuxdvb_rotor.c b/src/input/mpegts/linuxdvb/linuxdvb_rotor.c index 17e9fcac..25a00a2f 100644 --- a/src/input/mpegts/linuxdvb/linuxdvb_rotor.c +++ b/src/input/mpegts/linuxdvb/linuxdvb_rotor.c @@ -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