static int process_relocs(FILE *fp, struct flat_hdr * header, void * base) {
if (!header->reloc_count) { info("No relocation needed"); return 0; }
fseek(fp, header->reloc_start, SEEK_SET);
size_t size_to_copy = sizeof(uint32_t) * header->reloc_count;
uint32_t * offset_list = malloc(size_to_copy);
if (!offset_list) error_return("Failed to allocate temporary memory in process_relocs");
size_t size_read = fread(offset_list, sizeof(uint32_t), header->reloc_count, fp);
if (size_read < size_to_copy/sizeof(uint32_t)) {
free(offset_list);
error_return("Failed to read relocs");
}
endian_fix32(offset_list, header->reloc_count);
size_t i;
for (i=0; i<header->reloc_count; i++) {
uint32_t fixme = *(uint32_t*)((uint32_t)base + offset_list[i]);
void* relocd_addr = calc_reloc(fixme, header, base);
if (relocd_addr == (void*)-1) {
free(offset_list);
error_return("Unable to calculate relocation address");
}
*(uint32_t*)((uint32_t)base + offset_list[i]) = (uint32_t)relocd_addr;
}
free(offset_list);
return 0;
}
static int check_header(struct flat_hdr * header) {
if (memcmp(header->magic, "bFLT", 4) != 0) error_return("Magic number does not match");
if (header->rev != FLAT_VERSION) error_return("Version number does not match");
/* check for unsupported flags */
if (header->flags & (FLAT_FLAG_GZIP | FLAT_FLAG_GZDATA)) error_user_return("Unsupported flags detected - GZip'd data is not supported");
return 0;
}
int epos_position_profile_start(epos_node_t* node, epos_position_profile_t*
profile) {
int pos = epos_gear_from_angle(&node->gear, profile->target_value);
unsigned int vel = abs(epos_gear_from_angular_velocity(&node->gear,
profile->velocity));
unsigned int acc = abs(epos_gear_from_angular_acceleration(&node->gear,
profile->acceleration));
unsigned int dec = abs(epos_gear_from_angular_acceleration(&node->gear,
profile->deceleration));
short control = (profile->relative) ?
EPOS_POSITION_PROFILE_CONTROL_SET_RELATIVE :
EPOS_POSITION_PROFILE_CONTROL_SET_ABSOLUTE;
if (!epos_control_set_mode(&node->control, epos_control_profile_pos) &&
!epos_position_profile_set_velocity(&node->dev, vel) &&
!epos_profile_set_acceleration(&node->dev, acc) &&
!epos_profile_set_deceleration(&node->dev, dec) &&
!epos_profile_set_type(&node->dev, profile->type) &&
!epos_control_start(&node->control) &&
!epos_position_profile_set_target(&node->dev, pos)) {
profile->start_value = epos_node_get_position(node);
error_return(&node->dev.error);
timer_start(&profile->start_time);
epos_device_set_control(&node->dev, control);
timer_correct(&profile->start_time);
}
return node->dev.error.code;
}
static int init_server_data()
{
data.config.idx = data.input->server_idx;
data.config.len = MAX_SERVER_COUNT;
data.config.cid.size = data.input->group_size;
data.config.servers = (server_t*)malloc(data.config.len * sizeof(server_t));
if (NULL == data.config.servers) {
error_return(1, log_fp, "Cannot allocate configuration array\n");
}
memset(data.config.servers, 0, data.config.len * sizeof(server_t));
/* Set up log */
data.log = log_new();
if (NULL == data.log) {
error_return(1, log_fp, "Cannot allocate log\n");
}
return 0;
}
static int read_header(FILE* fp, struct flat_hdr * header) {
fseek(fp, 0, SEEK_SET);
size_t bytes_read = fread(header, 1, sizeof(struct flat_hdr), fp);
endian_fix32(&header->rev, ( &header->build_date - &header->rev ) + 1);
if (bytes_read == sizeof(struct flat_hdr)) {
return 0;
}else{
error_return("Error reading header");
}
}
static int copy_segments(FILE* fp, struct flat_hdr * header, void * mem, size_t max_size) {
/* each segment follows on one after the other */
/* [ .text ][ .data ][ .bss ] */
/* ^entry ^data_start ^data_end ^bss_end */
/* that means we can copy them all at once */
fseek(fp, header->entry, SEEK_SET);
size_t size_required = header->bss_end - header->entry;
size_t size_to_copy = header->data_end - header->entry;
if (size_required > max_size) error_return("Segment buffer not large enough");
/* zero out memory for bss */
memset( (char*)mem + size_to_copy, 0, size_required - size_to_copy );
if (fread(mem, 1, size_to_copy, fp) == size_to_copy) {
return 0;
}else{
error_return("Could not read all segments");
}
}
int epos_sensor_set_supervision(epos_sensor_t* sensor,
epos_sensor_supervision_t supervision) {
short configuration = epos_device_get_configuration(sensor->dev);
error_return(&sensor->dev->error);
configuration = supervision | (0xFFFC & configuration);
if (!epos_device_set_configuration(sensor->dev, configuration))
sensor->supervision = supervision;
return sensor->dev->error.code;
}
static int process_got(struct flat_hdr * header, void * base) {
uint32_t *got = (uint32_t*)((uint32_t)base + header->data_start - header->entry);
for (; *got != 0xffffffff; got++) {
void* relocd_addr = calc_reloc(*got, header, base);
if (relocd_addr == (void*)-1) {
error_return("Unable to calculate got address");
}
*got = (uint32_t)relocd_addr;
}
return 0;
}
int main(int argc, char **argv)
{
t_all *all;
if (argc > 1)
{
if (!(all = init_check_option(argv)))
return (error_return());
if ((all->a = check_stock(argv)) == NULL)
return (error_return());
if (LENA <= 0)
return (error_return());
if ((all->b = init_b(all->a->len + 1)) == NULL)
return (error_return());
put_state(all, 1);
sort(all);
put_state(all, 2);
if (all->opo == 1)
ft_printf("Operation : %d\n", all->op);
}
else
return (error_return());
return (0);
}
/*==================================================================
* Convert response to velocity first, then to specified units
*=================================================================*/
void convert_to_units(int inp, char *out_units, struct complex *data, double w) {
int out, l;
struct complex scale_val;
/* if default units were specified by the user, no conversion is made,
otherwise convert to unit the user specified. */
if (out_units != NULL && (l=strlen(out_units)) > 0) {
curr_seq_no = -1;
if(!strncmp(out_units, "DEF", 3))
return;
else if(!strncmp(out_units, "DIS", 3)) out = DIS;
else if(!strncmp(out_units, "VEL", 3)) out = VEL;
else if(!strncmp(out_units, "ACC", 3)) out = ACC;
else {
error_return(BAD_OUT_UNITS, "convert_to_units: bad output units");
}
}
else out = VEL;
if (inp == DIS) {
if (out == DIS) return;
if (w != 0.0) {
scale_val.real = 0.0; scale_val.imag = -1.0/w;
zmul(data, &scale_val);
}
else data->real = data->imag = 0.0;
}
else if (inp == ACC) {
if (out == ACC) return;
scale_val.real = 0.0; scale_val.imag = w;
zmul(data, &scale_val);
}
if (out == DIS) {
scale_val.real = 0.0; scale_val.imag = w;
zmul(data, &scale_val);
}
else if (out == ACC) {
if (w != 0.0) {
scale_val.real = 0.0; scale_val.imag = -1.0/w;
zmul(data, &scale_val);
}
else data->real = data->imag = 0.0;
}
}
int bflt_fload(FILE* fp, void **mem_ptr, size_t *mem_size, int (**entry_address_ptr)(int,char*[])) {
void * mem = NULL;
struct flat_hdr header;
info("Begin loading");
if (!fp) error_goto_error("Recieved bad file pointer");
if (read_header(fp, &header) != 0) error_goto_error("Could not parse header");
if (check_header(&header) != 0) error_goto_error("Bad header");
size_t binary_size = header.bss_end - header.entry;
info("Attempting to alloc %u bytes",binary_size);
mem = malloc(binary_size);
if (!mem) error_goto_error("Failed to alloc binary memory");
if (copy_segments(fp, &header, mem, binary_size) != 0) error_goto_error("Failed to copy segments");
if (process_relocs(fp, &header, mem) != 0) error_goto_error("Failed to relocate");
/* only attempt to process GOT if the flags tell us a GOT exists AND
if the Ndless startup file is not already doing so */
if (header.flags & FLAT_FLAG_GOTPIC && memcmp(mem, "PRG\0", 4) != 0) {
if (process_got(&header, mem) != 0) error_goto_error("Failed to process got");
}else{
info("No need to process got - skipping");
}
*mem_ptr = mem;
*mem_size = binary_size;
if (memcmp(mem, "PRG\0", 4) == 0) {
info("Detected as ndless program packaged in a bFLT file");
*entry_address_ptr = (int (*)(int,char*[]))((char*)mem + 4);
}else{
info("Detected as ordinary bFLT executable");
*entry_address_ptr = (int (*)(int,char*[]))mem;
}
info("Successfully loaded bFLT executable to memory");
return 0;
error:
if (mem) free(mem);
*mem_ptr = NULL;
*entry_address_ptr = NULL;
*mem_size = 0;
error_return("Caught error - exiting");
}
/*=================================================================
* Normalize response
*=================================================================*/
void norm_resp(struct channel *chan, int start_stage, int stop_stage) {
struct stage *stage_ptr;
struct blkt *fil, *last_fil, *main_filt;
int i, main_type, reset_gain, skipped_stages = 0;
double w, f;
double percent_diff;
struct complex of, df;
/* -------- TEST 1 -------- */
/*
A single stage response must specify a stage gain, a stage zero
sensitivity, or both. If the gain has been set, simply drop through
to the next test. If the gain is zero, set the gain equal to the
sensitivity and then go to the next test. */
if (chan->nstages == 1 ) { /* has no stage 0, does it have a gain??? */
stage_ptr = chan->first_stage;
fil = stage_ptr->first_blkt;
while(fil != (struct blkt *)NULL && fil->type != GAIN) {
last_fil = fil;
fil = last_fil->next_blkt;
}
if (fil == (struct blkt *)NULL) {
error_return(ILLEGAL_RESP_FORMAT, "norm_resp; no stage gain defined, zero sensitivity");
}
}
else if (chan->nstages == 2 ) { /* has a stage 0??? */
stage_ptr = chan->first_stage;
fil = stage_ptr->first_blkt;
while(fil != (struct blkt *)NULL && fil->type != GAIN) {
last_fil = fil;
fil = last_fil->next_blkt;
}
if (fil == (struct blkt *)NULL) {
if (chan->sensit == 0.0) {
error_return(ILLEGAL_RESP_FORMAT, "norm_resp; no stage gain defined, zero sensitivity");
}
else {
fil = alloc_gain();
fil->blkt_info.gain.gain = chan->sensit;
fil->blkt_info.gain.gain_freq = chan->sensfreq;
last_fil->next_blkt = fil;
}
}
}
/* -------- TEST 2 -------- */
/*
Compute the variable chan->calc_sensit. This value would normally be the
same as chan->sensit (the stage zero sensitivity) if the sensitivity
has been given and all the channel gains are at the sensitivity frequency.
Note that we must verify that each filter gain and normalization is
at the sensitivity frequency, before we can compute the overall
channel sensitivity. If the frequencies are different, calculate the
gain for the stage at the same frequency as the channel sensitivity
frequency.
Note on terminology:
chan->sensit = stage zero sensitivity read from input file
(i.e. the total sensitivity for a given channel).
chan->sensfreq = frequency at which chan-sensit is reported.
chan->calc_sensit = product of the individual stage gains. This is computed
below. The computation is done at the frequency
chan->sensfreq. */
/* Loop thru stages, checking for stage gains of zero */
stage_ptr = chan->first_stage;
for(i = 0; i < chan->nstages; i++) {
fil = stage_ptr->first_blkt;
curr_seq_no = stage_ptr->sequence_no;
while(fil) {
if (fil->type == GAIN && fil->blkt_info.gain.gain == 0.0 ) {
error_return(ILLEGAL_RESP_FORMAT, "norm_resp; zero stage gain");
}
fil = fil->next_blkt;
}
stage_ptr = stage_ptr->next_stage;
}
/*
If necessary, loop thru filters, looking for a non-zero frequency to
use for chan->sensfreq. If the channel sensitivity is defined, then
we will use chan->sensfreq, regardless of whether it is zero or not.
Otherwise, we use the last non-zero filter gain frequency. Since
filters are typically for low pass purposes, the last non-zero
frequency is likely the best choice, as its pass band is the
narrowest. If all the frequencies are zero, then use zero! */
if ( chan->sensit == 0.0 ) {
stage_ptr = chan->first_stage;
for(i = 0; i < chan->nstages; i++) {
fil = stage_ptr->first_blkt;
while(fil) {
if (fil->type == GAIN && fil->blkt_info.gain.gain_freq != 0.0 )
chan->sensfreq = fil->blkt_info.gain.gain_freq;
fil = fil->next_blkt;
}
stage_ptr = stage_ptr->next_stage;
}
}
/*
Loop thru filters, evaluate the filter gains and normalizations at
the single frequency, chan->sensfreq. Use the stage gains to
calculate a total channel sensitivity. */
chan->calc_sensit = 1.0;
f = chan->sensfreq;
w = twoPi * f;
stage_ptr = chan->first_stage;
for(i = 0; i < chan->nstages; i++) {
if(start_stage >= 0 && stop_stage && (stage_ptr->sequence_no < start_stage ||
stage_ptr->sequence_no > stop_stage)) {
if(stage_ptr->sequence_no)
skipped_stages = 1;
stage_ptr = stage_ptr->next_stage;
continue;
}
else if(start_stage >= 0 && !stop_stage && stage_ptr->sequence_no != start_stage) {
if(stage_ptr->sequence_no)
skipped_stages = 1;
stage_ptr = stage_ptr->next_stage;
continue;
}
fil = stage_ptr->first_blkt;
curr_seq_no = stage_ptr->sequence_no;
main_type = 0;
while(fil) {
switch(fil->type) {
case DECIMATION:
case REFERENCE:
break;
case ANALOG_PZ:
case LAPLACE_PZ:
case IIR_PZ:
case FIR_SYM_1:
case FIR_SYM_2:
case FIR_ASYM:
case IIR_COEFFS: /* IGD New type from v 3.2.17 */
main_filt = fil;
main_type = fil->type;
break;
case GAIN:
if(curr_seq_no) {
if((fil->blkt_info.gain.gain_freq != chan->sensfreq) ||
((main_type == ANALOG_PZ ||
main_type == LAPLACE_PZ ||
main_type == IIR_PZ) &&
(main_filt->blkt_info.pole_zero.a0_freq != chan->sensfreq))) {
reset_gain = 1;
if(main_type == ANALOG_PZ || main_type == LAPLACE_PZ) {
main_filt->blkt_info.pole_zero.a0 = 1.0;
analog_trans(main_filt, fil->blkt_info.gain.gain_freq, &df);
if(df.real == 0.0 && df.imag == 0.0) {
error_return(ILLEGAL_FILT_SPEC,
"norm_resp: Gain frequency of zero found in bandpass analog filter");
}
analog_trans(main_filt, f, &of);
if(of.real == 0.0 && of.imag == 0.0) {
error_return(ILLEGAL_FILT_SPEC,
"norm_resp: Chan. Sens. frequency found with bandpass analog filter");
}
}
else if(main_type == IIR_PZ) {
main_filt->blkt_info.pole_zero.a0 = 1.0;
iir_pz_trans(main_filt, twoPi * fil->blkt_info.gain.gain_freq, &df);
iir_pz_trans(main_filt, w, &of);
}
else if((main_type == FIR_SYM_1 || main_type == FIR_SYM_2) &&
main_filt->blkt_info.fir.ncoeffs) {
main_filt->blkt_info.fir.h0 = 1.0;
fir_sym_trans(main_filt, twoPi * fil->blkt_info.gain.gain_freq, &df);
fir_sym_trans(main_filt, w, &of);
}
else if(main_type == FIR_ASYM && main_filt->blkt_info.fir.ncoeffs) {
main_filt->blkt_info.fir.h0 = 1.0;
fir_asym_trans(main_filt, twoPi * fil->blkt_info.gain.gain_freq, &df);
fir_asym_trans(main_filt, w, &of);
}
else if(main_type == IIR_COEFFS) { /*IGD - new case for 3.2.17 */
main_filt->blkt_info.coeff.h0 = 1.0;
iir_trans(main_filt, twoPi * fil->blkt_info.gain.gain_freq, &df);
iir_trans(main_filt, w, &of);
}
else
reset_gain = 0;
if(reset_gain) {
fil->blkt_info.gain.gain /= sqrt(df.real*df.real + df.imag*df.imag);
fil->blkt_info.gain.gain *= sqrt(of.real*of.real + of.imag*of.imag);
fil->blkt_info.gain.gain_freq = f;
if(main_type == ANALOG_PZ || main_type == LAPLACE_PZ ||
main_type == IIR_PZ) {
main_filt->blkt_info.pole_zero.a0 = 1.0 / sqrt(of.real*of.real + of.imag*of.imag);
main_filt->blkt_info.pole_zero.a0_freq = f;
}
else if(main_type == FIR_SYM_1 || main_type == FIR_SYM_2 || main_type == FIR_ASYM) {
main_filt->blkt_info.fir.h0 = 1.0 / sqrt(of.real*of.real + of.imag*of.imag);
}
else if(main_type == IIR_COEFFS) {
main_filt->blkt_info.coeff.h0 = 1.0 / sqrt(of.real*of.real + of.imag*of.imag);
}
}
}
/* compute new overall sensitivity. */
chan->calc_sensit *= fil->blkt_info.gain.gain;
if(chan->nstages == 1) {
chan->sensit = chan->calc_sensit;
}
}
break;
default:
break;
}
fil = fil->next_blkt;
}
stage_ptr = stage_ptr->next_stage;
}
/* -------- TEST 3 -------- */
/* Finally, print a warning, if necessary */
if(!skipped_stages && chan->sensit != 0.0) {
percent_diff = fabs((chan->sensit - chan->calc_sensit) / chan->sensit);
if (percent_diff >= 0.05) {
fprintf(stderr,"%s WARNING (norm_resp): computed and reported sensitivities", myLabel);
fprintf(stderr,"%s differ by more than 5 percent. \n", myLabel);
fprintf(stderr,"%s\t Execution continuing.\n", myLabel);
fflush(stderr);
}
}
}
/*=================================================================
* Calculate response
*=================================================================*/
void calc_resp(struct channel *chan, double *freq, int nfreqs, struct complex *output,
char *out_units, int start_stage, int stop_stage, int useTotalSensitivityFlag) {
struct blkt *blkt_ptr;
struct stage *stage_ptr;
int i, j, units_code, eval_flag = 0, nc = 0, sym_fir = 0;
double w;
int matching_stages = 0, has_stage0 = 0, deciStageEvaluated = 0;
struct complex of, val;
double corr_applied, estim_delay, delay;
/* if(start_stage && start_stage > chan->nstages) {
error_return(NO_STAGE_MATCHED, "calc_resp: %s start_stage=%d, highest stage found=%d)",
"No Matching Stages Found (requested",start_stage, chan->nstages);
} */
/* for each frequency */
for(i = 0; i < nfreqs; i++) {
w = twoPi * freq[i];
val.real = 1.0; val.imag = 0.0;
/* loop through the stages and filters for each stage, calculating
the response for each frequency for all stages */
stage_ptr = chan->first_stage;
units_code = stage_ptr->input_units;
for(j = 0; j < chan->nstages; j++) {
nc = 0;
sym_fir = 0;
deciStageEvaluated = 0;
if(!stage_ptr->sequence_no)
has_stage0 = 1;
if(start_stage >= 0 && stop_stage && (stage_ptr->sequence_no < start_stage ||
stage_ptr->sequence_no > stop_stage)) {
stage_ptr = stage_ptr->next_stage;
continue;
}
else if(start_stage >= 0 && !stop_stage && stage_ptr->sequence_no != start_stage) {
stage_ptr = stage_ptr->next_stage;
continue;
}
matching_stages++;
blkt_ptr = stage_ptr->first_blkt;
while(blkt_ptr) {
eval_flag = 0;
switch(blkt_ptr->type) {
case ANALOG_PZ:
case LAPLACE_PZ:
analog_trans(blkt_ptr, freq[i], &of);
eval_flag = 1;
break;
case IIR_PZ:
if(blkt_ptr->blkt_info.pole_zero.nzeros || blkt_ptr->blkt_info.pole_zero.npoles) {
iir_pz_trans(blkt_ptr, w, &of);
eval_flag = 1;
}
break;
case FIR_SYM_1:
case FIR_SYM_2:
if(blkt_ptr->type == FIR_SYM_1)
nc = (double) blkt_ptr->blkt_info.fir.ncoeffs*2 - 1;
else if(blkt_ptr->type == FIR_SYM_2)
nc = (double) blkt_ptr->blkt_info.fir.ncoeffs*2;
if(blkt_ptr->blkt_info.fir.ncoeffs) {
fir_sym_trans(blkt_ptr, w, &of);
sym_fir = 1;
eval_flag = 1;
}
break;
case FIR_ASYM:
nc = (double) blkt_ptr->blkt_info.fir.ncoeffs;
if(blkt_ptr->blkt_info.fir.ncoeffs) {
fir_asym_trans(blkt_ptr, w, &of);
sym_fir = -1;
eval_flag = 1;
}
break;
case DECIMATION:
if(nc != 0) {
/* IGD 08/27/08 Use estimated delay instead of calculated */
estim_delay = (double) blkt_ptr->blkt_info.decimation.estim_delay;
corr_applied = blkt_ptr->blkt_info.decimation.applied_corr;
/* Asymmetric FIR coefficients require a delay correction */
if ( sym_fir == -1 ) {
if (TRUE == use_delay(QUERY_DELAY))
delay = estim_delay;
else
delay = corr_applied;
}
/* Otherwise delay has already been handled in fir_sym_trans() */
else {
delay = 0;
}
calc_time_shift (delay, w, &of);
eval_flag = 1;
}
break;
case LIST: /* This option is added in version 2.3.17 I.Dricker*/
calc_list (blkt_ptr, i, &of); /*compute real and imag parts for the i-th ampl and phase */
eval_flag = 1;
break;
case IIR_COEFFS: /* This option is added in version 2.3.17 I.Dricker*/
iir_trans(blkt_ptr, w, &of);
eval_flag = 1;
break;
default:
break;
}
if(eval_flag)
zmul(&val, &of);
blkt_ptr = blkt_ptr->next_blkt;
}
stage_ptr = stage_ptr->next_stage;
}
/* if no matching stages were found, then report the error */
if(!matching_stages && !has_stage0) {
error_return(NO_STAGE_MATCHED, "calc_resp: %s start_stage=%d, highest stage found=%d)",
"No Matching Stages Found (requested",start_stage, chan->nstages);
}
else if(!matching_stages) {
error_return(NO_STAGE_MATCHED, "calc_resp: %s start_stage=%d, highest stage found=%d)",
"No Matching Stages Found (requested",start_stage, chan->nstages-1);
}
/* Write output for freq[i] in output[i] (note: unitScaleFact is a global variable
set by the 'check_units' function that is used to convert to 'MKS' units when the
the response was given as a displacement, velocity, or acceleration in units other
than meters) */
if (0 == useTotalSensitivityFlag) {
output[i].real = val.real * chan->calc_sensit * unitScaleFact;
output[i].imag = val.imag * chan->calc_sensit * unitScaleFact;
}
else {
output[i].real = val.real * chan->sensit * unitScaleFact;
output[i].imag = val.imag * chan->sensit * unitScaleFact;
}
convert_to_units(units_code, out_units, &output[i], w);
}
}