//basically we want to delay unless there is a start command issued.
void cancellable_delay(unsigned int duration, byte state)
{
if (motor1_reversal_state)
{
for (unsigned int i=0; i<duration; i++)
{
delay(1);
//keep track of how far we go.
if (state == 1)
motor1_reversal_count++;
else if (state == 2)
motor1_reversal_count--;
//dont let it go below zero or above our forward duration.
motor1_reversal_count = max(0, motor1_reversal_count);
motor1_reversal_count = min(MOTOR_FORWARD_DURATION, motor1_reversal_count);
//check for packets.
process_packets();
//did we start up? break!
if (!motor1_reversal_state)
break;
}
}
}
int loop(void){
int i;
int status = 0;
struct emwin_server *es;
es = get_next_server();
if(es == NULL){
status = 1;
log_errx("No servers available.");
log_info("Waiting %d seconds.", g.retrysleep);
for(i = 0; i <= g.retrysleep; ++i){
sleep(1);
if(get_quit_flag() != 0)
break;
}
log_info("Trying server list again.");
}else if(es->fd == -1){
if(es->gai_code != 0){
log_errx("Cannot open connection to %s. %s", es->ip,
gai_strerror(es->gai_code));
}else{
log_err2("Cannot open connection to", es->ip);
}
status = 1;
}else if(es->fd == -2){
if(server_type_serial_device(es))
log_errx("Cannot configure or synchronize %s:%s", es->ip, es->port);
else
log_errx("Could not get packet from %s", es->ip);
status = 1;
}else{
log_info("Connected to %s @ %s", es->ip, es->port);
}
while((status == 0) && (get_quit_flag() == 0)){
if(g.f_server_enabled == 1)
server_loop();
status = process_packets(es);
periodic();
if(get_reload_servers_list_flag())
reload_servers_list();
if(get_send_bbclientid_flag())
bb_send_clientid(es);
}
if(get_quit_flag() != 0)
log_info("Closing processor.");
return(status);
}
void CALLBACK TimerProc(UINT uID,UINT uMsg,DWORD dwUser,DWORD dw1,DWORD dw2)
{
LONGLONG pc;
MSG msg;
QueryPerformanceCounter((_LARGE_INTEGER *)&pc);
//TIMING.acttime=pc;
check_keys();
if (GLOBAL.neurobit_available) NdProtocolEngine();
if ((!TIMING.pause_timer) && (!GLOBAL.loading))
{
// one second passed ? -> update PPS-info
if (pc-TIMING.timestamp >= TIMING.pcfreq)
{ TIMING.timestamp+=TIMING.pcfreq;
TIMING.actpps=(int) TIMING.ppscounter;
TIMING.ppscounter=0;
}
// Reading from Archive & next packet demanded? -> read from File and Process Packets
while ((pc-TIMING.readtimestamp >= TTY.packettime) || (GLOBAL.fly))
{
TIMING.readtimestamp+=TTY.packettime;
TIMING.acttime=TIMING.readtimestamp;
if(CAPTFILE.do_read&&(CAPTFILE.offset<=TIMING.packetcounter)&&(CAPTFILE.offset+CAPTFILE.length>TIMING.packetcounter))
{
long tmp;
tmp=TIMING.packetcounter;
read_captfile(TTY.amount_to_read);
ParseLocalInput(TTY.amount_to_read);
if ((TIMING.packetcounter-tmp-1)>0)
TIMING.readtimestamp+=TTY.packettime*(TIMING.packetcounter-tmp-1);
//return;
}
// process packets in case of no File-Read and no Com-Read
else if ((TTY.read_pause) && (!GLOBAL.neurobit_available)) process_packets();
if (GLOBAL.fly)
{
if(PeekMessage(&msg, NULL, 0, 0, PM_REMOVE))
{
TranslateMessage(&msg);
DispatchMessage(&msg);
}
}
}
}
}
int main(int argc, char **argv) {
ssize_t readen;
int have_data = 1;
int ntimeouts = 0;
time_t timeout_start = time(NULL);
int rtp_hdr_pos = 0, num_packets = 0;
struct rlimit rl;
if (getrlimit(RLIMIT_STACK, &rl) == 0) {
if (rl.rlim_cur > THREAD_STACK_SIZE) {
rl.rlim_cur = THREAD_STACK_SIZE;
setrlimit(RLIMIT_STACK, &rl);
}
}
memset(rtp_hdr[0], 0, RTP_HDR_SZ);
memset(rtp_hdr[1], 0, RTP_HDR_SZ);
data_init(&ts);
ts_set_log_func(LOG_func);
parse_options(&ts, argc, argv);
if (ts.pidfile)
daemonize(ts.pidfile);
if (ts.syslog_active) {
if (ts.syslog_remote) {
log_init(ts.ident, 1, 1, ts.syslog_host, ts.syslog_port);
remote_syslog = 1;
} else {
openlog(ts.ident, LOG_NDELAY | LOG_PID, LOG_USER);
local_syslog = 1;
}
}
ts.notify = notify_alloc(&ts);
ts_LOGf("Start %s\n", program_id);
notify(&ts, "START", "Starting %s", program_id);
if (ts.input.type == NET_IO && udp_connect_input(&ts.input) < 1)
goto EXIT;
if (ts.output.type == NET_IO && udp_connect_output(&ts.output) < 1)
goto EXIT;
signal(SIGCHLD, SIG_IGN);
signal(SIGPIPE, SIG_IGN);
signal(SIGINT , signal_quit);
signal(SIGTERM, signal_quit);
if (ts.threaded) {
pthread_create(&ts.decode_thread, &ts.thread_attr, &decode_thread, &ts);
pthread_create(&ts.write_thread , &ts.thread_attr, &write_thread , &ts);
}
ts.emm_last_report = time(NULL) + FIRST_REPORT_SEC;
ts.ecm_last_report = time(NULL) + FIRST_REPORT_SEC;
camd_start(&ts);
if (packet_from_file) {
uint8_t tmp[2048];
ts_hex_dump_buf((char *)tmp, sizeof(tmp), packet_buf, packet_buflen, 16);
ts_LOGf("%s | Processing packet with CAID 0x%04x\n", packet_type == ECM_MSG ? "ECM" : "EMM", ts.forced_caid);
ts_LOGf("%s | Packet dump:\n%s\n", packet_type == ECM_MSG ? "ECM" : "EMM", tmp);
camd_process_packet(&ts, camd_msg_alloc(packet_type, ts.forced_caid, ts.forced_service_id, packet_buf, packet_buflen));
goto EXIT;
}
do {
if (!ts.camd.constant_codeword)
do_reports(&ts);
if (ts.input.type == NET_IO) {
set_log_io_errors(0);
if (!ts.rtp_input) {
readen = fdread_ex(ts.input.fd, (char *)ts_packet, FRAME_SIZE, 250, 4, 1);
} else {
readen = fdread_ex(ts.input.fd, (char *)ts_packet, FRAME_SIZE + RTP_HDR_SZ, 250, 4, 1);
if (readen > RTP_HDR_SZ) {
memcpy(rtp_hdr[rtp_hdr_pos], ts_packet, RTP_HDR_SZ);
memmove(ts_packet, ts_packet + RTP_HDR_SZ, FRAME_SIZE);
readen -= RTP_HDR_SZ;
uint16_t ssrc = (rtp_hdr[rtp_hdr_pos][2] << 8) | rtp_hdr[rtp_hdr_pos][3];
uint16_t pssrc = (rtp_hdr[!rtp_hdr_pos][2] << 8) | rtp_hdr[!rtp_hdr_pos][3];
rtp_hdr_pos = !rtp_hdr_pos;
if (pssrc + 1 != ssrc && (ssrc != 0 && pssrc != 0xffff) && num_packets > 2)
if (ts.ts_discont)
ts_LOGf("--- | RTP discontinuity last_ssrc %5d, curr_ssrc %5d, lost %d packet\n",
pssrc, ssrc, ((ssrc - pssrc)-1) & 0xffff);
num_packets++;
}
}
set_log_io_errors(1);
if (!keep_running)
break;
if (readen < 0) {
ts_LOGf("--- | Input read timeout.\n");
if (!ntimeouts) {
timeout_start = time(NULL);
notify(&ts, "INPUT_TIMEOUT", "Read timeout on input %s://%s:%s/",
ts.rtp_input ? "rtp" : "udp",
ts.input.hostname, ts.input.service);
}
ts.no_input = 1;
ntimeouts++;
} else {
if (ntimeouts && readen > 0) {
time_t now = time(NULL);
ts_LOGf("+++ | Input OK after %ld sec timeout.\n", (now - timeout_start) + 2);
notify(&ts, "INPUT_OK", "Data is available on input %s://%s:%s/ after %ld seconds timeout.",
ts.rtp_input ? "rtp" : "udp",
ts.input.hostname, ts.input.service,
(now - timeout_start) + 2); // Timeout is detected when ~2 seconds there is no incoming data
ts.no_input = 0;
ntimeouts = 0;
}
}
} else {
readen = read(ts.input.fd, ts_packet, FRAME_SIZE);
have_data = !(readen <= 0);
}
if (readen > 0) {
if (ts.input_dump_file)
fwrite(ts_packet, readen, 1, ts.input_dump_file);
process_packets(&ts, ts_packet, readen);
}
} while (have_data && keep_running);
EXIT:
camd_stop(&ts);
// If pthread_join failes make sure we exit...
signal(SIGINT , SIG_DFL);
signal(SIGTERM, SIG_DFL);
signal(SIGALRM, signal_alarm);
alarm(2);
if (ts.threaded) {
ts.decode_stop = 1;
ts.write_stop = 1;
if (ts.decode_thread)
pthread_join(ts.decode_thread, NULL);
if (ts.write_thread)
pthread_join(ts.write_thread, NULL);
}
show_pid_report(&ts);
notify_sync(&ts, "STOP", "Stopping %s", program_id);
ts_LOGf("Stop %s\n", program_id);
if (ts.syslog_active) {
if (ts.syslog_remote)
log_close();
else
closelog();
}
if (ts.input_dump_file)
fclose(ts.input_dump_file);
if (ts.pidfile)
unlink(ts.pidfile);
if (log_file)
fclose(log_file);
notify_free(&ts.notify);
data_free(&ts);
exit(EXIT_SUCCESS);
}
/* Process received samples */
void NdProcSamples(word dc, word phase, word sum_st, const NdPackChan *chans)
{
/* Example: Write samples to text file */
/* For simplicity:
* Continuous time is assumed (phase is not checked).
* Detailed artifact info for each channel is not used here.
*/
/* Number of a sample value characters in output file */
#define FLOAT_WIDTH 9
static char s[MAX_PACK_PHASES * (MAX_SIGNALS * (FLOAT_WIDTH+1) + 2) + 1];
/* Number of channels */
static int i;
/* Array of sample scaling factors */
static float coeff[MAX_SIGNALS];
static word last_epoch_ph;
word endphase = 0; /* Flag of the last phase in input sample packet */
word snum[MAX_SIGNALS]; /* Array of indexes of current samples from individual channels */
if (!dev_chans) // first time: get channel number and coeff
{
NDGETVAL v;
if (NdGetParam(ND_PAR_CHAN_NUM, 0, &v) || !((v.type&~ND_T_LIST)==ND_T_INT))
dev_chans=0;
else dev_chans = v.val.i;
for (i=0;i<dev_chans;i++)
{
if (NdGetParam(ND_PAR_CH_RANGE_MAX, i, &v) || !((v.type&~ND_T_LIST)==ND_T_FLOAT))
coeff[i]=1;
else
coeff[i] = v.val.f / 0x80000000ul;
}
}
memset(snum, 0, sizeof(snum));
/* Loop by sampling phases */
do {
word i, *n;
const NdPackChan *ch;
/* Loop by channels */
for (i=0, ch=chans, n=snum; i<dev_chans; i++, ch++, n++) {
if (!ch->samps)
/* Channel disabled */
continue;
if ((phase+1)&ch->mask) {
/* Channel was not sampled at current phase */
continue;
}
/* There is a sample from this channel in current phase. */
current_chn[i]=((float)ch->samps[*n] * coeff[i]);
// write_logfile("chn %d (%.2f * %.12f) -> %.2f",i,(float)ch->samps[*n],coeff[i],current_chn[i]);
(*n)++;
if (*n==ch->num)
endphase = 1;
}
phase++;
process_packets();
} while (!endphase);
}
void process_emotiv(void) //calls process_packets() function
{
if (drv_lib==NULL) return;
if(EE_EngineGetNextEvent(eEvent) == EDK_OK)
{
EE_Event_t eventType = EE_EmoEngineEventGetType(eEvent);
if (eventType == EE_UserAdded)
{
std::cout << "User added event detected\n";
EE_EmoEngineEventGetUserId(eEvent, &userID);
// if (userID == userID_selected)
//{
std::cout << "User ID is: " << userID <<"\n";
EE_DataAcquisitionEnable(userID,true);
readytocollect = true;
//}
// else std::cout << "User ID does not match - acquisition not enabled\n";
}
}
if(readytocollect) // && (userID == userID_selected))
{
std::cout << "-";
EE_DataUpdateHandle(userID, hData);
EE_DataGetNumberOfSample(hData, &nSamplesTaken);
if(nSamplesTaken != 0)
{
for (int i = 0; i < MAX_CHANNELS; i++)
buffer[i].clear();
double* data = new double[nSamplesTaken];
for (int channel = 0 ; channel < MAX_CHANNELS; channel++)
{
EE_DataGet(hData, targetChannelList[channel+1], data, nSamplesTaken);
// std::cout << " DataGet for channel " << channel << " returned :";
for (unsigned int j = 0; j < nSamplesTaken; j++)
{
buffer[channel].push_back((const float)data[j]);
//std::cout << data[j] << ", ";
}
//std::cout << "\n";
}
delete[] data;
for (unsigned int j = 0; j < nSamplesTaken; j++)
{
for (int channel = 0 ; channel < MAX_CHANNELS; channel++) {
// std::cout << "now sending sample " << j+1 << " to output port " << channel <<": " << buffer[channel].at(j) << "\n";
gEMOTIV->pass_values(channel, buffer[channel].at(j));
}
process_packets();
std::cout << "*";
}
std::cout << "\n";
}
}
}
