int
xf86KbdOn()
{
if(QNX_kbd_fd == -1)
QNX_kbd_fd = open("/dev/kbd", O_RDWR);
if(QNX_kbd_proxy == -1)
QNX_kbd_proxy = qnx_proxy_attach(0, 0, 0, -1);
if (QNX_kbd_fd == -1) {
FatalError("xf86KbdOn: Could not open keyboard, errno = %d\n", errno);
}
if (QNX_kbd_proxy == -1) {
FatalError("xf86KbdOn: Could not create kbd proxy, errno = %d\n", errno);
}
if(xf86Verbose)
ErrorF("xf86KbdOn: fd = %d, proxy = %d\n", QNX_kbd_fd, QNX_kbd_proxy);
if (dev_arm(QNX_kbd_fd, QNX_kbd_proxy, _DEV_EVENT_RXRDY) == -1)
FatalError("xf86KbdOn: could not arm kbd proxy, errno %d\n", errno);
return(-1); /* We don't want to select on kbd handle... */
}
void main(int argc, char *argv[])
{int i,j,k,res;
int cnt_K2=0;
pid_t pid_timer;
timer_t id_timer;
struct itimerspec timer;
struct sigevent event;
// obmen_41_31_t from41;
short b2,num_KS=0; //номер канала связи 1 - нр-к1, 2 - пр-к2
unsigned int N_TIMER=0;//кол-во срабатываний таймера
short TIMER10=0;//обмен с пр1.0
short TIMER41=0;
float A; //угол поправки по разности
struct ispr_mo3k *ispr;
int direction=0; //направление движения антенны
float U1,U2; //расчет направления
//----- onucaHue gaHHblx npu pa6ome c MK -----//
int short owu6ka,i1;
pid_t pid,pid_K1,pid_41,pid_42;
dev_tx_t *dev;
unsigned short Kypc,Ckopocmb,KpeH,DuHT;
unsigned short q=1,ncu=2,mema=3;
int StateK1=1,StateK2=0;//0 - stop, 1-start
int TestK2=0;//доп настройки К2
float C1,C2,C3,C4,C5,C6,C7,C8;
int A1;
int TIMESEV,setANT=0,minus_x;
double PSI=0,TETA=0,oldPSI,oldTETA;
double x,y,x1,y1,C,S,ri,r1,r2,r3,
x2=0,y2=0;//дельты по качкам
double prim,primq,primcos;
float KK=0,KK1=0; //курс корабля
//const AgpecHK=28,AgpecCEB=31,nogAgpecHK=0,nogAgpecCEB=0;// agpeca OY
const AgpecHK=18,AgpecCEB=18,nogAgpecHK=0,nogAgpecCEB=0;// agpeca OY ???(CEB=17,18)
const Ynp_np1=1,HK=2,CEB=0;// No KAH MK
const Cnp1=1,CK1=Cnp1+8,CK2=CK1+20;//cMeweHue gaHHblx B Dout
const Cq=16,C42np1=10;//cMeweHue gaHHblx B Dout
unsigned pci_index=0;
unsigned char s,pewuM_K1;
int SIMF[6]={0,0,0,0}; //наличие симфонии 0,1 - ModA : 2,3 - ModB : 4,5 - sevA
int MODB[2]={0,0};
//----- onucaHue daHHblx npu pa6ome c np.4-1,4-2 -----//
#define V 1400
unsigned short DCEB[6];//Dout42[V];
C1=2048./pi;C2=4096.0/360.0;C3=180./pi;C4=C1*Kncu;
C5=C2*Kncu;C6=C1*Kq;C7=C3;C8=C2*Kq;
pid_timer = qnx_proxy_attach( 0, 0, 0, -1 );
if( pid_timer == -1 ) {printf( "Unable to attach proxy." );return;}
/* Attach to the timer */
event.sigev_signo = -pid_timer;
id_timer = timer_create( CLOCK_REALTIME, &event );
if( id_timer == -1 ) {printf( "Unable to attach timer." );return;}
timer.it_value.tv_sec = 1L; //start after X sec
timer.it_value.tv_nsec = 0L;
timer.it_interval.tv_sec = 0;
timer.it_interval.tv_nsec = 100*msec;
timer_settime( id_timer, 0, &timer, NULL );
printf("\nCTAPT M3\n");
create_shmem();
delay(1000);
open_shmem();
ispr = (struct ispr_mo3k *) & p->to_MO3.to42.Mispr;
p->pr1_c=0;
//----- Hacmpouka MaH4ecm.KaH. -----//
owu6ka=0;dev=OpenTx(pci_index);
if(regim_kk(dev,Ynp_np1,true)==-1){owu6ka|=0x4000;}//Heucnp-Mble.owu6ku
if(regim_ou(dev,HK,AgpecHK,true)==-1){owu6ka|=0x2000;}
if(regim_ou(dev,CEB,AgpecCEB,true)==-1) printf("Error OU CEB\n");
Init_ModB();
p->to_MO3.to42.Mispr=0;
p->toPR1[0]=0x07C7;
//p->toPR1[1]=p->toPR1[2]=p->toPR1[3]=0x0000;
p->M[0]=0;
p->M[1]=0x000e;
p->M[2]=0x0000;
p->M[3]=0x8410;
// p->M[3]=0xC430; //включение УМ
for(;;)//----- CEPBEP -----//
{
pid=Receive(0,dev->tx_B,N_B*2);s=0;owu6ka&=0xF000;//ucnp-Mble.owu6ku
if(pid==dev->proxyR[Ynp_np1])s=1;
if(pid==dev->proxyERR[Ynp_np1])s=2;
if(pid==dev->proxyR[HK])s=3;
if(pid==dev->proxyOUT[HK])s=4;
if(pid==dev->proxyMODE[HK])s=5;
if(pid==dev->proxyR[CEB])s=6;
if(pid==dev->proxyOUT[CEB])s=7;
if(pid==dev->proxyMODE[CEB])s=8;
if(pid==proxy_RS)s=9;
if(pid==pid_timer)s=12;
switch(s)
{ case 0:owu6ka|=1;break; // HEBEPEH proxy
case 1://--- ЧТЕНИЕ ДАННЫХ ИЗ ПР1
if(KK_end(dev,Ynp_np1,2)==-1){owu6ka|=2;break;}
p->pr1_c++;
for(i=0;i<8;i++) p->PR1[i]=dev->tx_B[10+i];
//for(i=3;i<7;i++) printf(" %d=%04x",i,dev->tx_B[10+i]);printf(" from\n");
//for(i=3;i<7;i++) printf(" %d=%x",i,toPR1[i]);printf(" to\n");
//for(i=3;i<8;i++) printf(" %x",p->PR1[i]);printf("\n");
/*if (!ispr->mo1k)
{
p->PR1[4]=p->PR1[4]|0x00ff; //новые Васины исправности
p->PR1[4]=p->PR1[4]|0x0f00; //ОС комп
if (p->PR1[4]&0x8000) p->PR1[4]=p->PR1[4]|0xf000; //TVK
p->PR1[3]=p->PR1[3]|0xf800; //ОС комп
}
*/
if (p->PR1[4]&0x4000) p->to_MO3.to42.priem_K2=1; else p->to_MO3.to42.priem_K2=0;
p->PR1[3]=p->PR1[3]|0x2000; //ОС комп
p->PR1[4]=p->PR1[4]|0x0800; //TVP SUM
p->PR1[4]=p->PR1[4]|0x00ff; //лишние ниже списка
p->to_MO3.to42.Ms1=p->PR1[3]; //состояние прибора 1.0
p->to_MO3.to42.Ms2=p->PR1[4];
p->to_MO3.to42.Ms3=p->PR1[5];
p->Dout41[Cq]=p->PR1[0]; // q
p->Dout41[Cq+1]=p->PR1[1];// ncu
p->Dout41[Cq+2]=p->PR1[2];// mema
if (alfa1>0.251) p->to_MO3.to42.alfa=0.258;
else if (alfa1<-0.251) p->to_MO3.to42.alfa=-0.255;
else if (p->PR1[1]&0x800) p->to_MO3.to42.alfa=(p->PR1[1]-0xFFF)/12.27/RADtoGRAD;//КРЕН
else p->to_MO3.to42.alfa=p->PR1[1]/C4;
//ANGLE
memcpy(&byta2,&p->Dout41[0],2); Flt=byta2*pi/(1<<14); p->simfonia41.Kg=Flt;// printf("Kypc=%8.4f \n",Flt);
p->to_MO3.to41.P_FACT=(p->Dout41[16]-1991)*2/RADtoGRAD+KK+x2;//Азимут+курс
if (p->to_MO3.to41.P_FACT<0) p->to_MO3.to41.P_FACT+=2*pi;//Азимут+курс
if (p->to_MO3.to41.P_FACT>2*PI) p->to_MO3.to41.P_FACT-=2*pi;//Азимут+курс
if (p->Dout41[18]&0x800) p->to_MO3.to41.beta_FACT=(360-p->Dout41[18]/C2)/C3; //УГОЛ МЕСТА
else p->to_MO3.to41.beta_FACT=-p->Dout41[18]/C1;
p->to_MO3.to41.beta_FACT+=y2;
//printf(" x2=%f y2=%f ",x2*57.32,y2*57.32);
//printf("\n");
TIMER10=0;//был обмен с пр1.0
break;
case 2://--- ОШИБКА ОБМЕНА С ПР1
if(KK_end(dev,Ynp_np1,2)==-1)owu6ka|=1024;else owu6ka|=4;
break;
case 3://--- HK
SIMF[0]++; //есть симфония
if (SIMF[0]==60000) SIMF[0]=0;
if(ou_read(dev,HK,nogAgpecHK)){owu6ka|=8;break;}
if((dev->tx_B[3])!=32) {owu6ka|=512;printf("error=%d\n",dev->tx_B[3]);break;}
if((dev->tx_B[1])!=0x12) break; //адрес кормовой качки
//printf("%d\n",dev->tx_B[3]);
for(j=0;j<15;j++) p->Dout41[j]=dev->tx_B[4+j]; //--- npueM HK
//printf("N=%d",SIMF[0]);
// printf("ModA simf- "); for(j=0;j<15;j++) printf("%x ",p->Dout41[j]);printf("\n");
// printf("ModA simf- "); for(j=0;j<15;j++) printf("%x ",dev->tx_B[j]);printf("\n");
//printf("%x\n",p->Dout41[4]);
//memcpy(&b2,&p->Dout41[5],2); KK=b2*pi/(1<<14);
KK=p->Dout41[5]*pi/(1<<14);
KK=KK+pi;
if (KK>2*pi) KK=KK-2*pi; //переворот с кормы в нос
// printf("KK=%f ",KK);
//КАЧКИ
// if (p->Dout41[3]&0x8000) PSI=-(p->Dout41[3])*NAVtoRAD/4;
// else PSI=(float)p->Dout41[4]*NAVtoRAD/4;
// if (p->Dout41[1]&0x8000) TETA=-(p->Dout41[2])*NAVtoRAD/4;
// else TETA=(float)p->Dout41[2]*NAVtoRAD/4;
if (p->Dout41[1]&0x8000) PSI=-(0xffff-p->Dout41[1])*pi/(1<<14);
else PSI=p->Dout41[1]*pi/(1<<14);
if (p->Dout41[3]&0x8000) TETA=-(0xffff-p->Dout41[3])*pi/(1<<14);
else TETA=p->Dout41[3]*pi/(1<<14);
// if (dev->tx_B[6]==0x8000) PSI=0;
// if (dev->tx_B[7]==0x8000) TETA=0;
// if (abs(PSI)>1/4) PSI=oldPSI;
// if (abs(TETA)>1/4) TETA=oldTETA;
// printf(" TETA=%f(%f) PSI=%f(%f)\n",PSI,PSI*57.32,TETA,TETA*57.32);
// printf(" A_simf "); for(j=0;j<9;j++) printf("%04x ",p->Dout41[j]);printf("\n");
break;
case 4:owu6ka|=32;break; // HEBEPEH proxy HK
case 5://--- npueM KY cuHxp HK ---//
ou_mode_read(dev,HK,0x8000);
break;
case 6://--- npueMHuk CEB ---//
SIMF[4]++; //есть ModB
if (SIMF[4]==60000) SIMF[2]=0;
if(ou_read(dev,CEB,nogAgpecCEB)){owu6ka|=64;break;}
//if((dev->tx_B[3])!=6){owu6ka|=128;break;}
for(j=0;j<6;j++) p->CEB[j]=dev->tx_B[4+j]; //--- npueM CEB
//for(j=0;j<6;j++) printf(" %x",p->CEB[j]);printf("\n"); //--- npueM CEB
//printf("%02x:%02x:%02x ", p->CEB[2]>>8,p->CEB[3]>>8,p->CEB[3]&0x00ff);printf("\n");
break;//--- end npueMHuk CEB ---//
case 7:case 8:owu6ka|=256;break; // HEBEPEH proxy CEB
case 9: //Обмен с МодБ
SIMF[2]++; //есть ModB
if (SIMF[2]==60000) SIMF[2]=0;
i=Read_ModB(); //читаем данные из Мод Б
//if (i!=48) break; //если не целый пакет - выход
//printf("nk=%x sev=%x\n",Din_ModB[1],Din_ModB[2]);
if (Din_ModB[1]==0) ispr->nkB=0; //всегда работает навигация в Мод Б
//if (Din_ModB[1]==0) ispr->nkB=1; //признак наличия навигации в Мод Б
else {
ispr->nkB=0; //навигац Мод Б исправна
if (ispr->nkA==1) //если нет навигации в Мод А
for(j=0;j<15;j++) p->Dout41[j]=Din_ModB[j+2]; //используем из Б
}
if (Din_ModB[2]==0) ispr->sevB=1; //признак наличия СЕВ в Мод Б
else {
ispr->sevB=0; //сев Мод Б исправен
p->Dout41[30]=(Din_ModB[20]>>8)&0x000F;
p->Dout41[30]+=(Din_ModB[20]>>12)*10; //hours
p->Dout41[31]=(Din_ModB[21]>>8)&0x000F;
p->Dout41[31]+=(Din_ModB[21]>>12)*10; //minutes
p->Dout41[32]=Din_ModB[21]&0x000F;
p->Dout41[32]+=((Din_ModB[21]>>4)&0x000f)*10; //seconds
}
//printf("ModB - "); for(j=0;j<3;j++) printf("%x ",Din_ModB[j]);printf("\n");
//ispr->nkB=ispr->sevB=0;//временно
break;
case 12://обработчик таймера (10 Гц)
N_TIMER++;//счетчик тиков
TIMER41++;
if (TIMER41>1) // 3/10 Hz
{
TIMER10++;
//printf("TIMER10=%d\n",TIMER10);
if (TIMER10<10) ispr->mo1k=0; //есть пр1.0
else ispr->mo1k=1; //нет пр1.0
//printf("1=%x 2=%x 3=%x\n",p->to_MO3.to42.Ms1,p->to_MO3.to42.Ms2,p->to_MO3.to42.Ms3);
//printf("H=%d M=%d S=%d T41=%d T31=%d \n",p->Dout41[30],p->Dout41[31],p->Dout41[32],p->from_MO3.from41.T_SS,p->Dout41[30]*3600+p->Dout41[31]*60+p->Dout41[32]);
//printf("navi=%d jump=%d \n",p->no_navi,p->jump);
//КАЧКИ
oldPSI=PSI;
oldTETA=TETA;
if (p->num_com==1) //подготовка к сеансу связи
{
KK1=p->from_MO3.from41.P_ANT_1-KK;
/*if ((KK1>pi/2)&&(p->from_MO3.from41.P_ANT_2>p->from_MO3.from41.P_ANT_1)) p->jump=-1;
else if ((KK1<-pi/2)&&(p->from_MO3.from41.P_ANT_2<p->from_MO3.from41.P_ANT_1)) p->jump=1;
if (KK1>4.71225) p->jump=-1;
if (KK1<-4.71225) p->jump=1;
KK1=KK1+2*p->jump*pi;
*/
oldKK=KK1; //сохраним установленный азимут
if (p->from_MO3.from41.beta_1>=0) p->toPR1[2]=-p->from_MO3.from41.beta_1*C1;//Угол места
else p->toPR1[2]=(360+(-p->from_MO3.from41.beta_1*C3))*C2;//
p->toPR1[0]=KK1*RADtoGRAD/2+1991;//Азимут
oldKOD=p->toPR1[0];//сохранение кода угла
setANT=1;//была настройка углов перед сеансом
}
else
if (p->num_com==2) //сеанс связи начался
{
KK1=p->from_MO3.from41.P_ANT-KK;
//KK1=KK1+2*p->jump*pi;
if (KK1>4.71225) KK1=KK1-2*PI;
if (KK1<-4.71225) KK1=KK1+2*PI;
if (p->from_MO3.from41.beta>=0) p->toPR1[2]=-p->from_MO3.from41.beta*C1;//Угол места
else p->toPR1[2]=(360+(-p->from_MO3.from41.beta*C3))*C2;//
p->toPR1[0]=KK1*RADtoGRAD/2+1991;//Азимут
//!!!
/*x=(double)KK1; //азимут от 4-1
if (x<0) {x+=2*PI;minus_x=1;} else minus_x=0;
y=(double)p->from_MO3.from41.beta;
// PSI=(double)i*rad;
// TETA=(double)i*rad; // град
r1=cos(y); r3=sin(y);
r2=r1*cos(x); r1=r1*sin(x);
C=cos(-PSI);S=sin(-PSI);
x1=C*r2+S*r3; r3=C*r3-S*r2;
S=sin(-TETA)*r1+cos(-TETA)*r3;
y1=asin(S);
prim=x1/cos(y1); if (prim>1) prim=1;
x1=acos(prim);
if (abs(x1-x)>abs(2*PI-x1-x)) x1=2*PI-x1;
if (minus_x==1) x1=x1-2*PI;
x2=x-x1; //дельта по x
y2=y-y1; //дельта по y
//printf(" x0=%3.1f y0=%3.1f PSI=%3.1f TETA=%3.1f x1=%3.1f y1=%3.1f\n",
//KK*grad,y*grad,PSI*grad,TETA*grad,x1*grad,y1*grad);
KK1=x1;
oldKOD=p->PR1[0];
oldKK=(oldKOD-1991)/325.94915;//Азимут установленный
if (KK1!=oldKK)
{
deltaKK=KK1-oldKK;
//if (deltaKK>0) deltaKK-=0.01;else deltaKK+=0.01;
if (deltaKK>5) {deltaKK-=2*pi;}//printf("-\n");}
if (deltaKK<-5) {deltaKK+=2*pi;}//printf("+\n");}
if (deltaKK>5) {deltaKK-=2*pi;}//printf("-\n");}
if (deltaKK<-5) {deltaKK+=2*pi;}//printf("+\n");}
//deltaKK=deltaKK*RADtoGRAD/2;
deltaKOD=(short)(deltaKK*RADtoGRAD/2);
//deltaKOD=(short)deltaKK;
//printf("P_ANT=%3.2f KK=%3.2f oldKK=%3.2f dKK=%3.2f dKOD=%d\n",
//p->from_MO3.from41.P_ANT*57.32,KK*57.32,oldKK*57.32,deltaKK,deltaKOD);
if (deltaKOD!=0)
{
p->toPR1[0]=oldKOD+deltaKOD;
// oldKOD=p->toPR1[0];
// oldKK=(oldKOD-1991)/325.94915;//Азимут установленный
// printf(" toPR1=%d deltaKOD=%d realKK=%f\n",p->toPR1[0],deltaKOD,oldKK);
}
}
if (y1>=0) p->toPR1[2]=-y1*C1;//Угол места
else p->toPR1[2]=(360+(-y1*C3))*C2;//
*/
}
else
//if ((p->num_com==4)||(p->num_com==5)) //4-я или 5-я команда из 4.2
if (p->num_com==4) //4-я или 5-я команда из 4.2
{ //управление пр. 1.0 из 4.2
//Углы
//printf("Az%f Um=%f\n",p->from_MO3.from42.q,p->from_MO3.from42.beta);
//memcpy(&b2,&p->Dout41[5],2); KK=b2*pi/(1<<14);
KK1=p->from_MO3.from42.q;//-KK;//Азимут
if (p->from_MO3.from42.Rejim_AS==1) //режим АС
{
//printf("lvl = %f r0 = %f ",p->U.SUM_20,p->U.RAZN_0);
p->to_MO3.to42.pr_rejim_AS=1;
if ((p->U.SUM_20>30)&&(abs(p->U.RAZN_0<1.1)))
A1=-p->U.RAZN_0*31.48;
else A1=0;
p->toPR1[0]=(p->PR1[0]&0x0fff)+A1;
//printf("Pr1=%d A1=%d newPr1+%d\n",p->PR1[0]&0x0fff,A1,p->toPR1[0]);
}
else //если не АС
{
if (KK1==0)
{
// //printf("PSI=%f TETA=%f\n",PSI,TETA);
beta1=p->from_MO3.from42.beta-PSI;
if (beta1>=0) p->toPR1[2]=-beta1*C1;//Угол места
else p->toPR1[2]=(360+(-beta1*C3))*C2;//
alfa1=p->from_MO3.from42.alfa-TETA;
if (alfa1>=0) p->toPR1[1]=alfa1*C4;//KPEH
else p->toPR1[1]=0xFFF+(alfa1*RADtoGRAD)*12.27;
p->toPR1[0]=KK1*RADtoGRAD/2+1991;//Азимут
//printf("alfa1=%f beta1=%f \n",alfa1,beta1);
}
else
{
if (p->from_MO3.from42.beta>=0) p->toPR1[2]=-p->from_MO3.from42.beta*C1;//Угол места
else p->toPR1[2]=(360+(-p->from_MO3.from42.beta*C3))*C2;//
if (p->from_MO3.from42.alfa>=0) p->toPR1[1]=p->from_MO3.from42.alfa*C4;//KPEH
else p->toPR1[1]=0xFFF+(p->from_MO3.from42.alfa*RADtoGRAD)*12.27;
p->toPR1[0]=KK1*RADtoGRAD/2+1991;//Азимут
//printf(" KK1=%f \n", KK1*57.32);
}
}
p->to_MO3.to42.pr_rejim_AS=p->from_MO3.from42.Rejim_AS;
}
//printf("n_c=%d \n",p->num_com);
if (p->num_com==301) //
{ //
if (p->from_MO3.fromAK.beta>=0) p->toPR1[2]=-p->from_MO3.fromAK.beta*C1;//╙уюы ьхёЄр
else p->toPR1[2]=(360+(-p->from_MO3.fromAK.beta*C3))*C2;//
//p->toPR1[0]=p->from_MO3.fromAK.Peleng*RADtoGRAD/2+1991;//└чшьєЄ
KK1=p->from_MO3.fromAK.Peleng-KK;
//if (KK1>pi) KK1=-KK1;
if (KK1>4.71225) KK1=KK1-2*PI;
if (KK1<-4.71225) KK1=KK1+2*PI;
printf("Peleng=%2.2f KK=%1.2f KK1=%1.2f\n", p->from_MO3.fromAK.Peleng, KK, KK1);
p->toPR1[0]=KK1*RADtoGRAD/2+1991;//└чшьєЄ
}
//-------------------------------------------------------------
TIMER41=0;
if (SIMF[1]<SIMF[0]) ispr->nkA=0; //есть симф A
else ispr->nkA=1; //нет симф
SIMF[1]=SIMF[0];
if (SIMF[3]<SIMF[2]) ispr->cvsB=0; //есть
else ispr->cvsB=1; //нет модБ,СЕВ,НК
SIMF[3]=SIMF[2];
if (SIMF[5]<SIMF[4]) ispr->sevA=0; //есть сев A
else ispr->sevA=1; //нет
SIMF[5]=SIMF[4];
if ((ispr->nkA==1)&&(ispr->nkB==1)) p->Dout41[24]=0;
else p->Dout41[24]=1;
//printf("ispr=%x \n",p->to_MO3.to42.Mispr);
//printf("n_k=%d \n",p->num_com);
//printf("jmp=%d \n",p->jump);
//printf("ISPR=%x sevB=%x\n",p->to_MO3.to42.Mispr,p->Dout41[24]);
} //конец 3-х Герц
//-------------------------- 10 Hz -------------------------
p->toPR1[3]=p->M[0]; p->toPR1[4]=p->M[1];
p->toPR1[5]=p->M[2]; p->toPR1[6]=p->M[3];
for(i=0;i<3;i++) p->toPR1[i]=p->toPR1[i]&0x0fff;
//-------------------------- 1 Pr -------------------------
for(i=0;i<8;i++) toPR1[i]=p->toPR1[i];
//for(i=3;i<8;i++) printf(" %x",toPR1[i]);printf(" to \n");
//printf("toPR1=%x from42=%f\n",toPR1[2],p->from_MO3.from41.beta);
if((KK_frame(dev,Ynp_np1,2,acmd))==-1){owu6ka|=16;break;}
if (ispr->nkA==1) Write_ModB(); //если нет навигации запрос в Модуль Б
else if (N_TIMER&1) Write_ModB();//иначе частота 5 Гц
break;
}// ----- end switch
} // ----- end CEPBEP for(;;)
}//----- end main()
main(int argc, char *argv[]) {
unsigned short cr_com=0;
unsigned int Tpr=0;
unsigned int Tpr1=0;
unsigned int Tstart=0;
short OC4=0;
pid_t proxy;
FILE *out_fp = NULL;
timer_t id;
unsigned char chkSUM=0,N=0;
struct itimerspec timer;
struct sigevent event;
int K2count=0;//счетчик бафтов в буфере К2
while( (i=getopt(argc, argv, "t:s:") )!=-1) {
switch(i){
case 't' : TM=1; break;
case 's' : TS=1; break;
//case 'c' : sscanf(optarg,"%d",COR_T); break;
}//switch
}//while
delay(500);
open_shmem();
p->to41.PrM_K2=0;
if (TS) printf(" УСТАНОВЛЕН РЕЖИМ РАБОТЫ ПО СИГНАЛУ ТВК\n");
if (TM) printf(" УСТАНОВЛЕН РЕЖИМ РАБОТЫ БЕЗ ОЖИДАНИЯ КОМАНДЫ НАЧАЛА СС\n");
if (COR_T!=0) printf(" УСТАНОВЛЕНА КОРРЕКЦИЯ ПО ВРЕМЕНИ %d МИНУТ\n",COR_T);
// printf("TM=%d TS=%d COR_T=%d\n\n",TM,TS,COR_T);
COR_T=COR_T*60;
if (!TM)
{
//if ( ( out_fp = fopen( "//1/home/seversk/new31/k2_log", "w" ) ) == NULL )
// fprintf( stderr, "Couldn't create/open file. %s\n", strerror( errno ) );
printf("ОЖИДАНИЕ КОМАНДЫ НАЧАЛА СС \n");
while((p->from41.num_com!=1)&&(p->from41.num_com!=2)) delay(500);
}
proxy = qnx_proxy_attach( 0, 0, 0, -1 );
if( proxy == -1 ) {
printf( "Unable to attach proxy." );
return;
}
/* Attach to the timer */
event.sigev_signo = -proxy;
id = timer_create( CLOCK_REALTIME, &event );
if( id == -1 ) {
printf( "Unable to attach timer." );
return;
}
timer.it_value.tv_sec = 3L; //start after X sec
timer.it_value.tv_nsec = 0L;
timer.it_interval.tv_sec = 0;
timer.it_interval.tv_nsec = 100*m_sec;
timer_settime( id, 0, &timer, NULL );
delay(1000);
Init_K2();
delay(500);
printf(" НАСТРОЙКА К2 команда %d\n",p->from41.num_com);
while(1)
{
pid=Receive(0,0,0); //получение всех системных сообщений
if (pid==proxy_DRV2)
{ //получили сообщение чтения данных из ПОСТ-3Ц
rd_cpcs_s.type=4;
rd_cpcs_s.cnl=chan2;
// выдача команды и прием данных
i=Send(pid_drv,&rd_cpcs_s,&rd_cpcs_r,sizeof(rd_cpcs_s),sizeof(rd_cpcs_r));
//printf("Ответ =");
//for(i=0;i<rd_cpcs_r.cnt;i++) printf(" %02x",rd_cpcs_r.uim.dt[i]);printf("\n");
// переформатирование
for (i=0;i<rd_cpcs_r.cnt;i++) buffer[i+K2count]=rd_cpcs_r.uim.dt[i];//дополним массив
//if ( out_fp != NULL )
// if ( fwrite( buffer+K2count, rd_cpcs_r.cnt, 1, out_fp ) != 1 )
// fprintf( stderr, "Failed to write to file. %s\n", strerror( errno ) );
K2count=K2count+rd_cpcs_r.cnt;//увеличим кол-во байт в буфере
}
if (pid==proxy)
{ //срабатывание таймера
Tcount++;
//for(i1=0;i1<K2count;i1++) printf("%x ",buffer[i1]);printf("\n");
//printf("com1=%2x ", c); //печать байта
//printf("count=%d\n",K2count);
if (K2count>4)
for(i=0;i<K2count;i++)
if (buffer[i]==0x55) //найден заголовок
{
N=buffer[i+1]; //кол-во байт в пакете без КС
if (K2count>N+i) //достаточное кол-во байт в буфере
{
//printf("N=%d \n",N);
//1_of_day=time(NULL);
//1strftime(b, 40 , "%T", localtime(&time_of_day));//D T
//1msec=div(Tcount,10);
//1printf("%s:%03d -->",b,msec.rem*100);
printf("%02x:%02x:%02x ", p->CEB[2]>>8,p->CEB[3]>>8,p->CEB[3]&0x00ff);
for(i1=0;i1<N;i1++) chkSUM+=buffer[i+i1]; //подсчет контр суммы
//printf("i=%d chkSUM=%x\n",i,chkSUM);
if (chkSUM!=buffer[N+i]) //если не совпадает контр сумма
{
printf(" error CHKSUM\n");//проверка контрольной суммы
K2count=chkSUM=0; //очистка буфера
break; //выходим и ждем дальше
}
//пришел правильный пакет
printf(" (");
for(i1=0;i1<N+1;i1++) printf("%x.",buffer[i1+i]);printf(")");
//анализ полученного пакета
switch(buffer[i+1])
{
case 4: if(buffer[i+2]==3)
{
//printf(" Получена квитанция - команда принята ");
//if(buffer[i+3]==0) {comOK[1]++;printf("ПРАВИЛЬНО");break;}
//else printf("неправильно");
if(buffer[i+3]==0) {comOK[1]++;printf("");break;}
else printf("");
}
else printf(" Получен неизвестный пакет");
break;
case 5: if(buffer[i+2]==2)
{
switch(buffer[i+3])
{
case 0x03 : printf(" ИЛС Сообщение принято - ");
if (buffer[i+4]==0) {printf("норма");comOK[0]=1;}
else printf("ненорма");
break;
case 0x06 : printf(" ЛК1 Сообщение принято - ");
if (buffer[i+4]==0) {printf("норма");comOK[17]=1;}
else printf("ненорма");
break;
case 0x18 : printf(" ЛК2 Сообщение принято - ");
if (buffer[i+4]==0) {printf("норма");comOK[5]=1;}
else printf("ненорма");
break;
case 0x01 : printf(" АвтК Сообщение принято - ");
if (buffer[i+4]==0) {printf("норма");comOK[7]=1;}
else printf("ненорма");
break;
case 0x02 : printf(" Дан.Н.НН - ");
if (buffer[i+4]&1) printf(" нет-ДанИ");
if (buffer[i+4]&2) printf(" нет-ДанП");
//printf("\n");
break;
case 0x010: printf(" ПРМ - ");
if (buffer[i+4]&0x01) {printf(" ПРС");}//p->to41.PrM_K2=1;}else p->to41.PrM_K2=0;
if (buffer[i+4]&0x02) {printf(" СС");p->to41.PrM_K2=1;}else p->to41.PrM_K2=0;
if (buffer[i+4]&0x04) printf(" ЗС");
if (buffer[i+4]&0x08) printf(" СБ");
if (buffer[i+4]&0x10) {printf(" ИДК");p->to41.Pr_ZI_K2=1;}else p->to41.Pr_ZI_K2=0;
if (buffer[i+4]&0x20) printf(" ИДД");
if (buffer[i+4]&0x40) printf(" ВИ");
if (buffer[i+4]&0x80) printf(" ПС");
//printf("\n");
break;
}
}
else printf(" Получен неизвестный пакет");
break;
case 6: if(buffer[i+2]==2)
{
switch(buffer[i+3])
{
case 0x07 : OC4=buffer[i+5];OC4=(OC4<<8)|buffer[i+4];
printf(" ОСЧ - %6.3f м/с ",OC4*0.582);break;
//printf(" ОСЧ - %d м/с ",OC4);break;
case 0x11 : printf(" Pтек - %d ",buffer[i+4]);
if (buffer[i+4]>0) p->to41.UR_sign_K2=buffer[i+4]*3;
else p->to41.UR_sign_K2=0;
break;
case 0x73 : printf(" ПРМ1 - ");
if (buffer[i+4]&0x01) {printf(" ПРС");}//p->to41.PrM_K2=1;}else p->to41.PrM_K2=0;
//if (buffer[i+4]&0x02) printf(" СС");
if (buffer[i+4]&0x02) {printf(" СС");p->to41.PrM_K2=1;}else p->to41.PrM_K2=0;
if (buffer[i+4]&0x04) printf(" ЗС");
if (buffer[i+4]&0x08) printf(" СБ");
if (buffer[i+4]&0x10) {printf(" ИДК");p->to41.Pr_ZI_K2=1;}else p->to41.Pr_ZI_K2=0;
if (buffer[i+4]&0x20) printf(" ИДД");
if (buffer[i+4]&0x40) printf(" ВИ");
if (buffer[i+4]&0x80) printf(" ПС");
if (buffer[i+5]&0x01) printf(" 27М1н");
if (buffer[i+5]&0x02) printf(" ПФ");
if (buffer[i+5]&0x04) printf(" РНС");
if (buffer[i+5]&0x08) printf(" РПС");
if (buffer[i+5]&0x10) printf(" РСС");
if (buffer[i+5]&0x20) printf(" ЗКС");
if (buffer[i+5]&0x40) printf(" ОСС");
if (buffer[i+5]&0x80) printf(" НС");
//printf("\n");
break;
}
}
else printf(" Получен неизвестный пакет");
break;
case 7: if(buffer[i+2]==2)
{
switch(buffer[i+3])
{
case 0x014: printf(" НУС Сообщение принято - ");
if (buffer[i+4]==0) {printf("норма");comOK[10]=1;}
else printf("ненорма");
comOK[10]=1;//!!!
break;
}
}
else printf(" Получен неизвестный пакет");
break;
case 0x1b:
if(buffer[i+2]==2)
{
printf("\n ");
if (buffer[i+5]||buffer[i+6]) printf("ГР=%02x%02x ",buffer[i+6],buffer[i+5]);
if (buffer[i+7]||buffer[i+8]) printf("НДИ=%02x%02x ",buffer[i+8],buffer[i+7]);
if (buffer[i+9]||buffer[i+10])printf("МН=%02x%02x ",buffer[i+10],buffer[i+9]);
if (buffer[i+11]||buffer[i+12])printf("СГ=%02x%02x ",buffer[i+12],buffer[i+11]);
if (buffer[i+13]||buffer[i+14])printf("ПГ=%02x%02x ",buffer[i+14],buffer[i+13]);
if (buffer[i+15]||buffer[i+16])printf("ВГ=%02x%02x ",buffer[i+16],buffer[i+15]);
if (buffer[i+17]||buffer[i+18])printf("ТГ=%02x%02x ",buffer[i+18],buffer[i+17]);
if (buffer[i+19]||buffer[i+20]||buffer[i+21]) printf("ИИ=%02x%02x%02x ",buffer[i+21],buffer[i+20],buffer[i+19]);
if (buffer[i+24]) {printf("СИГ=%x ",buffer[i+24]);p->to41.GL_priem=1;} else p->to41.GL_priem=0;
if (buffer[i+25]) {printf("СРГ=%x ",buffer[i+25]);p->to41.GL_CP=1;} else p->to41.GL_CP=0;
if (buffer[i+26]) printf("СбСИГ=%x ",buffer[i+26]);
}
else printf(" Получен неизвестный пакет");
break;
}
printf("\n");
chkSUM=0;
K2count-=N+i+1; // сдвигаем данные в буфере
for(i1=0;i1<K2count;i1++) buffer[i1]=buffer[i+N+i1+1];
}
}
void main(int argc, char *argv[]) {
int i, j, k, res, errHK=0, errCEB=0;
pid_t pid_timer;
timer_t id_timer;
struct itimerspec timer;
struct sigevent event;
pid_t proxy_RS=0;
//----- onucaHue gaHHblx npu pa6ome c MK -----//
int short owu6ka,i1;
pid_t pid;
dev_tx_t *dev;
//const AgpecHK=28,AgpecCEB=31,nogAgpecHK=0,nogAgpecCEB=0;// agpeca OY ???(CEB=17,18)
const AgpecHK=28,AgpecCEB=18,nogAgpecHK=0,nogAgpecCEB=0;// agpeca OY ???(CEB=17,18)
const HK=2,CEB=0;// No KAH MK
unsigned pci_index=0;
unsigned char s;
unsigned short Dout[24];
// <0>=0xba, <1>=0/1 -наличие НК, <2-16> -НК, <17>=0/1 -наличие СЕВ, <18-23> -СЕВ
pid_timer = qnx_proxy_attach( 0, 0, 0, -1 );
if( pid_timer == -1 ) {
printf( "Unable to attach proxy." );
return;
}
// Attach to the timer
event.sigev_signo = -pid_timer;
id_timer = timer_create( CLOCK_REALTIME, &event );
if( id_timer == -1 ) {
printf( "Unable to attach timer." );
return;
}
timer.it_value.tv_sec = 1L; //start after X sec
timer.it_value.tv_nsec = 0L;
timer.it_interval.tv_sec = 0;
timer.it_interval.tv_nsec = 100000000;
timer_settime( id_timer, 0, &timer, NULL );
//----- Настройка канала RS-422 -----//
// Проверка запуска драйвера модуля ПЦС ЦВС-3-1
pid_drv=get_pid_process("PCS",CVS_NODE);
if (!pid_drv) {
printf("!!! Драйвер не запущен\n");
exit(-1);
}
// Инициализация канала ПЦС для связи с ЦВС3.1-A
init_pcs_s.type=1;
init_pcs_s.cnl=1; // номер канала ПЦС
init_pcs_s.speed=19200; // скорость передачи данных
init_pcs_s.b_info=8; // кол-во бит в информационном байте
init_pcs_s.b_StrStp=1; // кол-во стоп-бит
init_pcs_s.b_prt='E'; // наличие и тип паритета (четность) УПОС
init_pcs_s.dev=0; // тип у-ва (0 - обычный RS канал)
init_pcs_s.lvl_inp=48; // уровень заполнения FIFO до прерывания
i=Send(pid_drv,&init_pcs_s,&init_pcs_r,sizeof(init_pcs_s),sizeof(init_pcs_r));
if (i==-1) printf("Нет общения с драйвером\n");
if (init_pcs_r.type==1)
if (init_pcs_r.status!=0) {
printf("!!! Инициализация канала 1 не прошла\n");
exit(-1);
}
printf("Инициализация канала 1 прошла\n");
// Старт канала ПЦС для связи с ЦВС3.1-A
start_pcs_s.type=2;
start_pcs_s.cnl=1;
start_pcs_s.ID_P=getpid();
printf("my_id= %d\n",start_pcs_s.ID_P);
Send(pid_drv,&start_pcs_s,&start_pcs_r,sizeof(start_pcs_s),sizeof(start_pcs_r));
if (start_pcs_r.type==2) {
if (start_pcs_r.status==-1) {
printf("!!! Старт канала 1 не выполнен\n");
exit(-1);
}
else { proxy_RS=start_pcs_r.Proxy;
printf("Старт канала 1 выполнен, proxy=%d\n", proxy_RS);
}
}
//----- Hacmpouka MaH4ecm.KaH. -----//
//owu6ka=0;
dev=OpenTx(pci_index);
if(dev==NULL) // ???
{owu6ka=-1;//--- pa6oma 6e3 MK ---//
printf("\nowu6ka MK=%d",owu6ka);fflush(stdout);
}//--- end pa6oma 6e3 MK ---//
//if(regim_ou(dev,HK,AgpecHK,true)==-1) {owu6ka|=0x2000;}
//if(regim_ou(dev,CEB,AgpecCEB,true)==-1) {owu6ka|=0x1000;}
if(regim_ou(dev,HK,AgpecHK,true)==-1) printf("Error OU HK\n");
if(regim_ou(dev,CEB,AgpecCEB,true)==-1) printf("Error OU CEB\n");
Dout[0]=0xba;
for(i=1;i<24;i++) Dout[i]=0;
//----- Ожидание сообщений по МК и запросов по RS
for(;;)//----- CEPBEP -----//
{
// pid=Receive(0,dev->tx_B,N_B*2);s=0;owu6ka&=0xF000;//ucnp-Mble.owu6ku
pid=Receive(0,0,0); s=0;
if(pid==dev->proxyR[HK]) s=1;
if(pid==dev->proxyOUT[HK]) s=2;
if(pid==dev->proxyMODE[HK]) s=3;
if(pid==dev->proxyR[CEB]) s=4;
if(pid==dev->proxyOUT[CEB]) s=5;
if(pid==dev->proxyMODE[CEB]) s=6;
if(pid==pid_timer) s=7;
if(pid==proxy_RS) s=8;
switch(s)
{
case 1://--- npueMHuk HK ---//
if(ou_read(dev,HK,nogAgpecHK)) {break;} //ошибка чтения
// if((dev->tx_B[3])!=15) {break;} //кол-во слов != 15
// for(j=0;j<15;j++) Dout[2+j]=dev->tx_B[4+j]; //--- npueM HK
errHK=0; Dout[1]=1; //есть симфония
// printf("Симфония\n");
// for(i=0;i<3;i++) printf(" %02x",Dout[i]); printf("\n");
break;//--- end npueMHuk HK ---//
case 2: break; // HEBEPEH proxy HK
case 3://--- npueM KY cuHxp HK ---//
ou_mode_read(dev,HK,0x8000);
break;//--- end npueM KY cuHxp HK ---//
case 4: //--- npueMHuk CEB ---//
if(ou_read(dev,CEB,nogAgpecCEB)) {break;} //ошибка чтения
// if((dev->tx_B[3])!=6) {break;} //кол-во слов != 6
// for(j=0;j<6;j++) Dout[18+j]=dev->tx_B[4+j]; //--- npueM CEB
errCEB=0; Dout[2]=1;
// printf("СЕВ\n");
break; //--- end npueMHuk CEB ---//
case 5:case 6: break; // HEBEPEH proxy CEB
case 7: //--- по таймеру
for(i=0;i<4;i++) printf(" %x",Dout[i]);
// printf(" errNK=%x errSEV=%x",errHK,errCEB);
printf("\n");
errHK++; errCEB++;
if(errHK>5) Dout[1]=0;
if(errCEB>5) Dout[2]=0;
break;
case 8: //--- запрос от ЦВС3.1-A
//Прием данных из канала 1 ПЦС
rd_cpcs_s.type=4;
rd_cpcs_s.cnl=1;
Send(pid_drv,&rd_cpcs_s,&rd_cpcs_r,sizeof(rd_cpcs_s),sizeof(rd_cpcs_r));
// printf ("From PCS_M rd_cpcs_r.type=%d rd_cpcs_r.status= %d\n",rd_cpcs_r.type,rd_cpcs_r.status);
// printf ("rd_cpcs_s.cnl=%d rd_cpcs_r.cnt= %d\n", chan_rd, rd_cpcs_r.cnt);
// for(i=0;i<rd_cpcs_r.cnt;i++) printf(" %2x",rd_cpcs_r.uim.dt[i]); printf("\n");
if((rd_cpcs_r.cnt==2) && (rd_cpcs_r.uim.dt[0]==0xab) && (rd_cpcs_r.uim.dt[1]==0xba)) {
//Передача данных в канал 1 ПЦС
wr_cpcs_s.type=5;
wr_cpcs_s.cnl=1;
wr_cpcs_s.cnt=4; //48
// for(i=0;i<24;i++) {wr_cpcs_s.uom.dt[i*2]=Dout[i]&0xff;
// wr_cpcs_s.uom.dt[i*2+1]=(Dout[i]>>8)&0xff;}
// memcpy(&wr_cpcs_s.uom.dt, &Dout, sizeof(Dout));
for(i=0;i<4;i++) wr_cpcs_s.uom.dt[i]=Dout[i];
Send(pid_drv,&wr_cpcs_s,&wr_cpcs_r,sizeof(wr_cpcs_s),sizeof(wr_cpcs_r));
//for(i=0;i<4;i++) printf(" %02x",wr_cpcs_s.uom.dt[i]); printf("\n");
}
break;
} // ----- end switch (s)
} // ----- end CEPBEP for(;;)
}//----- end main()
