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 <keithp@keithp.com>

diff --git a/ao-tools/ao-postflight/ao-postflight.c b/ao-tools/ao-postflight/ao-postflight.c
index 9371f35174ed2c8966ee1dff67439d421246d6a8..c5814c93de4e7fe30fccd3a89c832aeb5693d4b1 100644 (file)
--- a/ao-tools/ao-postflight/ao-postflight.c
+++ b/ao-tools/ao-postflight/ao-postflight.c
@@ -27,16 +27,6 @@
 
 #define NUM_BLOCK      512
 
 
 #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",
 static const char *state_names[] = {
        "startup",
        "idle",


@@ -51,20 +41,23 @@ static const char *state_names[] = {
 };
 
 void
 };
 
 void

-analyse_flight(struct cc_flightraw *f)
+analyse_flight(struct cc_flightraw *f, FILE *summary_file, FILE *detail_file)

 {
        double  height;
        double  accel;
 {
        double  height;
        double  accel;

+       double  speed;

        double  boost_start, boost_stop;
        double  min_pres;
        int     i;
        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;
        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;
                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);
 
        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);
        {
                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);
                        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);
        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);
        {
                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);
        }
                        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;
        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;
                        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);
                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);
                }
 
                               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);
                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);
                }
        }
                                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=<summary-file>] [--detail=<detail-file] {flight-log} ...\n", program);
+       exit(1);

 }
 
 int
 main (int argc, char **argv)
 {
        FILE                    *file;
 }
 
 int
 main (int argc, char **argv)
 {
        FILE                    *file;

+       FILE                    *summary_file;
+       FILE                    *detail_file;

        int                     i;
        int                     ret = 0;
        struct cc_flightraw     *raw;
        int                     c;
        int                     serial;
        char                    *s;
        int                     i;
        int                     ret = 0;
        struct cc_flightraw     *raw;
        int                     c;
        int                     serial;
        char                    *s;

+       char                    *summary_name, *detail_name;

 
 

-       while ((c = getopt_long(argc, argv, "", options, NULL)) != -1) {
+       while ((c = getopt_long(argc, argv, "S:D:", options, NULL)) != -1) {

                switch (c) {
                switch (c) {

+               case 'S':
+                       summary_name = optarg;
+                       break;
+               case 'D':
+                       detail_name = optarg;
+                       break;

                default:
                        usage(argv[0]);
                        break;
                }
        }
                default:
                        usage(argv[0]);
                        break;
                }
        }

+       summary_file = stdout;
+       detail_file = NULL;
+       if (summary_name) {
+               summary_file = fopen(summary_name, "w");
+               if (!summary_file) {
+                       perror (summary_name);
+                       exit(1);
+               }
+       }
+       if (detail_name) {
+               if (!strcmp (summary_name, detail_name))
+                       detail_file = summary_file;
+               else {
+                       detail_file = fopen(detail_name, "w");
+                       if (!detail_file) {
+                               perror(detail_name);
+                               exit(1);
+                       }
+               }
+       }

        for (i = optind; i < argc; i++) {
                file = fopen(argv[i], "r");
                if (!file) {
        for (i = optind; i < argc; i++) {
                file = fopen(argv[i], "r");
                if (!file) {


@@ -173,7 +260,7 @@ main (int argc, char **argv)
                }
                if (!raw->serial)
                        raw->serial = serial;
                }
                if (!raw->serial)
                        raw->serial = serial;

-               analyse_flight(raw);
+               analyse_flight(raw, summary_file, detail_file);

                cc_flightraw_free(raw);
        }
        return ret;
                cc_flightraw_free(raw);
        }
        return ret;

diff --git a/ao-tools/lib/Makefile.am b/ao-tools/lib/Makefile.am
index e682f757d68d8aa85c1e22d948c99c1443704849..79972f46f4ea6465781a19e20535e1bb481b6956 100644 (file)
--- 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 \
        ccdbg-state.c \
        cc-analyse.c \
        cc-convert.c \

+       cc-dsp.c \

        cc-log.c \
        cc-log.c \

+       cc-integrate.c \
+       cc-period.c \
+       cc-process.c \

        cc-usb.c \
        cc-usb.h \
        cc.h \
        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 \
        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 fc8a8417bd3d6cba210be05eccea23ac0ede12d9..0e020115affe54ca6017bb010785808ee21bce0c 100644 (file)
--- a/ao-tools/lib/cc-analyse.c
+++ b/ao-tools/lib/cc-analyse.c
@@ -16,6 +16,7 @@
  */
 
 #include "cc.h"
  */
 
 #include "cc.h"

+#include <math.h>

 
 int
 cc_timedata_min(struct cc_timedata *d, double min_time, double max_time)
 
 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;
 }
                        }
        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 (file)
index 0000000..518c1a6
--- /dev/null
+++ b/ao-tools/lib/cc-dsp.c
@@ -0,0 +1,145 @@
+/*
+ * Copyright © 2009 Keith Packard <keithp@keithp.com>
+ *
+ * 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 <math.h>
+#include <stdlib.h>
+
+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 (file)
index 0000000..08ca295
--- /dev/null
+++ b/ao-tools/lib/cc-integrate.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright © 2009 Keith Packard <keithp@keithp.com>
+ *
+ * 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 <stdlib.h>
+
+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 (file)
index 0000000..c74cf9d
--- /dev/null
+++ b/ao-tools/lib/cc-period.c
@@ -0,0 +1,64 @@
+/*
+ * Copyright © 2009 Keith Packard <keithp@keithp.com>
+ *
+ * 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 <stdlib.h>
+
+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 (file)
index 0000000..e906b63
--- /dev/null
+++ b/ao-tools/lib/cc-process.c
@@ -0,0 +1,140 @@
+/*
+ * Copyright © 2009 Keith Packard <keithp@keithp.com>
+ *
+ * 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 <stdlib.h>
+#include <math.h>
+#include <string.h>
+
+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 57f80b8d5e938220e709c4eb6026ee4ba9481404..356794e0558cee186e3d33d75eae482f5b3191aa 100644 (file)
--- 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_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_ */
 #endif /* _CC_H_ */

diff --git a/ao-tools/lib/cephes.h b/ao-tools/lib/cephes.h
new file mode 100644 (file)
index 0000000..f8ec264
--- /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 <keithp@keithp.com>
+ *
+ * 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 (file)
index 0000000..2289241
--- /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.
+ *
+ */
+\f/*                                                    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 (file)
index 0000000..2751aec
--- /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 <fnevgeny@plasma-gate.weizmann.ac.il>
+ *
+ *
+ *                           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 <math.h>
+#endif
+#if defined(HAVE_FLOAT_H)
+#  include <float.h>
+#endif
+#if defined(HAVE_IEEEFP_H)
+#  include <ieeefp.h>
+#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 <sys/param.h>
+#  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 (file)
index 0000000..6f7b5a5
--- /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 <keithp@keithp.com>
+ *
+ * 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
+ *
+ */
+\f/*                                                    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().
+ *
+ */
+\f
+/*                                                     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 <math.h>
+#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 (file)
index 0000000..af1ebb5
--- /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 <keithp@keithp.com>
+ *
+ * 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.
+ */
+\f
+/*
+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