diff options
Diffstat (limited to 'src')
32 files changed, 471 insertions, 181 deletions
diff --git a/src/avr/ao_usb_avr.c b/src/avr/ao_usb_avr.c index 9ba407af..2ef546c9 100644 --- a/src/avr/ao_usb_avr.c +++ b/src/avr/ao_usb_avr.c @@ -480,10 +480,10 @@ ao_usb_putchar(char c) __critical __reentrant ao_usb_in_flushed = 0; } -static char +static int _ao_usb_pollchar(void) { - char c; + uint8_t c; uint8_t intx; if (!ao_usb_running) @@ -517,10 +517,10 @@ _ao_usb_pollchar(void) return c; } -char +int ao_usb_pollchar(void) { - char c; + int c; cli(); c = _ao_usb_pollchar(); sei(); @@ -530,7 +530,7 @@ ao_usb_pollchar(void) char ao_usb_getchar(void) __critical { - char c; + int c; cli(); while ((c = _ao_usb_pollchar()) == AO_READ_AGAIN) diff --git a/src/cc1111/ao_adc.c b/src/cc1111/ao_adc.c index f8000410..bfdc418a 100644 --- a/src/cc1111/ao_adc.c +++ b/src/cc1111/ao_adc.c @@ -56,7 +56,7 @@ ao_adc_isr(void) __interrupt 1 uint8_t __xdata *a; sequence = (ADCCON2 & ADCCON2_SCH_MASK) >> ADCCON2_SCH_SHIFT; -#if TELEMETRUM_V_0_1 || TELEMETRUM_V_0_2 || TELEMETRUM_V_1_0 || TELEMETRUM_V_1_1 || TELEMETRUM_V_1_2 || TELELAUNCH_V_0_1 +#if TELEMETRUM_V_0_1 || TELEMETRUM_V_0_2 || TELEMETRUM_V_1_0 || TELEMETRUM_V_1_1 || TELEMETRUM_V_1_2 || TELELAUNCH_V_0_1 || TELEBALLOON_V_1_1 /* TeleMetrum readings */ #if HAS_ACCEL_REF if (sequence == 2) { diff --git a/src/cc1111/ao_dbg.c b/src/cc1111/ao_dbg.c index 847b5aaf..4e534697 100644 --- a/src/cc1111/ao_dbg.c +++ b/src/cc1111/ao_dbg.c @@ -193,54 +193,39 @@ ao_dbg_long_delay(void) #define AO_RESET_LOW_DELAY AO_MS_TO_TICKS(100) #define AO_RESET_HIGH_DELAY AO_MS_TO_TICKS(100) -void -ao_dbg_debug_mode(void) +static void +ao_dbg_send_bits_delay(uint8_t msk, uint8_t val) { - ao_dbg_set_pins(); ao_dbg_long_delay(); - ao_dbg_send_bits(DBG_CLOCK|DBG_DATA|DBG_RESET_N, DBG_CLOCK|DBG_DATA|DBG_RESET_N); - ao_dbg_long_delay(); - ao_dbg_send_bits(DBG_CLOCK|DBG_DATA|DBG_RESET_N, 0 |DBG_DATA| 0 ); - ao_delay(AO_RESET_LOW_DELAY); - ao_dbg_send_bits(DBG_CLOCK|DBG_DATA|DBG_RESET_N, DBG_CLOCK|DBG_DATA| 0 ); - ao_dbg_long_delay(); - ao_dbg_send_bits(DBG_CLOCK|DBG_DATA|DBG_RESET_N, 0 |DBG_DATA| 0 ); - ao_dbg_long_delay(); - ao_dbg_send_bits(DBG_CLOCK|DBG_DATA|DBG_RESET_N, DBG_CLOCK|DBG_DATA| 0 ); - ao_dbg_long_delay(); - ao_dbg_send_bits(DBG_CLOCK|DBG_DATA|DBG_RESET_N, 0 |DBG_DATA|DBG_RESET_N); - ao_delay(AO_RESET_HIGH_DELAY); + ao_dbg_send_bits(msk, val); } void -ao_dbg_reset(void) +ao_dbg_do_reset(uint8_t clock_up) { ao_dbg_set_pins(); - ao_dbg_long_delay(); - ao_dbg_send_bits(DBG_CLOCK|DBG_DATA|DBG_RESET_N, DBG_CLOCK|DBG_DATA|DBG_RESET_N); - ao_dbg_long_delay(); - ao_dbg_send_bits(DBG_CLOCK|DBG_DATA|DBG_RESET_N, DBG_CLOCK|DBG_DATA| 0 ); + ao_dbg_send_bits_delay(DBG_CLOCK|DBG_DATA|DBG_RESET_N, DBG_CLOCK|DBG_DATA|DBG_RESET_N); + ao_dbg_send_bits_delay(DBG_CLOCK|DBG_DATA|DBG_RESET_N, clock_up |DBG_DATA| 0 ); ao_delay(AO_RESET_LOW_DELAY); - ao_dbg_send_bits(DBG_CLOCK|DBG_DATA|DBG_RESET_N, DBG_CLOCK|DBG_DATA| 0 ); - ao_dbg_long_delay(); - ao_dbg_send_bits(DBG_CLOCK|DBG_DATA|DBG_RESET_N, DBG_CLOCK|DBG_DATA| 0 ); - ao_dbg_long_delay(); - ao_dbg_send_bits(DBG_CLOCK|DBG_DATA|DBG_RESET_N, DBG_CLOCK|DBG_DATA| 0 ); - ao_dbg_long_delay(); - ao_dbg_send_bits(DBG_CLOCK|DBG_DATA|DBG_RESET_N, DBG_CLOCK|DBG_DATA|DBG_RESET_N); + ao_dbg_send_bits (DBG_CLOCK|DBG_DATA|DBG_RESET_N, DBG_CLOCK|DBG_DATA| 0 ); + ao_dbg_send_bits_delay(DBG_CLOCK|DBG_DATA|DBG_RESET_N, clock_up |DBG_DATA| 0 ); + ao_dbg_send_bits_delay(DBG_CLOCK|DBG_DATA|DBG_RESET_N, DBG_CLOCK|DBG_DATA| 0 ); + ao_dbg_send_bits_delay(DBG_CLOCK|DBG_DATA|DBG_RESET_N, clock_up |DBG_DATA|DBG_RESET_N); ao_delay(AO_RESET_HIGH_DELAY); } static void debug_enable(void) { - ao_dbg_debug_mode(); + /* toggle clock line while holding reset low */ + ao_dbg_do_reset(0); } static void debug_reset(void) { - ao_dbg_reset(); + /* hold clock high while holding reset low */ + ao_dbg_do_reset(DBG_CLOCK); } static void @@ -281,22 +266,6 @@ debug_get(void) putchar('\n'); } -static uint8_t -getnibble(void) -{ - __pdata char c; - - c = getchar(); - if ('0' <= c && c <= '9') - return c - '0'; - if ('a' <= c && c <= 'f') - return c - ('a' - 10); - if ('A' <= c && c <= 'F') - return c - ('A' - 10); - ao_cmd_status = ao_cmd_lex_error; - return 0; -} - static void debug_input(void) { @@ -338,8 +307,8 @@ debug_output(void) return; ao_dbg_start_transfer(addr); while (count--) { - b = getnibble() << 4; - b |= getnibble(); + b = ao_getnibble() << 4; + b |= ao_getnibble(); if (ao_cmd_status != ao_cmd_success) return; ao_dbg_write_byte(b); diff --git a/src/cc1111/ao_serial.c b/src/cc1111/ao_serial.c index 48383802..8913a9b0 100644 --- a/src/cc1111/ao_serial.c +++ b/src/cc1111/ao_serial.c @@ -34,8 +34,14 @@ const __code struct ao_serial_speed ao_serial_speeds[] = { /* .baud = */ 59, /* .gcr = */ (11 << UxGCR_BAUD_E_SHIFT) | UxGCR_ORDER_LSB }, + /* [AO_SERIAL_SPEED_115200] = */ { + /* .baud = */ 59, + /* .gcr = */ (12 << UxGCR_BAUD_E_SHIFT) | UxGCR_ORDER_LSB + }, }; +#define AO_SERIAL_SPEED_MAX AO_SERIAL_SPEED_115200 + #if HAS_SERIAL_0 volatile __xdata struct ao_fifo ao_serial0_rx_fifo; @@ -85,10 +91,10 @@ ao_serial0_getchar(void) __critical } #if USE_SERIAL_0_STDIN -char +int ao_serial0_pollchar(void) __critical { - char c; + uint8_t c; if (ao_fifo_empty(ao_serial0_rx_fifo)) return AO_READ_AGAIN; ao_fifo_remove(ao_serial0_rx_fifo,c); @@ -116,7 +122,7 @@ void ao_serial0_set_speed(uint8_t speed) { ao_serial0_drain(); - if (speed > AO_SERIAL_SPEED_57600) + if (speed > AO_SERIAL_SPEED_MAX) return; U0UCR |= UxUCR_FLUSH; U0BAUD = ao_serial_speeds[speed].baud; @@ -173,10 +179,10 @@ ao_serial1_getchar(void) __critical } #if USE_SERIAL_1_STDIN -char +int ao_serial1_pollchar(void) __critical { - char c; + uint8_t c; if (ao_fifo_empty(ao_serial1_rx_fifo)) return AO_READ_AGAIN; ao_fifo_remove(ao_serial1_rx_fifo,c); @@ -204,7 +210,7 @@ void ao_serial1_set_speed(uint8_t speed) { ao_serial1_drain(); - if (speed > AO_SERIAL_SPEED_57600) + if (speed > AO_SERIAL_SPEED_MAX) return; U1UCR |= UxUCR_FLUSH; U1BAUD = ao_serial_speeds[speed].baud; diff --git a/src/cc1111/ao_usb.c b/src/cc1111/ao_usb.c index ce26e808..f66e807c 100644 --- a/src/cc1111/ao_usb.c +++ b/src/cc1111/ao_usb.c @@ -382,19 +382,19 @@ ao_usb_putchar(char c) __critical __reentrant ao_usb_in_send(); } -char +int ao_usb_pollchar(void) __critical { - char c; + uint8_t c; if (ao_usb_out_bytes == 0) { USBINDEX = AO_USB_OUT_EP; if ((USBCSOL & USBCSOL_OUTPKT_RDY) == 0) - return AO_READ_AGAIN; + return -1; ao_usb_out_bytes = (USBCNTH << 8) | USBCNTL; if (ao_usb_out_bytes == 0) { USBINDEX = AO_USB_OUT_EP; USBCSOL &= ~USBCSOL_OUTPKT_RDY; - return AO_READ_AGAIN; + return -1; } } --ao_usb_out_bytes; @@ -409,7 +409,7 @@ ao_usb_pollchar(void) __critical char ao_usb_getchar(void) __critical { - char c; + int c; while ((c = ao_usb_pollchar()) == AO_READ_AGAIN) ao_sleep(&ao_stdin_ready); diff --git a/src/core/ao.h b/src/core/ao.h index 81d92e72..54018b37 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -170,6 +170,10 @@ ao_cmd_hex(void); void ao_cmd_decimal(void); +/* Read a single hex nibble off stdin. */ +uint8_t +ao_getnibble(void); + uint8_t ao_match_word(__code char *word); @@ -595,10 +599,10 @@ ao_monitor_init(void) __reentrant; * ao_stdio.c */ -#define AO_READ_AGAIN ((char) -1) +#define AO_READ_AGAIN (-1) struct ao_stdio { - char (*pollchar)(void); + int (*pollchar)(void); void (*putchar)(char c) __reentrant; void (*flush)(void); uint8_t echo; @@ -617,7 +621,7 @@ uint8_t ao_echo(void); int8_t -ao_add_stdio(char (*pollchar)(void), +ao_add_stdio(int (*pollchar)(void), void (*putchar)(char) __reentrant, void (*flush)(void)) __reentrant; diff --git a/src/core/ao_cmd.c b/src/core/ao_cmd.c index 1814cecf..3d086a57 100644 --- a/src/core/ao_cmd.c +++ b/src/core/ao_cmd.c @@ -110,6 +110,22 @@ putnibble(uint8_t v) putchar(v + ('a' - 10)); } +uint8_t +ao_getnibble(void) +{ + char c; + + c = getchar(); + if ('0' <= c && c <= '9') + return c - '0'; + if ('a' <= c && c <= 'f') + return c - ('a' - 10); + if ('A' <= c && c <= 'F') + return c - ('A' - 10); + ao_cmd_status = ao_cmd_lex_error; + return 0; +} + void ao_cmd_put16(uint16_t v) { @@ -249,12 +265,25 @@ ao_reboot(void) static void version(void) { - printf("manufacturer %s\n", ao_manufacturer); - printf("product %s\n", ao_product); - printf("serial-number %u\n", ao_serial_number); + printf("manufacturer %s\n" + "product %s\n" + "serial-number %u\n" +#if HAS_FLIGHT + "current-flight %u\n" +#endif +#if HAS_LOG + "log-format %u\n" +#endif + , ao_manufacturer + , ao_product + , ao_serial_number +#if HAS_FLIGHT + , ao_flight_number +#endif #if HAS_LOG - printf("log-format %u\n", ao_log_format); + , ao_log_format #endif + ); #if HAS_MS5607 ao_ms5607_info(); #endif diff --git a/src/core/ao_config.c b/src/core/ao_config.c index e85ddcb4..797fe7ec 100644 --- a/src/core/ao_config.c +++ b/src/core/ao_config.c @@ -529,15 +529,15 @@ __code struct ao_config_var ao_config_vars[] = { ao_config_callsign_set, ao_config_callsign_show }, { "e <0 disable, 1 enable>\0Enable telemetry and RDF", ao_config_radio_enable_set, ao_config_radio_enable_show }, + { "f <cal>\0Radio calib (cal = rf/(xtal/2^16))", + ao_config_radio_cal_set, ao_config_radio_cal_show }, #endif /* HAS_RADIO */ #if HAS_ACCEL { "a <+g> <-g>\0Accel calib (0 for auto)", ao_config_accel_calibrate_set,ao_config_accel_calibrate_show }, + { "o <0 antenna up, 1 antenna down>\0Set pad orientation", + ao_config_pad_orientation_set,ao_config_pad_orientation_show }, #endif /* HAS_ACCEL */ -#if HAS_RADIO - { "f <cal>\0Radio calib (cal = rf/(xtal/2^16))", - ao_config_radio_cal_set, ao_config_radio_cal_show }, -#endif /* HAS_RADIO */ #if HAS_LOG { "l <size>\0Flight log size (kB)", ao_config_log_set, ao_config_log_show }, @@ -546,10 +546,6 @@ __code struct ao_config_var ao_config_vars[] = { { "i <0 dual, 1 apogee, 2 main>\0Set igniter mode", ao_config_ignite_mode_set, ao_config_ignite_mode_show }, #endif -#if HAS_ACCEL - { "o <0 antenna up, 1 antenna down>\0Set pad orientation", - ao_config_pad_orientation_set,ao_config_pad_orientation_show }, -#endif #if HAS_AES { "k <32 hex digits>\0Set AES encryption key", ao_config_key_set, ao_config_key_show }, diff --git a/src/core/ao_log_telem.c b/src/core/ao_log_telem.c index 18ab85dd..23ebf7dd 100644 --- a/src/core/ao_log_telem.c +++ b/src/core/ao_log_telem.c @@ -102,9 +102,9 @@ ao_log_single(void) while (ao_log_running) { /* Write samples to EEPROM */ while (ao_log_monitor_pos != ao_monitor_head) { - memcpy(&ao_log_single_write_data.telemetry, - &ao_monitor_ring[ao_log_monitor_pos], - AO_LOG_SINGLE_SIZE); + ao_xmemcpy(&ao_log_single_write_data.telemetry, + &ao_monitor_ring[ao_log_monitor_pos], + AO_LOG_SINGLE_SIZE); ao_log_single_write(); ao_log_monitor_pos = ao_monitor_ring_next(ao_log_monitor_pos); ao_log_telem_track(); diff --git a/src/core/ao_packet.h b/src/core/ao_packet.h index 0eafd3b2..08b184d6 100644 --- a/src/core/ao_packet.h +++ b/src/core/ao_packet.h @@ -62,7 +62,7 @@ ao_packet_flush(void); void ao_packet_putchar(char c) __reentrant; -char +int ao_packet_pollchar(void); #if PACKET_HAS_MASTER diff --git a/src/core/ao_send_packet.c b/src/core/ao_send_packet.c index 1a8e74de..66315d22 100644 --- a/src/core/ao_send_packet.c +++ b/src/core/ao_send_packet.c @@ -21,22 +21,6 @@ static __xdata uint8_t ao_send[AO_MAX_SEND]; -static uint8_t -getnibble(void) -{ - char c; - - c = getchar(); - if ('0' <= c && c <= '9') - return c - '0'; - if ('a' <= c && c <= 'f') - return c - ('a' - 10); - if ('A' <= c && c <= 'F') - return c - ('A' - 10); - ao_cmd_status = ao_cmd_lex_error; - return 0; -} - static void ao_send_packet(void) { @@ -53,8 +37,8 @@ ao_send_packet(void) return; } for (i = 0; i < count; i++) { - b = getnibble() << 4; - b |= getnibble(); + b = ao_getnibble() << 4; + b |= ao_getnibble(); if (ao_cmd_status != ao_cmd_success) return; ao_send[i] = b; diff --git a/src/core/ao_serial.h b/src/core/ao_serial.h index 53aa8a89..a799bf2c 100644 --- a/src/core/ao_serial.h +++ b/src/core/ao_serial.h @@ -22,6 +22,7 @@ #define AO_SERIAL_SPEED_9600 1 #define AO_SERIAL_SPEED_19200 2 #define AO_SERIAL_SPEED_57600 3 +#define AO_SERIAL_SPEED_115200 4 #if HAS_SERIAL_0 extern volatile __xdata struct ao_fifo ao_serial0_rx_fifo; @@ -30,6 +31,9 @@ extern volatile __xdata struct ao_fifo ao_serial0_tx_fifo; char ao_serial0_getchar(void); +int +ao_serial0_pollchar(void); + void ao_serial0_putchar(char c); @@ -47,7 +51,7 @@ extern volatile __xdata struct ao_fifo ao_serial1_tx_fifo; char ao_serial1_getchar(void); -char +int ao_serial1_pollchar(void); void @@ -67,7 +71,7 @@ extern volatile __xdata struct ao_fifo ao_serial2_tx_fifo; char ao_serial2_getchar(void); -char +int ao_serial2_pollchar(void); void @@ -80,6 +84,26 @@ void ao_serial2_set_speed(uint8_t speed); #endif +#if HAS_SERIAL_3 +extern volatile __xdata struct ao_fifo ao_serial3_rx_fifo; +extern volatile __xdata struct ao_fifo ao_serial3_tx_fifo; + +char +ao_serial3_getchar(void); + +int +ao_serial3_pollchar(void); + +void +ao_serial3_putchar(char c); + +void +ao_serial3_drain(void); + +void +ao_serial3_set_speed(uint8_t speed); +#endif + void ao_serial_init(void); diff --git a/src/core/ao_stdio.c b/src/core/ao_stdio.c index 8cf66a23..1748dfe8 100644 --- a/src/core/ao_stdio.c +++ b/src/core/ao_stdio.c @@ -98,7 +98,7 @@ __xdata uint8_t ao_stdin_ready; char getchar(void) __reentrant { - char c; + int c; ao_arch_critical( int8_t stdio = ao_cur_stdio; @@ -123,7 +123,7 @@ ao_echo(void) } int8_t -ao_add_stdio(char (*pollchar)(void), +ao_add_stdio(int (*pollchar)(void), void (*putchar)(char), void (*flush)(void)) __reentrant { diff --git a/src/core/ao_task.c b/src/core/ao_task.c index 0411fbdd..9cb074b5 100644 --- a/src/core/ao_task.c +++ b/src/core/ao_task.c @@ -305,6 +305,8 @@ ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *nam ); } +__data uint8_t ao_task_minimize_latency; + /* Task switching function. This must not use any stack variables */ void ao_yield(void) ao_arch_naked_define @@ -331,7 +333,12 @@ ao_yield(void) ao_arch_naked_define } ao_arch_isr_stack(); - ao_arch_block_interrupts(); +#if !HAS_TASK_QUEUE + if (ao_task_minimize_latency) + ao_arch_release_interrupts(); + else +#endif + ao_arch_block_interrupts(); #if AO_CHECK_STACK in_yield = 1; @@ -374,7 +381,7 @@ ao_yield(void) ao_arch_naked_define break; /* Wait for interrupts when there's nothing ready */ - if (ao_cur_task_index == ao_last_task_index) + if (ao_cur_task_index == ao_last_task_index && !ao_task_minimize_latency) ao_arch_wait_interrupt(); } } diff --git a/src/core/ao_task.h b/src/core/ao_task.h index 049f69a7..50bfb220 100644 --- a/src/core/ao_task.h +++ b/src/core/ao_task.h @@ -47,6 +47,7 @@ struct ao_task { extern __xdata struct ao_task * __xdata ao_tasks[AO_NUM_TASKS]; extern __data uint8_t ao_num_tasks; extern __xdata struct ao_task *__data ao_cur_task; +extern __data uint8_t ao_task_minimize_latency; /* Reduce IRQ latency */ /* ao_task.c diff --git a/src/core/ao_usb.h b/src/core/ao_usb.h index e051db93..4476ee6b 100644 --- a/src/core/ao_usb.h +++ b/src/core/ao_usb.h @@ -33,7 +33,7 @@ ao_usb_getchar(void); /* Poll for a charcter on the USB input queue. * returns AO_READ_AGAIN if none are available */ -char +int ao_usb_pollchar(void); /* Flush the USB output queue */ diff --git a/src/drivers/ao_btm.c b/src/drivers/ao_btm.c index f3816047..c862200a 100644 --- a/src/drivers/ao_btm.c +++ b/src/drivers/ao_btm.c @@ -120,7 +120,7 @@ uint8_t ao_btm_get_line(void) { uint8_t ao_btm_reply_len = 0; - char c; + int c; for (;;) { diff --git a/src/drivers/ao_companion.c b/src/drivers/ao_companion.c index c749adea..0ebe8429 100644 --- a/src/drivers/ao_companion.c +++ b/src/drivers/ao_companion.c @@ -118,10 +118,13 @@ ao_companion_status(void) __reentrant printf("Companion running: %d\n", ao_companion_running); if (!ao_companion_running) return; - printf("device: %d\n", ao_companion_setup.board_id); - printf("update period: %d\n", ao_companion_setup.update_period); - printf("channels: %d\n", ao_companion_setup.channels); - printf("data:"); + printf("device: %d\n" + "update period: %d\n" + "channels: %d\n" + "data:", + ao_companion_setup.board_id, + ao_companion_setup.update_period, + ao_companion_setup.channels); for(i = 0; i < ao_companion_setup.channels; i++) printf(" %5u", ao_companion_data[i]); printf("\n"); diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c index d80da97c..d2f67e6b 100644 --- a/src/drivers/ao_gps_skytraq.c +++ b/src/drivers/ao_gps_skytraq.c @@ -21,6 +21,7 @@ #ifndef ao_gps_getchar #define ao_gps_getchar ao_serial1_getchar +#define ao_gps_fifo ao_serial1_rx_fifo #endif #ifndef ao_gps_putchar @@ -453,6 +454,8 @@ ao_gps_nmea_parse(void) } } +static uint8_t ao_gps_updating; + void ao_gps(void) __reentrant { @@ -468,6 +471,13 @@ ao_gps(void) __reentrant if (ao_gps_getchar() == '$') { ao_gps_nmea_parse(); } +#ifndef AO_GPS_TEST + while (ao_gps_updating) { + ao_usb_putchar(ao_gps_getchar()); + if (ao_fifo_empty(ao_gps_fifo)) + flush(); + } +#endif } } @@ -492,8 +502,38 @@ gps_dump(void) __reentrant ao_mutex_put(&ao_gps_mutex); } +static __code uint8_t ao_gps_115200[] = { + SKYTRAQ_MSG_3(5,0,5,0) /* Set to 115200 baud */ +}; + +static void +ao_gps_set_speed_delay(uint8_t speed) { + ao_delay(AO_MS_TO_TICKS(500)); + ao_gps_set_speed(speed); + ao_delay(AO_MS_TO_TICKS(500)); +} + +static void +gps_update(void) __reentrant +{ + ao_gps_updating = 1; + ao_task_minimize_latency = 1; +#if HAS_ADC + ao_timer_set_adc_interval(0); +#endif + ao_skytraq_sendstruct(ao_gps_115200); + ao_gps_set_speed_delay(AO_SERIAL_SPEED_4800); + ao_skytraq_sendstruct(ao_gps_115200); + ao_gps_set_speed_delay(AO_SERIAL_SPEED_115200); + + /* It's a binary protocol; abandon attempts to escape */ + for (;;) + ao_gps_putchar(ao_usb_getchar()); +} + __code struct ao_cmds ao_gps_cmds[] = { { gps_dump, "g\0Display GPS" }, + { gps_update, "U\0Update GPS firmware" }, { 0, NULL }, }; diff --git a/src/drivers/ao_packet.c b/src/drivers/ao_packet.c index 3c1e7a18..91319923 100644 --- a/src/drivers/ao_packet.c +++ b/src/drivers/ao_packet.c @@ -21,8 +21,8 @@ __xdata struct ao_packet_recv ao_rx_packet; __xdata struct ao_packet ao_tx_packet; __pdata uint8_t ao_packet_rx_len, ao_packet_rx_used, ao_packet_tx_used; -static __xdata char tx_data[AO_PACKET_MAX]; -static __xdata char rx_data[AO_PACKET_MAX]; +static __xdata uint8_t tx_data[AO_PACKET_MAX]; +static __xdata uint8_t rx_data[AO_PACKET_MAX]; static __pdata uint8_t rx_seq; __xdata struct ao_task ao_packet_task; @@ -169,7 +169,7 @@ ao_packet_putchar(char c) __reentrant tx_data[ao_packet_tx_used++] = c; } -char +int ao_packet_pollchar(void) { /* No need to block interrupts, all variables here diff --git a/src/drivers/ao_packet_master.c b/src/drivers/ao_packet_master.c index 481232df..023c788b 100644 --- a/src/drivers/ao_packet_master.c +++ b/src/drivers/ao_packet_master.c @@ -20,7 +20,7 @@ static char ao_packet_getchar(void) { - char c; + int c; while ((c = ao_packet_pollchar()) == AO_READ_AGAIN) { if (!ao_packet_enable) break; @@ -35,7 +35,7 @@ ao_packet_getchar(void) static void ao_packet_echo(void) __reentrant { - char c; + int c; while (ao_packet_enable) { c = ao_packet_getchar(); if (c != AO_READ_AGAIN) diff --git a/src/megametrum-v0.1/ao_pins.h b/src/megametrum-v0.1/ao_pins.h index 5ae80ac5..f07dc26e 100644 --- a/src/megametrum-v0.1/ao_pins.h +++ b/src/megametrum-v0.1/ao_pins.h @@ -62,6 +62,7 @@ #define ao_gps_getchar ao_serial3_getchar #define ao_gps_putchar ao_serial3_putchar #define ao_gps_set_speed ao_serial3_set_speed +#define ao_gps_fifo (ao_stm_usart3.rx_fifo) #define HAS_EEPROM 1 #define USE_INTERNAL_FLASH 0 diff --git a/src/stm/ao_arch.h b/src/stm/ao_arch.h index e270199e..007f7e2e 100644 --- a/src/stm/ao_arch.h +++ b/src/stm/ao_arch.h @@ -123,42 +123,6 @@ void ao_lcd_font_init(void); void ao_lcd_font_string(char *s); -char -ao_serial1_getchar(void); - -void -ao_serial1_putchar(char c); - -char -ao_serial1_pollchar(void); - -void -ao_serial1_set_speed(uint8_t speed); - -char -ao_serial2_getchar(void); - -void -ao_serial2_putchar(char c); - -char -ao_serial2_pollchar(void); - -void -ao_serial2_set_speed(uint8_t speed); - -char -ao_serial3_getchar(void); - -void -ao_serial3_putchar(char c); - -char -ao_serial3_pollchar(void); - -void -ao_serial3_set_speed(uint8_t speed); - extern const uint32_t ao_radio_cal; void diff --git a/src/stm/ao_arch_funcs.h b/src/stm/ao_arch_funcs.h index d6ab1465..87bbe73e 100644 --- a/src/stm/ao_arch_funcs.h +++ b/src/stm/ao_arch_funcs.h @@ -210,6 +210,26 @@ ao_i2c_recv(void *block, uint16_t len, uint8_t i2c_index, uint8_t stop); void ao_i2c_init(void); +/* ao_serial_stm.c */ +struct ao_stm_usart { + struct ao_fifo rx_fifo; + struct ao_fifo tx_fifo; + struct stm_usart *reg; + uint8_t tx_started; +}; + +#if HAS_SERIAL_1 +extern struct ao_stm_usart ao_stm_usart1; +#endif + +#if HAS_SERIAL_2 +extern struct ao_stm_usart ao_stm_usart2; +#endif + +#if HAS_SERIAL_3 +extern struct ao_stm_usart ao_stm_usart3; +#endif + #define ARM_PUSH32(stack, val) (*(--(stack)) = (val)) static inline uint32_t diff --git a/src/stm/ao_serial_stm.c b/src/stm/ao_serial_stm.c index 00409f4a..ce33f97e 100644 --- a/src/stm/ao_serial_stm.c +++ b/src/stm/ao_serial_stm.c @@ -17,13 +17,6 @@ #include <ao.h> -struct ao_stm_usart { - struct ao_fifo rx_fifo; - struct ao_fifo tx_fifo; - struct stm_usart *reg; - uint8_t tx_started; -}; - void ao_debug_out(char c) { @@ -78,16 +71,19 @@ ao_usart_getchar(struct ao_stm_usart *usart) return c; } -char +int ao_usart_pollchar(struct ao_stm_usart *usart) { - char c; + int c; ao_arch_block_interrupts(); if (ao_fifo_empty(usart->rx_fifo)) c = AO_READ_AGAIN; - else - ao_fifo_remove(usart->rx_fifo,c); + else { + uint8_t u; + ao_fifo_remove(usart->rx_fifo,u); + c = u; + } ao_arch_release_interrupts(); return c; } @@ -127,12 +123,15 @@ static const struct { [AO_SERIAL_SPEED_57600] = { AO_PCLK1 / 57600 }, + [AO_SERIAL_SPEED_115200] = { + AO_PCLK1 / 115200 + }, }; void ao_usart_set_speed(struct ao_stm_usart *usart, uint8_t speed) { - if (speed > AO_SERIAL_SPEED_57600) + if (speed > AO_SERIAL_SPEED_115200) return; usart->reg->brr = ao_usart_speeds[speed].brr; } @@ -201,7 +200,7 @@ ao_serial1_putchar(char c) ao_usart_putchar(&ao_stm_usart1, c); } -char +int ao_serial1_pollchar(void) { return ao_usart_pollchar(&ao_stm_usart1); @@ -232,7 +231,7 @@ ao_serial2_putchar(char c) ao_usart_putchar(&ao_stm_usart2, c); } -char +int ao_serial2_pollchar(void) { return ao_usart_pollchar(&ao_stm_usart2); @@ -263,7 +262,7 @@ ao_serial3_putchar(char c) ao_usart_putchar(&ao_stm_usart3, c); } -char +int ao_serial3_pollchar(void) { return ao_usart_pollchar(&ao_stm_usart3); diff --git a/src/stm/ao_usb_stm.c b/src/stm/ao_usb_stm.c index d93a0c17..9379e5cd 100644 --- a/src/stm/ao_usb_stm.c +++ b/src/stm/ao_usb_stm.c @@ -873,10 +873,10 @@ _ao_usb_out_recv(void) ao_usb_set_stat_rx(AO_USB_OUT_EPR, STM_USB_EPR_STAT_RX_VALID); } -static char +static int _ao_usb_pollchar(void) { - char c; + uint8_t c; if (!ao_usb_running) return AO_READ_AGAIN; @@ -896,10 +896,10 @@ _ao_usb_pollchar(void) return c; } -char +int ao_usb_pollchar(void) { - char c; + int c; ao_arch_block_interrupts(); c = _ao_usb_pollchar(); ao_arch_release_interrupts(); @@ -909,7 +909,7 @@ ao_usb_pollchar(void) char ao_usb_getchar(void) { - char c; + int c; ao_arch_block_interrupts(); while ((c = _ao_usb_pollchar()) == AO_READ_AGAIN) diff --git a/src/teleballoon-v1.1/ao_pins.h b/src/teleballoon-v1.1/ao_pins.h index 3305719a..7ba48c96 100644 --- a/src/teleballoon-v1.1/ao_pins.h +++ b/src/teleballoon-v1.1/ao_pins.h @@ -43,9 +43,9 @@ #define PACKET_HAS_SLAVE 1 #define HAS_COMPANION 1 - #define COMPANION_CS_ON_P1 1 - #define COMPANION_CS_MASK 0x4 /* CS1 is P1_2 */ - #define COMPANION_CS P1_2 + #define AO_COMPANION_CS_PORT P1 + #define AO_COMPANION_CS_PIN 2 + #define AO_COMPANION_CS P1_2 #define AO_LED_RED 1 #define LEDS_AVAILABLE (AO_LED_RED) @@ -53,7 +53,7 @@ #define HAS_ACCEL_REF 1 #define SPI_CS_ON_P1 1 #define SPI_CS_ON_P0 0 - #define M25_CS_MASK 0x02 /* CS0 is P1_1 */ + #define AO_M25_SPI_CS_MASK 0x02 /* CS0 is P1_1 */ #define M25_MAX_CHIPS 1 #define HAS_ACCEL 1 #define HAS_IGNITE 0 @@ -114,6 +114,8 @@ #define SPI_CS_DIR P0DIR #endif +#define AO_M25_SPI_CS_PORT SPI_CS_PORT + #ifndef IGNITE_ON_P2 #error Please define IGNITE_ON_P2 #endif @@ -212,4 +214,16 @@ #define AO_IGNITER_FIRE_TIME AO_MS_TO_TICKS(50) #define AO_IGNITER_CHARGE_TIME AO_MS_TO_TICKS(2000) +struct ao_adc { + int16_t accel; /* accelerometer */ + int16_t pres; /* pressure sensor */ + int16_t temp; /* temperature sensor */ + int16_t v_batt; /* battery voltage */ + int16_t sense_d; /* drogue continuity sense */ + int16_t sense_m; /* main continuity sense */ +#if HAS_ACCEL_REF + uint16_t accel_ref; /* acceleration reference */ +#endif +}; + #endif /* _AO_PINS_H_ */ diff --git a/src/teleballoon-v1.1/ao_teleballoon.c b/src/teleballoon-v1.1/ao_teleballoon.c index 3f12a59c..c8bf7760 100644 --- a/src/teleballoon-v1.1/ao_teleballoon.c +++ b/src/teleballoon-v1.1/ao_teleballoon.c @@ -26,6 +26,8 @@ ao_ignite_set_pins(void) AO_IGNITER_DIR |= AO_IGNITER_DROGUE_BIT | AO_IGNITER_MAIN_BIT; } +__pdata uint16_t ao_motor_number; + void main(void) { diff --git a/src/test/Makefile b/src/test/Makefile index 44cee904..0dcdc949 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -29,7 +29,7 @@ ao_flight_test_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kal cc $(CFLAGS) -o $@ -DFORCE_ACCEL=1 ao_flight_test.c ao_flight_test_mm: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS) - cc -DMEGAMETRUM=1 $(CFLAGS) -o $@ $< + cc -DMEGAMETRUM=1 $(CFLAGS) -o $@ $< -lm ao_gps_test: ao_gps_test.c ao_gps_sirf.c ao_gps_print.c ao_host.h cc $(CFLAGS) -o $@ $< diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 7180f02d..acdf4d92 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -547,6 +547,202 @@ int32(uint8_t *bytes, int off) static int log_format; +#if MEGAMETRUM + +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; + + 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); + + 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); + + 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 + void ao_sleep(void *wchan) { @@ -635,6 +831,21 @@ ao_sleep(void *wchan) 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, rate_z, rate_y, tick); } ao_records_read++; ao_insert(); @@ -779,6 +990,8 @@ ao_sleep(void *wchan) continue; #if MEGAMETRUM + (void) a; + (void) b; #else switch (type) { case 'F': diff --git a/src/test/ao_gps_test.c b/src/test/ao_gps_test.c index d75a12ec..3844a326 100644 --- a/src/test/ao_gps_test.c +++ b/src/test/ao_gps_test.c @@ -88,6 +88,7 @@ ao_mutex_put(uint8_t *mutex) static int ao_gps_fd; +#if 0 static void ao_dbg_char(char c) { @@ -103,6 +104,7 @@ ao_dbg_char(char c) } write(1, line, strlen(line)); } +#endif #define QUEUE_LEN 4096 @@ -391,6 +393,7 @@ ao_serial1_putchar(char c) #define AO_SERIAL_SPEED_4800 0 #define AO_SERIAL_SPEED_57600 1 +#define AO_SERIAL_SPEED_115200 2 static void ao_serial1_set_speed(uint8_t speed) @@ -407,6 +410,9 @@ ao_serial1_set_speed(uint8_t speed) case AO_SERIAL_SPEED_57600: cfsetspeed(&termios, B57600); break; + case AO_SERIAL_SPEED_115200: + cfsetspeed(&termios, B115200); + break; } tcsetattr(fd, TCSAFLUSH, &termios); tcflush(fd, TCIFLUSH); @@ -420,7 +426,6 @@ ao_serial1_set_speed(uint8_t speed) void ao_dump_state(void *wchan) { - double lat, lon; int i; if (wchan == &ao_gps_data) ao_gps_print(&ao_gps_data); @@ -510,4 +515,5 @@ main (int argc, char **argv) } ao_gps_setup(); ao_gps(); + return 0; } diff --git a/src/test/ao_gps_test_skytraq.c b/src/test/ao_gps_test_skytraq.c index 846daa94..81008b39 100644 --- a/src/test/ao_gps_test_skytraq.c +++ b/src/test/ao_gps_test_skytraq.c @@ -397,6 +397,7 @@ ao_serial1_putchar(char c) #define AO_SERIAL_SPEED_4800 0 #define AO_SERIAL_SPEED_9600 1 #define AO_SERIAL_SPEED_57600 2 +#define AO_SERIAL_SPEED_115200 3 static void ao_serial1_set_speed(uint8_t speed) @@ -411,11 +412,14 @@ ao_serial1_set_speed(uint8_t speed) cfsetspeed(&termios, B4800); break; case AO_SERIAL_SPEED_9600: - cfsetspeed(&termios, B38400); + cfsetspeed(&termios, B9600); break; case AO_SERIAL_SPEED_57600: cfsetspeed(&termios, B57600); break; + case AO_SERIAL_SPEED_115200: + cfsetspeed(&termios, B115200); + break; } tcsetattr(fd, TCSAFLUSH, &termios); tcflush(fd, TCIFLUSH); @@ -423,6 +427,10 @@ ao_serial1_set_speed(uint8_t speed) #define ao_time() 0 +uint8_t ao_task_minimize_latency; + +#define ao_usb_getchar() 0 + #include "ao_gps_print.c" #include "ao_gps_skytraq.c" |