int main()
{
int code=5,i=0;
double duration,duration1,pos_arr[3][2], tmp1=0, tmp2=0, tmp3=0;
struct timeval start,end,start1;
RCSwitch mySwitch = RCSwitch();
mySwitch.enableTransmit(PIN);
receiver receiver1(1),receiver2(2),receiver3(3);
receiver1.init();
receiver2.init();
receiver3.init();
ofstream data;
data.open ("data.txt");
printf("activating emitter\n");
mySwitch.send(code,24);
// delay(3);
while (true)
{
gettimeofday(&start1,NULL);
std::thread t1(&receiver::dst,&receiver1);
std::thread t2(&receiver::dst,&receiver2);
std::thread t3(&receiver::dst,&receiver3);
t1.join();
t2.join();
t3.join();
// std::cout<<receiver1.distance<<" "<<receiver2.distance<<" "<<receiver3.distance<<" "<<i<<std::endl;
pos_arr[0][0]=tmp1;
pos_arr[1][0]=tmp2;
pos_arr[2][0]=tmp3;
pos_arr[0][1]=receiver1.distance;
pos_arr[1][1]=receiver2.distance;
pos_arr[2][1]=receiver3.distance;
tmp1= pos_arr[0][1];
tmp2= pos_arr[1][1];
tmp3= pos_arr[2][1];
gettimeofday(&start,NULL);
kalman(pos_arr);
gettimeofday(&end,NULL);
duration1=(abs(end.tv_usec-start1.tv_usec)/1000.0);
duration=(abs(end.tv_usec-start.tv_usec)/1000.0);
//std::cout<<"receiver1"<<receiver1.distance <<"receiver2"<<receiver2.distance<< "receiver3"<<receiver3.distance << std::endl;
std::cout<<"time for Kalman / calculations "<<duration<< " " << duration1<< std::endl;
delay(50-duration1);
// gettimeofday(&end1,NULL);
// duration1=((dend1.tv_usec-start1.tv_usec)*1000+(end1.tv_sec-start1.tv_sec)/1000)+0.5;
// std::cout<<"time for Kalman "<<duration<<std::endl;
// delay(50-duration1/1000000);
i++;
}
data.close();
return 0;
}
int main()
{
int code=5,i=0;
timespec tsleep{0};
long long duration2=0,duration1=0;
RCSwitch mySwitch = RCSwitch();
mySwitch.enableTransmit(PIN);
receiver receiver1(1),receiver2(2),receiver3(3);
receiver1.init();
receiver2.init();
receiver3.init();
printf("activating emitter\n");
mySwitch.send(code,24);
std::ofstream data;
data.open ("data.txt");
// delay(3);
while (true)
{
auto begin = std::chrono::high_resolution_clock::now();
////////////////////////////////////////////////////////////////////////////////////
// mySwitch.send(code,24);
receiver1.dst();
// receiver2.dst();
// receiver3.dst();
data<<receiver1.distance<<" "<<receiver2.distance<<" "<<receiver3.distance<<std::endl;
////////////////////////////////////////////////////////////////////////////////
auto end = std::chrono::high_resolution_clock::now();
// data<<end.tv_usec<<"\n";
duration2 = std::chrono::duration_cast <std::chrono::nanoseconds>(end-begin).count();
tsleep.tv_nsec=50000000-duration2;
nanosleep(&tsleep,NULL);
// i++;
// if (i==20)
// {
// mySwitch.send(code,24);
// i=0;
// }
// auto end2 = std::chrono::high_resolution_clock::now();
// duration1 = std::chrono::duration_cast <std::chrono::nanoseconds>(end2-begin).count();
// data<<duration1<<"\n";
}
return 0;
}
int main() {
int val;
int rc;
/* Receiver waits for sender. */
int ch1[2];
rc = chmake(ch1);
errno_assert(rc == 0);
assert(ch1[0] >= 0);
assert(ch1[1] >= 0);
int hndl1 = go(sender(ch1[0], 1, 333));
errno_assert(hndl1 >= 0);
rc = chrecv(ch1[1], &val, sizeof(val), -1);
errno_assert(rc == 0);
assert(val == 333);
rc = hclose(ch1[1]);
errno_assert(rc == 0);
rc = hclose(ch1[0]);
errno_assert(rc == 0);
rc = hclose(hndl1);
errno_assert(rc == 0);
/* Sender waits for receiver. */
int ch2[2];
rc = chmake(ch2);
errno_assert(rc == 0);
int hndl2 = go(sender(ch2[0], 0, 444));
errno_assert(hndl2 >= 0);
rc = chrecv(ch2[1], &val, sizeof(val), -1);
errno_assert(rc == 0);
assert(val == 444);
rc = hclose(ch2[1]);
errno_assert(rc == 0);
rc = hclose(ch2[0]);
errno_assert(rc == 0);
rc = hclose(hndl2);
errno_assert(rc == 0);
/* Test two simultaneous senders. */
int ch3[2];
rc = chmake(ch3);
errno_assert(rc == 0);
int hndl3[2];
hndl3[0] = go(sender(ch3[0], 0, 888));
errno_assert(hndl3[0] >= 0);
hndl3[1] = go(sender(ch3[0], 0, 999));
errno_assert(hndl3[1] >= 0);
rc = chrecv(ch3[1], &val, sizeof(val), -1);
errno_assert(rc == 0);
assert(val == 888);
rc = yield();
errno_assert(rc == 0);
rc = chrecv(ch3[1], &val, sizeof(val), -1);
errno_assert(rc == 0);
assert(val == 999);
rc = hclose(ch3[1]);
errno_assert(rc == 0);
rc = hclose(ch3[0]);
errno_assert(rc == 0);
rc = hclose(hndl3[0]);
errno_assert(rc == 0);
rc = hclose(hndl3[1]);
errno_assert(rc == 0);
/* Test two simultaneous receivers. */
int ch4[2];
rc = chmake(ch4);
errno_assert(rc == 0);
int hndl4[2];
hndl4[0] = go(receiver(ch4[0], 333));
errno_assert(hndl4[0] >= 0);
hndl4[1] = go(receiver(ch4[0], 444));
errno_assert(hndl4[1] >= 0);
val = 333;
rc = chsend(ch4[1], &val, sizeof(val), -1);
errno_assert(rc == 0);
val = 444;
rc = chsend(ch4[1], &val, sizeof(val), -1);
errno_assert(rc == 0);
rc = hclose(ch4[1]);
errno_assert(rc == 0);
rc = hclose(ch4[0]);
errno_assert(rc == 0);
rc = hclose(hndl4[0]);
errno_assert(rc == 0);
rc = hclose(hndl4[1]);
errno_assert(rc == 0);
/* Test simple chdone() scenario. */
int ch8[2];
rc = chmake(ch8);
errno_assert(rc == 0);
rc = chdone(ch8[0]);
errno_assert(rc == 0);
rc = chrecv(ch8[1], &val, sizeof(val), -1);
errno_assert(rc == -1 && errno == EPIPE);
rc = chrecv(ch8[1], &val, sizeof(val), -1);
errno_assert(rc == -1 && errno == EPIPE);
rc = chrecv(ch8[1], &val, sizeof(val), -1);
errno_assert(rc == -1 && errno == EPIPE);
rc = hclose(ch8[1]);
errno_assert(rc == 0);
rc = hclose(ch8[0]);
errno_assert(rc == 0);
/* Test whether chdone() unblocks all receivers. */
int ch12[2];
rc = chmake(ch12);
errno_assert(rc == 0);
int hndl6[2];
hndl6[0] = go(receiver2(ch12[0]));
errno_assert(hndl6[0] >= 0);
hndl6[1] = go(receiver2(ch12[0]));
errno_assert(hndl6[1] >= 0);
rc = chdone(ch12[1]);
errno_assert(rc == 0);
rc = chrecv(ch12[1], &val, sizeof(val), -1);
errno_assert(rc == 0);
assert(val == 0);
rc = chrecv(ch12[1], &val, sizeof(val), -1);
errno_assert(rc == 0);
assert(val == 0);
rc = hclose(ch12[1]);
errno_assert(rc == 0);
rc = hclose(ch12[0]);
errno_assert(rc == 0);
rc = hclose(hndl6[0]);
errno_assert(rc == 0);
rc = hclose(hndl6[1]);
errno_assert(rc == 0);
/* Test whether chdone() unblocks blocked senders. */
int ch15[2];
rc = chmake(ch15);
errno_assert(rc == 0);
int hndl8[3];
hndl8[0] = go(sender2(ch15[0]));
errno_assert(hndl8[0] >= 0);
hndl8[1] = go(sender2(ch15[0]));
errno_assert(hndl8[1] >= 0);
hndl8[2] = go(sender2(ch15[0]));
errno_assert(hndl8[2] >= 0);
rc = msleep(now() + 50);
errno_assert(rc == 0);
rc = chdone(ch15[1]);
errno_assert(rc == 0);
rc = hclose(ch15[1]);
errno_assert(rc == 0);
rc = hclose(ch15[0]);
errno_assert(rc == 0);
rc = hclose(hndl8[0]);
errno_assert(rc == 0);
rc = hclose(hndl8[1]);
errno_assert(rc == 0);
rc = hclose(hndl8[2]);
errno_assert(rc == 0);
/* Test whether hclose() unblocks blocked senders and receivers. */
int ch16[2];
rc = chmake(ch16);
errno_assert(rc == 0);
int hndl9[2];
hndl9[0] = go(receiver3(ch16[0]));
errno_assert(hndl9[0] >= 0);
hndl9[1] = go(receiver3(ch16[0]));
errno_assert(hndl9[1] >= 0);
rc = msleep(now() + 50);
errno_assert(rc == 0);
rc = hclose(ch16[1]);
errno_assert(rc == 0);
rc = hclose(ch16[0]);
errno_assert(rc == 0);
rc = hclose(hndl9[0]);
errno_assert(rc == 0);
rc = hclose(hndl9[1]);
errno_assert(rc == 0);
/* Test cancelation. */
int ch17[2];
rc = chmake(ch17);
errno_assert(rc == 0);
int hndl10 = go(cancel(ch17[0]));
errno_assert(hndl10 >= 0);
rc = hclose(hndl10);
errno_assert(rc == 0);
rc = hclose(ch17[1]);
errno_assert(rc == 0);
rc = hclose(ch17[0]);
errno_assert(rc == 0);
/* Receiver waits for sender (zero-byte message). */
int ch18[2];
rc = chmake(ch18);
errno_assert(rc == 0);
int hndl11 = go(sender3(ch18[0], 1));
errno_assert(hndl11 >= 0);
rc = chrecv(ch18[1], NULL, 0, -1);
errno_assert(rc == 0);
rc = hclose(ch18[1]);
errno_assert(rc == 0);
rc = hclose(ch18[0]);
errno_assert(rc == 0);
rc = hclose(hndl11);
errno_assert(rc == 0);
/* Sender waits for receiver (zero-byte message). */
int ch19[2];
rc = chmake(ch19);
errno_assert(rc == 0);
int hndl12 = go(sender3(ch19[0], 0));
errno_assert(hndl12 >= 0);
rc = chrecv(ch19[1], NULL, 0, -1);
errno_assert(rc == 0);
rc = hclose(ch19[1]);
errno_assert(rc == 0);
rc = hclose(ch19[0]);
errno_assert(rc == 0);
rc = hclose(hndl12);
errno_assert(rc == 0);
/* Channel with user-supplied storage. */
struct chstorage mem;
int ch20[2];
rc = chmake_mem(&mem, ch20);
errno_assert(rc == 0);
rc = chrecv(ch20[0], NULL, 0, now() + 50);
errno_assert(rc == -1 && errno == ETIMEDOUT);
rc = hclose(ch20[1]);
errno_assert(rc == 0);
rc = hclose(ch20[0]);
errno_assert(rc == 0);
return 0;
}
void fork_off_workers(void)
{
/* Fork off the two workers, M0 and M1. */
if (fork() != 0) {
/* This is the Parent. It will become main, but first fork off M1. */
if (fork() != 0) {
/* This is main. */
sigaction(SIGPIPE, &act, &oact);
setvbuf(stdout, (char *)0, _IONBF, (size_t)0);/*don't buffer*/
close(r1);
close(w1);
close(r2);
close(w2);
close(r3);
close(w4);
close(r5);
close(w6);
return;
} else {
/* This is the code for M1. Run protocol. */
sigaction(SIGPIPE, &act, &oact);
setvbuf(stdout, (char *)0, _IONBF, (size_t)0);/*don't buffer*/
close(w1);
close(r2);
close(r3);
close(w3);
close(r4);
close(w4);
close(w5);
close(r6);
id = 1; /* M1 gets id 1 */
mrfd = r5; /* fd for reading time from main */
mwfd = w6; /* fd for writing reply to main */
prfd = r1; /* fd for reading frames from worker 0 */
switch(protocol) {
case 2: receiver2(); break;
case 3: receiver3(); break;
case 4: protocol4(); break;
case 5: protocol5(); break;
case 6: protocol6(); break;
}
terminate("Impossible. Protocol terminated");
}
} else {
/* This is the code for M0. Run protocol. */
sigaction(SIGPIPE, &act, &oact);
setvbuf(stdout, (char *)0, _IONBF, (size_t)0);/*don't buffer*/
close(r1);
close(w2);
close(w3);
close(r4);
close(r5);
close(w5);
close(r6);
id = 0; /* M0 gets id 0 */
mrfd = r3; /* fd for reading time from main */
mwfd = w4; /* fd for writing reply to main */
prfd = r2; /* fd for reading frames from worker 1 */
switch(protocol) {
case 2: sender2(); break;
case 3: sender3(); break;
case 4: protocol4(); break;
case 5: protocol5(); break;
case 6: protocol6(); break;
}
terminate("Impossible. protocol terminated");
}
}
int
sc_main(int argc, char *argv[])
{
sc_signal<pkt> pkt_in0;
sc_signal<pkt> pkt_in1;
sc_signal<pkt> pkt_in2;
sc_signal<pkt> pkt_in3;
sc_signal<pkt> pkt_out0;
sc_signal<pkt> pkt_out1;
sc_signal<pkt> pkt_out2;
sc_signal<pkt> pkt_out3;
sc_signal<sc_int<4> > id0, id1, id2, id3;
sc_signal<bool> switch_cntrl;
sc_clock clock1("CLOCK1", 75, SC_NS, 0.5, 0.0, SC_NS);
sc_clock clock2("CLOCK2", 30, SC_NS, 0.5, 10.0, SC_NS);
// Module instiatiations follow
// Note that modules can be connected by hooking up ports
// to signals by name or by using a positional notation
sender sender0("SENDER0");
// hooking up signals to ports by name
sender0.pkt_out(pkt_in0);
sender0.source_id(id0);
sender0.CLK(clock1);
sender sender1("SENDER1");
// hooking up signals to ports by position
sender1(pkt_in1, id1, clock1);
sender sender2("SENDER2");
// hooking up signals to ports by name
sender2.pkt_out(pkt_in2);
sender2.source_id(id2);
sender2.CLK(clock1);
sender sender3("SENDER3");
// hooking up signals to ports by position
sender3( pkt_in3, id3, clock1 );
switch_clk switch_clk1("SWITCH_CLK");
// hooking up signals to ports by name
switch_clk1.switch_cntrl(switch_cntrl);
switch_clk1.CLK(clock2);
mcast_pkt_switch switch1("SWITCH");
// hooking up signals to ports by name
switch1.switch_cntrl(switch_cntrl);
switch1.in0(pkt_in0);
switch1.in1(pkt_in1);
switch1.in2(pkt_in2);
switch1.in3(pkt_in3);
switch1.out0(pkt_out0);
switch1.out1(pkt_out1);
switch1.out2(pkt_out2);
switch1.out3(pkt_out3);
receiver receiver0("RECEIVER0");
// hooking up signals to ports by name
receiver0.pkt_in(pkt_out0);
receiver0.sink_id(id0);
receiver receiver1("RECEIVER1");
// hooking up signals to ports by position
receiver1( pkt_out1, id1 );
receiver receiver2("RECEIVER2");
// hooking up signals to ports by name
receiver2.pkt_in(pkt_out2);
receiver2.sink_id(id2);
receiver receiver3("RECEIVER3");
// hooking up signals to ports by position
receiver3( pkt_out3, id3 );
sc_start(0, SC_NS);
#if !defined(__SUNPRO_CC)
id0.write(0);
id1.write(1);
id2.write(2);
id3.write(3);
#else
// you cannot do that with SC5.0
// since it doesn't support member templates
id0.write(sc_int<4>(0));
id0.write(sc_int<4>(1));
id0.write(sc_int<4>(2));
id0.write(sc_int<4>(3));
#endif
sc_start();
return 0;
}
