ao-tools/ao-list: Show devices that have no TTY
[fw/altos] / ao-tools / lib / cc.h
1 /*
2  * Copyright © 2009 Keith Packard <keithp@keithp.com>
3  *
4  * This program is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License as published by
6  * the Free Software Foundation; version 2 of the License.
7  *
8  * This program is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
11  * General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License along
14  * with this program; if not, write to the Free Software Foundation, Inc.,
15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
16  */
17
18 #ifndef _CC_H_
19 #define _CC_H_
20
21 #include <stdio.h>
22 #include <stdint.h>
23 #include "cc-telemetry.h"
24
25 char *
26 cc_fullname (char *dir, char *file);
27
28 char *
29 cc_basename(char *file);
30
31 int
32 cc_mkdir(char *dir);
33
34 struct cc_usbdev {
35         char    *sys;
36         char    *tty;
37         char    *manufacturer;
38         char    *product;
39         int     serial; /* AltOS always uses simple integer serial numbers */
40         int     idProduct;
41         int     idVendor;
42 };
43
44 struct cc_usbdevs {
45         struct cc_usbdev        **dev;
46         int                     ndev;
47 };
48
49 void
50 cc_usbdevs_free(struct cc_usbdevs *usbdevs);
51
52 struct cc_usbdevs *
53 cc_usbdevs_scan(int non_tty);
54
55 char *
56 cc_usbdevs_find_by_arg(char *arg, char *default_product);
57
58 void
59 cc_set_log_dir(char *dir);
60
61 char *
62 cc_get_log_dir(void);
63
64 char *
65 cc_make_filename(int serial, int flight, char *ext);
66
67 /*
68  * For sequential data which are not evenly spaced
69  */
70
71 struct cc_timedataelt {
72         double  time;
73         double  value;
74 };
75
76 struct cc_timedata {
77         int                     num;
78         int                     size;
79         struct cc_timedataelt   *data;
80         double                  time_offset;
81 };
82
83
84 /*
85  * For GPS data
86  */
87
88 struct cc_gpselt {
89         double          time;
90         int             hour;
91         int             minute;
92         int             second;
93         int             flags;
94         double          lat;
95         double          lon;
96         double          alt;
97 };
98
99 #define SIRF_SAT_STATE_ACQUIRED                 (1 << 0)
100 #define SIRF_SAT_STATE_CARRIER_PHASE_VALID      (1 << 1)
101 #define SIRF_SAT_BIT_SYNC_COMPLETE              (1 << 2)
102 #define SIRF_SAT_SUBFRAME_SYNC_COMPLETE         (1 << 3)
103 #define SIRF_SAT_CARRIER_PULLIN_COMPLETE        (1 << 4)
104 #define SIRF_SAT_CODE_LOCKED                    (1 << 5)
105 #define SIRF_SAT_ACQUISITION_FAILED             (1 << 6)
106 #define SIRF_SAT_EPHEMERIS_AVAILABLE            (1 << 7)
107
108 struct cc_gpssat {
109         double          time;
110         uint16_t        svid;
111         uint8_t         c_n;
112 };
113
114 struct cc_gpssats {
115         int                     nsat;
116         struct cc_gpssat        sat[12];
117 };
118
119 struct cc_gpsdata {
120         int                     num;
121         int                     size;
122         struct cc_gpselt        *data;
123         double                  time_offset;
124         int                     numsats;
125         int                     sizesats;
126         struct cc_gpssats       *sats;
127 };
128
129 /*
130  * For sequential data which are evenly spaced
131  */
132 struct cc_perioddata {
133         int             num;
134         double          start;
135         double          step;
136         double          *data;
137 };
138
139 enum ao_flight_state {
140         ao_flight_startup = 0,
141         ao_flight_idle = 1,
142         ao_flight_pad = 2,
143         ao_flight_boost = 3,
144         ao_flight_fast = 4,
145         ao_flight_coast = 5,
146         ao_flight_drogue = 6,
147         ao_flight_main = 7,
148         ao_flight_landed = 8,
149         ao_flight_invalid = 9
150 };
151
152 struct cc_flightraw {
153         int                     flight;
154         int                     serial;
155         double                  ground_accel;
156         double                  ground_pres;
157         int                     year, month, day;
158         struct cc_timedata      accel;
159         struct cc_timedata      pres;
160         struct cc_timedata      temp;
161         struct cc_timedata      volt;
162         struct cc_timedata      main;
163         struct cc_timedata      drogue;
164         struct cc_timedata      state;
165         struct cc_gpsdata       gps;
166 };
167
168 struct cc_flightraw *
169 cc_log_read(FILE *file);
170
171 void
172 cc_flightraw_free(struct cc_flightraw *raw);
173
174 struct cc_flightcooked {
175         double                  flight_start;
176         double                  flight_stop;
177
178         struct cc_perioddata    accel_accel;
179         struct cc_perioddata    accel_speed;
180         struct cc_perioddata    accel_pos;
181         struct cc_perioddata    pres_pos;
182         struct cc_perioddata    pres_speed;
183         struct cc_perioddata    pres_accel;
184         struct cc_perioddata    gps_lat;
185         struct cc_perioddata    gps_lon;
186         struct cc_perioddata    gps_alt;
187
188         /* unfiltered, but converted */
189         struct cc_timedata      pres;
190         struct cc_timedata      accel;
191         struct cc_timedata      state;
192 };
193
194 /*
195  * Telemetry data contents
196  */
197
198
199 struct cc_gps_time {
200         int year;
201         int month;
202         int day;
203         int hour;
204         int minute;
205         int second;
206 };
207
208 struct cc_gps {
209         int     nsat;
210         int     gps_locked;
211         int     gps_connected;
212         struct cc_gps_time gps_time;
213         double  lat;            /* degrees (+N -S) */
214         double  lon;            /* degrees (+E -W) */
215         int     alt;            /* m */
216
217         int     gps_extended;   /* has extra data */
218         double  ground_speed;   /* m/s */
219         int     course;         /* degrees */
220         double  climb_rate;     /* m/s */
221         double  hdop;           /* unitless? */
222         int     h_error;        /* m */
223         int     v_error;        /* m */
224 };
225
226 #define SIRF_SAT_STATE_ACQUIRED                 (1 << 0)
227 #define SIRF_SAT_STATE_CARRIER_PHASE_VALID      (1 << 1)
228 #define SIRF_SAT_BIT_SYNC_COMPLETE              (1 << 2)
229 #define SIRF_SAT_SUBFRAME_SYNC_COMPLETE         (1 << 3)
230 #define SIRF_SAT_CARRIER_PULLIN_COMPLETE        (1 << 4)
231 #define SIRF_SAT_CODE_LOCKED                    (1 << 5)
232 #define SIRF_SAT_ACQUISITION_FAILED             (1 << 6)
233 #define SIRF_SAT_EPHEMERIS_AVAILABLE            (1 << 7)
234
235 struct cc_gps_sat {
236         int     svid;
237         int     c_n0;
238 };
239
240 struct cc_gps_tracking {
241         int                     channels;
242         struct cc_gps_sat       sats[12];
243 };
244
245 struct cc_telem {
246         char    callsign[16];
247         int     serial;
248         int     flight;
249         int     rssi;
250         char    state[16];
251         int     tick;
252         int     accel;
253         int     pres;
254         int     temp;
255         int     batt;
256         int     drogue;
257         int     main;
258         int     flight_accel;
259         int     ground_accel;
260         int     flight_vel;
261         int     flight_pres;
262         int     ground_pres;
263         int     accel_plus_g;
264         int     accel_minus_g;
265         struct cc_gps   gps;
266         struct cc_gps_tracking  gps_tracking;
267 };
268
269 int
270 cc_telem_parse(const char *input_line, struct cc_telem *telem);
271
272 struct ao_log_mega {
273         char                    type;                   /* 0 */
274         uint8_t                 is_config;              /* 1 */
275         uint16_t                tick;                   /* 2 */
276         union {                                         /* 4 */
277                 /* AO_LOG_FLIGHT */
278                 struct {
279                         uint16_t        flight;         /* 4 */
280                         int16_t         ground_accel;   /* 6 */
281                         uint32_t        ground_pres;    /* 8 */
282                 } flight;                               /* 12 */
283                 /* AO_LOG_STATE */
284                 struct {
285                         uint16_t        state;
286                         uint16_t        reason;
287                 } state;
288                 /* AO_LOG_SENSOR */
289                 struct {
290                         uint32_t        pres;           /* 4 */
291                         uint32_t        temp;           /* 8 */
292                         int16_t         accel_x;        /* 12 */
293                         int16_t         accel_y;        /* 14 */
294                         int16_t         accel_z;        /* 16 */
295                         int16_t         gyro_x;         /* 18 */
296                         int16_t         gyro_y;         /* 20 */
297                         int16_t         gyro_z;         /* 22 */
298                         int16_t         mag_x;          /* 24 */
299                         int16_t         mag_y;          /* 26 */
300                         int16_t         mag_z;          /* 28 */
301                         int16_t         accel;          /* 30 */
302                 } sensor;       /* 32 */
303                 /* AO_LOG_TEMP_VOLT */
304                 struct {
305                         int16_t         v_batt;         /* 4 */
306                         int16_t         v_pbatt;        /* 6 */
307                         int16_t         n_sense;        /* 8 */
308                         int16_t         sense[10];      /* 10 */
309                         uint16_t        pyro;           /* 30 */
310                 } volt;                                 /* 32 */
311                 /* AO_LOG_GPS_TIME */
312                 struct {
313                         int32_t         latitude;       /* 4 */
314                         int32_t         longitude;      /* 8 */
315                         int16_t         altitude;       /* 12 */
316                         uint8_t         hour;           /* 14 */
317                         uint8_t         minute;         /* 15 */
318                         uint8_t         second;         /* 16 */
319                         uint8_t         flags;          /* 17 */
320                         uint8_t         year;           /* 18 */
321                         uint8_t         month;          /* 19 */
322                         uint8_t         day;            /* 20 */
323                         uint8_t         pad;            /* 21 */
324                 } gps;  /* 22 */
325                 /* AO_LOG_GPS_SAT */
326                 struct {
327                         uint16_t        channels;       /* 4 */
328                         struct {
329                                 uint8_t svid;
330                                 uint8_t c_n;
331                         } sats[12];                     /* 6 */
332                 } gps_sat;                              /* 30 */
333
334                 struct {
335                         uint32_t                kind;
336                         int32_t                 data[6];
337                 } config_int;
338
339                 struct {
340                         uint32_t                kind;
341                         char                    string[24];
342                 } config_str;
343
344                 /* Raw bytes */
345                 uint8_t bytes[28];
346         } u;
347 };
348
349 struct ao_log_metrum {
350         char                    type;                   /* 0 */
351         uint8_t                 csum;                   /* 1 */
352         uint16_t                tick;                   /* 2 */
353         union {                                         /* 4 */
354                 /* AO_LOG_FLIGHT */
355                 struct {
356                         uint16_t        flight;         /* 4 */
357                         int16_t         ground_accel;   /* 6 */
358                         uint32_t        ground_pres;    /* 8 */
359                         uint32_t        ground_temp;    /* 12 */
360                 } flight;       /* 16 */
361                 /* AO_LOG_STATE */
362                 struct {
363                         uint16_t        state;          /* 4 */
364                         uint16_t        reason;         /* 6 */
365                 } state;        /* 8 */
366                 /* AO_LOG_SENSOR */
367                 struct {
368                         uint32_t        pres;           /* 4 */
369                         uint32_t        temp;           /* 8 */
370                         int16_t         accel;          /* 12 */
371                 } sensor;       /* 14 */
372                 /* AO_LOG_TEMP_VOLT */
373                 struct {
374                         int16_t         v_batt;         /* 4 */
375                         int16_t         sense_a;        /* 6 */
376                         int16_t         sense_m;        /* 8 */
377                 } volt;         /* 10 */
378                 /* AO_LOG_GPS_POS */
379                 struct {
380                         int32_t         latitude;       /* 4 */
381                         int32_t         longitude;      /* 8 */
382                         uint16_t        altitude_low;   /* 12 */
383                         int16_t         altitude_high;  /* 14 */
384                 } gps;          /* 16 */
385                 /* AO_LOG_GPS_TIME */
386                 struct {
387                         uint8_t         hour;           /* 4 */
388                         uint8_t         minute;         /* 5 */
389                         uint8_t         second;         /* 6 */
390                         uint8_t         flags;          /* 7 */
391                         uint8_t         year;           /* 8 */
392                         uint8_t         month;          /* 9 */
393                         uint8_t         day;            /* 10 */
394                         uint8_t         pdop;           /* 11 */
395                 } gps_time;     /* 12 */
396                 /* AO_LOG_GPS_SAT (up to three packets) */
397                 struct {
398                         uint8_t channels;               /* 4 */
399                         uint8_t more;                   /* 5 */
400                         struct {
401                                 uint8_t svid;
402                                 uint8_t c_n;
403                         } sats[4];                      /* 6 */
404                 } gps_sat;                              /* 14 */
405                 uint8_t         raw[12];                /* 4 */
406         } u;    /* 16 */
407 };
408
409 struct ao_log_mini {
410         char            type;                           /* 0 */
411         uint8_t         csum;                           /* 1 */
412         uint16_t        tick;                           /* 2 */
413         union {                                         /* 4 */
414                 /* AO_LOG_FLIGHT */
415                 struct {
416                         uint16_t        flight;         /* 4 */
417                         uint16_t        r6;
418                         uint32_t        ground_pres;    /* 8 */
419                 } flight;
420                 /* AO_LOG_STATE */
421                 struct {
422                         uint16_t        state;          /* 4 */
423                         uint16_t        reason;         /* 6 */
424                 } state;
425                 /* AO_LOG_SENSOR */
426                 struct {
427                         uint8_t         pres[3];        /* 4 */
428                         uint8_t         temp[3];        /* 7 */
429                         int16_t         sense_a;        /* 10 */
430                         int16_t         sense_m;        /* 12 */
431                         int16_t         v_batt;         /* 14 */
432                 } sensor;                               /* 16 */
433         } u;                                            /* 16 */
434 };                                                      /* 16 */
435
436 #define ao_log_pack24(dst,value) do {           \
437                 (dst)[0] = (value);             \
438                 (dst)[1] = (value) >> 8;        \
439                 (dst)[2] = (value) >> 16;       \
440         } while (0)
441
442 struct ao_log_gps {
443         char                    type;                   /* 0 */
444         uint8_t                 csum;                   /* 1 */
445         uint16_t                tick;                   /* 2 */
446         union {                                         /* 4 */
447                 /* AO_LOG_FLIGHT */
448                 struct {
449                         uint16_t        flight;                 /* 4 */
450                         int16_t         start_altitude;         /* 6 */
451                         int32_t         start_latitude;         /* 8 */
452                         int32_t         start_longitude;        /* 12 */
453                 } flight;                                       /* 16 */
454                 /* AO_LOG_GPS_TIME */
455                 struct {
456                         int32_t         latitude;       /* 4 */
457                         int32_t         longitude;      /* 8 */
458                         uint16_t        altitude_low;   /* 12 */
459                         uint8_t         hour;           /* 14 */
460                         uint8_t         minute;         /* 15 */
461                         uint8_t         second;         /* 16 */
462                         uint8_t         flags;          /* 17 */
463                         uint8_t         year;           /* 18 */
464                         uint8_t         month;          /* 19 */
465                         uint8_t         day;            /* 20 */
466                         uint8_t         course;         /* 21 */
467                         uint16_t        ground_speed;   /* 22 */
468                         int16_t         climb_rate;     /* 24 */
469                         uint8_t         pdop;           /* 26 */
470                         uint8_t         hdop;           /* 27 */
471                         uint8_t         vdop;           /* 28 */
472                         uint8_t         mode;           /* 29 */
473                         int16_t         altitude_high;  /* 30 */
474                 } gps;  /* 31 */
475                 /* AO_LOG_GPS_SAT */
476                 struct {
477                         uint16_t        channels;       /* 4 */
478                         struct {
479                                 uint8_t svid;
480                                 uint8_t c_n;
481                         } sats[12];                     /* 6 */
482                 } gps_sat;                              /* 30 */
483         } u;
484 };
485
486 #define AO_CONFIG_CONFIG                1
487 #define AO_CONFIG_MAIN                  2
488 #define AO_CONFIG_APOGEE                3
489 #define AO_CONFIG_LOCKOUT               4
490 #define AO_CONFIG_FREQUENCY             5
491 #define AO_CONFIG_RADIO_ENABLE          6
492 #define AO_CONFIG_ACCEL_CAL             7
493 #define AO_CONFIG_RADIO_CAL             8
494 #define AO_CONFIG_MAX_LOG               9
495 #define AO_CONFIG_IGNITE_MODE           10
496 #define AO_CONFIG_PAD_ORIENTATION       11
497 #define AO_CONFIG_SERIAL_NUMBER         12
498 #define AO_CONFIG_LOG_FORMAT            13
499 #define AO_CONFIG_MS5607_RESERVED       14
500 #define AO_CONFIG_MS5607_SENS           15
501 #define AO_CONFIG_MS5607_OFF            16
502 #define AO_CONFIG_MS5607_TCS            17
503 #define AO_CONFIG_MS5607_TCO            18
504 #define AO_CONFIG_MS5607_TREF           19
505 #define AO_CONFIG_MS5607_TEMPSENS       20
506 #define AO_CONFIG_MS5607_CRC            21
507
508
509 #define AO_LOG_FLIGHT           'F'
510 #define AO_LOG_SENSOR           'A'
511 #define AO_LOG_TEMP_VOLT        'T'
512 #define AO_LOG_DEPLOY           'D'
513 #define AO_LOG_STATE            'S'
514 #define AO_LOG_GPS_TIME         'G'
515 #define AO_LOG_GPS_LAT          'N'
516 #define AO_LOG_GPS_LON          'W'
517 #define AO_LOG_GPS_ALT          'H'
518 #define AO_LOG_GPS_SAT          'V'
519 #define AO_LOG_GPS_DATE         'Y'
520 #define AO_LOG_GPS_POS          'P'
521
522 #define AO_LOG_CONFIG           'c'
523
524 #define AO_GPS_NUM_SAT_MASK     (0xf << 0)
525 #define AO_GPS_NUM_SAT_SHIFT    (0)
526
527 #define AO_GPS_VALID            (1 << 4)
528 #define AO_GPS_RUNNING          (1 << 5)
529 #define AO_GPS_DATE_VALID       (1 << 6)
530 #define AO_GPS_COURSE_VALID     (1 << 7)
531
532 #define AO_LOG_FORMAT_UNKNOWN           0       /* unknown; altosui will have to guess */
533 #define AO_LOG_FORMAT_FULL              1       /* 8 byte typed log records */
534 #define AO_LOG_FORMAT_TINY              2       /* two byte state/baro records */
535 #define AO_LOG_FORMAT_TELEMETRY         3       /* 32 byte ao_telemetry records */
536 #define AO_LOG_FORMAT_TELESCIENCE       4       /* 32 byte typed telescience records */
537 #define AO_LOG_FORMAT_TELEMEGA          5       /* 32 byte typed telemega records */
538 #define AO_LOG_FORMAT_EASYMINI          6       /* 16-byte MS5607 baro only, 3.0V supply */
539 #define AO_LOG_FORMAT_TELEMETRUM        7       /* 16-byte typed telemetrum records */
540 #define AO_LOG_FORMAT_TELEMINI          8       /* 16-byte MS5607 baro only, 3.3V supply */
541 #define AO_LOG_FORMAT_TELEGPS           9       /* 32 byte telegps records */
542 #define AO_LOG_FORMAT_NONE              127     /* No log at all */
543
544 int
545 cc_mega_parse(const char *input_line, struct ao_log_mega *l);
546
547 #ifndef TRUE
548 #define TRUE 1
549 #define FALSE 0
550 #endif
551
552 /* Conversion functions */
553 double
554 cc_pressure_to_altitude(double pressure);
555
556 double
557 cc_altitude_to_pressure(double altitude);
558
559 double
560 cc_altitude_to_temperature(double altitude);
561
562 double
563 cc_barometer_to_pressure(double baro);
564
565 double
566 cc_barometer_to_altitude(double baro);
567
568 double
569 cc_accelerometer_to_acceleration(double accel, double ground_accel);
570
571 double
572 cc_thermometer_to_temperature(double thermo);
573
574 double
575 cc_battery_to_voltage(double battery);
576
577 double
578 cc_ignitor_to_voltage(double ignite);
579
580 void
581 cc_great_circle (double start_lat, double start_lon,
582                  double end_lat, double end_lon,
583                  double *dist, double *bearing);
584
585 void
586 cc_timedata_limits(struct cc_timedata *d, double min_time, double max_time, int *start, int *stop);
587
588 int
589 cc_timedata_min(struct cc_timedata *d, double min_time, double max_time);
590
591 int
592 cc_timedata_min_mag(struct cc_timedata *d, double min_time, double max_time);
593
594 int
595 cc_timedata_max(struct cc_timedata *d, double min_time, double max_time);
596
597 int
598 cc_timedata_max_mag(struct cc_timedata *d, double min_time, double max_time);
599
600 double
601 cc_timedata_average(struct cc_timedata *d, double min_time, double max_time);
602
603 double
604 cc_timedata_average_mag(struct cc_timedata *d, double min_time, double max_time);
605
606 int
607 cc_perioddata_limits(struct cc_perioddata *d, double min_time, double max_time, int *start, int *stop);
608
609 int
610 cc_perioddata_min(struct cc_perioddata *d, double min_time, double max_time);
611
612 int
613 cc_perioddata_min_mag(struct cc_perioddata *d, double min_time, double max_time);
614
615 int
616 cc_perioddata_max(struct cc_perioddata *d, double min_time, double max_time);
617
618 int
619 cc_perioddata_max_mag(struct cc_perioddata *d, double min_time, double max_time);
620
621 double
622 cc_perioddata_average(struct cc_perioddata *d, double min_time, double max_time);
623
624 double
625 cc_perioddata_average_mag(struct cc_perioddata *d, double min_time, double max_time);
626
627 double *
628 cc_low_pass(double *data, int data_len, double omega_pass, double omega_stop, double error);
629
630 struct cc_perioddata *
631 cc_period_make(struct cc_timedata *td, double start_time, double stop_time);
632
633 struct cc_perioddata *
634 cc_period_low_pass(struct cc_perioddata *raw, double omega_pass, double omega_stop, double error);
635
636 struct cc_timedata *
637 cc_timedata_convert(struct cc_timedata *d, double (*f)(double v, double a), double a);
638
639 struct cc_timedata *
640 cc_timedata_integrate(struct cc_timedata *d, double min_time, double max_time);
641
642 struct cc_perioddata *
643 cc_perioddata_differentiate(struct cc_perioddata *i);
644
645 struct cc_flightcooked *
646 cc_flight_cook(struct cc_flightraw *raw);
647
648 void
649 cc_flightcooked_free(struct cc_flightcooked *cooked);
650
651 #endif /* _CC_H_ */