summaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-rw-r--r--src/drivers/ao_74hc165.c58
-rw-r--r--src/drivers/ao_74hc165.h27
-rw-r--r--src/drivers/ao_button.c83
-rw-r--r--src/drivers/ao_cc1120.c42
-rw-r--r--src/drivers/ao_cc1120.h2
-rw-r--r--src/drivers/ao_cc115l.c39
-rw-r--r--src/drivers/ao_event.c17
-rw-r--r--src/drivers/ao_event.h6
-rw-r--r--src/drivers/ao_gps_sirf.c7
-rw-r--r--src/drivers/ao_gps_skytraq.c28
-rw-r--r--src/drivers/ao_gps_ublox.c165
-rw-r--r--src/drivers/ao_m25.c32
-rw-r--r--src/drivers/ao_mma655x.c12
-rw-r--r--src/drivers/ao_mpu6000.c331
-rw-r--r--src/drivers/ao_mpu6000.h31
-rw-r--r--src/drivers/ao_ms5607.c71
-rw-r--r--src/drivers/ao_ms5607.h9
-rw-r--r--src/drivers/ao_ms5607_convert.c11
-rw-r--r--src/drivers/ao_ms5607_convert_8051.c136
-rw-r--r--src/drivers/ao_pad.c69
-rw-r--r--src/drivers/ao_pca9922.c7
-rw-r--r--src/drivers/ao_quadrature.c122
-rw-r--r--src/drivers/ublox-csum.5c23
23 files changed, 937 insertions, 391 deletions
diff --git a/src/drivers/ao_74hc165.c b/src/drivers/ao_74hc165.c
new file mode 100644
index 00000000..143f4e3f
--- /dev/null
+++ b/src/drivers/ao_74hc165.c
@@ -0,0 +1,58 @@
+/*
+ * Copyright © 2013 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.
+ */
+
+/*
+ * 74HC165 driver.
+ * Reads a single byte from the shift register
+ */
+
+#include <ao.h>
+#include <ao_74hc165.h>
+
+uint8_t
+ao_74hc165_read(void)
+{
+ static __xdata state;
+ ao_spi_get(AO_74HC165_SPI_BUS);
+ ao_spi_set_speed(AO_74HC165_SPI_BUS, AO_SPI_SPEED_FAST);
+ AO_74HC165_CS = 1;
+ ao_spi_recv(&state, 1, AO_74HC165_SPI_BUS);
+ AO_74HC165_CS = 0;
+ ao_spi_put(AO_74HC165_SPI_BUS);
+ return state;
+}
+
+static void
+ao_74hc165_cmd(void)
+{
+ uint8_t v;
+
+ v = ao_74hc165_read();
+ printf ("Switches: 0x%02x\n", v);
+}
+
+static const struct ao_cmds ao_74hc165_cmds[] = {
+ { ao_74hc165_cmd, "L\0Show 74hc165" },
+ { 0, NULL }
+};
+
+void
+ao_74hc165_init(void)
+{
+ ao_enable_output(AO_74HC165_CS_PORT, AO_74HC165_CS_PIN, AO_74HC165_CS, 0);
+ ao_cmd_register(&ao_74hc165_cmds[0]);
+}
diff --git a/src/drivers/ao_74hc165.h b/src/drivers/ao_74hc165.h
new file mode 100644
index 00000000..3ae51353
--- /dev/null
+++ b/src/drivers/ao_74hc165.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright © 2013 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_74HC165_H_
+#define _AO_74HC165_H_
+
+uint8_t
+ao_74hc165_read(void);
+
+void
+ao_74hc165_init(void);
+
+#endif /* _AO_74HC165_H_ */
diff --git a/src/drivers/ao_button.c b/src/drivers/ao_button.c
index a507c909..25c0cd5c 100644
--- a/src/drivers/ao_button.c
+++ b/src/drivers/ao_button.c
@@ -18,6 +18,7 @@
#include <ao.h>
#include <ao_button.h>
#include <ao_exti.h>
+#include <ao_debounce.h>
#if AO_EVENT
#include <ao_event.h>
#define ao_button_queue(b,v) ao_event_put_isr(AO_EVENT_BUTTON, b, v)
@@ -25,55 +26,71 @@
#define ao_button_queue(b,v)
#endif
-static uint8_t ao_button[AO_BUTTON_COUNT];
-static AO_TICK_TYPE ao_button_time[AO_BUTTON_COUNT];
+#define AO_BUTTON_DEBOUNCE_HOLD 10
-#define AO_DEBOUNCE AO_MS_TO_TICKS(20)
+static struct ao_debounce ao_button_debounce[AO_BUTTON_COUNT];
#define port(q) AO_BUTTON_ ## q ## _PORT
#define bit(q) AO_BUTTON_ ## q
#define pin(q) AO_BUTTON_ ## q ## _PIN
-static void
-ao_button_do(uint8_t b, uint8_t v)
-{
- /* Debounce */
- if ((AO_TICK_SIGNED) (ao_tick_count - ao_button_time[b]) < AO_DEBOUNCE)
- return;
-
- /* pins are inverted */
- v = !v;
- if (ao_button[b] != v) {
- ao_button[b] = v;
- ao_button_time[b] = ao_tick_count;
- ao_button_queue(b, v);
- ao_wakeup(&ao_button[b]);
- }
-}
+/* pins are inverted */
+#define ao_button_value(b) !ao_gpio_get(port(b), bit(b), pin(b))
-#define ao_button_update(b) ao_button_do(b, ao_gpio_get(port(b), bit(b), pin(b)))
-
-static void
-ao_button_isr(void)
+static uint8_t
+_ao_button_get(struct ao_debounce *debounce)
{
+ uint8_t b = debounce - ao_button_debounce;
+
+ switch (b) {
#if AO_BUTTON_COUNT > 0
- ao_button_update(0);
+ case 0: return ao_button_value(0);
#endif
#if AO_BUTTON_COUNT > 1
- ao_button_update(1);
+ case 1: return ao_button_value(1);
#endif
#if AO_BUTTON_COUNT > 2
- ao_button_update(2);
+ case 2: return ao_button_value(2);
#endif
#if AO_BUTTON_COUNT > 3
- ao_button_update(3);
+ case 3: return ao_button_value(3);
#endif
#if AO_BUTTON_COUNT > 4
- ao_button_update(4);
+ case 4: return ao_button_value(4);
#endif
+ }
+}
+
+static void
+_ao_button_set(struct ao_debounce *debounce, uint8_t value)
+{
+ uint8_t b = debounce - ao_button_debounce;
+
+ ao_button_queue(b, value);
+}
+
+
+#define ao_button_update(b) ao_button_do(b, ao_gpio_get(port(b), bit(b), pin(b)))
+
+static void
+ao_button_debounce_init(struct ao_debounce *debounce) {
+ ao_debounce_config(debounce,
+ _ao_button_get,
+ _ao_button_set,
+ AO_BUTTON_DEBOUNCE_HOLD);
+}
+
+static void
+ao_button_isr(void)
+{
+ uint8_t b;
+
+ for (b = 0; b < AO_BUTTON_COUNT; b++)
+ _ao_debounce_start(&ao_button_debounce[b]);
}
#define init(b) do { \
+ ao_button_debounce_init(&ao_button_debounce[b]); \
ao_enable_port(port(b)); \
\
ao_exti_setup(port(b), bit(b), \
@@ -91,4 +108,14 @@ ao_button_init(void)
#if AO_BUTTON_COUNT > 1
init(1);
#endif
+#if AO_BUTTON_COUNT > 2
+ init(2);
+#endif
+#if AO_BUTTON_COUNT > 3
+ init(3);
+#endif
+#if AO_BUTTON_COUNT > 4
+ init(4);
+#endif
+ ao_debounce_init();
}
diff --git a/src/drivers/ao_cc1120.c b/src/drivers/ao_cc1120.c
index 772014ee..37d04927 100644
--- a/src/drivers/ao_cc1120.c
+++ b/src/drivers/ao_cc1120.c
@@ -326,6 +326,8 @@ static const uint16_t packet_setup[] = {
(0 << CC1120_PKT_CFG0_PKG_BIT_LEN) |
(0 << CC1120_PKT_CFG0_UART_MODE_EN) |
(0 << CC1120_PKT_CFG0_UART_SWAP_EN)),
+ CC1120_PREAMBLE_CFG1, ((CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_4_BYTES << CC1120_PREAMBLE_CFG1_NUM_PREAMBLE) |
+ (CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_AA << CC1120_PREAMBLE_CFG1_PREAMBLE_WORD)),
AO_CC1120_MARC_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_MARC_MCU_WAKEUP,
};
@@ -389,6 +391,8 @@ static const uint16_t rdf_setup[] = {
(0 << CC1120_PKT_CFG0_PKG_BIT_LEN) |
(0 << CC1120_PKT_CFG0_UART_MODE_EN) |
(0 << CC1120_PKT_CFG0_UART_SWAP_EN)),
+ CC1120_PREAMBLE_CFG1, ((CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_NONE << CC1120_PREAMBLE_CFG1_NUM_PREAMBLE) |
+ (CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_AA << CC1120_PREAMBLE_CFG1_PREAMBLE_WORD)),
};
/*
@@ -433,6 +437,33 @@ static const uint16_t aprs_setup[] = {
(CC1120_PKT_CFG1_ADDR_CHECK_CFG_NONE << CC1120_PKT_CFG1_ADDR_CHECK_CFG) |
(CC1120_PKT_CFG1_CRC_CFG_DISABLED << CC1120_PKT_CFG1_CRC_CFG) |
(0 << CC1120_PKT_CFG1_APPEND_STATUS)),
+ CC1120_PREAMBLE_CFG1, ((CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_NONE << CC1120_PREAMBLE_CFG1_NUM_PREAMBLE) |
+ (CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_AA << CC1120_PREAMBLE_CFG1_PREAMBLE_WORD)),
+};
+
+/*
+ * For Test mode, we want an unmodulated carrier. To do that, we
+ * set the deviation to zero and enable a preamble so that the radio
+ * turns on before we send any data
+ */
+
+static const uint16_t test_setup[] = {
+ CC1120_DEVIATION_M, 0,
+ CC1120_MODCFG_DEV_E, ((CC1120_MODCFG_DEV_E_MODEM_MODE_NORMAL << CC1120_MODCFG_DEV_E_MODEM_MODE) |
+ (CC1120_MODCFG_DEV_E_MOD_FORMAT_2_GFSK << CC1120_MODCFG_DEV_E_MOD_FORMAT) |
+ (0 << CC1120_MODCFG_DEV_E_DEV_E)),
+ CC1120_DRATE2, ((APRS_DRATE_E << CC1120_DRATE2_DATARATE_E) |
+ (((APRS_DRATE_M >> 16) & CC1120_DRATE2_DATARATE_M_19_16_MASK) << CC1120_DRATE2_DATARATE_M_19_16)),
+ CC1120_DRATE1, ((APRS_DRATE_M >> 8) & 0xff),
+ CC1120_DRATE0, ((APRS_DRATE_M >> 0) & 0xff),
+ CC1120_PKT_CFG2, ((CC1120_PKT_CFG2_CCA_MODE_ALWAYS_CLEAR << CC1120_PKT_CFG2_CCA_MODE) |
+ (CC1120_PKT_CFG2_PKT_FORMAT_NORMAL << CC1120_PKT_CFG2_PKT_FORMAT)),
+ CC1120_PKT_CFG1, ((0 << CC1120_PKT_CFG1_WHITE_DATA) |
+ (CC1120_PKT_CFG1_ADDR_CHECK_CFG_NONE << CC1120_PKT_CFG1_ADDR_CHECK_CFG) |
+ (CC1120_PKT_CFG1_CRC_CFG_DISABLED << CC1120_PKT_CFG1_CRC_CFG) |
+ (0 << CC1120_PKT_CFG1_APPEND_STATUS)),
+ CC1120_PREAMBLE_CFG1, ((CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_4_BYTES << CC1120_PREAMBLE_CFG1_NUM_PREAMBLE) |
+ (CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_AA << CC1120_PREAMBLE_CFG1_PREAMBLE_WORD)),
};
#define AO_PKT_CFG0_INFINITE ((0 << CC1120_PKT_CFG0_RESERVED7) | \
@@ -456,8 +487,9 @@ static uint16_t ao_radio_mode;
#define AO_RADIO_MODE_BITS_PACKET_RX 16
#define AO_RADIO_MODE_BITS_RDF 32
#define AO_RADIO_MODE_BITS_APRS 64
-#define AO_RADIO_MODE_BITS_INFINITE 128
-#define AO_RADIO_MODE_BITS_FIXED 256
+#define AO_RADIO_MODE_BITS_TEST 128
+#define AO_RADIO_MODE_BITS_INFINITE 256
+#define AO_RADIO_MODE_BITS_FIXED 512
#define AO_RADIO_MODE_NONE 0
#define AO_RADIO_MODE_PACKET_TX_BUF (AO_RADIO_MODE_BITS_PACKET | AO_RADIO_MODE_BITS_PACKET_TX | AO_RADIO_MODE_BITS_TX_BUF)
@@ -467,6 +499,7 @@ static uint16_t ao_radio_mode;
#define AO_RADIO_MODE_APRS_BUF (AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_INFINITE | AO_RADIO_MODE_BITS_TX_BUF)
#define AO_RADIO_MODE_APRS_LAST_BUF (AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_FIXED | AO_RADIO_MODE_BITS_TX_BUF)
#define AO_RADIO_MODE_APRS_FINISH (AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_FIXED | AO_RADIO_MODE_BITS_TX_FINISH)
+#define AO_RADIO_MODE_TEST (AO_RADIO_MODE_BITS_TEST | AO_RADIO_MODE_BITS_INFINITE | AO_RADIO_MODE_BITS_TX_BUF)
static void
ao_radio_set_mode(uint16_t new_mode)
@@ -504,6 +537,10 @@ ao_radio_set_mode(uint16_t new_mode)
for (i = 0; i < sizeof (aprs_setup) / sizeof (aprs_setup[0]); i += 2)
ao_radio_reg_write(aprs_setup[i], aprs_setup[i+1]);
+ if (changes & AO_RADIO_MODE_BITS_TEST)
+ for (i = 0; i < sizeof (test_setup) / sizeof (test_setup[0]); i += 2)
+ ao_radio_reg_write(test_setup[i], test_setup[i+1]);
+
if (changes & AO_RADIO_MODE_BITS_INFINITE)
ao_radio_reg_write(CC1120_PKT_CFG0, AO_PKT_CFG0_INFINITE);
@@ -652,6 +689,7 @@ ao_radio_test_cmd(void)
ao_packet_slave_stop();
#endif
ao_radio_get(0xff);
+ ao_radio_set_mode(AO_RADIO_MODE_TEST);
ao_radio_strobe(CC1120_STX);
#if CC1120_TRACE
{ int t;
diff --git a/src/drivers/ao_cc1120.h b/src/drivers/ao_cc1120.h
index 5d226b64..a1d78c01 100644
--- a/src/drivers/ao_cc1120.h
+++ b/src/drivers/ao_cc1120.h
@@ -404,6 +404,8 @@
#define CC1120_MARC_SPARE (CC1120_EXTENDED_BIT | 0x03)
#define CC1120_ECG_CFG (CC1120_EXTENDED_BIT | 0x04)
#define CC1120_SOFT_TX_DATA_CFG (CC1120_EXTENDED_BIT | 0x05)
+#define CC1120_SOFT_TX_DATA_CFG_SYMBOL_MAP_CFG 5
+#define CC1120_SOFT_TX_DATA_CFG_SOFT_TX_DATA_EN 0
#define CC1120_EXT_CTRL (CC1120_EXTENDED_BIT | 0x06)
#define CC1120_RCCAL_FINE (CC1120_EXTENDED_BIT | 0x07)
#define CC1120_RCCAL_COARSE (CC1120_EXTENDED_BIT | 0x08)
diff --git a/src/drivers/ao_cc115l.c b/src/drivers/ao_cc115l.c
index 05e6a762..0fa1e899 100644
--- a/src/drivers/ao_cc115l.c
+++ b/src/drivers/ao_cc115l.c
@@ -245,6 +245,8 @@ ao_radio_idle(void)
uint8_t state = ao_radio_strobe(CC115L_SIDLE);
if ((state >> CC115L_STATUS_STATE) == CC115L_STATUS_STATE_IDLE)
break;
+ if ((state >> CC115L_STATUS_STATE) == CC115L_STATUS_STATE_TX_FIFO_UNDERFLOW)
+ ao_radio_strobe(CC115L_SFTX);
}
/* Flush any pending TX bytes */
ao_radio_strobe(CC115L_SFTX);
@@ -476,6 +478,8 @@ ao_radio_setup(void)
ao_config_get();
+ ao_radio_strobe(CC115L_SCAL);
+
ao_radio_configured = 1;
}
@@ -494,7 +498,6 @@ static void
ao_radio_get(void)
{
static uint32_t last_radio_setting;
- static uint8_t last_power_setting;
ao_mutex_get(&ao_radio_mutex);
if (!ao_radio_configured)
@@ -505,10 +508,6 @@ ao_radio_get(void)
ao_radio_reg_write(CC115L_FREQ0, ao_config.radio_setting);
last_radio_setting = ao_config.radio_setting;
}
- if (ao_config.radio_power != last_power_setting) {
- ao_radio_reg_write(CC115L_PA, ao_config.radio_power);
- last_power_setting = ao_config.radio_power;
- }
}
static void
@@ -614,6 +613,20 @@ ao_radio_rdf_abort(void)
ao_wakeup(&ao_radio_wake);
}
+#define POWER_STEP 0x08
+
+static void
+ao_radio_stx(void)
+{
+ uint8_t power;
+ ao_radio_pa_on();
+ ao_radio_reg_write(CC115L_PA, 0);
+ ao_radio_strobe(CC115L_STX);
+ for (power = POWER_STEP; power < ao_config.radio_power; power += POWER_STEP)
+ ao_radio_reg_write(CC115L_PA, power);
+ ao_radio_reg_write(CC115L_PA, ao_config.radio_power);
+}
+
static void
ao_radio_test_cmd(void)
{
@@ -633,11 +646,10 @@ ao_radio_test_cmd(void)
ao_packet_slave_stop();
#endif
ao_radio_get();
- ao_radio_set_len(0xff);
- ao_radio_set_mode(AO_RADIO_MODE_RDF|AO_RADIO_MODE_BITS_FIXED);
ao_radio_strobe(CC115L_SFTX);
- ao_radio_pa_on();
- ao_radio_strobe(CC115L_STX);
+ ao_radio_set_len(0xff);
+ ao_radio_set_mode(AO_RADIO_MODE_RDF);
+ ao_radio_stx();
radio_on = 1;
}
if (mode == 3) {
@@ -655,12 +667,14 @@ ao_radio_test_cmd(void)
}
}
+#if CC115L_TRACE
static inline int16_t
ao_radio_gpio_bits(void)
{
return AO_CC115L_DONE_INT_PORT->idr & ((1 << AO_CC115L_FIFO_INT_PIN) |
(1 << AO_CC115L_DONE_INT_PIN));
}
+#endif
static void
ao_radio_wait_fifo(void)
@@ -738,6 +752,10 @@ _ao_radio_send_lots(ao_radio_fill_func fill, uint8_t mode)
uint8_t fifo_space;
fifo_space = CC115L_FIFO_SIZE;
+ ao_radio_abort = 0;
+
+ ao_radio_strobe(CC115L_SFTX);
+
ao_radio_done = 0;
ao_radio_fifo = 0;
while (!done) {
@@ -784,8 +802,7 @@ _ao_radio_send_lots(ao_radio_fill_func fill, uint8_t mode)
ao_exti_enable(AO_CC115L_DONE_INT_PORT, AO_CC115L_DONE_INT_PIN);
if (!started) {
- ao_radio_pa_on();
- ao_radio_strobe(CC115L_STX);
+ ao_radio_stx();
started = 1;
}
}
diff --git a/src/drivers/ao_event.c b/src/drivers/ao_event.c
index 440ef2de..c428125d 100644
--- a/src/drivers/ao_event.c
+++ b/src/drivers/ao_event.c
@@ -25,11 +25,6 @@
#define ao_event_queue_empty() (ao_event_queue_insert == ao_event_queue_remove)
#define ao_event_queue_full() (ao_event_queue_next(ao_event_queue_insert) == ao_event_queue_remove)
-/*
- * Whether a sequence of events from the same device should be collapsed
- */
-#define ao_event_can_collapse(type) ((type) == AO_EVENT_QUADRATURE)
-
struct ao_event ao_event_queue[AO_EVENT_QUEUE];
uint8_t ao_event_queue_insert;
uint8_t ao_event_queue_remove;
@@ -48,17 +43,9 @@ ao_event_get(struct ao_event *ev)
/* called with interrupts disabled */
void
-ao_event_put_isr(uint8_t type, uint8_t unit, uint32_t value)
+ao_event_put_isr(uint8_t type, uint8_t unit, int32_t value)
{
if (!ao_event_queue_full()) {
-
- if (ao_event_can_collapse(type) && !ao_event_queue_empty()) {
- uint8_t prev = ao_event_queue_prev(ao_event_queue_insert);
-
- if (ao_event_queue[prev].type == type &&
- ao_event_queue[prev].unit == unit)
- ao_event_queue_insert = prev;
- }
ao_event_queue[ao_event_queue_insert] = (struct ao_event) {
.type = type,
.unit = unit,
@@ -71,7 +58,7 @@ ao_event_put_isr(uint8_t type, uint8_t unit, uint32_t value)
}
void
-ao_event_put(uint8_t type, uint8_t unit, uint32_t value)
+ao_event_put(uint8_t type, uint8_t unit, int32_t value)
{
ao_arch_critical(ao_event_put_isr(type, unit, value););
}
diff --git a/src/drivers/ao_event.h b/src/drivers/ao_event.h
index 25c49c35..ed9a7433 100644
--- a/src/drivers/ao_event.h
+++ b/src/drivers/ao_event.h
@@ -26,16 +26,16 @@ struct ao_event {
uint8_t type;
uint8_t unit;
uint16_t tick;
- uint32_t value;
+ int32_t value;
};
uint8_t
ao_event_get(struct ao_event *ev);
void
-ao_event_put_isr(uint8_t type, uint8_t unit, uint32_t value);
+ao_event_put_isr(uint8_t type, uint8_t unit, int32_t value);
void
-ao_event_put(uint8_t type, uint8_t unit, uint32_t value);
+ao_event_put(uint8_t type, uint8_t unit, int32_t value);
#endif /* _AO_EVENT_H_ */
diff --git a/src/drivers/ao_gps_sirf.c b/src/drivers/ao_gps_sirf.c
index 91fc948b..d89435b9 100644
--- a/src/drivers/ao_gps_sirf.c
+++ b/src/drivers/ao_gps_sirf.c
@@ -19,6 +19,7 @@
#include "ao.h"
#endif
+__xdata uint8_t ao_gps_new;
__xdata uint8_t ao_gps_mutex;
__pdata uint16_t ao_gps_tick;
__xdata struct ao_telemetry_location ao_gps_data;
@@ -422,8 +423,9 @@ ao_gps(void) __reentrant
else
ao_gps_data.v_error = ao_sirf_data.v_error / 100;
#endif
+ ao_gps_new |= AO_GPS_NEW_DATA;
ao_mutex_put(&ao_gps_mutex);
- ao_wakeup(&ao_gps_data);
+ ao_wakeup(&ao_gps_new);
break;
case 4:
ao_mutex_get(&ao_gps_mutex);
@@ -432,8 +434,9 @@ ao_gps(void) __reentrant
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_gps_new |= AO_GPS_NEW_TRACKING;
ao_mutex_put(&ao_gps_mutex);
- ao_wakeup(&ao_gps_tracking_data);
+ ao_wakeup(&ao_gps_new);
break;
}
}
diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c
index d2f67e6b..944a37f9 100644
--- a/src/drivers/ao_gps_skytraq.c
+++ b/src/drivers/ao_gps_skytraq.c
@@ -32,6 +32,7 @@
#define ao_gps_set_speed ao_serial1_set_speed
#endif
+__xdata uint8_t ao_gps_new;
__xdata uint8_t ao_gps_mutex;
static __data char ao_gps_char;
static __data uint8_t ao_gps_cksum;
@@ -293,10 +294,11 @@ ao_nmea_gga(void)
if (!ao_gps_error) {
ao_mutex_get(&ao_gps_mutex);
+ ao_gps_new |= AO_GPS_NEW_DATA;
ao_gps_tick = ao_gps_next_tick;
ao_xmemcpy(&ao_gps_data, PDATA_TO_XDATA(&ao_gps_next), sizeof (ao_gps_data));
ao_mutex_put(&ao_gps_mutex);
- ao_wakeup(&ao_gps_data);
+ ao_wakeup(&ao_gps_new);
}
}
@@ -352,9 +354,10 @@ ao_nmea_gsv(void)
ao_gps_tracking_next.channels = 0;
else if (done) {
ao_mutex_get(&ao_gps_mutex);
+ ao_gps_new |= AO_GPS_NEW_TRACKING;
ao_xmemcpy(&ao_gps_tracking_data, PDATA_TO_XDATA(&ao_gps_tracking_next), sizeof(ao_gps_tracking_data));
ao_mutex_put(&ao_gps_mutex);
- ao_wakeup(&ao_gps_tracking_data);
+ ao_wakeup(&ao_gps_new);
}
}
@@ -483,25 +486,6 @@ ao_gps(void) __reentrant
__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", (long) ao_gps_data.latitude, (long) 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);
-}
-
static __code uint8_t ao_gps_115200[] = {
SKYTRAQ_MSG_3(5,0,5,0) /* Set to 115200 baud */
};
@@ -532,7 +516,7 @@ gps_update(void) __reentrant
}
__code struct ao_cmds ao_gps_cmds[] = {
- { gps_dump, "g\0Display GPS" },
+ { ao_gps_show, "g\0Display GPS" },
{ gps_update, "U\0Update GPS firmware" },
{ 0, NULL },
};
diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c
index 22300df3..4fb90746 100644
--- a/src/drivers/ao_gps_ublox.c
+++ b/src/drivers/ao_gps_ublox.c
@@ -21,16 +21,37 @@
#include "ao_gps_ublox.h"
+#define AO_UBLOX_DEBUG 0
+
+#include <stdarg.h>
+
+__xdata uint8_t ao_gps_new;
__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$PUBX,41,1,3,1,57600,0*2d\r\n";
+#undef AO_SERIAL_SPEED_UBLOX
-const char ao_gps_config[] = {
+#ifndef AO_SERIAL_SPEED_UBLOX
+#define AO_SERIAL_SPEED_UBLOX AO_SERIAL_SPEED_9600
+#endif
-};
+#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_57600
+#define SERIAL_SPEED_STRING "57600"
+#define SERIAL_SPEED_CHECKSUM "2d"
+#endif
+#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_19200
+#define SERIAL_SPEED_STRING "19200"
+#define SERIAL_SPEED_CHECKSUM "23"
+#endif
+#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_9600
+#define SERIAL_SPEED_STRING "9600"
+#define SERIAL_SPEED_CHECKSUM "16"
+#endif
+
+static const char ao_gps_set_nmea[] =
+ "\r\n$PUBX,41,1,3,1," SERIAL_SPEED_STRING ",0*" SERIAL_SPEED_CHECKSUM "\r\n";
struct ao_ublox_cksum {
uint8_t a, b;
@@ -39,13 +60,34 @@ struct ao_ublox_cksum {
static __pdata struct ao_ublox_cksum ao_ublox_cksum;
static __pdata uint16_t ao_ublox_len;
-#ifndef ao_ublox_getchar
-#define ao_ublox_getchar ao_serial1_getchar
-#define ao_ublox_putchar ao_serial1_putchar
-#define ao_ublox_set_speed ao_serial1_set_speed
+#if AO_UBLOX_DEBUG
+
+static uint8_t ao_gps_dbg_enable;
+
+#define DBG_PROTO 1
+#define DBG_CHAR 2
+#define DBG_INIT 4
+
+static void ao_gps_dbg(int level, char *format, ...) {
+ va_list a;
+
+ if (level & ao_gps_dbg_enable) {
+ va_start(a, format);
+ vprintf(format, a);
+ va_end(a);
+ flush();
+ }
+}
+
+#else
+#define ao_gps_dbg(fmt, ...)
#endif
-#define ao_ublox_byte() ((uint8_t) ao_ublox_getchar())
+static inline uint8_t ao_ublox_byte(void) {
+ uint8_t c = (uint8_t) ao_gps_getchar();
+ ao_gps_dbg(DBG_CHAR, " %02x", c);
+ return c;
+}
static inline void add_cksum(struct ao_ublox_cksum *cksum, uint8_t c)
{
@@ -61,7 +103,8 @@ static void ao_ublox_init_cksum(void)
static void ao_ublox_put_u8(uint8_t c)
{
add_cksum(&ao_ublox_cksum, c);
- ao_ublox_putchar(c);
+ ao_gps_dbg(DBG_CHAR, " (%02x)", c);
+ ao_gps_putchar(c);
}
static void ao_ublox_put_i8(int8_t c)
@@ -183,6 +226,7 @@ ao_ublox_parse(void __xdata *target, const struct ublox_packet_parse *parse) __r
break;
}
}
+ ao_gps_dbg(DBG_PROTO, "\n");
}
/*
@@ -326,6 +370,7 @@ ao_ublox_parse_nav_svinfo(void)
{
uint8_t nsat;
nav_svinfo_nsat = 0;
+
ao_ublox_parse(&nav_svinfo, nav_svinfo_packet);
for (nsat = 0; nsat < nav_svinfo.num_ch && ao_ublox_len >= 12; nsat++) {
if (nsat < NAV_SVINFO_MAX_SAT) {
@@ -334,6 +379,17 @@ ao_ublox_parse_nav_svinfo(void)
ublox_discard(12);
}
}
+#if AO_UBLOX_DEBUG
+ ao_gps_dbg(DBG_PROTO, "svinfo num_ch %d flags %02x\n", nav_svinfo.num_ch, nav_svinfo.flags);
+ for (nsat = 0; nsat < nav_svinfo.num_ch; nsat++)
+ ao_gps_dbg(DBG_PROTO, "\t%d: chn %d svid %d flags %02x quality %d cno %d\n",
+ nsat,
+ nav_svinfo_sat[nsat].chn,
+ nav_svinfo_sat[nsat].svid,
+ nav_svinfo_sat[nsat].flags,
+ nav_svinfo_sat[nsat].quality,
+ nav_svinfo_sat[nsat].cno);
+#endif
}
/*
@@ -402,45 +458,60 @@ ao_ublox_parse_nav_velned(void)
*/
static void
-ao_gps_setup(void)
+ao_gps_delay(void)
{
- uint8_t i, k;
- ao_ublox_set_speed(AO_SERIAL_SPEED_9600);
+ uint8_t i;
/*
* A bunch of nulls so the start bit
* is clear
*/
+
for (i = 0; i < 64; i++)
- ao_ublox_putchar(0x00);
+ ao_gps_putchar(0x00);
+}
+
+static void
+ao_gps_setup(void)
+{
+ uint8_t i, k;
+
+ ao_delay(AO_SEC_TO_TICKS(3));
+
+ ao_gps_dbg(DBG_INIT, "Set speed 9600\n");
+ ao_gps_set_speed(AO_SERIAL_SPEED_9600);
/*
* Send the baud-rate setting and protocol-setting
* command three times
*/
- for (k = 0; k < 3; k++)
+ for (k = 0; k < 3; k++) {
+ ao_gps_delay();
+
+ ao_gps_dbg(DBG_INIT, "Send initial setting\n");
for (i = 0; i < sizeof (ao_gps_set_nmea); i++)
- ao_ublox_putchar(ao_gps_set_nmea[i]);
+ ao_gps_putchar(ao_gps_set_nmea[i]);
+ }
+
+ ao_gps_delay();
+#if AO_SERIAL_SPEED_UBLOX != AO_SERIAL_SPEED_9600
+ ao_gps_dbg(DBG_INIT, "Set speed high\n");
/*
* Increase the baud rate
*/
- ao_ublox_set_speed(AO_SERIAL_SPEED_57600);
+ ao_gps_set_speed(AO_SERIAL_SPEED_UBLOX);
+#endif
- /*
- * Pad with nulls to give the chip
- * time to see the baud rate switch
- */
- for (i = 0; i < 64; i++)
- ao_ublox_putchar(0x00);
+ ao_gps_delay();
}
static void
ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len)
{
ao_ublox_init_cksum();
- ao_ublox_putchar(0xb5);
- ao_ublox_putchar(0x62);
+ ao_gps_putchar(0xb5);
+ ao_gps_putchar(0x62);
ao_ublox_put_u8(class);
ao_ublox_put_u8(id);
ao_ublox_put_u8(len);
@@ -450,8 +521,8 @@ ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len)
static void
ao_ublox_putend(void)
{
- ao_ublox_putchar(ao_ublox_cksum.a);
- ao_ublox_putchar(ao_ublox_cksum.b);
+ ao_gps_putchar(ao_ublox_cksum.a);
+ ao_gps_putchar(ao_ublox_cksum.b);
}
static void
@@ -547,7 +618,7 @@ ao_gps(void) __reentrant
/* Enable all of the messages we want */
for (i = 0; i < sizeof (ublox_enable_nav); i++)
ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], 1);
-
+
ao_ublox_set_navigation_settings((1 << UBLOX_CFG_NAV5_MASK_DYN) | (1 << UBLOX_CFG_NAV5_MASK_FIXMODE),
UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_4G,
UBLOX_CFG_NAV5_FIXMODE_3D,
@@ -578,6 +649,8 @@ ao_gps(void) __reentrant
ao_ublox_len = header_byte();
ao_ublox_len |= header_byte() << 8;
+ ao_gps_dbg(DBG_PROTO, "class %02x id %02x len %d\n", class, id, ao_ublox_len);
+
if (ao_ublox_len > 1023)
continue;
@@ -618,8 +691,10 @@ ao_gps(void) __reentrant
break;
}
- if (ao_ublox_len != 0)
+ if (ao_ublox_len != 0) {
+ ao_gps_dbg(DBG_PROTO, "len left %d\n", ao_ublox_len);
continue;
+ }
/* verify checksum and end sequence */
cksum.a = ao_ublox_byte();
@@ -628,9 +703,9 @@ ao_gps(void) __reentrant
continue;
switch (class) {
- case 0x01:
+ case UBLOX_NAV:
switch (id) {
- case 0x21:
+ case UBLOX_NAV_TIMEUTC:
ao_mutex_get(&ao_gps_mutex);
ao_gps_tick = ao_time();
@@ -645,7 +720,7 @@ ao_gps(void) __reentrant
}
if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC))
ao_gps_data.flags |= AO_GPS_DATE_VALID;
-
+
ao_gps_data.altitude = nav_posllh.alt_msl / 1000;
ao_gps_data.latitude = nav_posllh.lat;
ao_gps_data.longitude = nav_posllh.lon;
@@ -667,7 +742,7 @@ ao_gps(void) __reentrant
ao_gps_data.ground_speed = nav_velned.g_speed;
ao_gps_data.climb_rate = -nav_velned.vel_d;
ao_gps_data.course = nav_velned.heading / 200000;
-
+
ao_gps_tracking_data.channels = 0;
struct ao_telemetry_satellite_info *dst = &ao_gps_tracking_data.sats[0];
@@ -686,8 +761,8 @@ ao_gps(void) __reentrant
}
ao_mutex_put(&ao_gps_mutex);
- ao_wakeup(&ao_gps_data);
- ao_wakeup(&ao_gps_tracking_data);
+ ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING;
+ ao_wakeup(&ao_gps_new);
break;
}
break;
@@ -695,10 +770,32 @@ ao_gps(void) __reentrant
}
}
+#if AO_UBLOX_DEBUG
+static void ao_gps_option(void)
+{
+ ao_cmd_hex();
+ if (ao_cmd_status != ao_cmd_success) {
+ ao_cmd_status = ao_cmd_success;
+ ao_gps_show();
+ } else {
+ ao_gps_dbg_enable = ao_cmd_lex_i;
+ printf ("gps debug set to %d\n", ao_gps_dbg_enable);
+ }
+}
+#else
+#define ao_gps_option ao_gps_show
+#endif
+
+__code struct ao_cmds ao_gps_cmds[] = {
+ { ao_gps_option, "g\0Display GPS" },
+ { 0, NULL },
+};
+
__xdata struct ao_task ao_gps_task;
void
ao_gps_init(void)
{
+ ao_cmd_register(&ao_gps_cmds[0]);
ao_add_task(&ao_gps_task, ao_gps, "gps");
}
diff --git a/src/drivers/ao_m25.c b/src/drivers/ao_m25.c
index 390637d7..e6c7bb4d 100644
--- a/src/drivers/ao_m25.c
+++ b/src/drivers/ao_m25.c
@@ -82,11 +82,11 @@ __pdata uint16_t ao_storage_unit;
#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 ao_port_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 ao_port_t ao_m25_wip; /* write in progress */
static __xdata uint8_t ao_m25_mutex;
@@ -112,7 +112,7 @@ static __xdata uint8_t ao_m25_instruction[4];
* Block until the specified chip is done writing
*/
static void
-ao_m25_wait_wip(uint8_t cs)
+ao_m25_wait_wip(ao_port_t cs)
{
if (ao_m25_wip & cs) {
M25_SELECT(cs);
@@ -132,7 +132,7 @@ ao_m25_wait_wip(uint8_t cs)
* so that future operations will block until the WIP bit goes off
*/
static void
-ao_m25_write_enable(uint8_t cs)
+ao_m25_write_enable(ao_port_t cs)
{
M25_SELECT(cs);
ao_m25_instruction[0] = M25_WREN;
@@ -146,7 +146,7 @@ ao_m25_write_enable(uint8_t cs)
* Returns the number of 64kB sectors
*/
static uint8_t
-ao_m25_read_capacity(uint8_t cs)
+ao_m25_read_capacity(ao_port_t cs)
{
uint8_t capacity;
M25_SELECT(cs);
@@ -166,12 +166,13 @@ ao_m25_read_capacity(uint8_t cs)
return 1 << (capacity - 0x10);
}
-static uint8_t
+static ao_port_t
ao_m25_set_address(uint32_t pos)
{
- uint8_t chip;
+ ao_port_t mask;
#if M25_MAX_CHIPS > 1
uint8_t size;
+ uint8_t chip;
for (chip = 0; chip < ao_m25_numchips; chip++) {
size = ao_m25_size[chip];
@@ -182,16 +183,16 @@ ao_m25_set_address(uint32_t pos)
if (chip == ao_m25_numchips)
return 0xff;
- chip = ao_m25_pin[chip];
+ mask = ao_m25_pin[chip];
#else
- chip = AO_M25_SPI_CS_MASK;
+ mask = AO_M25_SPI_CS_MASK;
#endif
- ao_m25_wait_wip(chip);
+ ao_m25_wait_wip(mask);
ao_m25_instruction[1] = pos >> 16;
ao_m25_instruction[2] = pos >> 8;
ao_m25_instruction[3] = pos;
- return chip;
+ return mask;
}
/*
@@ -239,7 +240,7 @@ ao_m25_scan(void)
uint8_t
ao_storage_erase(uint32_t pos) __reentrant
{
- uint8_t cs;
+ ao_port_t cs;
if (pos >= ao_storage_total || pos + ao_storage_block > ao_storage_total)
return 0;
@@ -249,7 +250,6 @@ ao_storage_erase(uint32_t pos) __reentrant
cs = ao_m25_set_address(pos);
- ao_m25_wait_wip(cs);
ao_m25_write_enable(cs);
ao_m25_instruction[0] = M25_SE;
@@ -268,7 +268,7 @@ ao_storage_erase(uint32_t pos) __reentrant
uint8_t
ao_storage_device_write(uint32_t pos, __xdata void *d, uint16_t len) __reentrant
{
- uint8_t cs;
+ ao_port_t cs;
if (pos >= ao_storage_total || pos + len > ao_storage_total)
return 0;
@@ -295,7 +295,7 @@ ao_storage_device_write(uint32_t pos, __xdata void *d, uint16_t len) __reentrant
uint8_t
ao_storage_device_read(uint32_t pos, __xdata void *d, uint16_t len) __reentrant
{
- uint8_t cs;
+ ao_port_t cs;
if (pos >= ao_storage_total || pos + len > ao_storage_total)
return 0;
@@ -332,7 +332,7 @@ void
ao_storage_device_info(void) __reentrant
{
#if M25_DEBUG
- uint8_t cs;
+ ao_port_t cs;
#endif
#if M25_MAX_CHIPS > 1
uint8_t chip;
diff --git a/src/drivers/ao_mma655x.c b/src/drivers/ao_mma655x.c
index 28fe1e08..ce83a5a3 100644
--- a/src/drivers/ao_mma655x.c
+++ b/src/drivers/ao_mma655x.c
@@ -206,10 +206,7 @@ ao_mma655x_setup(void)
ao_mma655x_reg_write(AO_MMA655X_AXISCFG,
AXISCFG_VALUE |
(1 << AO_MMA655X_AXISCFG_ST));
- for (i = 0; i < 10; i++) {
- a_st = ao_mma655x_value();
- printf ("SELF-TEST %2d = %6d\n", i, a_st);
- }
+ a_st = ao_mma655x_value();
stdefl = ao_mma655x_reg_read(AO_MMA655X_STDEFL);
@@ -218,11 +215,6 @@ ao_mma655x_setup(void)
(0 << AO_MMA655X_AXISCFG_ST));
a = ao_mma655x_value();
- for (i = 0; i < 10; i++) {
- a = ao_mma655x_value();
- printf("NORMAL %2d = %6d\n", i, a);
- }
-
ao_mma655x_reg_write(AO_MMA655X_DEVCFG,
DEVCFG_VALUE | (1 << AO_MMA655X_DEVCFG_ENDINIT));
s0 = ao_mma655x_reg_read(AO_MMA655X_SN0);
@@ -234,8 +226,6 @@ ao_mma655x_setup(void)
serial = lot & 0x1fff;
lot >>= 12;
pn = ao_mma655x_reg_read(AO_MMA655X_PN);
- printf ("MMA655X lot %d serial %d number %d\n", lot, serial, pn);
-
}
uint16_t ao_mma655x_current;
diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index c65aecbc..f8ce7346 100644
--- a/src/drivers/ao_mpu6000.c
+++ b/src/drivers/ao_mpu6000.c
@@ -19,84 +19,94 @@
#include <ao_mpu6000.h>
#include <ao_exti.h>
+#if HAS_MPU6000
+
static uint8_t ao_mpu6000_wake;
static uint8_t ao_mpu6000_configured;
-#define ao_mpu6000_spi_get() ao_spi_get_bit(AO_MPU6000_SPI_CS_PORT, \
- AO_MPU6000_SPI_CS_PIN, \
- AO_MPU6000_SPI_CS, \
- AO_MPU6000_SPI_BUS, \
- AO_SPI_SPEED_1MHz)
+#ifndef AO_MPU6000_I2C_INDEX
+#define AO_MPU6000_SPI 1
+#else
+#define AO_MPU6000_SPI 0
+#endif
+
+#if AO_MPU6000_SPI
+
+#define ao_mpu6000_spi_get() ao_spi_get(AO_MPU6000_SPI_BUS, AO_SPI_SPEED_1MHz)
+#define ao_mpu6000_spi_put() ao_spi_put(AO_MPU6000_SPI_BUS)
+
+#define ao_mpu6000_spi_start() ao_spi_set_cs(AO_MPU6000_SPI_CS_PORT, \
+ (1 << AO_MPU6000_SPI_CS_PIN))
+
+#define ao_mpu6000_spi_end() ao_spi_clr_cs(AO_MPU6000_SPI_CS_PORT, \
+ (1 << AO_MPU6000_SPI_CS_PIN))
-#define ao_mpu6000_spi_put() ao_spi_put_bit(AO_MPU6000_SPI_CS_PORT, \
- AO_MPU6000_SPI_CS_PIN, \
- AO_MPU6000_SPI_CS, \
- AO_MPU6000_SPI_BUS)
+#endif
static void
-ao_mpu6000_reg_write(uint8_t addr, uint8_t value)
+_ao_mpu6000_reg_write(uint8_t addr, uint8_t value)
{
uint8_t d[2] = { addr, value };
-#ifdef AO_MPU6000_I2C_INDEX
+#if AO_MPU6000_SPI
+ ao_mpu6000_spi_start();
+ ao_spi_send(d, 2, AO_MPU6000_SPI_BUS);
+ ao_mpu6000_spi_end();
+#else
ao_i2c_get(AO_MPU6000_I2C_INDEX);
ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
ao_i2c_send(d, 2, AO_MPU6000_I2C_INDEX, TRUE);
ao_i2c_put(AO_MPU6000_I2C_INDEX);
-#else
- ao_mpu6000_spi_get();
- ao_spi_send(d, 2, AO_MPU6000_SPI_BUS);
- ao_mpu6000_spi_put();
#endif
}
static void
-ao_mpu6000_read(uint8_t addr, void *data, uint8_t len)
+_ao_mpu6000_read(uint8_t addr, void *data, uint8_t len)
{
-#ifdef AO_MPU6000_I2C_INDEX
+#if AO_MPU6000_SPI
+ addr |= 0x80;
+ ao_mpu6000_spi_start();
+ ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS);
+ ao_spi_recv(data, len, AO_MPU6000_SPI_BUS);
+ ao_mpu6000_spi_end();
+#else
ao_i2c_get(AO_MPU6000_I2C_INDEX);
ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX, FALSE);
ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_READ);
ao_i2c_recv(data, len, AO_MPU6000_I2C_INDEX, TRUE);
ao_i2c_put(AO_MPU6000_I2C_INDEX);
-#else
- addr |= 0x80;
- ao_mpu6000_spi_get();
- ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS);
- ao_spi_recv(data, len, AO_MPU6000_SPI_BUS);
- ao_mpu6000_spi_put();
#endif
}
static uint8_t
-ao_mpu6000_reg_read(uint8_t addr)
+_ao_mpu6000_reg_read(uint8_t addr)
{
uint8_t value;
-#ifdef AO_MPU6000_I2C_INDEX
+#if AO_MPU6000_SPI
+ addr |= 0x80;
+ ao_mpu6000_spi_start();
+ ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS);
+ ao_spi_recv(&value, 1, AO_MPU6000_SPI_BUS);
+ ao_mpu6000_spi_end();
+#else
ao_i2c_get(AO_MPU6000_I2C_INDEX);
ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX, FALSE);
ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_READ);
ao_i2c_recv(&value, 1, AO_MPU6000_I2C_INDEX, TRUE);
ao_i2c_put(AO_MPU6000_I2C_INDEX);
-#else
- addr |= 0x80;
- ao_mpu6000_spi_get();
- ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS);
- ao_spi_recv(&value, 1, AO_MPU6000_SPI_BUS);
- ao_mpu6000_spi_put();
#endif
return value;
}
static void
-ao_mpu6000_sample(struct ao_mpu6000_sample *sample)
+_ao_mpu6000_sample(struct ao_mpu6000_sample *sample)
{
uint16_t *d = (uint16_t *) sample;
int i = sizeof (*sample) / 2;
- ao_mpu6000_read(MPU6000_ACCEL_XOUT_H, sample, sizeof (*sample));
+ _ao_mpu6000_read(MPU6000_ACCEL_XOUT_H, sample, sizeof (*sample));
#if __BYTE_ORDER == __LITTLE_ENDIAN
/* byte swap */
while (i--) {
@@ -108,6 +118,7 @@ ao_mpu6000_sample(struct ao_mpu6000_sample *sample)
#define G 981 /* in cm/s² */
+#if 0
static int16_t /* cm/s² */
ao_mpu6000_accel(int16_t v)
{
@@ -119,16 +130,17 @@ ao_mpu6000_gyro(int16_t v)
{
return (int16_t) ((v * (int32_t) 20000) / 32767);
}
+#endif
static uint8_t
ao_mpu6000_accel_check(int16_t normal, int16_t test, char *which)
{
int16_t diff = test - normal;
- if (diff < MPU6000_ST_ACCEL(16) / 2) {
+ if (diff < MPU6000_ST_ACCEL(16) / 4) {
return 1;
}
- if (diff > MPU6000_ST_ACCEL(16) * 2) {
+ if (diff > MPU6000_ST_ACCEL(16) * 4) {
return 1;
}
return 0;
@@ -141,142 +153,168 @@ ao_mpu6000_gyro_check(int16_t normal, int16_t test, char *which)
if (diff < 0)
diff = -diff;
- if (diff < MPU6000_ST_GYRO(2000) / 2) {
+ if (diff < MPU6000_ST_GYRO(2000) / 4) {
return 1;
}
- if (diff > MPU6000_ST_GYRO(2000) * 2) {
+ if (diff > MPU6000_ST_GYRO(2000) * 4) {
return 1;
}
return 0;
}
static void
-ao_mpu6000_setup(void)
+_ao_mpu6000_wait_alive(void)
+{
+ uint8_t i;
+
+ /* Wait for the chip to wake up */
+ for (i = 0; i < 30; i++) {
+ ao_delay(AO_MS_TO_TICKS(100));
+ if (_ao_mpu6000_reg_read(MPU6000_WHO_AM_I) == 0x68)
+ break;
+ }
+ if (i == 30)
+ ao_panic(AO_PANIC_SELF_TEST_MPU6000);
+}
+
+#define ST_TRIES 10
+
+static void
+_ao_mpu6000_setup(void)
{
struct ao_mpu6000_sample normal_mode, test_mode;
- int errors =0;
+ int errors;
+ int st_tries;
if (ao_mpu6000_configured)
return;
+ _ao_mpu6000_wait_alive();
+
/* Reset the whole chip */
- ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
- (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET));
+ _ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
+ (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET));
/* Wait for it to reset. If we talk too quickly, it appears to get confused */
- ao_delay(AO_MS_TO_TICKS(100));
- /* Reset signal conditioning */
- ao_mpu6000_reg_write(MPU6000_USER_CONTROL,
- (0 << MPU6000_USER_CONTROL_FIFO_EN) |
- (0 << MPU6000_USER_CONTROL_I2C_MST_EN) |
- (0 << MPU6000_USER_CONTROL_I2C_IF_DIS) |
- (0 << MPU6000_USER_CONTROL_FIFO_RESET) |
- (0 << MPU6000_USER_CONTROL_I2C_MST_RESET) |
- (1 << MPU6000_USER_CONTROL_SIG_COND_RESET));
+ _ao_mpu6000_wait_alive();
- while (ao_mpu6000_reg_read(MPU6000_USER_CONTROL) & (1 << MPU6000_USER_CONTROL_SIG_COND_RESET))
- ao_yield();
+ /* Reset signal conditioning, disabling I2C on SPI systems */
+ _ao_mpu6000_reg_write(MPU6000_USER_CTRL,
+ (0 << MPU6000_USER_CTRL_FIFO_EN) |
+ (0 << MPU6000_USER_CTRL_I2C_MST_EN) |
+ (AO_MPU6000_SPI << MPU6000_USER_CTRL_I2C_IF_DIS) |
+ (0 << MPU6000_USER_CTRL_FIFO_RESET) |
+ (0 << MPU6000_USER_CTRL_I2C_MST_RESET) |
+ (1 << MPU6000_USER_CTRL_SIG_COND_RESET));
+
+ while (_ao_mpu6000_reg_read(MPU6000_USER_CTRL) & (1 << MPU6000_USER_CTRL_SIG_COND_RESET))
+ ao_delay(AO_MS_TO_TICKS(10));
/* Reset signal paths */
- ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET,
- (1 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) |
- (1 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) |
- (1 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET));
+ _ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET,
+ (1 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) |
+ (1 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) |
+ (1 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET));
- ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET,
- (0 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) |
- (0 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) |
- (0 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET));
+ _ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET,
+ (0 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) |
+ (0 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) |
+ (0 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET));
/* Select clocks, disable sleep */
- ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
- (0 << MPU6000_PWR_MGMT_1_DEVICE_RESET) |
- (0 << MPU6000_PWR_MGMT_1_SLEEP) |
- (0 << MPU6000_PWR_MGMT_1_CYCLE) |
- (0 << MPU6000_PWR_MGMT_1_TEMP_DIS) |
- (MPU6000_PWR_MGMT_1_CLKSEL_PLL_X_AXIS << MPU6000_PWR_MGMT_1_CLKSEL));
+ _ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
+ (0 << MPU6000_PWR_MGMT_1_DEVICE_RESET) |
+ (0 << MPU6000_PWR_MGMT_1_SLEEP) |
+ (0 << MPU6000_PWR_MGMT_1_CYCLE) |
+ (0 << MPU6000_PWR_MGMT_1_TEMP_DIS) |
+ (MPU6000_PWR_MGMT_1_CLKSEL_PLL_X_AXIS << MPU6000_PWR_MGMT_1_CLKSEL));
- /* Set sample rate divider to sample at full speed
- ao_mpu6000_reg_write(MPU6000_SMPRT_DIV, 0);
+ /* Set sample rate divider to sample at full speed */
+ _ao_mpu6000_reg_write(MPU6000_SMPRT_DIV, 0);
/* Disable filtering */
- ao_mpu6000_reg_write(MPU6000_CONFIG,
- (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) |
- (MPU6000_CONFIG_DLPF_CFG_260_256 << MPU6000_CONFIG_DLPF_CFG));
-
- /* Configure accelerometer to +/-16G in self-test mode */
- ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
- (1 << MPU600_ACCEL_CONFIG_XA_ST) |
- (1 << MPU600_ACCEL_CONFIG_YA_ST) |
- (1 << MPU600_ACCEL_CONFIG_ZA_ST) |
- (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
-
- /* Configure gyro to +/- 2000°/s in self-test mode */
- ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
- (1 << MPU600_GYRO_CONFIG_XG_ST) |
- (1 << MPU600_GYRO_CONFIG_YG_ST) |
- (1 << MPU600_GYRO_CONFIG_ZG_ST) |
- (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
-
- ao_delay(AO_MS_TO_TICKS(200));
- ao_mpu6000_sample(&test_mode);
+ _ao_mpu6000_reg_write(MPU6000_CONFIG,
+ (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) |
+ (MPU6000_CONFIG_DLPF_CFG_260_256 << MPU6000_CONFIG_DLPF_CFG));
#if TRIDGE
// read the product ID rev c has 1/2 the sensitivity of rev d
- _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID);
- //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
-
- if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) ||
- (_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) {
- // Accel scale 8g (4096 LSB/g)
- // Rev C has different scaling than rev D
- register_write(MPUREG_ACCEL_CONFIG,1<<3);
- } else {
- // Accel scale 8g (4096 LSB/g)
- register_write(MPUREG_ACCEL_CONFIG,2<<3);
- }
- hal.scheduler->delay(1);
-
+ _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID);
+ //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
+
+ if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) ||
+ (_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) {
+ // Accel scale 8g (4096 LSB/g)
+ // Rev C has different scaling than rev D
+ register_write(MPUREG_ACCEL_CONFIG,1<<3);
+ } else {
+ // Accel scale 8g (4096 LSB/g)
+ register_write(MPUREG_ACCEL_CONFIG,2<<3);
+ }
+ hal.scheduler->delay(1);
#endif
- /* Configure accelerometer to +/-16G */
- ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
- (0 << MPU600_ACCEL_CONFIG_XA_ST) |
- (0 << MPU600_ACCEL_CONFIG_YA_ST) |
- (0 << MPU600_ACCEL_CONFIG_ZA_ST) |
- (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
-
- /* Configure gyro to +/- 2000°/s */
- ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
- (0 << MPU600_GYRO_CONFIG_XG_ST) |
- (0 << MPU600_GYRO_CONFIG_YG_ST) |
- (0 << MPU600_GYRO_CONFIG_ZG_ST) |
- (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
-
- ao_delay(AO_MS_TO_TICKS(10));
- ao_mpu6000_sample(&normal_mode);
+ for (st_tries = 0; st_tries < ST_TRIES; st_tries++) {
+ errors = 0;
+
+ /* Configure accelerometer to +/-16G in self-test mode */
+ _ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
+ (1 << MPU600_ACCEL_CONFIG_XA_ST) |
+ (1 << MPU600_ACCEL_CONFIG_YA_ST) |
+ (1 << MPU600_ACCEL_CONFIG_ZA_ST) |
+ (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
+
+ /* Configure gyro to +/- 2000°/s in self-test mode */
+ _ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
+ (1 << MPU600_GYRO_CONFIG_XG_ST) |
+ (1 << MPU600_GYRO_CONFIG_YG_ST) |
+ (1 << MPU600_GYRO_CONFIG_ZG_ST) |
+ (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
+
+ ao_delay(AO_MS_TO_TICKS(200));
+ _ao_mpu6000_sample(&test_mode);
+
+ /* Configure accelerometer to +/-16G */
+ _ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
+ (0 << MPU600_ACCEL_CONFIG_XA_ST) |
+ (0 << MPU600_ACCEL_CONFIG_YA_ST) |
+ (0 << MPU600_ACCEL_CONFIG_ZA_ST) |
+ (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
+
+ /* Configure gyro to +/- 2000°/s */
+ _ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
+ (0 << MPU600_GYRO_CONFIG_XG_ST) |
+ (0 << MPU600_GYRO_CONFIG_YG_ST) |
+ (0 << MPU600_GYRO_CONFIG_ZG_ST) |
+ (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
+
+ ao_delay(AO_MS_TO_TICKS(200));
+ _ao_mpu6000_sample(&normal_mode);
- errors += ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x, "x");
- errors += ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y, "y");
- errors += ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_z, "z");
-
- errors += ao_mpu6000_gyro_check(normal_mode.gyro_x, test_mode.gyro_x, "x");
- errors += ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y, "y");
- errors += ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z, "z");
+ errors += ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x, "x");
+ errors += ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y, "y");
+ errors += ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_z, "z");
+
+ errors += ao_mpu6000_gyro_check(normal_mode.gyro_x, test_mode.gyro_x, "x");
+ errors += ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y, "y");
+ errors += ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z, "z");
+ if (!errors)
+ break;
+ }
- if (errors)
- ao_panic(AO_PANIC_SELF_TEST_MPU6000);
+ if (st_tries == ST_TRIES)
+ ao_sensor_errors = 1;
/* Filter to about 100Hz, which also sets the gyro rate to 1000Hz */
- ao_mpu6000_reg_write(MPU6000_CONFIG,
- (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) |
- (MPU6000_CONFIG_DLPF_CFG_94_98 << MPU6000_CONFIG_DLPF_CFG));
+ _ao_mpu6000_reg_write(MPU6000_CONFIG,
+ (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) |
+ (MPU6000_CONFIG_DLPF_CFG_94_98 << MPU6000_CONFIG_DLPF_CFG));
/* Set sample rate divider to sample at 200Hz (v = gyro/rate - 1) */
- ao_mpu6000_reg_write(MPU6000_SMPRT_DIV,
- 1000 / 200 - 1);
+ _ao_mpu6000_reg_write(MPU6000_SMPRT_DIV,
+ 1000 / 200 - 1);
ao_delay(AO_MS_TO_TICKS(100));
ao_mpu6000_configured = 1;
@@ -287,10 +325,20 @@ struct ao_mpu6000_sample ao_mpu6000_current;
static void
ao_mpu6000(void)
{
- ao_mpu6000_setup();
+ /* ao_mpu6000_init already grabbed the SPI bus and mutex */
+ _ao_mpu6000_setup();
+#if AO_MPU6000_SPI
+ ao_mpu6000_spi_put();
+#endif
for (;;)
{
- ao_mpu6000_sample(&ao_mpu6000_current);
+#if AO_MPU6000_SPI
+ ao_mpu6000_spi_get();
+#endif
+ _ao_mpu6000_sample(&ao_mpu6000_current);
+#if AO_MPU6000_SPI
+ ao_mpu6000_spi_put();
+#endif
ao_arch_critical(
AO_DATA_PRESENT(AO_DATA_MPU6000);
AO_DATA_WAIT();
@@ -326,5 +374,20 @@ ao_mpu6000_init(void)
ao_mpu6000_configured = 0;
ao_add_task(&ao_mpu6000_task, ao_mpu6000, "mpu6000");
+
+#if AO_MPU6000_SPI
+ ao_spi_init_cs(AO_MPU6000_SPI_CS_PORT, (1 << AO_MPU6000_SPI_CS_PIN));
+
+ /* Pretend to be the mpu6000 task. Grab the SPI bus right away and
+ * hold it for the task so that nothing else uses the SPI bus before
+ * we get the I2C mode disabled in the chip
+ */
+
+ ao_cur_task = &ao_mpu6000_task;
+ ao_spi_get(AO_MPU6000_SPI_BUS, AO_SPI_SPEED_1MHz);
+ ao_cur_task = NULL;
+#endif
+
ao_cmd_register(&ao_mpu6000_cmds[0]);
}
+#endif
diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h
index f01e9e83..dc3a9fbf 100644
--- a/src/drivers/ao_mpu6000.h
+++ b/src/drivers/ao_mpu6000.h
@@ -18,6 +18,10 @@
#ifndef _AO_MPU6000_H_
#define _AO_MPU6000_H_
+#ifndef M_PI
+#define M_PI 3.1415926535897832384626433
+#endif
+
#define MPU6000_ADDR_WRITE 0xd0
#define MPU6000_ADDR_READ 0xd1
@@ -133,13 +137,13 @@
#define MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET 1
#define MPU6000_SIGNAL_PATH_RESET_TEMP_RESET 0
-#define MPU6000_USER_CONTROL 0x6a
-#define MPU6000_USER_CONTROL_FIFO_EN 6
-#define MPU6000_USER_CONTROL_I2C_MST_EN 5
-#define MPU6000_USER_CONTROL_I2C_IF_DIS 4
-#define MPU6000_USER_CONTROL_FIFO_RESET 2
-#define MPU6000_USER_CONTROL_I2C_MST_RESET 1
-#define MPU6000_USER_CONTROL_SIG_COND_RESET 0
+#define MPU6000_USER_CTRL 0x6a
+#define MPU6000_USER_CTRL_FIFO_EN 6
+#define MPU6000_USER_CTRL_I2C_MST_EN 5
+#define MPU6000_USER_CTRL_I2C_IF_DIS 4
+#define MPU6000_USER_CTRL_FIFO_RESET 2
+#define MPU6000_USER_CTRL_I2C_MST_RESET 1
+#define MPU6000_USER_CTRL_SIG_COND_RESET 0
#define MPU6000_PWR_MGMT_1 0x6b
#define MPU6000_PWR_MGMT_1_DEVICE_RESET 7
@@ -166,9 +170,20 @@
/* Self test gyro is approximately 50°/s */
#define MPU6000_ST_GYRO(full_scale) ((int16_t) (((int32_t) 32767 * (int32_t) 50) / (full_scale)))
-#define MPU6000_GYRO_FULLSCALE 2000
+#define MPU6000_GYRO_FULLSCALE ((float) 2000 * M_PI/180.0)
+
+static inline float
+ao_mpu6000_gyro(float sensor) {
+ return sensor * ((float) (MPU6000_GYRO_FULLSCALE / 32767.0));
+}
+
#define MPU6000_ACCEL_FULLSCALE 16
+static inline float
+ao_mpu6000_accel(int16_t sensor) {
+ return (float) sensor * ((float) (MPU6000_ACCEL_FULLSCALE * GRAVITY / 32767.0));
+}
+
struct ao_mpu6000_sample {
int16_t accel_x;
int16_t accel_y;
diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c
index bd57400e..58ab9197 100644
--- a/src/drivers/ao_ms5607.c
+++ b/src/drivers/ao_ms5607.c
@@ -21,19 +21,17 @@
#if HAS_MS5607 || HAS_MS5611
-static struct ao_ms5607_prom ms5607_prom;
-static uint8_t ms5607_configured;
+static __xdata struct ao_ms5607_prom ms5607_prom;
+static __xdata uint8_t ms5607_configured;
static void
ao_ms5607_start(void) {
- ao_spi_get(AO_MS5607_SPI_INDEX,AO_SPI_SPEED_FAST);
- ao_gpio_set(AO_MS5607_CS_PORT, AO_MS5607_CS_PIN, AO_MS5607_CS, 0);
+ ao_spi_get_bit(AO_MS5607_CS_PORT, AO_MS5607_CS_PIN, AO_MS5607_CS, AO_MS5607_SPI_INDEX, AO_SPI_SPEED_FAST);
}
static void
ao_ms5607_stop(void) {
- ao_gpio_set(AO_MS5607_CS_PORT, AO_MS5607_CS_PIN, AO_MS5607_CS, 1);
- ao_spi_put(AO_MS5607_SPI_INDEX);
+ ao_spi_put_bit(AO_MS5607_CS_PORT, AO_MS5607_CS_PIN, AO_MS5607_CS, AO_MS5607_SPI_INDEX);
}
static void
@@ -42,7 +40,7 @@ ao_ms5607_reset(void) {
cmd = AO_MS5607_RESET;
ao_ms5607_start();
- ao_spi_send(&cmd, 1, AO_MS5607_SPI_INDEX);
+ ao_spi_send(DATA_TO_XDATA(&cmd), 1, AO_MS5607_SPI_INDEX);
ao_delay(AO_MS_TO_TICKS(10));
ao_ms5607_stop();
}
@@ -71,17 +69,17 @@ ao_ms5607_crc(uint8_t *prom)
}
static void
-ao_ms5607_prom_read(struct ao_ms5607_prom *prom)
+ao_ms5607_prom_read(__xdata struct ao_ms5607_prom *prom)
{
- uint8_t addr;
- uint8_t crc;
- uint16_t *r;
+ uint8_t addr;
+ uint8_t crc;
+ __xdata uint16_t *r;
- r = (uint16_t *) prom;
+ r = (__xdata uint16_t *) prom;
for (addr = 0; addr < 8; addr++) {
uint8_t cmd = AO_MS5607_PROM_READ(addr);
ao_ms5607_start();
- ao_spi_send(&cmd, 1, AO_MS5607_SPI_INDEX);
+ ao_spi_send(DATA_TO_XDATA(&cmd), 1, AO_MS5607_SPI_INDEX);
ao_spi_recv(r, 2, AO_MS5607_SPI_INDEX);
ao_ms5607_stop();
r++;
@@ -116,25 +114,25 @@ ao_ms5607_setup(void)
ao_ms5607_prom_read(&ms5607_prom);
}
-static volatile uint8_t ao_ms5607_done;
+static __xdata volatile uint8_t ao_ms5607_done;
static void
ao_ms5607_isr(void)
{
ao_exti_disable(AO_MS5607_MISO_PORT, AO_MS5607_MISO_PIN);
ao_ms5607_done = 1;
- ao_wakeup((void *) &ao_ms5607_done);
+ ao_wakeup((__xdata void *) &ao_ms5607_done);
}
static uint32_t
ao_ms5607_get_sample(uint8_t cmd) {
- uint8_t reply[3];
- uint8_t read;
+ __xdata uint8_t reply[3];
+ __xdata uint8_t read;
ao_ms5607_done = 0;
ao_ms5607_start();
- ao_spi_send(&cmd, 1, AO_MS5607_SPI_INDEX);
+ ao_spi_send(DATA_TO_XDATA(&cmd), 1, AO_MS5607_SPI_INDEX);
ao_exti_enable(AO_MS5607_MISO_PORT, AO_MS5607_MISO_PIN);
@@ -142,7 +140,8 @@ ao_ms5607_get_sample(uint8_t cmd) {
ao_spi_put(AO_MS5607_SPI_INDEX);
#endif
ao_arch_block_interrupts();
- while (!ao_ms5607_done)
+ while (!ao_gpio_get(AO_MS5607_MISO_PORT, AO_MS5607_MISO_PIN, AO_MS5607_MISO) &&
+ !ao_ms5607_done)
ao_sleep((void *) &ao_ms5607_done);
ao_arch_release_interrupts();
#if AO_MS5607_PRIVATE_PINS
@@ -175,16 +174,20 @@ ao_ms5607_get_sample(uint8_t cmd) {
#define AO_CONVERT_D2 token_evaluator(AO_MS5607_CONVERT_D2_, AO_MS5607_TEMP_OVERSAMPLE)
void
-ao_ms5607_sample(struct ao_ms5607_sample *sample)
+ao_ms5607_sample(__xdata struct ao_ms5607_sample *sample)
{
sample->pres = ao_ms5607_get_sample(AO_CONVERT_D1);
sample->temp = ao_ms5607_get_sample(AO_CONVERT_D2);
}
+#ifdef _CC1111_H_
+#include "ao_ms5607_convert_8051.c"
+#else
#include "ao_ms5607_convert.c"
+#endif
#if HAS_TASK
-struct ao_ms5607_sample ao_ms5607_current;
+__xdata struct ao_ms5607_sample ao_ms5607_current;
static void
ao_ms5607(void)
@@ -193,10 +196,10 @@ ao_ms5607(void)
for (;;)
{
ao_ms5607_sample(&ao_ms5607_current);
- ao_arch_critical(
- AO_DATA_PRESENT(AO_DATA_MS5607);
- AO_DATA_WAIT();
- );
+ ao_arch_block_interrupts();
+ AO_DATA_PRESENT(AO_DATA_MS5607);
+ AO_DATA_WAIT();
+ ao_arch_release_interrupts();
}
}
@@ -218,11 +221,11 @@ ao_ms5607_info(void)
static void
ao_ms5607_dump(void)
{
- struct ao_ms5607_value value;
+ __xdata struct ao_ms5607_value value;
ao_ms5607_convert(&ao_ms5607_current, &value);
- printf ("Pressure: %8u %8d\n", ao_ms5607_current.pres, value.pres);
- printf ("Temperature: %8u %8d\n", ao_ms5607_current.temp, value.temp);
+ printf ("Pressure: %8lu %8ld\n", ao_ms5607_current.pres, value.pres);
+ printf ("Temperature: %8lu %8ld\n", ao_ms5607_current.temp, value.temp);
printf ("Altitude: %ld\n", ao_pa_to_altitude(value.pres));
}
@@ -249,17 +252,9 @@ ao_ms5607_init(void)
*/
ao_exti_setup(AO_MS5607_MISO_PORT,
AO_MS5607_MISO_PIN,
- AO_EXTI_MODE_RISING,
+ AO_EXTI_MODE_RISING|
+ AO_EXTI_PIN_NOCONFIGURE,
ao_ms5607_isr);
-
-#ifdef STM_MODER_ALTERNATE
- /* Reset the pin from INPUT to ALTERNATE so that SPI works
- * This needs an abstraction at some point...
- */
- stm_moder_set(AO_MS5607_MISO_PORT,
- AO_MS5607_MISO_PIN,
- STM_MODER_ALTERNATE);
-#endif
}
#endif
diff --git a/src/drivers/ao_ms5607.h b/src/drivers/ao_ms5607.h
index b2f98a59..206efd64 100644
--- a/src/drivers/ao_ms5607.h
+++ b/src/drivers/ao_ms5607.h
@@ -56,7 +56,7 @@ struct ao_ms5607_value {
int32_t temp; /* in °C * 100 */
};
-extern struct ao_ms5607_sample ao_ms5607_current;
+extern __xdata struct ao_ms5607_sample ao_ms5607_current;
void
ao_ms5607_setup(void);
@@ -68,12 +68,13 @@ void
ao_ms5607_info(void);
void
-ao_ms5607_sample(struct ao_ms5607_sample *sample);
+ao_ms5607_sample(__xdata struct ao_ms5607_sample *sample);
void
-ao_ms5607_convert(struct ao_ms5607_sample *sample, struct ao_ms5607_value *value);
+ao_ms5607_convert(__xdata struct ao_ms5607_sample *sample,
+ __xdata struct ao_ms5607_value *value);
void
-ao_ms5607_get_prom(struct ao_ms5607_prom *prom);
+ao_ms5607_get_prom(__data struct ao_ms5607_prom *prom);
#endif /* _AO_MS5607_H_ */
diff --git a/src/drivers/ao_ms5607_convert.c b/src/drivers/ao_ms5607_convert.c
index e61d19ed..bfb952a4 100644
--- a/src/drivers/ao_ms5607_convert.c
+++ b/src/drivers/ao_ms5607_convert.c
@@ -42,11 +42,14 @@ ao_ms5607_convert(struct ao_ms5607_sample *sample, struct ao_ms5607_value *value
int32_t TEMPM = TEMP - 2000;
int64_t OFF2 = (61 * (int64_t) TEMPM * (int64_t) TEMPM) >> 4;
int64_t SENS2 = 2 * (int64_t) TEMPM * (int64_t) TEMPM;
- if (TEMP < 1500) {
+ if (TEMP < -1500) {
int32_t TEMPP = TEMP + 1500;
- int64_t TEMPP2 = TEMPP * TEMPP;
- OFF2 = OFF2 + 15 * TEMPP2;
- SENS2 = SENS2 + 8 * TEMPP2;
+ /* You'd think this would need a 64-bit int, but
+ * that would imply a temperature below -327.67°C...
+ */
+ int32_t TEMPP2 = TEMPP * TEMPP;
+ OFF2 = OFF2 + (int64_t) 15 * TEMPP2;
+ SENS2 = SENS2 + (int64_t) 8 * TEMPP2;
}
TEMP -= T2;
OFF -= OFF2;
diff --git a/src/drivers/ao_ms5607_convert_8051.c b/src/drivers/ao_ms5607_convert_8051.c
new file mode 100644
index 00000000..f3a48c46
--- /dev/null
+++ b/src/drivers/ao_ms5607_convert_8051.c
@@ -0,0 +1,136 @@
+/*
+ * Copyright © 2012 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_ms5607.h>
+#include <ao_int64.h>
+
+#if HAS_MS5611
+#define SHIFT_OFF 16
+#define SHIFT_TCO 7
+#define SHIFT_SENS 15
+#define SHFIT_TCS 8
+#else
+#define SHIFT_OFF 17
+#define SHIFT_TCO 6
+#define SHIFT_SENS 16
+#define SHIFT_TCS 7
+#endif
+
+void
+ao_ms5607_convert(__xdata struct ao_ms5607_sample *sample,
+ __xdata struct ao_ms5607_value *value)
+{
+ __LOCAL int32_t dT;
+ __LOCAL int32_t TEMP;
+ __LOCAL ao_int64_t OFF;
+ __LOCAL ao_int64_t SENS;
+ __LOCAL ao_int64_t a;
+
+ dT = sample->temp - ((int32_t) ms5607_prom.tref << 8);
+
+ /* TEMP = 2000 + (((int64_t) dT * ms5607_prom.tempsens) >> 23); */
+ ao_mul64_32_32(&a, dT, ms5607_prom.tempsens);
+ ao_rshift64(&a, &a, 23);
+ TEMP = 2000 + a.low;
+ /* */
+
+ /* OFF = ((int64_t) ms5607_prom.off << SHIFT_OFF) + (((int64_t) ms5607_prom.tco * dT) >> SHIFT_TCO);*/
+#if SHIFT_OFF > 16
+ OFF.high = ms5607_prom.off >> (32 - SHIFT_OFF);
+#else
+ OFF.high = 0;
+#endif
+ OFF.low = (uint32_t) ms5607_prom.off << SHIFT_OFF;
+ ao_mul64_32_32(&a, ms5607_prom.tco, dT);
+ ao_rshift64(&a, &a, SHIFT_TCO);
+ ao_plus64(&OFF, &OFF, &a);
+ /**/
+
+ /* SENS = ((int64_t) ms5607_prom.sens << SHIFT_SENS) + (((int64_t) ms5607_prom.tcs * dT) >> SHIFT_TCS); */
+ SENS.high = 0;
+ SENS.low = (uint32_t) ms5607_prom.sens << SHIFT_SENS;
+ ao_mul64_32_32(&a, ms5607_prom.tcs, dT);
+ ao_rshift64(&a, &a, SHIFT_TCS);
+ ao_plus64(&SENS, &SENS, &a);
+ /**/
+
+ if (TEMP < 2000) {
+ __LOCAL int32_t T2;
+ __LOCAL int32_t TEMPM;
+ __LOCAL ao_int64_t OFF2;
+ __LOCAL ao_int64_t SENS2;
+
+ /* T2 = ((int64_t) dT * (int64_t) dT) >> 31; */
+ ao_mul64_32_32(&a, dT, dT);
+ T2 = (a.low >> 31) | (a.high << 1);
+ /**/
+
+ TEMPM = TEMP - 2000;
+
+ /* OFF2 = (61 * (int64_t) TEMPM * (int64_t) TEMPM) >> 4; */
+ ao_mul64_32_32(&OFF2, TEMPM, TEMPM);
+ ao_mul64_64_16(&OFF2, &OFF2, 61);
+ ao_rshift64(&OFF2, &OFF2, 4);
+ /**/
+
+ /* SENS2 = 2 * (int64_t) TEMPM * (int64_t) TEMPM; */
+ ao_mul64_32_32(&SENS2, TEMPM, TEMPM);
+ ao_lshift64(&SENS2, &SENS2, 1);
+ /**/
+
+ if (TEMP < -1500) {
+ int32_t TEMPP;
+ int32_t TEMPP2;
+
+ TEMPP = TEMP + 1500;
+ TEMPP2 = TEMPP * TEMPP;
+
+ /* OFF2 = OFF2 + 15 * TEMPP2; */
+ ao_mul64_32_32(&a, 15, TEMPP2);
+ ao_plus64(&OFF2, &OFF2, &a);
+ /**/
+
+ /* SENS2 = SENS2 + 8 * TEMPP2; */
+ a.high = 0;
+ a.low = TEMPP2;
+ ao_lshift64(&a, &a, 3);
+ ao_plus64(&SENS2, &SENS2, &a);
+ /**/
+ }
+ TEMP -= T2;
+
+ /* OFF -= OFF2; */
+ ao_minus64(&OFF, &OFF, &OFF2);
+ /**/
+
+ /* SENS -= SENS2; */
+ ao_minus64(&SENS, &SENS, &SENS2);
+ /**/
+ }
+
+ /* value->pres = ((((int64_t) sample->pres * SENS) >> 21) - OFF) >> 15; */
+ a.high = 0;
+ a.low = sample->pres;
+ ao_mul64(&a, &a, &SENS);
+ ao_rshift64(&a, &a, 21);
+ ao_minus64(&a, &a, &OFF);
+ ao_rshift64(&a, &a, 15);
+ value->pres = a.low;
+ /**/
+
+ value->temp = TEMP;
+}
diff --git a/src/drivers/ao_pad.c b/src/drivers/ao_pad.c
index 6cec98ab..62ae68e9 100644
--- a/src/drivers/ao_pad.c
+++ b/src/drivers/ao_pad.c
@@ -17,7 +17,7 @@
#include <ao.h>
#include <ao_pad.h>
-#include <ao_74hc497.h>
+#include <ao_74hc165.h>
#include <ao_radio_cmac.h>
static __xdata uint8_t ao_pad_ignite;
@@ -27,6 +27,7 @@ static __pdata uint8_t ao_pad_armed;
static __pdata uint16_t ao_pad_arm_time;
static __pdata uint8_t ao_pad_box;
static __xdata uint8_t ao_pad_disabled;
+static __pdata uint16_t ao_pad_packet_time;
#define DEBUG 1
@@ -135,6 +136,12 @@ ao_pad_monitor(void)
query.arm_status = AO_PAD_ARM_STATUS_UNKNOWN;
arm_beep_time = 0;
}
+ if ((ao_time() - ao_pad_packet_time) > AO_SEC_TO_TICKS(2))
+ cur |= AO_LED_RED;
+ else if (ao_radio_cmac_rssi < -90)
+ cur |= AO_LED_AMBER;
+ else
+ cur |= AO_LED_GREEN;
for (c = 0; c < AO_PAD_NUM; c++) {
int16_t sense = packet->adc.sense[c];
@@ -171,9 +178,10 @@ ao_pad_monitor(void)
query.igniter_status[c] = status;
}
if (cur != prev) {
- PRINTD("change leds from %02x to %02x mask %02x\n",
- prev, cur, AO_LED_CONTINUITY_MASK|AO_LED_ARMED);
- ao_led_set_mask(cur, AO_LED_CONTINUITY_MASK | AO_LED_ARMED);
+ PRINTD("change leds from %02x to %02x\n",
+ prev, cur);
+ FLUSHD();
+ ao_led_set(cur);
prev = cur;
}
@@ -182,10 +190,7 @@ ao_pad_monitor(void)
if (ao_pad_armed) {
ao_strobe(1);
- if (sample & 2)
- ao_siren(1);
- else
- ao_siren(0);
+ ao_siren(1);
beeping = 1;
} else if (query.arm_status == AO_PAD_ARM_STATUS_ARMED && !beeping) {
if (arm_beep_time == 0) {
@@ -218,6 +223,21 @@ ao_pad_enable(void)
ao_wakeup (&ao_pad_disabled);
}
+#if HAS_74HC165
+static uint8_t
+ao_pad_read_box(void)
+{
+ uint8_t byte = ao_74hc165_read();
+ uint8_t h, l;
+
+ h = byte >> 4;
+ l = byte & 0xf;
+ return h * 10 + l;
+}
+#else
+#define ao_pad_read_box() 0
+#endif
+
static void
ao_pad(void)
{
@@ -226,18 +246,20 @@ ao_pad(void)
ao_pad_box = 0;
ao_led_set(0);
- ao_led_on(AO_LED_POWER);
for (;;) {
FLUSHD();
while (ao_pad_disabled)
ao_sleep(&ao_pad_disabled);
ret = ao_radio_cmac_recv(&command, sizeof (command), 0);
- PRINTD ("cmac_recv %d\n", ret);
+ PRINTD ("cmac_recv %d %d\n", ret, ao_radio_cmac_rssi);
if (ret != AO_RADIO_CMAC_OK)
continue;
+ ao_pad_packet_time = ao_time();
- PRINTD ("tick %d box %d cmd %d channels %02x\n",
- command.tick, command.box, command.cmd, command.channels);
+ ao_pad_box = ao_pad_read_box();
+
+ PRINTD ("tick %d box %d (me %d) cmd %d channels %02x\n",
+ command.tick, command.box, ao_pad_box, command.cmd, command.channels);
switch (command.cmd) {
case AO_LAUNCH_ARM:
@@ -327,7 +349,7 @@ ao_pad_test(void)
}
for (c = 0; c < AO_PAD_NUM; c++) {
- printf ("Pad %d: ");
+ printf ("Pad %d: ", c);
switch (query.igniter_status[c]) {
case AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_CLOSED: printf ("No igniter. Relay closed\n"); break;
case AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN: printf ("No igniter. Relay open\n"); break;
@@ -362,6 +384,26 @@ ao_pad_set_debug(void)
if (ao_cmd_status == ao_cmd_success)
ao_pad_debug = ao_cmd_lex_i != 0;
}
+
+
+static void
+ao_pad_alarm_debug(void)
+{
+ uint8_t which, value;
+ ao_cmd_decimal();
+ if (ao_cmd_status != ao_cmd_success)
+ return;
+ which = ao_cmd_lex_i;
+ ao_cmd_decimal();
+ if (ao_cmd_status != ao_cmd_success)
+ return;
+ value = ao_cmd_lex_i;
+ printf ("Set %s to %d\n", which ? "siren" : "strobe", value);
+ if (which)
+ ao_siren(value);
+ else
+ ao_strobe(value);
+}
#endif
__code struct ao_cmds ao_pad_cmds[] = {
@@ -369,6 +411,7 @@ __code struct ao_cmds ao_pad_cmds[] = {
{ ao_pad_manual, "i <key> <n>\0Fire igniter. <key> is doit with D&I" },
#if DEBUG
{ ao_pad_set_debug, "D <0 off, 1 on>\0Debug" },
+ { ao_pad_alarm_debug, "S <0 strobe, 1 siren> <0 off, 1 on>\0Set alarm output" },
#endif
{ 0, NULL }
};
diff --git a/src/drivers/ao_pca9922.c b/src/drivers/ao_pca9922.c
index 6d8d18d8..d376b968 100644
--- a/src/drivers/ao_pca9922.c
+++ b/src/drivers/ao_pca9922.c
@@ -30,9 +30,12 @@ ao_led_apply(void)
/* Don't try the SPI bus during initialization */
if (!ao_cur_task)
return;
- ao_spi_get_bit(AO_PCA9922_CS_PORT, AO_PCA9922_CS_PIN, AO_PCA9922_CS, AO_PCA9922_SPI_BUS, AO_SPI_SPEED_FAST);
+ ao_spi_get(AO_PCA9922_SPI_BUS);
+ ao_spi_set_speed(AO_PCA9922_SPI_BUS,AO_SPI_SPEED_FAST);
+ AO_PCA9922_CS = 1;
ao_spi_send(&ao_led_state, 1, AO_PCA9922_SPI_BUS);
- ao_spi_put_bit(AO_PCA9922_CS_PORT, AO_PCA9922_CS_PIN, AO_PCA9922_CS, AO_PCA9922_SPI_BUS);
+ AO_PCA9922_CS = 0;
+ ao_spi_put(AO_PCA9922_SPI_BUS);
}
void
diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c
index 6cc2467a..d07488d0 100644
--- a/src/drivers/ao_quadrature.c
+++ b/src/drivers/ao_quadrature.c
@@ -18,16 +18,12 @@
#include <ao.h>
#include <ao_quadrature.h>
#include <ao_exti.h>
-#if AO_EVENT
+#include <ao_fast_timer.h>
#include <ao_event.h>
-#define ao_quadrature_queue(q) ao_event_put_isr(AO_EVENT_QUADRATURE, q, ao_quadrature_count[q])
-#else
-#define ao_quadrature_queue(q)
-#endif
__xdata int32_t ao_quadrature_count[AO_QUADRATURE_COUNT];
-
static uint8_t ao_quadrature_state[AO_QUADRATURE_COUNT];
+static int8_t ao_quadrature_raw[AO_QUADRATURE_COUNT];
#define BIT(a,b) ((a) | ((b) << 1))
#define STATE(old_a, old_b, new_a, new_b) (((BIT(old_a, old_b) << 2) | BIT(new_a, new_b)))
@@ -35,41 +31,76 @@ static uint8_t ao_quadrature_state[AO_QUADRATURE_COUNT];
#define port(q) AO_QUADRATURE_ ## q ## _PORT
#define bita(q) AO_QUADRATURE_ ## q ## _A
#define bitb(q) AO_QUADRATURE_ ## q ## _B
+#define pina(q) AO_QUADRATURE_ ## q ## _A ## _PIN
+#define pinb(q) AO_QUADRATURE_ ## q ## _B ## _PIN
+#define isr(q) ao_quadrature_isr_ ## q
-#define ao_quadrature_update(q) do { \
- ao_quadrature_state[q] = ((ao_quadrature_state[q] & 3) << 2); \
- ao_quadrature_state[q] |= ao_gpio_get(port(q), bita(q), 0); \
- ao_quadrature_state[q] |= ao_gpio_get(port(q), bitb(q), 0) << 1; \
- } while (0)
-
+static inline uint16_t
+ao_quadrature_read(struct stm_gpio *gpio, uint8_t pin_a, uint8_t pin_b) {
+ uint16_t v = stm_gpio_get_all(gpio);
+
+ return ~((((v >> pin_a) & 1) | (((v >> pin_b) & 1) << 1))) & 3;
+}
+
+#define _ao_quadrature_get(q) ao_quadrature_read(port(q), bita(q), bitb(q))
static void
-ao_quadrature_isr(void)
+_ao_quadrature_queue(uint8_t q, int8_t step)
{
- uint8_t q;
-#if AO_QUADRATURE_COUNT > 0
- ao_quadrature_update(0);
-#endif
-#if AO_QUADRATURE_COUNT > 1
- ao_quadrature_update(1);
+ ao_quadrature_count[q] += step;
+#if AO_EVENT
+ ao_event_put_isr(AO_EVENT_QUADRATURE, q, step);
#endif
+ ao_wakeup(&ao_quadrature_count[q]);
+}
- for (q = 0; q < AO_QUADRATURE_COUNT; q++) {
- switch (ao_quadrature_state[q]) {
- case STATE(0, 1, 0, 0):
- ao_quadrature_count[q]++;
- break;
- case STATE(1, 0, 0, 0):
- ao_quadrature_count[q]--;
- break;
- default:
- continue;
- }
- ao_quadrature_queue(q);
- ao_wakeup(&ao_quadrature_count[q]);
+static const int8_t step[16] = {
+ [STATE(0,0,0,0)] = 0,
+ [STATE(0,0,0,1)] = -1,
+ [STATE(0,0,1,0)] = 1,
+ [STATE(0,0,1,1)] = 0,
+ [STATE(0,1,0,0)] = 1,
+ [STATE(0,1,1,0)] = 0,
+ [STATE(0,1,1,1)] = -1,
+ [STATE(1,0,0,0)] = -1,
+ [STATE(1,0,0,1)] = 0,
+ [STATE(1,0,1,0)] = 0,
+ [STATE(1,0,1,1)] = 1,
+ [STATE(1,1,0,0)] = 0,
+ [STATE(1,1,0,1)] = 1,
+ [STATE(1,1,1,0)] = -1,
+ [STATE(1,1,1,1)] = 0
+};
+
+static void
+_ao_quadrature_set(uint8_t q, uint8_t value) {
+ uint8_t v;
+
+ v = ao_quadrature_state[q] & 3;
+ value = value & 3;
+
+ if (v == value)
+ return;
+
+ ao_quadrature_state[q] = (v << 2) | value;
+
+ ao_quadrature_raw[q] += step[ao_quadrature_state[q]];
+ if (value == 0) {
+ if (ao_quadrature_raw[q] == 4)
+ _ao_quadrature_queue(q, 1);
+ else if (ao_quadrature_raw[q] == -4)
+ _ao_quadrature_queue(q, -1);
+ ao_quadrature_raw[q] = 0;
}
}
+static void
+ao_quadrature_isr(void)
+{
+ _ao_quadrature_set(0, _ao_quadrature_get(0));
+ _ao_quadrature_set(1, _ao_quadrature_get(1));
+}
+
int32_t
ao_quadrature_poll(uint8_t q)
{
@@ -92,6 +123,15 @@ ao_quadrature_test(void)
ao_cmd_decimal();
q = ao_cmd_lex_i;
+ if (q >= AO_QUADRATURE_COUNT) {
+ ao_cmd_status = ao_cmd_syntax_error;
+ return;
+ }
+ printf ("count %d state %x raw %d\n",
+ ao_quadrature_count[q],
+ ao_quadrature_state[q],
+ ao_quadrature_raw[q]);
+#if 0
for (;;) {
int32_t c;
flush();
@@ -100,6 +140,7 @@ ao_quadrature_test(void)
if (c == 100)
break;
}
+#endif
}
static const struct ao_cmds ao_quadrature_cmds[] = {
@@ -107,18 +148,9 @@ static const struct ao_cmds ao_quadrature_cmds[] = {
{ 0, NULL }
};
-#define init(q) do { \
- ao_enable_port(port(q)); \
- \
- ao_exti_setup(port(q), bita(q), \
- AO_QUADRATURE_MODE|AO_EXTI_MODE_FALLING|AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_MED, \
- ao_quadrature_isr); \
- ao_exti_enable(port(q), bita(q)); \
- \
- ao_exti_setup(port(q), bitb(q), \
- AO_QUADRATURE_MODE|AO_EXTI_MODE_FALLING|AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_MED, \
- ao_quadrature_isr); \
- ao_exti_enable(port(q), bitb(q)); \
+#define init(q) do { \
+ ao_enable_input(port(q), bita(q), 0); \
+ ao_enable_input(port(q), bitb(q), 0); \
} while (0)
void
@@ -130,5 +162,7 @@ ao_quadrature_init(void)
#if AO_QUADRATURE_COUNT > 1
init(1);
#endif
+ ao_fast_timer_init();
+ ao_fast_timer_on(ao_quadrature_isr);
ao_cmd_register(&ao_quadrature_cmds[0]);
}
diff --git a/src/drivers/ublox-csum.5c b/src/drivers/ublox-csum.5c
new file mode 100644
index 00000000..4e0c7c5a
--- /dev/null
+++ b/src/drivers/ublox-csum.5c
@@ -0,0 +1,23 @@
+#!/usr/bin/nickle
+string[] speeds = { "57600", "19200", "9600" };
+
+string make_set_nmea(string speed) {
+ return sprintf ("PUBX,41,1,3,1,%s,0", speed);
+}
+
+int csum(string x) {
+ int csum = 0;
+ for (int i = 0; i < String::length(x); i++)
+ csum ^= x[i];
+ return csum;
+}
+
+for (int i = 0; i < dim(speeds); i++) {
+ string s = make_set_nmea(speeds[i]);
+ int c = csum(s);
+ printf ("/* $%s* */\n", s);
+ printf ("#define SERIAL_SPEED_STRING \"%s\"\n", speeds[i]);
+ printf ("#define SERIAL_SPEED_CHECKSUM \"%02x\"\n", c);
+}
+
+
generated by cgit v1.2.3 (git 2.39.1) at 2025-12-25 18:04:52 +0000