PROGS= telemetrum.ihx tidongle.ihx \
teleterra.ihx teledongle.ihx
-HOST_PROGS=ao_flight_test
+HOST_PROGS=ao_flight_test ao_gps_test
PCDB=$(PROGS:.ihx=.cdb)
PLNK=$(PROGS:.ihx=.lnk)
ao_flight_test: ao_flight.c ao_flight_test.c
cc -g -o $@ ao_flight_test.c
+
+ao_gps_test: ao_gps.c ao_gps_test.c ao_host.h
+ cc -g -o $@ ao_gps_test.c -lpthread
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
*/
+#ifndef AO_GPS_TEST
#include "ao.h"
+#endif
__xdata uint8_t ao_gps_mutex;
__xdata struct ao_gps_data ao_gps_data;
-const char ao_gps_config[] = {
- /* Serial param - binary, 4800 baud, 8 bits, 1 stop, no parity */
+const char ao_gps_set_binary[] = {
'$', 'P', 'S', 'R', 'F', '1', '0', '0', ',', '0', ',',
- '4', '8', '0', '0', ',', '8', ',', '1', ',', '0', '*',
- '0', 'F', '\r','\n',
+ '9', '6', '0', '0', ',', '8', ',', '1', ',', '0', '*',
+ '0', 'C', '\r','\n',
+
+ 0xa0, 0xa2, 0x00, 0x09, /* length 9 bytes */
+ 134, /* Set binary serial port */
+ 0, 0, 0x25, 0x80, /* 9600 baud */
+ 8, /* data bits */
+ 1, /* stop bits */
+ 0, /* parity */
+ 0, /* pad */
+ 0x01, 0x34, 0xb0, 0xb3,
+};
+const char ao_gps_config[] = {
0xa0, 0xa2, 0x00, 0x0e, /* length: 14 bytes */
136, /* mode control */
0, 0, /* reserved */
{
uint8_t i, offset;
+ printf("parse 41\n");
for (i = 0; ; i++) {
offset = geodetic_nav_data_packet[i].offset;
switch (geodetic_nav_data_packet[i].type) {
case SIRF_END:
+ printf("parse 41 done\n");
return;
case SIRF_DISCARD:
+ printf("parse 41 discard %d\n", offset);
sirf_discard(offset);
break;
case SIRF_U8:
+ printf("parse 41 u8 %d\n", offset);
sirf_u8(offset);
break;
case SIRF_U16:
+ printf("parse 41 u16 %d\n", offset);
sirf_u16(offset);
break;
case SIRF_U32:
+ printf("parse 41 u32 %d\n", offset);
sirf_u32(offset);
break;
}
}
}
+void
+ao_gps_setup(void) __reentrant
+{
+ uint8_t i, j;
+ for (j = 0; j < 2; j++) {
+#ifdef AO_GPS_TEST
+ ao_serial_set_speed(j);
+#endif
+ for (i = 0; i < sizeof (ao_gps_set_binary); i++)
+ ao_serial_putchar(ao_gps_set_binary[i]);
+ }
+}
+
void
ao_gps(void) __reentrant
{
- char c;
uint8_t i;
uint16_t cksum;
- for (i = 0; (c = ao_gps_config[i]); i++)
- ao_serial_putchar(c);
+ for (i = 0; i < sizeof (ao_gps_config); i++)
+ ao_serial_putchar(ao_gps_config[i]);
for (;;) {
/* Locate the begining of the next record */
while (ao_sirf_byte() != 0xa0)
/* message ID */
i = data_byte (); /* 0 */
+ printf ("message %d len %d\n", i, ao_sirf_len);
switch (i) {
case 41:
- if (ao_sirf_len < 91)
+ if (ao_sirf_len < 90)
break;
ao_sirf_parse_41();
break;
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
*/
+#ifndef AO_GPS_TEST
#include "ao.h"
+#endif
struct ao_gps_split {
uint8_t positive;
--- /dev/null
+/*
+ * 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.
+ */
+
+#define AO_GPS_TEST
+#include "ao_host.h"
+#include <termios.h>
+#include <errno.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <pthread.h>
+#include <semaphore.h>
+#define AO_GPS_NUM_SAT_MASK (0xf << 0)
+#define AO_GPS_NUM_SAT_SHIFT (0)
+
+#define AO_GPS_VALID (1 << 4)
+
+struct ao_gps_data {
+ uint8_t hour;
+ uint8_t minute;
+ uint8_t second;
+ uint8_t flags;
+ int32_t latitude; /* degrees * 10⁷ */
+ int32_t longitude; /* degrees * 10⁷ */
+ int16_t altitude; /* m */
+ uint16_t ground_speed; /* cm/s */
+ uint8_t course; /* degrees / 2 */
+ uint8_t hdop; /* * 5 */
+ int16_t climb_rate; /* cm/s */
+ uint16_t h_error; /* m */
+ uint16_t v_error; /* m */
+};
+
+void
+ao_mutex_get(uint8_t *mutex)
+{
+}
+
+void
+ao_mutex_put(uint8_t *mutex)
+{
+}
+
+static int
+ao_gps_fd;
+
+static void
+ao_dbg_char(char c)
+{
+ char line[128];
+ line[0] = '\0';
+ if (c < ' ') {
+ if (c == '\n')
+ sprintf (line, "\n");
+ else
+ sprintf (line, "\\%02x", ((int) c) & 0xff);
+ } else {
+ sprintf (line, "%c", c);
+ }
+ write(1, line, strlen(line));
+}
+
+#define QUEUE_LEN 4096
+
+static char input_queue[QUEUE_LEN];
+int input_head, input_tail;
+
+static sem_t input_semaphore;
+
+char
+ao_serial_getchar(void)
+{
+ char c;
+ int value;
+ char line[100];
+
+ sem_getvalue(&input_semaphore, &value);
+// printf ("ao_serial_getchar %d\n", value);
+ sem_wait(&input_semaphore);
+ c = input_queue[input_head];
+ input_head = (input_head + 1) % QUEUE_LEN;
+// sprintf (line, "%02x\n", ((int) c) & 0xff);
+// write(1, line, strlen(line));
+ return c;
+}
+
+void *
+ao_gps_input(void *arg)
+{
+ int i;
+ char c;
+
+ printf("ao_gps_input\n");
+ for (;;) {
+ i = read(ao_gps_fd, &c, 1);
+ if (i == 1) {
+ int v;
+
+ input_queue[input_tail] = c;
+ input_tail = (input_tail + 1) % QUEUE_LEN;
+ sem_post(&input_semaphore);
+ sem_getvalue(&input_semaphore, &v);
+// printf ("ao_gps_input %02x %d\n", ((int) c) & 0xff, v);
+ fflush(stdout);
+ continue;
+ }
+ if (i < 0 && (errno == EINTR || errno == EAGAIN))
+ continue;
+ perror("getchar");
+ exit(1);
+ }
+}
+
+void
+ao_serial_putchar(char c)
+{
+ int i;
+
+ ao_dbg_char(c);
+ for (;;) {
+ i = write(ao_gps_fd, &c, 1);
+ if (i == 1)
+ break;
+ if (i < 0 && (errno == EINTR || errno == EAGAIN))
+ continue;
+ perror("putchar");
+ exit(1);
+ }
+}
+
+static void
+ao_serial_set_speed(uint8_t fast)
+{
+ int fd = ao_gps_fd;
+ struct termios termios;
+
+ tcdrain(fd);
+ tcgetattr(fd, &termios);
+ cfsetspeed(&termios, fast ? B9600 : B4800);
+ tcsetattr(fd, TCSAFLUSH, &termios);
+ tcflush(fd, TCIFLUSH);
+}
+
+#include "ao_gps_print.c"
+#include "ao_gps.c"
+
+void
+ao_dump_state(void)
+{
+ printf ("dump state\n");
+}
+
+int
+ao_gps_open(const char *tty)
+{
+ struct termios termios;
+ int fd;
+
+ fd = open (tty, O_RDWR);
+ if (fd < 0)
+ return -1;
+
+ tcgetattr(fd, &termios);
+ cfmakeraw(&termios);
+ cfsetspeed(&termios, B4800);
+ tcsetattr(fd, TCSAFLUSH, &termios);
+
+ tcdrain(fd);
+ tcflush(fd, TCIFLUSH);
+ return fd;
+}
+
+pthread_t input_thread;
+
+int
+main (int argc, char **argv)
+{
+ char *gps_file = "/dev/ttyUSB0";
+
+ ao_gps_fd = ao_gps_open(gps_file);
+ if (ao_gps_fd < 0) {
+ perror (gps_file);
+ exit (1);
+ }
+ ao_gps_setup();
+ sem_init(&input_semaphore, 0, 0);
+ if (pthread_create(&input_thread, NULL, ao_gps_input, NULL) != 0)
+ perror("pthread_create");
+ ao_gps();
+}
--- /dev/null
+/*
+ * 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.
+ */
+
+#define _GNU_SOURCE
+
+#include <stddef.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#define AO_ADC_RING 64
+#define ao_adc_ring_next(n) (((n) + 1) & (AO_ADC_RING - 1))
+#define ao_adc_ring_prev(n) (((n) - 1) & (AO_ADC_RING - 1))
+
+/*
+ * One set of samples read from the A/D converter
+ */
+struct ao_adc {
+ uint16_t tick; /* tick when the sample was read */
+ int16_t accel; /* accelerometer */
+ int16_t pres; /* pressure sensor */
+ int16_t temp; /* temperature sensor */
+ int16_t v_batt; /* battery voltage */
+ int16_t sense_d; /* drogue continuity sense */
+ int16_t sense_m; /* main continuity sense */
+};
+
+#define __pdata
+#define __data
+#define __xdata
+#define __code
+#define __reentrant
+
+enum ao_flight_state {
+ ao_flight_startup = 0,
+ ao_flight_idle = 1,
+ ao_flight_pad = 2,
+ ao_flight_boost = 3,
+ ao_flight_fast = 4,
+ ao_flight_coast = 5,
+ ao_flight_drogue = 6,
+ ao_flight_main = 7,
+ ao_flight_landed = 8,
+ ao_flight_invalid = 9
+};
+
+struct ao_adc ao_adc_ring[AO_ADC_RING];
+uint8_t ao_adc_head;
+
+#define ao_led_on(l)
+#define ao_led_off(l)
+#define ao_timer_set_adc_interval(i)
+#define ao_wakeup(wchan) ao_dump_state()
+#define ao_cmd_register(c)
+#define ao_usb_disable()
+#define ao_telemetry_set_interval(x)
+
+enum ao_igniter {
+ ao_igniter_drogue = 0,
+ ao_igniter_main = 1
+};
+
+void
+ao_ignite(enum ao_igniter igniter)
+{
+ printf ("ignite %s\n", igniter == ao_igniter_drogue ? "drogue" : "main");
+}
+
+struct ao_task {
+ int dummy;
+};
+
+#define ao_add_task(t,f,n)
+
+#define ao_log_start()
+#define ao_log_stop()
+
+#define AO_MS_TO_TICKS(ms) ((ms) / 10)
+#define AO_SEC_TO_TICKS(s) ((s) * 100)
+
+#define AO_FLIGHT_TEST
+
+struct ao_adc ao_adc_static;
+
+FILE *emulator_in;
+
+void
+ao_dump_state(void);
+
+void
+ao_sleep(void *wchan);
+
+const char const * const ao_state_names[] = {
+ "startup", "idle", "pad", "boost", "fast",
+ "coast", "drogue", "main", "landed", "invalid"
+};
+
+struct ao_cmds {
+ char cmd;
+ void (*func)(void);
+ const char *help;
+};
+
+
+static int16_t altitude_table[2048] = {
+#include "altitude.h"
+};
+
+int16_t
+ao_pres_to_altitude(int16_t pres) __reentrant
+{
+ pres = pres >> 4;
+ if (pres < 0) pres = 0;
+ if (pres > 2047) pres = 2047;
+ return altitude_table[pres];
+}
+
+int16_t
+ao_altitude_to_pres(int16_t alt) __reentrant
+{
+ int16_t pres;
+
+ for (pres = 0; pres < 2047; pres++)
+ if (altitude_table[pres] <= alt)
+ break;
+ return pres << 4;
+}
+
+struct ao_config {
+ uint16_t main_deploy;
+ int16_t accel_zero_g;
+};
+
+#define ao_config_get()
+
+struct ao_config ao_config = { 250, 16000 };
projects / fw / altos / commitdiff