#include "control.h" #include "rt_info.h" #include double baseline_ms = DEFAULT_BASELINE_ms ; double probe_ms = DEFAULT_PROBE_ms ; double state_ms = DEFAULT_STATE_DURATION_ms ; int timeout_ms = DEFAULT_TIMEOUT_ms ; int triggered_attention_ms = DEFAULT_TRIGGERED_ATTENTION_ms; double trigger_time_ms; int trigger_noted = 0 ; int baseline_cycles; int probe_cycles; int new_baseline_cycles = 0 ; int new_probe_cycles = 0 ; int command_cycles; int timeout_cycles; int timeout_timer = 0; int save_traces = 0; int trace_stored = 0; int baseline_stored = 0; int ext_reference = 0; int probe_out = 0; int baseline_out = 0; int threshold_uV = DEFAULT_THRESHOLD_uV ; int polarity = DEFAULT_POLARITY ; double command_amplitude = DEFAULT_COMMAND_V ; double command_ms = DEFAULT_COMMAND_ms ; /* // print_string() is contributed by Sen for debugging only, use with caution: // does not always work, may result in loss of samples, may halt if in a loop struct tty_struct *last_tty; char string_buf[100]; void print_string(char *str) { struct tty_struct *my_tty; my_tty = current->tty; if (my_tty !=NULL) { last_tty=my_tty; } if (last_tty != NULL) { (*(last_tty->driver).write) ( last_tty, 0, str, strlen(str)); (*(last_tty->driver).write)(last_tty,0,"\015\012", 2); } } */ /* this is a listener for commands from 'communication' process see communication.c for details or ask Artem () */ int message[2]; int* adjust_params() { if (rtf_get(FIFO1, message, 2*sizeof(int)) == 2*sizeof(int)) { switch ((char)message[0]) { case 't': if (message[1] > 0) { threshold_uV = message[1]*1000; polarity = POSITIVE ; } else { threshold_uV = -message[1]*1000; polarity = NEGATIVE ; } break; case 'o': timeout_cycles = ms_to_CYCLES(message[1], INT_MAX); save_traces = 0; trace_stored = 0; timeout_timer = 0; break; case 'p': new_probe_cycles = ms_to_CYCLES((double)(message[1])/1000, MAX_STORAGE); break; case 'b': new_baseline_cycles = ms_to_CYCLES(message[1], MAX_STORAGE); save_traces = 0; baseline_stored = 0; trace_stored = 0; break; case 'd': state_ms = message[1]; break; case 'g': triggered_attention_ms = message[1]; trigger_time_ms = 0 ; break; case 's': save_traces = message[1]; trace_stored = 0 ; break; case 'm': if (message[1] == 0) { probe_out = 0; baseline_out = 0; } else if (message[1] == 1) { probe_out = 0; baseline_out = 1; } else if (message[1] == 2) { probe_out = 1; baseline_out = 0; } else if (message[1] == 3) { probe_out = 1; baseline_out = 1; } //sprintf(string_buf,"mode: base=%d, probe=%d", baseline_out, probe_out ); //print_string(string_buf); break; case 'e': if (message[1]) { ext_reference = 1; } else { ext_reference = 0; } break; case 'a': command_amplitude = (double)message[1]/1000; break; case 'w': command_cycles = ms_to_CYCLES( (double)message[1]/1000, MAX_STORAGE) ; break; default: ; //print_string("control: invalid option"); } } return NULL ; } /* sets initial values, called only once. */ void init_control() { baseline_cycles = ms_to_CYCLES(baseline_ms, MAX_STORAGE); probe_cycles = ms_to_CYCLES(probe_ms, MAX_STORAGE); timeout_cycles = ms_to_CYCLES(timeout_ms, MAX_STORAGE); command_cycles = ms_to_CYCLES(command_ms, MAX_STORAGE); } /* The algorithm is all about moving 'windows' along the time axis and averaging. A longer 'baseline' window is followed by a shorter 'probe' window. When the difference in baseline and probe average crosses the specified threshold the 'ON' state is noted. If this 'ON state' is sustained for a specified duration, an 'event' is 'detected' and corresponding output is issued. We also keep record of all the samples in 'baseline' and 'timeout' time intervals. After an event is reported, 'baseline' samples are frozen and 'timeout' samples are collected to allow the 'capture' of input signal trace of the 'event'. */ inline void control_algorithm(double *in, double *out) { static int baseline_storage_uV[MAX_STORAGE]; static int probe_storage_uV[MAX_STORAGE]; static int timeout_storage_uV[MAX_STORAGE]; static int baseline_index = 0; static int probe_index = 0; static int baseline_average_uV = 0; static int probe_average_uV = 0; static int command_timer = 0; static int report_due_cycles = REPORT_CYCLES ; static int report_timer = REPORT_CYCLES ; static int reported_events = 0; static int threshold_events = 0; static int trigger_events = 0; static int start_timestamp_sec = 0; static struct report_struct report; static double event_front_ms = -1.0; double sample_time_ms = rt_get_time_ns()/1e6; double command = COMMAND_BASELEVEL_V; int incoming_baseline_uV = probe_storage_uV[probe_index]; int in_0 = (int)(in[0]*1e6); // convert to uV int in_2 = (int)(in[2]*1e6); probe_average_uV += (in_0 - probe_average_uV) / probe_cycles ; probe_storage_uV[probe_index++] = in_0; if (probe_index == probe_cycles) { probe_index = 0 ; if (new_probe_cycles) { probe_cycles = new_probe_cycles; new_probe_cycles = 0; } } // if reportable event was recently detected if (timeout_timer) { // we keep the record of 'post event' samples in case trace capture is required if (save_traces && !trace_stored && baseline_stored) { timeout_storage_uV[timeout_cycles - timeout_timer] = in_0 ; if (timeout_timer == 1) trace_stored = 1; } timeout_timer--; // and we send a command to the output if (command_timer) { command += command_amplitude; command_timer--; } } else { // note: we only update baseline_average if we are not in the 'timeout' interval. // the following formula provides for a slow decay of past history, newer samples weigh more... baseline_average_uV += (incoming_baseline_uV - baseline_average_uV)/baseline_cycles ; /* We do not really need to update baseline_storage unless trace capture is required and the trace is not stored yet or unless baseline_cycles value needs to be updated */ if (new_baseline_cycles || (save_traces && !trace_stored)) { baseline_storage_uV[baseline_index++] = incoming_baseline_uV; if (baseline_index == baseline_cycles) { baseline_index = 0 ; if (new_baseline_cycles) { baseline_cycles = new_baseline_cycles; new_baseline_cycles = 0; } else { baseline_stored = 1; } } } // update trigger state in case it matters if (in[1] > TRIGGER_LEVEL_V && !trigger_noted) { trigger_time_ms = sample_time_ms; trigger_events++; trigger_noted = 1; } else if (in[1] < TRIGGER_LEVEL_V) { trigger_noted = 0; } /* Now we are going to deside whether an 'event' should be reported. We are in the 'ON' state if our probe average is beyond the threshold. We will issue a command pulse if the requirement on the duration of 'ON' state is not set (which is when state_ms is set to 0) or if 'ON' state duration exceeds state_ms setting. Otherwise, we make sure that front timestamp is reset if returning into 'OFF' state */ if ( ( !ext_reference && (polarity*(probe_average_uV - baseline_average_uV) > threshold_uV) ) || ( ext_reference && ((in_2 > 0 && probe_average_uV-baseline_average_uV > in_2) || (in_2 < 0 && probe_average_uV-baseline_average_uV < in_2)) ) ) { threshold_events++ ; // if not waiting for reset and ((no trigger required) or (trigger received not too long ago)) if ( event_front_ms != 0 && (!triggered_attention_ms || ( sample_time_ms - trigger_time_ms < triggered_attention_ms ) ) ) { if (state_ms > 0 && event_front_ms < 0) { event_front_ms = sample_time_ms ; // begin waiting for required 'ON' state duration } else if (state_ms == 0 || (sample_time_ms - event_front_ms) > state_ms) { command += command_amplitude; // report detection !! command_timer = command_cycles-1; timeout_timer = timeout_cycles; reported_events++; event_front_ms = 0 ; // signal will have to come near baseline level to reset this. // uncomment the next line if you do not want multiple detections on a single trigger pulse //trigger_time_ms = 0; } else { ; // still waiting to see if this 'ON' state is long enough } } // if tirgger Ok and reset Ok. } else { /* when we are below threshold ('OFF' state) 'reset' is performed to enable detection of the next event */ event_front_ms = -1; } } // end of else (end of 'if not timeout') if (--report_timer) { ; // no report this time } else { // timestamp in seconds if (!start_timestamp_sec) { start_timestamp_sec = (int)(sample_time_ms / 1000) - REPORT_PERIOD_sec; report.timestamp_sec = REPORT_PERIOD_sec; } else { report.timestamp_sec = (int)(sample_time_ms / 1000) - start_timestamp_sec ; } // pack all the other stuff report.reported_events = reported_events; report.threshold_events = threshold_events; report.trigger_events = trigger_events; report.upstate_ms = 0; report.resting_uV = baseline_average_uV ; // send rtf_put(FIFO, &report, sizeof(report)); // reset reported_events = 0; threshold_events = 0; trigger_events = 0; if (save_traces && trace_stored) { rtf_put(FIFO2, &(report.timestamp_sec), sizeof(int)); // first value is a timestamp in seconds rtf_put(FIFO2, baseline_storage_uV+baseline_index, (baseline_cycles-baseline_index)*sizeof(int)); rtf_put(FIFO2, baseline_storage_uV, baseline_index*sizeof(int)); rtf_put(FIFO2, timeout_storage_uV, timeout_cycles*sizeof(int)); rtf_sem_post(FIFO3); trace_stored = 0; baseline_stored = 0; save_traces--; } report_timer = report_due_cycles ; } // outputs if (probe_out) command += (double)probe_average_uV / 1e6 ; *(out + 1) = command ; if (baseline_out) *(out) = (double)baseline_average_uV / 1e6 ; else *(out) = (timeout_timer == 0)?0:1 ; }