summaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-rw-r--r--src/drivers/ao_aprs.c113
-rw-r--r--src/drivers/ao_button.c1
-rw-r--r--src/drivers/ao_cc1120.c14
-rw-r--r--src/drivers/ao_cc115l.c27
-rw-r--r--src/drivers/ao_companion.c4
-rw-r--r--src/drivers/ao_event.c2
-rw-r--r--src/drivers/ao_event.h2
-rw-r--r--src/drivers/ao_hmc5883.c1
-rw-r--r--src/drivers/ao_lco_cmd.c1
-rw-r--r--src/drivers/ao_lco_func.c1
-rw-r--r--src/drivers/ao_mma655x.c124
-rw-r--r--src/drivers/ao_mma655x.h8
-rw-r--r--src/drivers/ao_mpu6000.c17
13 files changed, 178 insertions, 137 deletions
diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c
index 6ab61e6a..56d98437 100644
--- a/src/drivers/ao_aprs.c
+++ b/src/drivers/ao_aprs.c
@@ -144,6 +144,7 @@
#endif
#include <ao_aprs.h>
+#include <math.h>
// Public methods, constants, and data structures for each class.
@@ -254,9 +255,9 @@ typedef enum
/// AX.25 compliant packet header that contains destination, station call sign, and path.
/// 0x76 for SSID-11, 0x78 for SSID-12
static uint8_t TNC_AX25_HEADER[] = {
- 'A' << 1, 'P' << 1, 'A' << 1, 'M' << 1, ' ' << 1, ' ' << 1, 0x60, \
- 'N' << 1, '0' << 1, 'C' << 1, 'A' << 1, 'L' << 1, 'L' << 1, 0x78, \
- 'W' << 1, 'I' << 1, 'D' << 1, 'E' << 1, '2' << 1, ' ' << 1, 0x65, \
+ 'A' << 1, 'P' << 1, 'A' << 1, 'M' << 1, ' ' << 1, ' ' << 1, 0x60,
+ 'N' << 1, '0' << 1, 'C' << 1, 'A' << 1, 'L' << 1, 'L' << 1, 0x78,
+ 'W' << 1, 'I' << 1, 'D' << 1, 'E' << 1, '2' << 1, ' ' << 1, 0x65,
0x03, 0xf0 };
#define TNC_CALLSIGN_OFF 7
@@ -479,6 +480,36 @@ static void tnc1200TimerTick()
} // END switch
}
+static void tncCompressInt(uint8_t *dest, int32_t value, int len) {
+ int i;
+ for (i = len - 1; i >= 0; i--) {
+ dest[i] = value % 91 + 33;
+ value /= 91;
+ }
+}
+
+#if HAS_ADC
+static int tncComment(uint8_t *buf)
+{
+ struct ao_data packet;
+
+ ao_arch_critical(ao_data_get(&packet););
+
+ int16_t battery = ao_battery_decivolt(packet.adc.v_batt);
+ int16_t apogee = ao_ignite_decivolt(AO_SENSE_DROGUE(&packet));
+ int16_t main = ao_ignite_decivolt(AO_SENSE_MAIN(&packet));
+
+ return sprintf((char *) buf,
+ "B:%d.%d A:%d.%d M:%d.%d",
+ battery/10,
+ battery % 10,
+ apogee/10,
+ apogee%10,
+ main/10,
+ main%10);
+}
+#endif
+
/**
* Generate the plain text position packet.
*/
@@ -487,57 +518,45 @@ static int tncPositionPacket(void)
int32_t latitude = ao_gps_data.latitude;
int32_t longitude = ao_gps_data.longitude;
int32_t altitude = ao_gps_data.altitude;
+ uint8_t *buf;
+
+ if (altitude < 0)
+ altitude = 0;
+ altitude = (altitude * (int32_t) 10000 + (3048/2)) / (int32_t) 3048;
+
+ buf = tncBuffer;
+ *buf++ = '!';
- uint16_t lat_deg;
- uint16_t lon_deg;
- uint16_t lat_min;
- uint16_t lat_frac;
- uint16_t lon_min;
- uint16_t lon_frac;
+ /* Symbol table ID */
+ *buf++ = '/';
- char lat_sign = 'N', lon_sign = 'E';
+ latitude = ((uint64_t) 380926 * (900000000 - latitude)) / 10000000;
+ longitude = ((uint64_t) 190463 * (1800000000 + longitude)) / 10000000;
- if (latitude < 0) {
- lat_sign = 'S';
- latitude = -latitude;
- }
+#define ALTITUDE_LOG_BASE 0.001998002662673f /* log(1.002) */
- if (longitude < 0) {
- lon_sign = 'W';
- longitude = -longitude;
- }
+ altitude = logf((float) altitude) * (1/ALTITUDE_LOG_BASE);
- /* Round latitude and longitude by 0.005 minutes */
- latitude = latitude + 833;
- if (latitude > 900000000)
- latitude = 900000000;
- longitude = longitude + 833;
- if (longitude > 1800000000)
- longitude = 1800000000;
-
- lat_deg = latitude / 10000000;
- latitude -= lat_deg * 10000000;
- latitude *= 60;
- lat_min = latitude / 10000000;
- latitude -= lat_min * 10000000;
- lat_frac = latitude / 100000;
-
- lon_deg = longitude / 10000000;
- longitude -= lon_deg * 10000000;
- longitude *= 60;
- lon_min = longitude / 10000000;
- longitude -= lon_min * 10000000;
- lon_frac = longitude / 100000;
+ tncCompressInt(buf, latitude, 4);
+ buf += 4;
+ tncCompressInt(buf, longitude, 4);
+ buf += 4;
- if (altitude < 0)
- altitude = 0;
+ /* Symbol code */
+ *buf++ = '\'';
- altitude = (altitude * (int32_t) 10000 + (3048/2)) / (int32_t) 3048;
-
- return sprintf ((char *) tncBuffer, "=%02u%02u.%02u%c\\%03u%02u.%02u%cO /A=%06u\015",
- lat_deg, lat_min, lat_frac, lat_sign,
- lon_deg, lon_min, lon_frac, lon_sign,
- altitude);
+ tncCompressInt(buf, altitude, 2);
+ buf += 2;
+
+ *buf++ = 33 + ((1 << 5) | (2 << 3));
+
+#if HAS_ADC
+ buf += tncComment(buf);
+#else
+ *buf = '\0';
+#endif
+
+ return buf - tncBuffer;
}
static int16_t
diff --git a/src/drivers/ao_button.c b/src/drivers/ao_button.c
index 25c0cd5c..cdf07352 100644
--- a/src/drivers/ao_button.c
+++ b/src/drivers/ao_button.c
@@ -59,6 +59,7 @@ _ao_button_get(struct ao_debounce *debounce)
case 4: return ao_button_value(4);
#endif
}
+ return 0;
}
static void
diff --git a/src/drivers/ao_cc1120.c b/src/drivers/ao_cc1120.c
index 37d04927..31225939 100644
--- a/src/drivers/ao_cc1120.c
+++ b/src/drivers/ao_cc1120.c
@@ -152,6 +152,7 @@ ao_radio_strobe(uint8_t addr)
return in;
}
+#if 0
static uint8_t
ao_radio_fifo_read(uint8_t *data, uint8_t len)
{
@@ -166,6 +167,7 @@ ao_radio_fifo_read(uint8_t *data, uint8_t len)
ao_radio_deselect();
return status;
}
+#endif
static uint8_t
ao_radio_fifo_write_start(void)
@@ -207,11 +209,13 @@ ao_radio_tx_fifo_space(void)
return CC1120_FIFO_SIZE - ao_radio_reg_read(CC1120_NUM_TXBYTES);
}
+#if 0
static uint8_t
ao_radio_status(void)
{
return ao_radio_strobe (CC1120_SNOP);
}
+#endif
void
ao_radio_recv_abort(void)
@@ -505,7 +509,7 @@ static void
ao_radio_set_mode(uint16_t new_mode)
{
uint16_t changes;
- int i;
+ unsigned int i;
if (new_mode == ao_radio_mode)
return;
@@ -559,7 +563,7 @@ static uint8_t ao_radio_configured = 0;
static void
ao_radio_setup(void)
{
- int i;
+ unsigned int i;
ao_radio_strobe(CC1120_SRES);
@@ -751,13 +755,11 @@ static uint8_t tx_data[(AO_RADIO_MAX_SEND + 4) * 2];
void
ao_radio_send(const void *d, uint8_t size)
{
- uint8_t marc_status;
uint8_t *e = tx_data;
uint8_t encode_len;
uint8_t this_len;
uint8_t started = 0;
uint8_t fifo_space;
- uint8_t q;
encode_len = ao_fec_encode(d, size, tx_data);
@@ -948,11 +950,9 @@ uint8_t
ao_radio_recv(__xdata void *d, uint8_t size, uint8_t timeout)
{
uint8_t len;
- uint16_t i;
uint8_t radio_rssi = 0;
uint8_t rssi0;
uint8_t ret;
- static int been_here = 0;
size -= 2; /* status bytes */
if (size > AO_RADIO_MAX_RECV) {
@@ -1334,8 +1334,6 @@ static const struct ao_cmds ao_radio_cmds[] = {
void
ao_radio_init(void)
{
- int i;
-
ao_radio_configured = 0;
ao_spi_init_cs (AO_CC1120_SPI_CS_PORT, (1 << AO_CC1120_SPI_CS_PIN));
diff --git a/src/drivers/ao_cc115l.c b/src/drivers/ao_cc115l.c
index 0fa1e899..f0f72d4d 100644
--- a/src/drivers/ao_cc115l.c
+++ b/src/drivers/ao_cc115l.c
@@ -29,8 +29,6 @@ static uint8_t ao_radio_fifo; /* fifo drained interrupt received */
static uint8_t ao_radio_done; /* tx done interrupt received */
static uint8_t ao_radio_wake; /* sleep address for radio interrupts */
static uint8_t ao_radio_abort; /* radio operation should abort */
-static uint8_t ao_radio_mcu_wake; /* MARC status change */
-static uint8_t ao_radio_marcstate; /* Last read MARC state value */
/* Debugging commands */
#define CC115L_DEBUG 0
@@ -106,7 +104,6 @@ static uint8_t
ao_radio_reg_read(uint8_t addr)
{
uint8_t data[1];
- uint8_t d;
data[0] = ((1 << CC115L_READ) |
(0 << CC115L_BURST) |
@@ -123,7 +120,6 @@ static void
ao_radio_reg_write(uint8_t addr, uint8_t value)
{
uint8_t data[2];
- uint8_t d;
trace_add(trace_write, addr, value, NULL);
data[0] = ((0 << CC115L_READ) |
@@ -135,11 +131,11 @@ ao_radio_reg_write(uint8_t addr, uint8_t value)
ao_radio_deselect();
}
+#if UNUSED
static void
ao_radio_burst_read_start (uint16_t addr)
{
uint8_t data[1];
- uint8_t d;
data[0] = ((1 << CC115L_READ) |
(1 << CC115L_BURST) |
@@ -153,6 +149,7 @@ ao_radio_burst_read_stop (void)
{
ao_radio_deselect();
}
+#endif
static uint8_t
@@ -200,19 +197,23 @@ ao_radio_tx_fifo_space(void)
return CC115L_FIFO_SIZE - (ao_radio_reg_read(CC115L_TXBYTES) & CC115L_TXBYTES_NUM_TX_BYTES_MASK);
}
+#if UNUSED
static uint8_t
ao_radio_status(void)
{
return ao_radio_strobe (CC115L_SNOP);
}
+#endif
#define ao_radio_rdf_value 0x55
+#if UNUSED
static uint8_t
ao_radio_get_marcstate(void)
{
return ao_radio_reg_read(CC115L_MARCSTATE) & CC115L_MARCSTATE_MASK;
}
+#endif
static void
ao_radio_done_isr(void)
@@ -233,11 +234,6 @@ ao_radio_fifo_isr(void)
}
static void
-ao_radio_start_tx(void)
-{
-}
-
-static void
ao_radio_idle(void)
{
ao_radio_pa_off();
@@ -401,7 +397,7 @@ static void
ao_radio_set_mode(uint16_t new_mode)
{
uint16_t changes;
- int i;
+ unsigned int i;
if (new_mode == ao_radio_mode)
return;
@@ -466,7 +462,7 @@ static uint8_t ao_radio_configured = 0;
static void
ao_radio_setup(void)
{
- int i;
+ unsigned int i;
ao_radio_strobe(CC115L_SRES);
ao_delay(AO_MS_TO_TICKS(10));
@@ -568,6 +564,7 @@ ao_radio_tone_run(struct ao_radio_tone *tones, int ntones)
ao_radio_tone = tones;
ao_radio_tone_current = 0;
ao_radio_tone_offset = 0;
+ ao_radio_tone_count = ntones;
_ao_radio_send_lots(ao_radio_tone_fill, AO_RADIO_MODE_RDF);
ao_radio_put();
}
@@ -730,8 +727,6 @@ ao_radio_send_fill(uint8_t *buf, int16_t len)
void
ao_radio_send(const void *d, uint8_t size)
{
- int i;
-
ao_radio_get();
ao_radio_send_len = ao_fec_encode(d, size, tx_data);
ao_radio_send_buf = tx_data;
@@ -912,7 +907,6 @@ static void ao_radio_packet(void) {
ao_radio_send(packet, sizeof (packet));
}
-#endif /* CC115L_DEBUG */
#if HAS_APRS
#include <ao_aprs.h>
@@ -926,6 +920,7 @@ ao_radio_aprs()
ao_aprs_send();
}
#endif
+#endif /* CC115L_DEBUG */
static const struct ao_cmds ao_radio_cmds[] = {
{ ao_radio_test_cmd, "C <1 start, 0 stop, none both>\0Radio carrier test" },
@@ -943,7 +938,9 @@ static const struct ao_cmds ao_radio_cmds[] = {
void
ao_radio_init(void)
{
+#if 0
int i;
+#endif
ao_radio_configured = 0;
ao_spi_init_cs (AO_CC115L_SPI_CS_PORT, (1 << AO_CC115L_SPI_CS_PIN));
diff --git a/src/drivers/ao_companion.c b/src/drivers/ao_companion.c
index 0f405253..570b9e40 100644
--- a/src/drivers/ao_companion.c
+++ b/src/drivers/ao_companion.c
@@ -67,8 +67,8 @@ ao_companion_get_setup(void)
ao_companion_send_command(AO_COMPANION_SETUP);
ao_spi_recv(&ao_companion_setup, sizeof (ao_companion_setup), AO_COMPANION_SPI_BUS);
COMPANION_DESELECT();
- return (ao_companion_setup.board_id ==
- (uint16_t) ~ao_companion_setup.board_id_inverse);
+ return ((int16_t) ao_companion_setup.board_id ==
+ (int16_t) (uint16_t) (~ao_companion_setup.board_id_inverse));
}
static void
diff --git a/src/drivers/ao_event.c b/src/drivers/ao_event.c
index c428125d..5c0d2863 100644
--- a/src/drivers/ao_event.c
+++ b/src/drivers/ao_event.c
@@ -30,7 +30,7 @@ uint8_t ao_event_queue_insert;
uint8_t ao_event_queue_remove;
-uint8_t
+void
ao_event_get(struct ao_event *ev)
{
ao_arch_critical(
diff --git a/src/drivers/ao_event.h b/src/drivers/ao_event.h
index ed9a7433..584a845a 100644
--- a/src/drivers/ao_event.h
+++ b/src/drivers/ao_event.h
@@ -29,7 +29,7 @@ struct ao_event {
int32_t value;
};
-uint8_t
+void
ao_event_get(struct ao_event *ev);
void
diff --git a/src/drivers/ao_hmc5883.c b/src/drivers/ao_hmc5883.c
index 782d03f4..2d217bcf 100644
--- a/src/drivers/ao_hmc5883.c
+++ b/src/drivers/ao_hmc5883.c
@@ -70,7 +70,6 @@ ao_hmc5883_sample(struct ao_hmc5883_sample *sample)
{
uint16_t *d = (uint16_t *) sample;
int i = sizeof (*sample) / 2;
- uint8_t single = HMC5883_MODE_SINGLE;
ao_hmc5883_done = 0;
ao_exti_enable(AO_HMC5883_INT_PORT, AO_HMC5883_INT_PIN);
diff --git a/src/drivers/ao_lco_cmd.c b/src/drivers/ao_lco_cmd.c
index 9c35b324..acbf589a 100644
--- a/src/drivers/ao_lco_cmd.c
+++ b/src/drivers/ao_lco_cmd.c
@@ -119,7 +119,6 @@ lco_report_cmd(void) __reentrant
static void
lco_fire_cmd(void) __reentrant
{
- static __xdata struct ao_pad_command command;
uint8_t secs;
uint8_t i;
int8_t r;
diff --git a/src/drivers/ao_lco_func.c b/src/drivers/ao_lco_func.c
index 99e58b76..a5d28e61 100644
--- a/src/drivers/ao_lco_func.c
+++ b/src/drivers/ao_lco_func.c
@@ -26,7 +26,6 @@ static __xdata uint8_t ao_lco_mutex;
int8_t
ao_lco_query(uint16_t box, struct ao_pad_query *query, uint16_t *tick_offset)
{
- uint8_t i;
int8_t r;
uint16_t sent_time;
diff --git a/src/drivers/ao_mma655x.c b/src/drivers/ao_mma655x.c
index ce83a5a3..c36858ad 100644
--- a/src/drivers/ao_mma655x.c
+++ b/src/drivers/ao_mma655x.c
@@ -20,14 +20,15 @@
#if HAS_MMA655X
-#if 0
-#define PRINTD(...) do { printf ("\r%5u %s: ", ao_tick_count, __func__); printf(__VA_ARGS__); } while(0)
+#define DEBUG 0
+#define DEBUG_LOW 1
+#define DEBUG_HIGH 2
+#if 1
+#define PRINTD(l, ...) do { if (DEBUG & (l)) { printf ("\r%5u %s: ", ao_tick_count, __func__); printf(__VA_ARGS__); flush(); } } while(0)
#else
-#define PRINTD(...)
+#define PRINTD(l,...)
#endif
-static uint8_t mma655x_configured;
-
uint8_t ao_mma655x_spi_index = AO_MMA655X_SPI_INDEX;
static void
@@ -53,7 +54,7 @@ ao_mma655x_restart(void) {
ao_gpio_set(AO_MMA655X_CS_PORT, AO_MMA655X_CS_PIN, AO_MMA655X_CS, 1);
/* Emperical testing on STM32L151 at 32MHz for this delay amount */
- for (i = 0; i < 9; i++)
+ for (i = 0; i < 10; i++)
ao_arch_nop();
ao_gpio_set(AO_MMA655X_CS_PORT, AO_MMA655X_CS_PIN, AO_MMA655X_CS, 0);
}
@@ -72,15 +73,17 @@ ao_parity(uint8_t v)
return p;
}
+#if 0
static void
ao_mma655x_cmd(uint8_t d[2])
{
ao_mma655x_start();
- PRINTD("\tSEND %02x %02x\n", d[0], d[1]);
+ PRINTD(DEBUG_LOW, "\tSEND %02x %02x\n", d[0], d[1]);
ao_spi_duplex(d, d, 2, AO_MMA655X_SPI_INDEX);
- PRINTD("\t\tRECV %02x %02x\n", d[0], d[1]);
+ PRINTD(DEBUG_LOW, "\t\tRECV %02x %02x\n", d[0], d[1]);
ao_mma655x_stop();
}
+#endif
static uint8_t
ao_mma655x_reg_read(uint8_t addr)
@@ -97,6 +100,7 @@ ao_mma655x_reg_read(uint8_t addr)
d[1] = 0x00;
ao_spi_duplex(&d, &d, 2, AO_MMA655X_SPI_INDEX);
ao_mma655x_stop();
+ PRINTD(DEBUG_LOW, "read %x = %x %x\n", addr, d[0], d[1]);
return d[1];
}
@@ -105,6 +109,7 @@ ao_mma655x_reg_write(uint8_t addr, uint8_t value)
{
uint8_t d[2];
+ PRINTD(DEBUG_LOW, "write %x %x\n", addr, value);
addr |= (1 << 6); /* write mode */
d[0] = addr | (ao_parity(addr^value) << 7);
d[1] = value;
@@ -113,8 +118,6 @@ ao_mma655x_reg_write(uint8_t addr, uint8_t value)
ao_mma655x_stop();
addr &= ~(1 << 6);
- PRINTD("write %x %x = %x\n",
- addr, value, ao_mma655x_reg_read(addr));
}
static uint16_t
@@ -131,14 +134,14 @@ ao_mma655x_value(void)
(0 << 1) | /* Arm disabled */
(1 << 0)); /* Odd parity */
ao_mma655x_start();
- PRINTD("value SEND %02x %02x\n", d[0], d[1]);
+ PRINTD(DEBUG_LOW, "value SEND %02x %02x\n", d[0], d[1]);
ao_spi_send(d, 2, AO_MMA655X_SPI_INDEX);
ao_mma655x_restart();
d[0] = 0x80;
d[1] = 0x00;
ao_spi_duplex(d, d, 2, AO_MMA655X_SPI_INDEX);
ao_mma655x_stop();
- PRINTD("value RECV %02x %02x\n", d[0], d[1]);
+ PRINTD(DEBUG_LOW, "value RECV %02x %02x\n", d[0], d[1]);
v = (uint16_t) d[1] << 2;
v |= d[0] >> 6;
@@ -148,15 +151,16 @@ ao_mma655x_value(void)
static void
ao_mma655x_reset(void) {
+ PRINTD(DEBUG_HIGH, "reset\n");
ao_mma655x_reg_write(AO_MMA655X_DEVCTL,
(0 << AO_MMA655X_DEVCTL_RES_1) |
- (0 << AO_MMA655X_DEVCTL_RES_1));
+ (0 << AO_MMA655X_DEVCTL_RES_0));
ao_mma655x_reg_write(AO_MMA655X_DEVCTL,
(1 << AO_MMA655X_DEVCTL_RES_1) |
- (1 << AO_MMA655X_DEVCTL_RES_1));
+ (1 << AO_MMA655X_DEVCTL_RES_0));
ao_mma655x_reg_write(AO_MMA655X_DEVCTL,
(0 << AO_MMA655X_DEVCTL_RES_1) |
- (1 << AO_MMA655X_DEVCTL_RES_1));
+ (1 << AO_MMA655X_DEVCTL_RES_0));
}
#define DEVCFG_VALUE (\
@@ -169,54 +173,73 @@ ao_mma655x_reset(void) {
(0 << AO_MMA655X_AXISCFG_LPF)) /* 100Hz 4-pole filter */
+#define AO_ST_TRIES 10
+#define AO_ST_DELAY AO_MS_TO_TICKS(100)
+
static void
ao_mma655x_setup(void)
{
- uint8_t v;
uint16_t a, a_st;
- uint8_t stdefl;
- uint8_t i;
+ int16_t st_change;
+ int tries;
+ uint8_t devstat;
+#if 0
uint8_t s0, s1, s2, s3;
- uint8_t pn;
uint32_t lot;
- uint16_t serial;
-
-
- if (mma655x_configured)
- return;
- mma655x_configured = 1;
- ao_delay(AO_MS_TO_TICKS(10)); /* Top */
- ao_mma655x_reset();
- ao_delay(AO_MS_TO_TICKS(10)); /* Top */
- (void) ao_mma655x_reg_read(AO_MMA655X_DEVSTAT);
- v = ao_mma655x_reg_read(AO_MMA655X_DEVSTAT);
-
- /* Configure R/W register values.
- * Most of them relate to the arming feature, which
- * we don't use, so the only registers we need to
- * write are DEVCFG and AXISCFG
- */
+#endif
- ao_mma655x_reg_write(AO_MMA655X_DEVCFG,
- DEVCFG_VALUE | (0 << AO_MMA655X_DEVCFG_ENDINIT));
+ for (tries = 0; tries < AO_ST_TRIES; tries++) {
+ ao_delay(AO_MS_TO_TICKS(10));
+ ao_mma655x_reset();
+ ao_delay(AO_MS_TO_TICKS(10));
- /* Test X axis
- */
+ devstat = ao_mma655x_reg_read(AO_MMA655X_DEVSTAT);
+ PRINTD(DEBUG_HIGH, "devstat %x\n", devstat);
+
+ if (!(devstat & (1 << AO_MMA655X_DEVSTAT_DEVRES)))
+ continue;
+
+ /* Configure R/W register values.
+ * Most of them relate to the arming feature, which
+ * we don't use, so the only registers we need to
+ * write are DEVCFG and AXISCFG
+ */
+
+ ao_mma655x_reg_write(AO_MMA655X_DEVCFG,
+ DEVCFG_VALUE | (0 << AO_MMA655X_DEVCFG_ENDINIT));
+
+ /* Test X axis
+ */
- ao_mma655x_reg_write(AO_MMA655X_AXISCFG,
- AXISCFG_VALUE |
- (1 << AO_MMA655X_AXISCFG_ST));
- a_st = ao_mma655x_value();
+ ao_mma655x_reg_write(AO_MMA655X_AXISCFG,
+ AXISCFG_VALUE |
+ (1 << AO_MMA655X_AXISCFG_ST));
+ ao_delay(AO_MS_TO_TICKS(10));
+
+ a_st = ao_mma655x_value();
+
+ ao_mma655x_reg_write(AO_MMA655X_AXISCFG,
+ AXISCFG_VALUE |
+ (0 << AO_MMA655X_AXISCFG_ST));
+
+ ao_delay(AO_MS_TO_TICKS(10));
+
+ a = ao_mma655x_value();
- stdefl = ao_mma655x_reg_read(AO_MMA655X_STDEFL);
+ st_change = a_st - a;
- ao_mma655x_reg_write(AO_MMA655X_AXISCFG,
- AXISCFG_VALUE |
- (0 << AO_MMA655X_AXISCFG_ST));
- a = ao_mma655x_value();
+ PRINTD(DEBUG_HIGH, "self test %d normal %d change %d\n", a_st, a, st_change);
+
+ if (AO_ST_MIN <= st_change && st_change <= AO_ST_MAX)
+ break;
+ ao_delay(AO_ST_DELAY);
+ }
+ if (tries == AO_ST_TRIES)
+ ao_sensor_errors = 1;
ao_mma655x_reg_write(AO_MMA655X_DEVCFG,
DEVCFG_VALUE | (1 << AO_MMA655X_DEVCFG_ENDINIT));
+#if 0
s0 = ao_mma655x_reg_read(AO_MMA655X_SN0);
s1 = ao_mma655x_reg_read(AO_MMA655X_SN1);
s2 = ao_mma655x_reg_read(AO_MMA655X_SN2);
@@ -226,6 +249,7 @@ ao_mma655x_setup(void)
serial = lot & 0x1fff;
lot >>= 12;
pn = ao_mma655x_reg_read(AO_MMA655X_PN);
+#endif
}
uint16_t ao_mma655x_current;
@@ -259,8 +283,6 @@ static __xdata struct ao_task ao_mma655x_task;
void
ao_mma655x_init(void)
{
- mma655x_configured = 0;
-
ao_cmd_register(&ao_mma655x_cmds[0]);
ao_spi_init_cs(AO_MMA655X_CS_PORT, (1 << AO_MMA655X_CS_PIN));
diff --git a/src/drivers/ao_mma655x.h b/src/drivers/ao_mma655x.h
index 2d951e07..e57e3377 100644
--- a/src/drivers/ao_mma655x.h
+++ b/src/drivers/ao_mma655x.h
@@ -78,6 +78,14 @@
#define AO_MMA655X_COUNT 0x15
#define AO_MMA655X_OFFCORR 0x16
+/*
+ * Range of valid self-test difference from
+ * normal measurement
+ */
+
+#define AO_ST_MIN 300
+#define AO_ST_MAX 800
+
extern uint16_t ao_mma655x_current;
void
diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index f8ce7346..c0458027 100644
--- a/src/drivers/ao_mpu6000.c
+++ b/src/drivers/ao_mpu6000.c
@@ -21,7 +21,6 @@
#if HAS_MPU6000
-static uint8_t ao_mpu6000_wake;
static uint8_t ao_mpu6000_configured;
#ifndef AO_MPU6000_I2C_INDEX
@@ -133,7 +132,7 @@ ao_mpu6000_gyro(int16_t v)
#endif
static uint8_t
-ao_mpu6000_accel_check(int16_t normal, int16_t test, char *which)
+ao_mpu6000_accel_check(int16_t normal, int16_t test)
{
int16_t diff = test - normal;
@@ -147,7 +146,7 @@ ao_mpu6000_accel_check(int16_t normal, int16_t test, char *which)
}
static uint8_t
-ao_mpu6000_gyro_check(int16_t normal, int16_t test, char *which)
+ao_mpu6000_gyro_check(int16_t normal, int16_t test)
{
int16_t diff = test - normal;
@@ -293,13 +292,13 @@ _ao_mpu6000_setup(void)
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_accel_check(normal_mode.accel_x, test_mode.accel_x);
+ errors += ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y);
+ errors += ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_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_gyro_check(normal_mode.gyro_x, test_mode.gyro_x);
+ errors += ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y);
+ errors += ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z);
if (!errors)
break;
}
generated by cgit v1.2.3 (git 2.39.1) at 2025-12-27 08:40:35 +0000