rendered paste bodyvoid DvbStream::gotoX( double angle )
{
double latitude = dvbDevice->usalsLatitude;
double longitude = dvbDevice->usalsLongitude;
double P, Ue, Us, az, x, el, Azimuth, USALS=0.0;
int CMD1=0x00, CMD2=0x00;
int DecimalLookup[10] = { 0x00, 0x02, 0x03, 0x05, 0x06, 0x08, 0x0A, 0x0B, 0x0D, 0x0E };
P = latitude*TO_RADS; // Earth Station Latitude
Ue = longitude*TO_RADS; // Earth Station Longitude
Us = angle*TO_RADS; // Satellite Longitude
az = M_PI+atan( tan( Us-Ue )/sin( P ) );
x = acos( cos(Us-Ue)*cos(P) );
el = atan( ( cos( x )-0.1513 )/sin( x ) );
Azimuth = atan( ( -cos(el)*sin(az) )/( sin(el)*cos(P)-cos(el)*sin(P)*cos(az)) )* TO_DEC;
if ( Azimuth>0.0 )
CMD1 = 0xE0; // East
else
CMD1 = 0xD0; // West
USALS = fabs( Azimuth );
while (USALS > 16) {
CMD1++;
USALS-= 16;
}
while (USALS >= 1.0) {
CMD2+=0x10;
USALS--;
}
USALS*= 10.0;
int rd = (int)USALS;
USALS-= rd;
if ( USALS>0.5 )
++rd;
CMD2+= DecimalLookup[rd];
rotorCommand( 12, CMD1, CMD2 );
}
void DvbStream::rotorCommand( int cmd, int n1, int n2, int n3 )
{
struct dvb_diseqc_master_cmd cmds[] = {
{ { 0xe0, 0x31, 0x60, 0x00, 0x00, 0x00 }, 3 }, //0 Stop Positioner movement
{ { 0xe0, 0x31, 0x63, 0x00, 0x00, 0x00 }, 3 }, //1 Disable Limits
{ { 0xe0, 0x31, 0x66, 0x00, 0x00, 0x00 }, 3 }, //2 Set East Limit
{ { 0xe0, 0x31, 0x67, 0x00, 0x00, 0x00 }, 3 }, //3 Set West Limit
{ { 0xe0, 0x31, 0x68, 0x00, 0x00, 0x00 }, 4 }, //4 Drive Motor East continously
{ { 0xe0, 0x31, 0x68,256-n1,0x00, 0x00 }, 4 }, //5 Drive Motor East nn steps
{ { 0xe0, 0x31, 0x69,256-n1,0x00, 0x00 }, 4 }, //6 Drive Motor West nn steps
{ { 0xe0, 0x31, 0x69, 0x00, 0x00, 0x00 }, 4 }, //7 Drive Motor West continously
{ { 0xe0, 0x31, 0x6a, n1, 0x00, 0x00 }, 4 }, //8 Store nn
{ { 0xe0, 0x31, 0x6b, n1, 0x00, 0x00 }, 4 }, //9 Goto nn
{ { 0xe0, 0x31, 0x6f, n1, n2, n3 }, 4}, //10 Recalculate Position
{ { 0xe0, 0x31, 0x6a, 0x00, 0x00, 0x00 }, 4 }, //11 Enable Limits
{ { 0xe0, 0x31, 0x6e, n1, n2, 0x00 }, 5 }, //12 Gotoxx
{ { 0xe0, 0x10, 0x38, 0xF4, 0x00, 0x00 }, 4 } //13 User
};
usleep(15*1000);
if ( ioctl( fdFrontend, FE_DISEQC_SEND_MASTER_CMD, &cmds[cmd] ) )
perror("Rotor : FE_DISEQC_SEND_MASTER_CMD failed");
usleep(15*1000);
}