diff options
Diffstat (limited to 'libarmep/sample.c')
| -rw-r--r-- | libarmep/sample.c | 1259 |
1 files changed, 1259 insertions, 0 deletions
diff --git a/libarmep/sample.c b/libarmep/sample.c new file mode 100644 index 0000000..def49f3 --- /dev/null +++ b/libarmep/sample.c @@ -0,0 +1,1259 @@ +#include "libarmep.h" + +#include <linux/serial.h> + +int somebody_triggered = -1; +struct aep_channel *who_triggered = NULL; + +static void track_limits_convert_to_current(struct aep_channel *ch, + double *v1, double *v2) +{ + double pwr; + + if ((*v1 - ch->voffset[0]) < ch->min[3]) + ch->min[3] = *v1 - ch->voffset[0]; + + if ((*v1 - ch->voffset[0]) > ch->max[3]) + ch->max[3] = *v1 - ch->voffset[0]; + + if ((*v2 - ch->voffset[1]) < ch->min[4]) + ch->min[4] = *v2 - ch->voffset[1]; + + if ((*v2 - ch->voffset[1]) > ch->max[4]) + ch->max[4] = *v2 - ch->voffset[1]; + + if (!ch->aep->aep_context->auto_zero) { + if (*v1 < 0) { + if (ch->samples_seen > IGNORE_NEG_PRIOR_TO_SAMPLES && + -(*v1) > ch->vnoise[0] / 2) { + if (ch->aep->aep_context->verbose) + fprintf(stderr, "*** reducing voltage " + "baseline by %fV to %fV\n", + *v1, ch->voffset[0] - *v1); + ch->voffset[0] -= *v1 * + NEGATIVE_ADJUST_SCALING_FACTOR; + } + *v1 = 0; + } + if (*v2 < 0) { + if (ch->samples_seen > IGNORE_NEG_PRIOR_TO_SAMPLES && + -(*v2) > ch->vnoise[1] / 2) { + if (ch->aep->aep_context->verbose) + fprintf(stderr, "*** reducing shunt " + "voltage baseline by " + "%fV to %fV\n", *v2, + ch->voffset[1] - *v2); + ch->voffset[1] -= *v2 * + NEGATIVE_ADJUST_SCALING_FACTOR; + } + *v2 = 0; + } + } + + *v2 = correct(ch->aep->aep_context->no_correction, *v1, *v2, ch->map_table->map, ch->map_table->len) / ch->rshunt; + + pwr = *v1 * *v2; + if (*v1 > ch->max[0]) + ch->max[0] = *v1; + if (*v1 < ch->min[0]) + ch->min[0] = *v1; + if (*v2 > ch->max[1]) + ch->max[1] = *v2; + if (*v2 < ch->min[1]) + ch->min[1] = *v2; + if (pwr > ch->max[2]) + ch->max[2] = pwr; + if (pwr < ch->min[2]) + ch->min[2] = pwr; +} + +int process_sample(struct aep *aep, int ch_index) +{ + struct aep_channel *ch = &aep->ch[ch_index], *ich; + double v1, v2, v1a, v2a, vo0 = 0, vo1 = 0; + int m, i; + + if (!aep->aep_context->no_correction) { + vo0 = ch->voffset[0]; + vo1 = ch->voffset[1]; + } + + if (ch->pretrig_ring) { + ch->pretrig_ring[ch->head].v = aep->voltage; + ch->pretrig_ring[ch->head].i = aep->current; + ch->head++; + if (ch->head == ch->ring_samples) + ch->head = 0; + if (ch->head == ch->tail) + ch->tail++; + if (ch->tail == ch->ring_samples) + ch->tail = 0; + + if (ch->pretrigger_samples_taken < ch->ring_samples) + ch->pretrigger_samples_taken++; + } + + if ((ch->triggered && aep->voltage < aep->aep_context->mv_min && ch->samples > 1000 && !ch->trigger_slave) || + (ch->trigger_slave && !ch->trigger_slave->triggered)) { + + ch->trigger_filter++; + if (ch->trigger_filter < aep->aep_context->end_trigger_filter_us / 100) + goto unripe; + + if (aep->aep_context->verbose) + fprintf(stderr, "trigger session completed " + "%dmV vs %dmV\n", aep->voltage, aep->aep_context->mv_min); + avg_mean_us_flush(&ch->avg_mean_voltage); + avg_mean_us_flush(&ch->avg_mean_current); + // ch->ignore = 0; + ch->triggered = 0; + ch->trigger_filter = 0; + ch->pretrigger_samples_taken = 0; + ch->head = 0; + ch->tail = 0; + if (aep->aep_context->require_off) { + if (aep->aep_context->verbose) + fprintf(stderr, "marking capture for " + "device %d done\n", aep->index); + aep->aep_context->awaiting_capture &= ~(1 << (aep->index * 3 + ch_index)); + } + return 0; + } else + if (ch->triggered) + ch->trigger_filter = 0; +unripe: + + ch->samples_seen++; + + if (ch->ignore) + return 0; + + if (!ch->triggered) { + avg_mean_us_add(&ch->avg_mean_voltage, aep->voltage); + avg_mean_us_add(&ch->avg_mean_current, aep->current); + + v1 = (avg_mean_us(&ch->avg_mean_voltage) / + ADC_COUNTS_PER_VOLT_CH1) + vo0; + v2 = (avg_mean_us(&ch->avg_mean_current) / + ADC_COUNTS_PER_VOLT_CH2) + vo1; + if ((!(ch->samples & 0x1ff)) && aep->aep_context->show_raw) + fprintf(stderr, "%fmV raw shunt; corr=%fmV\n", v2, + correct(aep->aep_context->no_correction, v1, v2, ch->map_table->map, + ch->map_table->len)); + + + track_limits_convert_to_current(ch, &v1, &v2); + + if ((((aep->voltage > aep->aep_context->mv_min + TRIG_HYSTERESIS_MV) || !aep->aep_context->mv_min) && + (((v1 * v2) > (aep->aep_context->mw_min_plus_hyst) / 1000.0) || !aep->aep_context->mw_min)) || + (somebody_triggered != -1 && ch->samples_seen >= somebody_triggered)) { + + ch->trigger_filter++; + if (ch->trigger_filter > aep->aep_context->trigger_filter_us / 100 || somebody_triggered != -1) { + + if (somebody_triggered != -1) { + ch->trigger_slave = who_triggered; + } else { + who_triggered = ch; + somebody_triggered = ch->samples_seen; + } + ch->triggered = 1; + ch->samples = 0; + ch->trigger_filter = 0; + if (aep->aep_context->verbose) + fprintf(stderr, "%s: triggered " + "%.2fV %.5fW pretrig %d " + "(%dms)\n", + ch->channel_name, v1, + v1 * v2, + ch->ring_samples, + ch->ring_samples / 10); + } + } else + ch->trigger_filter = 0; + + aep->rx_count++; + } + + if (!ch->samples && aep->aep_context->ms_holdoff) + fprintf(stderr, "%s: post-trigger holdoff for" + " %dms\n", ch->channel_name, aep->aep_context->ms_holdoff); + + if (ch->triggered) { + ch->samples++; + + if (ch->samples / 10 < aep->aep_context->ms_holdoff) + return 0; + if (ch->samples - 1 == aep->aep_context->ms_holdoff * 10 && aep->aep_context->verbose) + fprintf(stderr, "%s: Starting capture\n", ch->channel_name); + } + + if (ch->pretrig_ring) { + avg_mean_us_add(&ch->avg_mean_voltage, + ch->pretrig_ring[ch->tail].v); + avg_mean_us_add(&ch->avg_mean_current, + ch->pretrig_ring[ch->tail].i); + ch->tail++; + if (ch->tail == ch->ring_samples) + ch->tail = 0; + } else { + avg_mean_us_add(&ch->avg_mean_voltage, aep->voltage); + avg_mean_us_add(&ch->avg_mean_current, aep->current); + } + v1 = (avg_mean_us(&ch->avg_mean_voltage) / + ADC_COUNTS_PER_VOLT_CH1) + vo0; + v2 = (avg_mean_us(&ch->avg_mean_current) / + ADC_COUNTS_PER_VOLT_CH2) + vo1; + + if ((!(ch->samples & 0x1ff)) && aep->aep_context->show_raw) + fprintf(stderr, "%fmV raw shunt; corr=%fmV\n", v2, + correct(aep->aep_context->no_correction, v1, v2, ch->map_table->map, ch->map_table->len)); + + track_limits_convert_to_current(ch, &v1, &v2); + + if (aep->aep_context->auto_zero && v1 > REJECT_AUTOZERO_VOLTAGE) { + fprintf(stderr, "**** Autozero seeing > %fV... " + "probe should not be in powered target for " + "autozero! ****\n", REJECT_AUTOZERO_VOLTAGE); + return -2; + } + + if (aep->aep_context->auto_zero) { + ch->simple_avg[0] += v1; + ch->simple_avg[1] += v2; + ch->simple_avg[2] += v1 * v2; + ch->avg_count++; + } + + ch->decimation_counter++; + if (ch->decimation_counter != aep->aep_context->decimate) + goto done; + ch->decimation_counter = 0; + + if (aep->aep_context->auto_zero) { + fprintf(stderr, "Autozero for %s done \n", + ch->channel_name); + if (v1 > IGNORE_AUTOZERO_VOLTAGE || v1 < -IGNORE_AUTOZERO_VOLTAGE) + fprintf(stderr, " Voltage Autozero skipped as %fV " + "unfeasibly high\n", v1); + else { + ch->voffset[0] = -v1; + ch->vnoise[0] = ch->max[3] - ch->min[3]; + } + ch->voffset[1] = -avg_mean_us(&ch->avg_mean_current) / + ADC_COUNTS_PER_VOLT_CH2; + ch->vnoise[1] = ch->max[4] - ch->min[4]; + if (configure(aep->aep_context, aep, aep->dev_filepath, aep->aep_context->config_filepath, ch) < 0) + fprintf(stderr, + "Failed to write autozero info to config\n"); + goto done; + } + + /* not everyone else may be ready, save them up */ + if (v1 < 0) + v1 = 0; + if (v2 < 0) + v2 = 0; + ch->out_ring[0][ch->out_head] = v1; + ch->out_ring[1][ch->out_head++] = v2; + if (ch->out_head == MAX_RESULT_QUEUE_DEPTH) + ch->out_head = 0; + if (ch->out_head == ch->out_tail) + fprintf(stderr, "\n***** %s output ring overflow...%d %d\n", + ch->channel_name, ch->out_head, ch->out_tail); + + /* the other capture channels have data for this sample? */ + + for (m = 0; m <= aep->aep_context->highest; m++) { + if (!aep->aep_context->aeps[m].fd) + continue; + for (i = 0; i < CHANNELS_PER_PROBE; i++) + if (aep->aep_context->aeps[m].ch[i].out_head == aep->aep_context->aeps[m].ch[i].out_tail) { +// fprintf(stderr, "empy %s\n", aep->aep_context->aeps[m].ch[i].channel_name); + goto done; + } + } + + /* if so, output it all together */ + + aep->aep_context->samples(AEPSA_START_PRE + (ch->triggered * 3), ch, ((double)ch->samples - + (double)ch->ring_samples) / 10000.0, 0); + + for (m = 0; m <= aep->aep_context->highest; m++) { + if (!aep->aep_context->aeps[m].fd) + continue; + for (i = 0; i < CHANNELS_PER_PROBE; i++) { + + ich = &aep->aep_context->aeps[m].ch[i]; + + v1a = ich->out_ring[0][ich->out_tail]; + v2a = ich->out_ring[1][ich->out_tail]; + ich->out_tail++; + if (ich->out_tail == MAX_RESULT_QUEUE_DEPTH) + ich->out_tail = 0; + if (ch->samples >= ch->ring_samples) { + ich->simple_avg[0] += v1a; + ich->simple_avg[1] += v2a; + ich->simple_avg[2] += v1a * v2a; + ich->avg_count++; + } + if (!ich->requested) + continue; + + aep->aep_context->samples(AEPSA_SAMPLE_PRE + (ch->triggered * 3), ch, v1a, v2a); + } + } + + aep->aep_context->samples(AEPSA_END_PRE + (ch->triggered * 3), ch, 0, 0); + +done: + + if (aep->aep_context->ms_capture && (ch->samples / 10 - aep->aep_context->ms_holdoff) > aep->aep_context->ms_capture) { + fprintf(stderr, "%s: %dms capture complete ", + ch->channel_name, aep->aep_context->ms_capture); + ch->ignore = 1; + if (!aep->aep_context->require_off) { + aep->aep_context->awaiting_capture &= ~(1 << (aep->index * 3 + ch_index)); + fprintf(stderr, "\n"); + } else + fprintf(stderr, "(waiting for OFF)\n"); + } + + return 0; +} + +static int service_aep_buffers(struct aep *aep, int samples) +{ + unsigned char v; + static unsigned char cmd[] = { + AEPC_CONFIG, 0, 6, + }; + int bad = 0; + + while (aep->head != aep->tail && samples--) { + + v = aep->ring[aep->head++]; + if (aep->head == sizeof(aep->ring)) + aep->head = 0; + + if (aep->aep_context->verbose) + fprintf(stderr, "%02X ", v); + + switch (aep->state) { + + /* + * this filters the input to sync, it will keep returning to + * look for the first byte if any of AC FF FF FF FF FF FF FF FF + * is not correctly seen. + */ + + case APP_INIT_MAGIC: + switch (aep->counter++) { + case 0: + if (v != 0xac) + aep->counter = 0; + break; + case 1: + case 2: + case 3: + case 4: + case 5: + case 6: + case 7: + case 8: + if (v != 0xff) + aep->counter = 0; + break; + case 63: + if (aep->aep_context->verbose) + fprintf(stderr, + "magic ok, asking for version\n"); + v = AEPC_VERSION; + if (write(aep->fd, &v, 1) != 1) + return -1; + aep->state++; + aep->counter = 0; + break; + default: + break; + } + break; + + case APP_INIT_VERSION: + switch (aep->counter++) { + case 0: + if (v != 0xac) + return -1; + break; + case 1: + aep->version = v; + break; + case 2: + aep->version |= v << 8; + break; + case 3: + aep->version |= v << 16; + break; + case 4: + aep->version |= v << 24; + break; + case 63: + if (aep->aep_context->verbose) + fprintf(stderr, + "version ok, asking for vendor\n"); + v = AEPC_VENDOR; + if (write(aep->fd, &v, 1) != 1) + return -1; + aep->state++; + aep->counter = 0; + break; + default: + break; + } + break; + + case APP_INIT_VENDOR: + switch (aep->counter++) { + case 0: + if (v != 0xac) + return -1; + break; + case 63: + if (aep->aep_context->verbose) + fprintf(stderr, + "vendor ok, asking for rate\n"); + v = AEPC_RATE; + if (write(aep->fd, &v, 1) != 1) + return -1; + aep->state++; + aep->counter = 0; + aep->name[63] = '\0'; + break; + default: + aep->name[aep->counter - 2] = v; + break; + } + break; + + + case APP_INIT_RATE: + switch (aep->counter++) { + case 0: + if (v != 0xac) + return -1; + break; + case 1: + aep->rate = v; + break; + case 2: + aep->rate |= v << 8; + break; + case 63: + if (aep->aep_context->verbose) + fprintf(stderr, + "rate ok, doing config\n"); + aep->state++; + aep->counter = 0; + cmd[1] = 0; + if (write(aep->fd, &cmd[0], sizeof cmd) != + sizeof cmd) + return -1; + break; + default: + break; + } + break; + + case APP_INIT_CONFIG_1: + + switch (aep->counter++) { + case 0: + if (v != 0xac) + return -1; + break; + case 63: + if (aep->aep_context->verbose) + fprintf(stderr, + "rate ok, doing config\n"); + aep->state++; + aep->counter = 0; + cmd[1] = 1; + if (write(aep->fd, &cmd[0], sizeof cmd) != + sizeof cmd) + return -1; + break; + default: + break; + } + break; + + case APP_INIT_CONFIG_2: + + switch (aep->counter++) { + case 0: + if (v != 0xac) + return -1; + break; + case 63: + if (aep->aep_context->verbose) + fprintf(stderr, + "rate ok, doing config\n"); + aep->state++; + aep->counter = 0; + cmd[1] = 2; + if (write(aep->fd, &cmd[0], sizeof cmd) != + sizeof cmd) + return -1; + break; + default: + break; + } + break; + + + + case APP_INIT_CONFIG_3: + + switch (aep->counter++) { + case 0: + if (v != 0xac) + return -1; + break; + case 63: + if (aep->aep_context->verbose) + fprintf(stderr, + "Configured new probe instance " + "%s %s %08X %d\n", + aep->dev_filepath, aep->name, + aep->version, aep->rate); + + aep->done_config |= 2; + aep->state = APP_INIT_START_ACK; + aep->counter = 0; + break; + default: + break; + } + break; + + case APP_INIT_START_ACK: + if (v != 0xac) + return -1; + + if (aep->aep_context->verbose) + fprintf(stderr, + "Start acknowledge seen " + "%s %s %08X %d\n", + aep->dev_filepath, aep->name, + aep->version, aep->rate); + + aep->done_config |= 4; + aep->state = APP_FRAME_L; + aep->counter = 0; + break; + + case APP_FRAME_L: + if (v == (aep->predicted_frame & 0xff)) { + aep->state = APP_FRAME_H; + break; + } + fprintf(stderr, "%d:.L(%d %d)", aep->index, v, aep->predicted_frame & 0xff); + /* lost sync... */ + if (aep->invalid < AEP_INVALID_COVER) + aep->invalid = AEP_INVALID_COVER; + aep->state = APP_FRAME_H_BAD; + break; + + case APP_FRAME_H: + + if (v == aep->predicted_frame >> 8) { + aep->state = APP_VOLTAGE_L1; + aep->predicted_frame++; + if (aep->invalid) { + aep->invalid--; + if (aep->invalid == 0) + fprintf(stderr, "%d:donetime\n", aep->index); + else + fprintf(stderr, "-%d:%d-", aep->index, aep->invalid); + } + break; + } + + fprintf(stderr, "%d:.H(%d %d)", aep->index, v, aep->predicted_frame >> 8); + + /* fallthru */ + + case APP_FRAME_H_BAD: + /* lost sync - slip one byte and use that as frame prediction for next time */ + aep->predicted_frame = v; + if (aep->invalid < AEP_INVALID_COVER) + aep->invalid = AEP_INVALID_COVER; + aep->state = APP_VOLTAGE_L1_BAD; + break; + + case APP_VOLTAGE_L1_BAD: + aep->predicted_frame |= v << 8; + aep->predicted_frame++; + aep->state = APP_VOLTAGE_H1_BAD; + break; + + case APP_VOLTAGE_H1_BAD: + case APP_CURRENT_L1_BAD: + case APP_CURRENT_H1_BAD: + case APP_VOLTAGE_L2_BAD: + case APP_VOLTAGE_H2_BAD: + case APP_CURRENT_L2_BAD: + case APP_CURRENT_H2_BAD: + case APP_VOLTAGE_L3_BAD: + case APP_VOLTAGE_H3_BAD: + case APP_CURRENT_L3_BAD: + case APP_CURRENT_H3_BAD: + aep->state++; + break; + + case APP_SWALLOW: + + //if (aep->aep_context->verbose) + fprintf(stderr, "%s: LOST SYNC *\n", + aep->dev_filepath); + bad++; + if (bad > 16) { + fprintf(stderr, "%s: too much lost sync\n", + aep->dev_filepath); + return -1; + } + + aep->state = APP_FRAME_L; + break; + + /* good path */ + + case APP_VOLTAGE_L1: + aep->voltage = v; + aep->state++; + break; + + case APP_VOLTAGE_H1: + aep->voltage |= v << 8; + aep->state++; + break; + + case APP_CURRENT_L1: + aep->current = v; + aep->state++; + break; + + case APP_CURRENT_H1: + aep->current |= v << 8; + aep->state++; + + if (aep->invalid) + break; + + if (aep->current > ADC_COUNT_CURRENT_CLIP_LIMIT) + fprintf(stderr, "\n*** %d.0: %s Shunt diff voltage %fV above" + " %fV limit: clipping *** \n", + aep->index, aep->ch[0].channel_name, + (double)aep->current / ADC_COUNTS_PER_VOLT_CH2, + (double)ADC_COUNT_CURRENT_CLIP_LIMIT / + ADC_COUNTS_PER_VOLT_CH2); + + return process_sample(aep, 0); + case APP_VOLTAGE_L2: + aep->voltage = v; + aep->state++; + break; + + case APP_VOLTAGE_H2: + aep->voltage |= v << 8; + aep->state++; + break; + + case APP_CURRENT_L2: + aep->current = v; + aep->state++; + break; + + case APP_CURRENT_H2: + aep->current |= v << 8; + aep->state++; + + if (aep->invalid) + break; + + if (aep->current > ADC_COUNT_CURRENT_CLIP_LIMIT) + fprintf(stderr, "\n*** %d.1: %s Shunt diff voltage %fV above" + " %fV limit: clipping *** \n", + aep->index, aep->ch[1].channel_name, + (double)aep->current / ADC_COUNTS_PER_VOLT_CH2, + (double)ADC_COUNT_CURRENT_CLIP_LIMIT / + ADC_COUNTS_PER_VOLT_CH2); + + return process_sample(aep, 1); + + case APP_VOLTAGE_L3: + aep->voltage = v; + aep->state++; + break; + + case APP_VOLTAGE_H3: + aep->voltage |= v << 8; + aep->state++; + break; + + case APP_CURRENT_L3: + aep->current = v; + aep->state++; + break; + + case APP_CURRENT_H3: + aep->current |= v << 8; + aep->state = APP_FRAME_L; + + if (aep->invalid) { + fprintf(stderr, "#%d ", aep->index); + break; + } //else + //fprintf(stderr, "*%d ", aep->index); + + if (aep->current > ADC_COUNT_CURRENT_CLIP_LIMIT) + fprintf(stderr, "\n*** %d.2: %s Shunt diff voltage %fV above" + " %fV limit: clipping *** \n", + aep->index, aep->ch[2].channel_name, + (double)aep->current / ADC_COUNTS_PER_VOLT_CH2, + (double)ADC_COUNT_CURRENT_CLIP_LIMIT / + ADC_COUNTS_PER_VOLT_CH2); + + return process_sample(aep, 2); + + } + } + + return 0; +} + +static void init_aep(struct aep_context *aep_context, struct aep *aep, const char *device_filepath) +{ + int n, m; + struct aep_channel *ch; + + // fprintf(stderr, "***** init_aep *****\n"); + + aep->head = 0; + aep->tail = 0; + aep->predicted_frame = 0; + aep->state = APP_INIT_MAGIC; + aep->invalid = 0; + aep->counter = 0; + aep->started = 0; + aep->done_config = 0; + + aep->aep_context = aep_context; + + strncpy(aep->dev_filepath, device_filepath, + sizeof aep->dev_filepath - 1); + aep->dev_filepath[sizeof aep->dev_filepath - 1] = '\0'; + + for (n = 0; n < CHANNELS_PER_PROBE; n++) { + ch = &aep->ch[n]; + + ch->ignore = 0; + ch->triggered = 0; + ch->requested = 0; + ch->summary[0] = '\0'; + ch->aep = aep; + ch->trigger_slave = NULL; + ch->out_head = ch->out_tail = 0; + ch->voffset[0] = 0; + ch->vnoise[0] = 0; + ch->voffset[1] = 0; + ch->vnoise[1] = 0; + ch->rshunt = 0; + sprintf(ch->channel_name, "%s-%d", device_filepath, n); + ch->pretrig_ring = NULL; + ch->ring_samples = 0; + ch->trigger_filter = 0; + ch->pretrigger_samples_taken = 0; + avg_mean_us_init(&ch->avg_mean_voltage, aep->aep_context->average_len); + avg_mean_us_init(&ch->avg_mean_current, aep->aep_context->average_len); + ch->decimation_counter = 0; + for (m = 0; m < sizeof(ch->min) / sizeof(ch->min[0]); m++) { + ch->min[m] = 999; + ch->max[m] = 0; + } + ch->samples_seen = 0; + ch->simple_avg[0] = 0.0; + ch->simple_avg[1] = 0.0; + ch->simple_avg[2] = 0.0; + ch->avg_count = 0; + ch->samples = 0; + strcpy(ch->supply, "none"); + } +} + + +static void select_map(struct aep_channel *ch) +{ + int m; + int sel = -1; + double delta = 999999; + + for (m = 0; m < (sizeof interp_tables / sizeof interp_tables[0]); m++) { + if (ch->percentage_error_ref <= interp_tables[m].percentage_error_ref) { + if (delta > (interp_tables[m].percentage_error_ref - ch->percentage_error_ref)) { + delta = interp_tables[m].percentage_error_ref - ch->percentage_error_ref; + sel = m; + } + } else { + if (delta > (ch->percentage_error_ref - interp_tables[m].percentage_error_ref)) { + delta = ch->percentage_error_ref - interp_tables[m].percentage_error_ref; + sel = m; + } + } + } + if (ch->aep->aep_context->verbose) + fprintf(stderr, "%s = corr map %d %f%%\n", + ch->channel_name, sel, ch->percentage_error_ref); + ch->map_table = &interp_tables[sel]; +} + +void probe_close(struct aep *aep) +{ + unsigned char c = AEPC_STOP; + int n; + + write(aep->fd, &c, 1); + + for (n = 0; n < CHANNELS_PER_PROBE; n++) { + if (aep->ch[n].pretrig_ring) + free(aep->ch[n].pretrig_ring); + + avg_mean_us_free(&aep->ch[n].avg_mean_voltage); + avg_mean_us_free(&aep->ch[n].avg_mean_current); + } + + if (aep->aep_context->verbose) + fprintf(stderr, "closing %d\n", aep->fd); + close(aep->fd); +} + + +void add_pollfd(struct aep_context *aep_context, int fd, int events) +{ + aep_context->pollfds[aep_context->count_pollfds].fd = fd; + aep_context->pollfds[aep_context->count_pollfds].events = events; + aep_context->pollfds[aep_context->count_pollfds++].revents = 0; +} + +void remove_pollfd(struct aep_context *aep_context, int fd) +{ + int n; + + for (n = 0; n < aep_context->count_pollfds; n++) + if (aep_context->pollfds[n].fd == fd) { + while (n < aep_context->count_pollfds) { + aep_context->pollfds[n] = aep_context->pollfds[n + 1]; + n++; + } + aep_context->count_pollfds--; + } +} + + +int service_aeps(struct aep_context *aep_context) +{ + static unsigned char zero[8] = { 0, 0, 0, 0, 0, 0, 0, 0, }; + int fd; + int budget; + int len; + struct timeval tv; + unsigned char *p; + unsigned char c; + char filepath[20]; + struct termios tty; + struct serial_struct sinfo; + int n, m, i; + struct aep *aep; + struct aep_channel *ch; + int timeout = 20; + unsigned char buf[AEP_INPUT_QUEUE]; + + gettimeofday(&tv, NULL); + + /* look for any new devices appearing */ + + if (aep_context->aeps[aep_context->scan].fd > 0) + goto bail; + + sprintf(filepath, "/dev/ttyACM%d", aep_context->scan); + fd = open(filepath, O_RDWR | O_NONBLOCK | O_EXCL | O_NOCTTY); + + if (fd < 0) + goto bail; + + if (ioctl(fd, TIOCEXCL) < 0) { + fprintf(stderr, "Wasn't able to open %s exclusive", + filepath); + close(fd); + goto bail; + } + + tcflush(fd, TCIOFLUSH); + + if (ioctl(fd, TIOCGSERIAL, &sinfo) == 0) { + sinfo.closing_wait = ASYNC_CLOSING_WAIT_NONE; + ioctl(fd, TIOCSSERIAL, &sinfo); + } + + if (aep_context->verbose) + fprintf(stderr, "initing %s fd=%d\n", filepath, fd); + + /* enforce suitable tty state */ + + memset (&tty, 0, sizeof tty); + if (tcgetattr (fd, &tty)) { + fprintf(stderr, "tcgetattr failed on %s\n", filepath); + close(fd); + goto bail; + } + + tty.c_lflag &= ~(ISIG | ICANON | IEXTEN | ECHO | XCASE | + ECHOE | ECHOK | ECHONL | ECHOCTL | ECHOKE); + tty.c_iflag &= ~(INLCR | IGNBRK | IGNPAR | IGNCR | ICRNL | + IMAXBEL | IXON | IXOFF | IXANY | IUCLC); + tty.c_oflag &= ~(ONLCR | OPOST | OLCUC | OCRNL | ONLRET); + tty.c_cc[VMIN] = 1; + tty.c_cc[VTIME] = 0; + tty.c_cc[VEOF] = 1; + tty.c_cflag &= ~(CBAUD | CSIZE | CSTOPB | PARENB | CRTSCTS); + tty.c_cflag |= (8 | CREAD | 0 | 0 | 1 | CLOCAL); + + cfsetispeed(&tty, B115200); + cfsetospeed(&tty, B115200); + tcsetattr(fd, TCSANOW, &tty); + + init_aep(aep_context, &aep_context->aeps[aep_context->scan], filepath); + + if (configure(aep_context, &aep_context->aeps[aep_context->scan], filepath, + aep_context->config_filepath, NULL) < 0) { + fprintf(stderr, "config for %s failed\n", filepath); + close(fd); + goto bail; + } + + if (write(fd, zero, sizeof(zero)) != sizeof(zero)) { + close(fd); + goto bail; + } + if (aep_context->verbose) + fprintf(stderr, "sending reset\n"); + c = AEPC_RESET; + if (write(fd, &c, 1) != 1) { + close(fd); + goto bail; + } + + if (aep_context->verbose) + fprintf(stderr, "done reset\n"); + + aep_context->aeps[aep_context->scan].fd = fd; + aep_context->aeps[aep_context->scan].sec_last_traffic = tv.tv_sec; + aep_context->aeps[aep_context->scan].index = aep_context->scan; + + if (aep_context->scan > aep_context->highest) + aep_context->highest = aep_context->scan; + add_pollfd(aep_context, fd, POLLIN | POLLERR); + + for (n = 0; n < CHANNELS_PER_PROBE; n++) { + struct aep_channel *ch = &aep_context->aeps[aep_context->scan].ch[n]; + + select_map(ch); + + if (aep_context->original_count_channel_names) { + for (m = 0; m < aep_context->count_channel_names; m++) + if (!strcmp(ch->channel_name, aep_context->channel_name[m])) + break; + + if (m == aep_context->count_channel_names) + continue; + } else + if (aep_context->count_channel_names < (aep_context->matched_channel_names + 1)) + aep_context->count_channel_names++; + + ch->requested = 1; + aep_context->matched_channel_names++; + + aep_context->awaiting_capture |= 7 << (aep_context->scan * 3); + } + + aep_context->aeps[aep_context->scan].done_config |= 1; + +bail: + aep_context->scans++; + aep_context->scan++; + if (aep_context->scan == MAX_PROBES) + aep_context->scan = 0; + + /* if nothing waiting fail immediately if anything has buffered data to service */ + + for (n = 0; n <= aep_context->highest; n++) { + if (aep_context->aeps[n].fd <= 0) + continue; + + if (aep_context->aeps[n].head != aep_context->aeps[n].tail) + timeout = 0; + } + + if (aep_context->verbose) + fprintf(stderr, "count_pollfds=%d, [0].events=%d, ", + aep_context->count_pollfds, aep_context->pollfds[0].events); + n = poll(aep_context->pollfds, aep_context->count_pollfds, timeout); + if (aep_context->verbose) + fprintf(stderr, "n = %d %d\n", n, aep_context->pollfds[0].fd); + if (n < 0) { + fprintf(stderr, "poll failed %d\n", aep_context->count_pollfds); + return -1; + } + + /* if we're waiting to start but saw every channel / probe */ + +// fprintf(stderr, "* %d %d %d\n", aep_context->has_started, aep_context->count_channel_names, aep_context->matched_channel_names); + + if (!aep_context->has_started && + aep_context->count_channel_names == aep_context->matched_channel_names) { + /* nobody is waiting for config completion? */ + + c = 0; + for (m = 0; m <= aep_context->highest; m++) + if (aep_context->aeps[m].fd > 0) { +// fprintf(stderr, "a %d: %d\n", +// m, aep_context->aeps[m].done_config); + c = 1; + if (aep_context->aeps[m].done_config != 3 || aep_context->aeps[m].invalid) { +// fprintf(stderr, " done_config is %d\n", aep_context->aeps[m].done_config); + goto post_start; + } + } + + if (c == 0) + goto post_start; + + /* + * in the case we want to start all probes, don't know how many there are + * allow time to discover and recover from any initial comms error + */ + + if (aep_context->scans < (MAX_PROBES * 2)) + goto post_start; + + /* + * the number of guys we would start + * is all of them, is it? Becuase we need to + * eliminate cross-channel latency as far as we + * can + */ + + c = 0; + for (m = 0; m <= aep_context->highest; m++) { +// fprintf(stderr, "%d: %d %d\n", m, aep_context->aeps[m].fd, aep_context->aeps[m].done_config); + if (aep_context->aeps[m].fd > 0 && aep_context->aeps[m].done_config == 3) { + for (i = 0; i < CHANNELS_PER_PROBE; i++) + if (aep_context->aeps[m].ch[i].requested) + c++; + } + } + + if (c != aep_context->matched_channel_names) { +// fprintf(stderr, "c=%d mcn=%d\n", c, aep_context->matched_channel_names); + goto post_start; + } + + /* tell everyone to start */ + + fprintf(stderr, "Starting...\n"); + + aep_context->samples(AEPSA_DATA_STARTING, NULL, 0, 0); + + for (m = 0; m <= aep_context->highest; m++) { + if (aep_context->aeps[m].fd <= 0 || + aep_context->aeps[m].done_config != 3 ) { + fprintf(stderr, "not starting %d fd %d done_config %d\n", m, aep_context->aeps[m].fd, aep_context->aeps[m].done_config); + continue; + } + + if (aep_context->verbose) + fprintf(stderr, "sending start to %d\n", m); + c = AEPC_START; + if (write(aep_context->aeps[m].fd, &c, 1) != 1) { + fprintf(stderr, + "Failed to send start\n"); + } + + for (i = 0; i < CHANNELS_PER_PROBE; i++) { + ch = &aep_context->aeps[m].ch[i]; + + if (!ch->requested) + continue; + + aep_context->samples(AEPSA_DATA_STARTING_CHANNELS, ch, 0, 0); + } + + aep_context->aeps[m].started = 1; + aep_context->has_started = 1; + aep_context->aeps[m].sec_last_traffic = tv.tv_sec; + } + + aep_context->samples(AEPSA_DATA_STARTING_DONE, NULL, 0, 0); + } +post_start: + + /* somebody had something for us */ + + for (m = 0; m <= aep_context->highest; m++) { + + aep = &aep_context->aeps[m]; + + if (aep->fd < 1) + continue; + + /* check for timeout */ + + if (tv.tv_sec > aep->sec_last_traffic + 2) { +// fprintf(stderr, "%d: %d %d\n", m, aep_context->aeps[m].done_config, aep_context->aeps[m].started); + if (aep->done_config != 3 || aep->started) { + fprintf(stderr, "%s: timeout\n", aep->dev_filepath); + goto died; + } + } + + for (n = 0; n < aep_context->count_pollfds; n++) + if (aep->fd == aep_context->pollfds[n].fd) + break; + if (n == aep_context->count_pollfds) + continue; + + if (!aep_context->pollfds[n].revents) + continue; + + if (aep_context->verbose) + fprintf(stderr,"stuff for %d\n", m); + + aep->sec_last_traffic = tv.tv_sec; + + /* work out how much of the ring we can fill */ + + if (aep->head <= aep->tail) { + budget = (aep->head - 1) + (sizeof(aep->ring) - aep->tail); + p = aep->ring + aep->tail; + if (!budget) + continue; + + len = read(aep->fd, buf, budget); + if (len <= 0) { + fprintf(stderr, "failed to read data\n"); + goto died; + } + + if (aep_context->verbose) + fprintf(stderr, "%d (a) fetched %d (budget %d) head=%d tail=%d\n", m, len, budget, aep->head, aep->tail); + + n = sizeof(aep->ring) - aep->tail; + if (len < n) + n = len; + + memcpy(p, buf, n); + aep->tail += n; + if (aep->tail == sizeof(aep->ring)) + aep->tail = 0; + + len -= n; + if (len) { + memcpy(aep->ring, &buf[n], len); + aep->tail += len; + } + + } else { + budget = aep->head - aep->tail - 1; + p = aep->ring + aep->tail; + if (!budget) + continue; + + len = read(aep->fd, p, budget); + if (len <= 0) { + fprintf(stderr, "failed to read data\n"); + goto died; + } + + if (aep_context->verbose) + fprintf(stderr, "%d (b) fetched %d (budget %d) head=%d tail=%d\n", m, len, budget, aep->head, aep->tail); + + aep->tail += len; + } + + if (aep->tail == sizeof(aep->ring)) + aep->tail = 0; + + continue; + +died: + fprintf(stderr, "removing probe due to error...\n"); + for (i = 0; i < CHANNELS_PER_PROBE; i++) + if (aep->ch[i].requested) { + aep_context->awaiting_capture &= ~(1 << (m * 3 + i)); + aep_context->matched_channel_names--; + } + probe_close(aep); + remove_pollfd(aep_context, aep->fd); + aep->fd = 0; + m = aep_context->highest; + } + + + /* + * service the existing devices + * need to limit the amount of time spent in the service + * otherwise other probes will drop data + */ + + for (n = 0; n <= aep_context->highest; n++) { + + aep = &aep_context->aeps[n]; + + if (aep->fd <= 0) + continue; + m = service_aep_buffers(aep, MAX_BYTES_PER_AEP_SERVICE); + if (m >= 0) + continue; + if (m < -1) { + n = aep_context->highest + 1; + continue; + } + + if (!aep_context->verbose) + continue; + fprintf(stderr, "service failed\n"); + for (i = 0; i < CHANNELS_PER_PROBE; i++) { + if (!aep->ch[i].requested) + continue; + aep_context->awaiting_capture &= ~(1 << (n * 3 + i)); + aep_context->matched_channel_names--; + } + probe_close(aep); + remove_pollfd(aep_context, aep->fd); + aep->fd = 0; + } + + if (aep_context->scans > (5 * MAX_PROBES) && + aep_context->awaiting_capture == 0 && aep_context->exit_after_capture) { + fprintf(stderr, "done all capture\n"); + + + + return -1; + } + + return 0; +} + + + |