main ()
{
float C1,C2,C3,C4,C5,C6,C7,C8;
unsigned short cr_com; //порядковый номер предыдущей команды
short V,dV;
int cnt=0;
int i1=0,i2;
int rez;
unsigned short buf[4];
float Angle0;
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;
//поиск сервера
//qnx_name_attach(0,"4.1");
//инициализация канала UDP
i = Udp_Client_Ini(&Uc41,"194.1.1.6",SRC_PORT41,DST_PORT41);
printf(" Udp_Init=%d \n", i);
//gloriya(1,1,31);//test K2 по умолчанию
//gloriya(1,1,1);//work K2
//gloriya(1,0,2);//work K1
gloriya(1,0,31);//test K2 по умолчанию
delay(2000);
open_shmem();
delay(1000);
//Angle0=4;
//p->jump=-1;
while(1)
{
//for(i=0;i<sizeof(obmen_41_31_t);i++) bufi[i]=0;
bytes = Udp_Client_Read(&Uc41,bufi,4096);
// printf(" read=%d size1=%d size2=%d size3=%d sizeALL=%d\n",
// bytes,sizeof(obmen_42_31_2t),sizeof(obmen_41_31_2t),sizeof(obmen_AK_MN3_MO3K_t),sizeof(obmen_MO3_MO3K_t));
memcpy(&p->from_MO3,&bufi[4],sizeof(obmen_MO3_MO3K_t));
//выбор управляюще1 команды
if (p->from_MO3.from42.cr_com!=cr_com42)
{
printf(" New Command 4.2 = %d cr_com = %d\n",
p->from_MO3.from42.num_com,p->from_MO3.from42.cr_com);
p->num_com=p->from_MO3.from42.num_com;
cr_com42=p->from_MO3.from42.cr_com;
if (p->num_com==5)
{
p->M[0]=p->from_MO3.from42.M1;
p->M[1]=p->from_MO3.from42.M2;
p->M[2]=p->from_MO3.from42.M3;
p->M[3]=p->from_MO3.from42.M4;
buf[0]=0; //переворачиваем управляющие слова пр. 1
for(i1=0;i1<16;i1++) {buf[0]+=((p->M[0]>>i1)&1)<<(15-i1);} p->M[0]=buf[0];buf[0]=0;
for(i1=0;i1<16;i1++) {buf[0]+=((p->M[1]>>i1)&1)<<(15-i1);} p->M[1]=buf[0];buf[0]=0;
for(i1=0;i1<16;i1++) {buf[0]+=((p->M[2]>>i1)&1)<<(15-i1);} p->M[2]=buf[0];buf[0]=0;
for(i1=0;i1<16;i1++) {buf[0]+=((p->M[3]>>i1)&1)<<(15-i1);} p->M[3]=buf[0];buf[0]=0;
}
}
int main(){
signal(SIGQUIT, quit);
signal(SIGINT, quit);
signal(SIGUSR2, priority);
signal(SIGUSR1, feux);
key_t cle_shmem = KEY_SHMEM;
key_t key_mutex = KEY_MUTEX;
if((int)(id_mutex = open_semaphore(key_mutex)) == -1) {
printf("Impossible d'ouvrir le mutex.\n");
quit();
}
if((int)(id_shmem = open_shmem(cle_shmem, shmem_size)) == -1) {
printf("Feux : Impossible d'ouvrir la mémoire partagée.\n");
quit();
}
if((int)(pshmem = attach_shmem(id_shmem)) == -1) {
printf("Feux : Impossible de s'attacher à la mémoire partagée.\n");
quit();
}
printf("PID Feux : %d\n", getpid());
down(id_mutex);
pshmem[PID_FEUX]=getpid();
up(id_mutex);
feux();
}
int main()
{
int key = 123 ;
int memsize = sizeof(int) ;
int shmemId = open_shmem(key, memsize) ;
if (shmemId == -1) {
perror("shared segment opening error") ;
exit(EXIT_FAILURE) ;
}
int* buffer = (int*) attach_shmem(shmemId) ;
*buffer = 0 ;
int emptySemKey = 100 ;
int emptySemId = open_semaphore(emptySemKey) ;
int fullSemKey = 101 ;
int fullSemId = open_semaphore(fullSemKey) ;
if ((emptySemId == -1) || (fullSemId == -1)) {
perror("semaphore opening error") ;
exit(EXIT_FAILURE) ;
}
while (*buffer >= 0) {
down(fullSemId) ;
if (*buffer >= 0)
printf("%d\n", *buffer) ;
up(emptySemId) ;
}
detach_shmem(buffer) ;
}
void generateurTrafficPrioritaire(){
key_t key_shmem = KEY_SHMEM;
key_t key_mutex = KEY_MUTEX;
if((id_mutex = open_semaphore(key_mutex)) == -1) {
printf("Impossible d'ouvrir le mutex.\n");
quit();
}
if((int)(id_shmem = open_shmem(key_shmem, shmem_size)) == -1) {
printf("generateurTrafficPrioritaire : Impossible d'ouvrir la mémoire partagée.\n");
quit();
}
if((int)(pshmem = attach_shmem(id_shmem)) == -1) {
printf("generateurTrafficPrioritaire : Impossible de s'attacher à la mémoire partagée.\n");
quit();
}
down(id_mutex);
pshmem[SRC_PRIO] = 0;
pshmem[DEST_PRIO] = 0;
pshmem[ID_PRIO] = 0;
pshmem[PID_PRIO]=getpid();
up(id_mutex);
int pid_coord = pshmem[PID_COORD];
printf("PID generPrio : %d\n", getpid());
printf("PID Coord : %d\n", pid_coord);
int timer, source, dest, id=0;
srand(time(NULL));
for(;;){
timer=rand()%26;
while(timer<15){
timer=rand()%26;
}
printf("Timer is UP: %d s\n", timer);
sleep(timer);
printf("Timer is DOWN: %d s\n", timer);
source=(rand()%4)+1;
dest=(rand()%4)+1;
while(dest==source){
dest=(rand()%4)+1;
}
down(id_mutex);
pshmem[SRC_PRIO] = source;
pshmem[DEST_PRIO] = dest;
pshmem[ID_PRIO] = id;
up(id_mutex);
kill(pid_coord, SIGUSR1);
printf("! --- PRIORITAIRE --- ! Source: %d, Dest: %d, ID :%d\n", source, dest, id);
printf("Coordinateur is notified by signal !\n");
id++;
}
}
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 MAIN MAIN MAIN MAIN
main(int argc, char *argv[])
{
int n,i,j,col,command,i1;
unsigned char rele=0;// sosto9nie rele
short c_step=0,T0=0;
long rele_timer=0;
unsigned short save;
int ind=0,chk_num,Time_out=5,Con1=1;
if (p->verbose) printf("START MO3A<->RELE\n\n");
//===============================================================================
// timer 10ms
event_sig.sigev_signo = SIGALRM;
tm10 = timer_create(CLOCK_REALTIME,&event_sig ); // ёючфрэшх ЄрщьхЁр
if (tm10==-1) printf("\ntimer opening error: %s",strerror(errno));
timer_sig.it_value.tv_sec = 0L;
timer_sig.it_value.tv_nsec = 0L;
timer_sig.it_interval.tv_sec = 0L;
timer_sig.it_interval.tv_nsec = 0L;
//--------------------------------------------nastroika rele----------------------------
arginit(argc,argv,"тест модуля cp384\n");
argp("IO=", "%d", &ind, "Индекс модуля на шине CPCI");
argp("V_Br=","%x",&V_Bridge,"ID производителя");
argp("D_Br=", "%x", &D_Bridge, "ID устройства");
argp("delay=", "%d", &Time_out, "Таймаут запуска теста");
argp("Con1=", "%x", &Con1, "Режим надетой заглушки - 0 - нет");
delay(Time_out);
//Определение Базового адреса внутренних регистров моста
i=new_func_read(D_Bridge,V_Bridge,&my_device,BAR0,ind);
if (i==-1) {printf("Мост отсутствует");exit(1);}
else if (!PCI_IS_MEM(mass[BAR0])) {printf("PCI устройство не обнаружено");exit(1);}
//Отображение портов модуля в PCI Memory Space
printf ("Memory %08x\n",ba=PCI_MEM_ADDR(mass[BAR0]));
//----Попытка работать с памятью
fd1=shm_open("Physical",O_RDWR,0777);
ptr1=(char *)mmap(0,64*1024,PROT_READ|PROT_WRITE|PROT_NOCACHE,MAP_SHARED,fd1,
PCI_MEM_ADDR(mass[BAR0])&~4095);
printf ("ptr1 = %x %8x\n",ptr1,ba);
if (ptr1==(char *)-1) {printf ("FAULT\n"); exit (-1);}
addr1=ptr1 + (ba&4095);
printf ("Memory configuration addr = %x\n",addr1);
*(unsigned int*)(addr1)=0xFFFFFFFF;
i=1; printf("WRITE O_CTL %x %x\n",0x2000 + 0x400*i,*(unsigned int*)(addr1 +0x2000 + 0x400*i)=0);
i++; printf("READ O_STA_A %x, %x\n",0x2000 + 0x400*i,*(unsigned int*)(addr1 +0x2000 + 0x400*i));
i++; printf("WRITE I_CTL %x\n",*(unsigned int*)(addr1 +0x4400)=0x80);
//--------------------------------------------nastroika rele----------------------------
//create_shmem();
delay(500);
open_shmem();
delay(500);
//---------------------------------------------------------------------
while(1)
{
if (rele_timer!=p->sys_timer) //timer
{
rele_timer=p->sys_timer;
if (p->cur_step!=0) //est' wag dl9 vipolnenni9
{
c_step=p->cur_step;
for (i=0;i<p->work_com[c_step].num_mini_com;i++) //prosmotrim vse minicomandi na wage
if((p->work_com[c_step].s[i].n_chan==N_CHAN)&&(p->work_com[c_step].s[i].status!=2)) //na tekuwem wage (i - minikomanda) est' komanda dl9 nas
{
if((p->verbose>1)&&(p->work_com[c_step].s[i].status==0)) printf("\nSTEP=%d minicom for RELE : %d status=%d time %d \n", p->cur_step, p->work_com[c_step].s[i].n_com, p->work_com[c_step].s[i].status, p->sys_timer);
switch(p->work_com[c_step].s[i].n_com)
{
/*case 1: p->work_com[c_step].s[i].status=1;
rele|=(1<<p->inbufMN3.a_params[0]);
*(unsigned int*)(addr1 +0x2C00)=rele;
if(p->verbose>1) printf("WRITE OUT DATA 0x2C00 %x\n",rele);
if(p->verbose>1) printf("READ DATA %x\n",*(unsigned int*)(addr1 + 0x4C00));
p->work_com[c_step].s[i].status=2;
break;
case 2: p->work_com[c_step].s[i].status=1;
rele &= ~(1 << p->inbufMN3.a_params[0]);
*(unsigned int*)(addr1 +0x2C00)=rele;
if(p->verbose>1) printf("WRITE OUT DATA 0x2C00 %x\n",rele);
if(p->verbose>1) printf("READ DATA %x\n",*(unsigned int*)(addr1 + 0x4C00));
p->work_com[c_step].s[i].status=2;
break;
*/
case 10: //off 1 rele
if (p->work_com[c_step].s[i].status==0)
{
p->work_com[c_step].s[i].status=1;
rele &= ~1; *(unsigned int*)(addr1 +0x2C00)=rele;
if(p->verbose>1) printf("WRITE OUT DATA %x\n",rele);
}
if (p->work_com[c_step].s[i].status==1)
{
if ((*(unsigned int*)(addr1 + 0x4C00)&0x08)==0) p->work_com[c_step].s[i].status=2;
if(p->verbose>1) printf("READ DATA %x %x\n",*(unsigned int*)(addr1 + 0x4C00),*(unsigned int*)(addr1 + 0x4C00)&0x01);
}
break;
case 11: //on 1 rele
if (p->work_com[c_step].s[i].status==0)
{
p->work_com[c_step].s[i].status=1;
rele|=1; *(unsigned int*)(addr1 +0x2C00)=rele;
if(p->verbose>1) printf("WRITE OUT DATA %x\n",rele);
}
if (p->work_com[c_step].s[i].status==1)
{
if (*(unsigned int*)(addr1 + 0x4C00)&0x08) p->work_com[c_step].s[i].status=2;
if(p->verbose>1) printf("READ DATA %x \n",*(unsigned int*)(addr1 + 0x4C00));
}
break;
case 20: //off 2 rele
if (p->work_com[c_step].s[i].status==0)
{
p->work_com[c_step].s[i].status=1;
rele &= ~2; *(unsigned int*)(addr1 +0x2C00)=rele;
if(p->verbose>1) printf("WRITE OUT DATA %x\n",rele);
}
if (p->work_com[c_step].s[i].status==1)
{
if ((*(unsigned int*)(addr1 + 0x4C00)&0x02)==0) p->work_com[c_step].s[i].status=2;
if(p->verbose>1) printf("READ DATA %x %x\n",*(unsigned int*)(addr1 + 0x4C00),*(unsigned int*)(addr1 + 0x4C00)&0x01);
}
break;
case 21: //on 2 rele
if (p->work_com[c_step].s[i].status==0)
{
p->work_com[c_step].s[i].status=1;
rele|=2; *(unsigned int*)(addr1 +0x2C00)=rele;
if(p->verbose>1) printf("WRITE OUT DATA %x\n",rele);
}
if (p->work_com[c_step].s[i].status==1)
{
if (*(unsigned int*)(addr1 + 0x4C00)&0x02) p->work_com[c_step].s[i].status=2;
if(p->verbose>1) printf("READ DATA %x \n",*(unsigned int*)(addr1 + 0x4C00));
}
break;
case 30: //off 3 rele
if (p->work_com[c_step].s[i].status==0)
{
p->work_com[c_step].s[i].status=1;
rele &= ~4; *(unsigned int*)(addr1 +0x2C00)=rele;
if(p->verbose>1) printf("WRITE OUT DATA %x\n",rele);
}
if (p->work_com[c_step].s[i].status==1)
{
if ((p->inbufMN3.a_params[0]==0)&&((*(unsigned int*)(addr1 + 0x4C00)&0x04)==0)) p->work_com[c_step].s[i].status=2;
if(p->verbose>1) printf("READ DATA %x\n",*(unsigned int*)(addr1 + 0x4C00));
}
break;
case 31: //on 3 rele
if (p->work_com[c_step].s[i].status==0)
{
p->work_com[c_step].s[i].status=1;
rele|=4; *(unsigned int*)(addr1 +0x2C00)=rele;
if(p->verbose>1) printf("WRITE OUT DATA %x\n",rele);
//p->work_com[c_step].t_start = p->sys_timer;
//if(p->verbose>1) printf("READ DATA %x\n",*(unsigned int*)(addr1 + 0x4C00));
}
//if ((p->work_com[c_step].s[i].status==1)&&(p->sys_timer - p->work_com[c_step].t_start > 20))
if (p->work_com[c_step].s[i].status==1)
{
if (*(unsigned int*)(addr1 + 0x4C00)&0x04) p->work_com[c_step].s[i].status=2;
//if ((p->inbufMN3.a_params[0]==0)&&((*(unsigned int*)(addr1 + 0x4C00)&0x04)==0)) p->work_com[c_step].s[i].status=2;
if(p->verbose>1) printf("READ DATA %x\n",*(unsigned int*)(addr1 + 0x4C00));
}
break;
case 40: //off 4 rele
if (p->work_com[c_step].s[i].status==0)
{
p->work_com[c_step].s[i].status=2;
rele &= ~8; *(unsigned int*)(addr1 +0x2C00)=rele;
if(p->verbose>1) printf("WRITE OUT DATA %x\n",rele);
}
break;
case 41: //on 4 rele
if (p->work_com[c_step].s[i].status==0)
{
p->work_com[c_step].s[i].status=2;
rele|=8; *(unsigned int*)(addr1 +0x2C00)=rele;
if(p->verbose>1) printf("WRITE OUT DATA %x\n",rele);
}
break;
case 50: //off 5 rele
if (p->work_com[c_step].s[i].status==0)
{
p->work_com[c_step].s[i].status=2;
rele &= ~0x10; *(unsigned int*)(addr1 +0x2C00)=rele;
if(p->verbose>1) printf("WRITE OUT DATA %x\n",rele);
}
break;
case 51: //on 5 rele
if (p->work_com[c_step].s[i].status==0)
{
p->work_com[c_step].s[i].status=2;
rele|=0x10; *(unsigned int*)(addr1 +0x2C00)=rele;
if(p->verbose>1) printf("WRITE OUT DATA %x\n",rele);
}
break;
case 60: //off 6 rele
if (p->work_com[c_step].s[i].status==0)
{
p->work_com[c_step].s[i].status=1;
rele &= ~0x20; *(unsigned int*)(addr1 +0x2C00)=rele;
if(p->verbose>1) printf("WRITE OUT DATA 0x2C00 %x\n",rele);
}
if (p->work_com[c_step].s[i].status==1)
{
//if ((*(unsigned int*)(addr1 + 0x4C00)&0x20)==0)
p->work_com[c_step].s[i].status=2;
if(p->verbose>1) printf("READ DATA %x %x\n",*(unsigned int*)(addr1 + 0x4C00),*(unsigned int*)(addr1 + 0x4C00)&0x01);
}
break;
case 61: //on 6 rele
if (p->work_com[c_step].s[i].status==0)
{
p->work_com[c_step].s[i].status=1;
rele|=0x20; *(unsigned int*)(addr1 +0x2C00)=rele;
if(p->verbose>1) printf("WRITE OUT DATA 0x2C00 %x\n",rele);
}
if (p->work_com[c_step].s[i].status==1)
{
//if (*(unsigned int*)(addr1 + 0x4C00)&0x20)
p->work_com[c_step].s[i].status=2;
if(p->verbose>1) printf("READ DATA %x \n",*(unsigned int*)(addr1 + 0x4C00));
}
break;
case 25: p->work_com[c_step].s[i].status=1;
if (p->inbufMN3.a_params[0]) rele|=2;
else rele &= ~2;
*(unsigned int*)(addr1 +0x2C00)=rele;
if(p->verbose>1) printf("WRITE OUT DATA %x\n",rele);
if(p->verbose>1) printf("READ DATA %x\n",*(unsigned int*)(addr1 + 0x4C00));
p->work_com[c_step].s[i].status=2;
break;
case 29: p->work_com[c_step].s[i].status=1;
//if(p->verbose>1)
printf("READ DATA %x\n",*(unsigned int*)(addr1 + 0x4C00));
p->work_com[c_step].s[i].status=2;
break;
case 90: // pause
//if(p->verbose>1) printf(" %d\n",p->work_com[c_step].t_stop-p->sys_timer);
if (p->work_com[c_step].t_stop-p->sys_timer<20) p->work_com[c_step].s[i].status=2;
break;
case 104: //Ogidanie Ispravnosti R999
//p->work_com[c_step].s[i].status=1;
//if(p->verbose>1)
if (p->inbufMN3.a_params[0]&&(*(unsigned int*)(addr1 + 0x4C00)&0x04)) p->work_com[c_step].s[i].status=2;
if ((p->inbufMN3.a_params[0]==0)&&((*(unsigned int*)(addr1 + 0x4C00)&0x04)==0)) p->work_com[c_step].s[i].status=2;
//if ((*(unsigned int*)(addr1 + 0x4C00)&0x04)&&(p->inbufMN3.a_params[0])) p->work_com[c_step].s[i].status=2;
printf("READ DATA %x\n",*(unsigned int*)(addr1 + 0x4C00));
//p->work_com[c_step].s[i].status=2;
break;
default:
printf("Bad minicom %d for %d chan : %d",p->work_com[c_step].s[i].n_com, N_CHAN);
p->work_com[c_step].s[i].status=3;
}//switch (n_com)
//-------------------------------------------------------
//esli previweno vrem9 ozhidani9
//if ((p->work_com[c_step].s[i].status==1)&&(p->work_com[c_step].s[i].t_stop>p->sys_timer)) p->work_com[c_step].s[i].status=3;
//-------------------------------------------------------
}//ewe ne vipoln9li
//if (p->work_com[c_step].s[i].w_answ[0]==2) //esli nado gdat' otveta
//if (p->work_com[c_step].s[i].w_answ[1]=1)
} //step>0
}//timer
}//while
timer_delete(tm10);
}
//===============================================================================
// MAIN MAIN MAIN MAIN MAIN
main(int argc, char *argv[])
{
int n,i,j,col,command,i1;
packusoi p1;
packcmd p2;
short c_step=0,T0=0;
long cpp_timer=0;
int data_read; //4tenie dannih
//===============================================================================
// timer 10ms
event_sig.sigev_signo = SIGALRM;
tm10 = timer_create(CLOCK_REALTIME,&event_sig ); // ёючфрэшх ЄрщьхЁр
if (tm10==-1) printf("\ntimer opening error: %s",strerror(errno));
timer_sig.it_value.tv_sec = 0L;
timer_sig.it_value.tv_nsec = 0L;
timer_sig.it_interval.tv_sec = 0L;
timer_sig.it_interval.tv_nsec = 0L;
signal ( SIGALRM, SigHandler );
//create_shmem();
delay(2500);
open_shmem();
delay(100);
if (argc<3)
{
if (p->cvs==10) { Host="192.168.0.1"; port="4004"; }//4004
else { Host="CPP2"; port="4003"; }//4003
//else { Host="CPP0_1"; port="4004"; }//4003
//printf("\nhost and port number dont entering\n");
}
else
{
memcpy(Host,argv[1],5);
memcpy(port,argv[2],5);
}
printf("START MO3A<->CPP Host=%s port=%s cvs=%d\n\n",Host,port,p->cvs);
//---------------------------------------------------------------------
while(1)
{
if (cpp_timer!=p->sys_timer) //timer
{
cpp_timer=p->sys_timer;
if (p->cur_step!=0) //est' wag dl9 vipolnenni9
{
c_step=p->cur_step;
for (i=0;i<p->work_com[c_step].num_mini_com;i++) //prosmotrim vse minicomandi na wage
if((p->work_com[c_step].s[i].n_chan==N_CHAN)&&(p->work_com[c_step].s[i].status==0)) //na tekuwem wage (i - minikomanda) est' komanda dl9 nas
{
if (p->verbose) printf("\nSTEP=%d minicom for CPP : %d status=%d time %d \n", p->cur_step, p->work_com[c_step].s[i].n_com, p->work_com[c_step].s[i].status, p->sys_timer);
memset((char *)&f11, 0, sizeof(struct to_cpp11));
f11.zag.marker1=0xFFFF;
f11.zag.marker2=0xFFFF;
if (p->cvs==11) f11.zag.II=2; else f11.zag.II=3;
f11.zag.TS=3;
f11.zag.PS=1;
//f11.zag.reserv=sizeof(struct form11)/2;
f11.zag.KSS=sizeof(struct form11)/2;
f11.data.nf=11;
//if(p->verbose) printf(" KSS=%d %x\n",f11.zag.KSS,f11.zag.KSS);
col = sizeof(f11);
switch(p->work_com[c_step].s[i].n_com)
{
case 1: p->work_com[c_step].s[i].status=1;
if(p->verbose) printf(" SVCH work \n");
f11.data.KU0=0; //rezim raboti 0 - rabota, 1 - FK, 2 - SR
f11.data.ustKU0=1; // 1 - ustanovit' , 0 - ne ustanavlivat'
//col = sizeof(f11);
col=tcp_send_read(col);
if (col==0x14) //esli otet=sosto9nie
{
//if (f12->data.SS0_all)
p->work_com[c_step].s[i].status=2; // ispravnost'
}
else p->work_com[c_step].s[i].status=3;
//printf("col=%d status=%d\n",col/2,p->work_com[c_step].s[i].status);
break;
case 5: p->work_com[c_step].s[i].status=1;
if(p->verbose) printf(" SVCH PRD-PRM CHAN \n");
f11.data.KU6=f11.data.KU5=work_point[p->inbufMN3.a_params[0]-1]; //// RT PRD 1 - 6
f11.data.ustKU5=1; // 1 - ustanovit' , 0 - ne ustanavlivat'
//f11.data.KU6=p->inbufMN3.a_params[0]+6; //// RT PRM 7 - 13
f11.data.ustKU6=1; // 1 - ustanovit' , 0 - ne ustanavlivat'
col=tcp_send_read(col);
if (col==0x14) //esli otet=sosto9nie
{
if(p->verbose>1) printf("SS4=%d SS5=%d \n",f12->data.SS4,f12->data.SS5);
p->work_com[c_step].s[i].status=2; // ispravnost'
}
else p->work_com[c_step].s[i].status=3;
break;
case 8: p->work_com[c_step].s[i].status=1;
if(p->verbose) printf(" FM SHPS\n");
f11.data.KU4=p->inbufMN3.a_params[0];; // 0 - FM1, 1 - FM2
f11.data.ustKU4=1; // 1 - ustanovit' , 0 - ne ustanavlivat'
col=tcp_send_read(col);
if (col==0x14) //esli otet=sosto9nie
{
//if (f12->data.SS0_all)
p->work_com[c_step].s[i].status=2; // ispravnost'
}
else p->work_com[c_step].s[i].status=3;
if(p->verbose) printf("col=%d status=%d\n",col/2,p->work_com[c_step].s[i].status);
break;
case 12: p->work_com[c_step].s[i].status=1;
if(p->verbose) printf(" SVCH TKI-RLI \n");
f11.data.KU3=p->inbufMN3.a_params[0]; // 1 - TKI, 0 - RLI
f11.data.ustKU3=1; // 1 - ustanovit' , 0 - ne ustanavlivat'
col=tcp_send_read(col);
if (col==0x14) //esli otet=sosto9nie
{
if(p->verbose>1) printf("SS2=%d\n",f12->data.SS2_1);
p->work_com[c_step].s[i].status=2; // ispravnost'
}
else p->work_com[c_step].s[i].status=3;
break;
case 14: p->work_com[c_step].s[i].status=1;
if(p->verbose) printf(" PRIEM ONN\n");
f11.data.KU2=p->inbufMN3.a_params[0];
f11.data.ustKU2=1; // 1 - ustanovit' , 0 - ne ustanavlivat'
f11.data.KU1=0;
f11.data.ustKU1=1; // 1 - ustanovit' , 0 - ne ustanavlivat'
col=tcp_send_read(col);
if (col==0x14) //esli otet=sosto9nie
{
if(p->verbose>1) printf("SS2=%d\n",f12->data.SS2_0);
p->work_com[c_step].s[i].status=2; // ispravnost'
}
else p->work_com[c_step].s[i].status=3;
break;
case 15: p->work_com[c_step].s[i].status=1;
if(p->verbose) printf(" PEREDA4A ONN\n");
f11.data.KU1=p->inbufMN3.a_params[0];
f11.data.ustKU1=1; // 1 - ustanovit' , 0 - ne ustanavlivat'
f11.data.KU2=0;
f11.data.ustKU2=1; // 1 - ustanovit' , 0 - ne ustanavlivat'
col=tcp_send_read(col);
if (col==0x14) //esli otet=sosto9nie
{
if(p->verbose>1) printf("SS2=%d\n",f12->data.SS2_0);
p->work_com[c_step].s[i].status=2; // ispravnost'
}
else p->work_com[c_step].s[i].status=3;
break;
case 30: p->work_com[c_step].s[i].status=1;
if(p->verbose) printf(" SVCH ATT \n");
f11.data.KU7=p->inbufMN3.a_params[0]; // oslablenie 0 - 25
f11.data.ustKU7=1; // 1 - ustanovit' , 0 - ne ustanavlivat'
col=tcp_send_read(col);
if (col==0x14) //esli otet=sosto9nie
{
if(p->verbose>1) printf("SS6=%d\n",f12->data.SS6);
p->work_com[c_step].s[i].status=2; // ispravnost'
}
else p->work_com[c_step].s[i].status=3;
break;
case 32: case 42: p->work_com[c_step].s[i].status=1;
if(p->verbose) printf(" porog MI \n");
f11.data.KU10=p->inbufMN3.a_params[0]; // porog MI 1 - 15
f11.data.ustKU10=1; // 1 - ustanovit' , 0 - ne ustanavlivat'
col=tcp_send_read(col);
if (col==0x14) //esli otet=sosto9nie
{
if(p->verbose>1) printf("SS6=%d\n",f12->data.SS6);
p->work_com[c_step].s[i].status=2; // ispravnost'
}
else p->work_com[c_step].s[i].status=3;
break;
case 33: case 43: p->work_com[c_step].s[i].status=1;
if(p->verbose) printf(" porog SS \n");
f11.data.KU11=p->inbufMN3.a_params[0]; // porog MI 1 - 15
f11.data.ustKU11=1; // 1 - ustanovit' , 0 - ne ustanavlivat'
col=tcp_send_read(col);
if (col==0x14) //esli otet=sosto9nie
{
if(p->verbose>1) printf("SS6=%d\n",f12->data.SS6);
p->work_com[c_step].s[i].status=2; // ispravnost'
}
else p->work_com[c_step].s[i].status=3;
break;
case 61: p->work_com[c_step].s[i].status=1;
if(p->verbose) printf(" SVCH status \n");
f11.zag.KSS=0;
col = sizeof(struct zag_CPP);
col=tcp_send_read(col);
if (col==0x14) //esli otet=sosto9nie
{
//if (f12->data.SS0_all)
p->work_com[c_step].s[i].status=2; // ispravnost'
}
else p->work_com[c_step].s[i].status=3;
//printf("col=%d status=%d\n",col/2,p->work_com[c_step].s[i].status);
break;
case 65 : case 75: p->work_com[c_step].s[i].status=1;
if(p->verbose) printf(" FK %d \n",p->inbufMN3.a_params[0]);
f11.data.KU0=1; //rezim raboti 0 - rabota, 1 - FK, 2 - SR
f11.data.ustKU0=1; // 1 - ustanovit' , 0 - ne ustanavlivat'
f11.data.KU8=p->inbufMN3.a_params[0]; //FK 1 - 12
f11.data.ustKU8=1; // 1 - ustanovit' , 0 - ne ustanavlivat'
col=tcp_send_read(col);
if (col==0x14) //esli otet=sosto9nie
{
if(p->verbose>1)
{
if(p->verbose>1) printf("SS7=%d ",f12->data.SS7);
if(p->verbose) {if (f12->data.SS1==0) printf("WK \n"); else printf("FK\n");}
}
p->work_com[c_step].s[i].status=2; // ispravnost'
}
else p->work_com[c_step].s[i].status=3;
break;
case 103: if (p->work_com[c_step].s[i].status==0) //na4alo vipolneni9
{
printf("Send to 1 chan\n");
//p->work_com[c_step].s[i].t_start=p->sys_timer; // vrem9 starta
//p->work_com[c_step].s[i].t_stop =p->sys_timer+300; //stop ne pozdnee
T0=0; //ots4et vremeni (vremenno)
p->work_com[c_step].s[i].status=1;
}
if (p->work_com[c_step].s[i].status==1) //zdem otveta
{
if (data_read==1) //bilo 4tenie
{
p->work_com[c_step].s[i].status=2;
data_read=0;
}
}
break;
case 101: if (p->work_com[c_step].s[i].status==0) //na4alo vipolneni9
{
if(p->verbose) printf("Check CPP link (TC=0)\n");
//tcp_send_read();
p->work_com[c_step].s[i].status=2;
}
break;
default:
if(p->verbose) printf("Bad minicom %d for %d chan : %d",p->work_com[c_step].s[i].n_com,N_CHAN);
p->work_com[c_step].s[i].status=3;
}//switch (n_com)
//-------------------------------------------------------
//esli previweno vrem9 ozhidani9
//if ((p->work_com[c_step].s[i].status==1)&&(p->work_com[c_step].s[i].t_stop>p->sys_timer)) p->work_com[c_step].s[i].status=3;
//-------------------------------------------------------
}//ewe ne vipoln9li
//if (p->work_com[c_step].s[i].w_answ[0]==2) //esli nado gdat' otveta
//if (p->work_com[c_step].s[i].w_answ[1]=1)
//-------VREMENNO PRIEM DANNIH-------------------------------
T0++;
if (T0==30)
{
T0=0;
data_read=1;
}
//-----------------------------------------------------------
} //step>0
}//timer
}//while
timer_delete(tm10);
}
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];
}
}
int main( int argc, char *argv[] )
{
// unsigned long in_buf;
// char *data_buff;
// int ret;
int c;
optparams_t oparams;
int fd;
int oflag;
printf( "read_ft245 test program is started\n" );
oparams.dev_name = strdup( "//9/dev/ft245" ); //!!!!
oparams.fname = NULL;
oparams.nonblock = 0;
oparams.devinfo = 0;
oparams.rx_timeout = 0;
oparams.psize = 8;
oparams.npackets = 30;
oparams.verbose = 0;
oparams.reset = 0;
oparams.latency = 0;
while ( ( c = getopt( argc, argv, "d:f:nivrs:p:t:l:" ) ) != -1 ) {
switch( c ) {
case 'd':
oparams.dev_name = optarg;
break;
case 'f':
oparams.fname = optarg;
break;
case 'n':
oparams.nonblock = 1;
break;
case 'i':
oparams.devinfo = 1;
break;
case 'v':
oparams.verbose++;
break;
case 's':
oparams.psize = strtoul( optarg, NULL, 0 );
break;
case 'p':
oparams.npackets = strtoul( optarg, NULL, 0 );
break;
case 't':
TM = 1;
break;
case 'r':
oparams.reset = 1;
break;
case 'l':
oparams.latency = strtoul( optarg, NULL, 0 );
if ( oparams.latency == 0 ) {
fprintf( stderr, "Command line: Bad latency value. Correct range is 1 - 255\n" );
oparams.latency = 0;
}
break;
default:
fprintf( stderr, "Unsupported comand line option\n" );
break;
}
}
open_shmem();
// open device in block or nonblock mode
oflag = O_RDONLY | O_NONBLOCK;
if ( ( fd = open( oparams.dev_name, oflag ) ) == -1 ) {
fprintf( stderr, "open() failed. %s\n", strerror( errno ) );
return ( -1 );
}
//if ( oparams.reset ) reset_device( fd );
if ( oparams.devinfo )
get_dev_info( fd );
// timeout test
if ( oparams.rx_timeout )
set_rx_timeout( fd, oparams.rx_timeout );
if ( oparams.latency )
set_latency( fd, oparams.latency );
printf( "Device latency time is %d\n", get_latency( fd ) );
//if ( oparams.reset )
//reset_device( fd );
printf( "Start data read. \n");
read_data_nonblock1( fd, oparams.psize, oparams.npackets, oparams.fname );
printf( "read_ft245 test program is finished %d\n",sum_ret );
close( fd );
return( 0 );
}
universal_notifier_shmem_poller_t() : last_change_time(0), last_seed(0), region(NULL)
{
open_shmem();
}
