summaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-rw-r--r--src/drivers/ao_25lc1024.c241
-rw-r--r--src/drivers/ao_25lc1024.h41
-rw-r--r--src/drivers/ao_at45db161d.c318
-rw-r--r--src/drivers/ao_at45db161d.h45
-rw-r--r--src/drivers/ao_btm.c302
-rw-r--r--src/drivers/ao_companion.c132
-rw-r--r--src/drivers/ao_gps_sirf.c442
-rw-r--r--src/drivers/ao_gps_skytraq.c490
-rw-r--r--src/drivers/ao_m25.c380
9 files changed, 2391 insertions, 0 deletions
diff --git a/src/drivers/ao_25lc1024.c b/src/drivers/ao_25lc1024.c
new file mode 100644
index 00000000..738f8ce6
--- /dev/null
+++ b/src/drivers/ao_25lc1024.c
@@ -0,0 +1,241 @@
+/*
+ * 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 "ao.h"
+#include "ao_25lc1024.h"
+
+#define EE_BLOCK_SIZE ((uint16_t) (256))
+#define EE_BLOCK_SHIFT 8
+#define EE_DEVICE_SIZE ((uint32_t) 128 * (uint32_t) 1024)
+
+/* Total bytes of available storage */
+__pdata uint32_t ao_storage_total;
+
+/* Block size - device is erased in these units. At least 256 bytes */
+__pdata uint32_t ao_storage_block;
+
+/* Byte offset of config block. Will be ao_storage_block bytes long */
+__pdata uint32_t ao_storage_config;
+
+/* Storage unit size - device reads and writes must be within blocks of this size. Usually 256 bytes. */
+__pdata uint16_t ao_storage_unit;
+
+/*
+ * Using SPI on USART 0, with P1_2 as the chip select
+ */
+
+#define EE_CS P1_2
+#define EE_CS_INDEX 2
+
+static __xdata uint8_t ao_ee_mutex;
+
+#define ao_ee_delay() do { \
+ _asm nop _endasm; \
+ _asm nop _endasm; \
+ _asm nop _endasm; \
+} while(0)
+
+#define ao_ee_cs_low() ao_spi_get_bit(EE_CS)
+
+#define ao_ee_cs_high() ao_spi_put_bit(EE_CS)
+
+struct ao_ee_instruction {
+ uint8_t instruction;
+ uint8_t address[3];
+} __xdata ao_ee_instruction;
+
+static void
+ao_ee_write_enable(void)
+{
+ ao_ee_cs_low();
+ ao_ee_instruction.instruction = EE_WREN;
+ ao_spi_send(&ao_ee_instruction, 1);
+ ao_ee_cs_high();
+}
+
+static uint8_t
+ao_ee_rdsr(void)
+{
+ ao_ee_cs_low();
+ ao_ee_instruction.instruction = EE_RDSR;
+ ao_spi_send(&ao_ee_instruction, 1);
+ ao_spi_recv(&ao_ee_instruction, 1);
+ ao_ee_cs_high();
+ return ao_ee_instruction.instruction;
+}
+
+static void
+ao_ee_wrsr(uint8_t status)
+{
+ ao_ee_cs_low();
+ ao_ee_instruction.instruction = EE_WRSR;
+ ao_ee_instruction.address[0] = status;
+ ao_spi_send(&ao_ee_instruction, 2);
+ ao_ee_cs_high();
+}
+
+#define EE_BLOCK_NONE 0xffff
+
+static __xdata uint8_t ao_ee_data[EE_BLOCK_SIZE];
+static __pdata uint16_t ao_ee_block = EE_BLOCK_NONE;
+static __pdata uint8_t ao_ee_block_dirty;
+
+/* Write the current block to the EEPROM */
+static void
+ao_ee_write_block(void)
+{
+ uint8_t status;
+
+ status = ao_ee_rdsr();
+ if (status & (EE_STATUS_BP0|EE_STATUS_BP1|EE_STATUS_WPEN)) {
+ status &= ~(EE_STATUS_BP0|EE_STATUS_BP1|EE_STATUS_WPEN);
+ ao_ee_wrsr(status);
+ }
+ ao_ee_write_enable();
+ ao_ee_cs_low();
+ ao_ee_instruction.instruction = EE_WRITE;
+ ao_ee_instruction.address[0] = ao_ee_block >> 8;
+ ao_ee_instruction.address[1] = ao_ee_block;
+ ao_ee_instruction.address[2] = 0;
+ ao_spi_send(&ao_ee_instruction, 4);
+ ao_spi_send(ao_ee_data, EE_BLOCK_SIZE);
+ ao_ee_cs_high();
+ for (;;) {
+ uint8_t status = ao_ee_rdsr();
+ if ((status & EE_STATUS_WIP) == 0)
+ break;
+ }
+}
+
+/* Read the current block from the EEPROM */
+static void
+ao_ee_read_block(void)
+{
+ ao_ee_cs_low();
+ ao_ee_instruction.instruction = EE_READ;
+ ao_ee_instruction.address[0] = ao_ee_block >> 8;
+ ao_ee_instruction.address[1] = ao_ee_block;
+ ao_ee_instruction.address[2] = 0;
+ ao_spi_send(&ao_ee_instruction, 4);
+ ao_spi_recv(ao_ee_data, EE_BLOCK_SIZE);
+ ao_ee_cs_high();
+}
+
+static void
+ao_ee_flush_internal(void)
+{
+ if (ao_ee_block_dirty) {
+ ao_ee_write_block();
+ ao_ee_block_dirty = 0;
+ }
+}
+
+static void
+ao_ee_fill(uint16_t block)
+{
+ if (block != ao_ee_block) {
+ ao_ee_flush_internal();
+ ao_ee_block = block;
+ ao_ee_read_block();
+ }
+}
+
+uint8_t
+ao_storage_device_write(uint32_t pos, __xdata void *buf, uint16_t len) __reentrant
+{
+ uint16_t block = (uint16_t) (pos >> EE_BLOCK_SHIFT);
+
+ /* Transfer the data */
+ ao_mutex_get(&ao_ee_mutex); {
+ if (len != EE_BLOCK_SIZE)
+ ao_ee_fill(block);
+ else {
+ ao_ee_flush_internal();
+ ao_ee_block = block;
+ }
+ memcpy(ao_ee_data + (uint16_t) (pos & 0xff), buf, len);
+ ao_ee_block_dirty = 1;
+ } ao_mutex_put(&ao_ee_mutex);
+ return 1;
+}
+
+uint8_t
+ao_storage_device_read(uint32_t pos, __xdata void *buf, uint16_t len) __reentrant
+{
+ uint16_t block = (uint16_t) (pos >> EE_BLOCK_SHIFT);
+
+ /* Transfer the data */
+ ao_mutex_get(&ao_ee_mutex); {
+ ao_ee_fill(block);
+ memcpy(buf, ao_ee_data + (uint16_t) (pos & 0xff), len);
+ } ao_mutex_put(&ao_ee_mutex);
+ return 1;
+}
+
+void
+ao_storage_flush(void) __reentrant
+{
+ ao_mutex_get(&ao_ee_mutex); {
+ ao_ee_flush_internal();
+ } ao_mutex_put(&ao_ee_mutex);
+}
+
+uint8_t
+ao_storage_erase(uint32_t pos) __reentrant
+{
+ ao_mutex_get(&ao_ee_mutex); {
+ ao_ee_flush_internal();
+ ao_ee_block = (uint16_t) (pos >> EE_BLOCK_SHIFT);
+ memset(ao_ee_data, 0xff, EE_BLOCK_SIZE);
+ ao_ee_block_dirty = 1;
+ } ao_mutex_put(&ao_ee_mutex);
+ return 1;
+}
+
+static void
+ee_store(void) __reentrant
+{
+}
+
+void
+ao_storage_setup(void)
+{
+ if (ao_storage_total == 0) {
+ ao_storage_total = EE_DEVICE_SIZE;
+ ao_storage_block = EE_BLOCK_SIZE;
+ ao_storage_config = EE_DEVICE_SIZE - EE_BLOCK_SIZE;
+ ao_storage_unit = EE_BLOCK_SIZE;
+ }
+}
+
+void
+ao_storage_device_info(void) __reentrant
+{
+}
+
+/*
+ * To initialize the chip, set up the CS line and
+ * the SPI interface
+ */
+void
+ao_storage_device_init(void)
+{
+ /* set up CS */
+ EE_CS = 1;
+ P1DIR |= (1 << EE_CS_INDEX);
+ P1SEL &= ~(1 << EE_CS_INDEX);
+}
diff --git a/src/drivers/ao_25lc1024.h b/src/drivers/ao_25lc1024.h
new file mode 100644
index 00000000..44e52387
--- /dev/null
+++ b/src/drivers/ao_25lc1024.h
@@ -0,0 +1,41 @@
+/*
+ * 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.
+ */
+
+/* Defines for the 25LC1024 1Mbit SPI Bus Serial EEPROM */
+
+#ifndef _25LC1024_H_
+#define _25LC1024_H_
+
+#define EE_READ 0x03
+#define EE_WRITE 0x02
+#define EE_WREN 0x06
+#define EE_WRDI 0x04
+#define EE_RDSR 0x05
+#define EE_WRSR 0x01
+#define EE_PE 0x42
+#define EE_SE 0xd8
+#define EE_CE 0xc7
+#define EE_RDID 0xab
+#define EE_DPD 0xb9
+
+#define EE_STATUS_WIP (1 << 0)
+#define EE_STATUS_WEL (1 << 1)
+#define EE_STATUS_BP0 (1 << 2)
+#define EE_STATUS_BP1 (1 << 3)
+#define EE_STATUS_WPEN (1 << 7)
+
+#endif /* _25LC1024_H_ */
diff --git a/src/drivers/ao_at45db161d.c b/src/drivers/ao_at45db161d.c
new file mode 100644
index 00000000..aee9877a
--- /dev/null
+++ b/src/drivers/ao_at45db161d.c
@@ -0,0 +1,318 @@
+/*
+ * 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 "ao.h"
+#include "ao_at45db161d.h"
+
+/* Total bytes of available storage */
+__pdata uint32_t ao_storage_total;
+
+/* Block size - device is erased in these units. At least 256 bytes */
+__pdata uint32_t ao_storage_block;
+
+/* Byte offset of config block. Will be ao_storage_block bytes long */
+__pdata uint32_t ao_storage_config;
+
+/* Storage unit size - device reads and writes must be within blocks of this size. Usually 256 bytes. */
+__pdata uint16_t ao_storage_unit;
+
+#define FLASH_CS P1_1
+#define FLASH_CS_INDEX 1
+
+#define FLASH_BLOCK_SIZE_MAX 512
+
+__xdata uint8_t ao_flash_mutex;
+
+#define ao_flash_delay() do { \
+ _asm nop _endasm; \
+ _asm nop _endasm; \
+ _asm nop _endasm; \
+} while(0)
+
+#define ao_flash_cs_low() ao_spi_get_bit(FLASH_CS)
+
+#define ao_flash_cs_high() ao_spi_put_bit(FLASH_CS)
+
+struct ao_flash_instruction {
+ uint8_t instruction;
+ uint8_t address[3];
+} __xdata ao_flash_instruction;
+
+static void
+ao_flash_set_pagesize_512(void)
+{
+ ao_flash_cs_low();
+ ao_flash_instruction.instruction = FLASH_SET_CONFIG;
+ ao_flash_instruction.address[0] = FLASH_SET_512_BYTE_0;
+ ao_flash_instruction.address[1] = FLASH_SET_512_BYTE_1;
+ ao_flash_instruction.address[2] = FLASH_SET_512_BYTE_2;
+ ao_spi_send(&ao_flash_instruction, 4);
+ ao_flash_cs_high();
+}
+
+
+static uint8_t
+ao_flash_read_status(void)
+{
+ ao_flash_cs_low();
+ ao_flash_instruction.instruction = FLASH_READ_STATUS;
+ ao_spi_send(&ao_flash_instruction, 1);
+ ao_spi_recv(&ao_flash_instruction, 1);
+ ao_flash_cs_high();
+ return ao_flash_instruction.instruction;
+}
+
+#define FLASH_BLOCK_NONE 0xffff
+
+static __xdata uint8_t ao_flash_data[FLASH_BLOCK_SIZE_MAX];
+static __pdata uint16_t ao_flash_block = FLASH_BLOCK_NONE;
+static __pdata uint8_t ao_flash_block_dirty;
+static __pdata uint8_t ao_flash_write_pending;
+static __pdata uint8_t ao_flash_setup_done;
+static __pdata uint8_t ao_flash_block_shift;
+static __pdata uint16_t ao_flash_block_size;
+static __pdata uint16_t ao_flash_block_mask;
+
+void
+ao_storage_setup(void) __reentrant
+{
+ uint8_t status;
+
+ if (ao_flash_setup_done)
+ return;
+
+ ao_mutex_get(&ao_flash_mutex);
+ if (ao_flash_setup_done) {
+ ao_mutex_put(&ao_flash_mutex);
+ return;
+ }
+
+ /* On first use, check to see if the flash chip has
+ * been programmed to use 512 byte pages. If not, do so.
+ * And then, because the flash part must be power cycled
+ * for that change to take effect, panic.
+ */
+ status = ao_flash_read_status();
+
+ if (!(status & FLASH_STATUS_PAGESIZE_512)) {
+ ao_flash_set_pagesize_512();
+ ao_panic(AO_PANIC_FLASH);
+ }
+
+ switch (status & 0x3c) {
+
+ /* AT45DB321D */
+ case 0x34:
+ ao_flash_block_shift = 9;
+ ao_storage_total = ((uint32_t) 4 * (uint32_t) 1024 * (uint32_t) 1024);
+ break;
+
+ /* AT45DB161D */
+ case 0x2c:
+ ao_flash_block_shift = 9;
+ ao_storage_total = ((uint32_t) 2 * (uint32_t) 1024 * (uint32_t) 1024);
+ break;
+
+ /* AT45DB081D */
+ case 0x24:
+ ao_flash_block_shift = 8;
+ ao_storage_total = ((uint32_t) 1024 * (uint32_t) 1024);
+ break;
+
+ /* AT45DB041D */
+ case 0x1c:
+ ao_flash_block_shift = 8;
+ ao_storage_total = ((uint32_t) 512 * (uint32_t) 1024);
+ break;
+
+ /* AT45DB021D */
+ case 0x14:
+ ao_flash_block_shift = 8;
+ ao_storage_total = ((uint32_t) 256 * (uint32_t) 1024);
+ break;
+
+ /* AT45DB011D */
+ case 0x0c:
+ ao_flash_block_shift = 8;
+ ao_storage_total = ((uint32_t) 128 * (uint32_t) 1024);
+ break;
+
+ default:
+ ao_panic(AO_PANIC_FLASH);
+ }
+ ao_flash_block_size = 1 << ao_flash_block_shift;
+ ao_flash_block_mask = ao_flash_block_size - 1;
+
+ ao_storage_block = ao_flash_block_size;
+ ao_storage_config = ao_storage_total - ao_storage_block;
+ ao_storage_unit = ao_flash_block_size;
+
+ ao_flash_setup_done = 1;
+ ao_mutex_put(&ao_flash_mutex);
+}
+
+static void
+ao_flash_wait_write(void)
+{
+ if (ao_flash_write_pending) {
+ for (;;) {
+ uint8_t status = ao_flash_read_status();
+ if ((status & FLASH_STATUS_RDY))
+ break;
+ }
+ ao_flash_write_pending = 0;
+ }
+}
+
+/* Write the current block to the FLASHPROM */
+static void
+ao_flash_write_block(void)
+{
+ ao_flash_wait_write();
+ ao_flash_cs_low();
+ ao_flash_instruction.instruction = FLASH_WRITE;
+
+ /* 13/14 block bits + 9/8 byte bits (always 0) */
+ ao_flash_instruction.address[0] = ao_flash_block >> (16 - ao_flash_block_shift);
+ ao_flash_instruction.address[1] = ao_flash_block << (ao_flash_block_shift - 8);
+ ao_flash_instruction.address[2] = 0;
+ ao_spi_send(&ao_flash_instruction, 4);
+ ao_spi_send(ao_flash_data, ao_storage_block);
+ ao_flash_cs_high();
+ ao_flash_write_pending = 1;
+}
+
+/* Read the current block from the FLASHPROM */
+static void
+ao_flash_read_block(void)
+{
+ ao_flash_wait_write();
+ ao_flash_cs_low();
+ ao_flash_instruction.instruction = FLASH_READ;
+
+ /* 13/14 block bits + 9/8 byte bits (always 0) */
+ ao_flash_instruction.address[0] = ao_flash_block >> (16 - ao_flash_block_shift);
+ ao_flash_instruction.address[1] = ao_flash_block << (ao_flash_block_shift - 8);
+ ao_flash_instruction.address[2] = 0;
+ ao_spi_send(&ao_flash_instruction, 4);
+ ao_spi_recv(ao_flash_data, ao_flash_block_size);
+ ao_flash_cs_high();
+}
+
+static void
+ao_flash_flush_internal(void)
+{
+ if (ao_flash_block_dirty) {
+ ao_flash_write_block();
+ ao_flash_block_dirty = 0;
+ }
+}
+
+static void
+ao_flash_fill(uint16_t block)
+{
+ if (block != ao_flash_block) {
+ ao_flash_flush_internal();
+ ao_flash_block = block;
+ ao_flash_read_block();
+ }
+}
+
+uint8_t
+ao_storage_device_write(uint32_t pos, __xdata void *buf, uint16_t len) __reentrant
+{
+ uint16_t block = (uint16_t) (pos >> ao_flash_block_shift);
+
+ /* Transfer the data */
+ ao_mutex_get(&ao_flash_mutex); {
+ if (len != ao_flash_block_size)
+ ao_flash_fill(block);
+ else {
+ ao_flash_flush_internal();
+ ao_flash_block = block;
+ }
+ memcpy(ao_flash_data + (uint16_t) (pos & ao_flash_block_mask),
+ buf,
+ len);
+ ao_flash_block_dirty = 1;
+ } ao_mutex_put(&ao_flash_mutex);
+ return 1;
+}
+
+uint8_t
+ao_storage_device_read(uint32_t pos, __xdata void *buf, uint16_t len) __reentrant
+{
+ uint16_t block = (uint16_t) (pos >> ao_flash_block_shift);
+
+ /* Transfer the data */
+ ao_mutex_get(&ao_flash_mutex); {
+ ao_flash_fill(block);
+ memcpy(buf,
+ ao_flash_data + (uint16_t) (pos & ao_flash_block_mask),
+ len);
+ } ao_mutex_put(&ao_flash_mutex);
+ return 1;
+}
+
+void
+ao_storage_flush(void) __reentrant
+{
+ ao_mutex_get(&ao_flash_mutex); {
+ ao_flash_flush_internal();
+ } ao_mutex_put(&ao_flash_mutex);
+}
+
+uint8_t
+ao_storage_erase(uint32_t pos) __reentrant
+{
+ ao_mutex_get(&ao_flash_mutex); {
+ ao_flash_flush_internal();
+ ao_flash_block = (uint16_t) (pos >> ao_flash_block_shift);
+ memset(ao_flash_data, 0xff, ao_flash_block_size);
+ ao_flash_block_dirty = 1;
+ } ao_mutex_put(&ao_flash_mutex);
+ return 1;
+}
+
+void
+ao_storage_device_info(void) __reentrant
+{
+ uint8_t status;
+
+ ao_storage_setup();
+ ao_mutex_get(&ao_flash_mutex); {
+ status = ao_flash_read_status();
+ printf ("Flash status: 0x%02x\n", status);
+ printf ("Flash block shift: %d\n", ao_flash_block_shift);
+ printf ("Flash block size: %d\n", ao_flash_block_size);
+ printf ("Flash block mask: %d\n", ao_flash_block_mask);
+ printf ("Flash device size: %ld\n", ao_storage_total);
+ } ao_mutex_put(&ao_flash_mutex);
+}
+
+/*
+ * To initialize the chip, set up the CS line and
+ * the SPI interface
+ */
+void
+ao_storage_device_init(void)
+{
+ /* set up CS */
+ FLASH_CS = 1;
+ P1DIR |= (1 << FLASH_CS_INDEX);
+ P1SEL &= ~(1 << FLASH_CS_INDEX);
+}
diff --git a/src/drivers/ao_at45db161d.h b/src/drivers/ao_at45db161d.h
new file mode 100644
index 00000000..9ee6f1b6
--- /dev/null
+++ b/src/drivers/ao_at45db161d.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright © 2010 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.
+ */
+
+/* Defines for the Atmel AT45DB161D 16Mbit SPI Bus DataFlash® */
+
+#ifndef _AT45DB161D_H_
+#define _AT45DB161D_H_
+
+/*
+ * We reserve the last block on the device for
+ * configuration space. Writes and reads in this
+ * area return errors.
+ */
+
+
+#define FLASH_READ 0x03
+#define FLASH_WRITE 0x82
+#define FLASH_PAGE_ERASE 0x81
+#define FLASH_READ_STATUS 0xd7
+#define FLASH_SET_CONFIG 0x3d
+
+#define FLASH_SET_512_BYTE_0 0x2a
+#define FLASH_SET_512_BYTE_1 0x80
+#define FLASH_SET_512_BYTE_2 0xa6
+
+#define FLASH_STATUS_RDY (1 << 7)
+#define FLASH_STATUS_COMP (1 << 6)
+#define FLASH_STATUS_PROTECT (1 << 1)
+#define FLASH_STATUS_PAGESIZE_512 (1 << 0)
+
+#endif /* _AT45DB161D_H_ */
diff --git a/src/drivers/ao_btm.c b/src/drivers/ao_btm.c
new file mode 100644
index 00000000..44155ec1
--- /dev/null
+++ b/src/drivers/ao_btm.c
@@ -0,0 +1,302 @@
+/*
+ * Copyright © 2011 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 "ao.h"
+
+int8_t ao_btm_stdio;
+__xdata uint8_t ao_btm_connected;
+
+#define AO_BTM_MAX_REPLY 16
+__xdata char ao_btm_reply[AO_BTM_MAX_REPLY];
+
+extern volatile __xdata struct ao_fifo ao_usart1_rx_fifo;
+
+/*
+ * Read a line of data from the serial port, truncating
+ * it after a few characters.
+ */
+
+uint8_t
+ao_btm_get_line(void)
+{
+ uint8_t ao_btm_reply_len = 0;
+ char c;
+
+ for (;;) {
+
+ while ((c = ao_serial_pollchar()) != AO_READ_AGAIN) {
+ if (ao_btm_reply_len < sizeof (ao_btm_reply))
+ ao_btm_reply[ao_btm_reply_len++] = c;
+ if (c == '\r' || c == '\n')
+ goto done;
+ }
+ for (c = 0; c < 10; c++) {
+ ao_delay(AO_MS_TO_TICKS(10));
+ if (!ao_fifo_empty(ao_usart1_rx_fifo))
+ break;
+ }
+ if (c == 10)
+ goto done;
+ }
+done:
+ for (c = ao_btm_reply_len; c < sizeof (ao_btm_reply);)
+ ao_btm_reply[c++] = '\0';
+ return ao_btm_reply_len;
+}
+
+/*
+ * Drain the serial port completely
+ */
+void
+ao_btm_drain()
+{
+ while (ao_btm_get_line())
+ ;
+}
+
+/*
+ * Set the stdio echo for the bluetooth link
+ */
+void
+ao_btm_echo(uint8_t echo)
+{
+ ao_stdios[ao_btm_stdio].echo = echo;
+}
+
+/*
+ * Delay between command charaters; the BT module
+ * can't keep up with 57600 baud
+ */
+
+void
+ao_btm_putchar(char c)
+{
+ ao_serial_putchar(c);
+ ao_delay(1);
+}
+
+/*
+ * Wait for the bluetooth device to return
+ * status from the previously executed command
+ */
+uint8_t
+ao_btm_wait_reply(void)
+{
+ for (;;) {
+ ao_btm_get_line();
+ if (!strncmp(ao_btm_reply, "OK", 2))
+ return 1;
+ if (!strncmp(ao_btm_reply, "ERROR", 5))
+ return -1;
+ if (ao_btm_reply[0] == '\0')
+ return 0;
+ }
+}
+
+void
+ao_btm_string(__code char *cmd)
+{
+ char c;
+
+ while (c = *cmd++)
+ ao_btm_putchar(c);
+}
+
+uint8_t
+ao_btm_cmd(__code char *cmd)
+{
+ ao_btm_drain();
+ ao_btm_string(cmd);
+ return ao_btm_wait_reply();
+}
+
+uint8_t
+ao_btm_set_name(void)
+{
+ char sn[8];
+ char *s = sn + 8;
+ char c;
+ int n;
+ ao_btm_string("ATN=TeleBT-");
+ *--s = '\0';
+ *--s = '\r';
+ n = ao_serial_number;
+ do {
+ *--s = '0' + n % 10;
+ } while (n /= 10);
+ while ((c = *s++))
+ ao_btm_putchar(c);
+ return ao_btm_wait_reply();
+}
+
+uint8_t
+ao_btm_try_speed(uint8_t speed)
+{
+ ao_serial_set_speed(speed);
+ ao_btm_drain();
+ (void) ao_btm_cmd("\rATE0\rATQ0\r");
+ if (ao_btm_cmd("AT\r") == 1)
+ return 1;
+ return 0;
+}
+
+/*
+ * A thread to initialize the bluetooth device and
+ * hang around to blink the LED when connected
+ */
+void
+ao_btm(void)
+{
+ /*
+ * Wait for the bluetooth device to boot
+ */
+ ao_delay(AO_SEC_TO_TICKS(3));
+
+#if HAS_BEEP
+ ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200));
+#endif
+
+ /*
+ * The first time we connect, the BTM-180 comes up at 19200 baud.
+ * After that, it will remember and come up at 57600 baud. So, see
+ * if it is already running at 57600 baud, and if that doesn't work
+ * then tell it to switch to 57600 from 19200 baud.
+ */
+ while (!ao_btm_try_speed(AO_SERIAL_SPEED_57600)) {
+ ao_delay(AO_SEC_TO_TICKS(1));
+ if (ao_btm_try_speed(AO_SERIAL_SPEED_19200))
+ ao_btm_cmd("ATL4\r");
+ ao_delay(AO_SEC_TO_TICKS(1));
+ }
+
+ /* Disable echo */
+ ao_btm_cmd("ATE0\r");
+
+ /* Enable flow control */
+ ao_btm_cmd("ATC1\r");
+
+ /* Set the reported name to something we can find on the host */
+ ao_btm_set_name();
+
+ /* Turn off status reporting */
+ ao_btm_cmd("ATQ1\r");
+
+ ao_btm_stdio = ao_add_stdio(ao_serial_pollchar,
+ ao_serial_putchar,
+ NULL);
+ ao_btm_echo(0);
+
+ for (;;) {
+ while (!ao_btm_connected)
+ ao_sleep(&ao_btm_connected);
+ while (ao_btm_connected) {
+ ao_led_for(AO_LED_GREEN, AO_MS_TO_TICKS(20));
+ ao_delay(AO_SEC_TO_TICKS(3));
+ }
+ }
+}
+
+__xdata struct ao_task ao_btm_task;
+
+#if BT_LINK_ON_P2
+#define BT_PICTL_ICON PICTL_P2ICON
+#define BT_PIFG P2IFG
+#define BT_PDIR P2DIR
+#define BT_PINP P2INP
+#define BT_IEN2_PIE IEN2_P2IE
+#endif
+#if BT_LINK_ON_P1
+#define BT_PICTL_ICON PICTL_P1ICON
+#define BT_PIFG P1IFG
+#define BT_PDIR P1DIR
+#define BT_PINP P1INP
+#define BT_IEN2_PIE IEN2_P1IE
+#endif
+
+void
+ao_btm_check_link() __critical
+{
+ /* Check the pin and configure the interrupt detector to wait for the
+ * pin to flip the other way
+ */
+ if (BT_LINK_PIN) {
+ ao_btm_connected = 0;
+ PICTL |= BT_PICTL_ICON;
+ } else {
+ ao_btm_connected = 1;
+ PICTL &= ~BT_PICTL_ICON;
+ }
+}
+
+void
+ao_btm_isr(void)
+#if BT_LINK_ON_P1
+ __interrupt 15
+#endif
+{
+#if BT_LINK_ON_P1
+ P1IF = 0;
+#endif
+ if (BT_PIFG & (1 << BT_LINK_PIN_INDEX)) {
+ ao_btm_check_link();
+ ao_wakeup(&ao_btm_connected);
+ }
+ BT_PIFG = 0;
+}
+
+void
+ao_btm_init (void)
+{
+ ao_serial_init();
+ ao_serial_set_speed(AO_SERIAL_SPEED_19200);
+
+#if BT_LINK_ON_P1
+ /*
+ * Configure ser reset line
+ */
+
+ P1_6 = 0;
+ P1DIR |= (1 << 6);
+#endif
+
+ /*
+ * Configure link status line
+ */
+
+ /* Set pin to input */
+ BT_PDIR &= ~(1 << BT_LINK_PIN_INDEX);
+
+ /* Set pin to tri-state */
+ BT_PINP |= (1 << BT_LINK_PIN_INDEX);
+
+ /* Enable interrupts */
+ IEN2 |= BT_IEN2_PIE;
+
+ /* Check current pin state */
+ ao_btm_check_link();
+
+#if BT_LINK_ON_P2
+ /* Eable the pin interrupt */
+ PICTL |= PICTL_P2IEN;
+#endif
+#if BT_LINK_ON_P1
+ /* Enable pin interrupt */
+ P1IEN |= (1 << BT_LINK_PIN_INDEX);
+#endif
+
+ ao_add_task(&ao_btm_task, ao_btm, "bt");
+}
diff --git a/src/drivers/ao_companion.c b/src/drivers/ao_companion.c
new file mode 100644
index 00000000..4c8f4269
--- /dev/null
+++ b/src/drivers/ao_companion.c
@@ -0,0 +1,132 @@
+/*
+ * Copyright © 2011 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 "ao.h"
+
+#define ao_spi_slow() (U0GCR = (UxGCR_CPOL_NEGATIVE | \
+ UxGCR_CPHA_FIRST_EDGE | \
+ UxGCR_ORDER_MSB | \
+ (13 << UxGCR_BAUD_E_SHIFT)))
+
+#define ao_spi_fast() (U0GCR = (UxGCR_CPOL_NEGATIVE | \
+ UxGCR_CPHA_FIRST_EDGE | \
+ UxGCR_ORDER_MSB | \
+ (17 << UxGCR_BAUD_E_SHIFT)))
+
+#define COMPANION_SELECT() do { ao_spi_get_bit(COMPANION_CS); ao_spi_slow(); } while (0)
+#define COMPANION_DESELECT() do { ao_spi_fast(); ao_spi_put_bit(COMPANION_CS); } while (0)
+
+static __xdata struct ao_companion_command ao_companion_command;
+__xdata struct ao_companion_setup ao_companion_setup;
+
+__xdata uint16_t ao_companion_data[AO_COMPANION_MAX_CHANNELS];
+__pdata uint8_t ao_companion_running;
+__xdata uint8_t ao_companion_mutex;
+
+static void
+ao_companion_send_command(uint8_t command)
+{
+ ao_companion_command.command = command;
+ ao_companion_command.flight_state = ao_flight_state;
+ ao_companion_command.tick = ao_time();
+ ao_companion_command.serial = ao_serial_number;
+ ao_companion_command.flight = ao_flight_number;
+ ao_spi_send(&ao_companion_command, sizeof (ao_companion_command));
+}
+
+static uint8_t
+ao_companion_get_setup(void)
+{
+ COMPANION_SELECT();
+ ao_companion_send_command(AO_COMPANION_SETUP);
+ ao_spi_recv(&ao_companion_setup, sizeof (ao_companion_setup));
+ COMPANION_DESELECT();
+ return (ao_companion_setup.board_id ==
+ ~ao_companion_setup.board_id_inverse);
+}
+
+static void
+ao_companion_get_data(void)
+{
+ COMPANION_SELECT();
+ ao_companion_send_command(AO_COMPANION_FETCH);
+ ao_mutex_get(&ao_companion_mutex);
+ ao_spi_recv(&ao_companion_data, ao_companion_setup.channels * 2);
+ ao_mutex_put(&ao_companion_mutex);
+ COMPANION_DESELECT();
+}
+
+static void
+ao_companion_notify(void)
+{
+ COMPANION_SELECT();
+ ao_companion_send_command(AO_COMPANION_NOTIFY);
+ COMPANION_DESELECT();
+}
+
+void
+ao_companion(void)
+{
+ uint8_t i;
+ while (!ao_flight_number)
+ ao_sleep(&ao_flight_number);
+ for (i = 0; i < 10; i++) {
+ ao_delay(AO_SEC_TO_TICKS(1));
+ if ((ao_companion_running = ao_companion_get_setup()))
+ break;
+ }
+ while (ao_companion_running) {
+ ao_alarm(ao_companion_setup.update_period);
+ if (ao_sleep(DATA_TO_XDATA(&ao_flight_state)))
+ ao_companion_get_data();
+ else
+ ao_companion_notify();
+ }
+ ao_exit();
+}
+
+void
+ao_companion_status(void) __reentrant
+{
+ uint8_t i;
+ printf("Companion running: %d\n", ao_companion_running);
+ printf("device: %d\n", ao_companion_setup.board_id);
+ printf("update period: %d\n", ao_companion_setup.update_period);
+ printf("channels: %d\n", ao_companion_setup.channels);
+ printf("data:");
+ for(i = 0; i < ao_companion_setup.channels; i++)
+ printf(" %5u", ao_companion_data[i]);
+ printf("\n");
+}
+
+__code struct ao_cmds ao_companion_cmds[] = {
+ { ao_companion_status, "L\0Companion link status" },
+ { 0, NULL },
+};
+
+static __xdata struct ao_task ao_companion_task;
+
+void
+ao_companion_init(void)
+{
+ COMPANION_CS_PORT |= COMPANION_CS_MASK; /* raise all CS pins */
+ COMPANION_CS_DIR |= COMPANION_CS_MASK; /* set CS pins as outputs */
+ COMPANION_CS_SEL &= ~COMPANION_CS_MASK; /* set CS pins as GPIO */
+
+ ao_cmd_register(&ao_companion_cmds[0]);
+ ao_add_task(&ao_companion_task, ao_companion, "companion");
+}
diff --git a/src/drivers/ao_gps_sirf.c b/src/drivers/ao_gps_sirf.c
new file mode 100644
index 00000000..f2abbf84
--- /dev/null
+++ b/src/drivers/ao_gps_sirf.c
@@ -0,0 +1,442 @@
+/*
+ * 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.
+ */
+
+#ifndef AO_GPS_TEST
+#include "ao.h"
+#endif
+
+__xdata uint8_t ao_gps_mutex;
+__pdata uint16_t ao_gps_tick;
+__xdata struct ao_telemetry_location ao_gps_data;
+__xdata struct ao_telemetry_satellite ao_gps_tracking_data;
+
+static const char ao_gps_set_nmea[] = "\r\n$PSRF100,0,57600,8,1,0*37\r\n";
+
+const char ao_gps_config[] = {
+
+ 0xa0, 0xa2, 0x00, 0x0e, /* length: 14 bytes */
+ 136, /* mode control */
+ 0, 0, /* reserved */
+ 0, /* degraded mode (allow 1-SV navigation) */
+ 0, 0, /* reserved */
+ 0, 0, /* user specified altitude */
+ 2, /* alt hold mode (disabled, require 3d fixes) */
+ 0, /* alt hold source (use last computed altitude) */
+ 0, /* reserved */
+ 10, /* Degraded time out (10 sec) */
+ 10, /* Dead Reckoning time out (10 sec) */
+ 0, /* Track smoothing (disabled) */
+ 0x00, 0x8e, 0xb0, 0xb3,
+
+ 0xa0, 0xa2, 0x00, 0x08, /* length: 8 bytes */
+ 166, /* Set message rate */
+ 2, /* enable/disable all messages */
+ 0, /* message id (ignored) */
+ 0, /* update rate (0 = disable) */
+ 0, 0, 0, 0, /* reserved */
+ 0x00, 0xa8, 0xb0, 0xb3,
+
+ 0xa0, 0xa2, 0x00, 0x02, /* length: 2 bytes */
+ 143, /* static navigation */
+ 0, /* disable */
+ 0x00, 0x8f, 0xb0, 0xb3,
+};
+
+#define NAV_TYPE_GPS_FIX_TYPE_MASK (7 << 0)
+#define NAV_TYPE_NO_FIX (0 << 0)
+#define NAV_TYPE_SV_KF (1 << 0)
+#define NAV_TYPE_2_SV_KF (2 << 0)
+#define NAV_TYPE_3_SV_KF (3 << 0)
+#define NAV_TYPE_4_SV_KF (4 << 0)
+#define NAV_TYPE_2D_LEAST_SQUARES (5 << 0)
+#define NAV_TYPE_3D_LEAST_SQUARES (6 << 0)
+#define NAV_TYPE_DR (7 << 0)
+#define NAV_TYPE_TRICKLE_POWER (1 << 3)
+#define NAV_TYPE_ALTITUDE_HOLD_MASK (3 << 4)
+#define NAV_TYPE_ALTITUDE_HOLD_NONE (0 << 4)
+#define NAV_TYPE_ALTITUDE_HOLD_KF (1 << 4)
+#define NAV_TYPE_ALTITUDE_HOLD_USER (2 << 4)
+#define NAV_TYPE_ALTITUDE_HOLD_ALWAYS (3 << 4)
+#define NAV_TYPE_DOP_LIMIT_EXCEEDED (1 << 6)
+#define NAV_TYPE_DGPS_APPLIED (1 << 7)
+#define NAV_TYPE_SENSOR_DR (1 << 8)
+#define NAV_TYPE_OVERDETERMINED (1 << 9)
+#define NAV_TYPE_DR_TIMEOUT_EXCEEDED (1 << 10)
+#define NAV_TYPE_FIX_MI_EDIT (1 << 11)
+#define NAV_TYPE_INVALID_VELOCITY (1 << 12)
+#define NAV_TYPE_ALTITUDE_HOLD_DISABLED (1 << 13)
+#define NAV_TYPE_DR_ERROR_STATUS_MASK (3 << 14)
+#define NAV_TYPE_DR_ERROR_STATUS_GPS_ONLY (0 << 14)
+#define NAV_TYPE_DR_ERROR_STATUS_DR_FROM_GPS (1 << 14)
+#define NAV_TYPE_DR_ERROR_STATUS_DR_SENSOR_ERROR (2 << 14)
+#define NAV_TYPE_DR_ERROR_STATUS_DR_IN_TEST (3 << 14)
+
+struct sirf_geodetic_nav_data {
+ uint16_t nav_type;
+ uint16_t utc_year;
+ uint8_t utc_month;
+ uint8_t utc_day;
+ uint8_t utc_hour;
+ uint8_t utc_minute;
+ uint16_t utc_second;
+ int32_t lat;
+ int32_t lon;
+ int32_t alt_msl;
+ uint16_t ground_speed;
+ uint16_t course;
+ int16_t climb_rate;
+ uint32_t h_error;
+ uint32_t v_error;
+ uint8_t num_sv;
+ uint8_t hdop;
+};
+
+static __xdata struct sirf_geodetic_nav_data ao_sirf_data;
+
+struct sirf_measured_sat_data {
+ uint8_t svid;
+ uint8_t c_n_1;
+};
+
+struct sirf_measured_tracker_data {
+ int16_t gps_week;
+ uint32_t gps_tow;
+ uint8_t channels;
+ struct sirf_measured_sat_data sats[12];
+};
+
+static __xdata struct sirf_measured_tracker_data ao_sirf_tracker_data;
+
+static __pdata uint16_t ao_sirf_cksum;
+static __pdata uint16_t ao_sirf_len;
+
+#define ao_sirf_byte() ((uint8_t) ao_serial_getchar())
+
+static uint8_t data_byte(void)
+{
+ uint8_t c = ao_sirf_byte();
+ --ao_sirf_len;
+ ao_sirf_cksum += c;
+ return c;
+}
+
+static char __xdata *sirf_target;
+
+static void sirf_u16(uint8_t offset)
+{
+ uint16_t __xdata *ptr = (uint16_t __xdata *) (sirf_target + offset);
+ uint16_t val;
+
+ val = data_byte() << 8;
+ val |= data_byte ();
+ *ptr = val;
+}
+
+static void sirf_u8(uint8_t offset)
+{
+ uint8_t __xdata *ptr = (uint8_t __xdata *) (sirf_target + offset);
+ uint8_t val;
+
+ val = data_byte ();
+ *ptr = val;
+}
+
+static void sirf_u32(uint8_t offset) __reentrant
+{
+ uint32_t __xdata *ptr = (uint32_t __xdata *) (sirf_target + offset);
+ uint32_t val;
+
+ val = ((uint32_t) data_byte ()) << 24;
+ val |= ((uint32_t) data_byte ()) << 16;
+ val |= ((uint32_t) data_byte ()) << 8;
+ val |= ((uint32_t) data_byte ());
+ *ptr = val;
+}
+
+static void sirf_discard(uint8_t len)
+{
+ while (len--)
+ data_byte();
+}
+
+#define SIRF_END 0
+#define SIRF_DISCARD 1
+#define SIRF_U8 2
+#define SIRF_U16 3
+#define SIRF_U32 4
+#define SIRF_U8X10 5
+
+struct sirf_packet_parse {
+ uint8_t type;
+ uint8_t offset;
+};
+
+static void
+ao_sirf_parse(void __xdata *target, const struct sirf_packet_parse *parse) __reentrant
+{
+ uint8_t i, offset, j;
+
+ sirf_target = target;
+ for (i = 0; ; i++) {
+ offset = parse[i].offset;
+ switch (parse[i].type) {
+ case SIRF_END:
+ return;
+ case SIRF_DISCARD:
+ sirf_discard(offset);
+ break;
+ case SIRF_U8:
+ sirf_u8(offset);
+ break;
+ case SIRF_U16:
+ sirf_u16(offset);
+ break;
+ case SIRF_U32:
+ sirf_u32(offset);
+ break;
+ case SIRF_U8X10:
+ for (j = 10; j--;)
+ sirf_u8(offset++);
+ break;
+ }
+ }
+}
+
+static const struct sirf_packet_parse geodetic_nav_data_packet[] = {
+ { SIRF_DISCARD, 2 }, /* 1 nav valid */
+ { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, nav_type) }, /* 3 */
+ { SIRF_DISCARD, 6 }, /* 5 week number, time of week */
+ { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_year) }, /* 11 */
+ { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_month) }, /* 13 */
+ { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_day) }, /* 14 */
+ { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_hour) }, /* 15 */
+ { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_minute) }, /* 16 */
+ { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_second) }, /* 17 */
+ { SIRF_DISCARD, 4 }, /* satellite id list */ /* 19 */
+ { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lat) }, /* 23 */
+ { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lon) }, /* 27 */
+ { SIRF_DISCARD, 4 }, /* altitude from ellipsoid */ /* 31 */
+ { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, alt_msl) }, /* 35 */
+ { SIRF_DISCARD, 1 }, /* map datum */ /* 39 */
+ { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, ground_speed) }, /* 40 */
+ { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, course) }, /* 42 */
+ { SIRF_DISCARD, 2 }, /* magnetic variation */ /* 44 */
+ { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, climb_rate) }, /* 46 */
+ { SIRF_DISCARD, 2 }, /* turn rate */ /* 48 */
+ { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, h_error) }, /* 50 */
+ { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, v_error) }, /* 54 */
+ { SIRF_DISCARD, 30 }, /* time error, h_vel error, clock_bias,
+ clock bias error, clock drift,
+ clock drift error, distance,
+ distance error, heading error */ /* 58 */
+ { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, num_sv) }, /* 88 */
+ { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, hdop) }, /* 89 */
+ { SIRF_DISCARD, 1 }, /* additional mode info */ /* 90 */
+ { SIRF_END, 0 }, /* 91 */
+};
+
+static void
+ao_sirf_parse_41(void) __reentrant
+{
+ ao_sirf_parse(&ao_sirf_data, geodetic_nav_data_packet);
+}
+
+static const struct sirf_packet_parse measured_tracker_data_packet[] = {
+ { SIRF_U16, offsetof (struct sirf_measured_tracker_data, gps_week) }, /* 1 week */
+ { SIRF_U32, offsetof (struct sirf_measured_tracker_data, gps_tow) }, /* 3 time of week */
+ { SIRF_U8, offsetof (struct sirf_measured_tracker_data, channels) }, /* 7 channels */
+ { SIRF_END, 0 },
+};
+
+static const struct sirf_packet_parse measured_sat_data_packet[] = {
+ { SIRF_U8, offsetof (struct sirf_measured_sat_data, svid) }, /* 0 SV id */
+ { SIRF_DISCARD, 4 }, /* 1 azimuth, 2 elevation, 3 state */
+ { SIRF_U8, offsetof (struct sirf_measured_sat_data, c_n_1) }, /* C/N0 1 */
+ { SIRF_DISCARD, 9 }, /* C/N0 2-10 */
+ { SIRF_END, 0 },
+};
+
+static void
+ao_sirf_parse_4(void) __reentrant
+{
+ uint8_t i;
+ ao_sirf_parse(&ao_sirf_tracker_data, measured_tracker_data_packet);
+ for (i = 0; i < 12; i++)
+ ao_sirf_parse(&ao_sirf_tracker_data.sats[i], measured_sat_data_packet);
+}
+
+static void
+ao_gps_setup(void) __reentrant
+{
+ uint8_t i, k;
+ ao_serial_set_speed(AO_SERIAL_SPEED_4800);
+ for (i = 0; i < 64; i++)
+ ao_serial_putchar(0x00);
+ for (k = 0; k < 3; k++)
+ for (i = 0; i < sizeof (ao_gps_set_nmea); i++)
+ ao_serial_putchar(ao_gps_set_nmea[i]);
+ ao_serial_set_speed(AO_SERIAL_SPEED_57600);
+ for (i = 0; i < 64; i++)
+ ao_serial_putchar(0x00);
+}
+
+static const char ao_gps_set_message_rate[] = {
+ 0xa0, 0xa2, 0x00, 0x08,
+ 166,
+ 0,
+};
+
+void
+ao_sirf_set_message_rate(uint8_t msg, uint8_t rate) __reentrant
+{
+ uint16_t cksum = 0x00a6;
+ uint8_t i;
+
+ for (i = 0; i < sizeof (ao_gps_set_message_rate); i++)
+ ao_serial_putchar(ao_gps_set_message_rate[i]);
+ ao_serial_putchar(msg);
+ ao_serial_putchar(rate);
+ cksum = 0xa6 + msg + rate;
+ for (i = 0; i < 4; i++)
+ ao_serial_putchar(0);
+ ao_serial_putchar((cksum >> 8) & 0x7f);
+ ao_serial_putchar(cksum & 0xff);
+ ao_serial_putchar(0xb0);
+ ao_serial_putchar(0xb3);
+}
+
+static const uint8_t sirf_disable[] = {
+ 2,
+ 9,
+ 10,
+ 27,
+ 50,
+ 52,
+};
+
+void
+ao_gps(void) __reentrant
+{
+ uint8_t i, k;
+ uint16_t cksum;
+
+ ao_gps_setup();
+ for (k = 0; k < 5; k++)
+ {
+ for (i = 0; i < sizeof (ao_gps_config); i++)
+ ao_serial_putchar(ao_gps_config[i]);
+ for (i = 0; i < sizeof (sirf_disable); i++)
+ ao_sirf_set_message_rate(sirf_disable[i], 0);
+ ao_sirf_set_message_rate(41, 1);
+ ao_sirf_set_message_rate(4, 1);
+ }
+ for (;;) {
+ /* Locate the begining of the next record */
+ while (ao_sirf_byte() != (uint8_t) 0xa0)
+ ;
+ if (ao_sirf_byte() != (uint8_t) 0xa2)
+ continue;
+
+ /* Length */
+ ao_sirf_len = ao_sirf_byte() << 8;
+ ao_sirf_len |= ao_sirf_byte();
+ if (ao_sirf_len > 1023)
+ continue;
+
+ ao_sirf_cksum = 0;
+
+ /* message ID */
+ i = data_byte (); /* 0 */
+
+ switch (i) {
+ case 41:
+ if (ao_sirf_len < 90)
+ break;
+ ao_sirf_parse_41();
+ break;
+ case 4:
+ if (ao_sirf_len < 187)
+ break;
+ ao_sirf_parse_4();
+ break;
+ }
+ if (ao_sirf_len != 0)
+ continue;
+
+ /* verify checksum and end sequence */
+ ao_sirf_cksum &= 0x7fff;
+ cksum = ao_sirf_byte() << 8;
+ cksum |= ao_sirf_byte();
+ if (ao_sirf_cksum != cksum)
+ continue;
+ if (ao_sirf_byte() != (uint8_t) 0xb0)
+ continue;
+ if (ao_sirf_byte() != (uint8_t) 0xb3)
+ continue;
+
+ switch (i) {
+ case 41:
+ ao_mutex_get(&ao_gps_mutex);
+ ao_gps_tick = ao_time();
+ ao_gps_data.hour = ao_sirf_data.utc_hour;
+ ao_gps_data.minute = ao_sirf_data.utc_minute;
+ ao_gps_data.second = ao_sirf_data.utc_second / 1000;
+ ao_gps_data.flags = ((ao_sirf_data.num_sv << AO_GPS_NUM_SAT_SHIFT) & AO_GPS_NUM_SAT_MASK) | AO_GPS_RUNNING;
+ if ((ao_sirf_data.nav_type & NAV_TYPE_GPS_FIX_TYPE_MASK) >= NAV_TYPE_4_SV_KF)
+ ao_gps_data.flags |= AO_GPS_VALID;
+ ao_gps_data.latitude = ao_sirf_data.lat;
+ ao_gps_data.longitude = ao_sirf_data.lon;
+ ao_gps_data.altitude = ao_sirf_data.alt_msl / 100;
+ ao_gps_data.ground_speed = ao_sirf_data.ground_speed;
+ ao_gps_data.course = ao_sirf_data.course / 200;
+ ao_gps_data.hdop = ao_sirf_data.hdop;
+ ao_gps_data.climb_rate = ao_sirf_data.climb_rate;
+ ao_gps_data.flags |= AO_GPS_COURSE_VALID;
+#if 0
+ if (ao_sirf_data.h_error > 6553500)
+ ao_gps_data.h_error = 65535;
+ else
+ ao_gps_data.h_error = ao_sirf_data.h_error / 100;
+ if (ao_sirf_data.v_error > 6553500)
+ ao_gps_data.v_error = 65535;
+ else
+ ao_gps_data.v_error = ao_sirf_data.v_error / 100;
+#endif
+ ao_mutex_put(&ao_gps_mutex);
+ ao_wakeup(&ao_gps_data);
+ break;
+ case 4:
+ ao_mutex_get(&ao_gps_mutex);
+ ao_gps_tracking_data.channels = ao_sirf_tracker_data.channels;
+ for (i = 0; i < 12; i++) {
+ ao_gps_tracking_data.sats[i].svid = ao_sirf_tracker_data.sats[i].svid;
+ ao_gps_tracking_data.sats[i].c_n_1 = ao_sirf_tracker_data.sats[i].c_n_1;
+ }
+ ao_mutex_put(&ao_gps_mutex);
+ ao_wakeup(&ao_gps_tracking_data);
+ break;
+ }
+ }
+}
+
+__xdata struct ao_task ao_gps_task;
+
+void
+ao_gps_init(void)
+{
+ ao_add_task(&ao_gps_task, ao_gps, "gps");
+}
diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c
new file mode 100644
index 00000000..7ac26946
--- /dev/null
+++ b/src/drivers/ao_gps_skytraq.c
@@ -0,0 +1,490 @@
+/*
+ * 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.
+ */
+
+#ifndef AO_GPS_TEST
+#include "ao.h"
+#endif
+
+#define AO_GPS_LEADER 2
+
+static __code char ao_gps_header[] = "GP";
+
+__xdata uint8_t ao_gps_mutex;
+static __data char ao_gps_char;
+static __pdata uint8_t ao_gps_cksum;
+static __pdata uint8_t ao_gps_error;
+
+__pdata uint16_t ao_gps_tick;
+__xdata struct ao_telemetry_location ao_gps_data;
+__xdata struct ao_telemetry_satellite ao_gps_tracking_data;
+
+static __pdata uint16_t ao_gps_next_tick;
+static __xdata struct ao_telemetry_location ao_gps_next;
+static __pdata uint8_t ao_gps_date_flags;
+static __xdata struct ao_telemetry_satellite ao_gps_tracking_next;
+
+#define STQ_S 0xa0, 0xa1
+#define STQ_E 0x0d, 0x0a
+#define SKYTRAQ_MSG_2(id,a,b) \
+ STQ_S, 0, 3, id, a,b, (id^a^b), STQ_E
+#define SKYTRAQ_MSG_3(id,a,b,c) \
+ STQ_S, 0, 4, id, a,b,c, (id^a^b^c), STQ_E
+#define SKYTRAQ_MSG_8(id,a,b,c,d,e,f,g,h) \
+ STQ_S, 0, 9, id, a,b,c,d,e,f,g,h, (id^a^b^c^d^e^f^g^h), STQ_E
+#define SKYTRAQ_MSG_14(id,a,b,c,d,e,f,g,h,i,j,k,l,m,n) \
+ STQ_S, 0,15, id, a,b,c,d,e,f,g,h,i,j,k,l,m,n, \
+ (id^a^b^c^d^e^f^g^h^i^j^k^l^m^n), STQ_E
+
+static __code uint8_t ao_gps_config[] = {
+ SKYTRAQ_MSG_8(0x08, 1, 1, 1, 1, 1, 1, 1, 0), /* configure nmea */
+ /* gga interval */
+ /* gsa interval */
+ /* gsv interval */
+ /* gll interval */
+ /* rmc interval */
+ /* vtg interval */
+ /* zda interval */
+ /* attributes (0 = update to sram, 1 = update flash too) */
+
+ SKYTRAQ_MSG_2(0x3c, 0x00, 0x00), /* configure navigation mode */
+ /* 0 = car, 1 = pedestrian */
+ /* 0 = update to sram, 1 = update sram + flash */
+};
+
+static void
+ao_gps_lexchar(void)
+{
+ if (ao_gps_error)
+ ao_gps_char = '\n';
+ else
+ ao_gps_char = ao_serial_getchar();
+ ao_gps_cksum ^= ao_gps_char;
+}
+
+void
+ao_gps_skip_field(void)
+{
+ while (ao_gps_char != ',' && ao_gps_char != '*' && ao_gps_char != '\n')
+ ao_gps_lexchar();
+}
+
+void
+ao_gps_skip_sep(void)
+{
+ if (ao_gps_char == ',' || ao_gps_char == '.' || ao_gps_char == '*')
+ ao_gps_lexchar();
+}
+
+__pdata static uint8_t ao_gps_num_width;
+
+static int16_t
+ao_gps_decimal(uint8_t max_width)
+{
+ int16_t v;
+ __pdata uint8_t neg = 0;
+
+ ao_gps_skip_sep();
+ if (ao_gps_char == '-') {
+ neg = 1;
+ ao_gps_lexchar();
+ }
+ v = 0;
+ ao_gps_num_width = 0;
+ while (ao_gps_num_width < max_width) {
+ if (ao_gps_char < '0' || '9' < ao_gps_char)
+ break;
+ v = v * (int16_t) 10 + ao_gps_char - '0';
+ ao_gps_num_width++;
+ ao_gps_lexchar();
+ }
+ if (neg)
+ v = -v;
+ return v;
+}
+
+static uint8_t
+ao_gps_hex(uint8_t max_width)
+{
+ uint8_t v, d;
+
+ ao_gps_skip_sep();
+ v = 0;
+ ao_gps_num_width = 0;
+ while (ao_gps_num_width < max_width) {
+ if ('0' <= ao_gps_char && ao_gps_char <= '9')
+ d = ao_gps_char - '0';
+ else if ('A' <= ao_gps_char && ao_gps_char <= 'F')
+ d = ao_gps_char - 'A' + 10;
+ else if ('a' <= ao_gps_char && ao_gps_char <= 'f')
+ d = ao_gps_char - 'a' + 10;
+ else
+ break;
+ v = (v << 4) | d;
+ ao_gps_num_width++;
+ ao_gps_lexchar();
+ }
+ return v;
+}
+
+static int32_t
+ao_gps_parse_pos(uint8_t deg_width) __reentrant
+{
+ int32_t d;
+ int32_t m;
+ int32_t f;
+
+ d = ao_gps_decimal(deg_width);
+ m = ao_gps_decimal(2);
+ if (ao_gps_char == '.') {
+ f = ao_gps_decimal(4);
+ while (ao_gps_num_width < 4) {
+ f *= 10;
+ ao_gps_num_width++;
+ }
+ } else {
+ f = 0;
+ if (ao_gps_char != ',')
+ ao_gps_error = 1;
+ }
+ d = d * 10000000l;
+ m = m * 10000l + f;
+ d = d + m * 50 / 3;
+ return d;
+}
+
+static uint8_t
+ao_gps_parse_flag(char no_c, char yes_c) __reentrant
+{
+ uint8_t ret = 0;
+ ao_gps_skip_sep();
+ if (ao_gps_char == yes_c)
+ ret = 1;
+ else if (ao_gps_char == no_c)
+ ret = 0;
+ else
+ ao_gps_error = 1;
+ ao_gps_lexchar();
+ return ret;
+}
+
+static void
+ao_nmea_gga()
+{
+ uint8_t i;
+
+ /* Now read the data into the gps data record
+ *
+ * $GPGGA,025149.000,4528.1723,N,12244.2480,W,1,05,2.0,103.5,M,-19.5,M,,0000*66
+ *
+ * Essential fix data
+ *
+ * 025149.000 time (02:51:49.000 GMT)
+ * 4528.1723,N Latitude 45°28.1723' N
+ * 12244.2480,W Longitude 122°44.2480' W
+ * 1 Fix quality:
+ * 0 = invalid
+ * 1 = GPS fix (SPS)
+ * 2 = DGPS fix
+ * 3 = PPS fix
+ * 4 = Real Time Kinematic
+ * 5 = Float RTK
+ * 6 = estimated (dead reckoning)
+ * 7 = Manual input mode
+ * 8 = Simulation mode
+ * 05 Number of satellites (5)
+ * 2.0 Horizontal dilution
+ * 103.5,M Altitude, 103.5M above msl
+ * -19.5,M Height of geoid above WGS84 ellipsoid
+ * ? time in seconds since last DGPS update
+ * 0000 DGPS station ID
+ * *66 checksum
+ */
+
+ ao_gps_next_tick = ao_time();
+ ao_gps_next.flags = AO_GPS_RUNNING | ao_gps_date_flags;
+ ao_gps_next.hour = ao_gps_decimal(2);
+ ao_gps_next.minute = ao_gps_decimal(2);
+ ao_gps_next.second = ao_gps_decimal(2);
+ ao_gps_skip_field(); /* skip seconds fraction */
+
+ ao_gps_next.latitude = ao_gps_parse_pos(2);
+ if (ao_gps_parse_flag('N', 'S'))
+ ao_gps_next.latitude = -ao_gps_next.latitude;
+ ao_gps_next.longitude = ao_gps_parse_pos(3);
+ if (ao_gps_parse_flag('E', 'W'))
+ ao_gps_next.longitude = -ao_gps_next.longitude;
+
+ i = ao_gps_decimal(0xff);
+ if (i == 1)
+ ao_gps_next.flags |= AO_GPS_VALID;
+
+ i = ao_gps_decimal(0xff) << AO_GPS_NUM_SAT_SHIFT;
+ if (i > AO_GPS_NUM_SAT_MASK)
+ i = AO_GPS_NUM_SAT_MASK;
+ ao_gps_next.flags |= i;
+
+ ao_gps_lexchar();
+ i = ao_gps_decimal(0xff);
+ if (i <= 50) {
+ i = (uint8_t) 5 * i;
+ if (ao_gps_char == '.')
+ i = (i + ((uint8_t) ao_gps_decimal(1) >> 1));
+ } else
+ i = 255;
+ ao_gps_next.hdop = i;
+ ao_gps_skip_field();
+
+ ao_gps_next.altitude = ao_gps_decimal(0xff);
+ ao_gps_skip_field(); /* skip any fractional portion */
+
+ /* Skip remaining fields */
+ while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
+ ao_gps_lexchar();
+ ao_gps_skip_field();
+ }
+ if (ao_gps_char == '*') {
+ uint8_t cksum = ao_gps_cksum ^ '*';
+ if (cksum != ao_gps_hex(2))
+ ao_gps_error = 1;
+ } else
+ ao_gps_error = 1;
+ if (!ao_gps_error) {
+ ao_mutex_get(&ao_gps_mutex);
+ ao_gps_tick = ao_gps_next_tick;
+ memcpy(&ao_gps_data, &ao_gps_next, sizeof (ao_gps_data));
+ ao_mutex_put(&ao_gps_mutex);
+ ao_wakeup(&ao_gps_data);
+ }
+}
+
+static void
+ao_nmea_gsv(void)
+{
+ char c;
+ uint8_t i;
+ uint8_t done;
+ /* Now read the data into the GPS tracking data record
+ *
+ * $GPGSV,3,1,12,05,54,069,45,12,44,061,44,21,07,184,46,22,78,289,47*72<CR><LF>
+ *
+ * Satellites in view data
+ *
+ * 3 Total number of GSV messages
+ * 1 Sequence number of current GSV message
+ * 12 Total sats in view (0-12)
+ * 05 SVID
+ * 54 Elevation
+ * 069 Azimuth
+ * 45 C/N0 in dB
+ * ... other SVIDs
+ * 72 checksum
+ */
+ c = ao_gps_decimal(1); /* total messages */
+ i = ao_gps_decimal(1); /* message sequence */
+ if (i == 1) {
+ ao_gps_tracking_next.channels = 0;
+ }
+ done = (uint8_t) c == i;
+ ao_gps_lexchar();
+ ao_gps_skip_field(); /* sats in view */
+ while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
+ i = ao_gps_tracking_next.channels;
+ c = ao_gps_decimal(2); /* SVID */
+ if (i < AO_MAX_GPS_TRACKING)
+ ao_gps_tracking_next.sats[i].svid = c;
+ ao_gps_lexchar();
+ ao_gps_skip_field(); /* elevation */
+ ao_gps_lexchar();
+ ao_gps_skip_field(); /* azimuth */
+ c = ao_gps_decimal(2); /* C/N0 */
+ if (i < AO_MAX_GPS_TRACKING) {
+ if ((ao_gps_tracking_next.sats[i].c_n_1 = c) != 0)
+ ao_gps_tracking_next.channels = i + 1;
+ }
+ }
+ if (ao_gps_char == '*') {
+ uint8_t cksum = ao_gps_cksum ^ '*';
+ if (cksum != ao_gps_hex(2))
+ ao_gps_error = 1;
+ }
+ else
+ ao_gps_error = 1;
+ if (ao_gps_error)
+ ao_gps_tracking_next.channels = 0;
+ else if (done) {
+ ao_mutex_get(&ao_gps_mutex);
+ memcpy(&ao_gps_tracking_data, &ao_gps_tracking_next,
+ sizeof(ao_gps_tracking_data));
+ ao_mutex_put(&ao_gps_mutex);
+ ao_wakeup(&ao_gps_tracking_data);
+ }
+}
+
+static void
+ao_nmea_rmc(void)
+{
+ char a, c;
+ uint8_t i;
+ /* Parse the RMC record to read out the current date */
+
+ /* $GPRMC,111636.932,A,2447.0949,N,12100.5223,E,000.0,000.0,030407,,,A*61
+ *
+ * Recommended Minimum Specific GNSS Data
+ *
+ * 111636.932 UTC time 11:16:36.932
+ * A Data Valid (V = receiver warning)
+ * 2447.0949 Latitude
+ * N North/south indicator
+ * 12100.5223 Longitude
+ * E East/west indicator
+ * 000.0 Speed over ground
+ * 000.0 Course over ground
+ * 030407 UTC date (ddmmyy format)
+ * A Mode indicator:
+ * N = data not valid
+ * A = autonomous mode
+ * D = differential mode
+ * E = estimated (dead reckoning) mode
+ * M = manual input mode
+ * S = simulator mode
+ * 61 checksum
+ */
+ ao_gps_skip_field();
+ for (i = 0; i < 8; i++) {
+ ao_gps_lexchar();
+ ao_gps_skip_field();
+ }
+ a = ao_gps_decimal(2);
+ c = ao_gps_decimal(2);
+ i = ao_gps_decimal(2);
+ /* Skip remaining fields */
+ while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
+ ao_gps_lexchar();
+ ao_gps_skip_field();
+ }
+ if (ao_gps_char == '*') {
+ uint8_t cksum = ao_gps_cksum ^ '*';
+ if (cksum != ao_gps_hex(2))
+ ao_gps_error = 1;
+ } else
+ ao_gps_error = 1;
+ if (!ao_gps_error) {
+ ao_gps_next.year = i;
+ ao_gps_next.month = c;
+ ao_gps_next.day = a;
+ ao_gps_date_flags = AO_GPS_DATE_VALID;
+ }
+}
+
+#define ao_skytraq_sendstruct(s) ao_skytraq_sendbytes((s), sizeof(s))
+
+static void
+ao_skytraq_sendbytes(__code uint8_t *b, uint8_t l)
+{
+ while (l--) {
+ uint8_t c = *b++;
+ if (c == 0xa0)
+ ao_delay(AO_MS_TO_TICKS(500));
+ ao_serial_putchar(c);
+ }
+}
+
+static void
+ao_gps_nmea_parse(void)
+{
+ uint8_t a, b, c;
+
+ ao_gps_cksum = 0;
+ ao_gps_error = 0;
+
+ for (a = 0; a < AO_GPS_LEADER; a++) {
+ ao_gps_lexchar();
+ if (ao_gps_char != ao_gps_header[a])
+ return;
+ }
+
+ ao_gps_lexchar();
+ a = ao_gps_char;
+ ao_gps_lexchar();
+ b = ao_gps_char;
+ ao_gps_lexchar();
+ c = ao_gps_char;
+ ao_gps_lexchar();
+
+ if (ao_gps_char != ',')
+ return;
+
+ if (a == (uint8_t) 'G' && b == (uint8_t) 'G' && c == (uint8_t) 'A') {
+ ao_nmea_gga();
+ } else if (a == (uint8_t) 'G' && b == (uint8_t) 'S' && c == (uint8_t) 'V') {
+ ao_nmea_gsv();
+ } else if (a == (uint8_t) 'R' && b == (uint8_t) 'M' && c == (uint8_t) 'C') {
+ ao_nmea_rmc();
+ }
+}
+
+void
+ao_gps(void) __reentrant
+{
+ ao_serial_set_speed(AO_SERIAL_SPEED_9600);
+
+ /* give skytraq time to boot in case of cold start */
+ ao_delay(AO_MS_TO_TICKS(2000));
+
+ ao_skytraq_sendstruct(ao_gps_config);
+
+ for (;;) {
+ /* Locate the begining of the next record */
+ if (ao_serial_getchar() == '$') {
+ ao_gps_nmea_parse();
+ }
+
+ }
+}
+
+__xdata struct ao_task ao_gps_task;
+
+static void
+gps_dump(void) __reentrant
+{
+ uint8_t i;
+ ao_mutex_get(&ao_gps_mutex);
+ printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day);
+ printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second);
+ printf ("Lat/Lon: %ld %ld\n", ao_gps_data.latitude, ao_gps_data.longitude);
+ printf ("Alt: %d\n", ao_gps_data.altitude);
+ printf ("Flags: 0x%x\n", ao_gps_data.flags);
+ printf ("Sats: %d", ao_gps_tracking_data.channels);
+ for (i = 0; i < ao_gps_tracking_data.channels; i++)
+ printf (" %d %d",
+ ao_gps_tracking_data.sats[i].svid,
+ ao_gps_tracking_data.sats[i].c_n_1);
+ printf ("\ndone\n");
+ ao_mutex_put(&ao_gps_mutex);
+}
+
+__code struct ao_cmds ao_gps_cmds[] = {
+ { gps_dump, "g\0Display GPS" },
+ { 0, NULL },
+};
+
+void
+ao_gps_init(void)
+{
+ ao_add_task(&ao_gps_task, ao_gps, "gps");
+ ao_cmd_register(&ao_gps_cmds[0]);
+}
diff --git a/src/drivers/ao_m25.c b/src/drivers/ao_m25.c
new file mode 100644
index 00000000..d7208273
--- /dev/null
+++ b/src/drivers/ao_m25.c
@@ -0,0 +1,380 @@
+/*
+ * Copyright © 2010 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 "ao.h"
+
+/* Total bytes of available storage */
+__pdata uint32_t ao_storage_total;
+
+/* Block size - device is erased in these units. At least 256 bytes */
+__pdata uint32_t ao_storage_block;
+
+/* Byte offset of config block. Will be ao_storage_block bytes long */
+__pdata uint32_t ao_storage_config;
+
+/* Storage unit size - device reads and writes must be within blocks of this size. Usually 256 bytes. */
+__pdata uint16_t ao_storage_unit;
+
+/*
+ * Each flash chip is arranged in 64kB sectors; the
+ * chip cannot erase in units smaller than that.
+ *
+ * Writing happens in units of 256 byte pages and
+ * can only change bits from 1 to 0. So, you can rewrite
+ * the same contents, or append to an existing page easily enough
+ */
+
+#define M25_WREN 0x06 /* Write Enable */
+#define M25_WRDI 0x04 /* Write Disable */
+#define M25_RDID 0x9f /* Read Identification */
+#define M25_RDSR 0x05 /* Read Status Register */
+#define M25_WRSR 0x01 /* Write Status Register */
+#define M25_READ 0x03 /* Read Data Bytes */
+#define M25_FAST_READ 0x0b /* Read Data Bytes at Higher Speed */
+#define M25_PP 0x02 /* Page Program */
+#define M25_SE 0xd8 /* Sector Erase */
+#define M25_BE 0xc7 /* Bulk Erase */
+#define M25_DP 0xb9 /* Deep Power-down */
+
+/* RDID response */
+#define M25_MANUF_OFFSET 0
+#define M25_MEMORY_TYPE_OFFSET 1
+#define M25_CAPACITY_OFFSET 2
+#define M25_UID_OFFSET 3
+#define M25_CFI_OFFSET 4
+#define M25_RDID_LEN 4 /* that's all we need */
+
+#define M25_CAPACITY_128KB 0x11
+#define M25_CAPACITY_256KB 0x12
+#define M25_CAPACITY_512KB 0x13
+#define M25_CAPACITY_1MB 0x14
+#define M25_CAPACITY_2MB 0x15
+
+/*
+ * Status register bits
+ */
+
+#define M25_STATUS_SRWD (1 << 7) /* Status register write disable */
+#define M25_STATUS_BP_MASK (7 << 2) /* Block protect bits */
+#define M25_STATUS_BP_SHIFT (2)
+#define M25_STATUS_WEL (1 << 1) /* Write enable latch */
+#define M25_STATUS_WIP (1 << 0) /* Write in progress */
+
+/*
+ * On teleterra, the m25 chip select pins are
+ * wired on P0_0 through P0_3.
+ */
+
+#if M25_MAX_CHIPS > 1
+static uint8_t ao_m25_size[M25_MAX_CHIPS]; /* number of sectors in each chip */
+static uint8_t ao_m25_pin[M25_MAX_CHIPS]; /* chip select pin for each chip */
+static uint8_t ao_m25_numchips; /* number of chips detected */
+#endif
+static uint8_t ao_m25_total; /* total sectors available */
+static uint8_t ao_m25_wip; /* write in progress */
+
+static __xdata uint8_t ao_m25_mutex;
+
+/*
+ * This little array is abused to send and receive data. A particular
+ * caution -- the read and write addresses are written into the last
+ * three bytes of the array by ao_m25_set_page_address and then the
+ * first byte is used by ao_m25_wait_wip and ao_m25_write_enable, neither
+ * of which touch those last three bytes.
+ */
+
+static __xdata uint8_t ao_m25_instruction[4];
+
+#define M25_SELECT(cs) ao_spi_get_mask(SPI_CS_PORT,cs)
+#define M25_DESELECT(cs) ao_spi_put_mask(SPI_CS_PORT,cs)
+
+#define M25_BLOCK_SHIFT 16
+#define M25_BLOCK 65536L
+#define M25_POS_TO_SECTOR(pos) ((uint8_t) ((pos) >> M25_BLOCK_SHIFT))
+#define M25_SECTOR_TO_POS(sector) (((uint32_t) (sector)) << M25_BLOCK_SHIFT)
+
+/*
+ * Block until the specified chip is done writing
+ */
+static void
+ao_m25_wait_wip(uint8_t cs)
+{
+ if (ao_m25_wip & cs) {
+ M25_SELECT(cs);
+ ao_m25_instruction[0] = M25_RDSR;
+ ao_spi_send(ao_m25_instruction, 1);
+ do {
+ ao_spi_recv(ao_m25_instruction, 1);
+ } while (ao_m25_instruction[0] & M25_STATUS_WIP);
+ M25_DESELECT(cs);
+ ao_m25_wip &= ~cs;
+ }
+}
+
+/*
+ * Set the write enable latch so that page program and sector
+ * erase commands will work. Also mark the chip as busy writing
+ * so that future operations will block until the WIP bit goes off
+ */
+static void
+ao_m25_write_enable(uint8_t cs)
+{
+ M25_SELECT(cs);
+ ao_m25_instruction[0] = M25_WREN;
+ ao_spi_send(&ao_m25_instruction, 1);
+ M25_DESELECT(cs);
+ ao_m25_wip |= cs;
+}
+
+
+/*
+ * Returns the number of 64kB sectors
+ */
+static uint8_t
+ao_m25_read_capacity(uint8_t cs)
+{
+ uint8_t capacity;
+ M25_SELECT(cs);
+ ao_m25_instruction[0] = M25_RDID;
+ ao_spi_send(ao_m25_instruction, 1);
+ ao_spi_recv(ao_m25_instruction, M25_RDID_LEN);
+ M25_DESELECT(cs);
+
+ /* Check to see if the chip is present */
+ if (ao_m25_instruction[0] == 0xff)
+ return 0;
+ capacity = ao_m25_instruction[M25_CAPACITY_OFFSET];
+
+ /* Sanity check capacity number */
+ if (capacity < 0x11 || 0x1f < capacity)
+ return 0;
+ return 1 << (capacity - 0x10);
+}
+
+static uint8_t
+ao_m25_set_address(uint32_t pos)
+{
+ uint8_t chip;
+#if M25_MAX_CHIPS > 1
+ uint8_t size;
+
+ for (chip = 0; chip < ao_m25_numchips; chip++) {
+ size = ao_m25_size[chip];
+ if (M25_POS_TO_SECTOR(pos) < size)
+ break;
+ pos -= M25_SECTOR_TO_POS(size);
+ }
+ if (chip == ao_m25_numchips)
+ return 0xff;
+
+ chip = ao_m25_pin[chip];
+#else
+ chip = M25_CS_MASK;
+#endif
+ ao_m25_wait_wip(chip);
+
+ ao_m25_instruction[1] = pos >> 16;
+ ao_m25_instruction[2] = pos >> 8;
+ ao_m25_instruction[3] = pos;
+ return chip;
+}
+
+/*
+ * Scan the possible chip select lines
+ * to see which flash chips are connected
+ */
+static uint8_t
+ao_m25_scan(void)
+{
+#if M25_MAX_CHIPS > 1
+ uint8_t pin, size;
+#endif
+
+ if (ao_m25_total)
+ return 1;
+
+#if M25_MAX_CHIPS > 1
+ ao_m25_numchips = 0;
+ for (pin = 1; pin != 0; pin <<= 1) {
+ if (M25_CS_MASK & pin) {
+ size = ao_m25_read_capacity(pin);
+ if (size != 0) {
+ ao_m25_size[ao_m25_numchips] = size;
+ ao_m25_pin[ao_m25_numchips] = pin;
+ ao_m25_total += size;
+ ao_m25_numchips++;
+ }
+ }
+ }
+#else
+ ao_m25_total = ao_m25_read_capacity(M25_CS_MASK);
+#endif
+ if (!ao_m25_total)
+ return 0;
+ ao_storage_total = M25_SECTOR_TO_POS(ao_m25_total);
+ ao_storage_block = M25_BLOCK;
+ ao_storage_config = ao_storage_total - M25_BLOCK;
+ ao_storage_unit = 256;
+ return 1;
+}
+
+/*
+ * Erase the specified sector
+ */
+uint8_t
+ao_storage_erase(uint32_t pos) __reentrant
+{
+ uint8_t cs;
+
+ if (pos >= ao_storage_total || pos + ao_storage_block > ao_storage_total)
+ return 0;
+
+ ao_mutex_get(&ao_m25_mutex);
+ ao_m25_scan();
+
+ cs = ao_m25_set_address(pos);
+
+ ao_m25_wait_wip(cs);
+ ao_m25_write_enable(cs);
+
+ ao_m25_instruction[0] = M25_SE;
+ M25_SELECT(cs);
+ ao_spi_send(ao_m25_instruction, 4);
+ M25_DESELECT(cs);
+ ao_m25_wip |= cs;
+
+ ao_mutex_put(&ao_m25_mutex);
+ return 1;
+}
+
+/*
+ * Write to flash
+ */
+uint8_t
+ao_storage_device_write(uint32_t pos, __xdata void *d, uint16_t len) __reentrant
+{
+ uint8_t cs;
+
+ if (pos >= ao_storage_total || pos + len > ao_storage_total)
+ return 0;
+
+ ao_mutex_get(&ao_m25_mutex);
+ ao_m25_scan();
+
+ cs = ao_m25_set_address(pos);
+ ao_m25_write_enable(cs);
+
+ ao_m25_instruction[0] = M25_PP;
+ M25_SELECT(cs);
+ ao_spi_send(ao_m25_instruction, 4);
+ ao_spi_send(d, len);
+ M25_DESELECT(cs);
+
+ ao_mutex_put(&ao_m25_mutex);
+ return 1;
+}
+
+/*
+ * Read from flash
+ */
+uint8_t
+ao_storage_device_read(uint32_t pos, __xdata void *d, uint16_t len) __reentrant
+{
+ uint8_t cs;
+
+ if (pos >= ao_storage_total || pos + len > ao_storage_total)
+ return 0;
+ ao_mutex_get(&ao_m25_mutex);
+ ao_m25_scan();
+
+ cs = ao_m25_set_address(pos);
+
+ /* No need to use the FAST_READ as we're running at only 8MHz */
+ ao_m25_instruction[0] = M25_READ;
+ M25_SELECT(cs);
+ ao_spi_send(ao_m25_instruction, 4);
+ ao_spi_recv(d, len);
+ M25_DESELECT(cs);
+
+ ao_mutex_put(&ao_m25_mutex);
+ return 1;
+}
+
+void
+ao_storage_flush(void) __reentrant
+{
+}
+
+void
+ao_storage_setup(void)
+{
+ ao_mutex_get(&ao_m25_mutex);
+ ao_m25_scan();
+ ao_mutex_put(&ao_m25_mutex);
+}
+
+void
+ao_storage_device_info(void) __reentrant
+{
+ uint8_t cs;
+#if M25_MAX_CHIPS > 1
+ uint8_t chip;
+#endif
+
+ ao_mutex_get(&ao_m25_mutex);
+ ao_m25_scan();
+ ao_mutex_put(&ao_m25_mutex);
+
+#if M25_MAX_CHIPS > 1
+ printf ("Detected chips %d size %d\n", ao_m25_numchips, ao_m25_total);
+ for (chip = 0; chip < ao_m25_numchips; chip++)
+ printf ("Flash chip %d select %02x size %d\n",
+ chip, ao_m25_pin[chip], ao_m25_size[chip]);
+#else
+ printf ("Detected chips 1 size %d\n", ao_m25_total);
+#endif
+
+ printf ("Available chips:\n");
+ for (cs = 1; cs != 0; cs <<= 1) {
+ if ((M25_CS_MASK & cs) == 0)
+ continue;
+
+ ao_mutex_get(&ao_m25_mutex);
+ M25_SELECT(cs);
+ ao_m25_instruction[0] = M25_RDID;
+ ao_spi_send(ao_m25_instruction, 1);
+ ao_spi_recv(ao_m25_instruction, M25_RDID_LEN);
+ M25_DESELECT(cs);
+
+ printf ("Select %02x manf %02x type %02x cap %02x uid %02x\n",
+ cs,
+ ao_m25_instruction[M25_MANUF_OFFSET],
+ ao_m25_instruction[M25_MEMORY_TYPE_OFFSET],
+ ao_m25_instruction[M25_CAPACITY_OFFSET],
+ ao_m25_instruction[M25_UID_OFFSET]);
+ ao_mutex_put(&ao_m25_mutex);
+ }
+}
+
+void
+ao_storage_device_init(void)
+{
+ /* Set up chip select wires */
+ SPI_CS_PORT |= M25_CS_MASK; /* raise all CS pins */
+ SPI_CS_DIR |= M25_CS_MASK; /* set CS pins as outputs */
+ SPI_CS_SEL &= ~M25_CS_MASK; /* set CS pins as GPIO */
+}
generated by cgit v1.2.3 (git 2.39.1) at 2025-10-31 20:30:29 +0000