/*****************************************************************************
* clock_NewRef
*****************************************************************************/
void clock_NewRef( uint64_t i_clock, uint64_t i_wall )
{
uint64_t i_extrapoled_clock;
int64_t i_clock_diff = i_clock - input_clock.last_cr;
if ( i_clock_diff > (int64_t)i_cr_max_gap ||
i_clock_diff < -(int64_t)i_cr_max_gap )
{
msg_Warn( NULL, "clock gap, unexpected stream discontinuity %lld",
i_clock_diff );
clock_Init();
input_clock.cr_ref = input_clock.last_cr = i_clock;
input_clock.wall_ref = i_wall;
return;
}
input_clock.last_cr = i_clock;
/* Smooth clock reference variations. */
i_extrapoled_clock = input_clock.cr_ref
+ i_wall - input_clock.wall_ref;
i_clock_diff = i_extrapoled_clock - i_clock;
int64_t i_jitter = i_clock_diff - input_clock.delta_cr;
if ( i_jitter > (int64_t)i_cr_max_jitter ||
i_jitter < -(int64_t)i_cr_max_jitter )
{
/* The packet must come from outer space. */
input_clock.i_nb_space_packets++;
if ( input_clock.i_nb_space_packets > CR_MAX_SPACE_PACKETS )
{
msg_Warn( NULL, "too much jitter %lld", i_jitter );
clock_Init();
input_clock.cr_ref = input_clock.last_cr = i_clock;
input_clock.wall_ref = i_wall;
}
else
msg_Dbg( NULL, "ignoring space packet jitter %lld", i_jitter );
return;
}
input_clock.i_nb_space_packets = 0;
/* Bresenham algorithm to smooth variations. */
/* Gives a lot of importance to the first samples, but we suppose the
* buffer is *large*, and the most important is to avoid the delta_cr
* to change too quickly, otherwise packets will be in wrong order. */
input_clock.delta_cr = (input_clock.delta_cr * (i_cr_average - 1)
+ i_clock_diff) / i_cr_average;
}
int
main(int argc, char **argv)
{
struct clock clock_old = {0, 0};
struct clock first;
clock_Init ();
clock_NewTime ();
clock_GetTime (&first);
for (;;) {
struct clock now;
clock_NewTime ();
clock_GetTime (&now);
if (now.sec < clock_old.sec ||
(now.sec == clock_old.sec && now.usec < clock_old.usec))
abort ();
/* printf ("%6ld.%6ld\r", now.sec, now.usec); */
if (now.sec > first.sec + 10)
break;
}
return 0;
}
void
rxevent_Init(int nEvents, void (*scheduler) ())
{
if (initialized)
return;
clock_Init();
if (nEvents)
rxevent_allocUnit = nEvents;
queue_Init(&rxevent_free);
queue_Init(&rxevent_queue);
rxevent_nFree = rxevent_nPosted = 0;
rxevent_ScheduledEarlierEvent = scheduler;
initialized = 1;
}
void weifu_Init(void)
{
clock_Init();
axle_Init(0);
op_Init(0);
vss_Init();
wss_Init();
pwmin1_Init();
counter2_Init();
eng_speed_timer = time_get(1000);
wtout_timer = time_get(1000);
mcamos_init_ex(&lcm);
cfg_data.eng_speed = 0;
cfg_data.wtout = 0;
phase_diff = 2;
eng_factor = 10;
op_factor = 10;
//vss = 0;
vss = 0;
tim_dc = 0;
tim_frq = 0;
hfmsig = 0;
hfmref = 0;
/* for(index_result = 0;index_result < 120;index_result ++ )
{
result_axle[index_result] = 0;
result_op_37[index_result] = 0;
result_op_120[index_result] = 0;
result_axle[index_result] = IS_IN_RANGE(index_result, 58, 59) | IS_IN_RANGE(index_result, 118, 119);
result_op_37[index_result] = IS_IN_RANGE(index_result, 36, 36);
result_op_120[index_result] = IS_IN_RANGE(index_result, 28, 29) | IS_IN_RANGE(index_result, 58, 59) \
| IS_IN_RANGE(index_result, 88, 89) | IS_IN_RANGE(index_result, 118, 119);
}*/
for(index_result = 0;index_result < 3840;index_result ++ )
{
gear32_120[index_result] = gear32_1[index_result%32];
op32_120[index_result] = gear32_1[index_result%32];
if(IS_IN_RANGE(index_result, axle_min1, axle_max1) | IS_IN_RANGE(index_result, axle_min2, axle_max2))
{
gear32_120[index_result] = gear32_1[0];
}
if(IS_IN_RANGE(index_result, op_min1, op_max1) | IS_IN_RANGE(index_result, op_min2, op_max2) | IS_IN_RANGE(index_result, op_min3, op_max3) | IS_IN_RANGE(index_result, op_min4, op_max4))
{
op32_120[index_result] = gear32_1[0];
}
}
simulator_Start();
}
int
main(int argc, char **argv)
{
struct clock clock_old = {0, 0};
clock_Init ();
for (;;) {
struct clock now;
clock_NewTime ();
clock_GetTime (&now);
if (now.sec < clock_old.sec ||
(now.sec == clock_old.sec && now.usec < clock_old.usec))
abort ();
printf ("%6ld.%6ld\r", now.sec, now.usec);
}
return 0;
}
/*****************************************************************************
* Entry point
*****************************************************************************/
int main( int i_argc, char **pp_argv )
{
int i, c;
int i_priority = -1;
int i_ttl = 0;
struct pollfd *pfd = NULL;
int i_fd;
bool b_tcp;
#define ADD_INPUT \
p_inputs = realloc( p_inputs, ++i_nb_inputs * sizeof(input_t) ); \
p_inputs[i_nb_inputs - 1].i_fd = i_fd; \
p_inputs[i_nb_inputs - 1].b_tcp = b_tcp; \
p_inputs[i_nb_inputs - 1].p_block = NULL; \
pfd = realloc( pfd, i_nb_inputs * sizeof(struct pollfd) ); \
pfd[i_nb_inputs - 1].fd = i_fd; \
pfd[i_nb_inputs - 1].events = POLLIN | POLLERR | POLLRDHUP | POLLHUP;
while ( (c = getopt( i_argc, pp_argv, "i:t:b:g:j:r:n:x:X:Um:R:h" )) != -1 )
{
switch ( c )
{
case 'i':
i_priority = strtol( optarg, NULL, 0 );
break;
case 't':
i_ttl = strtol( optarg, NULL, 0 );
break;
case 'b':
i_buffer_length = strtoll( optarg, NULL, 0 ) * 27000;
break;
case 'g':
i_cr_max_gap = strtoll( optarg, NULL, 0 ) * 27000;
break;
case 'j':
i_cr_max_jitter = strtoll( optarg, NULL, 0 ) * 27000;
break;
case 'r':
i_cr_average = strtol( optarg, NULL, 0 );
break;
case 'n':
i_max_retx_burst = strtoul( optarg, NULL, 0 );
break;
case 'x':
i_retx_delay = strtoll( optarg, NULL, 0 ) * 27000;
break;
case 'X':
i_retx_fd = i_fd = OpenSocket( optarg, 0, 0, 0, NULL, &b_tcp, NULL );
if ( i_fd == -1 )
{
msg_Err( NULL, "unable to set up retx with %s\n", optarg );
exit(EXIT_FAILURE);
}
ADD_INPUT
break;
case 'U':
b_udp = 1;
break;
case 'm':
i_asked_payload_size = strtol( optarg, NULL, 0 );
break;
case 'R':
i_rtp_header_size = strtol( optarg, NULL, 0 );
break;
case 'h':
default:
usage();
break;
}
}
if ( optind >= i_argc - 1 )
usage();
while ( optind < i_argc - 1 )
{
i_fd = OpenSocket( pp_argv[optind], 0, DEFAULT_PORT, 0, NULL,
&b_tcp, NULL );
if ( i_fd == -1 )
{
msg_Err( NULL, "unable to open input %s\n", pp_argv[optind] );
exit(EXIT_FAILURE);
}
optind++;
ADD_INPUT
}
msg_Dbg( NULL, "%d inputs", i_nb_inputs );
i_nb_retx = (i_buffer_length - MIN_RETX_DELAY) / i_retx_delay;
pp_retx = malloc( i_nb_retx * sizeof(block_t *) );
for ( i = 0; i < i_nb_retx; i++ )
pp_retx[i] = NULL;
if ( i_retx_fd && i_nb_retx )
msg_Dbg( NULL, "%d retx passes", i_nb_retx );
i_output_fd = OpenSocket( pp_argv[optind], i_ttl, 0, DEFAULT_PORT, NULL,
NULL, NULL );
if ( i_output_fd == -1 )
{
msg_Err( NULL, "unable to open output %s\n", pp_argv[optind] );
exit(EXIT_FAILURE);
}
clock_Init();
if ( i_priority > 0 )
{
struct sched_param param;
int i_error;
memset( ¶m, 0, sizeof(struct sched_param) );
param.sched_priority = i_priority;
if ( (i_error = pthread_setschedparam( pthread_self(), SCHED_RR,
¶m )) )
{
msg_Warn( NULL, "couldn't set thread priority: %s",
strerror(i_error) );
}
}
for ( ; ; )
{
int i_timeout = -1;
uint64_t i_current_date;
while ( p_first != NULL
&& p_first->i_date <= (i_current_date = wall_Date()) + 26999 )
PacketSend();
i_current_date = wall_Date();
RetxCheck( i_current_date );
i_current_date = wall_Date();
if ( p_first != NULL )
i_timeout = (p_first->i_date - i_current_date) / 27000;
if ( poll( pfd, i_nb_inputs, i_timeout ) < 0 )
{
int saved_errno = errno;
msg_Warn( NULL, "couldn't poll(): %s", strerror(errno) );
if ( saved_errno == EINTR ) continue;
exit(EXIT_FAILURE);
}
i_current_date = wall_Date();
for ( i = 0; i < i_nb_inputs; i++ )
{
input_t *p_input = &p_inputs[i];
if ( pfd[i].revents & POLLIN )
{
ssize_t i_size = i_asked_payload_size + i_rtp_header_size;
uint8_t *p_buffer;
if ( p_input->p_block == NULL )
{
p_input->p_block = malloc( sizeof(block_t) + i_size );
p_buffer = p_input->p_block->p_data =
(uint8_t *)p_input->p_block + sizeof(block_t);
p_input->p_block->i_size = 0;
}
else
{
p_buffer = p_input->p_block->p_data +
p_input->p_block->i_size;
i_size -= p_input->p_block->i_size;
}
i_size = read( p_input->i_fd, p_buffer, i_size );
if ( i_size < 0 && errno != EAGAIN && errno != EINTR &&
errno != ECONNREFUSED )
{
msg_Err( NULL, "unrecoverable read error, dying (%s)",
strerror(errno) );
exit(EXIT_FAILURE);
}
if ( i_size <= 0 ) continue;
p_input->p_block->i_size += i_size;
if ( p_input->b_tcp &&
p_input->p_block->i_size !=
i_asked_payload_size + i_rtp_header_size )
continue;
if ( i_retx_fd == -1 || i )
PacketRecv( p_input->p_block, i_current_date );
else
PacketRecv( p_input->p_block, 0 );
p_input->p_block = NULL;
}
else if ( pfd[i].revents & (POLLERR | POLLRDHUP | POLLHUP) )
{
msg_Err( NULL, "poll error on input %d" );
exit(EXIT_FAILURE);
}
}
}
return EXIT_SUCCESS;
}
