From 5e9193f6375be27e5f7a0321fd34b6acfe81247f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 14 May 2013 09:12:29 -0700 Subject: altos: Add 'g' command to ublox GPS code. Take the gps_dump function from ao_gps_skytraq.c and move it to a new file so it can be shared with the u-blox driver. That affects every skytraq and u-blox user as they need to include the new file. Signed-off-by: Keith Packard --- src/drivers/ao_gps_skytraq.c | 21 +-------------------- src/drivers/ao_gps_ublox.c | 6 ++++++ 2 files changed, 7 insertions(+), 20 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c index d2f67e6b..9a9dff75 100644 --- a/src/drivers/ao_gps_skytraq.c +++ b/src/drivers/ao_gps_skytraq.c @@ -483,25 +483,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 +513,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..24e70ee3 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -695,10 +695,16 @@ ao_gps(void) __reentrant } } +__code struct ao_cmds ao_gps_cmds[] = { + { ao_gps_show, "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"); } -- cgit v1.2.3 From 461215eea72ff9d64748304e76b08da37ee3dfe9 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 14 May 2013 09:21:54 -0700 Subject: altos: Give u-blox 3 seconds after boot before we bug it Signed-off-by: Keith Packard --- src/drivers/ao_gps_ublox.c | 3 +++ 1 file changed, 3 insertions(+) (limited to 'src/drivers') diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index 24e70ee3..56a4da16 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -405,6 +405,9 @@ static void ao_gps_setup(void) { uint8_t i, k; + + ao_delay(AO_SEC_TO_TICKS(3)); + ao_ublox_set_speed(AO_SERIAL_SPEED_9600); /* -- cgit v1.2.3 From a4e4eec827d61a05fda52ddb68b55f17b6028d5e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 14 May 2013 09:25:08 -0700 Subject: altos: gps serial routines are called ao_gps_*, not ao_ublox_* This caused the u-blox driver to use serial port 1 instead of the project-specified serial port. Signed-off-by: Keith Packard --- src/drivers/ao_gps_ublox.c | 28 +++++++++++----------------- src/test/ao_gps_test_ublox.c | 6 +++--- 2 files changed, 14 insertions(+), 20 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index 56a4da16..80869561 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -39,13 +39,7 @@ 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 -#endif - -#define ao_ublox_byte() ((uint8_t) ao_ublox_getchar()) +#define ao_ublox_byte() ((uint8_t) ao_gps_getchar()) static inline void add_cksum(struct ao_ublox_cksum *cksum, uint8_t c) { @@ -61,7 +55,7 @@ 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_putchar(c); } static void ao_ublox_put_i8(int8_t c) @@ -408,14 +402,14 @@ ao_gps_setup(void) ao_delay(AO_SEC_TO_TICKS(3)); - ao_ublox_set_speed(AO_SERIAL_SPEED_9600); + ao_gps_set_speed(AO_SERIAL_SPEED_9600); /* * A bunch of nulls so the start bit * is clear */ for (i = 0; i < 64; i++) - ao_ublox_putchar(0x00); + ao_gps_putchar(0x00); /* * Send the baud-rate setting and protocol-setting @@ -423,27 +417,27 @@ ao_gps_setup(void) */ for (k = 0; k < 3; k++) 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]); /* * Increase the baud rate */ - ao_ublox_set_speed(AO_SERIAL_SPEED_57600); + ao_gps_set_speed(AO_SERIAL_SPEED_57600); /* * 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_putchar(0x00); } 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); @@ -453,8 +447,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 diff --git a/src/test/ao_gps_test_ublox.c b/src/test/ao_gps_test_ublox.c index afd4dba4..a0e04cb6 100644 --- a/src/test/ao_gps_test_ublox.c +++ b/src/test/ao_gps_test_ublox.c @@ -130,7 +130,7 @@ static uint16_t recv_len; static void check_ublox_message(char *which, uint8_t *msg); char -ao_serial1_getchar(void) +ao_gps_getchar(void) { char c; uint8_t uc; @@ -163,7 +163,7 @@ static int message_len; static uint16_t send_len; void -ao_serial1_putchar(char c) +ao_gps_putchar(char c) { int i; uint8_t uc = (uint8_t) c; @@ -196,7 +196,7 @@ ao_serial1_putchar(char c) #define AO_SERIAL_SPEED_115200 3 static void -ao_serial1_set_speed(uint8_t speed) +ao_gps_set_speed(uint8_t speed) { int fd = ao_gps_fd; struct termios termios; -- cgit v1.2.3 From 9beacd77b3e8106e036e50a67312dfee414fbc51 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 14 May 2013 09:01:49 -0700 Subject: altos: Initialize MPU6000 CS pin for SPI mode Without this, we can't talk to the chip very well Signed-off-by: Keith Packard --- src/drivers/ao_mpu6000.c | 6 ++++++ src/telemega-v0.3/ao_pins.h | 2 +- 2 files changed, 7 insertions(+), 1 deletion(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c index c65aecbc..fc768cc9 100644 --- a/src/drivers/ao_mpu6000.c +++ b/src/drivers/ao_mpu6000.c @@ -19,6 +19,8 @@ #include #include +#if HAS_MPU6000 + static uint8_t ao_mpu6000_wake; static uint8_t ao_mpu6000_configured; @@ -326,5 +328,9 @@ ao_mpu6000_init(void) ao_mpu6000_configured = 0; ao_add_task(&ao_mpu6000_task, ao_mpu6000, "mpu6000"); +#ifndef AO_MPU6000_I2C_INDEX + ao_spi_init_cs(AO_MPU6000_SPI_CS_PORT, (1 << AO_MPU6000_SPI_CS_PIN)); +#endif ao_cmd_register(&ao_mpu6000_cmds[0]); } +#endif diff --git a/src/telemega-v0.3/ao_pins.h b/src/telemega-v0.3/ao_pins.h index 88b473cc..bace5853 100644 --- a/src/telemega-v0.3/ao_pins.h +++ b/src/telemega-v0.3/ao_pins.h @@ -308,7 +308,7 @@ struct ao_adc { * mpu6000 */ -#define HAS_MPU6000 1 +#define HAS_MPU6000 1 #define AO_MPU6000_INT_PORT (&stm_gpioe) #define AO_MPU6000_INT_PIN 0 #define AO_MPU6000_SPI_BUS AO_SPI_1_PE13_PE14_PE15 -- cgit v1.2.3 From 0571531066918fdefe9447f3b4192d0c6c477afa Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 14 May 2013 10:48:24 -0700 Subject: altos: Grab SPI mutex until MPU6000 I2C mode is disabled If other drivers use the SPI bus, the MPU6000 gets confused as its sitting on the bus looking for I2C messages. Just grab the mutex before the OS is running and hold onto it until the MPU6000 has been initialized. Signed-off-by: Keith Packard --- src/drivers/ao_mpu6000.c | 265 ++++++++++++++++++++++++++++------------------- src/drivers/ao_mpu6000.h | 14 +-- src/stm/ao_arch_funcs.h | 7 +- 3 files changed, 168 insertions(+), 118 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c index fc768cc9..73057820 100644 --- a/src/drivers/ao_mpu6000.c +++ b/src/drivers/ao_mpu6000.c @@ -24,81 +24,89 @@ 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_put() ao_spi_put_bit(AO_MPU6000_SPI_CS_PORT, \ - AO_MPU6000_SPI_CS_PIN, \ - AO_MPU6000_SPI_CS, \ - AO_MPU6000_SPI_BUS) +#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)) + +#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--) { @@ -153,7 +161,22 @@ ao_mpu6000_gyro_check(int16_t normal, int16_t test, char *which) } 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); +} + +static void +_ao_mpu6000_setup(void) { struct ao_mpu6000_sample normal_mode, test_mode; int errors =0; @@ -161,104 +184,107 @@ ao_mpu6000_setup(void) 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(); + + /* 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_CONTROL) & (1 << MPU6000_USER_CONTROL_SIG_COND_RESET)) - ao_yield(); + 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)); + _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)); + _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_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_sample(&test_mode); #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)); + _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_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); + _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"); @@ -272,13 +298,13 @@ ao_mpu6000_setup(void) ao_panic(AO_PANIC_SELF_TEST_MPU6000); /* 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; @@ -289,10 +315,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(); @@ -328,9 +364,20 @@ ao_mpu6000_init(void) ao_mpu6000_configured = 0; ao_add_task(&ao_mpu6000_task, ao_mpu6000, "mpu6000"); -#ifndef AO_MPU6000_I2C_INDEX + +#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..a42f69c4 100644 --- a/src/drivers/ao_mpu6000.h +++ b/src/drivers/ao_mpu6000.h @@ -133,13 +133,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 diff --git a/src/stm/ao_arch_funcs.h b/src/stm/ao_arch_funcs.h index 1e78cabc..6fe86e62 100644 --- a/src/stm/ao_arch_funcs.h +++ b/src/stm/ao_arch_funcs.h @@ -87,13 +87,16 @@ extern uint16_t ao_spi_speed[STM_NUM_SPI]; void ao_spi_init(void); +#define ao_spi_set_cs(reg,mask) ((reg)->bsrr = ((uint32_t) (mask)) << 16) +#define ao_spi_clr_cs(reg,mask) ((reg)->bsrr = (mask)) + #define ao_spi_get_mask(reg,mask,bus, speed) do { \ ao_spi_get(bus, speed); \ - (reg)->bsrr = ((uint32_t) mask) << 16; \ + ao_spi_set_cs(reg,mask); \ } while (0) #define ao_spi_put_mask(reg,mask,bus) do { \ - (reg)->bsrr = mask; \ + ao_spi_clr_cs(reg,mask); \ ao_spi_put(bus); \ } while (0) -- cgit v1.2.3 From 69b9f613ad36b8039f223ed30f8c75913916d82c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 14 May 2013 22:19:07 -0700 Subject: altos: Remove some MMA655x debugging printfs Signed-off-by: Keith Packard --- src/drivers/ao_mma655x.c | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) (limited to 'src/drivers') 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; -- cgit v1.2.3 From 4ef0136c27e8f47a1eb38f9cbcd2c61288732d78 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 15 May 2013 15:32:59 -0700 Subject: altos: Generate unmodulated carrier for CC1120 test mode This sets the deviation to 0, enables the preamble and turns on the transmitter. It will sit there happily sending a bare carrier forever Signed-off-by: Keith Packard --- src/drivers/ao_cc1120.c | 42 ++++++++++++++++++++++++++++++++++++++++-- src/drivers/ao_cc1120.h | 2 ++ 2 files changed, 42 insertions(+), 2 deletions(-) (limited to 'src/drivers') 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) -- cgit v1.2.3 From d9b42470e8889b44bb08858a610285410a200ab9 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 18 May 2013 03:02:38 -0700 Subject: altos: Use ao_port_t in m25 driver This uses ao_port_t for all of the chip select masks Signed-off-by: Keith Packard --- src/drivers/ao_m25.c | 29 +++++++++++++++-------------- 1 file changed, 15 insertions(+), 14 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_m25.c b/src/drivers/ao_m25.c index 390637d7..9b768012 100644 --- a/src/drivers/ao_m25.c +++ b/src/drivers/ao_m25.c @@ -82,7 +82,7 @@ __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 */ @@ -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; @@ -268,7 +269,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 +296,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 +333,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; -- cgit v1.2.3 From f794e6c95697b034be315632fddb3a5475c43b5b Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 19 May 2013 19:57:23 -0700 Subject: altos: Use ao_spi_get/put_bit in MS5607 driver Replace open-coded ao_spi_get/put and ao_gpio_set sequences Signed-off-by: Keith Packard --- src/drivers/ao_ms5607.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c index bd57400e..0eece584 100644 --- a/src/drivers/ao_ms5607.c +++ b/src/drivers/ao_ms5607.c @@ -26,14 +26,12 @@ static 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 -- cgit v1.2.3 From 35a05041d3ca3e69a146bd3bf8038c0f1cbc1b42 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 19 May 2013 20:04:29 -0700 Subject: altos: Add EXTI_PIN_NOCONFIGURE to exti interface, use for MS5607 This asks the EXTI code to not mess with the pin configuration so that the MS5607 driver can get interrupts on the MISO pin while still using it for SPI. Signed-off-by: Keith Packard --- src/attiny/ao_exti.h | 1 + src/drivers/ao_ms5607.c | 12 ++---------- src/lpc/ao_exti.h | 1 + src/lpc/ao_exti_lpc.c | 3 +++ src/stm/ao_exti.h | 1 + src/stm/ao_exti_stm.c | 30 ++++++++++++++++-------------- 6 files changed, 24 insertions(+), 24 deletions(-) (limited to 'src/drivers') diff --git a/src/attiny/ao_exti.h b/src/attiny/ao_exti.h index 2ea4f47d..85bb2fba 100644 --- a/src/attiny/ao_exti.h +++ b/src/attiny/ao_exti.h @@ -30,5 +30,6 @@ ao_exti_setup_port(uint8_t pin, uint8_t mode, void (*callback)(void)); #define ao_exti_init() #define AO_EXTI_MODE_RISING 1 +#define AO_EXTI_PIN_NOCONFIGURE 0 #endif /* _AO_EXTI_H_ */ diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c index 0eece584..8f1dcbe1 100644 --- a/src/drivers/ao_ms5607.c +++ b/src/drivers/ao_ms5607.c @@ -247,17 +247,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/lpc/ao_exti.h b/src/lpc/ao_exti.h index cbe63eaa..e8599eb4 100644 --- a/src/lpc/ao_exti.h +++ b/src/lpc/ao_exti.h @@ -25,6 +25,7 @@ #define AO_EXTI_PRIORITY_LOW 16 #define AO_EXTI_PRIORITY_MED 0 #define AO_EXTI_PRIORITY_HIGH 32 +#define AO_EXTI_PIN_NOCONFIGURE 64 void ao_exti_setup(uint8_t gpio, uint8_t pin, uint8_t mode, void (*callback)()); diff --git a/src/lpc/ao_exti_lpc.c b/src/lpc/ao_exti_lpc.c index ce98b4ad..588cf58c 100644 --- a/src/lpc/ao_exti_lpc.c +++ b/src/lpc/ao_exti_lpc.c @@ -91,6 +91,9 @@ ao_exti_setup (uint8_t port, uint8_t pin, uint8_t mode, void (*callback)(void)) if (pint == LPC_NUM_PINT) ao_panic(AO_PANIC_EXTI); + if (!mode & AO_EXTI_PIN_NOCONFIGURE) + ao_enable_input(port, pin, mode); + ao_arch_block_interrupts(); mask = (1 << pint); ao_pint_inuse |= mask; diff --git a/src/stm/ao_exti.h b/src/stm/ao_exti.h index 35b56b57..ebea224d 100644 --- a/src/stm/ao_exti.h +++ b/src/stm/ao_exti.h @@ -25,6 +25,7 @@ #define AO_EXTI_PRIORITY_LOW 16 #define AO_EXTI_PRIORITY_MED 0 #define AO_EXTI_PRIORITY_HIGH 32 +#define AO_EXTI_PIN_NOCONFIGURE 64 void ao_exti_setup(struct stm_gpio *gpio, uint8_t pin, uint8_t mode, void (*callback)()); diff --git a/src/stm/ao_exti_stm.c b/src/stm/ao_exti_stm.c index 1361d0d4..c1dcdf85 100644 --- a/src/stm/ao_exti_stm.c +++ b/src/stm/ao_exti_stm.c @@ -70,21 +70,23 @@ ao_exti_setup (struct stm_gpio *gpio, uint8_t pin, uint8_t mode, void (*callback /* configure gpio to interrupt routing */ stm_exticr_set(gpio, pin); - /* configure pin as input, setting selected pull-up/down mode */ - stm_moder_set(gpio, pin, STM_MODER_INPUT); - switch (mode & (AO_EXTI_MODE_PULL_UP|AO_EXTI_MODE_PULL_DOWN)) { - case 0: - default: - pupdr = STM_PUPDR_NONE; - break; - case AO_EXTI_MODE_PULL_UP: - pupdr = STM_PUPDR_PULL_UP; - break; - case AO_EXTI_MODE_PULL_DOWN: - pupdr = STM_PUPDR_PULL_DOWN; - break; + if (!(mode & AO_EXTI_PIN_NOCONFIGURE)) { + /* configure pin as input, setting selected pull-up/down mode */ + stm_moder_set(gpio, pin, STM_MODER_INPUT); + switch (mode & (AO_EXTI_MODE_PULL_UP|AO_EXTI_MODE_PULL_DOWN)) { + case 0: + default: + pupdr = STM_PUPDR_NONE; + break; + case AO_EXTI_MODE_PULL_UP: + pupdr = STM_PUPDR_PULL_UP; + break; + case AO_EXTI_MODE_PULL_DOWN: + pupdr = STM_PUPDR_PULL_DOWN; + break; + } + stm_pupdr_set(gpio, pin, pupdr); } - stm_pupdr_set(gpio, pin, pupdr); /* Set interrupt mask and rising/falling mode */ stm_exti.imr &= ~mask; -- cgit v1.2.3 From 8ba2035c78293bc312804722249df76dd4692d71 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 9 Jun 2013 09:53:07 -0700 Subject: altos: Add driver for 74hc165 shift register Just reads one byte from the shift register using the SPI driver and returns it Signed-off-by: Keith Packard --- src/drivers/ao_74hc165.c | 58 ++++++++++++++++++++++++++++++++++++++++++++++++ src/drivers/ao_74hc165.h | 27 ++++++++++++++++++++++ 2 files changed, 85 insertions(+) create mode 100644 src/drivers/ao_74hc165.c create mode 100644 src/drivers/ao_74hc165.h (limited to 'src/drivers') diff --git a/src/drivers/ao_74hc165.c b/src/drivers/ao_74hc165.c new file mode 100644 index 00000000..f24fce37 --- /dev/null +++ b/src/drivers/ao_74hc165.c @@ -0,0 +1,58 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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 +#include + +uint8_t +ao_74hc165_read(void) +{ + static __xdata state; + ao_mutex_get(&ao_spi_mutex); + ao_spi_set_speed(AO_SPI_SPEED_FAST); + AO_74HC165_CS = 1; + ao_spi_recv(&state, 1, AO_74HC165_SPI_BUS); + AO_74HC165_CS = 0; + ao_mutex_put(&ao_spi_mutex); + 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 + * + * 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_ */ -- cgit v1.2.3 From 187f661c2512e4260d0ca64134de8fad199f5944 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 9 Jun 2013 10:00:54 -0700 Subject: altos: Add telefire v0.2 support Signed-off-by: Keith Packard --- src/Makefile | 9 +-- src/cc1111/ao_adc.c | 2 +- src/drivers/ao_pad.c | 25 +++++-- src/telefire-v0.2/Makefile | 103 ++++++++++++++++++++++++++++ src/telefire-v0.2/ao_pins.h | 144 ++++++++++++++++++++++++++++++++++++++++ src/telefire-v0.2/ao_telefire.c | 45 +++++++++++++ 6 files changed, 317 insertions(+), 11 deletions(-) create mode 100644 src/telefire-v0.2/Makefile create mode 100644 src/telefire-v0.2/ao_pins.h create mode 100644 src/telefire-v0.2/ao_telefire.c (limited to 'src/drivers') diff --git a/src/Makefile b/src/Makefile index ff26ac20..ee76c325 100644 --- a/src/Makefile +++ b/src/Makefile @@ -16,14 +16,11 @@ include Version SDCCDIRS=\ telemetrum-v1.2 telemetrum-v1.1 telemetrum-v1.0 \ - teledongle-v0.2 teledongle-v0.1 \ - telemini-v1.0 telenano-v0.1 \ + teledongle-v0.2 \ + telemini-v1.0 \ telebt-v1.0 \ - telemetrum-v0.1-sky telemetrum-v0.1-sirf \ - telelaunch-v0.1 tidongle test \ teleterra-v0.2 teleshield-v0.1 \ - telefire-v0.1 \ - spiradio-v0.1 + telefire-v0.1 telefire-v0.2 AVRDIRS=\ telescience-v0.1 telescience-pwm micropeak diff --git a/src/cc1111/ao_adc.c b/src/cc1111/ao_adc.c index ed76179b..4a58023d 100644 --- a/src/cc1111/ao_adc.c +++ b/src/cc1111/ao_adc.c @@ -144,7 +144,7 @@ ao_adc_isr(void) __interrupt 1 } #endif /* telemini || telenano */ -#ifdef TELEFIRE_V_0_1 +#if defined(TELEFIRE_V_0_1) || defined(TELEFIRE_V_0_2) a = (uint8_t __xdata *) (&ao_data_ring[ao_data_head].adc.sense[0] + sequence - AO_ADC_FIRST_PIN); a[0] = ADCL; a[1] = ADCH; diff --git a/src/drivers/ao_pad.c b/src/drivers/ao_pad.c index 6cec98ab..e0c03c74 100644 --- a/src/drivers/ao_pad.c +++ b/src/drivers/ao_pad.c @@ -17,7 +17,7 @@ #include #include -#include +#include #include static __xdata uint8_t ao_pad_ignite; @@ -218,6 +218,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) { @@ -236,8 +251,10 @@ ao_pad(void) if (ret != AO_RADIO_CMAC_OK) continue; - 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 +344,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; diff --git a/src/telefire-v0.2/Makefile b/src/telefire-v0.2/Makefile new file mode 100644 index 00000000..3353bc1a --- /dev/null +++ b/src/telefire-v0.2/Makefile @@ -0,0 +1,103 @@ +# +# TeleFire build file +# + +TELEFIRE_VER=0.2 +TELEFIRE_DEF=0_2 + +vpath %.c ..:../core:../cc1111:../drivers:../product +vpath %.h ..:../core:../cc1111:../drivers:../product +vpath ao-make-product.5c ../util + +ifndef VERSION +include ../Version +endif + +INC = \ + ao.h \ + ao_pins.h \ + ao_arch.h \ + ao_arch_funcs.h \ + ao_pad.h \ + cc1111.h \ + ao_product.h + +CORE_SRC = \ + ao_cmd.c \ + ao_config.c \ + ao_convert.c \ + ao_mutex.c \ + ao_panic.c \ + ao_stdio.c \ + ao_storage.c \ + ao_task.c \ + ao_freq.c + +CC1111_SRC = \ + ao_adc.c \ + ao_aes.c \ + ao_beep.c \ + ao_dma.c \ + ao_intflash.c \ + ao_radio.c \ + ao_radio_cmac.c \ + ao_romconfig.c \ + ao_serial.c \ + ao_spi.c \ + ao_string.c \ + ao_timer.c \ + ao_usb.c \ + _bp.c + +DRIVER_SRC = \ + ao_pca9922.c \ + ao_74hc165.c \ + ao_pad.c \ + ao_radio_cmac_cmd.c + +PRODUCT_SRC = \ + ao_telefire.c + +SRC = \ + $(CORE_SRC) \ + $(CC1111_SRC) \ + $(DRIVER_SRC) \ + $(PRODUCT_SRC) + +PROGNAME = telefire-v$(TELEFIRE_VER) +PROG = $(PROGNAME)-$(VERSION).ihx +PRODUCT=TeleFire-v$(TELEFIRE_VER) +PRODUCT_DEF=-DTELEFIRE_V_$(TELEFIRE_DEF) +IDPRODUCT=0x000f +CODESIZE=0x6700 + +include ../cc1111/Makefile.cc1111 + +NICKLE=nickle +CHECK_STACK=sh ../util/check-stack + +V=0 +# The user has explicitly enabled quiet compilation. +ifeq ($(V),0) +quiet = @printf " $1 $2 $@\n"; $($1) +endif +# Otherwise, print the full command line. +quiet ?= $($1) + +all: ../$(PROG) + +../$(PROG): $(REL) Makefile + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) .. + $(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@ + +ao_product.h: ao-make-product.5c ../Version + $(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@ + +distclean: clean + +clean: clean-cc1111 + +install: + +uninstall: + diff --git a/src/telefire-v0.2/ao_pins.h b/src/telefire-v0.2/ao_pins.h new file mode 100644 index 00000000..f4050722 --- /dev/null +++ b/src/telefire-v0.2/ao_pins.h @@ -0,0 +1,144 @@ +/* + * Copyright © 2010 Keith Packard + * + * 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_PINS_H_ +#define _AO_PINS_H_ + +#define HAS_RADIO 1 + +#define HAS_FLIGHT 0 +#define HAS_USB 1 +#define HAS_BEEP 0 +#define HAS_GPS 0 +#define HAS_SERIAL_1 0 +#define HAS_ADC 1 +#define HAS_DBG 0 +#define HAS_EEPROM 1 +#define HAS_LOG 0 +#define HAS_PAD 1 +#define USE_INTERNAL_FLASH 1 +#define DBG_ON_P1 0 +#define IGNITE_ON_P2 0 +#define IGNITE_ON_P1 1 +#define IGNITE_ON_P0 0 +#define PACKET_HAS_MASTER 0 +#define PACKET_HAS_SLAVE 0 + +#define AO_LED_CONTINUITY(c) (1 << (c)) +#define AO_LED_CONTINUITY_MASK (0xf) +#define AO_LED_RX 0x10 +#define AO_LED_TX 0x20 +#define AO_LED_ARMED 0x40 +#define AO_LED_POWER 0x80 + +#define AO_LED_RED AO_LED_TX +#define AO_LED_GREEN AO_LED_RX + +#define LEDS_AVAILABLE (0xff) +#define HAS_EXTERNAL_TEMP 0 +#define HAS_ACCEL_REF 0 +#define SPI_CS_ON_P1 1 +#define HAS_AES 1 +#define DMA_SHARE_AES_RADIO 1 + +#define SPI_CS_PORT P1 +#define SPI_CS_SEL P1SEL +#define SPI_CS_DIR P1DIR + +#define SPI_CONST 0x00 + +#define HAS_SPI_0 0 +#define HAS_SPI_1 1 +#define SPI_1_ALT_1 0 +#define SPI_1_ALT_2 1 + +#define HAS_74HC165 1 +#define AO_74HC165_CS_PORT P1 +#define AO_74HC165_CS_PIN 4 +#define AO_74HC165_CS P1_4 + +#define AO_PCA9922_CS_PORT P2 +#define AO_PCA9922_CS_PIN 0 +#define AO_PCA9922_CS P2_0 + +#define AO_PAD_NUM 4 +#define AO_PAD_PORT P1 +#define AO_PAD_DIR P1DIR + +#define AO_PAD_PIN_0 0 +#define AO_PAD_0 P1_0 +#define AO_PAD_ADC_0 0 + +#define AO_PAD_PIN_1 1 +#define AO_PAD_1 P1_1 +#define AO_PAD_ADC_1 1 + +#define AO_PAD_PIN_2 2 +#define AO_PAD_2 P1_2 +#define AO_PAD_ADC_2 2 + +#define AO_PAD_PIN_3 3 +#define AO_PAD_3 P1_3 +#define AO_PAD_ADC_3 3 + +#define AO_PAD_ALL_PINS ((1 << AO_PAD_PIN_0) | (1 << AO_PAD_PIN_1) | (1 << AO_PAD_PIN_2) | (1 << AO_PAD_PIN_3)) +#define AO_PAD_ALL_CHANNELS ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3)) + +#define AO_SIREN_PORT P2 +#define AO_SIREN_DIR P2DIR +#define AO_SIREN_PIN 3 +#define AO_SIREN P2_3 + +#define AO_STROBE_PORT P2 +#define AO_STROBE_DIR P2DIR +#define AO_STROBE_PIN 4 +#define AO_STROBE P2_4 + +/* test these values with real igniters */ +#define AO_PAD_RELAY_CLOSED 3524 +#define AO_PAD_NO_IGNITER 16904 +#define AO_PAD_GOOD_IGNITER 22514 + +#define AO_PAD_ADC_PYRO 4 +#define AO_PAD_ADC_BATT 5 + +#define AO_ADC_FIRST_PIN 0 + +struct ao_adc { + int16_t sense[AO_PAD_NUM]; + int16_t pyro; + int16_t batt; +}; + +#define AO_ADC_DUMP(p) \ + printf ("tick: %5u 0: %5d 1: %5d 2: %5d 3: %5d pyro: %5d batt %5d\n", \ + (p)->tick, \ + (p)->adc.sense[0], \ + (p)->adc.sense[1], \ + (p)->adc.sense[2], \ + (p)->adc.sense[3], \ + (p)->adc.pyro, \ + (p)->adc.batt) + +#define AO_ADC_PINS ((1 << AO_PAD_ADC_0) | \ + (1 << AO_PAD_ADC_1) | \ + (1 << AO_PAD_ADC_2) | \ + (1 << AO_PAD_ADC_3) | \ + (1 << AO_PAD_ADC_PYRO) | \ + (1 << AO_PAD_ADC_BATT)) + +#endif /* _AO_PINS_H_ */ diff --git a/src/telefire-v0.2/ao_telefire.c b/src/telefire-v0.2/ao_telefire.c new file mode 100644 index 00000000..f27ca5e8 --- /dev/null +++ b/src/telefire-v0.2/ao_telefire.c @@ -0,0 +1,45 @@ +/* + * Copyright © 2012 Keith Packard + * + * 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 +#include +#include +#include + +void +main(void) +{ + ao_clock_init(); + + ao_led_init(LEDS_AVAILABLE); + + ao_task_init(); + + ao_timer_init(); + ao_adc_init(); + ao_cmd_init(); + ao_spi_init(); + ao_74hc165_init(); + ao_storage_init(); + ao_usb_init(); + ao_radio_init(); + ao_aes_init(); + ao_pad_init(); +// ao_radio_cmac_cmd_init(); + ao_config_init(); + ao_start_scheduler(); +} -- cgit v1.2.3 From 47b7e1d819e48aaebf6ffda49effbee041ce8750 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 9 Jun 2013 12:13:06 -0700 Subject: altos/telefire: Leave siren on all the time. Add siren/strobe debugging. The 50% duty cycle wasn't actually loud enough outside. Signed-off-by: Keith Packard --- src/drivers/ao_pad.c | 26 ++++++++++++++++++++++---- 1 file changed, 22 insertions(+), 4 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_pad.c b/src/drivers/ao_pad.c index e0c03c74..e205f99b 100644 --- a/src/drivers/ao_pad.c +++ b/src/drivers/ao_pad.c @@ -182,10 +182,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) { @@ -379,6 +376,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[] = { @@ -386,6 +403,7 @@ __code struct ao_cmds ao_pad_cmds[] = { { ao_pad_manual, "i \0Fire igniter. 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 } }; -- cgit v1.2.3 From 72b6c699d355fcd41addb9919d846e63105b9db7 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 13 May 2013 22:34:19 -0700 Subject: altos: Add debounce helper. Use in button and quadrature drivers for TeleLCO Signed-off-by: Keith Packard --- src/core/ao_debounce.h | 52 ++++++++++++ src/drivers/ao_button.c | 82 ++++++++++++------- src/drivers/ao_event.c | 17 +--- src/drivers/ao_event.h | 6 +- src/drivers/ao_quadrature.c | 108 +++++++++++++++++-------- src/stm-demo/Makefile | 5 +- src/stm-demo/ao_demo.c | 4 +- src/stm/ao_debounce.c | 187 ++++++++++++++++++++++++++++++++++++++++++++ src/telelco-v0.2/Makefile | 2 + src/telelco-v0.2/ao_lco.c | 22 ++---- 10 files changed, 389 insertions(+), 96 deletions(-) create mode 100644 src/core/ao_debounce.h create mode 100644 src/stm/ao_debounce.c (limited to 'src/drivers') diff --git a/src/core/ao_debounce.h b/src/core/ao_debounce.h new file mode 100644 index 00000000..ebe290e1 --- /dev/null +++ b/src/core/ao_debounce.h @@ -0,0 +1,52 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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_DEBOUNCE_H_ +#define _AO_DEBOUNCE_H_ + +struct ao_debounce { + struct ao_debounce *next; + + /* time that pin value must be stable before accepting */ + int8_t hold; + + /* last value reported to app; don't report it twice */ + uint8_t value; + + /* current count of intervals pin value has been stable */ + int8_t count; + + /* This pin is running */ + uint8_t running; + + /* Get the current pin value */ + uint8_t (*_get)(struct ao_debounce *debounce); + + /* The stable value has changed */ + void (*_set)(struct ao_debounce *debounce, uint8_t value); +}; + +void +_ao_debounce_start(struct ao_debounce *debounce); + +void +_ao_debounce_stop(struct ao_debounce *debounce); + +void +ao_debounce_init(void); + +#endif /* _AO_DEBOUNCE_H_ */ diff --git a/src/drivers/ao_button.c b/src/drivers/ao_button.c index a507c909..7b1fb530 100644 --- a/src/drivers/ao_button.c +++ b/src/drivers/ao_button.c @@ -18,6 +18,7 @@ #include #include #include +#include #if AO_EVENT #include #define ao_button_queue(b,v) ao_event_put_isr(AO_EVENT_BUTTON, b, v) @@ -25,55 +26,70 @@ #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) { + debounce->hold = AO_BUTTON_DEBOUNCE_HOLD; + debounce->_get = _ao_button_get; + debounce->_set = _ao_button_set; +} + +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 +107,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_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_quadrature.c b/src/drivers/ao_quadrature.c index 6cc2467a..cfa58da6 100644 --- a/src/drivers/ao_quadrature.c +++ b/src/drivers/ao_quadrature.c @@ -18,12 +18,14 @@ #include #include #include -#if AO_EVENT +#include #include -#define ao_quadrature_queue(q) ao_event_put_isr(AO_EVENT_QUADRATURE, q, ao_quadrature_count[q]) -#else -#define ao_quadrature_queue(q) -#endif + +#define AO_QUADRATURE_DEBOUNCE_HOLD 3 + +static __xdata struct ao_debounce ao_quadrature_debounce[AO_QUADRATURE_COUNT]; + +#define debounce_id(d) ((d) - ao_quadrature_debounce) __xdata int32_t ao_quadrature_count[AO_QUADRATURE_COUNT]; @@ -35,41 +37,77 @@ 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 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) - +#define q_case(q) case q: v = (!ao_gpio_get(port(q), bita(q), pina(q))) | ((!ao_gpio_get(port(q), bitb(q), pinb(q))) << 1); break -static void -ao_quadrature_isr(void) -{ - uint8_t q; +uint8_t quad_raw[64]; +uint8_t quad_r; + +static uint8_t +_ao_quadrature_get(struct ao_debounce *debounce) { + uint8_t q = debounce_id(debounce); + uint8_t v = 0; + + switch (q) { #if AO_QUADRATURE_COUNT > 0 - ao_quadrature_update(0); + q_case(0); #endif #if AO_QUADRATURE_COUNT > 1 - ao_quadrature_update(1); + q_case(1); #endif + } + if (q == 0) { + quad_raw[quad_r] = v; + quad_r = (quad_r + 1) & 63; + } + return v; +} - 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 void +_ao_quadrature_queue(uint8_t q, int8_t step) +{ + ao_quadrature_count[q] += step; +#if AO_EVENT + ao_event_put_isr(AO_EVENT_QUADRATURE, q, step); +#endif + ao_wakeup(&ao_quadrature_count[q]); +} + +uint8_t quad_history[64]; +uint8_t quad_h; + +static void +_ao_quadrature_set(struct ao_debounce *debounce, uint8_t value) { + uint8_t q = debounce_id(debounce); + + ao_quadrature_state[q] = ((ao_quadrature_state[q] & 3) << 2); + ao_quadrature_state[q] |= value; + + if (q == 0) { + quad_history[quad_h] = ao_quadrature_state[0]; + quad_h = (quad_h + 1) & 63; + } + + switch (ao_quadrature_state[q]) { + case STATE(0, 1, 0, 0): + _ao_quadrature_queue(q, 1); + break; + case STATE(1, 0, 0, 0): + _ao_quadrature_queue(q, -1); + break; } } +static void +ao_quadrature_isr(void) +{ + uint8_t q; + for (q = 0; q < AO_QUADRATURE_COUNT; q++) + _ao_debounce_start(&ao_quadrature_debounce[q]); +} + int32_t ao_quadrature_poll(uint8_t q) { @@ -107,9 +145,16 @@ static const struct ao_cmds ao_quadrature_cmds[] = { { 0, NULL } }; +static void +ao_quadrature_debounce_init(struct ao_debounce *debounce) { + debounce->hold = AO_QUADRATURE_DEBOUNCE_HOLD; + debounce->_get = _ao_quadrature_get; + debounce->_set = _ao_quadrature_set; +} + #define init(q) do { \ ao_enable_port(port(q)); \ - \ + ao_quadrature_debounce_init(&ao_quadrature_debounce[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); \ @@ -124,6 +169,7 @@ static const struct ao_cmds ao_quadrature_cmds[] = { void ao_quadrature_init(void) { + ao_debounce_init(); #if AO_QUADRATURE_COUNT > 0 init(0); #endif diff --git a/src/stm-demo/Makefile b/src/stm-demo/Makefile index d1f825db..48fa07eb 100644 --- a/src/stm-demo/Makefile +++ b/src/stm-demo/Makefile @@ -36,10 +36,7 @@ ALTOS_SRC = \ ao_data.c \ ao_i2c_stm.c \ ao_usb_stm.c \ - ao_exti_stm.c \ - ao_event.c \ - ao_quadrature.c \ - ao_button.c + ao_exti_stm.c PRODUCT=StmDemo-v0.0 PRODUCT_DEF=-DSTM_DEMO diff --git a/src/stm-demo/ao_demo.c b/src/stm-demo/ao_demo.c index 5677cdf4..58cf651b 100644 --- a/src/stm-demo/ao_demo.c +++ b/src/stm-demo/ao_demo.c @@ -153,6 +153,7 @@ ao_temp (void) printf ("temp: %d\n", temp); } +#if 0 static void ao_event(void) { @@ -168,6 +169,7 @@ ao_event(void) } } +#endif __code struct ao_cmds ao_demo_cmds[] = { { ao_dma_test, "D\0DMA test" }, @@ -175,7 +177,7 @@ __code struct ao_cmds ao_demo_cmds[] = { { ao_spi_read, "R\0SPI read" }, { ao_i2c_write, "i\0I2C write" }, { ao_temp, "t\0Show temp" }, - { ao_event, "e\0Monitor event queue" }, +/* { ao_event, "e\0Monitor event queue" }, */ { 0, NULL } }; diff --git a/src/stm/ao_debounce.c b/src/stm/ao_debounce.c new file mode 100644 index 00000000..22cdf230 --- /dev/null +++ b/src/stm/ao_debounce.c @@ -0,0 +1,187 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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 +#include + +static uint8_t ao_debounce_initialized; +static uint8_t ao_debounce_running; +static struct ao_debounce *ao_debounce; + +static void +ao_debounce_on(void) +{ + stm_tim6.cr1 = ((0 << STM_TIM67_CR1_ARPE) | + (0 << STM_TIM67_CR1_OPM) | + (1 << STM_TIM67_CR1_URS) | + (0 << STM_TIM67_CR1_UDIS) | + (1 << STM_TIM67_CR1_CEN)); +} + +static void +ao_debounce_off(void) +{ + stm_tim6.cr1 = ((0 << STM_TIM67_CR1_ARPE) | + (0 << STM_TIM67_CR1_OPM) | + (1 << STM_TIM67_CR1_URS) | + (0 << STM_TIM67_CR1_UDIS) | + (0 << STM_TIM67_CR1_CEN)); +} + +static void +_ao_debounce_set(struct ao_debounce *debounce, uint8_t value) +{ + if (value != debounce->value) { + debounce->value = value; + debounce->_set(debounce, value); + } + _ao_debounce_stop(debounce); +} + +/* + * Get the current value, set the result when we've + * reached the debounce count limit + */ +static void +_ao_debounce_check(struct ao_debounce *debounce) +{ + if (debounce->_get(debounce)) { + if (debounce->count < 0) + debounce->count = 0; + if (debounce->count < debounce->hold) { + if (++debounce->count == debounce->hold) + _ao_debounce_set(debounce, 1); + } + } else { + if (debounce->count > 0) + debounce->count = 0; + if (debounce->count > -debounce->hold) { + if (--debounce->count == -debounce->hold) + _ao_debounce_set(debounce, 0); + } + } +} + +/* + * Start monitoring one pin + */ +void +_ao_debounce_start(struct ao_debounce *debounce) +{ + if (debounce->running) + return; + debounce->running = 1; + + /* Reset the counter */ + debounce->count = 0; + + /* Link into list */ + debounce->next = ao_debounce; + ao_debounce = debounce; + + /* Make sure the timer is running */ + if (!ao_debounce_running++) + ao_debounce_on(); + + /* And go check the current value */ + _ao_debounce_check(debounce); +} + +/* + * Stop monitoring one pin + */ +void +_ao_debounce_stop(struct ao_debounce *debounce) +{ + struct ao_debounce **prev; + if (!debounce->running) + return; + + debounce->running = 0; + + /* Unlink */ + for (prev = &ao_debounce; (*prev); prev = &((*prev)->next)) { + if (*prev == debounce) { + *prev = debounce->next; + break; + } + } + debounce->next = NULL; + + /* Turn off the timer if possible */ + if (!--ao_debounce_running) + ao_debounce_off(); +} + +void stm_tim6_isr(void) +{ + struct ao_debounce *debounce, *next; + if (stm_tim6.sr & (1 << STM_TIM67_SR_UIF)) { + stm_tim6.sr = 0; + + /* Walk the current list, allowing the current + * object to be removed from the list + */ + for (debounce = ao_debounce; debounce; debounce = next) { + next = debounce->next; + _ao_debounce_check(debounce); + } + } +} + +/* + * According to the STM clock-configuration, timers run + * twice as fast as the APB1 clock *if* the APB1 prescaler + * is greater than 1. + */ + +#if AO_APB1_PRESCALER > 1 +#define TIMER_23467_SCALER 2 +#else +#define TIMER_23467_SCALER 1 +#endif + +#define TIMER_100kHz ((AO_PCLK1 * TIMER_23467_SCALER) / 100000) + +void +ao_debounce_init(void) +{ + if (ao_debounce_initialized) + return; + ao_debounce_initialized = 1; + + stm_nvic_set_enable(STM_ISR_TIM6_POS); + stm_nvic_set_priority(STM_ISR_TIM6_POS, AO_STM_NVIC_CLOCK_PRIORITY); + + /* Turn on timer 6 */ + stm_rcc.apb1enr |= (1 << STM_RCC_APB1ENR_TIM6EN); + + stm_tim6.psc = TIMER_100kHz; + stm_tim6.arr = 9; + stm_tim6.cnt = 0; + + /* Enable update interrupt */ + stm_tim6.dier = (1 << STM_TIM67_DIER_UIE); + + /* Poke timer to reload values */ + stm_tim6.egr |= (1 << STM_TIM67_EGR_UG); + + stm_tim6.cr2 = (STM_TIM67_CR2_MMS_RESET << STM_TIM67_CR2_MMS); + + /* And turn it off (for now) */ + ao_debounce_off(); +} diff --git a/src/telelco-v0.2/Makefile b/src/telelco-v0.2/Makefile index cc6e62c4..42a5a7ee 100644 --- a/src/telelco-v0.2/Makefile +++ b/src/telelco-v0.2/Makefile @@ -21,6 +21,7 @@ INC = \ ao_radio_spi.h \ ao_radio_cmac.h \ ao_cc1120_CC1120.h \ + ao_debounce.h \ stm32l.h # @@ -59,6 +60,7 @@ ALTOS_SRC = \ ao_fec_tx.c \ ao_fec_rx.c \ ao_seven_segment.c \ + ao_debounce.c \ ao_quadrature.c \ ao_button.c \ ao_event.c \ diff --git a/src/telelco-v0.2/ao_lco.c b/src/telelco-v0.2/ao_lco.c index 418c0509..6e490247 100644 --- a/src/telelco-v0.2/ao_lco.c +++ b/src/telelco-v0.2/ao_lco.c @@ -114,11 +114,9 @@ ao_lco_input(void) switch (event.unit) { case AO_QUADRATURE_PAD: if (!ao_lco_armed) { - if (event.value == ao_lco_pad) - break; - dir = ((int8_t) event.value - (int8_t) ao_lco_pad) > 0 ? 1 : -1; - new_pad = event.value; - while (!ao_lco_pad_present(new_pad)) { + dir = (int8_t) event.value; + new_pad = ao_lco_pad; + do { new_pad += dir; if (new_pad > AO_PAD_MAX_CHANNELS) new_pad = 0; @@ -126,21 +124,18 @@ ao_lco_input(void) new_pad = AO_PAD_MAX_CHANNELS - 1; if (new_pad == ao_lco_pad) break; - } + } while (!ao_lco_pad_present(new_pad)); if (new_pad != ao_lco_pad) { ao_lco_pad = new_pad; - ao_quadrature_count[AO_QUADRATURE_PAD] = ao_lco_pad; ao_lco_set_pad(); } } break; case AO_QUADRATURE_BOX: if (!ao_lco_armed) { - if (event.value == ao_lco_box) - break; - dir = ((int8_t) event.value - (int8_t) ao_lco_box) > 0 ? 1 : -1; - new_box = event.value; - while (!ao_lco_box_present(new_box)) { + dir = (int8_t) event.value; + new_box = ao_lco_box; + do { new_box += dir; if (new_box > ao_lco_max_box) new_box = ao_lco_min_box; @@ -148,8 +143,7 @@ ao_lco_input(void) new_box = ao_lco_min_box; if (new_box == ao_lco_box) break; - } - ao_quadrature_count[AO_QUADRATURE_PAD] = new_box; + } while (!ao_lco_box_present(new_box)); if (ao_lco_box != new_box) { ao_lco_box = new_box; ao_lco_got_channels = 0; -- cgit v1.2.3 From 988924b51980ad43e39bc4785a625ff25eb16449 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 9 Jun 2013 22:09:13 -0700 Subject: altos: Add fast-timer API. Use for quadrature and button drivers This splits the fast-timer portion out of the debounce helper code and shares that with the quadrature driver which now uses it directly. Signed-off-by: Keith Packard --- src/core/ao.h | 1 + src/core/ao_debounce.c | 163 ++++++++++++++++++++++++++++++++++++++ src/core/ao_debounce.h | 26 +++++- src/drivers/ao_button.c | 7 +- src/drivers/ao_quadrature.c | 130 ++++++++++++++---------------- src/stm/ao_debounce.c | 187 -------------------------------------------- src/stm/ao_fast_timer.c | 120 ++++++++++++++++++++++++++++ src/stm/ao_fast_timer.h | 34 ++++++++ src/stm/stm32l.h | 5 ++ src/telelco-v0.2/Makefile | 1 + src/telelco-v0.2/ao_pins.h | 1 - 11 files changed, 411 insertions(+), 264 deletions(-) create mode 100644 src/core/ao_debounce.c delete mode 100644 src/stm/ao_debounce.c create mode 100644 src/stm/ao_fast_timer.c create mode 100644 src/stm/ao_fast_timer.h (limited to 'src/drivers') diff --git a/src/core/ao.h b/src/core/ao.h index 0886260f..caa0ec19 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -75,6 +75,7 @@ typedef AO_PORT_TYPE ao_port_t; #define AO_PANIC_CRASH 14 /* Processor crashed */ #define AO_PANIC_BUFIO 15 /* Mis-using bufio API */ #define AO_PANIC_EXTI 16 /* Mis-using exti API */ +#define AO_PANIC_FAST_TIMER 17 /* Mis-using fast timer API */ #define AO_PANIC_SELF_TEST_CC1120 0x40 | 1 /* Self test failure */ #define AO_PANIC_SELF_TEST_HMC5883 0x40 | 2 /* Self test failure */ #define AO_PANIC_SELF_TEST_MPU6000 0x40 | 3 /* Self test failure */ diff --git a/src/core/ao_debounce.c b/src/core/ao_debounce.c new file mode 100644 index 00000000..b9d67729 --- /dev/null +++ b/src/core/ao_debounce.c @@ -0,0 +1,163 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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 +#include +#include + +static uint8_t ao_debounce_initialized; +static uint8_t ao_debounce_running; +static struct ao_debounce *ao_debounce; + +static uint8_t values[64]; +static uint8_t n; + +#define d_step(n) (((n) + 1) & 63) + +static void +_ao_debounce_set(struct ao_debounce *debounce, uint8_t value) +{ + if (value != debounce->value) { + values[n] = value; + n = (n + 1) & 63; + debounce->value = value; + debounce->_set(debounce, value); + } + _ao_debounce_stop(debounce); +} + +void +ao_debounce_dump(void) +{ + uint8_t s; + + for (s = 0; s < n; s++) { + printf ("%d: %d\n", + s, values[s]); + } + n = 0; +} + +/* + * Get the current value, set the result when we've + * reached the debounce count limit + */ +static void +_ao_debounce_check(struct ao_debounce *debounce) +{ + uint8_t next = debounce->_get(debounce); + + if (next == debounce->current) { + if (debounce->count < debounce->hold) { + if (++debounce->count == debounce->hold) + _ao_debounce_set(debounce, debounce->current); + } + } else { + debounce->count = 0; + debounce->current = next; + } +} + +static void +_ao_debounce_isr(void) +{ + struct ao_debounce *debounce, *next; + + for (debounce = ao_debounce; debounce; debounce = next) { + next = debounce->next; + _ao_debounce_check(debounce); + } +} + +static void +ao_debounce_on(void) +{ + ao_fast_timer_on(_ao_debounce_isr); +} + +static void +ao_debounce_off(void) +{ + ao_fast_timer_off(_ao_debounce_isr); +} + +/* + * Start monitoring one pin + */ +void +_ao_debounce_start(struct ao_debounce *debounce) +{ + uint32_t m; + + m = ao_arch_irqsave(); + if (!debounce->running) { + debounce->running = 1; + + /* Reset the counter */ + debounce->count = 0; + + /* Link into list */ + debounce->next = ao_debounce; + ao_debounce = debounce; + + /* Make sure the timer is running */ + if (!ao_debounce_running++) + ao_debounce_on(); + + /* And go check the current value */ + _ao_debounce_check(debounce); + } + ao_arch_irqrestore(m); +} + +/* + * Stop monitoring one pin + */ +void +_ao_debounce_stop(struct ao_debounce *debounce) +{ + struct ao_debounce **prev; + uint32_t m; + + m = ao_arch_irqsave(); + if (debounce->running) { + debounce->running = 0; + + /* Unlink */ + for (prev = &ao_debounce; (*prev); prev = &((*prev)->next)) { + if (*prev == debounce) { + *prev = debounce->next; + break; + } + } + debounce->next = NULL; + + /* Turn off the timer if possible */ + if (!--ao_debounce_running) + ao_debounce_off(); + } + ao_arch_irqrestore(m); +} + +void +ao_debounce_init(void) +{ + if (ao_debounce_initialized) + return; + ao_debounce_initialized = 1; + ao_fast_timer_init(); +} diff --git a/src/core/ao_debounce.h b/src/core/ao_debounce.h index ebe290e1..19c620f5 100644 --- a/src/core/ao_debounce.h +++ b/src/core/ao_debounce.h @@ -22,13 +22,16 @@ struct ao_debounce { struct ao_debounce *next; /* time that pin value must be stable before accepting */ - int8_t hold; + uint8_t hold; /* last value reported to app; don't report it twice */ uint8_t value; + /* current value received from pins */ + uint8_t current; + /* current count of intervals pin value has been stable */ - int8_t count; + uint8_t count; /* This pin is running */ uint8_t running; @@ -40,6 +43,22 @@ struct ao_debounce { void (*_set)(struct ao_debounce *debounce, uint8_t value); }; +static inline void +ao_debounce_config(struct ao_debounce *debounce, + uint8_t (*_get)(struct ao_debounce *debounce), + void (*_set)(struct ao_debounce *debounce, uint8_t value), + uint8_t hold) +{ + debounce->next = 0; + debounce->hold = hold; + debounce->value = 0xff; + debounce->current = 0xff; + debounce->count = 0; + debounce->running = 0; + debounce->_get = _get; + debounce->_set = _set; +} + void _ao_debounce_start(struct ao_debounce *debounce); @@ -49,4 +68,7 @@ _ao_debounce_stop(struct ao_debounce *debounce); void ao_debounce_init(void); +void +ao_debounce_dump(void); + #endif /* _AO_DEBOUNCE_H_ */ diff --git a/src/drivers/ao_button.c b/src/drivers/ao_button.c index 7b1fb530..25c0cd5c 100644 --- a/src/drivers/ao_button.c +++ b/src/drivers/ao_button.c @@ -74,9 +74,10 @@ _ao_button_set(struct ao_debounce *debounce, uint8_t value) static void ao_button_debounce_init(struct ao_debounce *debounce) { - debounce->hold = AO_BUTTON_DEBOUNCE_HOLD; - debounce->_get = _ao_button_get; - debounce->_set = _ao_button_set; + ao_debounce_config(debounce, + _ao_button_get, + _ao_button_set, + AO_BUTTON_DEBOUNCE_HOLD); } static void diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c index cfa58da6..d07488d0 100644 --- a/src/drivers/ao_quadrature.c +++ b/src/drivers/ao_quadrature.c @@ -18,18 +18,12 @@ #include #include #include -#include +#include #include -#define AO_QUADRATURE_DEBOUNCE_HOLD 3 - -static __xdata struct ao_debounce ao_quadrature_debounce[AO_QUADRATURE_COUNT]; - -#define debounce_id(d) ((d) - ao_quadrature_debounce) - __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))) @@ -39,32 +33,17 @@ static uint8_t ao_quadrature_state[AO_QUADRATURE_COUNT]; #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 q_case(q) case q: v = (!ao_gpio_get(port(q), bita(q), pina(q))) | ((!ao_gpio_get(port(q), bitb(q), pinb(q))) << 1); break - -uint8_t quad_raw[64]; -uint8_t quad_r; - -static uint8_t -_ao_quadrature_get(struct ao_debounce *debounce) { - uint8_t q = debounce_id(debounce); - uint8_t v = 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); - switch (q) { -#if AO_QUADRATURE_COUNT > 0 - q_case(0); -#endif -#if AO_QUADRATURE_COUNT > 1 - q_case(1); -#endif - } - if (q == 0) { - quad_raw[quad_r] = v; - quad_r = (quad_r + 1) & 63; - } - return v; + 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_queue(uint8_t q, int8_t step) { @@ -75,37 +54,51 @@ _ao_quadrature_queue(uint8_t q, int8_t step) ao_wakeup(&ao_quadrature_count[q]); } -uint8_t quad_history[64]; -uint8_t quad_h; +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(struct ao_debounce *debounce, uint8_t value) { - uint8_t q = debounce_id(debounce); +_ao_quadrature_set(uint8_t q, uint8_t value) { + uint8_t v; - ao_quadrature_state[q] = ((ao_quadrature_state[q] & 3) << 2); - ao_quadrature_state[q] |= value; - - if (q == 0) { - quad_history[quad_h] = ao_quadrature_state[0]; - quad_h = (quad_h + 1) & 63; - } + v = ao_quadrature_state[q] & 3; + value = value & 3; - switch (ao_quadrature_state[q]) { - case STATE(0, 1, 0, 0): - _ao_quadrature_queue(q, 1); - break; - case STATE(1, 0, 0, 0): - _ao_quadrature_queue(q, -1); - break; + 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) { - uint8_t q; - for (q = 0; q < AO_QUADRATURE_COUNT; q++) - _ao_debounce_start(&ao_quadrature_debounce[q]); + _ao_quadrature_set(0, _ao_quadrature_get(0)); + _ao_quadrature_set(1, _ao_quadrature_get(1)); } int32_t @@ -130,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(); @@ -138,6 +140,7 @@ ao_quadrature_test(void) if (c == 100) break; } +#endif } static const struct ao_cmds ao_quadrature_cmds[] = { @@ -145,36 +148,21 @@ static const struct ao_cmds ao_quadrature_cmds[] = { { 0, NULL } }; -static void -ao_quadrature_debounce_init(struct ao_debounce *debounce) { - debounce->hold = AO_QUADRATURE_DEBOUNCE_HOLD; - debounce->_get = _ao_quadrature_get; - debounce->_set = _ao_quadrature_set; -} - -#define init(q) do { \ - ao_enable_port(port(q)); \ - ao_quadrature_debounce_init(&ao_quadrature_debounce[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 ao_quadrature_init(void) { - ao_debounce_init(); #if AO_QUADRATURE_COUNT > 0 init(0); #endif #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/stm/ao_debounce.c b/src/stm/ao_debounce.c deleted file mode 100644 index 22cdf230..00000000 --- a/src/stm/ao_debounce.c +++ /dev/null @@ -1,187 +0,0 @@ -/* - * Copyright © 2013 Keith Packard - * - * 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 -#include - -static uint8_t ao_debounce_initialized; -static uint8_t ao_debounce_running; -static struct ao_debounce *ao_debounce; - -static void -ao_debounce_on(void) -{ - stm_tim6.cr1 = ((0 << STM_TIM67_CR1_ARPE) | - (0 << STM_TIM67_CR1_OPM) | - (1 << STM_TIM67_CR1_URS) | - (0 << STM_TIM67_CR1_UDIS) | - (1 << STM_TIM67_CR1_CEN)); -} - -static void -ao_debounce_off(void) -{ - stm_tim6.cr1 = ((0 << STM_TIM67_CR1_ARPE) | - (0 << STM_TIM67_CR1_OPM) | - (1 << STM_TIM67_CR1_URS) | - (0 << STM_TIM67_CR1_UDIS) | - (0 << STM_TIM67_CR1_CEN)); -} - -static void -_ao_debounce_set(struct ao_debounce *debounce, uint8_t value) -{ - if (value != debounce->value) { - debounce->value = value; - debounce->_set(debounce, value); - } - _ao_debounce_stop(debounce); -} - -/* - * Get the current value, set the result when we've - * reached the debounce count limit - */ -static void -_ao_debounce_check(struct ao_debounce *debounce) -{ - if (debounce->_get(debounce)) { - if (debounce->count < 0) - debounce->count = 0; - if (debounce->count < debounce->hold) { - if (++debounce->count == debounce->hold) - _ao_debounce_set(debounce, 1); - } - } else { - if (debounce->count > 0) - debounce->count = 0; - if (debounce->count > -debounce->hold) { - if (--debounce->count == -debounce->hold) - _ao_debounce_set(debounce, 0); - } - } -} - -/* - * Start monitoring one pin - */ -void -_ao_debounce_start(struct ao_debounce *debounce) -{ - if (debounce->running) - return; - debounce->running = 1; - - /* Reset the counter */ - debounce->count = 0; - - /* Link into list */ - debounce->next = ao_debounce; - ao_debounce = debounce; - - /* Make sure the timer is running */ - if (!ao_debounce_running++) - ao_debounce_on(); - - /* And go check the current value */ - _ao_debounce_check(debounce); -} - -/* - * Stop monitoring one pin - */ -void -_ao_debounce_stop(struct ao_debounce *debounce) -{ - struct ao_debounce **prev; - if (!debounce->running) - return; - - debounce->running = 0; - - /* Unlink */ - for (prev = &ao_debounce; (*prev); prev = &((*prev)->next)) { - if (*prev == debounce) { - *prev = debounce->next; - break; - } - } - debounce->next = NULL; - - /* Turn off the timer if possible */ - if (!--ao_debounce_running) - ao_debounce_off(); -} - -void stm_tim6_isr(void) -{ - struct ao_debounce *debounce, *next; - if (stm_tim6.sr & (1 << STM_TIM67_SR_UIF)) { - stm_tim6.sr = 0; - - /* Walk the current list, allowing the current - * object to be removed from the list - */ - for (debounce = ao_debounce; debounce; debounce = next) { - next = debounce->next; - _ao_debounce_check(debounce); - } - } -} - -/* - * According to the STM clock-configuration, timers run - * twice as fast as the APB1 clock *if* the APB1 prescaler - * is greater than 1. - */ - -#if AO_APB1_PRESCALER > 1 -#define TIMER_23467_SCALER 2 -#else -#define TIMER_23467_SCALER 1 -#endif - -#define TIMER_100kHz ((AO_PCLK1 * TIMER_23467_SCALER) / 100000) - -void -ao_debounce_init(void) -{ - if (ao_debounce_initialized) - return; - ao_debounce_initialized = 1; - - stm_nvic_set_enable(STM_ISR_TIM6_POS); - stm_nvic_set_priority(STM_ISR_TIM6_POS, AO_STM_NVIC_CLOCK_PRIORITY); - - /* Turn on timer 6 */ - stm_rcc.apb1enr |= (1 << STM_RCC_APB1ENR_TIM6EN); - - stm_tim6.psc = TIMER_100kHz; - stm_tim6.arr = 9; - stm_tim6.cnt = 0; - - /* Enable update interrupt */ - stm_tim6.dier = (1 << STM_TIM67_DIER_UIE); - - /* Poke timer to reload values */ - stm_tim6.egr |= (1 << STM_TIM67_EGR_UG); - - stm_tim6.cr2 = (STM_TIM67_CR2_MMS_RESET << STM_TIM67_CR2_MMS); - - /* And turn it off (for now) */ - ao_debounce_off(); -} diff --git a/src/stm/ao_fast_timer.c b/src/stm/ao_fast_timer.c new file mode 100644 index 00000000..d61b40c9 --- /dev/null +++ b/src/stm/ao_fast_timer.c @@ -0,0 +1,120 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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 +#include + +static void (*ao_fast_timer_callback[AO_FAST_TIMER_MAX])(void); +static uint8_t ao_fast_timer_count; +static uint8_t ao_fast_timer_users; + +static void +ao_fast_timer_enable(void) +{ + stm_tim6.cr1 = ((0 << STM_TIM67_CR1_ARPE) | + (0 << STM_TIM67_CR1_OPM) | + (1 << STM_TIM67_CR1_URS) | + (0 << STM_TIM67_CR1_UDIS) | + (1 << STM_TIM67_CR1_CEN)); +} + +static void +ao_fast_timer_disable(void) +{ + stm_tim6.cr1 = ((0 << STM_TIM67_CR1_ARPE) | + (0 << STM_TIM67_CR1_OPM) | + (1 << STM_TIM67_CR1_URS) | + (0 << STM_TIM67_CR1_UDIS) | + (0 << STM_TIM67_CR1_CEN)); +} + +void +ao_fast_timer_on(void (*callback)(void)) +{ + ao_fast_timer_callback[ao_fast_timer_count] = callback; + if (!ao_fast_timer_count++) + ao_fast_timer_enable(); +} + +void +ao_fast_timer_off(void (*callback)(void)) +{ + uint8_t n; + + for (n = 0; n < ao_fast_timer_count; n++) + if (ao_fast_timer_callback[n] == callback) { + for (; n < ao_fast_timer_count-1; n++) { + ao_fast_timer_callback[n] = ao_fast_timer_callback[n+1]; + } + if (!--ao_fast_timer_count) + ao_fast_timer_disable(); + break; + } +} + +void stm_tim6_isr(void) +{ + uint8_t i; + if (stm_tim6.sr & (1 << STM_TIM67_SR_UIF)) { + stm_tim6.sr = 0; + + for (i = 0; i < ao_fast_timer_count; i++) + (*ao_fast_timer_callback[i])(); + } +} + +/* + * According to the STM clock-configuration, timers run + * twice as fast as the APB1 clock *if* the APB1 prescaler + * is greater than 1. + */ + +#if AO_APB1_PRESCALER > 1 +#define TIMER_23467_SCALER 2 +#else +#define TIMER_23467_SCALER 1 +#endif + +#define TIMER_10kHz ((AO_PCLK1 * TIMER_23467_SCALER) / 10000) + +void +ao_fast_timer_init(void) +{ + if (!ao_fast_timer_users) { + stm_nvic_set_enable(STM_ISR_TIM6_POS); + stm_nvic_set_priority(STM_ISR_TIM6_POS, AO_STM_NVIC_CLOCK_PRIORITY); + + /* Turn on timer 6 */ + stm_rcc.apb1enr |= (1 << STM_RCC_APB1ENR_TIM6EN); + + stm_tim6.psc = TIMER_10kHz; + stm_tim6.arr = 9; + stm_tim6.cnt = 0; + + /* Enable update interrupt */ + stm_tim6.dier = (1 << STM_TIM67_DIER_UIE); + + /* Poke timer to reload values */ + stm_tim6.egr |= (1 << STM_TIM67_EGR_UG); + + stm_tim6.cr2 = (STM_TIM67_CR2_MMS_RESET << STM_TIM67_CR2_MMS); + ao_fast_timer_disable(); + } + if (ao_fast_timer_users == AO_FAST_TIMER_MAX) + ao_panic(AO_PANIC_FAST_TIMER); + ao_fast_timer_users++; +} + diff --git a/src/stm/ao_fast_timer.h b/src/stm/ao_fast_timer.h new file mode 100644 index 00000000..90fb3930 --- /dev/null +++ b/src/stm/ao_fast_timer.h @@ -0,0 +1,34 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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_FAST_TIMER_H_ +#define _AO_FAST_TIMER_H_ + +void +ao_fast_timer_init(void); + +#ifndef AO_FAST_TIMER_MAX +#define AO_FAST_TIMER_MAX 2 +#endif + +void +ao_fast_timer_on(void (*callback)(void)); + +void +ao_fast_timer_off(void (*callback)(void)); + +#endif /* _AO_FAST_TIMER_H_ */ diff --git a/src/stm/stm32l.h b/src/stm/stm32l.h index 63bde0f8..1868468f 100644 --- a/src/stm/stm32l.h +++ b/src/stm/stm32l.h @@ -171,6 +171,11 @@ stm_gpio_get(struct stm_gpio *gpio, int pin) { return (gpio->idr >> pin) & 1; } +static inline uint16_t +stm_gpio_get_all(struct stm_gpio *gpio) { + return gpio->idr; +} + extern struct stm_gpio stm_gpioa; extern struct stm_gpio stm_gpiob; extern struct stm_gpio stm_gpioc; diff --git a/src/telelco-v0.2/Makefile b/src/telelco-v0.2/Makefile index 42a5a7ee..f78ca9e2 100644 --- a/src/telelco-v0.2/Makefile +++ b/src/telelco-v0.2/Makefile @@ -50,6 +50,7 @@ ALTOS_SRC = \ ao_beep_stm.c \ ao_storage.c \ ao_eeprom_stm.c \ + ao_fast_timer.c \ ao_lcd_stm.c \ ao_usb_stm.c \ ao_exti_stm.c \ diff --git a/src/telelco-v0.2/ao_pins.h b/src/telelco-v0.2/ao_pins.h index 07ea1b45..d86782f3 100644 --- a/src/telelco-v0.2/ao_pins.h +++ b/src/telelco-v0.2/ao_pins.h @@ -235,7 +235,6 @@ */ #define AO_QUADRATURE_COUNT 2 -#define AO_QUADRATURE_MODE 0 #define AO_QUADRATURE_0_PORT &stm_gpioe #define AO_QUADRATURE_0_A 3 -- cgit v1.2.3 From 2e2f3f2556e714833d8b7d0f65877b07b3dc2cb5 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 16 Jun 2013 22:32:16 -0700 Subject: altos: Declare m25 write-in-progress as 'ao_port_t' This lets us use port bits greater than 7 for M25 chip selects Signed-off-by: Keith Packard --- src/drivers/ao_m25.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_m25.c b/src/drivers/ao_m25.c index 9b768012..e6c7bb4d 100644 --- a/src/drivers/ao_m25.c +++ b/src/drivers/ao_m25.c @@ -86,7 +86,7 @@ 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; @@ -250,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; -- cgit v1.2.3 From d90c2fa650de4cdb008d5e2559463c08da8db934 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 19 Jun 2013 22:44:16 -0700 Subject: altos: PCA9922 LED driver needs Enable driven low to latch values Driving Enable high means anything going past on the clock and data pair is reflected on the LEDs, which isn't terribly useful Signed-off-by: Keith Packard --- src/drivers/ao_pca9922.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_pca9922.c b/src/drivers/ao_pca9922.c index 6d8d18d8..fe070b88 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_mutex_get(&ao_spi_mutex); + ao_spi_set_speed(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_mutex_put(&ao_spi_mutex); } void -- cgit v1.2.3 From e9e713bc8ab2080d5c1c38570b112f13c886bd11 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 19 Jun 2013 22:45:54 -0700 Subject: altos/telefire: Radio status (no data, weak data, good data) on LEDs Instead of blinking RX/TX, report the radio status on the telefire nodes, just like telelco does. This makes the LEDs on telefire *exactly the same* as the LEDs on telelco, which seems like a good idea. Signed-off-by: Keith Packard --- src/drivers/ao_pad.c | 18 +++++++++++++----- src/telefire-v0.2/ao_pins.h | 11 ++++------- 2 files changed, 17 insertions(+), 12 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_pad.c b/src/drivers/ao_pad.c index e205f99b..62ae68e9 100644 --- a/src/drivers/ao_pad.c +++ b/src/drivers/ao_pad.c @@ -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; } @@ -238,15 +246,15 @@ 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(); ao_pad_box = ao_pad_read_box(); diff --git a/src/telefire-v0.2/ao_pins.h b/src/telefire-v0.2/ao_pins.h index f4050722..96e6b066 100644 --- a/src/telefire-v0.2/ao_pins.h +++ b/src/telefire-v0.2/ao_pins.h @@ -40,13 +40,10 @@ #define AO_LED_CONTINUITY(c) (1 << (c)) #define AO_LED_CONTINUITY_MASK (0xf) -#define AO_LED_RX 0x10 -#define AO_LED_TX 0x20 -#define AO_LED_ARMED 0x40 -#define AO_LED_POWER 0x80 - -#define AO_LED_RED AO_LED_TX -#define AO_LED_GREEN AO_LED_RX +#define AO_LED_ARMED 0x10 +#define AO_LED_RED 0x20 +#define AO_LED_AMBER 0x40 +#define AO_LED_GREEN 0x80 #define LEDS_AVAILABLE (0xff) #define HAS_EXTERNAL_TEMP 0 -- cgit v1.2.3 From d95a2c5d1ddce913dcb1d1ab5dc59f6a588ab599 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 24 Jun 2013 14:29:43 -0700 Subject: altos: Remove ao_radio_gpio_bits from normal build Only needed for the CC115L_TRACE code, and it only builds on STM Signed-off-by: Keith Packard --- src/drivers/ao_cc115l.c | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/drivers') diff --git a/src/drivers/ao_cc115l.c b/src/drivers/ao_cc115l.c index 05e6a762..56afc3c7 100644 --- a/src/drivers/ao_cc115l.c +++ b/src/drivers/ao_cc115l.c @@ -655,12 +655,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) -- cgit v1.2.3 From 9a22a300009679a14d66214a5d61e9e6a177279f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 17 Aug 2013 17:40:33 +0200 Subject: altos: Allow ublox to run at other baud rates Provides a configuration option to set the ublox serial baud rate to something other than 57600 baud Signed-off-by: Keith Packard --- src/drivers/ao_gps_ublox.c | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index 80869561..fa6ff0e0 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -26,11 +26,21 @@ __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"; +#ifndef AO_SERIAL_SPEED_UBLOX +#define AO_SERIAL_SPEED_UBLOX AO_SERIAL_SPEED_57600 +#endif -const char ao_gps_config[] = { +#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_57600 +#define SERIAL_SPEED_STRING "57600" +#endif +#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_19200 +#define SERIAL_SPEED_STRING "19200" +#endif +#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_9600 +#define SERIAL_SPEED_STRING "9600" +#endif -}; +static const char ao_gps_set_nmea[] = "\r\n$PUBX,41,1,3,1," SERIAL_SPEED_STRING ",0*2d\r\n"; struct ao_ublox_cksum { uint8_t a, b; @@ -419,10 +429,12 @@ ao_gps_setup(void) for (i = 0; i < sizeof (ao_gps_set_nmea); i++) ao_gps_putchar(ao_gps_set_nmea[i]); +#if AO_SERIAL_SPEED_UBLOX != AO_SERIAL_SPEED_9600 /* * Increase the baud rate */ - ao_gps_set_speed(AO_SERIAL_SPEED_57600); + ao_gps_set_speed(AO_SERIAL_SPEED_UBLOX); +#endif /* * Pad with nulls to give the chip -- cgit v1.2.3 From e2f385946132690ca6dc141d7c7830ae0cfe3458 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 20 Aug 2013 08:54:44 -0700 Subject: altos: various cc115l driver hacks Try to recover from TX_FIFO_UNDERFLOW by resetting the chip at idle time. Do a calibration phase during setup. Program power to ramp up to limit key down noise. Signed-off-by: Keith Packard --- src/drivers/ao_cc115l.c | 37 ++++++++++++++++++++++++++----------- 1 file changed, 26 insertions(+), 11 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_cc115l.c b/src/drivers/ao_cc115l.c index 56afc3c7..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) { @@ -740,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) { @@ -786,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; } } -- cgit v1.2.3 From b363a628fc6137c3395a48ef13de7a799ec3e2c3 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 22 May 2013 19:31:15 -0600 Subject: altos: MS5607 pressure computation for low temperatures was wrong MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Second correction only applies to temps < -15°C, not 15°C. Signed-off-by: Keith Packard --- src/drivers/ao_ms5607_convert.c | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) (limited to 'src/drivers') 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; -- cgit v1.2.3 From d0b4e926ecececa7499a301b6135189be119512e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 22 May 2013 13:03:06 -0700 Subject: Initial TeleMini bits Signed-off-by: Keith Packard --- src/cc1111/ao_adc.c | 5 ++ src/cc1111/ao_usb.c | 5 ++ src/core/ao_data.c | 2 + src/core/ao_data.h | 2 +- src/core/ao_log_mini.c | 1 - src/core/ao_telemetry.c | 40 +++++++++- src/core/ao_telemetry.h | 27 +++++++ src/drivers/ao_ms5607.c | 40 +++++----- src/drivers/ao_ms5607.h | 2 +- src/drivers/ao_ms5607_convert_8051.c | 60 +++++++++++++++ src/telemini-v2.0/Makefile | 108 +++++++++++++++++++++++++++ src/telemini-v2.0/ao_pins.h | 140 +++++++++++++++++++++++++++++++++++ src/telemini-v2.0/ao_telemini.c | 54 ++++++++++++++ src/test/Makefile | 5 +- 14 files changed, 466 insertions(+), 25 deletions(-) create mode 100644 src/drivers/ao_ms5607_convert_8051.c create mode 100644 src/telemini-v2.0/Makefile create mode 100644 src/telemini-v2.0/ao_pins.h create mode 100644 src/telemini-v2.0/ao_telemini.c (limited to 'src/drivers') diff --git a/src/cc1111/ao_adc.c b/src/cc1111/ao_adc.c index 4a58023d..6cc08399 100644 --- a/src/cc1111/ao_adc.c +++ b/src/cc1111/ao_adc.c @@ -162,6 +162,11 @@ ao_adc_isr(void) __interrupt 1 #define GOT_ADC #endif +#ifdef FETCH_ADC + FETCH_ADC() +#define GOT_ADC +#endif + #ifndef GOT_ADC #error No known ADC configuration set #endif diff --git a/src/cc1111/ao_usb.c b/src/cc1111/ao_usb.c index a655d1be..b0ab409d 100644 --- a/src/cc1111/ao_usb.c +++ b/src/cc1111/ao_usb.c @@ -201,6 +201,11 @@ ao_usb_ep0_setup(void) ao_usb_ep0_queue_byte(0); break; case AO_USB_REQ_SET_ADDRESS: +#if USB_FORCE_FLIGHT_IDLE + /* Go to idle mode if USB is connected + */ + ao_flight_force_idle = 1; +#endif ao_usb_set_address(ao_usb_setup.value); break; case AO_USB_REQ_GET_DESCRIPTOR: diff --git a/src/core/ao_data.c b/src/core/ao_data.c index 38d2f7ff..6a3d02a1 100644 --- a/src/core/ao_data.c +++ b/src/core/ao_data.c @@ -22,6 +22,7 @@ volatile __xdata struct ao_data ao_data_ring[AO_DATA_RING]; volatile __data uint8_t ao_data_head; volatile __data uint8_t ao_data_present; +#ifndef ao_data_count void ao_data_get(__xdata struct ao_data *packet) { @@ -32,3 +33,4 @@ ao_data_get(__xdata struct ao_data *packet) #endif memcpy(packet, (void *) &ao_data_ring[i], sizeof (struct ao_data)); } +#endif diff --git a/src/core/ao_data.h b/src/core/ao_data.h index b0f086f8..080a534f 100644 --- a/src/core/ao_data.h +++ b/src/core/ao_data.h @@ -101,7 +101,7 @@ extern volatile __data uint8_t ao_data_count; * signaled by the timer tick */ #define AO_DATA_WAIT() do { \ - ao_sleep((void *) &ao_data_count); \ + ao_sleep(DATA_TO_XDATA ((void *) &ao_data_count)); \ } while (0) #endif /* AO_DATA_RING */ diff --git a/src/core/ao_log_mini.c b/src/core/ao_log_mini.c index 1273b0e3..46b285f3 100644 --- a/src/core/ao_log_mini.c +++ b/src/core/ao_log_mini.c @@ -79,7 +79,6 @@ void ao_log(void) { __pdata uint16_t next_sensor, next_other; - uint8_t i; ao_storage_setup(); diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c index 03a8a273..9c673030 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -171,6 +171,36 @@ ao_send_mega_data(void) } #endif /* AO_SEND_MEGA */ +#ifdef AO_SEND_MINI + +static void +ao_send_mini(void) +{ + __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; + + telemetry.generic.tick = packet->tick; + telemetry.generic.type = AO_TELEMETRY_MINI; + + telemetry.mini.state = ao_flight_state; + + telemetry.mini.v_batt = packet->adc.v_batt; + telemetry.mini.sense_a = packet->adc.sense_a; + telemetry.mini.sense_m = packet->adc.sense_m; + + telemetry.mini.pres = ao_data_pres(packet); + telemetry.mini.temp = ao_data_temp(packet); + + telemetry.mini.acceleration = ao_accel; + telemetry.mini.speed = ao_speed; + telemetry.mini.height = ao_height; + + telemetry.mini.ground_pres = ao_ground_pres; + + ao_radio_send(&telemetry, sizeof (telemetry)); +} + +#endif + #ifdef AO_SEND_ALL_BARO static uint8_t ao_baro_sample; @@ -323,12 +353,16 @@ ao_telemetry(void) ao_send_baro(); #endif #if HAS_FLIGHT -#ifdef AO_SEND_MEGA +# ifdef AO_SEND_MEGA ao_send_mega_sensor(); ao_send_mega_data(); -#else +# else +# ifdef AO_SEND_MINI + ao_send_mini(); +# else ao_send_sensor(); -#endif +# endif +# endif #endif #if HAS_COMPANION diff --git a/src/core/ao_telemetry.h b/src/core/ao_telemetry.h index f2d201de..77601529 100644 --- a/src/core/ao_telemetry.h +++ b/src/core/ao_telemetry.h @@ -207,6 +207,32 @@ struct ao_telemetry_mega_data { }; +#define AO_TELEMETRY_MINI 0x10 + +struct ao_telemetry_mini { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + + uint8_t state; /* 5 flight state */ + + int16_t v_batt; /* 6 battery voltage */ + int16_t sense_a; /* 8 apogee continuity */ + int16_t sense_m; /* 10 main continuity */ + + int32_t pres; /* 12 Pa * 10 */ + int16_t temp; /* 16 °C * 100 */ + + int16_t acceleration; /* 18 m/s² * 16 */ + int16_t speed; /* 20 m/s * 16 */ + int16_t height; /* 22 m */ + + int32_t ground_pres; /* 24 average pres on pad */ + + int32_t pad28; /* 28 */ + /* 32 */ +}; + /* #define AO_SEND_ALL_BARO */ #define AO_TELEMETRY_BARO 0x80 @@ -240,6 +266,7 @@ union ao_telemetry_all { struct ao_telemetry_companion companion; struct ao_telemetry_mega_sensor mega_sensor; struct ao_telemetry_mega_data mega_data; + struct ao_telemetry_mini mini; struct ao_telemetry_baro baro; }; diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c index 8f1dcbe1..5259b265 100644 --- a/src/drivers/ao_ms5607.c +++ b/src/drivers/ao_ms5607.c @@ -21,7 +21,7 @@ #if HAS_MS5607 || HAS_MS5611 -static struct ao_ms5607_prom ms5607_prom; +static __xdata struct ao_ms5607_prom ms5607_prom; static uint8_t ms5607_configured; static void @@ -40,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(); } @@ -69,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++; @@ -114,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); @@ -179,10 +179,14 @@ ao_ms5607_sample(struct ao_ms5607_sample *sample) 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) @@ -191,10 +195,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(); } } diff --git a/src/drivers/ao_ms5607.h b/src/drivers/ao_ms5607.h index b2f98a59..3fd43fd4 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); diff --git a/src/drivers/ao_ms5607_convert_8051.c b/src/drivers/ao_ms5607_convert_8051.c new file mode 100644 index 00000000..f47972c9 --- /dev/null +++ b/src/drivers/ao_ms5607_convert_8051.c @@ -0,0 +1,60 @@ +/* + * Copyright © 2012 Keith Packard + * + * 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 + +void +ao_ms5607_convert(struct ao_ms5607_sample *sample, struct ao_ms5607_value *value) +{ +#if 0 + int32_t dT; + int32_t TEMP; + int64_t OFF; + int64_t SENS; + + dT = sample->temp - ((int32_t) ms5607_prom.tref << 8); + + TEMP = 2000 + (((int64_t) dT * ms5607_prom.tempsens) >> 23); + +#if HAS_MS5611 + OFF = ((int64_t) ms5607_prom.off << 16) + (((int64_t) ms5607_prom.tco * dT) >> 7); + SENS = ((int64_t) ms5607_prom.sens << 15) + (((int64_t) ms5607_prom.tcs * dT) >> 8); +#else + OFF = ((int64_t) ms5607_prom.off << 17) + (((int64_t) ms5607_prom.tco * dT) >> 6); + SENS = ((int64_t) ms5607_prom.sens << 16) + (((int64_t) ms5607_prom.tcs * dT) >> 7); +#endif + + if (TEMP < 2000) { + int32_t T2 = ((int64_t) dT * (int64_t) dT) >> 31; + 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) { + int32_t TEMPP = TEMP + 1500; + int64_t TEMPP2 = TEMPP * TEMPP; + OFF2 = OFF2 + 15 * TEMPP2; + SENS2 = SENS2 + 8 * TEMPP2; + } + TEMP -= T2; + OFF -= OFF2; + SENS -= SENS2; + } + + value->pres = ((((int64_t) sample->pres * SENS) >> 21) - OFF) >> 15; + value->temp = TEMP; +#endif +} diff --git a/src/telemini-v2.0/Makefile b/src/telemini-v2.0/Makefile new file mode 100644 index 00000000..afd529f0 --- /dev/null +++ b/src/telemini-v2.0/Makefile @@ -0,0 +1,108 @@ +# +# TeleMini build file +# + +TELEMINI_VER=2.0 +TELEMINI_DEF=2_0 + +vpath %.c ..:../core:../cc1111:../drivers:../product +vpath %.h ..:../core:../cc1111:../drivers:../product +vpath ao-make-product.5c ../util + +ifndef VERSION +include ../Version +endif + +INC = \ + ao.h \ + ao_pins.h \ + ao_arch.h \ + ao_arch_funcs.h \ + cc1111.h \ + ao_product.h + +CORE_SRC = \ + ao_cmd.c \ + ao_config.c \ + ao_convert.c \ + ao_flight.c \ + ao_kalman.c \ + ao_log.c \ + ao_log_mini.c \ + ao_mutex.c \ + ao_panic.c \ + ao_report.c \ + ao_sample.c \ + ao_stdio.c \ + ao_storage.c \ + ao_task.c \ + ao_telemetry.c \ + ao_freq.c + +CC1111_SRC = \ + ao_adc.c \ + ao_dma.c \ + ao_ignite.c \ + ao_led.c \ + ao_packet.c \ + ao_packet_slave.c \ + ao_radio.c \ + ao_romconfig.c \ + ao_string.c \ + ao_spi.c \ + ao_usb.c \ + ao_convert_pa.c \ + ao_data.c \ + ao_beep.c \ + ao_timer.c \ + _bp.c + +DRIVER_SRC = \ + ao_ms5607.c \ + ao_m25.c + +PRODUCT_SRC = \ + ao_telemini.c + +SRC = \ + $(CORE_SRC) \ + $(CC1111_SRC) \ + $(DRIVER_SRC) \ + $(PRODUCT_SRC) + +PROGNAME = telemini-v$(TELEMINI_VER) +PROG = $(PROGNAME)-$(VERSION).ihx +PRODUCT=TeleMini-v$(TELEMINI_VER) +PRODUCT_DEF=-DTELEMINI_V_$(TELEMINI_DEF) +IDPRODUCT=0x000a + +include ../cc1111/Makefile.cc1111 + +NICKLE=nickle +CHECK_STACK=sh ../util/check-stack + +V=0 +# The user has explicitly enabled quiet compilation. +ifeq ($(V),0) +quiet = @printf " $1 $2 $@\n"; $($1) +endif +# Otherwise, print the full command line. +quiet ?= $($1) + +all: ../$(PROG) + +../$(PROG): $(REL) Makefile + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) .. + $(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@ + +ao_product.h: ao-make-product.5c ../Version + $(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@ + +distclean: clean + +clean: clean-cc1111 + +install: + +uninstall: + diff --git a/src/telemini-v2.0/ao_pins.h b/src/telemini-v2.0/ao_pins.h new file mode 100644 index 00000000..386c8dc3 --- /dev/null +++ b/src/telemini-v2.0/ao_pins.h @@ -0,0 +1,140 @@ +/* + * Copyright © 2010 Keith Packard + * + * 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_PINS_H_ +#define _AO_PINS_H_ + +#define HAS_RADIO 1 + +#define HAS_FLIGHT 1 +#define HAS_USB 1 +#define USB_FORCE_FLIGHT_IDLE 1 +#define HAS_BEEP 1 +#define HAS_GPS 0 +#define HAS_SERIAL_1 0 +#define HAS_EEPROM 1 +#define HAS_LOG 1 +#define USE_INTERNAL_FLASH 0 +#define HAS_DBG 0 +#define PACKET_HAS_SLAVE 1 +#define USE_FAST_ASCENT_LOG 1 + +#define AO_LED_GREEN 1 +#define AO_LED_RED 2 +#define LEDS_AVAILABLE (AO_LED_RED|AO_LED_GREEN) +#define HAS_EXTERNAL_TEMP 0 +#define HAS_ACCEL 0 +#define HAS_IGNITE 1 +#define HAS_IGNITE_REPORT 1 +#define HAS_MONITOR 0 + +/* + * SPI + */ + +#define SPI_CS_PORT P1 +#define SPI_CS_SEL P1SEL +#define SPI_CS_DIR P1DIR + +/* + * Flash + */ +#define AO_M25_SPI_CS_PORT SPI_CS_PORT +#define AO_M25_SPI_CS_MASK 0x04 /* cs_flash is P1_2 */ +#define M25_MAX_CHIPS 1 + +/* + * MS5607 + */ + +#define HAS_MS5607 1 +#define HAS_MS5611 0 +#define AO_MS5607_PRIVATE_PINS 0 +#define AO_MS5607_CS_PORT P1 +#define AO_MS5607_CS_PIN 3 +#define AO_MS5607_CS P1_3 +#define AO_MS5607_CS_MASK (1 << AO_MS5607_CS_PIN) +#define AO_MS5607_MISO_PORT P0 +#define AO_MS5607_MISO_PIN 2 +#define AO_MS5607_MISO_MASK (1 << AO_MS5607_MISO_PIN) +#define AO_MS5607_SPI_INDEX 0 + +/* + * Igniters + */ +#define AO_IGNITER_PORT P2 +#define AO_IGNITER_DROGUE_PORT AO_IGNITER_PORT +#define AO_IGNITER_DROGUE P2_3 +#define AO_IGNITER_MAIN P2_4 +#define AO_IGNITER_DIR P2DIR +#define AO_IGNITER_DROGUE_BIT (1 << 3) +#define AO_IGNITER_MAIN_BIT (1 << 4) +#define AO_IGNITER_DROGUE_PIN 3 +#define AO_IGNITER_MAIN_PIN 4 + +#define AO_IGNITER_DROGUE_PORT AO_IGNITER_PORT +#define AO_IGNITER_MAIN_PORT AO_IGNITER_PORT + +/* test these values with real igniters */ +#define AO_IGNITER_OPEN 1000 +#define AO_IGNITER_CLOSED 7000 +#define AO_IGNITER_FIRE_TIME AO_MS_TO_TICKS(50) +#define AO_IGNITER_CHARGE_TIME AO_MS_TO_TICKS(2000) + +#define AO_SEND_MINI + +/* + * ADC + */ + +#define HAS_ADC 1 +#define AO_ADC_FIRST_PIN 0 + +struct ao_adc { + int16_t sense_a; /* apogee continuity sense */ + int16_t sense_m; /* main continuity sense */ + int16_t v_batt; /* battery voltage */ +}; + +#define ao_data_count ao_adc_count + +#define AO_SENSE_DROGUE(p) ((p)->adc.sense_a) +#define AO_SENSE_MAIN(p) ((p)->adc.sense_m) + +#define AO_ADC_DUMP(p) \ + printf("tick: %5u apogee: %5d main: %5d batt: %5d\n", \ + (p)->tick, (p)->adc.sense_a, (p)->adc.sense_m, (p)->adc.v_batt) + +#define FETCH_ADC() \ + a = (uint8_t __xdata *) (&ao_data_ring[ao_data_head].adc); \ + switch (sequence) { \ + case 4: \ + a += 4; \ + sequence = 0; \ + break; \ + case 1: \ + a += 2; \ + sequence = 4; \ + break; \ + case 0: \ + sequence = 1; \ + break; \ + } \ + if (sequence) \ + ; + +#endif /* _AO_PINS_H_ */ diff --git a/src/telemini-v2.0/ao_telemini.c b/src/telemini-v2.0/ao_telemini.c new file mode 100644 index 00000000..84df2b8c --- /dev/null +++ b/src/telemini-v2.0/ao_telemini.c @@ -0,0 +1,54 @@ +/* + * Copyright © 2011 Keith Packard + * + * 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_pins.h" + +__xdata uint8_t ao_force_freq; + +void +main(void) +{ + /* + * Reduce the transient on the ignite pins at startup by + * pulling the pins low as soon as possible at power up + */ + ao_ignite_set_pins(); + + ao_clock_init(); + + /* Turn on the red LED until the system is stable */ + ao_led_init(LEDS_AVAILABLE); + ao_led_on(AO_LED_RED); + + ao_task_init(); + + ao_timer_init(); + ao_adc_init(); + ao_cmd_init(); + ao_storage_init(); + ao_ms5607_init(); + ao_flight_init(); + ao_log_init(); + ao_report_init(); + ao_telemetry_init(); + ao_radio_init(); + ao_packet_slave_init(TRUE); + ao_igniter_init(); + ao_config_init(); + ao_start_scheduler(); +} diff --git a/src/test/Makefile b/src/test/Makefile index 75b1f848..9c304318 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -2,7 +2,7 @@ vpath % ..:../core:../drivers:../util:../micropeak:../aes PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_flight_test_mm \ ao_gps_test ao_gps_test_skytraq ao_gps_test_ublox ao_convert_test ao_convert_pa_test ao_fec_test \ - ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test + ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h @@ -73,3 +73,6 @@ ao_fat_test: ao_fat_test.c ao_fat.c ao_bufio.c ao_aes_test: ao_aes_test.c ao_aes.c ao_aes_tables.c cc $(CFLAGS) -o $@ ao_aes_test.c + +ao_int64_test: ao_int64_test.c ao_int64.c ao_int64.h + cc $(CFLAGS) -o $@ ao_int64_test.c -- cgit v1.2.3 From 56911f27376b0fe91a464e369bb8aa1531b3c7dc Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 23 May 2013 02:17:51 -0600 Subject: altos: Make TeleMini v2.0 fit Mash lots of storage locations and code around to shrink stuff down to size Signed-off-by: Keith Packard --- src/core/ao.h | 2 +- src/core/ao_cmd.c | 4 +- src/core/ao_int64.c | 88 ++++++++++++------------ src/core/ao_int64.h | 22 +++--- src/core/ao_kalman.c | 4 +- src/core/ao_log_telem.c | 2 +- src/core/ao_sample.h | 4 +- src/core/ao_task.h | 3 + src/drivers/ao_ms5607.c | 4 +- src/drivers/ao_ms5607.h | 7 +- src/drivers/ao_ms5607_convert_8051.c | 126 ++++++++++++++++++++++++++++------- src/telemini-v2.0/Makefile | 9 ++- src/telemini-v2.0/ao_pins.h | 2 + src/test/Makefile | 6 +- src/test/ao_int64_test.c | 5 ++ src/test/ao_ms5607_convert_test.c | 96 ++++++++++++++++++++++++++ 16 files changed, 289 insertions(+), 95 deletions(-) create mode 100644 src/test/ao_ms5607_convert_test.c (limited to 'src/drivers') diff --git a/src/core/ao.h b/src/core/ao.h index caa0ec19..e7320327 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -182,7 +182,7 @@ void ao_cmd_hex(void); void -ao_cmd_decimal(void); +ao_cmd_decimal(void) __reentrant; /* Read a single hex nibble off stdin. */ uint8_t diff --git a/src/core/ao_cmd.c b/src/core/ao_cmd.c index 5113548b..4ebaa607 100644 --- a/src/core/ao_cmd.c +++ b/src/core/ao_cmd.c @@ -206,9 +206,9 @@ ao_cmd_hex(void) } void -ao_cmd_decimal(void) +ao_cmd_decimal(void) __reentrant { - __pdata uint8_t r = ao_cmd_lex_error; + uint8_t r = ao_cmd_lex_error; ao_cmd_lex_u32 = 0; ao_cmd_white(); diff --git a/src/core/ao_int64.c b/src/core/ao_int64.c index 07cdb357..aa23dbe0 100644 --- a/src/core/ao_int64.c +++ b/src/core/ao_int64.c @@ -17,8 +17,10 @@ #include -void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { - uint32_t t; +__pdata ao_int64_t *__data ao_64r, *__data ao_64a, *__data ao_64b; + +void ao_plus64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, __pdata ao_int64_t *b) __FATTR { + __LOCAL uint32_t t; r->high = a->high + b->high; t = a->low + b->low; @@ -27,9 +29,8 @@ void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { r->low = t; } -void ao_minus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { - uint32_t t; - +void ao_minus64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, __pdata ao_int64_t *b) __FATTR { + __LOCAL uint32_t t; r->high = a->high - b->high; t = a->low - b->low; @@ -38,7 +39,7 @@ void ao_minus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { r->low = t; } -void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) { +void ao_rshift64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, uint8_t d) __FATTR { if (d < 32) { r->low = a->low >> d; if (d) @@ -51,7 +52,7 @@ void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) { } } -void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) { +void ao_lshift64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, uint8_t d) __FATTR { if (d < 32) { r->high = a->high << d; if (d) @@ -64,53 +65,49 @@ void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) { } } -static void ao_umul64_32_32(ao_int64_t *r, uint32_t a, uint32_t b) -{ - uint32_t r1; - uint32_t r2, r3, r4; - ao_int64_t s,t,u,v; - r1 = (uint32_t) (uint16_t) a * (uint16_t) b; - r2 = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) b; - r3 = (uint32_t) (uint16_t) a * (uint16_t) (b >> 16); - r4 = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) (b >> 16); - - s.low = r1; - s.high = r4; - - t.high = r2 >> 16; - t.low = r2 << 16; - ao_plus64(&u, &s, &t); - - v.high = r3 >> 16; - v.low = r3 << 16; - ao_plus64(r, &u, &v); +static void ao_umul64_32_32(__ARG ao_int64_t *r, uint32_t a, uint32_t b) __reentrant { + __LOCAL uint32_t s; + __LOCAL ao_int64_t t; + r->low = (uint32_t) (uint16_t) a * (uint16_t) b; + r->high = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) (b >> 16); + + s = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) b; + + t.high = s >> 16; + t.low = s << 16; + ao_plus64(r, r, &t); + + s = (uint32_t) (uint16_t) a * (uint16_t) (b >> 16); + + t.high = s >> 16; + t.low = s << 16; + ao_plus64(r, r, &t); } -void ao_neg64(ao_int64_t *r, ao_int64_t *a) { +void ao_neg64(__pdata ao_int64_t *r, __pdata ao_int64_t *a) __FATTR { r->high = ~a->high; - r->low = ~a->low; - if (!++r->low) + if (!(r->low = ~a->low + 1)) r->high++; } -void ao_mul64_32_32(ao_int64_t *r, int32_t a, int32_t b) { +void ao_mul64_32_32(__ARG ao_int64_t *r, int32_t a, int32_t b) __FATTR { uint8_t negative = 0; if (a < 0) { a = -a; - negative = ~0; + ++negative; } if (b < 0) { b = -b; - negative = ~negative; + --negative; } ao_umul64_32_32(r, a, b); if (negative) ao_neg64(r, r); } -static void ao_umul64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { - ao_int64_t r2, r3; +static void ao_umul64(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG ao_int64_t *b) __reentrant { + __LOCAL ao_int64_t r2, r3; ao_umul64_32_32(&r2, a->high, b->low); ao_umul64_32_32(&r3, a->low, b->high); @@ -119,38 +116,41 @@ static void ao_umul64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { r->high += r2.low + r3.low; } -void ao_mul64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) { +static __ARG ao_int64_t ap, bp; + +void ao_mul64(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG ao_int64_t *b) __FATTR { uint8_t negative = 0; - ao_int64_t ap, bp; if (ao_int64_negativep(a)) { ao_neg64(&ap, a); a = ≈ - negative = ~0; + ++negative; } if (ao_int64_negativep(b)) { ao_neg64(&bp, b); b = &bp; - negative = ~negative; + --negative; } ao_umul64(r, a, b); if (negative) ao_neg64(r, r); } -void ao_umul64_64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) { - uint32_t h = a->high * b; +static void ao_umul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, uint16_t b) __reentrant { + __LOCAL uint32_t h; + + h = a->high * b; ao_umul64_32_32(r, a->low, b); r->high += h; } -void ao_mul64_64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) { - ao_int64_t ap; +void ao_mul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG uint16_t b) __FATTR { uint8_t negative = 0; + if ((int32_t) a->high < 0) { ao_neg64(&ap, a); a = ≈ - negative = ~0; + negative++; } else ao_umul64_64_16(r, a, b); if (negative) diff --git a/src/core/ao_int64.h b/src/core/ao_int64.h index cf12f0d8..b16db58c 100644 --- a/src/core/ao_int64.h +++ b/src/core/ao_int64.h @@ -25,16 +25,18 @@ typedef struct { uint32_t low; } ao_int64_t; -void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b); -void ao_minus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b); -void ao_neg64(ao_int64_t *r, ao_int64_t *a); -void ao_lshift64_16(ao_int64_t *r, uint16_t a, uint8_t d); -void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d); -void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d); -void ao_mul64_64_64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b); -void ao_mul64_32_32(ao_int64_t *r, int32_t a, int32_t b); -void ao_mul64_64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b); -void ao_mul64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b); +#define __FATTR +#define __ARG __pdata +#define __LOCAL static __pdata + +void ao_plus64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, __pdata ao_int64_t *ao_64b) __FATTR; +void ao_minus64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, __pdata ao_int64_t *ao_64b) __FATTR; +void ao_neg64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a) __FATTR; +void ao_rshift64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, uint8_t d) __FATTR; +void ao_lshift64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, uint8_t d) __FATTR; +void ao_mul64_32_32(__ARG ao_int64_t *r, __ARG int32_t a, __ARG int32_t b) __FATTR; +void ao_mul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG uint16_t b) __FATTR; +void ao_mul64(__ARG ao_int64_t * __ARG r, __ARG ao_int64_t * __ARG a, __ARG ao_int64_t *__ARG b) __FATTR; #define ao_int64_init32(r, a) (((r)->high = 0), (r)->low = (a)) #define ao_int64_init64(r, a, b) (((r)->high = (a)), (r)->low = (b)) diff --git a/src/core/ao_kalman.c b/src/core/ao_kalman.c index 59ffd8b2..762b2c0a 100644 --- a/src/core/ao_kalman.c +++ b/src/core/ao_kalman.c @@ -40,9 +40,9 @@ static __pdata int32_t ao_k_accel; __pdata int16_t ao_height; __pdata int16_t ao_speed; __pdata int16_t ao_accel; -__pdata int16_t ao_max_height; +__xdata int16_t ao_max_height; static __pdata int32_t ao_avg_height_scaled; -__pdata int16_t ao_avg_height; +__xdata int16_t ao_avg_height; __pdata int16_t ao_error_h; __pdata int16_t ao_error_h_sq_avg; diff --git a/src/core/ao_log_telem.c b/src/core/ao_log_telem.c index 23ebf7dd..095aca37 100644 --- a/src/core/ao_log_telem.c +++ b/src/core/ao_log_telem.c @@ -23,7 +23,7 @@ __code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMETRY; static __data uint8_t ao_log_monitor_pos; __pdata enum ao_flight_state ao_flight_state; -__pdata int16_t ao_max_height; /* max of ao_height */ +__xdata int16_t ao_max_height; /* max of ao_height */ __pdata int16_t sense_d, sense_m; __pdata uint8_t ao_igniter_present; diff --git a/src/core/ao_sample.h b/src/core/ao_sample.h index a2dac979..5bd29536 100644 --- a/src/core/ao_sample.h +++ b/src/core/ao_sample.h @@ -136,8 +136,8 @@ uint8_t ao_sample(void); extern __pdata int16_t ao_height; /* meters */ extern __pdata int16_t ao_speed; /* m/s * 16 */ extern __pdata int16_t ao_accel; /* m/s² * 16 */ -extern __pdata int16_t ao_max_height; /* max of ao_height */ -extern __pdata int16_t ao_avg_height; /* running average of height */ +extern __xdata int16_t ao_max_height; /* max of ao_height */ +extern __xdata int16_t ao_avg_height; /* running average of height */ extern __pdata int16_t ao_error_h; extern __pdata int16_t ao_error_h_sq_avg; diff --git a/src/core/ao_task.h b/src/core/ao_task.h index 1a4b5b6b..e3a311ed 100644 --- a/src/core/ao_task.h +++ b/src/core/ao_task.h @@ -45,7 +45,10 @@ struct ao_task { #endif }; +#ifndef AO_NUM_TASKS #define AO_NUM_TASKS 16 /* maximum number of tasks */ +#endif + #define AO_NO_TASK 0 /* no task id */ extern __xdata struct ao_task * __xdata ao_tasks[AO_NUM_TASKS]; diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c index 5259b265..4b4403a7 100644 --- a/src/drivers/ao_ms5607.c +++ b/src/drivers/ao_ms5607.c @@ -173,7 +173,7 @@ 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); @@ -220,7 +220,7 @@ 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); diff --git a/src/drivers/ao_ms5607.h b/src/drivers/ao_ms5607.h index 3fd43fd4..206efd64 100644 --- a/src/drivers/ao_ms5607.h +++ b/src/drivers/ao_ms5607.h @@ -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_8051.c b/src/drivers/ao_ms5607_convert_8051.c index f47972c9..f3a48c46 100644 --- a/src/drivers/ao_ms5607_convert_8051.c +++ b/src/drivers/ao_ms5607_convert_8051.c @@ -16,45 +16,121 @@ */ #include +#include + +#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(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) { -#if 0 - int32_t dT; - int32_t TEMP; - int64_t OFF; - int64_t SENS; + __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); + /* 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; + /* */ -#if HAS_MS5611 - OFF = ((int64_t) ms5607_prom.off << 16) + (((int64_t) ms5607_prom.tco * dT) >> 7); - SENS = ((int64_t) ms5607_prom.sens << 15) + (((int64_t) ms5607_prom.tcs * dT) >> 8); + /* 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 = ((int64_t) ms5607_prom.off << 17) + (((int64_t) ms5607_prom.tco * dT) >> 6); - SENS = ((int64_t) ms5607_prom.sens << 16) + (((int64_t) ms5607_prom.tcs * dT) >> 7); + 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) { - int32_t T2 = ((int64_t) dT * (int64_t) dT) >> 31; - 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) { - int32_t TEMPP = TEMP + 1500; - int64_t TEMPP2 = TEMPP * TEMPP; - OFF2 = OFF2 + 15 * TEMPP2; - SENS2 = SENS2 + 8 * TEMPP2; + __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; - SENS -= SENS2; + + /* OFF -= OFF2; */ + ao_minus64(&OFF, &OFF, &OFF2); + /**/ + + /* SENS -= SENS2; */ + ao_minus64(&SENS, &SENS, &SENS2); + /**/ } - value->pres = ((((int64_t) sample->pres * SENS) >> 21) - OFF) >> 15; + /* 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; -#endif } diff --git a/src/telemini-v2.0/Makefile b/src/telemini-v2.0/Makefile index afd529f0..984406a9 100644 --- a/src/telemini-v2.0/Makefile +++ b/src/telemini-v2.0/Makefile @@ -19,7 +19,11 @@ INC = \ ao_arch.h \ ao_arch_funcs.h \ cc1111.h \ - ao_product.h + ao_ms5607.h \ + ao_ms5607_convert_8051.c \ + ao_product.h \ + ao_int64.h \ + ao_sample.h CORE_SRC = \ ao_cmd.c \ @@ -37,7 +41,8 @@ CORE_SRC = \ ao_storage.c \ ao_task.c \ ao_telemetry.c \ - ao_freq.c + ao_freq.c \ + ao_int64.c CC1111_SRC = \ ao_adc.c \ diff --git a/src/telemini-v2.0/ao_pins.h b/src/telemini-v2.0/ao_pins.h index 386c8dc3..fad029e2 100644 --- a/src/telemini-v2.0/ao_pins.h +++ b/src/telemini-v2.0/ao_pins.h @@ -115,6 +115,8 @@ struct ao_adc { #define AO_SENSE_DROGUE(p) ((p)->adc.sense_a) #define AO_SENSE_MAIN(p) ((p)->adc.sense_m) +#define AO_NUM_TASKS 10 + #define AO_ADC_DUMP(p) \ printf("tick: %5u apogee: %5d main: %5d batt: %5d\n", \ (p)->tick, (p)->adc.sense_a, (p)->adc.sense_m, (p)->adc.v_batt) diff --git a/src/test/Makefile b/src/test/Makefile index 9c304318..5eee6bbb 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -2,7 +2,8 @@ vpath % ..:../core:../drivers:../util:../micropeak:../aes PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_flight_test_mm \ ao_gps_test ao_gps_test_skytraq ao_gps_test_ublox ao_convert_test ao_convert_pa_test ao_fec_test \ - ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test + ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test \ + ao_ms5607_convert_test INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h @@ -76,3 +77,6 @@ ao_aes_test: ao_aes_test.c ao_aes.c ao_aes_tables.c ao_int64_test: ao_int64_test.c ao_int64.c ao_int64.h cc $(CFLAGS) -o $@ ao_int64_test.c + +ao_ms5607_convert_test: ao_ms5607_convert_test.c ao_ms5607_convert_8051.c ao_int64.c ao_int64.h + cc $(CFLAGS) -o $@ ao_ms5607_convert_test.c diff --git a/src/test/ao_int64_test.c b/src/test/ao_int64_test.c index c26a63b0..8557a1c7 100644 --- a/src/test/ao_int64_test.c +++ b/src/test/ao_int64_test.c @@ -15,6 +15,11 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. */ +#define __data +#define __pdata +#define __xdata +#define __reentrant + #include #include #include diff --git a/src/test/ao_ms5607_convert_test.c b/src/test/ao_ms5607_convert_test.c new file mode 100644 index 00000000..ad593204 --- /dev/null +++ b/src/test/ao_ms5607_convert_test.c @@ -0,0 +1,96 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#define __xdata +#define __data +#define __pdata +#define __reentrant + +#include +#include + +struct ao_ms5607_prom ms5607_prom = { + 0x002c, + 0xa6e0, + 0x988e, + 0x6814, + 0x5eff, + 0x8468, + 0x6c86, + 0xa271, +}; + +int32_t D1_mm = 6179630; +int32_t D2_mm = 8933155; + +#include +#define ao_ms5607_convert ao_ms5607_convert_8051 +#include +#include +#include +#include + +struct ao_ms5607_sample ao_sample = { + 6179630, + 8933155 +}; + +int errors; + +void test(int trial, struct ao_ms5607_sample *sample) +{ + struct ao_ms5607_value value, value_8051; + + ao_ms5607_convert(sample, &value); + ao_ms5607_convert_8051(sample, &value_8051); + if (value.temp != value_8051.temp || value.pres != value_8051.pres) { + ++errors; + printf ("trial %d: %d, %d -> %d, %d (should be %d, %d)\n", + trial, + sample->pres, sample->temp, + value_8051.pres, value_8051.temp, + value.pres, value.temp); + } +} + +#define TESTS 10000000 + +#include + +static int32_t rand24(void) { return random() & 0xffffff; } + +int +main(int argc, char **argv) +{ + struct ao_ms5607_sample sample; + int i, start; + + if (argv[1]) + start = atoi(argv[1]); + else + start = 0; + + srandom(10000); + test(-1, &ao_sample); + for (i = 0; i < TESTS; i++) { + sample.pres = rand24(); + sample.temp = rand24(); + if (i >= start) + test(i, &sample); + } + return errors; +} -- cgit v1.2.3 From 312f6194a4bc75473cb0d61a6d58b66fb1f7c068 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 12 Jun 2013 00:43:31 -0700 Subject: altos/teletiny-v2.0: Support multiple SPI busses on CC1111 Needed for TeleMini v2.0 Signed-off-by: Keith Packard --- src/cc1111/ao_arch.h | 5 + src/cc1111/ao_arch_funcs.h | 82 +++++++++---- src/cc1111/ao_spi.c | 257 +++++++++++++++++++++++++--------------- src/drivers/ao_ms5607.c | 2 +- src/telemini-v2.0/ao_pins.h | 4 + src/telemini-v2.0/ao_telemini.c | 1 + 6 files changed, 231 insertions(+), 120 deletions(-) (limited to 'src/drivers') diff --git a/src/cc1111/ao_arch.h b/src/cc1111/ao_arch.h index 9097557f..34235b08 100644 --- a/src/cc1111/ao_arch.h +++ b/src/cc1111/ao_arch.h @@ -321,4 +321,9 @@ void ao_serial1_tx_isr(void) ao_arch_interrupt(14); #endif +#if HAS_EXTI_0 +void +ao_p0_isr(void) __interrupt(13); +#endif + #endif /* _AO_ARCH_H_ */ diff --git a/src/cc1111/ao_arch_funcs.h b/src/cc1111/ao_arch_funcs.h index 8f1cc094..ae184108 100644 --- a/src/cc1111/ao_arch_funcs.h +++ b/src/cc1111/ao_arch_funcs.h @@ -19,46 +19,74 @@ * ao_spi.c */ -extern __xdata uint8_t ao_spi_mutex; +#if !HAS_SPI_0 && !HAS_SPI_1 +#define HAS_SPI_0 1 +#define SPI_0_ALT_2 1 +#endif + +#if HAS_SPI_0 && HAS_SPI_1 +#define MULTI_SPI 1 +#define N_SPI 2 +#else +#define MULTI_SPI 0 +#define N_SPI 1 +#endif + +extern __xdata uint8_t ao_spi_mutex[N_SPI]; + +#if MULTI_SPI +#define ao_spi_get(bus) ao_mutex_get(&ao_spi_mutex[bus]) +#define ao_spi_put(bus) ao_mutex_put(&ao_spi_mutex[bus]) +#else +#define ao_spi_get(bus) ao_mutex_get(&ao_spi_mutex[0]) +#define ao_spi_put(bus) ao_mutex_put(&ao_spi_mutex[0]) +#endif #define AO_SPI_SPEED_FAST 17 #define AO_SPI_SPEED_200kHz 13 -#define ao_spi_set_speed(speed) (U0GCR = (UxGCR_CPOL_NEGATIVE | \ - UxGCR_CPHA_FIRST_EDGE | \ - UxGCR_ORDER_MSB | \ - ((speed) << UxGCR_BAUD_E_SHIFT))) +#if MULTI_SPI +#define ao_spi_set_speed(bus,speed) (*(bus ? &U1GCR : &U0GCR) =(UxGCR_CPOL_NEGATIVE | \ + UxGCR_CPHA_FIRST_EDGE | \ + UxGCR_ORDER_MSB | \ + ((speed) << UxGCR_BAUD_E_SHIFT))) +#else +#define ao_spi_set_speed(bus,speed) (U0GCR = (UxGCR_CPOL_NEGATIVE | \ + UxGCR_CPHA_FIRST_EDGE | \ + UxGCR_ORDER_MSB | \ + ((speed) << UxGCR_BAUD_E_SHIFT))) +#endif #define ao_spi_get_slave(bus) do { \ - ao_mutex_get(&ao_spi_mutex); \ - ao_spi_set_speed(AO_SPI_SPEED_FAST); \ + ao_spi_get(bus); \ + ao_spi_set_speed(bus,AO_SPI_SPEED_FAST); \ } while (0) #define ao_spi_put_slave(bus) do { \ - ao_mutex_put(&ao_spi_mutex); \ + ao_spi_put(bus); \ } while (0) #define ao_spi_get_mask(reg,mask,bus,speed) do { \ - ao_mutex_get(&ao_spi_mutex); \ - ao_spi_set_speed(speed); \ + ao_spi_get(bus); \ + ao_spi_set_speed(bus,speed); \ (reg) &= ~(mask); \ } while (0) #define ao_spi_put_mask(reg,mask,bus) do { \ (reg) |= (mask); \ - ao_mutex_put(&ao_spi_mutex); \ + ao_spi_put(bus); \ } while (0) #define ao_spi_get_bit(reg,bit,pin,bus,speed) do { \ - ao_mutex_get(&ao_spi_mutex); \ - ao_spi_set_speed(speed); \ - pin = 0; \ + ao_spi_get(bus); \ + ao_spi_set_speed(bus,speed); \ + pin = 0; \ } while (0) #define ao_spi_put_bit(reg,bit,pin,bus) do { \ pin = 1; \ - ao_mutex_put(&ao_spi_mutex); \ + ao_spi_put(bus); \ } while (0) @@ -68,6 +96,13 @@ extern __xdata uint8_t ao_spi_mutex; * from chip select low to chip select high */ +#if MULTI_SPI +void +ao_spi_send(void __xdata *block, uint16_t len, uint8_t bus) __reentrant; + +void +ao_spi_recv(void __xdata *block, uint16_t len, uint8_t bus) __reentrant; +#else void ao_spi_send_bus(void __xdata *block, uint16_t len) __reentrant; @@ -76,6 +111,7 @@ ao_spi_recv_bus(void __xdata *block, uint16_t len) __reentrant; #define ao_spi_send(block, len, bus) ao_spi_send_bus(block, len) #define ao_spi_recv(block, len, bus) ao_spi_recv_bus(block, len) +#endif #if AO_SPI_SLAVE void @@ -88,10 +124,15 @@ ao_spi_recv_wait(void); void ao_spi_init(void); -#define ao_spi_init_cs(port, mask) do { \ - SPI_CS_PORT |= mask; \ - SPI_CS_DIR |= mask; \ - SPI_CS_SEL &= ~mask; \ +#define token_paster(x,y) x ## y +#define token_paster3(x,y,z) x ## y ## z +#define token_evaluator(x,y) token_paster(x,y) +#define token_evaluator3(x,y,z) token_paster3(x,y,z) + +#define ao_spi_init_cs(port, mask) do { \ + port |= mask; \ + token_evaluator(port,DIR) |= mask; \ + token_evaluator(port,SEL) &= ~mask; \ } while (0) #define cc1111_enable_output(port,dir,sel,pin,bit,v) do { \ @@ -102,7 +143,6 @@ ao_spi_init(void); #define disable_unreachable _Pragma("disable_warning 126") -#define token_paster(x,y) x ## y -#define token_evaluator(x,y) token_paster(x,y) #define ao_enable_output(port,bit,pin,v) cc1111_enable_output(port,token_evaluator(port,DIR), token_evaluator(port,SEL), pin, bit, v) #define ao_gpio_set(port, bit, pin, v) ((pin) = (v)) + diff --git a/src/cc1111/ao_spi.c b/src/cc1111/ao_spi.c index cdef6bda..fb08f3f5 100644 --- a/src/cc1111/ao_spi.c +++ b/src/cc1111/ao_spi.c @@ -18,10 +18,6 @@ #include "ao.h" /* Default pin usage for existing Altus Metrum devices */ -#if !HAS_SPI_0 && !HAS_SPI_1 -#define HAS_SPI_0 1 -#define SPI_0_ALT_2 1 -#endif #ifndef SPI_CONST #define SPI_CONST 0xff @@ -61,62 +57,107 @@ */ #if HAS_SPI_0 -#define SPI_CSR U0CSR -#define SPI_BUF U0DBUFXADDR -#define SPI_BAUD U0BAUD -#define SPI_GCR U0GCR -#define SPI_CFG_MASK PERCFG_U0CFG_ALT_MASK -#define SPI_DMA_TX DMA_CFG0_TRIGGER_UTX0 -#define SPI_DMA_RX DMA_CFG0_TRIGGER_URX0 +#define SPI_BUF_0 &U0DBUFXADDR +#define SPI_CSR_0 U0CSR +#define SPI_BAUD_0 U0BAUD +#define SPI_GCR_0 U0GCR +#define SPI_CFG_MASK_0 PERCFG_U0CFG_ALT_MASK +#define SPI_DMA_TX_0 DMA_CFG0_TRIGGER_UTX0 +#define SPI_DMA_RX_0 DMA_CFG0_TRIGGER_URX0 #if SPI_0_ALT_1 -#define SPI_CFG PERCFG_U0CFG_ALT_1 -#define SPI_SEL P0SEL -#define SPI_BITS (1 << 3) | (1 << 2) | (1 << 5) -#define SPI_CSS_BIT (1 << 4) +#define SPI_CFG_0 PERCFG_U0CFG_ALT_1 +#define SPI_SEL_0 P0SEL +#define SPI_BITS_0 (1 << 3) | (1 << 2) | (1 << 5) +#define SPI_CSS_BIT_0 (1 << 4) #endif #if SPI_0_ALT_2 -#define SPI_CFG PERCFG_U0CFG_ALT_2 -#define SPI_SEL P1SEL -#define SPI_PRI P2SEL_PRI3P1_USART0 -#define SPI_BITS (1 << 5) | (1 << 4) | (1 << 3) -#define SPI_CSS_BIT (1 << 2) +#define SPI_CFG_0 PERCFG_U0CFG_ALT_2 +#define SPI_SEL_0 P1SEL +#define SPI_PRI_0 P2SEL_PRI3P1_USART0 +#define SPI_BITS_0 (1 << 5) | (1 << 4) | (1 << 3) +#define SPI_CSS_BIT_0 (1 << 2) #endif #endif #if HAS_SPI_1 -#define SPI_CSR U1CSR -#define SPI_BUF U1DBUFXADDR -#define SPI_BAUD U1BAUD -#define SPI_GCR U1GCR -#define SPI_CFG_MASK PERCFG_U1CFG_ALT_MASK -#define SPI_DMA_TX DMA_CFG0_TRIGGER_UTX1 -#define SPI_DMA_RX DMA_CFG0_TRIGGER_URX1 +#define SPI_BUF_1 &U1DBUFXADDR +#define SPI_CSR_1 U1CSR +#define SPI_BAUD_1 U1BAUD +#define SPI_GCR_1 U1GCR +#define SPI_CFG_MASK_1 PERCFG_U1CFG_ALT_MASK +#define SPI_DMA_TX_1 DMA_CFG0_TRIGGER_UTX1 +#define SPI_DMA_RX_1 DMA_CFG0_TRIGGER_URX1 #if SPI_1_ALT_1 -#define SPI_CFG PERCFG_U1CFG_ALT_1 -#define SPI_SEL P0SEL -#define SPI_BITS (1 << 4) | (1 << 5) | (1 << 3) -#define SPI_CSS_BIT (1 << 2) +#define SPI_CFG_1 PERCFG_U1CFG_ALT_1 +#define SPI_SEL_1 P0SEL +#define SPI_BITS_1 (1 << 4) | (1 << 5) | (1 << 3) +#define SPI_CSS_BIT_1 (1 << 2) #endif #if SPI_1_ALT_2 -#define SPI_CFG PERCFG_U1CFG_ALT_2 -#define SPI_SEL P1SEL -#define SPI_PRI P2SEL_PRI3P1_USART1 -#define SPI_BITS (1 << 6) | (1 << 7) | (1 << 5) -#define SPI_CSS_BIT (1 << 4) +#define SPI_CFG_1 PERCFG_U1CFG_ALT_2 +#define SPI_SEL_1 P1SEL +#define SPI_PRI_1 P2SEL_PRI3P1_USART1 +#define SPI_BITS_1 (1 << 6) | (1 << 7) | (1 << 5) +#define SPI_CSS_BIT_1 (1 << 4) #endif #endif +#if MULTI_SPI + +#define SPI_BUF(bus) ((bus) ? SPI_BUF_1 : SPI_BUF_0) +#define SPI_CSR(bus) ((bus) ? SPI_CSR_1 : SPI_CSR_0) +#define SPI_BAUD(bus) ((bus) ? SPI_BAUD_1 : SPI_BAUD_0) +#define SPI_GCR(bus) ((bus) ? SPI_GCR_1 : SPI_GCR_0) +#define SPI_CFG_MASK(bus) ((bus) ? SPI_CFG_MASK_1 : SPI_CFG_MASK_0) +#define SPI_DMA_TX(bus) ((bus) ? SPI_DMA_TX_1 : SPI_DMA_TX_0) +#define SPI_DMA_RX(bus) ((bus) ? SPI_DMA_RX_1 : SPI_DMA_RX_0) +#define SPI_CFG(bus) ((bus) ? SPI_CFG_1 : SPI_CFG_0) +#define SPI_SEL(bus) ((bus) ? SPI_SEL_1 : SPI_SEL_0) +#define SPI_BITS(bus) ((bus) ? SPI_BITS_1 : SPI_BITS_0) +#define SPI_CSS_BIT(bus) ((bus) ? SPI_CSS_BIT_1 : SPI_CSS_BIT_0) + +#else + +#if HAS_SPI_0 +#define SPI_BUF(bus) SPI_BUF_0 +#define SPI_CSR(bus) SPI_CSR_0 +#define SPI_BAUD(bus) SPI_BAUD_0 +#define SPI_GCR(bus) SPI_GCR_0 +#define SPI_CFG_MASK(bus) SPI_CFG_MASK_0 +#define SPI_DMA_TX(bus) SPI_DMA_TX_0 +#define SPI_DMA_RX(bus) SPI_DMA_RX_0 +#define SPI_CFG(bus) SPI_CFG_0 +#define SPI_SEL(bus) SPI_SEL_0 +#define SPI_BITS(bus) SPI_BITS_0 +#define SPI_CSS_BIT(bus) SPI_CSS_BIT_0 +#endif +#if HAS_SPI_1 +#define SPI_BUF(bus) SPI_BUF_1 +#define SPI_CSR(bus) SPI_CSR_1 +#define SPI_BAUD(bus) SPI_BAUD_1 +#define SPI_GCR(bus) SPI_GCR_1 +#define SPI_CFG_MASK(bus) SPI_CFG_MASK_1 +#define SPI_DMA_TX(bus) SPI_DMA_TX_1 +#define SPI_DMA_RX(bus) SPI_DMA_RX_1 +#define SPI_CFG(bus) SPI_CFG_1 +#define SPI_SEL(bus) SPI_SEL_1 +#define SPI_BITS(bus) SPI_BITS_1 +#define SPI_CSS_BIT(bus) SPI_CSS_BIT_1 +#endif + +#endif /* MULTI_SPI */ + #if AO_SPI_SLAVE -#define CSS SPI_CSS_BIT +#define CSS(bus) SPI_CSS_BIT(bus) #define UxCSR_DIRECTION UxCSR_SLAVE #else -#define CSS 0 +#define CSS(bus) 0 #define UxCSR_DIRECTION UxCSR_MASTER #endif @@ -124,15 +165,16 @@ * operation, from CS low to CS high. This means that any SPI * user must protect the SPI bus with this mutex */ -__xdata uint8_t ao_spi_mutex; -__xdata uint8_t ao_spi_dma_in_done; -__xdata uint8_t ao_spi_dma_out_done; +__xdata uint8_t ao_spi_mutex[N_SPI]; +__xdata uint8_t ao_spi_dma_in_done[N_SPI]; +__xdata uint8_t ao_spi_dma_out_done[N_SPI]; -uint8_t ao_spi_dma_out_id; -uint8_t ao_spi_dma_in_id; +uint8_t ao_spi_dma_out_id[N_SPI]; +uint8_t ao_spi_dma_in_id[N_SPI]; static __xdata uint8_t ao_spi_const; + /* Send bytes over SPI. * * This sets up two DMA engines, one writing the data and another reading @@ -140,45 +182,52 @@ static __xdata uint8_t ao_spi_const; * is complete, as the transmit register is double buffered and hence signals * completion one byte before the transfer is actually complete */ +#if MULTI_SPI +void +ao_spi_send(void __xdata *block, uint16_t len, uint8_t bus) __reentrant +#else void ao_spi_send_bus(void __xdata *block, uint16_t len) __reentrant +#define bus 0 +#endif { - ao_dma_set_transfer(ao_spi_dma_in_id, - &SPI_BUF, + ao_dma_set_transfer(ao_spi_dma_in_id[bus], + SPI_BUF(bus), &ao_spi_const, len, DMA_CFG0_WORDSIZE_8 | DMA_CFG0_TMODE_SINGLE | - SPI_DMA_RX, + SPI_DMA_RX(bus), DMA_CFG1_SRCINC_0 | DMA_CFG1_DESTINC_0 | DMA_CFG1_PRIORITY_NORMAL); - ao_dma_set_transfer(ao_spi_dma_out_id, + ao_dma_set_transfer(ao_spi_dma_out_id[bus], block, - &SPI_BUF, + SPI_BUF(bus), len, DMA_CFG0_WORDSIZE_8 | DMA_CFG0_TMODE_SINGLE | - SPI_DMA_TX, + SPI_DMA_TX(bus), DMA_CFG1_SRCINC_1 | DMA_CFG1_DESTINC_0 | DMA_CFG1_PRIORITY_NORMAL); - ao_dma_start(ao_spi_dma_in_id); - ao_dma_start(ao_spi_dma_out_id); - ao_dma_trigger(ao_spi_dma_out_id); + ao_dma_start(ao_spi_dma_in_id[bus]); + ao_dma_start(ao_spi_dma_out_id[bus]); + ao_dma_trigger(ao_spi_dma_out_id[bus]); #if !AO_SPI_SLAVE - __critical while (!ao_spi_dma_in_done) - ao_sleep(&ao_spi_dma_in_done); + __critical while (!ao_spi_dma_in_done[bus]) + ao_sleep(&ao_spi_dma_in_done[bus]); #endif +#undef bus } #if AO_SPI_SLAVE void ao_spi_send_wait(void) { - __critical while (!ao_spi_dma_in_done) - ao_sleep(&ao_spi_dma_in_done); + __critical while (!ao_spi_dma_in_done[0]) + ao_sleep(&ao_spi_dma_in_done[0]); } #endif @@ -188,16 +237,22 @@ ao_spi_send_wait(void) * writing constant values to the SPI transmitter as that is what * clocks the data coming in. */ +#if MULTI_SPI +void +ao_spi_recv(void __xdata *block, uint16_t len, uint8_t bus) __reentrant +#else void ao_spi_recv_bus(void __xdata *block, uint16_t len) __reentrant +#define bus 0 +#endif { - ao_dma_set_transfer(ao_spi_dma_in_id, - &SPI_BUF, + ao_dma_set_transfer(ao_spi_dma_in_id[bus], + SPI_BUF(bus), block, len, DMA_CFG0_WORDSIZE_8 | DMA_CFG0_TMODE_SINGLE | - SPI_DMA_RX, + SPI_DMA_RX(bus), DMA_CFG1_SRCINC_0 | DMA_CFG1_DESTINC_1 | DMA_CFG1_PRIORITY_NORMAL); @@ -205,24 +260,24 @@ ao_spi_recv_bus(void __xdata *block, uint16_t len) __reentrant ao_spi_const = SPI_CONST; #if !AO_SPI_SLAVE - ao_dma_set_transfer(ao_spi_dma_out_id, + ao_dma_set_transfer(ao_spi_dma_out_id[bus], &ao_spi_const, - &SPI_BUF, + SPI_BUF(bus), len, DMA_CFG0_WORDSIZE_8 | DMA_CFG0_TMODE_SINGLE | - SPI_DMA_TX, + SPI_DMA_TX(bus), DMA_CFG1_SRCINC_0 | DMA_CFG1_DESTINC_0 | DMA_CFG1_PRIORITY_NORMAL); #endif - ao_dma_start(ao_spi_dma_in_id); + ao_dma_start(ao_spi_dma_in_id[bus]); #if !AO_SPI_SLAVE - ao_dma_start(ao_spi_dma_out_id); - ao_dma_trigger(ao_spi_dma_out_id); - __critical while (!ao_spi_dma_in_done) - ao_sleep(&ao_spi_dma_in_done); + ao_dma_start(ao_spi_dma_out_id[bus]); + ao_dma_trigger(ao_spi_dma_out_id[bus]); + __critical while (!ao_spi_dma_in_done[bus]) + ao_sleep(&ao_spi_dma_in_done[bus]); #endif } @@ -230,17 +285,43 @@ ao_spi_recv_bus(void __xdata *block, uint16_t len) __reentrant void ao_spi_recv_wait(void) { - __critical while (!ao_spi_dma_in_done) - ao_sleep(&ao_spi_dma_in_done); + __critical while (!ao_spi_dma_in_done[0]) + ao_sleep(&ao_spi_dma_in_done[0]); } #endif +/* Set up the USART. + * + * SPI master/slave mode + */ +/* Set the baud rate and signal parameters + * + * The cc1111 is limited to a 24/8 MHz SPI clock. + * Every peripheral I've ever seen goes faster than that, + * so set the clock to 3MHz (BAUD_E 17, BAUD_M 0) + */ +#define SPI_INIT(bus,o) do { \ + /* Set up the USART pin assignment */ \ + PERCFG = (PERCFG & ~SPI_CFG_MASK(bus)) | SPI_CFG(bus); \ + \ + /* Make the SPI pins be controlled by the USART peripheral */ \ + SPI_SEL(bus) |= SPI_BITS(bus) | CSS(bus); \ + SPI_CSR(bus) = (UxCSR_MODE_SPI | UxCSR_RE | UxCSR_DIRECTION); \ + SPI_BAUD(bus) = 0; \ + SPI_GCR(bus) = (UxGCR_CPOL_NEGATIVE | \ + UxGCR_CPHA_FIRST_EDGE | \ + UxGCR_ORDER_MSB | \ + (17 << UxGCR_BAUD_E_SHIFT)); \ + /* Set up OUT DMA */ \ + ao_spi_dma_out_id[o] = ao_dma_alloc(&ao_spi_dma_out_done[o]); \ + \ + /* Set up IN DMA */ \ + ao_spi_dma_in_id[o] = ao_dma_alloc(&ao_spi_dma_in_done[o]); \ + } while (0) + void ao_spi_init(void) { - /* Set up the USART pin assignment */ - PERCFG = (PERCFG & ~SPI_CFG_MASK) | SPI_CFG; - /* Ensure that SPI USART takes precidence over the other USART * for pins that they share */ @@ -248,30 +329,10 @@ ao_spi_init(void) P2SEL = (P2SEL & ~P2SEL_PRI3P1_MASK) | SPI_PRI; #endif - /* Make the SPI pins be controlled by the USART peripheral */ - SPI_SEL |= SPI_BITS | CSS; - - /* Set up OUT DMA */ - ao_spi_dma_out_id = ao_dma_alloc(&ao_spi_dma_out_done); - - /* Set up IN DMA */ - ao_spi_dma_in_id = ao_dma_alloc(&ao_spi_dma_in_done); - - /* Set up the USART. - * - * SPI master/slave mode - */ - SPI_CSR = (UxCSR_MODE_SPI | UxCSR_RE | UxCSR_DIRECTION); - - /* Set the baud rate and signal parameters - * - * The cc1111 is limited to a 24/8 MHz SPI clock. - * Every peripheral I've ever seen goes faster than that, - * so set the clock to 3MHz (BAUD_E 17, BAUD_M 0) - */ - SPI_BAUD = 0; - SPI_GCR = (UxGCR_CPOL_NEGATIVE | - UxGCR_CPHA_FIRST_EDGE | - UxGCR_ORDER_MSB | - (17 << UxGCR_BAUD_E_SHIFT)); +#if HAS_SPI_0 + SPI_INIT(0, 0); +#endif +#if HAS_SPI_1 + SPI_INIT(1, MULTI_SPI); +#endif } diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c index 4b4403a7..7c1acdd1 100644 --- a/src/drivers/ao_ms5607.c +++ b/src/drivers/ao_ms5607.c @@ -22,7 +22,7 @@ #if HAS_MS5607 || HAS_MS5611 static __xdata struct ao_ms5607_prom ms5607_prom; -static uint8_t ms5607_configured; +static __xdata uint8_t ms5607_configured; static void ao_ms5607_start(void) { diff --git a/src/telemini-v2.0/ao_pins.h b/src/telemini-v2.0/ao_pins.h index c27f47f1..9ecd076e 100644 --- a/src/telemini-v2.0/ao_pins.h +++ b/src/telemini-v2.0/ao_pins.h @@ -47,7 +47,9 @@ */ #define HAS_SPI_0 1 +#define SPI_0_ALT_1 1 #define HAS_SPI_1 1 +#define SPI_1_ALT_2 1 #define SPI_CS_PORT P1 #define SPI_CS_SEL P1SEL #define SPI_CS_DIR P1DIR @@ -55,6 +57,7 @@ /* * Flash */ +#define AO_M25_SPI_BUS 1 #define AO_M25_SPI_CS_PORT SPI_CS_PORT #define AO_M25_SPI_CS_MASK 0x04 /* cs_flash is P1_2 */ #define M25_MAX_CHIPS 1 @@ -74,6 +77,7 @@ #define AO_MS5607_MISO_PIN 2 #define AO_MS5607_MISO_MASK (1 << AO_MS5607_MISO_PIN) #define AO_MS5607_SPI_INDEX 0 +#define HAS_EXTI_0 1 /* * Igniters diff --git a/src/telemini-v2.0/ao_telemini.c b/src/telemini-v2.0/ao_telemini.c index b85ce8c8..6b7545fb 100644 --- a/src/telemini-v2.0/ao_telemini.c +++ b/src/telemini-v2.0/ao_telemini.c @@ -17,6 +17,7 @@ #include "ao.h" #include "ao_pins.h" +#include __xdata uint8_t ao_force_freq; -- cgit v1.2.3 From e72147e215a982ce701099626424b9a856ac9d09 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 25 Aug 2013 22:33:30 -0700 Subject: altos: Changes required by cc1111 multi-spi support These drivers got missed Signed-off-by: Keith Packard --- src/drivers/ao_74hc165.c | 6 +++--- src/drivers/ao_pca9922.c | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_74hc165.c b/src/drivers/ao_74hc165.c index f24fce37..143f4e3f 100644 --- a/src/drivers/ao_74hc165.c +++ b/src/drivers/ao_74hc165.c @@ -27,12 +27,12 @@ uint8_t ao_74hc165_read(void) { static __xdata state; - ao_mutex_get(&ao_spi_mutex); - ao_spi_set_speed(AO_SPI_SPEED_FAST); + 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_mutex_put(&ao_spi_mutex); + ao_spi_put(AO_74HC165_SPI_BUS); return state; } diff --git a/src/drivers/ao_pca9922.c b/src/drivers/ao_pca9922.c index fe070b88..d376b968 100644 --- a/src/drivers/ao_pca9922.c +++ b/src/drivers/ao_pca9922.c @@ -30,12 +30,12 @@ ao_led_apply(void) /* Don't try the SPI bus during initialization */ if (!ao_cur_task) return; - ao_mutex_get(&ao_spi_mutex); - ao_spi_set_speed(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_PCA9922_CS = 0; - ao_mutex_put(&ao_spi_mutex); + ao_spi_put(AO_PCA9922_SPI_BUS); } void -- cgit v1.2.3 From 7274b77666df9d2cab2854ec1a403d80e5fce73b Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 26 Aug 2013 17:18:17 -0700 Subject: altos: Use %ld and %lu for MS5607 debug output The value are 'long', so use the right printf format. Signed-off-by: Keith Packard --- src/drivers/ao_ms5607.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c index 7c1acdd1..8b2b6333 100644 --- a/src/drivers/ao_ms5607.c +++ b/src/drivers/ao_ms5607.c @@ -223,8 +223,8 @@ ao_ms5607_dump(void) __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)); } -- cgit v1.2.3 From 7e941695aa27e5eaf453ca1128b8d835472410a4 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 26 Aug 2013 18:43:20 -0700 Subject: altos: Check for MS5607 MISO low before sleeping If the MISO line goes low before we manage to configure the interrupts, we'll miss it entirely unless we check the pin explicitly. Signed-off-by: Keith Packard --- src/attiny/ao_arch_funcs.h | 2 ++ src/cc1111/ao_arch_funcs.h | 1 + src/drivers/ao_ms5607.c | 3 ++- src/telemini-v2.0/ao_pins.h | 1 + 4 files changed, 6 insertions(+), 1 deletion(-) (limited to 'src/drivers') diff --git a/src/attiny/ao_arch_funcs.h b/src/attiny/ao_arch_funcs.h index 76dc7820..d4584a9f 100644 --- a/src/attiny/ao_arch_funcs.h +++ b/src/attiny/ao_arch_funcs.h @@ -41,6 +41,8 @@ PORTB &= ~(1 << bit); \ } while (0) +#define ao_gpio_get(port, bit, pin) ((PORTB >> (bit)) & 1) + /* * The SPI mutex must be held to call either of these * functions -- this mutex covers the entire SPI operation, diff --git a/src/cc1111/ao_arch_funcs.h b/src/cc1111/ao_arch_funcs.h index ae184108..ea340dfd 100644 --- a/src/cc1111/ao_arch_funcs.h +++ b/src/cc1111/ao_arch_funcs.h @@ -145,4 +145,5 @@ ao_spi_init(void); #define ao_enable_output(port,bit,pin,v) cc1111_enable_output(port,token_evaluator(port,DIR), token_evaluator(port,SEL), pin, bit, v) #define ao_gpio_set(port, bit, pin, v) ((pin) = (v)) +#define ao_gpio_get(port, bit, pin) (pin) diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c index 8b2b6333..58ab9197 100644 --- a/src/drivers/ao_ms5607.c +++ b/src/drivers/ao_ms5607.c @@ -140,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 diff --git a/src/telemini-v2.0/ao_pins.h b/src/telemini-v2.0/ao_pins.h index c1a36f8c..264ad16d 100644 --- a/src/telemini-v2.0/ao_pins.h +++ b/src/telemini-v2.0/ao_pins.h @@ -74,6 +74,7 @@ #define AO_MS5607_CS_MASK (1 << AO_MS5607_CS_PIN) #define AO_MS5607_MISO_PORT P0 #define AO_MS5607_MISO_PIN 2 +#define AO_MS5607_MISO P0_2 #define AO_MS5607_MISO_MASK (1 << AO_MS5607_MISO_PIN) #define AO_MS5607_SPI_INDEX 0 #define HAS_EXTI_0 1 -- cgit v1.2.3 From 61163980f096d555a843e25cd9fe1aec93bbbbba Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 28 Aug 2013 22:02:48 -0600 Subject: altos: Add debugging to ublox GPS driver The new max 7 parts seem to be unhappy about switching baud rates, so I've added a pile of debugging to help out. Some day, I'll figure out how to make them work, this code is being left in place to help with that. Signed-off-by: Keith Packard --- src/drivers/ao_gps_ublox.c | 113 +++++++++++++++++++++++++++++++++++++-------- 1 file changed, 94 insertions(+), 19 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index fa6ff0e0..a11ca3f7 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -21,6 +21,10 @@ #include "ao_gps_ublox.h" +#define AO_UBLOX_DEBUG 1 + +#include + __xdata uint8_t ao_gps_mutex; __pdata uint16_t ao_gps_tick; __xdata struct ao_telemetry_location ao_gps_data; @@ -49,7 +53,34 @@ struct ao_ublox_cksum { static __pdata struct ao_ublox_cksum ao_ublox_cksum; static __pdata uint16_t ao_ublox_len; -#define ao_ublox_byte() ((uint8_t) ao_gps_getchar()) +#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 + +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) { @@ -65,6 +96,7 @@ static void ao_ublox_init_cksum(void) static void ao_ublox_put_u8(uint8_t c) { add_cksum(&ao_ublox_cksum, c); + ao_gps_dbg(DBG_CHAR, " (%02x)", c); ao_gps_putchar(c); } @@ -187,6 +219,7 @@ ao_ublox_parse(void __xdata *target, const struct ublox_packet_parse *parse) __r break; } } + ao_gps_dbg(DBG_PROTO, "\n"); } /* @@ -330,6 +363,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) { @@ -338,6 +372,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 } /* @@ -406,42 +451,52 @@ ao_ublox_parse_nav_velned(void) */ static void -ao_gps_setup(void) +ao_gps_delay(void) { - uint8_t i, k; - - ao_delay(AO_SEC_TO_TICKS(3)); - - ao_gps_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_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_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_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_gps_putchar(0x00); + ao_gps_delay(); } static void @@ -556,7 +611,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, @@ -587,6 +642,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; @@ -627,8 +684,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(); @@ -654,7 +713,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; @@ -676,7 +735,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]; @@ -704,8 +763,24 @@ 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_show, "g\0Display GPS" }, + { ao_gps_option, "g\0Display GPS" }, { 0, NULL }, }; -- cgit v1.2.3 From 4887af0bf90661a3fdca76f1797a704888edab06 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 28 Aug 2013 22:04:18 -0600 Subject: altos: Force u-blox to 9600 baud for now The Max-7 parts just aren't happy switching baud rates, managing only about half the time. Someday I'll figure out why, but until then, make things work by just leaving the chips at 9600 baud Signed-off-by: Keith Packard --- src/drivers/ao_gps_ublox.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index a11ca3f7..1bc2a68f 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -30,21 +30,27 @@ __pdata uint16_t ao_gps_tick; __xdata struct ao_telemetry_location ao_gps_data; __xdata struct ao_telemetry_satellite ao_gps_tracking_data; +#undef AO_SERIAL_SPEED_UBLOX + #ifndef AO_SERIAL_SPEED_UBLOX -#define AO_SERIAL_SPEED_UBLOX AO_SERIAL_SPEED_57600 +#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*2d\r\n"; +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; -- cgit v1.2.3 From 5c4b3658a96f1a64ccebf7bddda06b15b4ac4a6f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 14 Oct 2013 21:49:39 -0700 Subject: altos: Use #define values for ublox packet types One case was using hex values instead of the #define equivalents. Signed-off-by: Keith Packard --- src/drivers/ao_gps_ublox.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index 1bc2a68f..e9168348 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -702,9 +702,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(); -- cgit v1.2.3 From 039446f54ef6968a3f0b37ce32ca6bdcdbe62546 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 14 Oct 2013 22:41:43 -0700 Subject: altos: Merge GPS logging into a single function Create a new global, ao_gps_new, which indicates new GPS position and satellite data. Use ao_gps_new as the new sleep/wakeup address. Merge the separate gps position/satellite logging tasks into a single function which waits for new data and writes out the changed values. Signed-off-by: Keith Packard --- src/core/ao.h | 4 ++ src/core/ao_flight.c | 4 +- src/core/ao_gps_report.c | 102 ++++++++++++++++------------------- src/core/ao_gps_report_mega.c | 88 +++++++++++++------------------ src/core/ao_gps_report_metrum.c | 108 +++++++++++++++++--------------------- src/drivers/ao_gps_sirf.c | 7 ++- src/drivers/ao_gps_skytraq.c | 7 ++- src/drivers/ao_gps_ublox.c | 5 +- src/product/ao_terraui.c | 2 +- src/teleballoon-v1.1/ao_balloon.c | 4 +- src/test/ao_gps_test.c | 13 +++-- src/test/ao_gps_test_skytraq.c | 2 +- src/test/ao_gps_test_ublox.c | 2 +- 13 files changed, 164 insertions(+), 184 deletions(-) (limited to 'src/drivers') diff --git a/src/core/ao.h b/src/core/ao.h index e7320327..ea37885e 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -342,6 +342,10 @@ ao_spi_slave(void); #define AO_GPS_DATE_VALID (1 << 6) #define AO_GPS_COURSE_VALID (1 << 7) +#define AO_GPS_NEW_DATA 1 +#define AO_GPS_NEW_TRACKING 2 + +extern __xdata uint8_t ao_gps_new; extern __pdata uint16_t ao_gps_tick; extern __xdata uint8_t ao_gps_mutex; extern __xdata struct ao_telemetry_location ao_gps_data; diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 88dc816d..2495a44b 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -194,8 +194,8 @@ ao_flight(void) #if HAS_GPS /* Record current GPS position by waking up GPS log tasks */ - 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); #endif ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); diff --git a/src/core/ao_gps_report.c b/src/core/ao_gps_report.c index c52ef621..8d15c083 100644 --- a/src/core/ao_gps_report.c +++ b/src/core/ao_gps_report.c @@ -22,78 +22,68 @@ ao_gps_report(void) { static __xdata struct ao_log_record gps_log; static __xdata struct ao_telemetry_location gps_data; + static __xdata struct ao_telemetry_satellite gps_tracking_data; uint8_t date_reported = 0; + uint8_t new; for (;;) { - ao_sleep(&ao_gps_data); ao_mutex_get(&ao_gps_mutex); - ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); + while ((new = ao_gps_new) == 0) + ao_sleep(&ao_gps_new); + if (new & AO_GPS_NEW_DATA) + ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); + if (new & AO_GPS_NEW_TRACKING) + ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); + ao_gps_new = 0; ao_mutex_put(&ao_gps_mutex); - if (!(gps_data.flags & AO_GPS_VALID)) - continue; - - gps_log.tick = ao_gps_tick; - gps_log.type = AO_LOG_GPS_TIME; - gps_log.u.gps_time.hour = gps_data.hour; - gps_log.u.gps_time.minute = gps_data.minute; - gps_log.u.gps_time.second = gps_data.second; - gps_log.u.gps_time.flags = gps_data.flags; - ao_log_data(&gps_log); - gps_log.type = AO_LOG_GPS_LAT; - gps_log.u.gps_latitude = gps_data.latitude; - ao_log_data(&gps_log); - gps_log.type = AO_LOG_GPS_LON; - gps_log.u.gps_longitude = gps_data.longitude; - ao_log_data(&gps_log); - gps_log.type = AO_LOG_GPS_ALT; - gps_log.u.gps_altitude.altitude = gps_data.altitude; - gps_log.u.gps_altitude.unused = 0xffff; - ao_log_data(&gps_log); - if (!date_reported && (gps_data.flags & AO_GPS_DATE_VALID)) { - gps_log.type = AO_LOG_GPS_DATE; - gps_log.u.gps_date.year = gps_data.year; - gps_log.u.gps_date.month = gps_data.month; - gps_log.u.gps_date.day = gps_data.day; - gps_log.u.gps_date.extra = 0; - date_reported = ao_log_data(&gps_log); + if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) { + gps_log.tick = ao_gps_tick; + gps_log.type = AO_LOG_GPS_TIME; + gps_log.u.gps_time.hour = gps_data.hour; + gps_log.u.gps_time.minute = gps_data.minute; + gps_log.u.gps_time.second = gps_data.second; + gps_log.u.gps_time.flags = gps_data.flags; + ao_log_data(&gps_log); + gps_log.type = AO_LOG_GPS_LAT; + gps_log.u.gps_latitude = gps_data.latitude; + ao_log_data(&gps_log); + gps_log.type = AO_LOG_GPS_LON; + gps_log.u.gps_longitude = gps_data.longitude; + ao_log_data(&gps_log); + gps_log.type = AO_LOG_GPS_ALT; + gps_log.u.gps_altitude.altitude = gps_data.altitude; + gps_log.u.gps_altitude.unused = 0xffff; + ao_log_data(&gps_log); + if (!date_reported && (gps_data.flags & AO_GPS_DATE_VALID)) { + gps_log.type = AO_LOG_GPS_DATE; + gps_log.u.gps_date.year = gps_data.year; + gps_log.u.gps_date.month = gps_data.month; + gps_log.u.gps_date.day = gps_data.day; + gps_log.u.gps_date.extra = 0; + date_reported = ao_log_data(&gps_log); + } } - } -} - -void -ao_gps_tracking_report(void) -{ - static __xdata struct ao_log_record gps_log; - static __xdata struct ao_telemetry_satellite gps_tracking_data; - uint8_t c, n; - - for (;;) { - ao_sleep(&ao_gps_tracking_data); - ao_mutex_get(&ao_gps_mutex); - gps_log.tick = ao_gps_tick; - ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); - ao_mutex_put(&ao_gps_mutex); + if (new & AO_GPS_NEW_TRACKING) { + uint8_t c, n; - if (!(n = gps_tracking_data.channels)) - continue; - - gps_log.type = AO_LOG_GPS_SAT; - for (c = 0; c < n; c++) - if ((gps_log.u.gps_sat.svid = gps_tracking_data.sats[c].svid)) - { - gps_log.u.gps_sat.c_n = gps_tracking_data.sats[c].c_n_1; - ao_log_data(&gps_log); + if ((n = gps_tracking_data.channels) != 0) { + gps_log.type = AO_LOG_GPS_SAT; + for (c = 0; c < n; c++) + if ((gps_log.u.gps_sat.svid = gps_tracking_data.sats[c].svid)) + { + gps_log.u.gps_sat.c_n = gps_tracking_data.sats[c].c_n_1; + ao_log_data(&gps_log); + } } + } } } __xdata struct ao_task ao_gps_report_task; -__xdata struct ao_task ao_gps_tracking_report_task; void ao_gps_report_init(void) { ao_add_task(&ao_gps_report_task, ao_gps_report, "gps_report"); - ao_add_task(&ao_gps_tracking_report_task, ao_gps_tracking_report, "gps_tracking_report"); } diff --git a/src/core/ao_gps_report_mega.c b/src/core/ao_gps_report_mega.c index e3af4307..e2adbfbc 100644 --- a/src/core/ao_gps_report_mega.c +++ b/src/core/ao_gps_report_mega.c @@ -23,66 +23,55 @@ ao_gps_report_mega(void) { static __xdata struct ao_log_mega gps_log; static __xdata struct ao_telemetry_location gps_data; - uint8_t date_reported = 0; - - for (;;) { - ao_sleep(&ao_gps_data); - ao_mutex_get(&ao_gps_mutex); - ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); - ao_mutex_put(&ao_gps_mutex); - - if (!(gps_data.flags & AO_GPS_VALID)) - continue; - - gps_log.tick = ao_gps_tick; - gps_log.type = AO_LOG_GPS_TIME; - gps_log.u.gps.latitude = gps_data.latitude; - gps_log.u.gps.longitude = gps_data.longitude; - gps_log.u.gps.altitude = gps_data.altitude; - - gps_log.u.gps.hour = gps_data.hour; - gps_log.u.gps.minute = gps_data.minute; - gps_log.u.gps.second = gps_data.second; - gps_log.u.gps.flags = gps_data.flags; - gps_log.u.gps.year = gps_data.year; - gps_log.u.gps.month = gps_data.month; - gps_log.u.gps.day = gps_data.day; - ao_log_mega(&gps_log); - } -} - -void -ao_gps_tracking_report_mega(void) -{ - static __xdata struct ao_log_mega gps_log; static __xdata struct ao_telemetry_satellite gps_tracking_data; + uint8_t date_reported = 0; + uint8_t new; uint8_t c, n, i; for (;;) { - ao_sleep(&ao_gps_tracking_data); ao_mutex_get(&ao_gps_mutex); - ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); + while (!(new = ao_gps_new)) + ao_sleep(&ao_gps_new); + if (new & AO_GPS_NEW_DATA) + ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); + if (new & AO_GPS_NEW_TRACKING) + ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); + ao_gps_new = 0; ao_mutex_put(&ao_gps_mutex); - if (!(n = gps_tracking_data.channels)) - continue; + if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) { + gps_log.tick = ao_gps_tick; + gps_log.type = AO_LOG_GPS_TIME; + gps_log.u.gps.latitude = gps_data.latitude; + gps_log.u.gps.longitude = gps_data.longitude; + gps_log.u.gps.altitude = gps_data.altitude; - gps_log.type = AO_LOG_GPS_SAT; - gps_log.tick = ao_gps_tick; - i = 0; - for (c = 0; c < n; c++) - if ((gps_log.u.gps_sat.sats[i].svid = gps_tracking_data.sats[c].svid)) - { - gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1; - i++; - } - gps_log.u.gps_sat.channels = i; - ao_log_mega(&gps_log); + gps_log.u.gps.hour = gps_data.hour; + gps_log.u.gps.minute = gps_data.minute; + gps_log.u.gps.second = gps_data.second; + gps_log.u.gps.flags = gps_data.flags; + gps_log.u.gps.year = gps_data.year; + gps_log.u.gps.month = gps_data.month; + gps_log.u.gps.day = gps_data.day; + ao_log_mega(&gps_log); + } + if ((new & AO_GPS_NEW_TRACKING) && (n = gps_tracking_data.channels) != 0) { + gps_log.tick = ao_gps_tick; + gps_log.type = AO_LOG_GPS_SAT; + i = 0; + for (c = 0; c < n; c++) + if ((gps_log.u.gps_sat.sats[i].svid = gps_tracking_data.sats[c].svid)) + { + gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1; + i++; + } + gps_log.u.gps_sat.channels = i; + ao_log_mega(&gps_log); + } } } __xdata struct ao_task ao_gps_report_mega_task; -__xdata struct ao_task ao_gps_tracking_report_mega_task; void ao_gps_report_mega_init(void) @@ -90,7 +79,4 @@ ao_gps_report_mega_init(void) ao_add_task(&ao_gps_report_mega_task, ao_gps_report_mega, "gps_report"); - ao_add_task(&ao_gps_tracking_report_mega_task, - ao_gps_tracking_report_mega, - "gps_tracking_report"); } diff --git a/src/core/ao_gps_report_metrum.c b/src/core/ao_gps_report_metrum.c index 4fefe223..b82936dd 100644 --- a/src/core/ao_gps_report_metrum.c +++ b/src/core/ao_gps_report_metrum.c @@ -23,80 +23,69 @@ ao_gps_report_metrum(void) { static __xdata struct ao_log_metrum gps_log; static __xdata struct ao_telemetry_location gps_data; - uint8_t date_reported = 0; - - for (;;) { - ao_sleep(&ao_gps_data); - ao_mutex_get(&ao_gps_mutex); - ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); - ao_mutex_put(&ao_gps_mutex); - - if (!(gps_data.flags & AO_GPS_VALID)) - continue; - - gps_log.tick = ao_gps_tick; - gps_log.type = AO_LOG_GPS_POS; - gps_log.u.gps.latitude = gps_data.latitude; - gps_log.u.gps.longitude = gps_data.longitude; - gps_log.u.gps.altitude = gps_data.altitude; - ao_log_metrum(&gps_log); - - gps_log.type = AO_LOG_GPS_TIME; - gps_log.u.gps_time.hour = gps_data.hour; - gps_log.u.gps_time.minute = gps_data.minute; - gps_log.u.gps_time.second = gps_data.second; - gps_log.u.gps_time.flags = gps_data.flags; - gps_log.u.gps_time.year = gps_data.year; - gps_log.u.gps_time.month = gps_data.month; - gps_log.u.gps_time.day = gps_data.day; - ao_log_metrum(&gps_log); - } -} - -void -ao_gps_tracking_report_metrum(void) -{ - static __xdata struct ao_log_metrum gps_log; static __xdata struct ao_telemetry_satellite gps_tracking_data; uint8_t c, n, i, p, valid, packets; uint8_t svid; + uint8_t date_reported = 0; for (;;) { - ao_sleep(&ao_gps_tracking_data); ao_mutex_get(&ao_gps_mutex); - ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); + while (!(new = ao_gps_new)) + ao_sleep(&ao_gps_new); + if (new & AO_GPS_NEW_DATA) + ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); + if (new & AO_GPS_NEW_TRACKING) + ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); + ao_gps_new = 0; ao_mutex_put(&ao_gps_mutex); - if (!(n = gps_tracking_data.channels)) - continue; + if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) { + gps_log.tick = ao_gps_tick; + gps_log.type = AO_LOG_GPS_POS; + gps_log.u.gps.latitude = gps_data.latitude; + gps_log.u.gps.longitude = gps_data.longitude; + gps_log.u.gps.altitude = gps_data.altitude; + ao_log_metrum(&gps_log); - gps_log.type = AO_LOG_GPS_SAT; - gps_log.tick = ao_gps_tick; - i = 0; - for (c = 0; c < n; c++) { - svid = gps_tracking_data.sats[c].svid; - if (svid != 0) { - if (i == 4) { - gps_log.u.gps_sat.channels = i; - gps_log.u.gps_sat.more = 1; - ao_log_metrum(&gps_log); - i = 0; + gps_log.type = AO_LOG_GPS_TIME; + gps_log.u.gps_time.hour = gps_data.hour; + gps_log.u.gps_time.minute = gps_data.minute; + gps_log.u.gps_time.second = gps_data.second; + gps_log.u.gps_time.flags = gps_data.flags; + gps_log.u.gps_time.year = gps_data.year; + gps_log.u.gps_time.month = gps_data.month; + gps_log.u.gps_time.day = gps_data.day; + ao_log_metrum(&gps_log); + } + + if ((new & AO_GPS_NEW_TRACKING) && (n = gps_tracking_data.channels)) { + gps_log.type = AO_LOG_GPS_SAT; + gps_log.tick = ao_gps_tick; + i = 0; + for (c = 0; c < n; c++) { + svid = gps_tracking_data.sats[c].svid; + if (svid != 0) { + if (i == 4) { + gps_log.u.gps_sat.channels = i; + gps_log.u.gps_sat.more = 1; + ao_log_metrum(&gps_log); + i = 0; + } + gps_log.u.gps_sat.sats[i].svid = svid; + gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1; + i++; } - gps_log.u.gps_sat.sats[i].svid = svid; - gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1; - i++; } - } - if (i) { - gps_log.u.gps_sat.channels = i; - gps_log.u.gps_sat.more = 0; - ao_log_metrum(&gps_log); + if (i) { + gps_log.u.gps_sat.channels = i; + gps_log.u.gps_sat.more = 0; + ao_log_metrum(&gps_log); + } } } } __xdata struct ao_task ao_gps_report_metrum_task; -__xdata struct ao_task ao_gps_tracking_report_metrum_task; void ao_gps_report_metrum_init(void) @@ -104,7 +93,4 @@ ao_gps_report_metrum_init(void) ao_add_task(&ao_gps_report_metrum_task, ao_gps_report_metrum, "gps_report"); - ao_add_task(&ao_gps_tracking_report_metrum_task, - ao_gps_tracking_report_metrum, - "gps_tracking_report"); } 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 9a9dff75..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); } } diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index e9168348..3582d6e0 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -25,6 +25,7 @@ #include +__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; @@ -760,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; diff --git a/src/product/ao_terraui.c b/src/product/ao_terraui.c index 1866eb0c..8fd97033 100644 --- a/src/product/ao_terraui.c +++ b/src/product/ao_terraui.c @@ -629,7 +629,7 @@ ao_terragps(void) for (;;) { while (ao_gps_tick == gps_tick) - ao_sleep(&ao_gps_data); + ao_sleep(&ao_gps_new); gps_tick = ao_gps_tick; ao_gps_progress = (ao_gps_progress + 1) & 3; } diff --git a/src/teleballoon-v1.1/ao_balloon.c b/src/teleballoon-v1.1/ao_balloon.c index e8972655..12752d1f 100644 --- a/src/teleballoon-v1.1/ao_balloon.c +++ b/src/teleballoon-v1.1/ao_balloon.c @@ -105,8 +105,8 @@ ao_flight(void) #if HAS_GPS /* Record current GPS position by waking up GPS log tasks */ - 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); #endif ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); diff --git a/src/test/ao_gps_test.c b/src/test/ao_gps_test.c index 3844a326..b6cc9ba7 100644 --- a/src/test/ao_gps_test.c +++ b/src/test/ao_gps_test.c @@ -427,11 +427,18 @@ void ao_dump_state(void *wchan) { int i; - if (wchan == &ao_gps_data) + + if (wchan != &ao_gps_new) + return; + + if (ao_gps_new & AO_GPS_NEW_DATA) { ao_gps_print(&ao_gps_data); - else + putchar('\n'); + } + if (ao_gps_new & AO_GPS_NEW_TRACKING) { ao_gps_tracking_print(&ao_gps_tracking_data); - putchar('\n'); + putchar('\n'); + } return; printf ("%02d:%02d:%02d", ao_gps_data.hour, ao_gps_data.minute, diff --git a/src/test/ao_gps_test_skytraq.c b/src/test/ao_gps_test_skytraq.c index 89cbd767..bf2ab5b8 100644 --- a/src/test/ao_gps_test_skytraq.c +++ b/src/test/ao_gps_test_skytraq.c @@ -443,7 +443,7 @@ uint8_t ao_task_minimize_latency; void ao_dump_state(void *wchan) { - if (wchan == &ao_gps_data) + if (wchan == &ao_gps_new) ao_gps_show(); } diff --git a/src/test/ao_gps_test_ublox.c b/src/test/ao_gps_test_ublox.c index a0e04cb6..31c7af60 100644 --- a/src/test/ao_gps_test_ublox.c +++ b/src/test/ao_gps_test_ublox.c @@ -347,7 +347,7 @@ check_ublox_message(char *which, uint8_t *msg) void ao_dump_state(void *wchan) { - if (wchan == &ao_gps_data) + if (wchan == &ao_gps_new) ao_gps_show(); return; } -- cgit v1.2.3 From ba99630f33440b993c69830856d2a7741ffdef71 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 25 Oct 2013 04:03:39 -0700 Subject: altos: Fix GPS test frameworks to handle shared ao_gps_new variable Signed-off-by: Keith Packard --- src/drivers/ao_gps_ublox.c | 2 +- src/test/ao_gps_test.c | 3 +++ src/test/ao_gps_test_skytraq.c | 3 +++ src/test/ao_gps_test_ublox.c | 3 +++ 4 files changed, 10 insertions(+), 1 deletion(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index 3582d6e0..4fb90746 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -21,7 +21,7 @@ #include "ao_gps_ublox.h" -#define AO_UBLOX_DEBUG 1 +#define AO_UBLOX_DEBUG 0 #include diff --git a/src/test/ao_gps_test.c b/src/test/ao_gps_test.c index b6cc9ba7..e799ab0f 100644 --- a/src/test/ao_gps_test.c +++ b/src/test/ao_gps_test.c @@ -26,6 +26,9 @@ #define AO_GPS_NUM_SAT_MASK (0xf << 0) #define AO_GPS_NUM_SAT_SHIFT (0) +#define AO_GPS_NEW_DATA 1 +#define AO_GPS_NEW_TRACKING 2 + #define AO_GPS_VALID (1 << 4) #define AO_GPS_RUNNING (1 << 5) #define AO_GPS_DATE_VALID (1 << 6) diff --git a/src/test/ao_gps_test_skytraq.c b/src/test/ao_gps_test_skytraq.c index bf2ab5b8..1b590d5e 100644 --- a/src/test/ao_gps_test_skytraq.c +++ b/src/test/ao_gps_test_skytraq.c @@ -26,6 +26,9 @@ #define AO_GPS_NUM_SAT_MASK (0xf << 0) #define AO_GPS_NUM_SAT_SHIFT (0) +#define AO_GPS_NEW_DATA 1 +#define AO_GPS_NEW_TRACKING 2 + #define AO_GPS_VALID (1 << 4) #define AO_GPS_RUNNING (1 << 5) #define AO_GPS_DATE_VALID (1 << 6) diff --git a/src/test/ao_gps_test_ublox.c b/src/test/ao_gps_test_ublox.c index 31c7af60..4eb4b837 100644 --- a/src/test/ao_gps_test_ublox.c +++ b/src/test/ao_gps_test_ublox.c @@ -26,6 +26,9 @@ #define AO_GPS_NUM_SAT_MASK (0xf << 0) #define AO_GPS_NUM_SAT_SHIFT (0) +#define AO_GPS_NEW_DATA 1 +#define AO_GPS_NEW_TRACKING 2 + #define AO_GPS_VALID (1 << 4) #define AO_GPS_RUNNING (1 << 5) #define AO_GPS_DATE_VALID (1 << 6) -- cgit v1.2.3 From 08143a922fe27bc50a19924f46538f9476ab5fd1 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 25 Oct 2013 04:05:09 -0700 Subject: altos: Add gyro-based orientation tracking This tracks the angle-from-vertical as an additional input to the pyro channels. Signed-off-by: Keith Packard --- src/core/ao_data.h | 6 +- src/core/ao_flight.c | 4 + src/core/ao_kalman.c | 3 - src/core/ao_pyro.c | 4 +- src/core/ao_quaternion.h | 116 +++++++++++++++++++ src/core/ao_sample.c | 98 +++++++++++++++- src/core/ao_sample.h | 3 +- src/core/ao_telemetry.c | 1 + src/core/ao_telemetry.h | 2 +- src/drivers/ao_mpu6000.c | 2 + src/drivers/ao_mpu6000.h | 17 ++- src/stm/Makefile.defs | 4 +- src/telemega-v0.3/Makefile | 16 +++ src/test/Makefile | 9 +- src/test/ao_flight_test.c | 259 ++---------------------------------------- src/test/ao_quaternion_test.c | 67 +++++++++++ 16 files changed, 343 insertions(+), 268 deletions(-) create mode 100644 src/core/ao_quaternion.h create mode 100644 src/test/ao_quaternion_test.c (limited to 'src/drivers') diff --git a/src/core/ao_data.h b/src/core/ao_data.h index 339afe69..5a232885 100644 --- a/src/core/ao_data.h +++ b/src/core/ao_data.h @@ -18,6 +18,8 @@ #ifndef _AO_DATA_H_ #define _AO_DATA_H_ +#define GRAVITY 9.80665 + #if HAS_ADC #define AO_DATA_ADC (1 << 0) #else @@ -300,8 +302,8 @@ typedef int16_t accel_t; #define HAS_GYRO 1 -typedef int16_t gyro_t; -typedef int32_t angle_t; +typedef int16_t gyro_t; /* in raw sample units */ +typedef int16_t angle_t; /* in degrees */ /* Y axis is aligned with the direction of motion (along) */ /* X axis is aligned in the other board axis (across) */ diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 2495a44b..240b348a 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -20,6 +20,10 @@ #include #endif +#if HAS_MPU6000 +#include +#endif + #ifndef HAS_ACCEL #error Please define HAS_ACCEL #endif diff --git a/src/core/ao_kalman.c b/src/core/ao_kalman.c index 762b2c0a..7fd4f889 100644 --- a/src/core/ao_kalman.c +++ b/src/core/ao_kalman.c @@ -292,7 +292,4 @@ ao_kalman(void) else #endif ao_avg_height = (ao_avg_height_scaled + 63) >> 7; -#ifdef AO_FLIGHT_TEST - ao_sample_prev_tick = ao_sample_tick; -#endif } diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c index 531e1f50..24c9fe99 100644 --- a/src/core/ao_pyro.c +++ b/src/core/ao_pyro.c @@ -113,7 +113,7 @@ ao_pyro_ready(struct ao_pyro *pyro) continue; break; -#if HAS_ORIENT +#if HAS_GYRO case ao_pyro_orient_less: if (ao_orient <= pyro->orient_less) continue; @@ -310,7 +310,7 @@ const struct { { "h<", ao_pyro_height_less, offsetof(struct ao_pyro, height_less), HELP("height less (m)") }, { "h>", ao_pyro_height_greater, offsetof(struct ao_pyro, height_greater), HELP("height greater (m)") }, -#if HAS_ORIENT +#if HAS_GYRO { "o<", ao_pyro_orient_less, offsetof(struct ao_pyro, orient_less), HELP("orient less (deg)") }, { "o>", ao_pyro_orient_greater, offsetof(struct ao_pyro, orient_greater), HELP("orient greater (deg)") }, #endif diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h new file mode 100644 index 00000000..f4b8aaa4 --- /dev/null +++ b/src/core/ao_quaternion.h @@ -0,0 +1,116 @@ +/* + * Copyright © 2013 Keith Packard + * + * 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_QUATERNION_H_ +#define _AO_QUATERNION_H_ + +#include + +struct ao_quaternion { + float r; /* real bit */ + float x, y, z; /* imaginary bits */ +}; + +static inline void ao_quaternion_multiply(struct ao_quaternion *r, + struct ao_quaternion *a, + struct ao_quaternion *b) +{ + struct ao_quaternion t; +#define T(_a,_b) (((a)->_a) * ((b)->_b)) + t.r = T(r,r) - T(x,x) - T(y,y) - T(z,z); + t.x = T(r,x) + T(x,r) + T(y,z) - T(z,y); + t.y = T(r,y) - T(x,z) + T(y,r) + T(z,x); + t.z = T(r,z) + T(x,y) - T(y,x) + T(z,r); +#undef T + *r = t; +} + +static inline void ao_quaternion_conjugate(struct ao_quaternion *r, + struct ao_quaternion *a) +{ + r->r = a->r; + r->x = -a->x; + r->y = -a->y; + r->z = -a->z; +} + +static inline float ao_quaternion_normal(struct ao_quaternion *a) +{ +#define S(_a) (((a)->_a) * ((a)->_a)) + return S(r) + S(x) + S(y) + S(z); +#undef S +} + +static inline void ao_quaternion_scale(struct ao_quaternion *r, + struct ao_quaternion *a, + float b) +{ + r->r = a->r * b; + r->x = a->x * b; + r->y = a->y * b; + r->z = a->z * b; +} + +static inline void ao_quaternion_normalize(struct ao_quaternion *r, + struct ao_quaternion *a) +{ + float n = ao_quaternion_normal(a); + + if (n > 0) + ao_quaternion_scale(r, a, 1/sqrtf(n)); + else + *r = *a; +} + +static inline void ao_quaternion_rotate(struct ao_quaternion *r, + struct ao_quaternion *a, + struct ao_quaternion *b) +{ + struct ao_quaternion c; + struct ao_quaternion t; + + ao_quaternion_conjugate(&c, b); + ao_quaternion_multiply(&t, b, a); + ao_quaternion_multiply(r, &t, &c); +} + +static inline void ao_quaternion_init_vector(struct ao_quaternion *r, + float x, float y, float z) +{ + r->r = 0; + r->x = x; + r->y = y; + r->z = z; +} + +static inline void ao_quaternion_init_rotation(struct ao_quaternion *r, + float x, float y, float z, + float s, float c) +{ + r->r = c; + r->x = s * x; + r->y = s * y; + r->z = s * z; +} + +static inline void ao_quaternion_init_zero_rotation(struct ao_quaternion *r) +{ + r->r = 1; + r->x = r->y = r->z = 0; +} + +#endif /* _AO_QUATERNION_H_ */ diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c index dec44f9f..676e0ffd 100644 --- a/src/core/ao_sample.c +++ b/src/core/ao_sample.c @@ -20,6 +20,10 @@ #include #endif +#if HAS_GYRO +#include +#endif + /* * Current sensor values */ @@ -44,8 +48,7 @@ __pdata accel_t ao_sample_accel_through; __pdata gyro_t ao_sample_roll; __pdata gyro_t ao_sample_pitch; __pdata gyro_t ao_sample_yaw; -__pdata angle_t ao_sample_angle; -__pdata angle_t ao_sample_roll_angle; +__pdata angle_t ao_orient; #endif __data uint8_t ao_sample_data; @@ -86,6 +89,8 @@ __pdata int32_t ao_sample_accel_through_sum; __pdata int32_t ao_sample_pitch_sum; __pdata int32_t ao_sample_yaw_sum; __pdata int32_t ao_sample_roll_sum; +static struct ao_quaternion ao_rotation; +static struct ao_quaternion ao_pad_orientation; #endif static void @@ -129,11 +134,91 @@ ao_sample_preflight_set(void) ao_sample_pitch_sum = 0; ao_sample_yaw_sum = 0; ao_sample_roll_sum = 0; - ao_sample_angle = 0; + ao_orient = 0; + + /* No rotation yet */ + ao_quaternion_init_zero_rotation(&ao_rotation); + + /* XXX Assume we're pointing straight up for now */ + ao_quaternion_init_vector(&ao_pad_orientation, + ao_ground_accel_across, + ao_ground_accel_through, + -ao_ground_accel_along); + ao_quaternion_normalize(&ao_pad_orientation, + &ao_pad_orientation); + + printf ("pad r%8.5f x%8.5f y%8.5f z%8.5f\n", + ao_pad_orientation.r, + ao_pad_orientation.x, + ao_pad_orientation.y, + ao_pad_orientation.z); #endif nsamples = 0; } +#if HAS_GYRO +static void +ao_sample_rotate(void) +{ +#ifdef AO_FLIGHT_TEST + float dt = (ao_sample_tick - ao_sample_prev_tick) / 100.0; +#else + static const float dt = 1/100.0; +#endif + float x = ao_mpu6000_gyro(ao_sample_pitch - ao_ground_pitch) * dt; + float y = ao_mpu6000_gyro(ao_sample_yaw - ao_ground_yaw) * dt; + float z = ao_mpu6000_gyro(ao_sample_roll - ao_ground_roll) * dt; + + float n_2, n; + float s, c; + + struct ao_quaternion rot; + struct ao_quaternion point; + + /* The amount of rotation is just the length of the vector. Now, + * here's the trick -- assume that the rotation amount is small. In this case, + * sin(x) ≃ x, so we can just make this the sin. + */ + + n_2 = x*x + y*y + z*z; + n = sqrtf(n_2); + s = n / 2; + if (s > 1) + s = 1; + c = sqrtf(1 - s*s); + + /* Make unit vector */ + if (n > 0) { + x /= n; + y /= n; + z /= n; + } + + /* Now compute the unified rotation quaternion */ + + ao_quaternion_init_rotation(&rot, + x, y, z, + s, c); + + /* Integrate with the previous rotation amount */ + ao_quaternion_multiply(&ao_rotation, &ao_rotation, &rot); + + /* And normalize to make sure it remains a unit vector */ + ao_quaternion_normalize(&ao_rotation, &ao_rotation); + + /* Compute pitch angle from vertical by taking the pad + * orientation vector and rotating it by the current total + * rotation value. That will be a unit vector pointing along + * the airframe axis. The Z value will be the cosine of the + * change in the angle from vertical since boost + */ + + ao_quaternion_rotate(&point, &ao_pad_orientation, &ao_rotation); + + ao_orient = acosf(point.z) * (float) (180.0/M_PI); +} +#endif + static void ao_sample_preflight(void) { @@ -232,9 +317,12 @@ ao_sample(void) ao_sample_preflight_update(); ao_kalman(); #if HAS_GYRO - /* do quaternion stuff here... */ + ao_sample_rotate(); #endif } +#ifdef AO_FLIGHT_TEST + ao_sample_prev_tick = ao_sample_tick; +#endif ao_sample_data = ao_data_ring_next(ao_sample_data); } return !ao_preflight; @@ -264,7 +352,7 @@ ao_sample_init(void) ao_sample_pitch = 0; ao_sample_yaw = 0; ao_sample_roll = 0; - ao_sample_angle = 0; + ao_orient = 0; #endif ao_sample_data = ao_data_head; ao_preflight = TRUE; diff --git a/src/core/ao_sample.h b/src/core/ao_sample.h index 5bd29536..1d1bcc7c 100644 --- a/src/core/ao_sample.h +++ b/src/core/ao_sample.h @@ -64,8 +64,6 @@ * for all further flight computations */ -#define GRAVITY 9.80665 - /* * Above this height, the baro sensor doesn't work */ @@ -118,6 +116,7 @@ extern __pdata accel_t ao_ground_accel_through; extern __pdata gyro_t ao_ground_pitch; extern __pdata gyro_t ao_ground_yaw; extern __pdata gyro_t ao_ground_roll; +extern __pdata angle_t ao_orient; #endif void ao_sample_init(void); diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c index 6b47a06a..a2726016 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -115,6 +115,7 @@ ao_send_mega_sensor(void) telemetry.generic.tick = packet->tick; telemetry.generic.type = AO_TELEMETRY_MEGA_SENSOR; + telemetry.mega_sensor.orient = ao_orient; telemetry.mega_sensor.accel = ao_data_accel(packet); telemetry.mega_sensor.pres = ao_data_pres(packet); telemetry.mega_sensor.temp = ao_data_temp(packet); diff --git a/src/core/ao_telemetry.h b/src/core/ao_telemetry.h index fd6b76b3..237a35ab 100644 --- a/src/core/ao_telemetry.h +++ b/src/core/ao_telemetry.h @@ -162,7 +162,7 @@ struct ao_telemetry_mega_sensor { uint16_t tick; /* 2 */ uint8_t type; /* 4 */ - uint8_t pad5; /* 5 */ + uint8_t orient; /* 5 angle from vertical */ int16_t accel; /* 6 Z axis */ int32_t pres; /* 8 Pa * 10 */ diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c index 73057820..5e75b78a 100644 --- a/src/drivers/ao_mpu6000.c +++ b/src/drivers/ao_mpu6000.c @@ -118,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) { @@ -129,6 +130,7 @@ 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) diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h index a42f69c4..2241bf80 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 @@ -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(int16_t sensor) { + return (float) 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/stm/Makefile.defs b/src/stm/Makefile.defs index ee1cb658..0710d747 100644 --- a/src/stm/Makefile.defs +++ b/src/stm/Makefile.defs @@ -1,4 +1,4 @@ -vpath % ../stm:../product:../drivers:../core:../util:../kalman:../aes:.. +vpath % ../stm:../product:../drivers:../core:../util:../kalman:../aes:../math:.. vpath make-altitude ../util vpath make-kalman ../util vpath kalman.5c ../kalman @@ -24,7 +24,7 @@ include $(TOPDIR)/Makedefs CC=$(ARM_CC) LIBS=-lpdclib-cortex-m3 -lgcc -AO_CFLAGS=-I. -I../stm -I../core -I../drivers -I.. +AO_CFLAGS=-I. -I../stm -I../core -I../drivers -I../math -I.. STM_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m3 -mthumb -ffreestanding -nostdlib $(AO_CFLAGS) LDFLAGS=-L../stm -Wl,-Taltos.ld diff --git a/src/telemega-v0.3/Makefile b/src/telemega-v0.3/Makefile index 6e5da721..2f460105 100644 --- a/src/telemega-v0.3/Makefile +++ b/src/telemega-v0.3/Makefile @@ -27,6 +27,7 @@ INC = \ ao_sample_profile.h \ ao_mpu.h \ stm32l.h \ + math.h \ Makefile # @@ -44,6 +45,20 @@ INC = \ #STACK_GUARD=ao_mpu_stm.c #STACK_GUARD_DEF=-DHAS_STACK_GUARD=1 +MATH_SRC=\ + ef_acos.c \ + ef_sqrt.c + +# ef_rem_pio2.c \ +# kf_cos.c \ +# kf_sin.c \ +# kf_rem_pio2.c \ +# sf_copysign.c \ +# sf_cos.c \ +# sf_fabs.c \ +# sf_floor.c \ +# sf_scalbn.c + ALTOS_SRC = \ ao_boot_chain.c \ ao_interrupt.c \ @@ -93,6 +108,7 @@ ALTOS_SRC = \ ao_companion.c \ ao_pyro.c \ ao_aprs.c \ + $(MATH_SRC) \ $(PROFILE) \ $(SAMPLE_PROFILE) \ $(STACK_GUARD) diff --git a/src/test/Makefile b/src/test/Makefile index 5eee6bbb..f64a9531 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -1,15 +1,15 @@ -vpath % ..:../core:../drivers:../util:../micropeak:../aes +vpath % ..:../core:../drivers:../util:../micropeak:../aes:../product PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_flight_test_mm \ ao_gps_test ao_gps_test_skytraq ao_gps_test_ublox ao_convert_test ao_convert_pa_test ao_fec_test \ ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test \ - ao_ms5607_convert_test + ao_ms5607_convert_test ao_quaternion_test INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h KALMAN=make-kalman -CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -O0 -g -Wall +CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -I../product -O0 -g -Wall all: $(PROGS) ao_aprs_data.wav @@ -80,3 +80,6 @@ ao_int64_test: ao_int64_test.c ao_int64.c ao_int64.h ao_ms5607_convert_test: ao_ms5607_convert_test.c ao_ms5607_convert_8051.c ao_int64.c ao_int64.h cc $(CFLAGS) -o $@ ao_ms5607_convert_test.c + +ao_quaternion_test: ao_quaternion_test.c ao_quaternion.h + cc $(CFLAGS) -o $@ ao_quaternion_test.c -lm \ No newline at end of file diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index faf31aa7..e2f63e34 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -25,6 +25,8 @@ #include #include +#define GRAVITY 9.80665 + #define AO_HERTZ 100 #define HAS_ADC 1 @@ -36,6 +38,11 @@ #define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16)) #define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16)) +#define AO_GPS_NEW_DATA 1 +#define AO_GPS_NEW_TRACKING 2 + +int ao_gps_new; + #if TELEMEGA #define AO_ADC_NUM_SENSE 6 #define HAS_MS5607 1 @@ -240,7 +247,6 @@ struct ao_config ao_config; #define DATA_TO_XDATA(x) (x) -#define GRAVITY 9.80665 extern int16_t ao_ground_accel, ao_flight_accel; extern int16_t ao_accel_2g; @@ -339,20 +345,6 @@ ao_test_exit(void) exit(0); } -#if HAS_MPU6000 -static double -ao_mpu6000_accel(int16_t sensor) -{ - return sensor / 32767.0 * MPU6000_ACCEL_FULLSCALE * GRAVITY; -} - -static double -ao_mpu6000_gyro(int32_t sensor) -{ - return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE; -} -#endif - void ao_insert(void) { @@ -408,23 +400,23 @@ ao_insert(void) if (!ao_summary) { printf("%7.2f height %8.2f accel %8.3f " #if TELEMEGA - "roll %8.3f angle %8.3f qangle %8.3f " - "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f " + "angle %5d " +/* "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f " */ #endif "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n", time, height, accel, #if TELEMEGA - ao_mpu6000_gyro(ao_sample_roll_angle) / 100.0, - ao_mpu6000_gyro(ao_sample_angle) / 100.0, - ao_sample_qangle, + ao_orient, +/* ao_mpu6000_accel(ao_data_static.mpu6000.accel_x), ao_mpu6000_accel(ao_data_static.mpu6000.accel_y), ao_mpu6000_accel(ao_data_static.mpu6000.accel_z), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z), +*/ #endif ao_state_names[ao_flight_state], ao_k_height / 65536.0, @@ -589,207 +581,6 @@ int32(uint8_t *bytes, int off) static int log_format; -#if TELEMEGA - -static double -ao_vec_norm(double x, double y, double z) -{ - return x*x + y*y + z*z; -} - -static void -ao_vec_normalize(double *x, double *y, double *z) -{ - double scale = 1/sqrt(ao_vec_norm(*x, *y, *z)); - - *x *= scale; - *y *= scale; - *z *= scale; -} - -struct ao_quat { - double q0, q1, q2, q3; -}; - -static void -ao_quat_mul(struct ao_quat *r, struct ao_quat *a, struct ao_quat *b) -{ - r->q0 = a->q0 * b->q0 - a->q1 * b->q1 - a->q2 * b->q2 - a->q3 * b->q3; - r->q1 = a->q0 * b->q1 + a->q1 * b->q0 + a->q2 * b->q3 - a->q3 * b->q2; - r->q2 = a->q0 * b->q2 - a->q1 * b->q3 + a->q2 * b->q0 + a->q3 * b->q1; - r->q3 = a->q0 * b->q3 + a->q1 * b->q2 - a->q2 * b->q1 + a->q3 * b->q0; -} - -#if 0 -static void -ao_quat_scale(struct ao_quat *r, struct ao_quat *a, double s) -{ - r->q0 = a->q0 * s; - r->q1 = a->q1 * s; - r->q2 = a->q2 * s; - r->q3 = a->q3 * s; -} -#endif - -static void -ao_quat_conj(struct ao_quat *r, struct ao_quat *a) -{ - r->q0 = a->q0; - r->q1 = -a->q1; - r->q2 = -a->q2; - r->q3 = -a->q3; -} - -static void -ao_quat_rot(struct ao_quat *r, struct ao_quat *a, struct ao_quat *q) -{ - struct ao_quat t; - struct ao_quat c; - ao_quat_mul(&t, q, a); - ao_quat_conj(&c, q); - ao_quat_mul(r, &t, &c); -} - -static void -ao_quat_from_angle(struct ao_quat *r, - double x_rad, - double y_rad, - double z_rad) -{ - double angle = sqrt (x_rad * x_rad + y_rad * y_rad + z_rad * z_rad); - double s = sin(angle/2); - double c = cos(angle/2); - - r->q0 = c; - r->q1 = x_rad * s / angle; - r->q2 = y_rad * s / angle; - r->q3 = z_rad * s / angle; -} - -static void -ao_quat_from_vector(struct ao_quat *r, double x, double y, double z) -{ - ao_vec_normalize(&x, &y, &z); - double x_rad = atan2(z, y); - double y_rad = atan2(x, z); - double z_rad = atan2(y, x); - - ao_quat_from_angle(r, x_rad, y_rad, z_rad); -} - -static double -ao_quat_norm(struct ao_quat *a) -{ - return (a->q0 * a->q0 + - a->q1 * a->q1 + - a->q2 * a->q2 + - a->q3 * a->q3); -} - -static void -ao_quat_normalize(struct ao_quat *a) -{ - double norm = ao_quat_norm(a); - - if (norm) { - double m = 1/sqrt(norm); - - a->q0 *= m; - a->q1 *= m; - a->q2 *= m; - a->q3 *= m; - } -} - -static struct ao_quat ao_up, ao_current; -static struct ao_quat ao_orient; -static int ao_orient_tick; - -void -set_orientation(double x, double y, double z, int tick) -{ - struct ao_quat t; - - printf ("set_orientation %g %g %g\n", x, y, z); - ao_quat_from_vector(&ao_orient, x, y, z); - ao_up.q1 = ao_up.q2 = 0; - ao_up.q0 = ao_up.q3 = sqrt(2)/2; - ao_orient_tick = tick; - - ao_orient.q0 = 1; - ao_orient.q1 = 0; - ao_orient.q2 = 0; - ao_orient.q3 = 0; - - printf ("orient (%g) %g %g %g up (%g) %g %g %g\n", - ao_orient.q0, - ao_orient.q1, - ao_orient.q2, - ao_orient.q3, - ao_up.q0, - ao_up.q1, - ao_up.q2, - ao_up.q3); - - ao_quat_rot(&t, &ao_up, &ao_orient); - printf ("pad orient (%g) %g %g %g\n", - t.q0, - t.q1, - t.q2, - t.q3); - -} - -void -update_orientation (double rate_x, double rate_y, double rate_z, int tick) -{ - struct ao_quat q_dot; - double lambda; - double dt = (tick - ao_orient_tick) / 100.0; - - ao_orient_tick = tick; - -// lambda = 1 - ao_quat_norm(&ao_orient); - lambda = 0; - - q_dot.q0 = -0.5 * (ao_orient.q1 * rate_x + ao_orient.q2 * rate_y + ao_orient.q3 * rate_z) + lambda * ao_orient.q0; - q_dot.q1 = 0.5 * (ao_orient.q0 * rate_x + ao_orient.q2 * rate_z - ao_orient.q3 * rate_y) + lambda * ao_orient.q1; - q_dot.q2 = 0.5 * (ao_orient.q0 * rate_y + ao_orient.q3 * rate_x - ao_orient.q1 * rate_z) + lambda * ao_orient.q2; - q_dot.q3 = 0.5 * (ao_orient.q0 * rate_z + ao_orient.q1 * rate_y - ao_orient.q2 * rate_x) + lambda * ao_orient.q3; - -#if 0 - printf ("update_orientation %g %g %g (%g s)\n", rate_x, rate_y, rate_z, dt); - printf ("q_dot (%g) %g %g %g\n", - q_dot.q0, - q_dot.q1, - q_dot.q2, - q_dot.q3); -#endif - - ao_orient.q0 += q_dot.q0 * dt; - ao_orient.q1 += q_dot.q1 * dt; - ao_orient.q2 += q_dot.q2 * dt; - ao_orient.q3 += q_dot.q3 * dt; - - ao_quat_normalize(&ao_orient); - - ao_quat_rot(&ao_current, &ao_up, &ao_orient); - - ao_sample_qangle = 180 / M_PI * acos(ao_current.q3 * sqrt(2)); -#if 0 - printf ("orient (%g) %g %g %g current (%g) %g %g %g\n", - ao_orient.q0, - ao_orient.q1, - ao_orient.q2, - ao_orient.q3, - ao_current.q0, - ao_current.q1, - ao_current.q2, - ao_current.q3); -#endif -} -#endif - void ao_sleep(void *wchan) { @@ -872,32 +663,6 @@ ao_sleep(void *wchan) #if HAS_MMA655X ao_data_static.mma655x = int16(bytes, 26); #endif - if (ao_records_read == 0) - ao_ground_mpu6000 = ao_data_static.mpu6000; - else if (ao_records_read < 10) { -#define f(f) ao_ground_mpu6000.f = ao_ground_mpu6000.f + ((ao_data_static.mpu6000.f - ao_ground_mpu6000.f) >> 2) - f(accel_x); - f(accel_y); - f(accel_z); - f(gyro_x); - f(gyro_y); - f(gyro_z); - - double accel_x = ao_mpu6000_accel(ao_ground_mpu6000.accel_x); - double accel_y = ao_mpu6000_accel(ao_ground_mpu6000.accel_y); - double accel_z = ao_mpu6000_accel(ao_ground_mpu6000.accel_z); - - /* X and Y are in the ground plane, arbitraryily picked as MPU X and Z axes - * Z is normal to the ground, the MPU y axis - */ - set_orientation(accel_x, accel_z, accel_y, tick); - } else { - double rate_x = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x); - double rate_y = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y); - double rate_z = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z); - - update_orientation(rate_x * M_PI / 180, rate_z * M_PI / 180, rate_y * M_PI / 180, tick); - } ao_records_read++; ao_insert(); return; diff --git a/src/test/ao_quaternion_test.c b/src/test/ao_quaternion_test.c new file mode 100644 index 00000000..0e4b5b07 --- /dev/null +++ b/src/test/ao_quaternion_test.c @@ -0,0 +1,67 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#define _GNU_SOURCE + +#include +#include +#include +#include +#include +#include +#include + +#include "ao_quaternion.h" + +static void +print_q(char *name, struct ao_quaternion *q) +{ + printf ("%8.8s: r%8.5f x%8.5f y%8.5f z%8.5f ", name, + q->r, q->x, q->y, q->z); +} + +int main(int argc, char **argv) +{ + struct ao_quaternion position; + struct ao_quaternion rotation; + struct ao_quaternion little_rotation; + int i; + + /* unit x vector */ + ao_quaternion_init_vector(&position, 1, 0, 0); + + /* zero rotation */ + ao_quaternion_init_zero_rotation(&rotation); + + /* π/16 rotation around Z axis */ + ao_quaternion_init_rotation(&little_rotation, 0, 0, 1, + sin((M_PI/16)/2), + cos((M_PI/16)/2)); + for (i = 0; i <= 16; i++) { + struct ao_quaternion rotated; + + ao_quaternion_rotate(&rotated, &position, &rotation); + print_q("position", &position); + print_q("rotated", &rotated); + print_q("rotation", &rotation); + printf ("\n"); + ao_quaternion_multiply(&rotation, &rotation, &little_rotation); + ao_quaternion_normalize(&rotation, &rotation); + } + return 0; +} + -- cgit v1.2.3 From 195fd70cdc7f519cd8d4ac323088ed0b6c188280 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Oct 2013 23:42:58 -0700 Subject: altos: Change ao_mpu6000_gyro arg to float This lets callers pass more precision than just the original sensor value Signed-off-by: Keith Packard --- src/drivers/ao_mpu6000.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h index 2241bf80..dc3a9fbf 100644 --- a/src/drivers/ao_mpu6000.h +++ b/src/drivers/ao_mpu6000.h @@ -173,8 +173,8 @@ #define MPU6000_GYRO_FULLSCALE ((float) 2000 * M_PI/180.0) static inline float -ao_mpu6000_gyro(int16_t sensor) { - return (float) sensor * ((float) (MPU6000_GYRO_FULLSCALE / 32767.0)); +ao_mpu6000_gyro(float sensor) { + return sensor * ((float) (MPU6000_GYRO_FULLSCALE / 32767.0)); } #define MPU6000_ACCEL_FULLSCALE 16 -- cgit v1.2.3 From 9d2eb0b00a5a0faefce95bce949be7206b0aad37 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 12 Nov 2013 14:48:21 +0900 Subject: Add ublox checksum generating program --- src/drivers/ublox-csum.5c | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) create mode 100644 src/drivers/ublox-csum.5c (limited to 'src/drivers') 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); +} + + -- cgit v1.2.3 From ee4279613b4757453d0d8f8afc06037c61eeb520 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 18 Dec 2013 20:32:05 -0800 Subject: altos: Try IMU self-test 10 times before giving up This should keep the device from failing to boot unless the IMU is actually broken. Oh, and if self test does fail, this places the flight computer in 'Invalid' state rather than panic. Signed-off-by: Keith Packard --- src/core/ao_flight.c | 8 ++++ src/core/ao_flight.h | 4 ++ src/drivers/ao_mpu6000.c | 104 ++++++++++++++++++++++++-------------------- src/telemega-v0.1/ao_pins.h | 1 + src/telemega-v1.0/ao_pins.h | 3 +- 5 files changed, 70 insertions(+), 50 deletions(-) (limited to 'src/drivers') diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 4a53bdc6..463ff4a2 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -46,6 +46,11 @@ __pdata enum ao_flight_state ao_flight_state; /* current flight state */ __pdata uint16_t ao_boost_tick; /* time of launch detect */ __pdata uint16_t ao_motor_number; /* number of motors burned so far */ +#if HAS_IMU +/* Any sensor can set this to mark the flight computer as 'broken' */ +__xdata uint8_t ao_sensor_errors; +#endif + /* * track min/max data over a long interval to detect * resting @@ -99,6 +104,9 @@ ao_flight(void) ao_config.accel_minus_g == 0 || ao_ground_accel < ao_config.accel_plus_g - ACCEL_NOSE_UP || ao_ground_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP || +#if HAS_IMU + ao_sensor_errors || +#endif ao_ground_height < -1000 || ao_ground_height > 7000) { diff --git a/src/core/ao_flight.h b/src/core/ao_flight.h index ac3e9cfb..c7c02ccf 100644 --- a/src/core/ao_flight.h +++ b/src/core/ao_flight.h @@ -41,6 +41,10 @@ extern __pdata enum ao_flight_state ao_flight_state; extern __pdata uint16_t ao_boost_tick; extern __pdata uint16_t ao_motor_number; +#if HAS_IMU +extern __xdata uint8_t ao_sensor_errors; +#endif + extern __pdata uint16_t ao_launch_time; extern __pdata uint8_t ao_flight_force_idle; diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c index 5e75b78a..f8ce7346 100644 --- a/src/drivers/ao_mpu6000.c +++ b/src/drivers/ao_mpu6000.c @@ -137,10 +137,10 @@ 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; @@ -153,10 +153,10 @@ 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; @@ -177,11 +177,14 @@ _ao_mpu6000_wait_alive(void) 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; @@ -236,23 +239,6 @@ _ao_mpu6000_setup(void) (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); - #if TRIDGE // read the product ID rev c has 1/2 the sensitivity of rev d _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID); @@ -268,36 +254,58 @@ _ao_mpu6000_setup(void) 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, diff --git a/src/telemega-v0.1/ao_pins.h b/src/telemega-v0.1/ao_pins.h index 7ba3a1a7..daeb9f17 100644 --- a/src/telemega-v0.1/ao_pins.h +++ b/src/telemega-v0.1/ao_pins.h @@ -316,6 +316,7 @@ struct ao_adc { #define AO_MPU6000_INT_PORT (&stm_gpioc) #define AO_MPU6000_INT_PIN 13 #define AO_MPU6000_I2C_INDEX STM_I2C_INDEX(1) +#define HAS_IMU 1 /* * mma655x diff --git a/src/telemega-v1.0/ao_pins.h b/src/telemega-v1.0/ao_pins.h index 7ff676ea..95644dae 100644 --- a/src/telemega-v1.0/ao_pins.h +++ b/src/telemega-v1.0/ao_pins.h @@ -318,8 +318,7 @@ struct ao_adc { #define AO_MPU6000_SPI_BUS AO_SPI_1_PE13_PE14_PE15 #define AO_MPU6000_SPI_CS_PORT (&stm_gpiod) #define AO_MPU6000_SPI_CS_PIN 2 - -#define HAS_HIGHG_ACCEL 1 +#define HAS_IMU 1 /* * mma655x -- cgit v1.2.3