From 7a19aac5e881e635962a64fff73027ca2143b96f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 6 Sep 2009 12:51:48 -0700 Subject: [PATCH] Add DSP code to filter data, allowing for integration/differentiation This adds the computation of speed from both accelerometer and barometer measurements and then presents a periodic flight profile using filtered data as a detailed flight record. Signed-off-by: Keith Packard --- ao-tools/ao-postflight/ao-postflight.c | 135 ++++++-- ao-tools/lib/Makefile.am | 10 +- ao-tools/lib/cc-analyse.c | 57 ++++ ao-tools/lib/cc-dsp.c | 145 +++++++++ ao-tools/lib/cc-integrate.c | 78 +++++ ao-tools/lib/cc-period.c | 64 ++++ ao-tools/lib/cc-process.c | 140 +++++++++ ao-tools/lib/cc.h | 28 ++ ao-tools/lib/cephes.h | 122 ++++++++ ao-tools/lib/chbevl.c | 81 +++++ ao-tools/lib/cmath.h | 179 +++++++++++ ao-tools/lib/i0.c | 414 +++++++++++++++++++++++++ ao-tools/lib/mconf.h | 211 +++++++++++++ 13 files changed, 1639 insertions(+), 25 deletions(-) create mode 100644 ao-tools/lib/cc-dsp.c create mode 100644 ao-tools/lib/cc-integrate.c create mode 100644 ao-tools/lib/cc-period.c create mode 100644 ao-tools/lib/cc-process.c create mode 100644 ao-tools/lib/cephes.h create mode 100644 ao-tools/lib/chbevl.c create mode 100644 ao-tools/lib/cmath.h create mode 100644 ao-tools/lib/i0.c create mode 100644 ao-tools/lib/mconf.h diff --git a/ao-tools/ao-postflight/ao-postflight.c b/ao-tools/ao-postflight/ao-postflight.c index 9371f351..c5814c93 100644 --- a/ao-tools/ao-postflight/ao-postflight.c +++ b/ao-tools/ao-postflight/ao-postflight.c @@ -27,16 +27,6 @@ #define NUM_BLOCK 512 -static const struct option options[] = { - { 0, 0, 0, 0}, -}; - -static void usage(char *program) -{ - fprintf(stderr, "usage: %s {flight-log} ...\n", program); - exit(1); -} - static const char *state_names[] = { "startup", "idle", @@ -51,20 +41,23 @@ static const char *state_names[] = { }; void -analyse_flight(struct cc_flightraw *f) +analyse_flight(struct cc_flightraw *f, FILE *summary_file, FILE *detail_file) { double height; double accel; + double speed; double boost_start, boost_stop; double min_pres; int i; - int pres_i, accel_i; + int pres_i, accel_i, speed_i; int boost_start_set = 0; int boost_stop_set = 0; enum ao_flight_state state; double state_start, state_stop; + struct cc_flightcooked *cooked; + double apogee; - printf ("Flight: %9d\nSerial: %9d\n", + fprintf(summary_file, "Flight: %9d\nSerial: %9d\n", f->flight, f->serial); boost_start = f->accel.data[0].time; boost_stop = f->accel.data[f->accel.num-1].time; @@ -81,25 +74,37 @@ analyse_flight(struct cc_flightraw *f) pres_i = cc_timedata_min(&f->pres, f->pres.data[0].time, f->pres.data[f->pres.num-1].time); - if (pres_i) + if (pres_i >= 0) { min_pres = f->pres.data[pres_i].value; height = cc_barometer_to_altitude(min_pres) - cc_barometer_to_altitude(f->ground_pres); - printf ("Max height: %9.2fm %9.2fft %9.2fs\n", + fprintf(summary_file, "Max height: %9.2fm %9.2fft %9.2fs\n", height, height * 100 / 2.54 / 12, (f->pres.data[pres_i].time - boost_start) / 100.0); + apogee = f->pres.data[pres_i].time; } + cooked = cc_flight_cook(f); + if (cooked) { + speed_i = cc_perioddata_max(&cooked->accel_speed, boost_start, boost_stop); + if (speed_i >= 0) { + speed = cooked->accel_speed.data[speed_i]; + fprintf(summary_file, "Max speed: %9.2fm/s %9.2fft/s %9.2fs\n", + speed, speed * 100 / 2.4 / 12.0, + (cooked->accel_speed.start + speed_i * cooked->accel_speed.step - boost_start) / 100.0); + } + } accel_i = cc_timedata_min(&f->accel, boost_start, boost_stop); - if (accel_i) + if (accel_i >= 0) { accel = cc_accelerometer_to_acceleration(f->accel.data[accel_i].value, f->ground_accel); - printf ("Max accel: %9.2fm/s² %9.2fg %9.2fs\n", + fprintf(summary_file, "Max accel: %9.2fm/s² %9.2fg %9.2fs\n", accel, accel / 9.80665, (f->accel.data[accel_i].time - boost_start) / 100.0); } + for (i = 0; i < f->state.num; i++) { state = f->state.data[i].value; state_start = f->state.data[i].time; @@ -109,50 +114,132 @@ analyse_flight(struct cc_flightraw *f) state_stop = f->state.data[i + 1].time; else state_stop = f->accel.data[f->accel.num-1].time; - printf("State: %s\n", state_names[state]); - printf("\tStart: %9.2fs\n", (state_start - boost_start) / 100.0); - printf("\tDuration: %9.2fs\n", (state_stop - state_start) / 100.0); + fprintf(summary_file, "State: %s\n", state_names[state]); + fprintf(summary_file, "\tStart: %9.2fs\n", (state_start - boost_start) / 100.0); + fprintf(summary_file, "\tDuration: %9.2fs\n", (state_stop - state_start) / 100.0); accel_i = cc_timedata_min(&f->accel, state_start, state_stop); if (accel_i >= 0) { accel = cc_accelerometer_to_acceleration(f->accel.data[accel_i].value, f->ground_accel); - printf("\tMax accel: %9.2fm/s² %9.2fg %9.2fs\n", + fprintf(summary_file, "\tMax accel: %9.2fm/s² %9.2fg %9.2fs\n", accel, accel / 9.80665, (f->accel.data[accel_i].time - boost_start) / 100.0); } + if (cooked) { + if (state_start < apogee) { + speed_i = cc_perioddata_max(&cooked->accel_speed, state_start, state_stop); + if (speed_i >= 0) + speed = cooked->accel_speed.data[speed_i]; + } else { + speed_i = cc_perioddata_max(&cooked->pres_speed, state_start, state_stop); + if (speed_i >= 0) + speed = cooked->pres_speed.data[speed_i]; + } + if (speed_i >= 0) + fprintf(summary_file, "\tMax speed: %9.2fm/s %9.2fft/s %9.2fs\n", + speed, speed * 100 / 2.4 / 12.0, + (cooked->accel_speed.start + speed_i * cooked->accel_speed.step - boost_start) / 100.0); + } pres_i = cc_timedata_min(&f->pres, state_start, state_stop); if (pres_i >= 0) { min_pres = f->pres.data[pres_i].value; height = cc_barometer_to_altitude(min_pres) - cc_barometer_to_altitude(f->ground_pres); - printf ("\tMax height: %9.2fm %9.2fft %9.2fs\n", + fprintf(summary_file, "\tMax height: %9.2fm %9.2fft %9.2fs\n", height, height * 100 / 2.54 / 12, (f->pres.data[pres_i].time - boost_start) / 100.0); } } + if (cooked && detail_file) { + double apogee_time; + double max_height = 0; + int i; + + for (i = 0; i < cooked->pres_pos.num; i++) { + if (cooked->pres_pos.data[i] > max_height) { + max_height = cooked->pres_pos.data[i]; + apogee_time = cooked->pres_pos.start + cooked->pres_pos.step * i; + } + } + fprintf(detail_file, "%9s %9s %9s %9s\n", + "time", "height", "speed", "accel"); + for (i = 0; i < cooked->pres_pos.num; i++) { + double time = (cooked->accel_accel.start + i * cooked->accel_accel.step - boost_start) / 100.0; + double accel = cooked->accel_accel.data[i]; + double pos = cooked->pres_pos.data[i]; + double speed; + if (cooked->pres_pos.start + cooked->pres_pos.step * i < apogee_time) + speed = cooked->accel_speed.data[i]; + else + speed = cooked->pres_speed.data[i]; + fprintf(detail_file, "%9.2f %9.2f %9.2f %9.2f\n", + time, pos, speed, accel); + } + } +} + +static const struct option options[] = { + { .name = "summary", .has_arg = 1, .val = 'S' }, + { .name = "detail", .has_arg = 1, .val = 'D' }, + { 0, 0, 0, 0}, +}; + +static void usage(char *program) +{ + fprintf(stderr, "usage: %s [--summary=] [--detail=serial) raw->serial = serial; - analyse_flight(raw); + analyse_flight(raw, summary_file, detail_file); cc_flightraw_free(raw); } return ret; diff --git a/ao-tools/lib/Makefile.am b/ao-tools/lib/Makefile.am index e682f757..79972f46 100644 --- a/ao-tools/lib/Makefile.am +++ b/ao-tools/lib/Makefile.am @@ -16,7 +16,11 @@ libao_tools_a_SOURCES = \ ccdbg-state.c \ cc-analyse.c \ cc-convert.c \ + cc-dsp.c \ cc-log.c \ + cc-integrate.c \ + cc-period.c \ + cc-process.c \ cc-usb.c \ cc-usb.h \ cc.h \ @@ -27,4 +31,8 @@ libao_tools_a_SOURCES = \ cc-logfile.c \ cc-telem.c \ cp-usb-async.c \ - cp-usb-async.h + cp-usb-async.h \ + i0.c \ + chbevl.c \ + mconf.h \ + cephes.h diff --git a/ao-tools/lib/cc-analyse.c b/ao-tools/lib/cc-analyse.c index fc8a8417..0e020115 100644 --- a/ao-tools/lib/cc-analyse.c +++ b/ao-tools/lib/cc-analyse.c @@ -16,6 +16,7 @@ */ #include "cc.h" +#include int cc_timedata_min(struct cc_timedata *d, double min_time, double max_time) @@ -56,3 +57,59 @@ cc_timedata_max(struct cc_timedata *d, double min_time, double max_time) } return max_i; } + +int +cc_perioddata_min(struct cc_perioddata *d, double min_time, double max_time) +{ + int start, stop; + int i; + double min; + int min_i; + + if (d->num == 0) + return -1; + start = (int) ceil((min_time - d->start) / d->step); + if (start < 0) + start = 0; + stop = (int) floor((max_time - d->start) / d->step); + if (stop >= d->num) + stop = d->num - 1; + if (stop < start) + return -1; + min = d->data[start]; + min_i = start; + for (i = start + 1; i <= stop; i++) + if (d->data[i] < min) { + min = d->data[i]; + min_i = i; + } + return min_i; +} + +int +cc_perioddata_max(struct cc_perioddata *d, double min_time, double max_time) +{ + int start, stop; + int i; + double max; + int max_i; + + if (d->num == 0) + return -1; + start = (int) ceil((min_time - d->start) / d->step); + if (start < 0) + start = 0; + stop = (int) floor((max_time - d->start) / d->step); + if (stop >= d->num) + stop = d->num - 1; + if (stop < start) + return -1; + max = d->data[start]; + max_i = start; + for (i = start + 1; i <= stop; i++) + if (fabs(d->data[i]) > max) { + max = fabs(d->data[i]); + max_i = i; + } + return max_i; +} diff --git a/ao-tools/lib/cc-dsp.c b/ao-tools/lib/cc-dsp.c new file mode 100644 index 00000000..518c1a68 --- /dev/null +++ b/ao-tools/lib/cc-dsp.c @@ -0,0 +1,145 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "cc.h" +#include "cephes.h" +#include +#include + +static inline double sqr (double x) { return x * x; } + +/* + * Kaiser Window digital filter + */ + +#if 0 +/* not used in this program */ +static double highpass (double n, double m, double wc) +{ + double alpha = m/2; + double dist; + + dist = n - alpha; + if (dist == 0) + return (M_PI/2 - wc) / M_PI; + return -sin(dist * (M_PI/2-wc)) / (M_PI * dist); +} +#endif + +static double lowpass (double n, double m, double wc) +{ + double alpha = m/2; + double dist; + dist = n - alpha; + if (dist == 0) + return wc / M_PI; + return sin (wc * dist) / (M_PI * dist); +} + +static double kaiser (double n, double m, double beta) +{ + double alpha = m / 2; + return i0 (beta * sqrt (1 - sqr((n - alpha) / alpha))) / i0(beta); +} + +static double beta (double A) +{ + if (A > 50) + return 0.1102 * (A - 8.7); + else if (A >= 21) + return 0.5842 * pow((A - 21), 0.4) + 0.07886 * (A - 21); + else + return 0.0; +} + +static int M (double A, double delta_omega) +{ + if (A > 21) + return ceil ((A - 7.95) / (2.285 * delta_omega)); + else + return ceil(5.79 / delta_omega); +} + +struct filter_param { + double omega_pass; + double delta_omega; + double A; + double beta; + int M; +} filter_param_t; + +static struct filter_param +filter (double omega_pass, double omega_stop, double error) +{ + struct filter_param p; + p.omega_pass = omega_pass; + p.delta_omega = omega_stop - omega_pass; + p.A = -20 * log10 (error); + p.beta = beta (p.A); + p.M = M (p.A, p.delta_omega); + if ((p.M & 1) == 1) + p.M++; + return p; +} + +static double * +make_low_pass_filter(double omega_pass, double omega_stop, double error, int *length_p) +{ + struct filter_param p = filter(omega_pass, omega_stop, error); + int length; + int n; + double *lpf; + + length = p.M + 1; + lpf = calloc (length, sizeof(double)); + for (n = 0; n < length; n++) + lpf[n] = lowpass(n, p.M, omega_pass) * kaiser(n, p.M, p.beta); + *length_p = length; + return lpf; +} + +static double * +convolve(double *d, int d_len, double *e, int e_len) +{ + int w = (e_len - 1) / 2; + int n; + double *con = calloc (d_len, sizeof (double)); + + for (n = 0; n < d_len; n++) { + double v = 0; + int o; + for (o = -w; o <= w; o++) { + int p = n + o; + double sample = p < 0 ? d[0] : p >= d_len ? d[d_len-1] : d[p]; + v += sample * e[o + w]; + } + con[n] = v; + } + return con; +} + +double * +cc_low_pass(double *data, int data_len, double omega_pass, double omega_stop, double error) +{ + int fir_len; + double *fir = make_low_pass_filter(omega_pass, omega_stop, error, &fir_len); + double *result; + + result = convolve(data, data_len, fir, fir_len); + free(fir); + return result; +} diff --git a/ao-tools/lib/cc-integrate.c b/ao-tools/lib/cc-integrate.c new file mode 100644 index 00000000..08ca295c --- /dev/null +++ b/ao-tools/lib/cc-integrate.c @@ -0,0 +1,78 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "cc.h" +#include + +struct cc_timedata * +cc_timedata_convert(struct cc_timedata *d, double (*f)(double v, double a), double a) +{ + struct cc_timedata *r; + int n; + + r = calloc (1, sizeof (struct cc_timedata)); + r->num = d->num; + r->size = d->num; + r->data = calloc (r->size, sizeof (struct cc_timedataelt)); + r->time_offset = d->time_offset; + for (n = 0; n < d->num; n++) { + r->data[n].time = d->data[n].time; + r->data[n].value = f(d->data[n].value, a); + } + return r; +} + +struct cc_timedata * +cc_timedata_integrate(struct cc_timedata *d) +{ + struct cc_timedata *i; + int n; + + i = calloc (1, sizeof (struct cc_timedata)); + i->num = d->num; + i->size = d->num; + i->data = calloc (i->size, sizeof (struct cc_timedataelt)); + i->time_offset = d->time_offset; + for (n = 0; n < d->num; n++) { + i->data[n].time = d->data[n].time; + if (n == 0) { + i->data[n].value = 0; + } else { + i->data[n].value = i->data[n-1].value + + (d->data[n].value + d->data[n-1].value) / 2 * + ((d->data[n].time - d->data[n-1].time) / 100.0); + } + } + return i; +} + +struct cc_perioddata * +cc_perioddata_differentiate(struct cc_perioddata *i) +{ + struct cc_perioddata *d; + int n; + + d = calloc (1, sizeof (struct cc_perioddata)); + d->num = i->num; + d->start = i->start; + d->step = i->step; + d->data = calloc (d->num, sizeof(double)); + for (n = 1; n < d->num; n++) + d->data[n] = (i->data[n] - i->data[n-1]) / i->step; + d->data[0] = d->data[1]; + return d; +} diff --git a/ao-tools/lib/cc-period.c b/ao-tools/lib/cc-period.c new file mode 100644 index 00000000..c74cf9dc --- /dev/null +++ b/ao-tools/lib/cc-period.c @@ -0,0 +1,64 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "cc.h" +#include + +struct cc_perioddata * +cc_period_make(struct cc_timedata *td, double start_time, double stop_time) +{ + int len = stop_time - start_time + 1; + struct cc_perioddata *pd; + int i; + double prev_time; + double next_time; + double interval; + + pd = calloc(1, sizeof (struct cc_perioddata)); + pd->start = start_time; + pd->step = 1; + pd->num = len; + pd->data = calloc(len, sizeof(double)); + prev_time = start_time; + for (i = 0; i < td->num; i++) { + if (start_time <= td->data[i].time && td->data[i].time <= stop_time) { + int pos = td->data[i].time - start_time; + + if (i < td->num - 1 && td->data[i+1].time < stop_time) + next_time = (td->data[i].time + td->data[i+1].time) / 2.0; + else + next_time = stop_time; + interval = next_time - prev_time; + pd->data[pos] = td->data[i].value * interval; + prev_time = next_time; + } + } + return pd; +} + +struct cc_perioddata * +cc_period_low_pass(struct cc_perioddata *raw, double omega_pass, double omega_stop, double error) +{ + struct cc_perioddata *filtered; + + filtered = calloc (1, sizeof (struct cc_perioddata)); + filtered->start = raw->start; + filtered->step = raw->step; + filtered->num = raw->num; + filtered->data = cc_low_pass(raw->data, raw->num, omega_pass, omega_stop, error); + return filtered; +} diff --git a/ao-tools/lib/cc-process.c b/ao-tools/lib/cc-process.c new file mode 100644 index 00000000..e906b635 --- /dev/null +++ b/ao-tools/lib/cc-process.c @@ -0,0 +1,140 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "cc.h" +#include +#include +#include + +static void +cook_timed(struct cc_timedata *td, struct cc_perioddata *pd, + double start_time, double stop_time, + double omega_pass, double omega_stop, double error) +{ + struct cc_perioddata *unfiltered, *filtered; + + unfiltered = cc_period_make(td, start_time, stop_time); + filtered = cc_period_low_pass (unfiltered, omega_pass, omega_stop, error); + *pd = *filtered; + free (filtered); + free (unfiltered->data); + free (unfiltered); + free (td->data); + free (td); +} + +static double +barometer_to_altitude(double b, double pad_alt) +{ + return cc_barometer_to_altitude(b) - pad_alt; +} + +struct cc_flightcooked * +cc_flight_cook(struct cc_flightraw *raw) +{ + struct cc_flightcooked *cooked; + double flight_start; + double flight_stop; + int start_set = 0; + int stop_set = 0; + int i; + struct cc_timedata *accel; + struct cc_timedata *accel_speed; + struct cc_timedata *accel_pos; + struct cc_timedata *pres; + struct cc_perioddata *pres_speed; + struct cc_perioddata *pres_accel; + + if (raw->accel.num == 0) + return NULL; + + cooked = calloc (1, sizeof (struct cc_flightcooked)); + + /* + * Find flight start and stop times by looking at + * state transitions. The stop time is set to the time + * of landing, which may be long after it landed (due to radio + * issues). Refine this value by looking through the sensor data + */ + for (i = 0; i < raw->state.num; i++) { + if (!start_set && raw->state.data[i].value > ao_flight_pad) { + flight_start = raw->state.data[i].time; + start_set = 1; + } + if (!stop_set && raw->state.data[i].value > ao_flight_main) { + flight_stop = raw->state.data[i].time; + stop_set = 1; + } + } + + if (!start_set) + flight_start = raw->accel.data[0].time; + if (stop_set) { + for (i = 0; i < raw->accel.num - 1; i++) { + if (raw->accel.data[i+1].time >= flight_stop) { + flight_stop = raw->accel.data[i].time; + break; + } + } + } else { + flight_stop = raw->accel.data[raw->accel.num-1].time; + } + + /* Integrate the accelerometer data to get speed and position */ + accel = cc_timedata_convert(&raw->accel, cc_accelerometer_to_acceleration, raw->ground_accel); + accel_speed = cc_timedata_integrate(accel); + accel_pos = cc_timedata_integrate(accel_speed); + +#define ACCEL_OMEGA_PASS (2 * M_PI * 5 / 100) +#define ACCEL_OMEGA_STOP (2 * M_PI * 8 / 100) +#define BARO_OMEGA_PASS (2 * M_PI * .5 / 100) +#define BARO_OMEGA_STOP (2 * M_PI * 1 / 100) +#define FILTER_ERROR (1e-8) + + cook_timed(accel, &cooked->accel_accel, + flight_start, flight_stop, + ACCEL_OMEGA_PASS, ACCEL_OMEGA_STOP, FILTER_ERROR); + cook_timed(accel_speed, &cooked->accel_speed, + flight_start, flight_stop, + ACCEL_OMEGA_PASS, ACCEL_OMEGA_STOP, FILTER_ERROR); + cook_timed(accel_pos, &cooked->accel_pos, + flight_start, flight_stop, + ACCEL_OMEGA_PASS, ACCEL_OMEGA_STOP, FILTER_ERROR); + + /* Filter the pressure data */ + pres = cc_timedata_convert(&raw->pres, barometer_to_altitude, + cc_barometer_to_altitude(raw->ground_pres)); + cook_timed(pres, &cooked->pres_pos, + flight_start, flight_stop, + BARO_OMEGA_PASS, BARO_OMEGA_STOP, FILTER_ERROR); + /* differentiate twice to get to acceleration */ + pres_speed = cc_perioddata_differentiate(&cooked->pres_pos); + pres_accel = cc_perioddata_differentiate(pres_speed); + + cooked->pres_speed = *pres_speed; + free(pres_speed); + cooked->pres_accel = *pres_accel; + free(pres_accel); + + /* copy state */ + cooked->state.num = raw->state.num; + cooked->state.size = raw->state.num; + cooked->state.data = calloc(cooked->state.num, sizeof (struct cc_timedataelt)); + memcpy(cooked->state.data, raw->state.data, cooked->state.num * sizeof (struct cc_timedataelt)); + cooked->state.time_offset = raw->state.time_offset; + return cooked; +} diff --git a/ao-tools/lib/cc.h b/ao-tools/lib/cc.h index 57f80b8d..356794e0 100644 --- a/ao-tools/lib/cc.h +++ b/ao-tools/lib/cc.h @@ -268,4 +268,32 @@ cc_timedata_min(struct cc_timedata *d, double min_time, double max_time); int cc_timedata_max(struct cc_timedata *d, double min_time, double max_time); +int +cc_perioddata_min(struct cc_perioddata *d, double min_time, double max_time); + +int +cc_perioddata_max(struct cc_perioddata *d, double min_time, double max_time); + +double * +cc_low_pass(double *data, int data_len, double omega_pass, double omega_stop, double error); + +struct cc_perioddata * +cc_period_make(struct cc_timedata *td, double start_time, double stop_time); + +struct cc_perioddata * +cc_period_low_pass(struct cc_perioddata *raw, double omega_pass, double omega_stop, double error); + +struct cc_timedata * +cc_timedata_convert(struct cc_timedata *d, double (*f)(double v, double a), double a); + +struct cc_timedata * +cc_timedata_integrate(struct cc_timedata *d); + +struct cc_perioddata * +cc_perioddata_differentiate(struct cc_perioddata *i); + +struct cc_flightcooked * +cc_flight_cook(struct cc_flightraw *raw); + + #endif /* _CC_H_ */ diff --git a/ao-tools/lib/cephes.h b/ao-tools/lib/cephes.h new file mode 100644 index 00000000..f8ec264f --- /dev/null +++ b/ao-tools/lib/cephes.h @@ -0,0 +1,122 @@ +/* + * This file comes from the cephes math library, which was + * released under the GPLV2+ license as a part of the Debian labplot + * package (I've included the GPLV2 license reference here to make + * this clear) - Keith Packard + * + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ +/* + * Prototypes of Cephes functions + */ + +#ifndef _CEPHES_H_ +#define _CEPHES_H_ + +/* Variable for error reporting. See mtherr.c. */ +extern int merror; + +#if 0 +extern int airy ( double x, double *ai, double *aip, double *bi, double *bip ); +extern double beta ( double a, double b ); +extern double lbeta ( double a, double b ); +extern double chdtrc ( double df, double x ); +extern double chdtr ( double df, double x ); +extern double chdtri ( double df, double y ); +extern double dawsn ( double xx ); +extern double ellie ( double phi, double m ); +extern double ellik ( double phi, double m ); +extern double ellpe ( double x ); +extern double ellpk ( double x ); +extern double expn ( int n, double x ); +extern double fac ( int i ); +extern double fdtrc ( int ia, int ib, double x ); +extern double fdtr ( int ia, int ib, double x ); +extern double fdtri ( int ia, int ib, double y ); +extern double frexp ( double x, int *pw2 ); +extern double ldexp ( double x, int pw2 ); +extern int fresnl ( double xxa, double *ssa, double *cca ); +extern double gdtr ( double a, double b, double x ); +extern double gdtrc ( double a, double b, double x ); +extern double hyp2f0 ( double a, double b, double x, int type, double *err ); +extern double hyp2f1 ( double a, double b, double c, double x ); +extern double hyperg ( double a, double b, double x ); +#endif +extern double i0 ( double x ); +extern double i0e ( double x ); +#if 0 +extern double i1 ( double x ); +extern double i1e ( double x ); +extern double iv ( double v, double x ); +extern double igamc ( double a, double x ); +extern double igam ( double a, double x ); +extern double igami ( double a, double y0_ ); +extern double incbet ( double aa, double bb, double xx ); +extern double incbi ( double aa, double bb, double yy0 ); +extern double jv ( double n, double x ); +extern double k0 ( double x ); +extern double k0e ( double x ); +extern double k1 ( double x ); +extern double k1e ( double x ); +extern double kn ( int nn, double x ); +extern int mtherr ( char *name, int code ); +extern double ndtr ( double a ); +extern double ndtri ( double y0_ ); +extern double pdtrc ( int k, double m ); +extern double pdtr ( int k, double m ); +extern double pdtri ( int k, double y ); +extern double psi ( double x ); +extern void revers ( double y[], double x[], int n ); +extern double true_gamma ( double x ); +extern double rgamma ( double x ); +extern int shichi ( double x, double *si, double *ci ); +extern int sici ( double x, double *si, double *ci ); +extern double spence ( double x ); +extern double stdtr ( int k, double t ); +extern double stdtri ( int k, double p ); +extern double onef2 ( double a, double b, double c, double x, double *err ); +extern double threef0 ( double a, double b, double c, double x, double *err ); +extern double struve ( double v, double x ); +extern double log1p ( double x ); +extern double expm1 ( double x ); +extern double cosm1 ( double x ); +extern double yv ( double v, double x ); +extern double zeta ( double x, double q ); +extern double zetac ( double x ); + +#endif +extern double chbevl ( double x, void *P, int n ); +#if 0 +extern double polevl ( double x, void *P, int n ); +extern double p1evl ( double x, void *P, int n ); + +/* polyn.c */ +extern void polini ( int maxdeg ); +extern void polprt ( double a[], int na, int d ); +extern void polclr ( double *a, int n ); +extern void polmov ( double *a, int na, double *b ); +extern void polmul ( double a[], int na, double b[], int nb, double c[] ); +extern void poladd ( double a[], int na, double b[], int nb, double c[] ); +extern void polsub ( double a[], int na, double b[], int nb, double c[] ); +extern int poldiv ( double a[], int na, double b[], int nb, double c[] ); +extern void polsbt ( double a[], int na, double b[], int nb, double c[] ); +extern double poleva ( double a[], int na, double x ); + +#endif + +#endif /* _CEPHES_H_ */ diff --git a/ao-tools/lib/chbevl.c b/ao-tools/lib/chbevl.c new file mode 100644 index 00000000..22892413 --- /dev/null +++ b/ao-tools/lib/chbevl.c @@ -0,0 +1,81 @@ +/* chbevl.c + * + * Evaluate Chebyshev series + * + * + * + * SYNOPSIS: + * + * int N; + * double x, y, coef[N], chebevl(); + * + * y = chbevl( x, coef, N ); + * + * + * + * DESCRIPTION: + * + * Evaluates the series + * + * N-1 + * - ' + * y = > coef[i] T (x/2) + * - i + * i=0 + * + * of Chebyshev polynomials Ti at argument x/2. + * + * Coefficients are stored in reverse order, i.e. the zero + * order term is last in the array. Note N is the number of + * coefficients, not the order. + * + * If coefficients are for the interval a to b, x must + * have been transformed to x -> 2(2x - b - a)/(b-a) before + * entering the routine. This maps x from (a, b) to (-1, 1), + * over which the Chebyshev polynomials are defined. + * + * If the coefficients are for the inverted interval, in + * which (a, b) is mapped to (1/b, 1/a), the transformation + * required is x -> 2(2ab/x - b - a)/(b-a). If b is infinity, + * this becomes x -> 4a/x - 1. + * + * + * + * SPEED: + * + * Taking advantage of the recurrence properties of the + * Chebyshev polynomials, the routine requires one more + * addition per loop than evaluating a nested polynomial of + * the same degree. + * + */ + /* chbevl.c */ + +/* +Cephes Math Library Release 2.0: April, 1987 +Copyright 1985, 1987 by Stephen L. Moshier +Direct inquiries to 30 Frost Street, Cambridge, MA 02140 +*/ + +#include "cephes.h" + +double chbevl(double x,void* array,int n ) +{ +double b0, b1, b2, *p; +int i; + +p = (double *) array; +b0 = *p++; +b1 = 0.0; +i = n - 1; + +do + { + b2 = b1; + b1 = b0; + b0 = x * b1 - b2 + *p++; + } +while( --i ); + +return( 0.5*(b0-b2) ); +} diff --git a/ao-tools/lib/cmath.h b/ao-tools/lib/cmath.h new file mode 100644 index 00000000..2751aecf --- /dev/null +++ b/ao-tools/lib/cmath.h @@ -0,0 +1,179 @@ +/* + * Grace - GRaphing, Advanced Computation and Exploration of data + * + * Home page: http://plasma-gate.weizmann.ac.il/Grace/ + * + * Copyright (c) 1991-1995 Paul J Turner, Portland, OR + * Copyright (c) 1996-2000 Grace Development Team + * + * Maintained by Evgeny Stambulchik + * + * + * All Rights Reserved + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +/* cmath.h - replacement for math.h or missing in libm functions */ + +#if defined(HAVE_MATH_H) +# include +#endif +#if defined(HAVE_FLOAT_H) +# include +#endif +#if defined(HAVE_IEEEFP_H) +# include +#endif + +#ifndef __GRACE_SOURCE_ + +#ifndef MACHEP +extern double MACHEP; +#endif + +#ifndef UFLOWTHRESH +extern double UFLOWTHRESH; +#endif + +#ifndef MAXNUM +extern double MAXNUM; +#endif + +#endif /* __GRACE_SOURCE_ */ + +#ifndef M_PI +# define M_PI 3.14159265358979323846 +#endif + +#ifndef M_SQRT2 +# define M_SQRT2 1.41421356237309504880 /* sqrt(2) */ +#endif + +#ifndef M_SQRT1_2 +# define M_SQRT1_2 0.70710678118654752440 /* 1/sqrt(2) */ +#endif + +#ifndef M_SQRT1_3 +# define M_SQRT1_3 0.57735026918962576451 /* 1/sqrt(3) */ +#endif + +#ifndef HAVE_HYPOT +# define hypot(x, y) sqrt((x)*(x) + (y)*(y)) +#endif + +extern double round ( double x ); +#ifndef HAVE_RINT +# define rint round +#else +# ifndef HAVE_RINT_DECL +extern double rint ( double x ); +# endif +#endif + +#ifndef HAVE_CBRT_DECL +extern double cbrt ( double x ); +#endif + +/* Cygnus gnuwin32 has the log2 macro */ +#ifdef log2 +# undef log2 +#endif + +#ifndef HAVE_LOG2_DECL +extern double log2 ( double x ); +#endif + +#ifndef HAVE_LGAMMA +extern int sgngam; +# define lgamma lgam +# define signgam sgngam +extern double lgam ( double x ); +#else +# ifndef HAVE_LGAMMA_DECL +extern double lgamma ( double x ); +# endif +# ifndef HAVE_SIGNGAM_DECL +extern int signgam; +# endif +# define lgam lgamma +# define sgngam signgam +#endif + +#ifndef HAVE_ACOSH_DECL +extern double acosh ( double x ); +#endif + +#ifndef HAVE_ASINH_DECL +extern double asinh ( double x ); +#endif + +#ifndef HAVE_ATANH_DECL +extern double atanh ( double x ); +#endif + +#ifndef HAVE_ERF_DECL +extern double erf ( double x ); +#endif + +#ifndef HAVE_ERFC_DECL +extern double erfc ( double x ); +#endif + +#ifndef HAVE_Y0_DECL +extern double y0 ( double x ); +#endif +#ifndef HAVE_Y1_DECL +extern double y1 ( double x ); +#endif +#ifndef HAVE_YN_DECL +extern double yn ( int n, double x ); +#endif +#ifndef HAVE_J0_DECL +extern double j0 ( double x ); +#endif +#ifndef HAVE_J1_DECL +extern double j1 ( double x ); +#endif +#ifndef HAVE_JN_DECL +extern double jn ( int n, double x ); +#endif + +/* isfinite is a macro */ +#ifdef isfinite +# define HAVE_ISFINITE_MACRO +#endif + +#ifndef HAVE_FINITE +# define finite isfinite +# if !defined(HAVE_ISFINITE_DECL) && !defined(HAVE_ISFINITE_MACRO) +extern int isfinite ( double x ); +# endif +#else +# ifndef HAVE_FINITE_DECL +extern int finite ( double x ); +# endif +#endif + +#ifndef HAVE_ISNAN_DECL +#ifdef __FreeBSD__ +# include +# if __FreeBSD_version < 500100 +extern int isnan ( double x ); +# endif +#endif +#else +extern int isnan ( double x ); +#endif diff --git a/ao-tools/lib/i0.c b/ao-tools/lib/i0.c new file mode 100644 index 00000000..6f7b5a57 --- /dev/null +++ b/ao-tools/lib/i0.c @@ -0,0 +1,414 @@ +/* + * This file comes from the cephes math library, which was + * released under the GPLV2+ license as a part of the Debian labplot + * package (I've included the GPLV2 license reference here to make + * this clear) - Keith Packard + * + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +/* i0.c + * + * Modified Bessel function of order zero + * + * + * + * SYNOPSIS: + * + * double x, y, i0(); + * + * y = i0( x ); + * + * + * + * DESCRIPTION: + * + * Returns modified Bessel function of order zero of the + * argument. + * + * The function is defined as i0(x) = j0( ix ). + * + * The range is partitioned into the two intervals [0,8] and + * (8, infinity). Chebyshev polynomial expansions are employed + * in each interval. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * DEC 0,30 6000 8.2e-17 1.9e-17 + * IEEE 0,30 30000 5.8e-16 1.4e-16 + * + */ + /* i0e.c + * + * Modified Bessel function of order zero, + * exponentially scaled + * + * + * + * SYNOPSIS: + * + * double x, y, i0e(); + * + * y = i0e( x ); + * + * + * + * DESCRIPTION: + * + * Returns exponentially scaled modified Bessel function + * of order zero of the argument. + * + * The function is defined as i0e(x) = exp(-|x|) j0( ix ). + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 30000 5.4e-16 1.2e-16 + * See i0(). + * + */ + +/* i0.c */ + + +/* +Cephes Math Library Release 2.0: April, 1987 +Copyright 1984, 1987 by Stephen L. Moshier +Direct inquiries to 30 Frost Street, Cambridge, MA 02140 +*/ + +#include +#include "mconf.h" +#include "cephes.h" + +/* Chebyshev coefficients for exp(-x) I0(x) + * in the interval [0,8]. + * + * lim(x->0){ exp(-x) I0(x) } = 1. + */ + +#ifdef UNK +static double A[] = +{ +-4.41534164647933937950E-18, + 3.33079451882223809783E-17, +-2.43127984654795469359E-16, + 1.71539128555513303061E-15, +-1.16853328779934516808E-14, + 7.67618549860493561688E-14, +-4.85644678311192946090E-13, + 2.95505266312963983461E-12, +-1.72682629144155570723E-11, + 9.67580903537323691224E-11, +-5.18979560163526290666E-10, + 2.65982372468238665035E-9, +-1.30002500998624804212E-8, + 6.04699502254191894932E-8, +-2.67079385394061173391E-7, + 1.11738753912010371815E-6, +-4.41673835845875056359E-6, + 1.64484480707288970893E-5, +-5.75419501008210370398E-5, + 1.88502885095841655729E-4, +-5.76375574538582365885E-4, + 1.63947561694133579842E-3, +-4.32430999505057594430E-3, + 1.05464603945949983183E-2, +-2.37374148058994688156E-2, + 4.93052842396707084878E-2, +-9.49010970480476444210E-2, + 1.71620901522208775349E-1, +-3.04682672343198398683E-1, + 6.76795274409476084995E-1 +}; +#endif + +#ifdef DEC +static unsigned short A[] = { +0121642,0162671,0004646,0103567, +0022431,0115424,0135755,0026104, +0123214,0023533,0110365,0156635, +0023767,0033304,0117662,0172716, +0124522,0100426,0012277,0157531, +0025254,0155062,0054461,0030465, +0126010,0131143,0013560,0153604, +0026517,0170577,0006336,0114437, +0127227,0162253,0152243,0052734, +0027724,0142766,0061641,0160200, +0130416,0123760,0116564,0125262, +0031066,0144035,0021246,0054641, +0131537,0053664,0060131,0102530, +0032201,0155664,0165153,0020652, +0132617,0061434,0074423,0176145, +0033225,0174444,0136147,0122542, +0133624,0031576,0056453,0020470, +0034211,0175305,0172321,0041314, +0134561,0054462,0147040,0165315, +0035105,0124333,0120203,0162532, +0135427,0013750,0174257,0055221, +0035726,0161654,0050220,0100162, +0136215,0131361,0000325,0041110, +0036454,0145417,0117357,0017352, +0136702,0072367,0104415,0133574, +0037111,0172126,0072505,0014544, +0137302,0055601,0120550,0033523, +0037457,0136543,0136544,0043002, +0137633,0177536,0001276,0066150, +0040055,0041164,0100655,0010521 +}; +#endif + +#ifdef IBMPC +static unsigned short A[] = { +0xd0ef,0x2134,0x5cb7,0xbc54, +0xa589,0x977d,0x3362,0x3c83, +0xbbb4,0x721e,0x84eb,0xbcb1, +0x5eba,0x93f6,0xe6d8,0x3cde, +0xfbeb,0xc297,0x5022,0xbd0a, +0x2627,0x4b26,0x9b46,0x3d35, +0x1af0,0x62ee,0x164c,0xbd61, +0xd324,0xe19b,0xfe2f,0x3d89, +0x6abc,0x7a94,0xfc95,0xbdb2, +0x3c10,0xcc74,0x98be,0x3dda, +0x9556,0x13ae,0xd4fe,0xbe01, +0xcb34,0xa454,0xd903,0x3e26, +0x30ab,0x8c0b,0xeaf6,0xbe4b, +0x6435,0x9d4d,0x3b76,0x3e70, +0x7f8d,0x8f22,0xec63,0xbe91, +0xf4ac,0x978c,0xbf24,0x3eb2, +0x6427,0xcba5,0x866f,0xbed2, +0x2859,0xbe9a,0x3f58,0x3ef1, +0x1d5a,0x59c4,0x2b26,0xbf0e, +0x7cab,0x7410,0xb51b,0x3f28, +0xeb52,0x1f15,0xe2fd,0xbf42, +0x100e,0x8a12,0xdc75,0x3f5a, +0xa849,0x201a,0xb65e,0xbf71, +0xe3dd,0xf3dd,0x9961,0x3f85, +0xb6f0,0xf121,0x4e9e,0xbf98, +0xa32d,0xcea8,0x3e8a,0x3fa9, +0x06ea,0x342d,0x4b70,0xbfb8, +0x88c0,0x77ac,0xf7ac,0x3fc5, +0xcd8d,0xc057,0x7feb,0xbfd3, +0xa22a,0x9035,0xa84e,0x3fe5, +}; +#endif + +#ifdef MIEEE +static unsigned short A[] = { +0xbc54,0x5cb7,0x2134,0xd0ef, +0x3c83,0x3362,0x977d,0xa589, +0xbcb1,0x84eb,0x721e,0xbbb4, +0x3cde,0xe6d8,0x93f6,0x5eba, +0xbd0a,0x5022,0xc297,0xfbeb, +0x3d35,0x9b46,0x4b26,0x2627, +0xbd61,0x164c,0x62ee,0x1af0, +0x3d89,0xfe2f,0xe19b,0xd324, +0xbdb2,0xfc95,0x7a94,0x6abc, +0x3dda,0x98be,0xcc74,0x3c10, +0xbe01,0xd4fe,0x13ae,0x9556, +0x3e26,0xd903,0xa454,0xcb34, +0xbe4b,0xeaf6,0x8c0b,0x30ab, +0x3e70,0x3b76,0x9d4d,0x6435, +0xbe91,0xec63,0x8f22,0x7f8d, +0x3eb2,0xbf24,0x978c,0xf4ac, +0xbed2,0x866f,0xcba5,0x6427, +0x3ef1,0x3f58,0xbe9a,0x2859, +0xbf0e,0x2b26,0x59c4,0x1d5a, +0x3f28,0xb51b,0x7410,0x7cab, +0xbf42,0xe2fd,0x1f15,0xeb52, +0x3f5a,0xdc75,0x8a12,0x100e, +0xbf71,0xb65e,0x201a,0xa849, +0x3f85,0x9961,0xf3dd,0xe3dd, +0xbf98,0x4e9e,0xf121,0xb6f0, +0x3fa9,0x3e8a,0xcea8,0xa32d, +0xbfb8,0x4b70,0x342d,0x06ea, +0x3fc5,0xf7ac,0x77ac,0x88c0, +0xbfd3,0x7feb,0xc057,0xcd8d, +0x3fe5,0xa84e,0x9035,0xa22a +}; +#endif + + +/* Chebyshev coefficients for exp(-x) sqrt(x) I0(x) + * in the inverted interval [8,infinity]. + * + * lim(x->inf){ exp(-x) sqrt(x) I0(x) } = 1/sqrt(2pi). + */ + +#ifdef UNK +static double B[] = +{ +-7.23318048787475395456E-18, +-4.83050448594418207126E-18, + 4.46562142029675999901E-17, + 3.46122286769746109310E-17, +-2.82762398051658348494E-16, +-3.42548561967721913462E-16, + 1.77256013305652638360E-15, + 3.81168066935262242075E-15, +-9.55484669882830764870E-15, +-4.15056934728722208663E-14, + 1.54008621752140982691E-14, + 3.85277838274214270114E-13, + 7.18012445138366623367E-13, +-1.79417853150680611778E-12, +-1.32158118404477131188E-11, +-3.14991652796324136454E-11, + 1.18891471078464383424E-11, + 4.94060238822496958910E-10, + 3.39623202570838634515E-9, + 2.26666899049817806459E-8, + 2.04891858946906374183E-7, + 2.89137052083475648297E-6, + 6.88975834691682398426E-5, + 3.36911647825569408990E-3, + 8.04490411014108831608E-1 +}; +#endif + +#ifdef DEC +static unsigned short B[] = { +0122005,0066672,0123124,0054311, +0121662,0033323,0030214,0104602, +0022515,0170300,0113314,0020413, +0022437,0117350,0035402,0007146, +0123243,0000135,0057220,0177435, +0123305,0073476,0144106,0170702, +0023777,0071755,0017527,0154373, +0024211,0052214,0102247,0033270, +0124454,0017763,0171453,0012322, +0125072,0166316,0075505,0154616, +0024612,0133770,0065376,0025045, +0025730,0162143,0056036,0001632, +0026112,0015077,0150464,0063542, +0126374,0101030,0014274,0065457, +0127150,0077271,0125763,0157617, +0127412,0104350,0040713,0120445, +0027121,0023765,0057500,0001165, +0030407,0147146,0003643,0075644, +0031151,0061445,0044422,0156065, +0031702,0132224,0003266,0125551, +0032534,0000076,0147153,0005555, +0033502,0004536,0004016,0026055, +0034620,0076433,0142314,0171215, +0036134,0146145,0013454,0101104, +0040115,0171425,0062500,0047133 +}; +#endif + +#ifdef IBMPC +static unsigned short B[] = { +0x8b19,0x54ca,0xadb7,0xbc60, +0x9130,0x6611,0x46da,0xbc56, +0x8421,0x12d9,0xbe18,0x3c89, +0x41cd,0x0760,0xf3dd,0x3c83, +0x1fe4,0xabd2,0x600b,0xbcb4, +0xde38,0xd908,0xaee7,0xbcb8, +0xfb1f,0xa3ea,0xee7d,0x3cdf, +0xe6d7,0x9094,0x2a91,0x3cf1, +0x629a,0x7e65,0x83fe,0xbd05, +0xbb32,0xcf68,0x5d99,0xbd27, +0xc545,0x0d5f,0x56ff,0x3d11, +0xc073,0x6b83,0x1c8c,0x3d5b, +0x8cec,0xfa26,0x4347,0x3d69, +0x8d66,0x0317,0x9043,0xbd7f, +0x7bf2,0x357e,0x0fd7,0xbdad, +0x7425,0x0839,0x511d,0xbdc1, +0x004f,0xabe8,0x24fe,0x3daa, +0x6f75,0xc0f4,0xf9cc,0x3e00, +0x5b87,0xa922,0x2c64,0x3e2d, +0xd56d,0x80d6,0x5692,0x3e58, +0x616e,0xd9cd,0x8007,0x3e8b, +0xc586,0xc101,0x412b,0x3ec8, +0x9e52,0x7899,0x0fa3,0x3f12, +0x9049,0xa2e5,0x998c,0x3f6b, +0x09cb,0xaca8,0xbe62,0x3fe9 +}; +#endif + +#ifdef MIEEE +static unsigned short B[] = { +0xbc60,0xadb7,0x54ca,0x8b19, +0xbc56,0x46da,0x6611,0x9130, +0x3c89,0xbe18,0x12d9,0x8421, +0x3c83,0xf3dd,0x0760,0x41cd, +0xbcb4,0x600b,0xabd2,0x1fe4, +0xbcb8,0xaee7,0xd908,0xde38, +0x3cdf,0xee7d,0xa3ea,0xfb1f, +0x3cf1,0x2a91,0x9094,0xe6d7, +0xbd05,0x83fe,0x7e65,0x629a, +0xbd27,0x5d99,0xcf68,0xbb32, +0x3d11,0x56ff,0x0d5f,0xc545, +0x3d5b,0x1c8c,0x6b83,0xc073, +0x3d69,0x4347,0xfa26,0x8cec, +0xbd7f,0x9043,0x0317,0x8d66, +0xbdad,0x0fd7,0x357e,0x7bf2, +0xbdc1,0x511d,0x0839,0x7425, +0x3daa,0x24fe,0xabe8,0x004f, +0x3e00,0xf9cc,0xc0f4,0x6f75, +0x3e2d,0x2c64,0xa922,0x5b87, +0x3e58,0x5692,0x80d6,0xd56d, +0x3e8b,0x8007,0xd9cd,0x616e, +0x3ec8,0x412b,0xc101,0xc586, +0x3f12,0x0fa3,0x7899,0x9e52, +0x3f6b,0x998c,0xa2e5,0x9049, +0x3fe9,0xbe62,0xaca8,0x09cb +}; +#endif + +double i0(double x) +{ +double y; + +if( x < 0 ) + x = -x; +if( x <= 8.0 ) + { + y = (x/2.0) - 2.0; + return( exp(x) * chbevl( y, A, 30 ) ); + } + +return( exp(x) * chbevl( 32.0/x - 2.0, B, 25 ) / sqrt(x) ); + +} + + + + +double i0e(double x ) +{ +double y; + +if( x < 0 ) + x = -x; +if( x <= 8.0 ) + { + y = (x/2.0) - 2.0; + return( chbevl( y, A, 30 ) ); + } + +return( chbevl( 32.0/x - 2.0, B, 25 ) / sqrt(x) ); + +} diff --git a/ao-tools/lib/mconf.h b/ao-tools/lib/mconf.h new file mode 100644 index 00000000..af1ebb51 --- /dev/null +++ b/ao-tools/lib/mconf.h @@ -0,0 +1,211 @@ +/* + * This file comes from the cephes math library, which was + * released under the GPLV2+ license as a part of the Debian labplot + * package (I've included the GPLV2 license reference here to make + * this clear) - Keith Packard + * + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ +/* mconf.h + * + * Common include file for math routines + * + * + * + * SYNOPSIS: + * + * #include "mconf.h" + * + * + * + * DESCRIPTION: + * + * This file contains definitions for error codes that are + * passed to the common error handling routine mtherr() + * (which see). + * + * The file also includes a conditional assembly definition + * for the type of computer arithmetic (IEEE, DEC, Motorola + * IEEE, or UNKnown). + * + * For Digital Equipment PDP-11 and VAX computers, certain + * IBM systems, and others that use numbers with a 56-bit + * significand, the symbol DEC should be defined. In this + * mode, most floating point constants are given as arrays + * of octal integers to eliminate decimal to binary conversion + * errors that might be introduced by the compiler. + * + * For little-endian computers, such as IBM PC, that follow the + * IEEE Standard for Binary Floating Point Arithmetic (ANSI/IEEE + * Std 754-1985), the symbol IBMPC should be defined. These + * numbers have 53-bit significands. In this mode, constants + * are provided as arrays of hexadecimal 16 bit integers. + * + * Big-endian IEEE format is denoted MIEEE. On some RISC + * systems such as Sun SPARC, double precision constants + * must be stored on 8-byte address boundaries. Since integer + * arrays may be aligned differently, the MIEEE configuration + * may fail on such machines. + * + * To accommodate other types of computer arithmetic, all + * constants are also provided in a normal decimal radix + * which one can hope are correctly converted to a suitable + * format by the available C language compiler. To invoke + * this mode, define the symbol UNK. + * + * An important difference among these modes is a predefined + * set of machine arithmetic constants for each. The numbers + * MACHEP (the machine roundoff error), MAXNUM (largest number + * represented), and several other parameters are preset by + * the configuration symbol. Check the file const.c to + * ensure that these values are correct for your computer. + * + * Configurations NANS, INFINITIES, MINUSZERO, and DENORMAL + * may fail on many systems. Verify that they are supposed + * to work on your computer. + */ + +/* +Cephes Math Library Release 2.3: June, 1995 +Copyright 1984, 1987, 1989, 1995 by Stephen L. Moshier + +Adjusted for use with ACE/gr by Evgeny Stambulchik, October 1997 +*/ + +#define __GRACE_SOURCE_ + +#include "cmath.h" + +/* Type of computer arithmetic */ +/* In ACE/gr, defined as a compiler directive - no need to define here */ + +/* PDP-11, Pro350, VAX: + */ +#if defined(HAVE_DEC_FPU) +# define DEC 1 +#endif + +/* Intel IEEE, low order words come first: + */ +#if defined(HAVE_LIEEE_FPU) +# define IBMPC 1 +#endif + +/* Motorola IEEE, high order words come first + * (Sun 680x0 workstation): + */ +#if defined(HAVE_BIEEE_FPU) +# define MIEEE 1 +#endif + +/* UNKnown arithmetic, invokes coefficients given in + * normal decimal format. Beware of range boundary + * problems (MACHEP, MAXLOG, etc. in const.c) and + * roundoff problems in pow.c: + * (Sun SPARCstation) + */ + +#if (!defined (DEC) && !defined (IBMPC) && !defined (MIEEE)) +# define UNK 1 +#endif + +/* Define this `volatile' if your compiler thinks + * that floating point arithmetic obeys the associative + * and distributive laws. It will defeat some optimizations + * (but probably not enough of them). + * + * #define VOLATILE volatile + */ + +#ifndef VOLATILE +# define VOLATILE +#endif + +#ifdef PI +# undef PI +#endif + +#ifdef NAN +# undef NAN +#endif + +#ifdef INFINITY +# undef INFINITY +#endif + +/* Constant definitions for math error conditions + */ + +#if defined(DOMAIN) +# undef DOMAIN +#endif +#define DOMAIN 1 /* argument domain error */ + +#if defined(SING) +# undef SING +#endif +#define SING 2 /* argument singularity */ + +#if defined(OVERFLOW) +# undef OVERFLOW +#endif +#define OVERFLOW 3 /* overflow range error */ + +#if defined(UNDERFLOW) +# undef UNDERFLOW +#endif +#define UNDERFLOW 4 /* underflow range error */ + +#if defined(TLOSS) +# undef TLOSS +#endif +#define TLOSS 5 /* total loss of precision */ + +#if defined(PLOSS) +# undef PLOSS +#endif +#define PLOSS 6 /* partial loss of precision */ + +#if defined(EDOM) +# undef EDOM +#endif +#define EDOM 33 + +#if defined(ERANGE) +# undef ERANGE +#endif +#define ERANGE 34 + +#if !defined (UNK) + /* Define to support tiny denormal numbers, else undefine. */ +# define DENORMAL 1 + + /* Define to ask for infinity support, else undefine. */ +# define INFINITIES 1 + + /* Define to ask for support of numbers that are Not-a-Number, + else undefine. This may automatically define INFINITIES in some files. */ +# define NANS 1 + + /* Define to distinguish between -0.0 and +0.0. */ +# define MINUSZERO 1 +#endif + +/* Define 1 for ANSI C atan2() function + See atan.c and clog.c. */ +#define ANSIC 1 -- 2.30.2