From ff4deb417a460d96645fb6934890c2c195142be3 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 28 Mar 2014 23:33:25 -0700 Subject: altos: HAS_LED is useless; remove it ao_flight was trying to decide whether to turn off the red LED by checking whether HAS_LED was defined. And yet, none of the flight firmware defines that anymore, except for easymini which defines it to zero. Remove all uses and defines of this value, substituting AO_LED_RED in ao_flight.c, which has to be defined for the ao_led_off call to work. Signed-off-by: Keith Packard --- src/core/ao_flight.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/core/ao_flight.c') diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 702c3403..24099347 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -143,7 +143,7 @@ ao_flight(void) ao_rdf_set(1); ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD); #endif -#if HAS_LED +#if AO_LED_RED /* signal successful initialization by turning off the LED */ ao_led_off(AO_LED_RED); #endif @@ -160,7 +160,7 @@ ao_flight(void) ao_packet_slave_start(); #endif -#if HAS_LED +#if AO_LED_RED /* signal successful initialization by turning off the LED */ ao_led_off(AO_LED_RED); #endif -- cgit v1.2.3 From 24167015705ae831692b95735968b04a876f935e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 4 Apr 2014 23:34:48 -0700 Subject: altos: Rename 'core' to 'kernel' core remains a bad name to use -- dirvish skips files (and directories, it seems) with that name. Signed-off-by: Keith Packard --- configure.ac | 2 +- src/avr-demo/Makefile | 4 +- src/cc1111/Makefile.cc1111 | 2 +- src/core/altitude.h | 132 ----- src/core/ao.h | 1041 ------------------------------------- src/core/ao_adc.h | 35 -- src/core/ao_aes.h | 54 -- src/core/ao_balloon.c | 136 ----- src/core/ao_beep.h | 74 --- src/core/ao_btm.h | 36 -- src/core/ao_cmd.c | 420 --------------- src/core/ao_companion.h | 55 -- src/core/ao_config.c | 805 ---------------------------- src/core/ao_config.h | 53 -- src/core/ao_convert.c | 89 ---- src/core/ao_convert_pa.c | 82 --- src/core/ao_convert_pa_test.c | 76 --- src/core/ao_convert_test.c | 76 --- src/core/ao_convert_volt.c | 33 -- src/core/ao_data.c | 36 -- src/core/ao_data.h | 338 ------------ src/core/ao_dbg.h | 62 --- src/core/ao_debounce.c | 163 ------ src/core/ao_debounce.h | 74 --- src/core/ao_ee_fake.c | 37 -- src/core/ao_eeprom.h | 43 -- src/core/ao_fec.h | 84 --- src/core/ao_fec_rx.c | 318 ----------- src/core/ao_fec_tx.c | 134 ----- src/core/ao_flight.c | 472 ----------------- src/core/ao_flight.h | 70 --- src/core/ao_flight_nano.c | 120 ----- src/core/ao_freq.c | 55 -- src/core/ao_gps_print.c | 112 ---- src/core/ao_gps_report.c | 89 ---- src/core/ao_gps_report_mega.c | 90 ---- src/core/ao_gps_report_metrum.c | 96 ---- src/core/ao_gps_show.c | 39 -- src/core/ao_host.h | 135 ----- src/core/ao_ignite.c | 241 --------- src/core/ao_int64.c | 158 ------ src/core/ao_int64.h | 48 -- src/core/ao_kalman.c | 295 ----------- src/core/ao_lcd.h | 60 --- src/core/ao_led.h | 59 --- src/core/ao_list.h | 213 -------- src/core/ao_log.c | 291 ----------- src/core/ao_log.h | 386 -------------- src/core/ao_log_big.c | 162 ------ src/core/ao_log_mega.c | 199 ------- src/core/ao_log_metrum.c | 176 ------- src/core/ao_log_micro.c | 121 ----- src/core/ao_log_micro.h | 39 -- src/core/ao_log_mini.c | 162 ------ src/core/ao_log_single.c | 198 ------- src/core/ao_log_telem.c | 131 ----- src/core/ao_log_telescience.c | 119 ----- src/core/ao_log_tiny.c | 161 ------ src/core/ao_microflight.c | 143 ----- src/core/ao_microkalman.c | 74 --- src/core/ao_monitor.c | 308 ----------- src/core/ao_mutex.c | 41 -- src/core/ao_notask.c | 46 -- src/core/ao_notask.h | 27 - src/core/ao_packet.h | 91 ---- src/core/ao_panic.c | 90 ---- src/core/ao_product.c | 161 ------ src/core/ao_pyro.c | 518 ------------------ src/core/ao_pyro.h | 83 --- src/core/ao_quaternion.h | 249 --------- src/core/ao_radio_cmac.c | 164 ------ src/core/ao_radio_cmac.h | 43 -- src/core/ao_radio_cmac_cmd.c | 104 ---- src/core/ao_radio_cmac_cmd.h | 24 - src/core/ao_report.c | 198 ------- src/core/ao_report_micro.c | 57 -- src/core/ao_rssi.c | 53 -- src/core/ao_sample.c | 372 ------------- src/core/ao_sample.h | 156 ------ src/core/ao_sample_profile.c | 173 ------ src/core/ao_sample_profile.h | 29 -- src/core/ao_send_packet.c | 58 --- src/core/ao_send_packet.h | 24 - src/core/ao_serial.h | 110 ---- src/core/ao_sqrt.c | 48 -- src/core/ao_state.c | 23 - src/core/ao_stdio.c | 158 ------ src/core/ao_storage.c | 186 ------- src/core/ao_storage.h | 98 ---- src/core/ao_task.c | 548 ------------------- src/core/ao_task.h | 117 ----- src/core/ao_telem.h | 172 ------ src/core/ao_telemetry.c | 555 -------------------- src/core/ao_telemetry.h | 321 ------------ src/core/ao_usb.h | 142 ----- src/kernel/altitude.h | 132 +++++ src/kernel/ao.h | 1041 +++++++++++++++++++++++++++++++++++++ src/kernel/ao_adc.h | 35 ++ src/kernel/ao_aes.h | 54 ++ src/kernel/ao_balloon.c | 136 +++++ src/kernel/ao_beep.h | 74 +++ src/kernel/ao_btm.h | 36 ++ src/kernel/ao_cmd.c | 420 +++++++++++++++ src/kernel/ao_companion.h | 55 ++ src/kernel/ao_config.c | 805 ++++++++++++++++++++++++++++ src/kernel/ao_config.h | 53 ++ src/kernel/ao_convert.c | 89 ++++ src/kernel/ao_convert_pa.c | 82 +++ src/kernel/ao_convert_pa_test.c | 76 +++ src/kernel/ao_convert_test.c | 76 +++ src/kernel/ao_convert_volt.c | 33 ++ src/kernel/ao_data.c | 36 ++ src/kernel/ao_data.h | 338 ++++++++++++ src/kernel/ao_dbg.h | 62 +++ src/kernel/ao_debounce.c | 163 ++++++ src/kernel/ao_debounce.h | 74 +++ src/kernel/ao_ee_fake.c | 37 ++ src/kernel/ao_eeprom.h | 43 ++ src/kernel/ao_fec.h | 84 +++ src/kernel/ao_fec_rx.c | 318 +++++++++++ src/kernel/ao_fec_tx.c | 134 +++++ src/kernel/ao_flight.c | 472 +++++++++++++++++ src/kernel/ao_flight.h | 70 +++ src/kernel/ao_flight_nano.c | 120 +++++ src/kernel/ao_freq.c | 55 ++ src/kernel/ao_gps_print.c | 112 ++++ src/kernel/ao_gps_report.c | 89 ++++ src/kernel/ao_gps_report_mega.c | 90 ++++ src/kernel/ao_gps_report_metrum.c | 96 ++++ src/kernel/ao_gps_show.c | 39 ++ src/kernel/ao_host.h | 135 +++++ src/kernel/ao_ignite.c | 241 +++++++++ src/kernel/ao_int64.c | 158 ++++++ src/kernel/ao_int64.h | 48 ++ src/kernel/ao_kalman.c | 295 +++++++++++ src/kernel/ao_lcd.h | 60 +++ src/kernel/ao_led.h | 59 +++ src/kernel/ao_list.h | 213 ++++++++ src/kernel/ao_log.c | 291 +++++++++++ src/kernel/ao_log.h | 386 ++++++++++++++ src/kernel/ao_log_big.c | 162 ++++++ src/kernel/ao_log_mega.c | 199 +++++++ src/kernel/ao_log_metrum.c | 176 +++++++ src/kernel/ao_log_micro.c | 121 +++++ src/kernel/ao_log_micro.h | 39 ++ src/kernel/ao_log_mini.c | 162 ++++++ src/kernel/ao_log_single.c | 198 +++++++ src/kernel/ao_log_telem.c | 131 +++++ src/kernel/ao_log_telescience.c | 119 +++++ src/kernel/ao_log_tiny.c | 161 ++++++ src/kernel/ao_microflight.c | 143 +++++ src/kernel/ao_microkalman.c | 74 +++ src/kernel/ao_monitor.c | 308 +++++++++++ src/kernel/ao_mutex.c | 41 ++ src/kernel/ao_notask.c | 46 ++ src/kernel/ao_notask.h | 27 + src/kernel/ao_packet.h | 91 ++++ src/kernel/ao_panic.c | 90 ++++ src/kernel/ao_product.c | 161 ++++++ src/kernel/ao_pyro.c | 518 ++++++++++++++++++ src/kernel/ao_pyro.h | 83 +++ src/kernel/ao_quaternion.h | 249 +++++++++ src/kernel/ao_radio_cmac.c | 164 ++++++ src/kernel/ao_radio_cmac.h | 43 ++ src/kernel/ao_radio_cmac_cmd.c | 104 ++++ src/kernel/ao_radio_cmac_cmd.h | 24 + src/kernel/ao_report.c | 198 +++++++ src/kernel/ao_report_micro.c | 57 ++ src/kernel/ao_rssi.c | 53 ++ src/kernel/ao_sample.c | 372 +++++++++++++ src/kernel/ao_sample.h | 156 ++++++ src/kernel/ao_sample_profile.c | 173 ++++++ src/kernel/ao_sample_profile.h | 29 ++ src/kernel/ao_send_packet.c | 58 +++ src/kernel/ao_send_packet.h | 24 + src/kernel/ao_serial.h | 110 ++++ src/kernel/ao_sqrt.c | 48 ++ src/kernel/ao_state.c | 23 + src/kernel/ao_stdio.c | 158 ++++++ src/kernel/ao_storage.c | 186 +++++++ src/kernel/ao_storage.h | 98 ++++ src/kernel/ao_task.c | 548 +++++++++++++++++++ src/kernel/ao_task.h | 117 +++++ src/kernel/ao_telem.h | 172 ++++++ src/kernel/ao_telemetry.c | 555 ++++++++++++++++++++ src/kernel/ao_telemetry.h | 321 ++++++++++++ src/kernel/ao_usb.h | 142 +++++ src/lpc/Makefile-lpc.defs | 4 +- src/micropeak/Makefile | 4 +- src/nanopeak-v0.1/Makefile | 4 +- src/product/Makefile.teledongle | 4 +- src/product/Makefile.telelaunch | 4 +- src/product/Makefile.telemetrum | 4 +- src/product/Makefile.telemini | 4 +- src/product/Makefile.telenano | 4 +- src/spiradio-v0.1/.sdcdbrc | 2 +- src/spiradio-v0.1/Makefile | 4 +- src/stm-bringup/Makefile | 2 +- src/stm/Makefile-flash.defs | 4 +- src/stm/Makefile.defs | 4 +- src/teleballoon-v1.1/Makefile | 4 +- src/telebt-v1.0/.sdcdbrc | 2 +- src/telebt-v1.0/Makefile | 4 +- src/telefire-v0.1/.sdcdbrc | 2 +- src/telefire-v0.1/Makefile | 4 +- src/telefire-v0.2/.sdcdbrc | 2 +- src/telefire-v0.2/Makefile | 4 +- src/telemetrum-v1.0/.sdcdbrc | 2 +- src/telemetrum-v1.1/.sdcdbrc | 2 +- src/telemini-v1.0/.sdcdbrc | 2 +- src/telemini-v2.0/.sdcdbrc | 2 +- src/telemini-v2.0/Makefile | 4 +- src/telepyro-v0.1/Makefile | 4 +- src/telescience-pwm/Makefile | 4 +- src/telescience-v0.1/Makefile | 4 +- src/teleshield-v0.1/Makefile | 4 +- src/teleterra-v0.2/.sdcdbrc | 2 +- src/teleterra-v0.2/Makefile | 4 +- src/test/Makefile | 6 +- src/tidongle/Makefile | 4 +- 220 files changed, 14808 insertions(+), 14808 deletions(-) delete mode 100644 src/core/altitude.h delete mode 100644 src/core/ao.h delete mode 100644 src/core/ao_adc.h delete mode 100644 src/core/ao_aes.h delete mode 100644 src/core/ao_balloon.c delete mode 100644 src/core/ao_beep.h delete mode 100644 src/core/ao_btm.h delete mode 100644 src/core/ao_cmd.c delete mode 100644 src/core/ao_companion.h delete mode 100644 src/core/ao_config.c delete mode 100644 src/core/ao_config.h delete mode 100644 src/core/ao_convert.c delete mode 100644 src/core/ao_convert_pa.c delete mode 100644 src/core/ao_convert_pa_test.c delete mode 100644 src/core/ao_convert_test.c delete mode 100644 src/core/ao_convert_volt.c delete mode 100644 src/core/ao_data.c delete mode 100644 src/core/ao_data.h delete mode 100644 src/core/ao_dbg.h delete mode 100644 src/core/ao_debounce.c delete mode 100644 src/core/ao_debounce.h delete mode 100644 src/core/ao_ee_fake.c delete mode 100644 src/core/ao_eeprom.h delete mode 100644 src/core/ao_fec.h delete mode 100644 src/core/ao_fec_rx.c delete mode 100644 src/core/ao_fec_tx.c delete mode 100644 src/core/ao_flight.c delete mode 100644 src/core/ao_flight.h delete mode 100644 src/core/ao_flight_nano.c delete mode 100644 src/core/ao_freq.c delete mode 100644 src/core/ao_gps_print.c delete mode 100644 src/core/ao_gps_report.c delete mode 100644 src/core/ao_gps_report_mega.c delete mode 100644 src/core/ao_gps_report_metrum.c delete mode 100644 src/core/ao_gps_show.c delete mode 100644 src/core/ao_host.h delete mode 100644 src/core/ao_ignite.c delete mode 100644 src/core/ao_int64.c delete mode 100644 src/core/ao_int64.h delete mode 100644 src/core/ao_kalman.c delete mode 100644 src/core/ao_lcd.h delete mode 100644 src/core/ao_led.h delete mode 100644 src/core/ao_list.h delete mode 100644 src/core/ao_log.c delete mode 100644 src/core/ao_log.h delete mode 100644 src/core/ao_log_big.c delete mode 100644 src/core/ao_log_mega.c delete mode 100644 src/core/ao_log_metrum.c delete mode 100644 src/core/ao_log_micro.c delete mode 100644 src/core/ao_log_micro.h delete mode 100644 src/core/ao_log_mini.c delete mode 100644 src/core/ao_log_single.c delete mode 100644 src/core/ao_log_telem.c delete mode 100644 src/core/ao_log_telescience.c delete mode 100644 src/core/ao_log_tiny.c delete mode 100644 src/core/ao_microflight.c delete mode 100644 src/core/ao_microkalman.c delete mode 100644 src/core/ao_monitor.c delete mode 100644 src/core/ao_mutex.c delete mode 100644 src/core/ao_notask.c delete mode 100644 src/core/ao_notask.h delete mode 100644 src/core/ao_packet.h delete mode 100644 src/core/ao_panic.c delete mode 100644 src/core/ao_product.c delete mode 100644 src/core/ao_pyro.c delete mode 100644 src/core/ao_pyro.h delete mode 100644 src/core/ao_quaternion.h delete mode 100644 src/core/ao_radio_cmac.c delete mode 100644 src/core/ao_radio_cmac.h delete mode 100644 src/core/ao_radio_cmac_cmd.c delete mode 100644 src/core/ao_radio_cmac_cmd.h delete mode 100644 src/core/ao_report.c delete mode 100644 src/core/ao_report_micro.c delete mode 100644 src/core/ao_rssi.c delete mode 100644 src/core/ao_sample.c delete mode 100644 src/core/ao_sample.h delete mode 100644 src/core/ao_sample_profile.c delete mode 100644 src/core/ao_sample_profile.h delete mode 100644 src/core/ao_send_packet.c delete mode 100644 src/core/ao_send_packet.h delete mode 100644 src/core/ao_serial.h delete mode 100644 src/core/ao_sqrt.c delete mode 100644 src/core/ao_state.c delete mode 100644 src/core/ao_stdio.c delete mode 100644 src/core/ao_storage.c delete mode 100644 src/core/ao_storage.h delete mode 100644 src/core/ao_task.c delete mode 100644 src/core/ao_task.h delete mode 100644 src/core/ao_telem.h delete mode 100644 src/core/ao_telemetry.c delete mode 100644 src/core/ao_telemetry.h delete mode 100644 src/core/ao_usb.h create mode 100644 src/kernel/altitude.h create mode 100644 src/kernel/ao.h create mode 100644 src/kernel/ao_adc.h create mode 100644 src/kernel/ao_aes.h create mode 100644 src/kernel/ao_balloon.c create mode 100644 src/kernel/ao_beep.h create mode 100644 src/kernel/ao_btm.h create mode 100644 src/kernel/ao_cmd.c create mode 100644 src/kernel/ao_companion.h create mode 100644 src/kernel/ao_config.c create mode 100644 src/kernel/ao_config.h create mode 100644 src/kernel/ao_convert.c create mode 100644 src/kernel/ao_convert_pa.c create mode 100644 src/kernel/ao_convert_pa_test.c create mode 100644 src/kernel/ao_convert_test.c create mode 100644 src/kernel/ao_convert_volt.c create mode 100644 src/kernel/ao_data.c create mode 100644 src/kernel/ao_data.h create mode 100644 src/kernel/ao_dbg.h create mode 100644 src/kernel/ao_debounce.c create mode 100644 src/kernel/ao_debounce.h create mode 100644 src/kernel/ao_ee_fake.c create mode 100644 src/kernel/ao_eeprom.h create mode 100644 src/kernel/ao_fec.h create mode 100644 src/kernel/ao_fec_rx.c create mode 100644 src/kernel/ao_fec_tx.c create mode 100644 src/kernel/ao_flight.c create mode 100644 src/kernel/ao_flight.h create mode 100644 src/kernel/ao_flight_nano.c create mode 100644 src/kernel/ao_freq.c create mode 100644 src/kernel/ao_gps_print.c create mode 100644 src/kernel/ao_gps_report.c create mode 100644 src/kernel/ao_gps_report_mega.c create mode 100644 src/kernel/ao_gps_report_metrum.c create mode 100644 src/kernel/ao_gps_show.c create mode 100644 src/kernel/ao_host.h create mode 100644 src/kernel/ao_ignite.c create mode 100644 src/kernel/ao_int64.c create mode 100644 src/kernel/ao_int64.h create mode 100644 src/kernel/ao_kalman.c create mode 100644 src/kernel/ao_lcd.h create mode 100644 src/kernel/ao_led.h create mode 100644 src/kernel/ao_list.h create mode 100644 src/kernel/ao_log.c create mode 100644 src/kernel/ao_log.h create mode 100644 src/kernel/ao_log_big.c create mode 100644 src/kernel/ao_log_mega.c create mode 100644 src/kernel/ao_log_metrum.c create mode 100644 src/kernel/ao_log_micro.c create mode 100644 src/kernel/ao_log_micro.h create mode 100644 src/kernel/ao_log_mini.c create mode 100644 src/kernel/ao_log_single.c create mode 100644 src/kernel/ao_log_telem.c create mode 100644 src/kernel/ao_log_telescience.c create mode 100644 src/kernel/ao_log_tiny.c create mode 100644 src/kernel/ao_microflight.c create mode 100644 src/kernel/ao_microkalman.c create mode 100644 src/kernel/ao_monitor.c create mode 100644 src/kernel/ao_mutex.c create mode 100644 src/kernel/ao_notask.c create mode 100644 src/kernel/ao_notask.h create mode 100644 src/kernel/ao_packet.h create mode 100644 src/kernel/ao_panic.c create mode 100644 src/kernel/ao_product.c create mode 100644 src/kernel/ao_pyro.c create mode 100644 src/kernel/ao_pyro.h create mode 100644 src/kernel/ao_quaternion.h create mode 100644 src/kernel/ao_radio_cmac.c create mode 100644 src/kernel/ao_radio_cmac.h create mode 100644 src/kernel/ao_radio_cmac_cmd.c create mode 100644 src/kernel/ao_radio_cmac_cmd.h create mode 100644 src/kernel/ao_report.c create mode 100644 src/kernel/ao_report_micro.c create mode 100644 src/kernel/ao_rssi.c create mode 100644 src/kernel/ao_sample.c create mode 100644 src/kernel/ao_sample.h create mode 100644 src/kernel/ao_sample_profile.c create mode 100644 src/kernel/ao_sample_profile.h create mode 100644 src/kernel/ao_send_packet.c create mode 100644 src/kernel/ao_send_packet.h create mode 100644 src/kernel/ao_serial.h create mode 100644 src/kernel/ao_sqrt.c create mode 100644 src/kernel/ao_state.c create mode 100644 src/kernel/ao_stdio.c create mode 100644 src/kernel/ao_storage.c create mode 100644 src/kernel/ao_storage.h create mode 100644 src/kernel/ao_task.c create mode 100644 src/kernel/ao_task.h create mode 100644 src/kernel/ao_telem.h create mode 100644 src/kernel/ao_telemetry.c create mode 100644 src/kernel/ao_telemetry.h create mode 100644 src/kernel/ao_usb.h (limited to 'src/core/ao_flight.c') diff --git a/configure.ac b/configure.ac index b28b549b..27dbc6a4 100644 --- a/configure.ac +++ b/configure.ac @@ -19,7 +19,7 @@ dnl Process this file with autoconf to create configure. AC_PREREQ(2.57) AC_INIT([altos], 1.3.2.1) -AC_CONFIG_SRCDIR([src/core/ao.h]) +AC_CONFIG_SRCDIR([src/kernel/ao.h]) AM_INIT_AUTOMAKE([foreign dist-bzip2]) AM_MAINTAINER_MODE diff --git a/src/avr-demo/Makefile b/src/avr-demo/Makefile index 6d9bfea2..e21ad047 100644 --- a/src/avr-demo/Makefile +++ b/src/avr-demo/Makefile @@ -2,7 +2,7 @@ # AltOS build # # -vpath % ..:../core:../product:../drivers:../avr +vpath % ..:../kernel:../product:../drivers:../avr vpath make-altitude .. vpath make-kalman .. vpath kalman.5c ../kalman @@ -46,7 +46,7 @@ PRODUCT=AvrDemo-v0.0 MCU=atmega32u4 PRODUCT_DEF=-DAVR_DEMO IDPRODUCT=0x000a -CFLAGS = $(PRODUCT_DEF) -I. -I../avr -I../core -I.. +CFLAGS = $(PRODUCT_DEF) -I. -I../avr -I../kernel -I.. CFLAGS += -g -mmcu=$(MCU) -Wall -Wstrict-prototypes -Os -mcall-prologues NICKLE=nickle diff --git a/src/cc1111/Makefile.cc1111 b/src/cc1111/Makefile.cc1111 index 78b653b3..0ea30e1d 100644 --- a/src/cc1111/Makefile.cc1111 +++ b/src/cc1111/Makefile.cc1111 @@ -3,7 +3,7 @@ CC=$(SDCC) CFLAGS=--model-small --debug --opt-code-speed -DCODESIZE=$(CODESIZE) -CFLAGS += $(PRODUCT_DEF) -I. -I.. -I../core -I../cc1111 -I../drivers -I../product +CFLAGS += $(PRODUCT_DEF) -I. -I.. -I../kernel -I../cc1111 -I../drivers -I../product CODESIZE ?= 0x8000 diff --git a/src/core/altitude.h b/src/core/altitude.h deleted file mode 100644 index a278bbc6..00000000 --- a/src/core/altitude.h +++ /dev/null @@ -1,132 +0,0 @@ -/*max error 3.197865153490684 at 0.782%. Average error 0.260150920474668*/ -#define NALT 129 -#define ALT_FRAC_BITS 8 - 15835, /* 10.56 kPa 0.000% */ - 15332, /* 11.42 kPa 0.781% */ - 14868, /* 12.29 kPa 1.563% */ - 14435, /* 13.16 kPa 2.344% */ - 14030, /* 14.02 kPa 3.125% */ - 13649, /* 14.90 kPa 3.906% */ - 13290, /* 15.76 kPa 4.688% */ - 12950, /* 16.63 kPa 5.469% */ - 12627, /* 17.50 kPa 6.250% */ - 12320, /* 18.37 kPa 7.031% */ - 12027, /* 19.24 kPa 7.813% */ - 11747, /* 20.10 kPa 8.594% */ - 11479, /* 20.97 kPa 9.375% */ - 11222, /* 21.84 kPa 10.156% */ - 10975, /* 22.71 kPa 10.938% */ - 10736, /* 23.58 kPa 11.719% */ - 10504, /* 24.44 kPa 12.500% */ - 10278, /* 25.31 kPa 13.281% */ - 10059, /* 26.18 kPa 14.063% */ - 9846, /* 27.05 kPa 14.844% */ - 9638, /* 27.91 kPa 15.625% */ - 9435, /* 28.78 kPa 16.406% */ - 9237, /* 29.65 kPa 17.188% */ - 9044, /* 30.52 kPa 17.969% */ - 8855, /* 31.39 kPa 18.750% */ - 8670, /* 32.26 kPa 19.531% */ - 8490, /* 33.13 kPa 20.313% */ - 8313, /* 33.99 kPa 21.094% */ - 8140, /* 34.86 kPa 21.875% */ - 7970, /* 35.73 kPa 22.656% */ - 7803, /* 36.60 kPa 23.438% */ - 7640, /* 37.47 kPa 24.219% */ - 7480, /* 38.33 kPa 25.000% */ - 7322, /* 39.20 kPa 25.781% */ - 7168, /* 40.07 kPa 26.563% */ - 7016, /* 40.94 kPa 27.344% */ - 6867, /* 41.80 kPa 28.125% */ - 6720, /* 42.67 kPa 28.906% */ - 6575, /* 43.54 kPa 29.688% */ - 6433, /* 44.41 kPa 30.469% */ - 6294, /* 45.28 kPa 31.250% */ - 6156, /* 46.15 kPa 32.031% */ - 6020, /* 47.01 kPa 32.813% */ - 5887, /* 47.88 kPa 33.594% */ - 5755, /* 48.75 kPa 34.375% */ - 5625, /* 49.62 kPa 35.156% */ - 5497, /* 50.49 kPa 35.938% */ - 5371, /* 51.35 kPa 36.719% */ - 5247, /* 52.22 kPa 37.500% */ - 5124, /* 53.09 kPa 38.281% */ - 5003, /* 53.96 kPa 39.063% */ - 4883, /* 54.83 kPa 39.844% */ - 4765, /* 55.69 kPa 40.625% */ - 4648, /* 56.56 kPa 41.406% */ - 4533, /* 57.43 kPa 42.188% */ - 4419, /* 58.30 kPa 42.969% */ - 4307, /* 59.17 kPa 43.750% */ - 4196, /* 60.03 kPa 44.531% */ - 4086, /* 60.90 kPa 45.313% */ - 3977, /* 61.77 kPa 46.094% */ - 3870, /* 62.63 kPa 46.875% */ - 3764, /* 63.51 kPa 47.656% */ - 3659, /* 64.38 kPa 48.438% */ - 3555, /* 65.24 kPa 49.219% */ - 3453, /* 66.11 kPa 50.000% */ - 3351, /* 66.98 kPa 50.781% */ - 3250, /* 67.85 kPa 51.563% */ - 3151, /* 68.72 kPa 52.344% */ - 3052, /* 69.58 kPa 53.125% */ - 2955, /* 70.45 kPa 53.906% */ - 2858, /* 71.32 kPa 54.688% */ - 2763, /* 72.19 kPa 55.469% */ - 2668, /* 73.06 kPa 56.250% */ - 2574, /* 73.92 kPa 57.031% */ - 2482, /* 74.79 kPa 57.813% */ - 2390, /* 75.66 kPa 58.594% */ - 2298, /* 76.52 kPa 59.375% */ - 2208, /* 77.40 kPa 60.156% */ - 2119, /* 78.26 kPa 60.938% */ - 2030, /* 79.13 kPa 61.719% */ - 1942, /* 80.00 kPa 62.500% */ - 1855, /* 80.87 kPa 63.281% */ - 1769, /* 81.74 kPa 64.063% */ - 1683, /* 82.60 kPa 64.844% */ - 1598, /* 83.47 kPa 65.625% */ - 1514, /* 84.34 kPa 66.406% */ - 1430, /* 85.21 kPa 67.188% */ - 1347, /* 86.08 kPa 67.969% */ - 1265, /* 86.94 kPa 68.750% */ - 1184, /* 87.81 kPa 69.531% */ - 1103, /* 88.68 kPa 70.313% */ - 1023, /* 89.55 kPa 71.094% */ - 943, /* 90.41 kPa 71.875% */ - 864, /* 91.28 kPa 72.656% */ - 786, /* 92.15 kPa 73.438% */ - 708, /* 93.02 kPa 74.219% */ - 631, /* 93.89 kPa 75.000% */ - 554, /* 94.76 kPa 75.781% */ - 478, /* 95.63 kPa 76.563% */ - 403, /* 96.49 kPa 77.344% */ - 328, /* 97.36 kPa 78.125% */ - 254, /* 98.23 kPa 78.906% */ - 180, /* 99.10 kPa 79.688% */ - 106, /* 99.97 kPa 80.469% */ - 34, /* 100.83 kPa 81.250% */ - -39, /* 101.70 kPa 82.031% */ - -111, /* 102.57 kPa 82.813% */ - -182, /* 103.44 kPa 83.594% */ - -253, /* 104.30 kPa 84.375% */ - -323, /* 105.17 kPa 85.156% */ - -393, /* 106.04 kPa 85.938% */ - -462, /* 106.91 kPa 86.719% */ - -531, /* 107.78 kPa 87.500% */ - -600, /* 108.65 kPa 88.281% */ - -668, /* 109.51 kPa 89.063% */ - -736, /* 110.38 kPa 89.844% */ - -803, /* 111.25 kPa 90.625% */ - -870, /* 112.12 kPa 91.406% */ - -936, /* 112.99 kPa 92.188% */ - -1002, /* 113.85 kPa 92.969% */ - -1068, /* 114.72 kPa 93.750% */ - -1133, /* 115.59 kPa 94.531% */ - -1198, /* 116.46 kPa 95.313% */ - -1262, /* 117.33 kPa 96.094% */ - -1326, /* 118.19 kPa 96.875% */ - -1389, /* 119.06 kPa 97.656% */ - -1453, /* 119.93 kPa 98.438% */ - -1516, /* 120.80 kPa 99.219% */ - -1578, /* 121.67 kPa 100.000% */ diff --git a/src/core/ao.h b/src/core/ao.h deleted file mode 100644 index 29ad2603..00000000 --- a/src/core/ao.h +++ /dev/null @@ -1,1041 +0,0 @@ -/* - * Copyright © 2009 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_H_ -#define _AO_H_ - -#include -#include -#include -#include -#include -#include - -#define TRUE 1 -#define FALSE 0 - -/* Convert a __data pointer into an __xdata pointer */ -#ifndef DATA_TO_XDATA -#define DATA_TO_XDATA(a) (a) -#endif -#ifndef PDATA_TO_XDATA -#define PDATA_TO_XDATA(a) (a) -#endif -#ifndef CODE_TO_XDATA -#define CODE_TO_XDATA(a) (a) -#endif - -#ifndef HAS_TASK -#define HAS_TASK 1 -#endif - -#ifndef AO_PORT_TYPE -#define AO_PORT_TYPE uint8_t -#endif - -typedef AO_PORT_TYPE ao_port_t; - -#if HAS_TASK -#include -#else -#include -#endif - -/* - * ao_panic.c - */ - -#define AO_PANIC_NO_TASK 1 /* AO_NUM_TASKS is not large enough */ -#define AO_PANIC_DMA 2 /* Attempt to start DMA while active */ -#define AO_PANIC_MUTEX 3 /* Mis-using mutex API */ -#define AO_PANIC_EE 4 /* Mis-using eeprom API */ -#define AO_PANIC_LOG 5 /* Failing to read/write log data */ -#define AO_PANIC_CMD 6 /* Too many command sets registered */ -#define AO_PANIC_STDIO 7 /* Too many stdio handlers registered */ -#define AO_PANIC_REBOOT 8 /* Reboot failed */ -#define AO_PANIC_FLASH 9 /* Invalid flash part (or wrong blocksize) */ -#define AO_PANIC_USB 10 /* Trying to send USB packet while busy */ -#define AO_PANIC_BT 11 /* Communications with bluetooth device failed */ -#define AO_PANIC_STACK 12 /* Stack overflow */ -#define AO_PANIC_SPI 13 /* SPI communication failure */ -#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 */ -#define AO_PANIC_SELF_TEST_MS5607 0x40 | 4 /* Self test failure */ - -/* Stop the operating system, beeping and blinking the reason */ -void -ao_panic(uint8_t reason); - -/* - * ao_timer.c - */ - -#ifndef AO_TICK_TYPE -#define AO_TICK_TYPE uint16_t -#define AO_TICK_SIGNED int16_t -#endif - -extern volatile __data AO_TICK_TYPE ao_tick_count; - -/* Our timer runs at 100Hz */ -#ifndef AO_HERTZ -#define AO_HERTZ 100 -#endif -#define AO_MS_TO_TICKS(ms) ((ms) / (1000 / AO_HERTZ)) -#define AO_SEC_TO_TICKS(s) ((s) * AO_HERTZ) - -/* Returns the current time in ticks */ -AO_TICK_TYPE -ao_time(void); - -/* Suspend the current task until ticks time has passed */ -void -ao_delay(uint16_t ticks); - -/* Set the ADC interval */ -void -ao_timer_set_adc_interval(uint8_t interval); - -/* Timer interrupt */ -void -ao_timer_isr(void) ao_arch_interrupt(9); - -/* Initialize the timer */ -void -ao_timer_init(void); - -/* Initialize the hardware clock. Must be called first */ -void -ao_clock_init(void); - -/* - * ao_mutex.c - */ - -#ifndef ao_mutex_get -void -ao_mutex_get(__xdata uint8_t *ao_mutex) __reentrant; - -void -ao_mutex_put(__xdata uint8_t *ao_mutex) __reentrant; -#endif - -/* - * ao_cmd.c - */ - -enum ao_cmd_status { - ao_cmd_success = 0, - ao_cmd_lex_error = 1, - ao_cmd_syntax_error = 2, -}; - -extern __pdata uint16_t ao_cmd_lex_i; -extern __pdata uint32_t ao_cmd_lex_u32; -extern __pdata char ao_cmd_lex_c; -extern __pdata enum ao_cmd_status ao_cmd_status; - -void -ao_put_string(__code char *s); - -void -ao_cmd_lex(void); - -void -ao_cmd_put8(uint8_t v); - -void -ao_cmd_put16(uint16_t v); - -uint8_t -ao_cmd_is_white(void); - -void -ao_cmd_white(void); - -int8_t -ao_cmd_hexchar(char c); - -void -ao_cmd_hexbyte(void); - -void -ao_cmd_hex(void); - -void -ao_cmd_decimal(void) __reentrant; - -/* Read a single hex nibble off stdin. */ -uint8_t -ao_getnibble(void); - -uint8_t -ao_match_word(__code char *word); - -struct ao_cmds { - void (*func)(void); - __code char *help; -}; - -void -ao_cmd_register(const __code struct ao_cmds *cmds); - -void -ao_cmd_init(void); - -#if HAS_CMD_FILTER -/* - * Provided by an external module to filter raw command lines - */ -uint8_t -ao_cmd_filter(void); -#endif - -/* - * Various drivers - */ -#if HAS_ADC -#include -#endif - -#if HAS_BEEP -#include -#endif - -#if LEDS_AVAILABLE -#include -#endif - -#if HAS_USB -#include -#endif - -#if HAS_EEPROM -#include -#endif - -#if HAS_LOG -#include -#endif - -#if HAS_FLIGHT -#include -#include -#endif - -/* - * ao_report.c - */ - -#define AO_RDF_INTERVAL_TICKS AO_SEC_TO_TICKS(5) -#define AO_RDF_LENGTH_MS 500 -#define AO_RDF_CONTINUITY_MS 32 -#define AO_RDF_CONTINUITY_PAUSE 96 -#define AO_RDF_CONTINUITY_TOTAL ((AO_RDF_CONTINUITY_PAUSE + AO_RDF_CONTINUITY_MS) * 3 + AO_RDF_CONTINUITY_PAUSE) - -/* This assumes that we're generating a 1kHz tone, which - * modulates the carrier at 2kbps, or 250kBps - */ -#define AO_MS_TO_RDF_LEN(ms) ((ms) / 4) - -#define AO_RADIO_RDF_LEN AO_MS_TO_RDF_LEN(AO_RDF_LENGTH_MS) -#define AO_RADIO_CONT_TONE_LEN AO_MS_TO_RDF_LEN(AO_RDF_CONTINUITY_MS) -#define AO_RADIO_CONT_PAUSE_LEN AO_MS_TO_RDF_LEN(AO_RDF_CONTINUITY_PAUSE) -#define AO_RADIO_CONT_TOTAL_LEN AO_MS_TO_RDF_LEN(AO_RDF_CONTINUITY_TOTAL) - -/* returns a value 0-3 to indicate igniter continuity */ -uint8_t -ao_report_igniter(void); - -void -ao_report_init(void); - -/* - * ao_convert.c - * - * Given raw data, convert to SI units - */ - -/* pressure from the sensor to altitude in meters */ -int16_t -ao_pres_to_altitude(int16_t pres) __reentrant; - -int16_t -ao_altitude_to_pres(int16_t alt) __reentrant; - -int16_t -ao_temp_to_dC(int16_t temp) __reentrant; - -/* - * ao_convert_pa.c - * - * Convert between pressure in Pa and altitude in meters - */ - -#include - -alt_t -ao_pa_to_altitude(int32_t pa); - -int32_t -ao_altitude_to_pa(alt_t alt); - -#if HAS_DBG -#include -#endif - -#if HAS_SERIAL_0 || HAS_SERIAL_1 || HAS_SERIAL_2 || HAS_SERIAL_3 -#include -#endif - -/* - * ao_convert_volt.c - * - * Convert ADC readings to decivolts - */ - -int16_t -ao_battery_decivolt(int16_t adc); - -int16_t -ao_ignite_decivolt(int16_t adc); - -/* - * ao_spi_slave.c - */ - -uint8_t -ao_spi_slave_recv(void *buf, uint16_t len); - -void -ao_spi_slave_send(void *buf, uint16_t len); - -void -ao_spi_slave_init(void); - -/* This must be defined by the product; it will get called when chip - * select goes low, at which point it should use ao_spi_read and - * ao_spi_write to deal with the request - */ - -void -ao_spi_slave(void); - -#include -/* - * ao_gps.c - */ - -#define AO_GPS_NUM_SAT_MASK (0xf << 0) -#define AO_GPS_NUM_SAT_SHIFT (0) - -#define AO_GPS_VALID (1 << 4) -#define AO_GPS_RUNNING (1 << 5) -#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; -extern __xdata struct ao_telemetry_satellite ao_gps_tracking_data; - -struct ao_gps_orig { - uint8_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t minute; - uint8_t second; - uint8_t flags; - int32_t latitude; /* degrees * 10⁷ */ - int32_t longitude; /* degrees * 10⁷ */ - int16_t altitude; /* m */ - uint16_t ground_speed; /* cm/s */ - uint8_t course; /* degrees / 2 */ - uint8_t hdop; /* * 5 */ - int16_t climb_rate; /* cm/s */ - uint16_t h_error; /* m */ - uint16_t v_error; /* m */ -}; - -struct ao_gps_sat_orig { - uint8_t svid; - uint8_t c_n_1; -}; - -#define AO_MAX_GPS_TRACKING 12 - -struct ao_gps_tracking_orig { - uint8_t channels; - struct ao_gps_sat_orig sats[AO_MAX_GPS_TRACKING]; -}; - -void -ao_gps(void); - -void -ao_gps_print(__xdata struct ao_gps_orig *gps_data); - -void -ao_gps_tracking_print(__xdata struct ao_gps_tracking_orig *gps_tracking_data); - -void -ao_gps_show(void) __reentrant; - -void -ao_gps_init(void); - -/* - * ao_gps_report.c - */ - -void -ao_gps_report(void); - -void -ao_gps_report_init(void); - -/* - * ao_gps_report_mega.c - */ - -void -ao_gps_report_mega(void); - -void -ao_gps_report_mega_init(void); - -/* - * ao_telemetry_orig.c - */ - -#if LEGACY_MONITOR -struct ao_adc_orig { - uint16_t tick; /* tick when the sample was read */ - 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 */ -}; - -struct ao_telemetry_orig { - uint16_t serial; - uint16_t flight; - uint8_t flight_state; - int16_t accel; - int16_t ground_accel; - union { - struct { - int16_t speed; - int16_t unused; - } k; - int32_t flight_vel; - } u; - int16_t height; - int16_t ground_pres; - int16_t accel_plus_g; - int16_t accel_minus_g; - struct ao_adc_orig adc; - struct ao_gps_orig gps; - char callsign[AO_MAX_CALLSIGN]; - struct ao_gps_tracking_orig gps_tracking; -}; - -struct ao_telemetry_tiny { - uint16_t serial; - uint16_t flight; - uint8_t flight_state; - int16_t height; /* AGL in meters */ - int16_t speed; /* in m/s * 16 */ - int16_t accel; /* in m/s² * 16 */ - int16_t ground_pres; /* sensor units */ - struct ao_adc adc; /* raw ADC readings */ - char callsign[AO_MAX_CALLSIGN]; -}; - -struct ao_telemetry_orig_recv { - struct ao_telemetry_orig telemetry_orig; - int8_t rssi; - uint8_t status; -}; - -struct ao_telemetry_tiny_recv { - struct ao_telemetry_tiny telemetry_tiny; - int8_t rssi; - uint8_t status; -}; - -#endif /* LEGACY_MONITOR */ - -/* Unfortunately, we've exposed the CC1111 rssi units as the 'usual' method - * for reporting RSSI. So, now we use these values everywhere - */ -#define AO_RSSI_FROM_RADIO(radio) ((int16_t) ((int8_t) (radio) >> 1) - 74) -#define AO_RADIO_FROM_RSSI(rssi) (((int8_t) (rssi) + 74) << 1) - -/* - * ao_radio_recv tacks on rssi and status bytes - */ - -struct ao_telemetry_raw_recv { - uint8_t packet[AO_MAX_TELEMETRY + 2]; -}; - -/* Set delay between telemetry reports (0 to disable) */ - -#ifdef AO_SEND_ALL_BARO -#define AO_TELEMETRY_INTERVAL_PAD AO_MS_TO_TICKS(100) -#define AO_TELEMETRY_INTERVAL_FLIGHT AO_MS_TO_TICKS(100) -#define AO_TELEMETRY_INTERVAL_RECOVER AO_MS_TO_TICKS(100) -#else -#define AO_TELEMETRY_INTERVAL_PAD AO_MS_TO_TICKS(1000) -#define AO_TELEMETRY_INTERVAL_FLIGHT AO_MS_TO_TICKS(100) -#define AO_TELEMETRY_INTERVAL_RECOVER AO_MS_TO_TICKS(1000) -#endif - -void -ao_telemetry_set_interval(uint16_t interval); - -void -ao_rdf_set(uint8_t rdf); - -void -ao_telemetry_init(void); - -void -ao_telemetry_orig_init(void); - -void -ao_telemetry_tiny_init(void); - -/* - * ao_radio.c - */ - -extern __xdata uint8_t ao_radio_dma; - -extern __xdata int8_t ao_radio_rssi; - -#ifdef PKT_APPEND_STATUS_1_CRC_OK -#define AO_RADIO_STATUS_CRC_OK PKT_APPEND_STATUS_1_CRC_OK -#else -#include -#define AO_RADIO_STATUS_CRC_OK AO_FEC_DECODE_CRC_OK -#endif - -#ifndef HAS_RADIO_RECV -#define HAS_RADIO_RECV HAS_RADIO -#endif -#ifndef HAS_RADIO_XMIT -#define HAS_RADIO_XMIT HAS_RADIO -#endif - -void -ao_radio_general_isr(void) ao_arch_interrupt(16); - -#if HAS_RADIO_XMIT -void -ao_radio_send(const __xdata void *d, uint8_t size) __reentrant; -#endif - -#if HAS_RADIO_RECV -uint8_t -ao_radio_recv(__xdata void *d, uint8_t size, uint8_t timeout) __reentrant; - -void -ao_radio_recv_abort(void); -#endif - -void -ao_radio_test(uint8_t on); - -typedef int16_t (*ao_radio_fill_func)(uint8_t *buffer, int16_t len); - -void -ao_radio_send_aprs(ao_radio_fill_func fill); - -/* - * ao_radio_pa - */ - -#if HAS_RADIO_AMP -void -ao_radio_pa_on(void); - -void -ao_radio_pa_off(void); - -void -ao_radio_pa_init(void); -#else -#define ao_radio_pa_on() -#define ao_radio_pa_off() -#define ao_radio_pa_init() -#endif - -/* - * Compute the packet length as follows: - * - * 2000 bps (for a 1kHz tone) - * so, for 'ms' milliseconds, we need - * 2 * ms bits, or ms / 4 bytes - */ - -void -ao_radio_rdf(void); - -void -ao_radio_continuity(uint8_t c); - -void -ao_radio_rdf_abort(void); - -void -ao_radio_init(void); - -/* - * ao_monitor.c - */ - -#if HAS_MONITOR - -extern const char const * const ao_state_names[]; - -#define AO_MONITOR_RING 8 - -union ao_monitor { - struct ao_telemetry_raw_recv raw; - struct ao_telemetry_all_recv all; -#if LEGACY_MONITOR - struct ao_telemetry_orig_recv orig; - struct ao_telemetry_tiny_recv tiny; -#endif -}; - -extern __xdata union ao_monitor ao_monitor_ring[AO_MONITOR_RING]; - -#define ao_monitor_ring_next(n) (((n) + 1) & (AO_MONITOR_RING - 1)) - -extern __data uint8_t ao_monitoring; -extern __data uint8_t ao_monitor_head; - -void -ao_monitor(void); - -#define AO_MONITORING_OFF 0 -#define AO_MONITORING_ORIG 1 - -void -ao_monitor_set(uint8_t monitoring); - -void -ao_monitor_disable(void); - -void -ao_monitor_enable(void); - -void -ao_monitor_init(void) __reentrant; - -#endif - -/* - * ao_stdio.c - */ - -#define AO_READ_AGAIN (-1) - -struct ao_stdio { - int (*_pollchar)(void); /* Called with interrupts blocked */ - void (*putchar)(char c) __reentrant; - void (*flush)(void); - uint8_t echo; -}; - -extern __xdata struct ao_stdio ao_stdios[]; -extern __pdata int8_t ao_cur_stdio; -extern __pdata int8_t ao_num_stdios; - -void -flush(void); - -extern __xdata uint8_t ao_stdin_ready; - -uint8_t -ao_echo(void); - -int8_t -ao_add_stdio(int (*pollchar)(void), - void (*putchar)(char) __reentrant, - void (*flush)(void)) __reentrant; - -/* - * ao_ignite.c - */ - -enum ao_igniter { - ao_igniter_drogue = 0, - ao_igniter_main = 1 -}; - -void -ao_ignite(enum ao_igniter igniter); - -enum ao_igniter_status { - ao_igniter_unknown, /* unknown status (ambiguous voltage) */ - ao_igniter_ready, /* continuity detected */ - ao_igniter_active, /* igniter firing */ - ao_igniter_open, /* open circuit detected */ -}; - -struct ao_ignition { - uint8_t request; - uint8_t fired; - uint8_t firing; -}; - -extern __code char * __code ao_igniter_status_names[]; - -extern __xdata struct ao_ignition ao_ignition[2]; - -enum ao_igniter_status -ao_igniter_status(enum ao_igniter igniter); - -extern __pdata uint8_t ao_igniter_present; - -void -ao_ignite_set_pins(void); - -void -ao_igniter_init(void); - -/* - * ao_config.c - */ - -#if AO_PYRO_NUM -#include -#endif - -#if HAS_FORCE_FREQ -/* - * Set this to force the frequency to 434.550MHz - */ -extern __xdata uint8_t ao_force_freq; -#endif - -#define AO_CONFIG_MAJOR 1 -#define AO_CONFIG_MINOR 15 - -#define AO_AES_LEN 16 - -extern __xdata uint8_t ao_config_aes_seq; - -struct ao_config { - uint8_t major; - uint8_t minor; - uint16_t main_deploy; - int16_t accel_plus_g; /* changed for minor version 2 */ - uint8_t _legacy_radio_channel; - char callsign[AO_MAX_CALLSIGN + 1]; - uint8_t apogee_delay; /* minor version 1 */ - int16_t accel_minus_g; /* minor version 2 */ - uint32_t radio_cal; /* minor version 3 */ - uint32_t flight_log_max; /* minor version 4 */ - uint8_t ignite_mode; /* minor version 5 */ - uint8_t pad_orientation; /* minor version 6 */ - uint32_t radio_setting; /* minor version 7 */ - uint8_t radio_enable; /* minor version 8 */ - uint8_t aes_key[AO_AES_LEN]; /* minor version 9 */ - uint32_t frequency; /* minor version 10 */ - uint16_t apogee_lockout; /* minor version 11 */ -#if AO_PYRO_NUM - struct ao_pyro pyro[AO_PYRO_NUM]; /* minor version 12 */ -#endif - uint16_t aprs_interval; /* minor version 13 */ -#if HAS_RADIO_POWER - uint8_t radio_power; /* minor version 14 */ -#endif -#if HAS_RADIO_AMP - uint8_t radio_amp; /* minor version 14 */ -#endif -#if HAS_GYRO - int16_t accel_zero_along; /* minor version 15 */ - int16_t accel_zero_across; /* minor version 15 */ - int16_t accel_zero_through; /* minor version 15 */ -#endif -}; - -#define AO_IGNITE_MODE_DUAL 0 -#define AO_IGNITE_MODE_APOGEE 1 -#define AO_IGNITE_MODE_MAIN 2 - -#define AO_RADIO_ENABLE_CORE 1 -#define AO_RADIO_DISABLE_TELEMETRY 2 -#define AO_RADIO_DISABLE_RDF 4 - -#define AO_PAD_ORIENTATION_ANTENNA_UP 0 -#define AO_PAD_ORIENTATION_ANTENNA_DOWN 1 - -extern __xdata struct ao_config ao_config; - -#define AO_CONFIG_MAX_SIZE 128 - -void -_ao_config_edit_start(void); - -void -_ao_config_edit_finish(void); - -void -ao_config_get(void); - -void -ao_config_put(void); - -void -ao_config_set_radio(void); - -void -ao_config_init(void); - -/* - * ao_rssi.c - */ - -void -ao_rssi_set(int rssi_value); - -void -ao_rssi_init(uint8_t rssi_led); - -/* - * ao_product.c - * - * values which need to be defined for - * each instance of a product - */ - -extern const char ao_version[]; -extern const char ao_manufacturer[]; -extern const char ao_product[]; - -/* - * Fifos - */ - -#define AO_FIFO_SIZE 32 - -struct ao_fifo { - uint8_t insert; - uint8_t remove; - char fifo[AO_FIFO_SIZE]; -}; - -#define ao_fifo_insert(f,c) do { \ - (f).fifo[(f).insert] = (c); \ - (f).insert = ((f).insert + 1) & (AO_FIFO_SIZE-1); \ -} while(0) - -#define ao_fifo_remove(f,c) do {\ - c = (f).fifo[(f).remove]; \ - (f).remove = ((f).remove + 1) & (AO_FIFO_SIZE-1); \ -} while(0) - -#define ao_fifo_full(f) ((((f).insert + 1) & (AO_FIFO_SIZE-1)) == (f).remove) -#define ao_fifo_empty(f) ((f).insert == (f).remove) - -#if PACKET_HAS_MASTER || PACKET_HAS_SLAVE -#include -#endif - -#if HAS_BTM -#include -#endif - -#if HAS_COMPANION -#include -#endif - -#if HAS_LCD -#include -#endif - -#if HAS_AES -#include -#endif - -/* ao_launch.c */ - -struct ao_launch_command { - uint16_t tick; - uint16_t serial; - uint8_t cmd; - uint8_t channel; - uint16_t unused; -}; - -#define AO_LAUNCH_QUERY 1 - -struct ao_launch_query { - uint16_t tick; - uint16_t serial; - uint8_t channel; - uint8_t valid; - uint8_t arm_status; - uint8_t igniter_status; -}; - -#define AO_LAUNCH_ARM 2 -#define AO_LAUNCH_FIRE 3 - -void -ao_launch_init(void); - -/* - * ao_log_single.c - */ - -#define AO_LOG_TELESCIENCE_START ((uint8_t) 's') -#define AO_LOG_TELESCIENCE_DATA ((uint8_t) 'd') - -#define AO_LOG_TELESCIENCE_NUM_ADC 12 - -struct ao_log_telescience { - uint8_t type; - uint8_t csum; - uint16_t tick; - uint16_t tm_tick; - uint8_t tm_state; - uint8_t unused; - uint16_t adc[AO_LOG_TELESCIENCE_NUM_ADC]; -}; - -#define AO_LOG_SINGLE_SIZE 32 - -union ao_log_single { - struct ao_log_telescience telescience; - union ao_telemetry_all telemetry; - uint8_t bytes[AO_LOG_SINGLE_SIZE]; -}; - -extern __xdata union ao_log_single ao_log_single_write_data; -extern __xdata union ao_log_single ao_log_single_read_data; - -void -ao_log_single_extra_query(void); - -void -ao_log_single_list(void); - -void -ao_log_single_main(void); - -uint8_t -ao_log_single_write(void); - -uint8_t -ao_log_single_read(uint32_t pos); - -void -ao_log_single_start(void); - -void -ao_log_single_stop(void); - -void -ao_log_single_restart(void); - -void -ao_log_single_set(void); - -void -ao_log_single_delete(void); - -void -ao_log_single_init(void); - -void -ao_log_single(void); - -/* - * ao_pyro_slave.c - */ - -#define AO_TELEPYRO_NUM_ADC 9 - -#ifndef ao_xmemcpy -#define ao_xmemcpy(d,s,c) memcpy(d,s,c) -#define ao_xmemset(d,v,c) memset(d,v,c) -#define ao_xmemcmp(d,s,c) memcmp(d,s,c) -#endif - -/* - * ao_terraui.c - */ - -void -ao_terraui_init(void); - -/* - * ao_battery.c - */ - -#ifdef BATTERY_PIN -void -ao_battery_isr(void) ao_arch_interrupt(1); - -uint16_t -ao_battery_get(void); - -void -ao_battery_init(void); -#endif /* BATTERY_PIN */ - -/* - * ao_sqrt.c - */ - -uint32_t -ao_sqrt(uint32_t op); - -/* - * ao_freq.c - */ - -int32_t ao_freq_to_set(int32_t freq, int32_t cal) __reentrant; - -/* - * ao_ms5607.c - */ - -void ao_ms5607_init(void); - -#include - -#endif /* _AO_H_ */ diff --git a/src/core/ao_adc.h b/src/core/ao_adc.h deleted file mode 100644 index 373db1c4..00000000 --- a/src/core/ao_adc.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_ADC_H_ -#define _AO_ADC_H_ - -#include - -/* Trigger a conversion sequence (called from the timer interrupt) */ -void -ao_adc_poll(void); - -/* Suspend the current task until another A/D sample is converted */ -void -ao_adc_sleep(void); - -/* Initialize the A/D converter */ -void -ao_adc_init(void); - -#endif /* _AO_ADC_H_ */ diff --git a/src/core/ao_aes.h b/src/core/ao_aes.h deleted file mode 100644 index c47bc2db..00000000 --- a/src/core/ao_aes.h +++ /dev/null @@ -1,54 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_AES_H_ -#define _AO_AES_H_ - -/* ao_aes.c */ - -extern __xdata uint8_t ao_aes_mutex; - -/* AES keys and blocks are 128 bits */ - -enum ao_aes_mode { - ao_aes_mode_cbc_mac -}; - -#if HAS_AES -#ifdef SDCC -void -ao_aes_isr(void) __interrupt 4; -#endif -#endif - -void -ao_aes_set_mode(enum ao_aes_mode mode); - -void -ao_aes_set_key(__xdata uint8_t *in); - -void -ao_aes_zero_iv(void); - -void -ao_aes_run(__xdata uint8_t *in, - __xdata uint8_t *out); - -void -ao_aes_init(void); - -#endif /* _AO_AES_H_ */ diff --git a/src/core/ao_balloon.c b/src/core/ao_balloon.c deleted file mode 100644 index 904a9c08..00000000 --- a/src/core/ao_balloon.c +++ /dev/null @@ -1,136 +0,0 @@ -/* - * Copyright © 2009 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_FLIGHT_TEST -#include "ao.h" -#endif - -#ifndef HAS_ACCEL -#error Please define HAS_ACCEL -#endif - -#ifndef HAS_GPS -#error Please define HAS_GPS -#endif - -#ifndef HAS_USB -#error Please define HAS_USB -#endif - -#if HAS_SENSOR_ERRORS -/* Any sensor can set this to mark the flight computer as 'broken' */ -__xdata uint8_t ao_sensor_errors; -#endif - -__pdata uint16_t ao_motor_number; /* number of motors burned so far */ - -/* Main flight thread. */ - -__pdata enum ao_flight_state ao_flight_state; /* current flight state */ - -__pdata uint8_t ao_flight_force_idle; - -void -ao_flight(void) -{ - ao_sample_init(); - ao_flight_state = ao_flight_startup; - for (;;) { - - /* - * Process ADC samples, just looping - * until the sensors are calibrated. - */ - if (!ao_sample()) - continue; - - switch (ao_flight_state) { - case ao_flight_startup: - - /* Check to see what mode we should go to. - * - Invalid mode if accel cal appears to be out - * - pad mode if we're upright, - * - idle mode otherwise - */ - if (!ao_flight_force_idle) - { - /* Set pad mode - we can fly! */ - ao_flight_state = ao_flight_pad; -#if HAS_USB - /* Disable the USB controller in flight mode - * to save power - */ - if (!ao_usb_running) - ao_usb_disable(); -#endif - - /* Disable packet mode in pad state */ - ao_packet_slave_stop(); - - /* Turn on telemetry system */ - ao_rdf_set(1); - ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_BALLOON); - - /* signal successful initialization by turning off the LED */ - ao_led_off(AO_LED_RED); - } else { - /* Set idle mode */ - ao_flight_state = ao_flight_idle; - - /* signal successful initialization by turning off the LED */ - ao_led_off(AO_LED_RED); - } - /* wakeup threads due to state change */ - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - - break; - case ao_flight_pad: - - /* pad to coast: - * - * barometer: > 20m vertical motion - */ - if (ao_height > AO_M_TO_HEIGHT(20)) - { - ao_flight_state = ao_flight_drogue; - - /* start logging data */ - ao_log_start(); - -#if HAS_GPS - /* Record current GPS position by waking up GPS log tasks */ - 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)); - } - break; - default: - break; - } - } -} - -static __xdata struct ao_task flight_task; - -void -ao_flight_init(void) -{ - ao_flight_state = ao_flight_startup; - ao_add_task(&flight_task, ao_flight, "flight"); -} diff --git a/src/core/ao_beep.h b/src/core/ao_beep.h deleted file mode 100644 index 55f61171..00000000 --- a/src/core/ao_beep.h +++ /dev/null @@ -1,74 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_BEEP_H_ -#define _AO_BEEP_H_ - -/* - * ao_beep.c - */ - -/* - * Various pre-defined beep frequencies - * - * frequency = 1/2 (24e6/32) / beep - */ - -#define AO_BEEP_LOW 150 /* 2500Hz */ -#define AO_BEEP_MID 94 /* 3989Hz */ -#define AO_BEEP_HIGH 75 /* 5000Hz */ -#define AO_BEEP_OFF 0 /* off */ - -#define AO_BEEP_g 240 /* 1562.5Hz */ -#define AO_BEEP_gs 227 /* 1652Hz (1655Hz) */ -#define AO_BEEP_aa 214 /* 1752Hz (1754Hz) */ -#define AO_BEEP_bbf 202 /* 1856Hz (1858Hz) */ -#define AO_BEEP_bb 190 /* 1974Hz (1969Hz) */ -#define AO_BEEP_cc 180 /* 2083Hz (2086Hz) */ -#define AO_BEEP_ccs 170 /* 2205Hz (2210Hz) */ -#define AO_BEEP_dd 160 /* 2344Hz (2341Hz) */ -#define AO_BEEP_eef 151 /* 2483Hz (2480Hz) */ -#define AO_BEEP_ee 143 /* 2622Hz (2628Hz) */ -#define AO_BEEP_ff 135 /* 2778Hz (2784Hz) */ -#define AO_BEEP_ffs 127 /* 2953Hz (2950Hz) */ -#define AO_BEEP_gg 120 /* 3125Hz */ -#define AO_BEEP_ggs 113 /* 3319Hz (3311Hz) */ -#define AO_BEEP_aaa 107 /* 3504Hz (3508Hz) */ -#define AO_BEEP_bbbf 101 /* 3713Hz (3716Hz) */ -#define AO_BEEP_bbb 95 /* 3947Hz (3937Hz) */ -#define AO_BEEP_ccc 90 /* 4167Hz (4171Hz) */ -#define AO_BEEP_cccs 85 /* 4412Hz (4419Hz) */ -#define AO_BEEP_ddd 80 /* 4688Hz (4682Hz) */ -#define AO_BEEP_eeef 76 /* 4934Hz (4961Hz) */ -#define AO_BEEP_eee 71 /* 5282Hz (5256Hz) */ -#define AO_BEEP_fff 67 /* 5597Hz (5568Hz) */ -#define AO_BEEP_fffs 64 /* 5859Hz (5899Hz) */ -#define AO_BEEP_ggg 60 /* 6250Hz */ - -/* Set the beeper to the specified tone */ -void -ao_beep(uint8_t beep); - -/* Turn on the beeper for the specified time */ -void -ao_beep_for(uint8_t beep, uint16_t ticks) __reentrant; - -/* Initialize the beeper */ -void -ao_beep_init(void); - -#endif /* _AO_BEEP_H_ */ diff --git a/src/core/ao_btm.h b/src/core/ao_btm.h deleted file mode 100644 index 484e5d7f..00000000 --- a/src/core/ao_btm.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_BTM_H_ -#define _AO_BTM_H_ - -/* ao_btm.c */ - -/* If bt_link is on P2, this interrupt is shared by USB, so the USB - * code calls this function. Otherwise, it's a regular ISR. - */ - -void -ao_btm_isr(void) -#if BT_LINK_ON_P1 - __interrupt 15 -#endif - ; -void -ao_btm_init(void); - -#endif /* _AO_BTM_H_ */ diff --git a/src/core/ao_cmd.c b/src/core/ao_cmd.c deleted file mode 100644 index 4ebaa607..00000000 --- a/src/core/ao_cmd.c +++ /dev/null @@ -1,420 +0,0 @@ -/* - * Copyright © 2009 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_task.h" - -__pdata uint16_t ao_cmd_lex_i; -__pdata uint32_t ao_cmd_lex_u32; -__pdata char ao_cmd_lex_c; -__pdata enum ao_cmd_status ao_cmd_status; - -#define CMD_LEN 48 - -static __xdata char cmd_line[CMD_LEN]; -static __pdata uint8_t cmd_len; -static __pdata uint8_t cmd_i; - -void -ao_put_string(__code char *s) -{ - char c; - while ((c = *s++)) - putchar(c); -} - -static void -backspace(void) -{ - ao_put_string ("\010 \010"); -} - -static void -readline(void) -{ - char c; - if (ao_echo()) - ao_put_string("> "); - cmd_len = 0; - for (;;) { - flush(); - c = getchar(); - /* backspace/delete */ - if (c == '\010' || c == '\177') { - if (cmd_len != 0) { - if (ao_echo()) - backspace(); - --cmd_len; - } - continue; - } - - /* ^U */ - if (c == '\025') { - while (cmd_len != 0) { - if (ao_echo()) - backspace(); - --cmd_len; - } - continue; - } - - /* map CR to NL */ - if (c == '\r') - c = '\n'; - - if (c == '\n') { - if (ao_echo()) - putchar('\n'); - break; - } - - if (cmd_len >= CMD_LEN - 2) - continue; - cmd_line[cmd_len++] = c; - if (ao_echo()) - putchar(c); - } - cmd_line[cmd_len++] = '\n'; - cmd_line[cmd_len++] = '\0'; - cmd_i = 0; -} - -void -ao_cmd_lex(void) -{ - ao_cmd_lex_c = '\n'; - if (cmd_i < cmd_len) - ao_cmd_lex_c = cmd_line[cmd_i++]; -} - -static void -putnibble(uint8_t v) -{ - if (v < 10) - putchar(v + '0'); - else - 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) -{ - ao_cmd_put8(v >> 8); - ao_cmd_put8(v); -} - -void -ao_cmd_put8(uint8_t v) -{ - putnibble((v >> 4) & 0xf); - putnibble(v & 0xf); -} - -uint8_t -ao_cmd_is_white(void) -{ - return ao_cmd_lex_c == ' ' || ao_cmd_lex_c == '\t'; -} - -void -ao_cmd_white(void) -{ - while (ao_cmd_is_white()) - ao_cmd_lex(); -} - -int8_t -ao_cmd_hexchar(char c) -{ - 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); - return -1; -} - -void -ao_cmd_hexbyte(void) -{ - uint8_t i; - int8_t n; - - ao_cmd_lex_i = 0; - ao_cmd_white(); - for (i = 0; i < 2; i++) { - n = ao_cmd_hexchar(ao_cmd_lex_c); - if (n < 0) { - ao_cmd_status = ao_cmd_syntax_error; - break; - } - ao_cmd_lex_i = (ao_cmd_lex_i << 4) | n; - ao_cmd_lex(); - } -} - -void -ao_cmd_hex(void) -{ - __pdata uint8_t r = ao_cmd_lex_error; - int8_t n; - - ao_cmd_lex_i = 0; - ao_cmd_white(); - for(;;) { - n = ao_cmd_hexchar(ao_cmd_lex_c); - if (n < 0) - break; - ao_cmd_lex_i = (ao_cmd_lex_i << 4) | n; - r = ao_cmd_success; - ao_cmd_lex(); - } - if (r != ao_cmd_success) - ao_cmd_status = r; -} - -void -ao_cmd_decimal(void) __reentrant -{ - uint8_t r = ao_cmd_lex_error; - - ao_cmd_lex_u32 = 0; - ao_cmd_white(); - for(;;) { - if ('0' <= ao_cmd_lex_c && ao_cmd_lex_c <= '9') - ao_cmd_lex_u32 = (ao_cmd_lex_u32 * 10) + (ao_cmd_lex_c - '0'); - else - break; - r = ao_cmd_success; - ao_cmd_lex(); - } - if (r != ao_cmd_success) - ao_cmd_status = r; - ao_cmd_lex_i = (uint16_t) ao_cmd_lex_u32; -} - -uint8_t -ao_match_word(__code char *word) -{ - while (*word) { - if (ao_cmd_lex_c != *word) { - ao_cmd_status = ao_cmd_syntax_error; - return 0; - } - word++; - ao_cmd_lex(); - } - return 1; -} - -static void -echo(void) -{ - ao_cmd_hex(); - if (ao_cmd_status == ao_cmd_success) - ao_stdios[ao_cur_stdio].echo = ao_cmd_lex_i != 0; -} - -static void -ao_reboot(void) -{ - ao_cmd_white(); - if (!ao_match_word("eboot")) - return; - /* Delay waiting for the packet master to be turned off - * so that we don't end up back in idle mode because we - * received a packet after boot. - */ - flush(); - ao_delay(AO_SEC_TO_TICKS(1)); - ao_arch_reboot(); - ao_panic(AO_PANIC_REBOOT); -} - -#ifndef HAS_VERSION -#define HAS_VERSION 1 -#endif - -#if HAS_VERSION -static void -version(void) -{ - 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 - , ao_log_format -#endif - ); - printf("software-version %s\n", ao_version); -} -#endif - -#ifndef NUM_CMDS -#define NUM_CMDS 11 -#endif - -static __code struct ao_cmds *__xdata (ao_cmds[NUM_CMDS]); -static __pdata uint8_t ao_ncmds; - -static void -help(void) -{ - __pdata uint8_t cmds; - __pdata uint8_t cmd; - __code struct ao_cmds * __pdata cs; - __code const char *h; - uint8_t e; - - for (cmds = 0; cmds < ao_ncmds; cmds++) { - cs = ao_cmds[cmds]; - for (cmd = 0; cs[cmd].func; cmd++) { - h = cs[cmd].help; - ao_put_string(h); - e = strlen(h); - h += e + 1; - e = 45 - e; - while (e--) - putchar(' '); - ao_put_string(h); - putchar('\n'); - } - } -} - -static void -report(void) -{ - switch(ao_cmd_status) { - case ao_cmd_lex_error: - case ao_cmd_syntax_error: - puts("Syntax error"); - ao_cmd_status = 0; - default: - break; - } -} - -void -ao_cmd_register(__code struct ao_cmds *cmds) -{ - if (ao_ncmds >= NUM_CMDS) - ao_panic(AO_PANIC_CMD); - ao_cmds[ao_ncmds++] = cmds; -} - -void -ao_cmd(void) -{ - __pdata char c; - uint8_t cmd, cmds; - __code struct ao_cmds * __xdata cs; - void (*__xdata func)(void); - - for (;;) { - readline(); - ao_cmd_lex(); - ao_cmd_white(); - c = ao_cmd_lex_c; - ao_cmd_lex(); - if (c == '\r' || c == '\n') - continue; - func = (void (*)(void)) NULL; - for (cmds = 0; cmds < ao_ncmds; cmds++) { - cs = ao_cmds[cmds]; - for (cmd = 0; cs[cmd].func; cmd++) - if (cs[cmd].help[0] == c) { - func = cs[cmd].func; - break; - } - if (func) - break; - } - if (func) - (*func)(); - else - ao_cmd_status = ao_cmd_syntax_error; - report(); - } -} - -#if HAS_BOOT_LOADER - -#include - -static void -ao_loader(void) -{ - flush(); - ao_boot_loader(); -} -#endif - -__xdata struct ao_task ao_cmd_task; - -__code struct ao_cmds ao_base_cmds[] = { - { help, "?\0Help" }, -#if HAS_TASK_INFO - { ao_task_info, "T\0Tasks" }, -#endif - { echo, "E <0 off, 1 on>\0Echo" }, - { ao_reboot, "r eboot\0Reboot" }, -#if HAS_VERSION - { version, "v\0Version" }, -#endif -#if HAS_BOOT_LOADER - { ao_loader, "X\0Switch to boot loader" }, -#endif - { 0, NULL }, -}; - -void -ao_cmd_init(void) -{ - ao_cmd_register(&ao_base_cmds[0]); - ao_add_task(&ao_cmd_task, ao_cmd, "cmd"); -} diff --git a/src/core/ao_companion.h b/src/core/ao_companion.h deleted file mode 100644 index 035325a3..00000000 --- a/src/core/ao_companion.h +++ /dev/null @@ -1,55 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_COMPANION_H_ -#define _AO_COMPANION_H_ - -/* ao_companion.c */ - -#define AO_COMPANION_SETUP 1 -#define AO_COMPANION_FETCH 2 -#define AO_COMPANION_NOTIFY 3 - -struct ao_companion_command { - uint8_t command; - uint8_t flight_state; - uint16_t tick; - uint16_t serial; - uint16_t flight; - int16_t accel; - int16_t speed; - int16_t height; - int16_t motor_number; -}; - -struct ao_companion_setup { - uint16_t board_id; - uint16_t board_id_inverse; - uint8_t update_period; - uint8_t channels; -}; - -extern __pdata uint8_t ao_companion_running; -extern __xdata uint8_t ao_companion_mutex; -extern __xdata struct ao_companion_command ao_companion_command; -extern __xdata struct ao_companion_setup ao_companion_setup; -extern __xdata uint16_t ao_companion_data[AO_COMPANION_MAX_CHANNELS]; - -void -ao_companion_init(void); - -#endif /* _AO_COMPANION_H_ */ diff --git a/src/core/ao_config.c b/src/core/ao_config.c deleted file mode 100644 index 4482f673..00000000 --- a/src/core/ao_config.c +++ /dev/null @@ -1,805 +0,0 @@ -/* - * Copyright © 2009 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_log.h" -#include -#if HAS_FLIGHT -#include -#include -#endif - -__xdata struct ao_config ao_config; -__pdata uint8_t ao_config_loaded; -__pdata uint8_t ao_config_dirty; -__xdata uint8_t ao_config_mutex; - -#ifndef AO_CONFIG_DEFAULT_APRS_INTERVAL -#define AO_CONFIG_DEFAULT_APRS_INTERVAL 0 -#endif -#define AO_CONFIG_DEFAULT_MAIN_DEPLOY 250 -#define AO_CONFIG_DEFAULT_RADIO_CHANNEL 0 -#define AO_CONFIG_DEFAULT_CALLSIGN "N0CALL" -#define AO_CONFIG_DEFAULT_ACCEL_ZERO_G 16000 -#define AO_CONFIG_DEFAULT_APOGEE_DELAY 0 -#define AO_CONFIG_DEFAULT_IGNITE_MODE AO_IGNITE_MODE_DUAL -#define AO_CONFIG_DEFAULT_PAD_ORIENTATION AO_PAD_ORIENTATION_ANTENNA_UP -#if HAS_EEPROM -#ifndef USE_INTERNAL_FLASH -#error Please define USE_INTERNAL_FLASH -#endif -#endif -#ifndef AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX -#if USE_INTERNAL_FLASH -#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX ao_storage_config -#else -#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX ((uint32_t) 192 * (uint32_t) 1024) -#endif -#endif -#ifndef AO_CONFIG_DEFAULT_RADIO_POWER -#define AO_CONFIG_DEFAULT_RADIO_POWER 0x60 -#endif -#define AO_CONFIG_DEFAULT_RADIO_AMP 0 - -#if HAS_EEPROM -static void -_ao_config_put(void) -{ - ao_config_setup(); - ao_config_erase(); - ao_config_write(0, &ao_config, sizeof (ao_config)); -#if HAS_FLIGHT - ao_log_write_erase(0); -#endif - ao_config_flush(); -} - -void -ao_config_put(void) -{ - ao_mutex_get(&ao_config_mutex); - _ao_config_put(); - ao_mutex_put(&ao_config_mutex); -} -#endif - -#if HAS_RADIO -void -ao_config_set_radio(void) -{ - ao_config.radio_setting = ao_freq_to_set(ao_config.frequency, ao_config.radio_cal); -} -#endif /* HAS_RADIO */ - -static void -_ao_config_get(void) -{ - uint8_t minor; - - if (ao_config_loaded) - return; -#if HAS_EEPROM - /* Yes, I know ao_storage_read calls ao_storage_setup, - * but ao_storage_setup *also* sets ao_storage_config, which we - * need before calling ao_storage_read here - */ - ao_config_setup(); - ao_config_read(0, &ao_config, sizeof (ao_config)); -#endif - if (ao_config.major != AO_CONFIG_MAJOR) { - ao_config.major = AO_CONFIG_MAJOR; - ao_config.minor = 0; - - /* Version 0 stuff */ - ao_config.main_deploy = AO_CONFIG_DEFAULT_MAIN_DEPLOY; - ao_xmemset(&ao_config.callsign, '\0', sizeof (ao_config.callsign)); - ao_xmemcpy(&ao_config.callsign, CODE_TO_XDATA(AO_CONFIG_DEFAULT_CALLSIGN), - sizeof(AO_CONFIG_DEFAULT_CALLSIGN) - 1); - ao_config._legacy_radio_channel = 0; - } - minor = ao_config.minor; - if (minor != AO_CONFIG_MINOR) { - /* Fixups for minor version 1 */ - if (minor < 1) - ao_config.apogee_delay = AO_CONFIG_DEFAULT_APOGEE_DELAY; - /* Fixups for minor version 2 */ - if (minor < 2) { - ao_config.accel_plus_g = 0; - ao_config.accel_minus_g = 0; - } - /* Fixups for minor version 3 */ -#if HAS_RADIO - if (minor < 3) - ao_config.radio_cal = ao_radio_cal; -#endif - /* Fixups for minor version 4 */ -#if HAS_FLIGHT - if (minor < 4) - ao_config.flight_log_max = AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX; -#endif - /* Fixupes for minor version 5 */ - if (minor < 5) - ao_config.ignite_mode = AO_CONFIG_DEFAULT_IGNITE_MODE; - if (minor < 6) - ao_config.pad_orientation = AO_CONFIG_DEFAULT_PAD_ORIENTATION; - if (minor < 8) - ao_config.radio_enable = AO_RADIO_ENABLE_CORE; - if (minor < 9) - ao_xmemset(&ao_config.aes_key, '\0', AO_AES_LEN); - if (minor < 10) - ao_config.frequency = 434550 + ao_config._legacy_radio_channel * 100; - if (minor < 11) - ao_config.apogee_lockout = 0; -#if AO_PYRO_NUM - if (minor < 12) - memset(&ao_config.pyro, '\0', sizeof (ao_config.pyro)); -#endif - if (minor < 13) - ao_config.aprs_interval = AO_CONFIG_DEFAULT_APRS_INTERVAL; -#if HAS_RADIO_POWER - if (minor < 14) - ao_config.radio_power = AO_CONFIG_DEFAULT_RADIO_POWER; - #endif -#if HAS_RADIO_AMP - if (minor < 14) - ao_config.radio_amp = AO_CONFIG_DEFAULT_RADIO_AMP; -#endif -#if HAS_GYRO - if (minor < 15) { - ao_config.accel_zero_along = 0; - ao_config.accel_zero_across = 0; - ao_config.accel_zero_through = 0; - - /* Reset the main accel offsets to force - * re-calibration - */ - ao_config.accel_plus_g = 0; - ao_config.accel_minus_g = 0; - } -#endif - ao_config.minor = AO_CONFIG_MINOR; - ao_config_dirty = 1; - } -#if HAS_RADIO -#if HAS_FORCE_FREQ - if (ao_force_freq) { - ao_config.frequency = 434550; - ao_config.radio_cal = ao_radio_cal; - ao_xmemcpy(&ao_config.callsign, CODE_TO_XDATA(AO_CONFIG_DEFAULT_CALLSIGN), - sizeof(AO_CONFIG_DEFAULT_CALLSIGN) - 1); - } -#endif - ao_config_set_radio(); -#endif - ao_config_loaded = 1; -} - -void -_ao_config_edit_start(void) -{ - ao_mutex_get(&ao_config_mutex); - _ao_config_get(); -} - -void -_ao_config_edit_finish(void) -{ - ao_config_dirty = 1; - ao_mutex_put(&ao_config_mutex); -} - -void -ao_config_get(void) -{ - _ao_config_edit_start(); - ao_mutex_put(&ao_config_mutex); -} - -void -ao_config_callsign_show(void) -{ - printf ("Callsign: \"%s\"\n", ao_config.callsign); -} - -void -ao_config_callsign_set(void) __reentrant -{ - uint8_t c; - static __xdata char callsign[AO_MAX_CALLSIGN + 1]; - - ao_xmemset(callsign, '\0', sizeof callsign); - ao_cmd_white(); - c = 0; - while (ao_cmd_lex_c != '\n') { - if (c < AO_MAX_CALLSIGN) - callsign[c++] = ao_cmd_lex_c; - else - ao_cmd_status = ao_cmd_lex_error; - ao_cmd_lex(); - } - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_xmemcpy(&ao_config.callsign, &callsign, - AO_MAX_CALLSIGN + 1); - _ao_config_edit_finish(); -} - -#if HAS_RADIO - -void -ao_config_frequency_show(void) __reentrant -{ - printf("Frequency: %ld\n", - ao_config.frequency); -} - -void -ao_config_frequency_set(void) __reentrant -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.frequency = ao_cmd_lex_u32; - ao_config_set_radio(); - _ao_config_edit_finish(); -#if HAS_RADIO_RECV - ao_radio_recv_abort(); -#endif -} -#endif - -#if HAS_FLIGHT - -void -ao_config_main_deploy_show(void) __reentrant -{ - printf("Main deploy: %d meters\n", - ao_config.main_deploy); -} - -void -ao_config_main_deploy_set(void) __reentrant -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.main_deploy = ao_cmd_lex_i; - _ao_config_edit_finish(); -} - -#if HAS_ACCEL -void -ao_config_accel_calibrate_show(void) __reentrant -{ - printf("Accel cal +1g: %d -1g: %d\n", - ao_config.accel_plus_g, ao_config.accel_minus_g); -#if HAS_GYRO - printf ("IMU cal along %d across %d through %d\n", - ao_config.accel_zero_along, - ao_config.accel_zero_across, - ao_config.accel_zero_through); -#endif -} - -#define ACCEL_CALIBRATE_SAMPLES 1024 -#define ACCEL_CALIBRATE_SHIFT 10 - -#if HAS_GYRO -static int16_t accel_cal_along; -static int16_t accel_cal_across; -static int16_t accel_cal_through; -#endif - -static int16_t -ao_config_accel_calibrate_auto(char *orientation) __reentrant -{ - uint16_t i; - int32_t accel_total; - uint8_t cal_data_ring; -#if HAS_GYRO - int32_t accel_along_total = 0; - int32_t accel_across_total = 0; - int32_t accel_through_total = 0; -#endif - - printf("Orient antenna %s and press a key...", orientation); - flush(); - (void) getchar(); - puts("\r\n"); flush(); - puts("Calibrating..."); flush(); - i = ACCEL_CALIBRATE_SAMPLES; - accel_total = 0; - cal_data_ring = ao_sample_data; - while (i) { - ao_sleep(DATA_TO_XDATA(&ao_sample_data)); - while (i && cal_data_ring != ao_sample_data) { - accel_total += (int32_t) ao_data_accel(&ao_data_ring[cal_data_ring]); -#if HAS_GYRO - accel_along_total += (int32_t) ao_data_along(&ao_data_ring[cal_data_ring]); - accel_across_total += (int32_t) ao_data_across(&ao_data_ring[cal_data_ring]); - accel_through_total += (int32_t) ao_data_through(&ao_data_ring[cal_data_ring]); -#endif - cal_data_ring = ao_data_ring_next(cal_data_ring); - i--; - } - } -#if HAS_GYRO - accel_cal_along = accel_along_total >> ACCEL_CALIBRATE_SHIFT; - accel_cal_across = accel_across_total >> ACCEL_CALIBRATE_SHIFT; - accel_cal_through = accel_through_total >> ACCEL_CALIBRATE_SHIFT; -#endif - return accel_total >> ACCEL_CALIBRATE_SHIFT; -} - -void -ao_config_accel_calibrate_set(void) __reentrant -{ - int16_t up, down; -#if HAS_GYRO - int16_t accel_along_up = 0, accel_along_down = 0; - int16_t accel_across_up = 0, accel_across_down = 0; - int16_t accel_through_up = 0, accel_through_down = 0; -#endif - - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - if (ao_cmd_lex_i == 0) { - up = ao_config_accel_calibrate_auto("up"); -#if HAS_GYRO - accel_along_up = accel_cal_along; - accel_across_up = accel_cal_across; - accel_through_up = accel_cal_through; -#endif - down = ao_config_accel_calibrate_auto("down"); -#if HAS_GYRO - accel_along_down = accel_cal_along; - accel_across_down = accel_cal_across; - accel_through_down = accel_cal_through; -#endif - } else { - up = ao_cmd_lex_i; - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - down = ao_cmd_lex_i; - } - if (up >= down) { - printf("Invalid accel: up (%d) down (%d)\n", - up, down); - return; - } - _ao_config_edit_start(); - ao_config.accel_plus_g = up; - ao_config.accel_minus_g = down; -#if HAS_GYRO - if (ao_cmd_lex_i == 0) { - ao_config.accel_zero_along = (accel_along_up + accel_along_down) / 2; - ao_config.accel_zero_across = (accel_across_up + accel_across_down) / 2; - ao_config.accel_zero_through = (accel_through_up + accel_through_down) / 2; - } -#endif - _ao_config_edit_finish(); -} -#endif /* HAS_ACCEL */ - -void -ao_config_apogee_delay_show(void) __reentrant -{ - printf("Apogee delay: %d seconds\n", - ao_config.apogee_delay); -} - -void -ao_config_apogee_delay_set(void) __reentrant -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.apogee_delay = ao_cmd_lex_i; - _ao_config_edit_finish(); -} - -void -ao_config_apogee_lockout_show(void) __reentrant -{ - printf ("Apogee lockout: %d seconds\n", - ao_config.apogee_lockout); -} - -void -ao_config_apogee_lockout_set(void) __reentrant -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.apogee_lockout = ao_cmd_lex_i; - _ao_config_edit_finish(); -} - -#endif /* HAS_FLIGHT */ - -#if HAS_RADIO -void -ao_config_radio_cal_show(void) __reentrant -{ - printf("Radio cal: %ld\n", ao_config.radio_cal); -} - -void -ao_config_radio_cal_set(void) __reentrant -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.radio_cal = ao_cmd_lex_u32; - ao_config_set_radio(); - _ao_config_edit_finish(); -} -#endif - -#if HAS_LOG -void -ao_config_log_show(void) __reentrant -{ - printf("Max flight log: %d kB\n", (int16_t) (ao_config.flight_log_max >> 10)); -} - -void -ao_config_log_set(void) __reentrant -{ - uint16_t block = (uint16_t) (ao_storage_block >> 10); - uint16_t log_max = (uint16_t) (ao_storage_log_max >> 10); - - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - if (ao_log_present()) - printf("Storage must be empty before changing log size\n"); - else if (block > 1024 && (ao_cmd_lex_i & (block - 1))) - printf("Flight log size must be multiple of %d kB\n", block); - else if (ao_cmd_lex_i > log_max) - printf("Flight log max %d kB\n", log_max); - else { - _ao_config_edit_start(); - ao_config.flight_log_max = (uint32_t) ao_cmd_lex_i << 10; - _ao_config_edit_finish(); - } -} -#endif /* HAS_LOG */ - -#if HAS_IGNITE -void -ao_config_ignite_mode_show(void) __reentrant -{ - printf("Ignite mode: %d\n", ao_config.ignite_mode); -} - -void -ao_config_ignite_mode_set(void) __reentrant -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.ignite_mode = ao_cmd_lex_i; - _ao_config_edit_finish(); -} -#endif - -#if HAS_ACCEL -void -ao_config_pad_orientation_show(void) __reentrant -{ - printf("Pad orientation: %d\n", ao_config.pad_orientation); -} - -#ifndef AO_ACCEL_INVERT -#define AO_ACCEL_INVERT 0x7fff -#endif - -void -ao_config_pad_orientation_set(void) __reentrant -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_cmd_lex_i &= 1; - if (ao_config.pad_orientation != ao_cmd_lex_i) { - int16_t t; - t = ao_config.accel_plus_g; - ao_config.accel_plus_g = AO_ACCEL_INVERT - ao_config.accel_minus_g; - ao_config.accel_minus_g = AO_ACCEL_INVERT - t; - } - ao_config.pad_orientation = ao_cmd_lex_i; - _ao_config_edit_finish(); -} -#endif - -#if HAS_RADIO -void -ao_config_radio_enable_show(void) __reentrant -{ - printf("Radio enable: %d\n", ao_config.radio_enable); -} - -void -ao_config_radio_enable_set(void) __reentrant -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.radio_enable = ao_cmd_lex_i; - _ao_config_edit_finish(); -} -#endif /* HAS_RADIO */ - -#if HAS_AES - -__xdata uint8_t ao_config_aes_seq = 1; - -void -ao_config_key_show(void) __reentrant -{ - uint8_t i; - printf("AES key: "); - for (i = 0; i < AO_AES_LEN; i++) - printf ("%02x", ao_config.aes_key[i]); - printf("\n"); -} - -void -ao_config_key_set(void) __reentrant -{ - uint8_t i; - - _ao_config_edit_start(); - for (i = 0; i < AO_AES_LEN; i++) { - ao_cmd_hexbyte(); - if (ao_cmd_status != ao_cmd_success) - break; - ao_config.aes_key[i] = ao_cmd_lex_i; - } - ++ao_config_aes_seq; - _ao_config_edit_finish(); -} -#endif - -#if HAS_APRS - -void -ao_config_aprs_show(void) -{ - printf ("APRS interval: %d\n", ao_config.aprs_interval); -} - -void -ao_config_aprs_set(void) -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.aprs_interval = ao_cmd_lex_i; - _ao_config_edit_finish(); -} - -#endif /* HAS_APRS */ - -#if HAS_RADIO_AMP - -void -ao_config_radio_amp_show(void) -{ - printf ("Radio amp setting: %d\n", ao_config.radio_amp); -} - -void -ao_config_radio_amp_set(void) -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.radio_amp = ao_cmd_lex_i; - _ao_config_edit_finish(); -} - -#endif - -#if HAS_RADIO_POWER - -void -ao_config_radio_power_show(void) -{ - printf ("Radio power setting: %d\n", ao_config.radio_power); -} - -void -ao_config_radio_power_set(void) -{ - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - _ao_config_edit_start(); - ao_config.radio_power = ao_cmd_lex_i; - _ao_config_edit_finish(); -} - -#endif - -struct ao_config_var { - __code char *str; - void (*set)(void) __reentrant; - void (*show)(void) __reentrant; -}; - -static void -ao_config_help(void) __reentrant; - -static void -ao_config_show(void) __reentrant; - -#if HAS_EEPROM -static void -ao_config_save(void) __reentrant; -#endif - -__code struct ao_config_var ao_config_vars[] = { -#if HAS_FLIGHT - { "m \0Main deploy (m)", - ao_config_main_deploy_set, ao_config_main_deploy_show, }, - { "d \0Apogee delay (s)", - ao_config_apogee_delay_set, ao_config_apogee_delay_show }, - { "L \0Apogee detect lockout (s)", - ao_config_apogee_lockout_set, ao_config_apogee_lockout_show, }, -#endif /* HAS_FLIGHT */ -#if HAS_RADIO - { "F \0Frequency (kHz)", - ao_config_frequency_set, ao_config_frequency_show }, - { "c \0Callsign (8 char max)", - 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 \0Radio calib (cal = rf/(xtal/2^16))", - ao_config_radio_cal_set, ao_config_radio_cal_show }, -#if HAS_RADIO_POWER - { "p \0Radio power setting (0-255)", - ao_config_radio_power_set, ao_config_radio_power_show }, -#endif -#if HAS_RADIO_AMP - { "d \0Radio amplifier setting (0-3)", - ao_config_radio_amp_set, ao_config_radio_amp_show }, -#endif -#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_LOG - { "l \0Flight log size (kB)", - ao_config_log_set, ao_config_log_show }, -#endif -#if HAS_IGNITE - { "i <0 dual, 1 apogee, 2 main>\0Set igniter mode", - ao_config_ignite_mode_set, ao_config_ignite_mode_show }, -#endif -#if HAS_AES - { "k <32 hex digits>\0Set AES encryption key", - ao_config_key_set, ao_config_key_show }, -#endif -#if AO_PYRO_NUM - { "P \0Configure pyro channels", - ao_pyro_set, ao_pyro_show }, -#endif -#if HAS_APRS - { "A \0APRS packet interval (0 disable)", - ao_config_aprs_set, ao_config_aprs_show }, -#endif - { "s\0Show", - ao_config_show, 0 }, -#if HAS_EEPROM - { "w\0Write to eeprom", - ao_config_save, 0 }, -#endif - { "?\0Help", - ao_config_help, 0 }, - { 0, 0, 0 } -}; - -void -ao_config_set(void) -{ - char c; - uint8_t cmd; - - ao_cmd_white(); - c = ao_cmd_lex_c; - ao_cmd_lex(); - for (cmd = 0; ao_config_vars[cmd].str != NULL; cmd++) - if (ao_config_vars[cmd].str[0] == c) { - (*ao_config_vars[cmd].set)(); - return; - } - ao_cmd_status = ao_cmd_syntax_error; -} - -static void -ao_config_help(void) __reentrant -{ - uint8_t cmd; - for (cmd = 0; ao_config_vars[cmd].str != NULL; cmd++) - printf("%-20s %s\n", - ao_config_vars[cmd].str, - ao_config_vars[cmd].str+1+ - strlen(ao_config_vars[cmd].str)); -} - -static void -ao_config_show(void) __reentrant -{ - uint8_t cmd; - ao_config_get(); - printf("Config version: %d.%d\n", - ao_config.major, ao_config.minor); - for (cmd = 0; ao_config_vars[cmd].str != NULL; cmd++) - if (ao_config_vars[cmd].show) - (*ao_config_vars[cmd].show)(); -#if HAS_MS5607 - ao_ms5607_info(); -#endif -} - -#if HAS_EEPROM -static void -ao_config_save(void) __reentrant -{ - uint8_t saved = 0; - ao_mutex_get(&ao_config_mutex); - if (ao_config_dirty) { - _ao_config_put(); - ao_config_dirty = 0; - saved = 1; - } - ao_mutex_put(&ao_config_mutex); - if (saved) - puts("Saved"); - else - puts("Nothing to save"); -} -#endif - -__code struct ao_cmds ao_config_cmds[] = { - { ao_config_set, "c \0Set config (? for help, s to show)" }, - { 0, NULL }, -}; - -void -ao_config_init(void) -{ - ao_cmd_register(&ao_config_cmds[0]); -} diff --git a/src/core/ao_config.h b/src/core/ao_config.h deleted file mode 100644 index e101af8e..00000000 --- a/src/core/ao_config.h +++ /dev/null @@ -1,53 +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. - */ - -#ifndef _AO_CONFIG_H_ -#define _AO_CONFIG_H_ - -#ifndef USE_STORAGE_CONFIG -#define USE_STORAGE_CONFIG 1 -#endif - -#ifndef USE_EEPROM_CONFIG -#define USE_EEPROM_CONFIG 0 -#endif - -#if USE_STORAGE_CONFIG - -#include - -#define ao_config_setup() ao_storage_setup() -#define ao_config_erase() ao_storage_erase(ao_storage_config) -#define ao_config_write(pos,bytes, len) ao_storage_write(ao_storage_config+(pos), bytes, len) -#define ao_config_read(pos,bytes, len) ao_storage_read(ao_storage_config+(pos), bytes, len) -#define ao_config_flush() ao_storage_flush() - -#endif - -#if USE_EEPROM_CONFIG - -#include - -#define ao_config_setup() -#define ao_config_erase() -#define ao_config_write(pos,bytes, len) ao_eeprom_write(pos, bytes, len) -#define ao_config_read(pos,bytes, len) ao_eeprom_read(pos, bytes, len) -#define ao_config_flush() - -#endif - -#endif /* _AO_CONFIG_H_ */ diff --git a/src/core/ao_convert.c b/src/core/ao_convert.c deleted file mode 100644 index aa9b5f48..00000000 --- a/src/core/ao_convert.c +++ /dev/null @@ -1,89 +0,0 @@ -/* - * Copyright © 2009 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. - */ - -#if !defined(AO_CONVERT_TEST) && !defined(AO_FLIGHT_TEST) -#include "ao.h" -#endif - -static const int16_t altitude_table[] = { -#include "altitude.h" -}; - -#define ALT_FRAC_SCALE (1 << ALT_FRAC_BITS) -#define ALT_FRAC_MASK (ALT_FRAC_SCALE - 1) - -int16_t -ao_pres_to_altitude(int16_t pres) __reentrant -{ - uint8_t o; - int16_t part; - - if (pres < 0) - pres = 0; - o = pres >> ALT_FRAC_BITS; - part = pres & ALT_FRAC_MASK; - - return ((int32_t) altitude_table[o] * (ALT_FRAC_SCALE - part) + - (int32_t) altitude_table[o+1] * part + (ALT_FRAC_SCALE >> 1)) >> ALT_FRAC_BITS; -} - -#if AO_NEED_ALTITUDE_TO_PRES -int16_t -ao_altitude_to_pres(int16_t alt) __reentrant -{ - int16_t span, sub_span; - uint8_t l, h, m; - int32_t pres; - - l = 0; - h = NALT - 1; - while ((h - l) != 1) { - m = (l + h) >> 1; - if (altitude_table[m] < alt) - h = m; - else - l = m; - } - span = altitude_table[l] - altitude_table[h]; - sub_span = altitude_table[l] - alt; - pres = ((((int32_t) l * (span - sub_span) + (int32_t) h * sub_span) << ALT_FRAC_BITS) + (span >> 1)) / span; - if (pres > 32767) - pres = 32767; - if (pres < 0) - pres = 0; - return (int16_t) pres; -} -#endif - -#if 0 -int16_t -ao_temp_to_dC(int16_t temp) __reentrant -{ - int16_t ret; - - /* Output voltage at 0°C = 0.755V - * Coefficient = 0.00247V/°C - * Reference voltage = 1.25V - * - * temp = ((value / 32767) * 1.25 - 0.755) / 0.00247 - * = (value - 19791.268) / 32768 * 1.25 / 0.00247 - * ≃ (value - 19791) * 1012 / 65536 - */ - ret = ((temp - 19791) * 1012L) >> 16; - return ret; -} -#endif diff --git a/src/core/ao_convert_pa.c b/src/core/ao_convert_pa.c deleted file mode 100644 index fe6e0ef6..00000000 --- a/src/core/ao_convert_pa.c +++ /dev/null @@ -1,82 +0,0 @@ -/* - * 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. - */ - -#if !defined(AO_CONVERT_TEST) && !defined(AO_FLIGHT_TEST) -#include "ao.h" -#endif - -#ifndef AO_CONST_ATTRIB -#define AO_CONST_ATTRIB -#endif - -static const alt_t altitude_table[] AO_CONST_ATTRIB = { -#include "altitude-pa.h" -}; - -#ifndef FETCH_ALT -#define FETCH_ALT(o) altitude_table[o] -#endif - -#define ALT_SCALE (1 << ALT_SHIFT) -#define ALT_MASK (ALT_SCALE - 1) - -alt_t -ao_pa_to_altitude(int32_t pa) -{ - int16_t o; - int16_t part; - int32_t low, high; - - if (pa < 0) - pa = 0; - if (pa > 120000L) - pa = 120000L; - o = pa >> ALT_SHIFT; - part = pa & ALT_MASK; - - low = (int32_t) FETCH_ALT(o) * (ALT_SCALE - part); - high = (int32_t) FETCH_ALT(o+1) * part + (ALT_SCALE >> 1); - return (low + high) >> ALT_SHIFT; -} - -#ifdef AO_CONVERT_TEST -int32_t -ao_altitude_to_pa(int32_t alt) -{ - int32_t span, sub_span; - uint16_t l, h, m; - int32_t pa; - - l = 0; - h = NALT - 1; - while ((h - l) != 1) { - m = (l + h) >> 1; - if (altitude_table[m] < alt) - h = m; - else - l = m; - } - span = altitude_table[l] - altitude_table[h]; - sub_span = altitude_table[l] - alt; - pa = ((((int32_t) l * (span - sub_span) + (int32_t) h * sub_span) << ALT_SHIFT) + (span >> 1)) / span; - if (pa > 120000) - pa = 120000; - if (pa < 0) - pa = 0; - return pa; -} -#endif diff --git a/src/core/ao_convert_pa_test.c b/src/core/ao_convert_pa_test.c deleted file mode 100644 index 7d5b1922..00000000 --- a/src/core/ao_convert_pa_test.c +++ /dev/null @@ -1,76 +0,0 @@ -/* - * 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 -#define AO_CONVERT_TEST -typedef int32_t alt_t; -#include "ao_host.h" -#include "ao_convert_pa.c" - -#define STEP_P 1 -#define STEP_A 1 - -static inline int i_abs(int i) { return i < 0 ? -i : i; } - -int -main (int argc, char **argv) -{ - int i; - int32_t p_to_a, p_to_a_to_p; - int32_t a_to_p, a_to_p_to_a; - int max_p_error = 0, max_p_error_p = -1; - int max_a_error = 0, max_a_error_a = -1; - int p_error; - int a_error; - int ret = 0; - - for (i = 0; i < 120000 + STEP_P; i += STEP_P) { - if (i > 120000) - i = 120000; - p_to_a = ao_pa_to_altitude(i); - p_to_a_to_p = ao_altitude_to_pa(p_to_a); - p_error = i_abs(p_to_a_to_p - i); - if (p_error > max_p_error) { - max_p_error = p_error; - max_p_error_p = i; - } -// printf ("pa %d alt %d pa %d\n", -// i, p_to_a, p_to_a_to_p); - } - for (i = -1450; i < 40000 + STEP_A; i += STEP_A) { - a_to_p = ao_altitude_to_pa(i); - a_to_p_to_a = ao_pa_to_altitude(a_to_p); - a_error = i_abs(a_to_p_to_a - i); - if (a_error > max_a_error) { - max_a_error = a_error; - max_a_error_a = i; - } -// printf ("alt %d pa %d alt %d\n", -// i, a_to_p, a_to_p_to_a); - } - if (max_p_error > 2) { - printf ("max p error %d at %d\n", max_p_error, - max_p_error_p); - ret++; - } - if (max_a_error > 1) { - printf ("max a error %d at %d\n", max_a_error, - max_a_error_a); - ret++; - } - return ret; -} diff --git a/src/core/ao_convert_test.c b/src/core/ao_convert_test.c deleted file mode 100644 index 87e76841..00000000 --- a/src/core/ao_convert_test.c +++ /dev/null @@ -1,76 +0,0 @@ -/* - * 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. - */ - -#include -#define AO_CONVERT_TEST -#define AO_NEED_ALTITUDE_TO_PRES 1 -#include "ao_host.h" -#include "ao_convert.c" - -#define STEP 1 - -static inline int i_abs(int i) { return i < 0 ? -i : i; } - -int main (int argc, char **argv) -{ - int i; - int16_t p_to_a, p_to_a_to_p; - int16_t a_to_p, a_to_p_to_a; - int max_p_error = 0, max_p_error_p = -1; - int max_a_error = 0, max_a_error_a = -1; - int p_error; - int a_error; - int ret = 0; - - for (i = 0; i < 32767 + STEP; i += STEP) { - if (i > 32767) - i = 32767; - p_to_a = ao_pres_to_altitude(i); - p_to_a_to_p = ao_altitude_to_pres(p_to_a); - p_error = i_abs(p_to_a_to_p - i); - if (p_error > max_p_error) { - max_p_error = p_error; - max_p_error_p = i; - } -// printf ("pres %d alt %d pres %d\n", -// i, p_to_a, p_to_a_to_p); - } - for (i = -1578; i < 15835 + STEP; i += STEP) { - if (i > 15835) - i = 15835; - a_to_p = ao_altitude_to_pres(i); - a_to_p_to_a = ao_pres_to_altitude(a_to_p); - a_error = i_abs(a_to_p_to_a - i); - if (a_error > max_a_error) { - max_a_error = a_error; - max_a_error_a = i; - } -// printf ("alt %d pres %d alt %d\n", -// i, a_to_p, a_to_p_to_a); - } - if (max_p_error > 2) { - printf ("max p error %d at %d\n", max_p_error, - max_p_error_p); - ret++; - } - if (max_a_error > 1) { - printf ("max a error %d at %d\n", max_a_error, - max_a_error_a); - ret++; - } - return ret; -} diff --git a/src/core/ao_convert_volt.c b/src/core/ao_convert_volt.c deleted file mode 100644 index 8556d423..00000000 --- a/src/core/ao_convert_volt.c +++ /dev/null @@ -1,33 +0,0 @@ -/* - * Copyright © 2014 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" - -#define scale(v,p,m) ((int32_t) (v) * (AO_ADC_REFERENCE_DV * ((p) + (m))) / (AO_ADC_MAX * (m))) - -int16_t -ao_battery_decivolt(int16_t adc) -{ - return scale(adc, AO_BATTERY_DIV_PLUS, AO_BATTERY_DIV_MINUS); -} - -int16_t -ao_ignite_decivolt(int16_t adc) -{ - return scale(adc, AO_IGNITE_DIV_PLUS, AO_IGNITE_DIV_MINUS); -} - diff --git a/src/core/ao_data.c b/src/core/ao_data.c deleted file mode 100644 index 6a3d02a1..00000000 --- a/src/core/ao_data.c +++ /dev/null @@ -1,36 +0,0 @@ -/* - * 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 - -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) -{ -#if HAS_FLIGHT - uint8_t i = ao_data_ring_prev(ao_sample_data); -#else - uint8_t i = ao_data_ring_prev(ao_data_head); -#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 deleted file mode 100644 index c4b062fd..00000000 --- a/src/core/ao_data.h +++ /dev/null @@ -1,338 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_DATA_H_ -#define _AO_DATA_H_ - -#define GRAVITY 9.80665 - -#if HAS_ADC -#define AO_DATA_ADC (1 << 0) -#else -#define AO_DATA_ADC 0 -#endif - -#if HAS_MS5607 -#include -#define AO_DATA_MS5607 (1 << 1) -#else -#define AO_DATA_MS5607 0 -#endif - -#if HAS_MPU6000 -#include -#define AO_DATA_MPU6000 (1 << 2) -#else -#define AO_DATA_MPU6000 0 -#endif - -#if HAS_HMC5883 -#include -#define AO_DATA_HMC5883 (1 << 3) -#else -#define AO_DATA_HMC5883 0 -#endif - -#if HAS_MMA655X -#include -#define AO_DATA_MMA655X (1 << 4) -#else -#define AO_DATA_MMA655X 0 -#endif - -#ifdef AO_DATA_RING - -#define AO_DATA_ALL (AO_DATA_ADC|AO_DATA_MS5607|AO_DATA_MPU6000|AO_DATA_HMC5883|AO_DATA_MMA655X) - -struct ao_data { - uint16_t tick; -#if HAS_ADC - struct ao_adc adc; -#endif -#if HAS_MS5607 - struct ao_ms5607_sample ms5607_raw; - struct ao_ms5607_value ms5607_cooked; -#endif -#if HAS_MPU6000 - struct ao_mpu6000_sample mpu6000; -#if !HAS_MMA655X - int16_t z_accel; -#endif -#endif -#if HAS_HMC5883 - struct ao_hmc5883_sample hmc5883; -#endif -#if HAS_MMA655X - uint16_t mma655x; -#endif -}; - -#define ao_data_ring_next(n) (((n) + 1) & (AO_DATA_RING - 1)) -#define ao_data_ring_prev(n) (((n) - 1) & (AO_DATA_RING - 1)) - -/* Get a copy of the last complete sample set */ -void -ao_data_get(__xdata struct ao_data *packet); - -extern volatile __xdata struct ao_data ao_data_ring[AO_DATA_RING]; -extern volatile __data uint8_t ao_data_head; -extern volatile __data uint8_t ao_data_present; -extern volatile __data uint8_t ao_data_count; - -/* - * Mark a section of data as ready, check for data complete - */ -#define AO_DATA_PRESENT(bit) (ao_data_present |= (bit)) - -/* - * Wait until it is time to write a sensor sample; this is - * signaled by the timer tick - */ -#define AO_DATA_WAIT() do { \ - ao_sleep(DATA_TO_XDATA ((void *) &ao_data_count)); \ - } while (0) - -#endif /* AO_DATA_RING */ - -#if !HAS_BARO && HAS_MS5607 - -/* Either an MS5607 or an MS5611 hooked to a SPI port - */ - -#define HAS_BARO 1 - -typedef int32_t pres_t; - -#ifndef AO_ALT_TYPE -#define AO_ALT_TYPE int32_t -#endif - -typedef AO_ALT_TYPE alt_t; - -#define ao_data_pres_cook(packet) ao_ms5607_convert(&packet->ms5607_raw, &packet->ms5607_cooked) - -#define ao_data_pres(packet) ((packet)->ms5607_cooked.pres) -#define ao_data_temp(packet) ((packet)->ms5607_cooked.temp) - -#define pres_to_altitude(p) ao_pa_to_altitude(p) - -#endif - -#if !HAS_BARO && HAS_ADC - -#define HAS_BARO 1 - -typedef int16_t pres_t; -typedef int16_t alt_t; - -#define ao_data_pres(packet) ((packet)->adc.pres) -#define ao_data_temp(packet) ((packet)->adc.temp) -#define pres_to_altitude(p) ao_pres_to_altitude(p) -#define ao_data_pres_cook(p) - -#endif - -#if !HAS_BARO -typedef int16_t alt_t; -#endif - -/* - * Need a few macros to pull data from the sensors: - * - * ao_data_accel_sample - pull raw sensor and convert to normalized values - * ao_data_accel - pull normalized value (lives in the same memory) - * ao_data_set_accel - store normalized value back in the sensor location - * ao_data_accel_invert - flip rocket ends for positive acceleration - */ - -#if HAS_ACCEL - -/* This section is for an analog accelerometer hooked to one of the ADC pins. As - * those are 5V parts, this also requires that the 5V supply be hooked to to anothe ADC - * pin so that the both can be measured to correct for changes between the 3.3V and 5V rails - */ - -typedef int16_t accel_t; -#define ao_data_accel(packet) ((packet)->adc.accel) -#define ao_data_set_accel(packet, a) ((packet)->adc.accel = (a)) -#define ao_data_accel_invert(a) (0x7fff -(a)) - -/* - * Ok, the math here is a bit tricky. - * - * ao_sample_accel: ADC output for acceleration - * ao_accel_ref: ADC output for the 5V reference. - * ao_cook_accel: Corrected acceleration value - * Vcc: 3.3V supply to the CC1111 - * Vac: 5V supply to the accelerometer - * accel: input voltage to accelerometer ADC pin - * ref: input voltage to 5V reference ADC pin - * - * - * Measured acceleration is ratiometric to Vcc: - * - * ao_sample_accel accel - * ------------ = ----- - * 32767 Vcc - * - * Measured 5v reference is also ratiometric to Vcc: - * - * ao_accel_ref ref - * ------------ = ----- - * 32767 Vcc - * - * - * ao_accel_ref = 32767 * (ref / Vcc) - * - * Acceleration is measured ratiometric to the 5V supply, - * so what we want is: - * - * ao_cook_accel accel - * ------------- = ----- - * 32767 ref - * - * - * accel Vcc - * = ----- * --- - * Vcc ref - * - * ao_sample_accel 32767 - * = ------------ * ------------ - * 32767 ao_accel_ref - * - * Multiply through by 32767: - * - * ao_sample_accel * 32767 - * ao_cook_accel = -------------------- - * ao_accel_ref - * - * Now, the tricky part. Getting this to compile efficiently - * and keeping all of the values in-range. - * - * First off, we need to use a shift of 16 instead of * 32767 as SDCC - * does the obvious optimizations for byte-granularity shifts: - * - * ao_cook_accel = (ao_sample_accel << 16) / ao_accel_ref - * - * Next, lets check our input ranges: - * - * 0 <= ao_sample_accel <= 0x7fff (singled ended ADC conversion) - * 0x7000 <= ao_accel_ref <= 0x7fff (the 5V ref value is close to 0x7fff) - * - * Plugging in our input ranges, we get an output range of 0 - 0x12490, - * which is 17 bits. That won't work. If we take the accel ref and shift - * by a bit, we'll change its range: - * - * 0xe000 <= ao_accel_ref<<1 <= 0xfffe - * - * ao_cook_accel = (ao_sample_accel << 16) / (ao_accel_ref << 1) - * - * Now the output range is 0 - 0x9248, which nicely fits in 16 bits. It - * is, however, one bit too large for our signed computations. So, we - * take the result and shift that by a bit: - * - * ao_cook_accel = ((ao_sample_accel << 16) / (ao_accel_ref << 1)) >> 1 - * - * This finally creates an output range of 0 - 0x4924. As the ADC only - * provides 11 bits of data, we haven't actually lost any precision, - * just dropped a bit of noise off the low end. - */ - -#if HAS_ACCEL_REF - -#define ao_data_accel_cook(packet) \ - ((uint16_t) ((((uint32_t) (packet)->adc.accel << 16) / ((packet)->adc.accel_ref << 1))) >> 1) - -#else - -#define ao_data_accel_cook(packet) ((packet)->adc.accel) - -#endif /* HAS_ACCEL_REF */ - -#endif /* HAS_ACCEL */ - -#if !HAS_ACCEL && HAS_MMA655X - -#define HAS_ACCEL 1 - -typedef int16_t accel_t; - -/* MMA655X is hooked up so that positive values represent negative acceleration */ - -#define AO_ACCEL_INVERT 4095 - -#define ao_data_accel(packet) ((packet)->mma655x) -#if AO_MMA655X_INVERT -#define ao_data_accel_cook(packet) (AO_ACCEL_INVERT - (packet)->mma655x) -#else -#define ao_data_accel_cook(packet) ((packet)->mma655x) -#endif -#define ao_data_set_accel(packet, accel) ((packet)->mma655x = (accel)) -#define ao_data_accel_invert(accel) (AO_ACCEL_INVERT - (accel)) - -#endif - -#if !HAS_ACCEL && HAS_MPU6000 - -#define HAS_ACCEL 1 - -#define AO_ACCEL_INVERT 0 - -typedef int16_t accel_t; - -/* MPU6000 is hooked up so that positive y is positive acceleration */ -#define ao_data_accel(packet) ((packet)->z_accel) -#define ao_data_accel_cook(packet) (-(packet)->mpu6000.accel_y) -#define ao_data_set_accel(packet, accel) ((packet)->z_accel = (accel)) -#define ao_data_accel_invert(a) (-(a)) - -#endif - -#if !HAS_GYRO && HAS_MPU6000 - -#define HAS_GYRO 1 - -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) */ -/* Z axis is aligned perpendicular to the board (through) */ - -#define ao_data_along(packet) ((packet)->mpu6000.accel_y) -#define ao_data_across(packet) ((packet)->mpu6000.accel_x) -#define ao_data_through(packet) ((packet)->mpu6000.accel_z) - -#define ao_data_roll(packet) ((packet)->mpu6000.gyro_y) -#define ao_data_pitch(packet) ((packet)->mpu6000.gyro_x) -#define ao_data_yaw(packet) ((packet)->mpu6000.gyro_z) - -#endif - -#if !HAS_MAG && HAS_HMC5883 - -#define HAS_MAG 1 - -typedef int16_t ao_mag_t; /* in raw sample units */ - -#define ao_data_mag_along(packet) ((packet)->hmc5883.x) -#define ao_data_mag_across(packet) ((packet)->hmc5883.y) -#define ao_data_mag_through(packet) ((packet)->hmc5883.z) - -#endif - -#endif /* _AO_DATA_H_ */ diff --git a/src/core/ao_dbg.h b/src/core/ao_dbg.h deleted file mode 100644 index 181e6ec2..00000000 --- a/src/core/ao_dbg.h +++ /dev/null @@ -1,62 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_DBG_H_ -#define _AO_DBG_H_ - -/* - * ao_dbg.c - * - * debug another telemetrum board - */ - -/* Send a byte to the dbg target */ -void -ao_dbg_send_byte(uint8_t byte); - -/* Receive a byte from the dbg target */ -uint8_t -ao_dbg_recv_byte(void); - -/* Start a bulk transfer to/from dbg target memory */ -void -ao_dbg_start_transfer(uint16_t addr); - -/* End a bulk transfer to/from dbg target memory */ -void -ao_dbg_end_transfer(void); - -/* Write a byte to dbg target memory */ -void -ao_dbg_write_byte(uint8_t byte); - -/* Read a byte from dbg target memory */ -uint8_t -ao_dbg_read_byte(void); - -/* Enable dbg mode, switching use of the pins */ -void -ao_dbg_debug_mode(void); - -/* Reset the dbg target */ -void -ao_dbg_reset(void); - -void -ao_dbg_init(void); - -#endif /* _AO_DBG_H_ */ diff --git a/src/core/ao_debounce.c b/src/core/ao_debounce.c deleted file mode 100644 index b9d67729..00000000 --- a/src/core/ao_debounce.c +++ /dev/null @@ -1,163 +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 -#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 deleted file mode 100644 index 19c620f5..00000000 --- a/src/core/ao_debounce.h +++ /dev/null @@ -1,74 +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. - */ - -#ifndef _AO_DEBOUNCE_H_ -#define _AO_DEBOUNCE_H_ - -struct ao_debounce { - struct ao_debounce *next; - - /* time that pin value must be stable before accepting */ - 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 */ - uint8_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); -}; - -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); - -void -_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/core/ao_ee_fake.c b/src/core/ao_ee_fake.c deleted file mode 100644 index 7fcfcab0..00000000 --- a/src/core/ao_ee_fake.c +++ /dev/null @@ -1,37 +0,0 @@ -/* - * Copyright © 2009 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" - -/* - * For hardware without eeprom, the config code still - * wants to call these functions - */ -uint8_t -ao_ee_write_config(uint8_t *buf, uint16_t len) __reentrant -{ - (void) buf; - (void) len; - return 1; -} - -uint8_t -ao_ee_read_config(uint8_t *buf, uint16_t len) __reentrant -{ - ao_xmemset(buf, '\0', len); - return 1; -} diff --git a/src/core/ao_eeprom.h b/src/core/ao_eeprom.h deleted file mode 100644 index 915522bf..00000000 --- a/src/core/ao_eeprom.h +++ /dev/null @@ -1,43 +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. - */ - -#ifndef _AO_EEPROM_H_ -#define _AO_EEPROM_H_ - -extern const ao_pos_t ao_eeprom_total; - -/* - * Write to eeprom - */ - -uint8_t -ao_eeprom_write(ao_pos_t pos32, __xdata void *v, uint16_t len); - -/* - * Read from eeprom - */ -uint8_t -ao_eeprom_read(ao_pos_t pos, __xdata void *v, uint16_t len); - -/* - * Initialize eeprom - */ - -void -ao_eeprom_init(void); - -#endif /* _AO_EEPROM_H_ */ diff --git a/src/core/ao_fec.h b/src/core/ao_fec.h deleted file mode 100644 index 618756c1..00000000 --- a/src/core/ao_fec.h +++ /dev/null @@ -1,84 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_FEC_H_ -#define _AO_FEC_H_ - -#include - -#define AO_FEC_CRC_INIT 0xffff -#define AO_FEC_TRELLIS_TERMINATOR 0x0b -#define AO_FEC_PREPARE_EXTRA 4 - -extern const uint8_t ao_fec_whiten_table[]; - -#if AO_FEC_DEBUG -void -ao_fec_dump_bytes(const uint8_t *bytes, uint16_t len, const char *name); -#endif - -static inline uint16_t -ao_fec_crc_byte(uint8_t byte, uint16_t crc) -{ - uint8_t bit; - - for (bit = 0; bit < 8; bit++) { - if (((crc & 0x8000) >> 8) ^ (byte & 0x80)) - crc = (crc << 1) ^ 0x8005; - else - crc = (crc << 1); - byte <<= 1; - } - return crc; -} - -uint16_t -ao_fec_crc(const uint8_t *bytes, uint8_t len); - -/* - * 'len' is the length of the original data; 'bytes' - * must be four bytes longer than that, and the first - * two after 'len' must be the received crc - */ -uint8_t -ao_fec_check_crc(const uint8_t *bytes, uint8_t len); - -/* - * Compute CRC, whiten, convolve and interleave data. 'out' must be (len + 4) * 2 bytes long - */ -uint8_t -ao_fec_encode(const uint8_t *in, uint8_t len, uint8_t *out); - -/* - * Decode data. 'in' is one byte per bit, soft decision - * 'out' must be len/8 bytes long - */ - -#define AO_FEC_DECODE_BLOCK (32) /* callback must return multiples of this many bits */ - -#define AO_FEC_DECODE_CRC_OK 0x80 /* stored in out[out_len-1] */ - -uint8_t -ao_fec_decode(const uint8_t *in, uint16_t in_len, uint8_t *out, uint8_t out_len, uint16_t (*callback)(void)); - -/* - * Interleave data packed in bytes. 'out' must be 'len' bytes long. - */ -uint16_t -ao_fec_interleave_bytes(uint8_t *in, uint16_t len, uint8_t *out); - -#endif /* _AO_FEC_H_ */ diff --git a/src/core/ao_fec_rx.c b/src/core/ao_fec_rx.c deleted file mode 100644 index c4f5559a..00000000 --- a/src/core/ao_fec_rx.c +++ /dev/null @@ -1,318 +0,0 @@ -/* - * 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 - -#ifdef TELEMEGA -#include -#endif - -#if AO_PROFILE -#include - -uint32_t ao_fec_decode_start, ao_fec_decode_end; -#endif - -/* - * byte order repeats through 3 2 1 0 - * - * bit-pair order repeats through - * - * 1/0 3/2 5/4 7/6 - * - * So, the over all order is: - * - * 3,1/0 2,1/0 1,1/0 0,1/0 - * 3,3/2 2,3/2 1,3/2 0,3/2 - * 3,5/4 2,5/4 1,5/4 0,5/4 - * 3,7/6 2,7/6 1,7/6 0,7/6 - * - * The raw bit order is thus - * - * 1e/1f 16/17 0e/0f 06/07 - * 1c/1d 14/15 0c/0d 04/05 - * 1a/1b 12/13 0a/0b 02/03 - * 18/19 10/11 08/09 00/01 - */ - -static const uint8_t ao_interleave_order[] = { - 0x1e, 0x16, 0x0e, 0x06, - 0x1c, 0x14, 0x0c, 0x04, - 0x1a, 0x12, 0x0a, 0x02, - 0x18, 0x10, 0x08, 0x00 -}; - -static inline uint16_t ao_interleave_index(uint16_t i) { - return (i & ~0x1e) | ao_interleave_order[(i & 0x1e) >> 1]; -} - -#define NUM_STATE 8 -#define NUM_HIST 24 - -typedef uint32_t bits_t; - -#define V_0 0xff -#define V_1 0x00 - -/* - * These are just the 'zero' states; the 'one' states mirror them - */ -static const uint8_t ao_fec_decode_table[NUM_STATE*2] = { - V_0, V_0, /* 000 */ - V_0, V_1, /* 001 */ - V_1, V_1, /* 010 */ - V_1, V_0, /* 011 */ - V_1, V_1, /* 100 */ - V_1, V_0, /* 101 */ - V_0, V_0, /* 110 */ - V_0, V_1 /* 111 */ -}; - -static inline uint8_t -ao_next_state(uint8_t state, uint8_t bit) -{ - return ((state << 1) | bit) & 0x7; -} - -/* - * 'in' is 8-bits per symbol soft decision data - * 'len' is input byte length. 'out' must be - * 'len'/16 bytes long - */ - -uint8_t -ao_fec_decode(const uint8_t *in, uint16_t len, uint8_t *out, uint8_t out_len, uint16_t (*callback)(void)) -{ - static uint32_t cost[2][NUM_STATE]; /* path cost */ - static bits_t bits[2][NUM_STATE]; /* save bits to quickly output them */ - - uint16_t i; /* input byte index */ - uint16_t b; /* encoded symbol index (bytes/2) */ - uint16_t o; /* output bit index */ - uint8_t p; /* previous cost/bits index */ - uint8_t n; /* next cost/bits index */ - uint8_t state; /* state index */ - const uint8_t *whiten = ao_fec_whiten_table; - uint16_t interleave; /* input byte array index */ - uint8_t s0, s1; - uint16_t avail; - uint16_t crc = AO_FEC_CRC_INIT; -#if AO_PROFILE - uint32_t start_tick; -#endif - - p = 0; - for (state = 0; state < NUM_STATE; state++) { - cost[0][state] = 0x7fffffff; - bits[0][state] = 0; - } - cost[0][0] = 0; - - if (callback) - avail = 0; - else - avail = len; - -#if AO_PROFILE - if (!avail) { - avail = callback(); - if (!avail) - return 0; - } - start_tick = ao_profile_tick(); -#endif - o = 0; - for (i = 0; i < len; i += 2) { - b = i/2; - n = p ^ 1; - - if (!avail) { - avail = callback(); - if (!avail) - return 0; - } - - /* Fetch one pair of input bytes, de-interleaving - * the input. - */ - interleave = ao_interleave_index(i); - s0 = in[interleave]; - s1 = in[interleave+1]; - - avail -= 2; - - /* Compute path costs and accumulate output bit path - * for each state and encoded bit value. Unrolling - * this loop is worth about > 30% performance boost. - * Decoding 76-byte remote access packets is reduced - * from 14.700ms to 9.3ms. Redoing the loop to - * directly compare the two pasts for each future state - * reduces this down to 5.7ms - */ - - /* Ok, of course this is tricky, it's optimized. - * - * First, it's important to realize that we have 8 - * states representing the combinations of the three - * most recent bits from the encoder. Flipping any - * of these three bits flips both output bits. - * - * 'state<<1' represents the target state for a new - * bit value of 0. '(state<<1)+1' represents the - * target state for a new bit value of 1. - * - * 'state' is the previous state with an oldest bit - * value of 0. 'state + 4' is the previous state with - * an oldest bit value of 1. These two states will - * either lead to 'state<<1' or '(state<<1)+1', depending - * on whether the next encoded bit was a zero or a one. - * - * m0 and m1 are the cost of coming to 'state<<1' from - * one of the two possible previous states 'state' and - * 'state + 4'. - * - * Because we know the expected values of each - * received bit are flipped between these two previous - * states: - * - * bitcost(state+4) = 510 - bitcost(state) - * - * With those two total costs in hand, we then pick - * the lower as the cost of the 'state<<1', and compute - * the path of bits leading to that state. - * - * Then, do the same for '(state<<1) + 1'. This time, - * instead of computing the m0 and m1 values from - * scratch, because the only difference is that we're - * expecting a one bit instead of a zero bit, we just - * flip the bitcost values around to match the - * expected transmitted bits with some tricky - * arithmetic which is equivalent to: - * - * m0 = cost[p][state] + (510 - bitcost); - * m1 = cost[p][state+4] + bitcost - * - * Then, the lowest cost and bit trace of the new state - * is saved. - */ - -#define DO_STATE(state) { \ - uint32_t bitcost; \ - \ - uint32_t m0; \ - uint32_t m1; \ - uint32_t bit; \ - \ - bitcost = ((uint32_t) (s0 ^ ao_fec_decode_table[(state<<1)]) + \ - (uint32_t) (s1 ^ ao_fec_decode_table[(state<<1)|1])); \ - \ - m0 = cost[p][state] + bitcost; \ - m1 = cost[p][state+4] + (510 - bitcost); \ - bit = m0 > m1; \ - cost[n][state<<1] = bit ? m1 : m0; \ - bits[n][state<<1] = (bits[p][state + (bit<<2)] << 1) | (state&1); \ - \ - m0 -= (bitcost+bitcost-510); \ - m1 += (bitcost+bitcost-510); \ - bit = m0 > m1; \ - cost[n][(state<<1)+1] = bit ? m1 : m0; \ - bits[n][(state<<1)+1] = (bits[p][state + (bit<<2)] << 1) | (state&1); \ - } - - DO_STATE(0); - DO_STATE(1); - DO_STATE(2); - DO_STATE(3); - -#if 0 - printf ("bit %3d symbol %2x %2x:", i/2, s0, s1); - for (state = 0; state < NUM_STATE; state++) { - printf (" %8u(%08x)", cost[n][state], bits[n][state]); - } - printf ("\n"); -#endif - p = n; - - /* A loop is needed to handle the last output byte. It - * won't have any bits of future data to perform full - * error correction, but we might as well give the - * best possible answer anyways. - */ - while ((b - o) >= (8 + NUM_HIST) || (i + 2 >= len && b > o)) { - - /* Compute number of bits to the end of the - * last full byte of data. This is generally - * NUM_HIST, unless we've reached - * the end of the input, in which case - * it will be seven. - */ - int8_t dist = b - (o + 8); /* distance to last ready-for-writing bit */ - uint32_t min_cost; /* lowest cost */ - uint8_t min_state; /* lowest cost state */ - uint8_t byte; - - /* Find the best fit at the current point - * of the decode. - */ - min_cost = cost[p][0]; - min_state = 0; - for (state = 1; state < NUM_STATE; state++) { - if (cost[p][state] < min_cost) { - min_cost = cost[p][state]; - min_state = state; - } - } - - /* The very last byte of data has the very last bit - * of data left in the state value; just smash the - * bits value in place and reset the 'dist' from - * -1 to 0 so that the full byte is read out - */ - if (dist < 0) { - bits[p][min_state] = (bits[p][min_state] << 1) | (min_state & 1); - dist = 0; - } - -#if 0 - printf ("\tbit %3d min_cost %5d old bit %3d old_state %x bits %02x whiten %0x\n", - i/2, min_cost, o + 8, min_state, (bits[p][min_state] >> dist) & 0xff, *whiten); -#endif - byte = (bits[p][min_state] >> dist) ^ *whiten++; - *out++ = byte; - if (out_len > 2) - crc = ao_fec_crc_byte(byte, crc); - - if (!--out_len) { - if ((out[-2] == (uint8_t) (crc >> 8)) && - out[-1] == (uint8_t) crc) - out[-1] = AO_FEC_DECODE_CRC_OK; - else - out[-1] = 0; - out[-2] = 0; - goto done; - } - o += 8; - } - } -done: -#if AO_PROFILE - ao_fec_decode_start = start_tick; - ao_fec_decode_end = ao_profile_tick(); -#endif - return 1; -} diff --git a/src/core/ao_fec_tx.c b/src/core/ao_fec_tx.c deleted file mode 100644 index 4941d745..00000000 --- a/src/core/ao_fec_tx.c +++ /dev/null @@ -1,134 +0,0 @@ -/* - * 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 - -#if AO_FEC_DEBUG -void -ao_fec_dump_bytes(const uint8_t *bytes, uint16_t len, const char *name) -{ - uint16_t i; - - printf ("%s (%d):", name, len); - for (i = 0; i < len; i++) { - if ((i & 7) == 0) - printf ("\n\t%02x:", i); - printf(" %02x", bytes[i]); - } - printf ("\n"); -} -#endif - -uint16_t -ao_fec_crc(const uint8_t *bytes, uint8_t len) -{ - uint16_t crc = AO_FEC_CRC_INIT; - - while (len--) - crc = ao_fec_crc_byte(*bytes++, crc); - return crc; -} - -/* - * len is the length of the data; the crc will be - * the fist two bytes after that - */ - -uint8_t -ao_fec_check_crc(const uint8_t *bytes, uint8_t len) -{ - uint16_t computed_crc = ao_fec_crc(bytes, len); - uint16_t received_crc = (bytes[len] << 8) | (bytes[len+1]); - - return computed_crc == received_crc; -} - -/* - * Compute CRC and trellis-terminator/interleave-pad bytes - */ -static uint8_t -ao_fec_prepare(const uint8_t *in, uint8_t len, uint8_t *extra) -{ - uint16_t crc = ao_fec_crc (in, len); - uint8_t i = 0; - uint8_t num_fec; - - /* Append CRC */ - extra[i++] = crc >> 8; - extra[i++] = crc; - - /* Append FEC -- 1 byte if odd, two bytes if even */ - num_fec = 2 - (i & 1); - while (num_fec--) - extra[i++] = AO_FEC_TRELLIS_TERMINATOR; - return i; -} - -const uint8_t ao_fec_whiten_table[] = { -#include "ao_whiten.h" -}; - -static const uint8_t ao_fec_encode_table[16] = { -/* next 0 1 state */ - 0, 3, /* 000 */ - 1, 2, /* 001 */ - 3, 0, /* 010 */ - 2, 1, /* 011 */ - 3, 0, /* 100 */ - 2, 1, /* 101 */ - 0, 3, /* 110 */ - 1, 2 /* 111 */ -}; - -uint8_t -ao_fec_encode(const uint8_t *in, uint8_t len, uint8_t *out) -{ - uint8_t extra[AO_FEC_PREPARE_EXTRA]; - uint8_t extra_len; - uint32_t encode, interleave; - uint8_t pair, byte, bit; - uint16_t fec = 0; - const uint8_t *whiten = ao_fec_whiten_table; - - extra_len = ao_fec_prepare(in, len, extra); - for (pair = 0; pair < len + extra_len; pair += 2) { - encode = 0; - for (byte = 0; byte < 2; byte++) { - if (pair + byte == len) - in = extra; - fec |= *in++ ^ *whiten++; - for (bit = 0; bit < 8; bit++) { - encode = encode << 2 | ao_fec_encode_table[fec >> 7]; - fec = (fec << 1) & 0x7ff; - } - } - - interleave = 0; - for (bit = 0; bit < 4 * 4; bit++) { - uint8_t byte_shift = (bit & 0x3) << 3; - uint8_t bit_shift = (bit & 0xc) >> 1; - - interleave = (interleave << 2) | ((encode >> (byte_shift + bit_shift)) & 0x3); - } - *out++ = interleave >> 24; - *out++ = interleave >> 16; - *out++ = interleave >> 8; - *out++ = interleave >> 0; - } - return (len + extra_len) * 2; -} diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c deleted file mode 100644 index 24099347..00000000 --- a/src/core/ao_flight.c +++ /dev/null @@ -1,472 +0,0 @@ -/* - * Copyright © 2009 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_FLIGHT_TEST -#include "ao.h" -#include -#endif - -#if HAS_MPU6000 -#include -#endif - -#ifndef HAS_ACCEL -#error Please define HAS_ACCEL -#endif - -#ifndef HAS_GPS -#error Please define HAS_GPS -#endif - -#ifndef HAS_USB -#error Please define HAS_USB -#endif - -#ifndef HAS_TELEMETRY -#define HAS_TELEMETRY HAS_RADIO -#endif - -/* Main flight thread. */ - -__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_SENSOR_ERRORS -/* 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 - */ -static __data uint16_t ao_interval_end; -static __data int16_t ao_interval_min_height; -static __data int16_t ao_interval_max_height; -#if HAS_ACCEL -static __data int16_t ao_coast_avg_accel; -#endif - -__pdata uint8_t ao_flight_force_idle; - -/* We also have a clock, which can be used to sanity check things in - * case of other failures - */ - -#define BOOST_TICKS_MAX AO_SEC_TO_TICKS(15) - -/* Landing is detected by getting constant readings from both pressure and accelerometer - * for a fairly long time (AO_INTERVAL_TICKS) - */ -#define AO_INTERVAL_TICKS AO_SEC_TO_TICKS(10) - -#define abs(a) ((a) < 0 ? -(a) : (a)) - -void -ao_flight(void) -{ - ao_sample_init(); - ao_flight_state = ao_flight_startup; - for (;;) { - - /* - * Process ADC samples, just looping - * until the sensors are calibrated. - */ - if (!ao_sample()) - continue; - - switch (ao_flight_state) { - case ao_flight_startup: - - /* Check to see what mode we should go to. - * - Invalid mode if accel cal appears to be out - * - pad mode if we're upright, - * - idle mode otherwise - */ -#if HAS_ACCEL - if (ao_config.accel_plus_g == 0 || - 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 || - ao_ground_height < -1000 || - ao_ground_height > 7000) - { - /* Detected an accel value outside -1.5g to 1.5g - * (or uncalibrated values), so we go into invalid mode - */ - ao_flight_state = ao_flight_invalid; - -#if HAS_RADIO && PACKET_HAS_SLAVE - /* Turn on packet system in invalid mode on TeleMetrum */ - ao_packet_slave_start(); -#endif - } else -#endif - if (!ao_flight_force_idle -#if HAS_ACCEL - && ao_ground_accel < ao_config.accel_plus_g + ACCEL_NOSE_UP -#endif - ) - { - /* Set pad mode - we can fly! */ - ao_flight_state = ao_flight_pad; -#if HAS_USB && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE - /* Disable the USB controller in flight mode - * to save power - */ - ao_usb_disable(); -#endif - -#if !HAS_ACCEL && PACKET_HAS_SLAVE - /* Disable packet mode in pad state on TeleMini */ - ao_packet_slave_stop(); -#endif - -#if HAS_TELEMETRY - /* Turn on telemetry system */ - ao_rdf_set(1); - ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD); -#endif -#if AO_LED_RED - /* signal successful initialization by turning off the LED */ - ao_led_off(AO_LED_RED); -#endif - } else { - /* Set idle mode */ - ao_flight_state = ao_flight_idle; -#if HAS_SENSOR_ERRORS - if (ao_sensor_errors) - ao_flight_state = ao_flight_invalid; -#endif - -#if HAS_ACCEL && HAS_RADIO && PACKET_HAS_SLAVE - /* Turn on packet system in idle mode on TeleMetrum */ - ao_packet_slave_start(); -#endif - -#if AO_LED_RED - /* signal successful initialization by turning off the LED */ - ao_led_off(AO_LED_RED); -#endif - } - /* wakeup threads due to state change */ - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - - break; - case ao_flight_pad: - - /* pad to boost: - * - * barometer: > 20m vertical motion - * OR - * accelerometer: > 2g AND velocity > 5m/s - * - * The accelerometer should always detect motion before - * the barometer, but we use both to make sure this - * transition is detected. If the device - * doesn't have an accelerometer, then ignore the - * speed and acceleration as they are quite noisy - * on the pad. - */ - if (ao_height > AO_M_TO_HEIGHT(20) -#if HAS_ACCEL - || (ao_accel > AO_MSS_TO_ACCEL(20) && - ao_speed > AO_MS_TO_SPEED(5)) -#endif - ) - { - ao_flight_state = ao_flight_boost; - ao_boost_tick = ao_sample_tick; - - /* start logging data */ - ao_log_start(); - -#if HAS_TELEMETRY - /* Increase telemetry rate */ - ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_FLIGHT); - - /* disable RDF beacon */ - ao_rdf_set(0); -#endif - -#if HAS_GPS - /* Record current GPS position by waking up GPS log tasks */ - 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)); - } - break; - case ao_flight_boost: - - /* boost to fast: - * - * accelerometer: start to fall at > 1/4 G - * OR - * time: boost for more than 15 seconds - * - * Detects motor burn out by the switch from acceleration to - * deceleration, or by waiting until the maximum burn duration - * (15 seconds) has past. - */ - if ((ao_accel < AO_MSS_TO_ACCEL(-2.5) && ao_height > AO_M_TO_HEIGHT(100)) || - (int16_t) (ao_sample_tick - ao_boost_tick) > BOOST_TICKS_MAX) - { -#if HAS_ACCEL - ao_flight_state = ao_flight_fast; - ao_coast_avg_accel = ao_accel; -#else - ao_flight_state = ao_flight_coast; -#endif - ++ao_motor_number; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - break; -#if HAS_ACCEL - case ao_flight_fast: - /* - * This is essentially the same as coast, - * but the barometer is being ignored as - * it may be unreliable. - */ - if (ao_speed < AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) - { - ao_flight_state = ao_flight_coast; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } else - goto check_re_boost; - break; -#endif - case ao_flight_coast: - - /* - * By customer request - allow the user - * to lock out apogee detection for a specified - * number of seconds. - */ - if (ao_config.apogee_lockout) { - if ((ao_sample_tick - ao_boost_tick) < - AO_SEC_TO_TICKS(ao_config.apogee_lockout)) - break; - } - - /* apogee detect: coast to drogue deploy: - * - * speed: < 0 - * - * Also make sure the model altitude is tracking - * the measured altitude reasonably closely; otherwise - * we're probably transsonic. - */ - if (ao_speed < 0 -#if !HAS_ACCEL - && (ao_sample_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg < 100) -#endif - ) - { -#if HAS_IGNITE - /* ignite the drogue charge */ - ao_ignite(ao_igniter_drogue); -#endif - -#if HAS_TELEMETRY - /* slow down the telemetry system */ - ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_RECOVER); - - /* Turn the RDF beacon back on */ - ao_rdf_set(1); -#endif - - /* and enter drogue state */ - ao_flight_state = ao_flight_drogue; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } -#if HAS_ACCEL - else { - check_re_boost: - ao_coast_avg_accel = ao_coast_avg_accel - (ao_coast_avg_accel >> 6) + (ao_accel >> 6); - if (ao_coast_avg_accel > AO_MSS_TO_ACCEL(20)) { - ao_boost_tick = ao_sample_tick; - ao_flight_state = ao_flight_boost; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - } -#endif - - break; - case ao_flight_drogue: - - /* drogue to main deploy: - * - * barometer: reach main deploy altitude - * - * Would like to use the accelerometer for this test, but - * the orientation of the flight computer is unknown after - * drogue deploy, so we ignore it. Could also detect - * high descent rate using the pressure sensor to - * recognize drogue deploy failure and eject the main - * at that point. Perhaps also use the drogue sense lines - * to notice continutity? - */ - if (ao_height <= ao_config.main_deploy) - { -#if HAS_IGNITE - ao_ignite(ao_igniter_main); -#endif - - /* - * Start recording min/max height - * to figure out when the rocket has landed - */ - - /* initialize interval values */ - ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; - - ao_interval_min_height = ao_interval_max_height = ao_avg_height; - - ao_flight_state = ao_flight_main; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - break; - - /* fall through... */ - case ao_flight_main: - - /* main to land: - * - * barometer: altitude stable - */ - - if (ao_avg_height < ao_interval_min_height) - ao_interval_min_height = ao_avg_height; - if (ao_avg_height > ao_interval_max_height) - ao_interval_max_height = ao_avg_height; - - if ((int16_t) (ao_sample_tick - ao_interval_end) >= 0) { - if (ao_interval_max_height - ao_interval_min_height <= AO_M_TO_HEIGHT(4)) - { - ao_flight_state = ao_flight_landed; - - /* turn off the ADC capture */ - ao_timer_set_adc_interval(0); - - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - ao_interval_min_height = ao_interval_max_height = ao_avg_height; - ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; - } - break; -#if HAS_FLIGHT_DEBUG - case ao_flight_test: -#if HAS_GYRO - printf ("angle %4d pitch %7d yaw %7d roll %7d\n", - ao_sample_orient, - ((ao_sample_pitch << 9) - ao_ground_pitch) >> 9, - ((ao_sample_yaw << 9) - ao_ground_yaw) >> 9, - ((ao_sample_roll << 9) - ao_ground_roll) >> 9); -#endif - flush(); - break; -#endif /* HAS_FLIGHT_DEBUG */ - default: - break; - } - } -} - -#if HAS_FLIGHT_DEBUG -static inline int int_part(int16_t i) { return i >> 4; } -static inline int frac_part(int16_t i) { return ((i & 0xf) * 100 + 8) / 16; } - -static void -ao_flight_dump(void) -{ -#if HAS_ACCEL - int16_t accel; - - accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16; -#endif - - printf ("sample:\n"); - printf (" tick %d\n", ao_sample_tick); - printf (" raw pres %d\n", ao_sample_pres); -#if HAS_ACCEL - printf (" raw accel %d\n", ao_sample_accel); -#endif - printf (" ground pres %d\n", ao_ground_pres); - printf (" ground alt %d\n", ao_ground_height); -#if HAS_ACCEL - printf (" raw accel %d\n", ao_sample_accel); - printf (" groundaccel %d\n", ao_ground_accel); - printf (" accel_2g %d\n", ao_accel_2g); -#endif - - printf (" alt %d\n", ao_sample_alt); - printf (" height %d\n", ao_sample_height); -#if HAS_ACCEL - printf (" accel %d.%02d\n", int_part(accel), frac_part(accel)); -#endif - - - printf ("kalman:\n"); - printf (" height %d\n", ao_height); - printf (" speed %d.%02d\n", int_part(ao_speed), frac_part(ao_speed)); - printf (" accel %d.%02d\n", int_part(ao_accel), frac_part(ao_accel)); - printf (" max_height %d\n", ao_max_height); - printf (" avg_height %d\n", ao_avg_height); - printf (" error_h %d\n", ao_error_h); - printf (" error_avg %d\n", ao_error_h_sq_avg); -} - -static void -ao_gyro_test(void) -{ - ao_flight_state = ao_flight_test; - ao_getchar(); - ao_flight_state = ao_flight_idle; -} - -uint8_t ao_orient_test; - -static void -ao_orient_test_select(void) -{ - ao_orient_test = !ao_orient_test; -} - -__code struct ao_cmds ao_flight_cmds[] = { - { ao_flight_dump, "F\0Dump flight status" }, - { ao_gyro_test, "G\0Test gyro code" }, - { ao_orient_test_select,"O\0Test orientation code" }, - { 0, NULL }, -}; -#endif - -static __xdata struct ao_task flight_task; - -void -ao_flight_init(void) -{ - ao_flight_state = ao_flight_startup; -#if HAS_FLIGHT_DEBUG - ao_cmd_register(&ao_flight_cmds[0]); -#endif - ao_add_task(&flight_task, ao_flight, "flight"); -} diff --git a/src/core/ao_flight.h b/src/core/ao_flight.h deleted file mode 100644 index 01d21c11..00000000 --- a/src/core/ao_flight.h +++ /dev/null @@ -1,70 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_FLIGHT_H_ -#define _AO_FLIGHT_H_ - - -/* - * ao_flight.c - */ - -enum ao_flight_state { - ao_flight_startup = 0, - ao_flight_idle = 1, - ao_flight_pad = 2, - ao_flight_boost = 3, - ao_flight_fast = 4, - ao_flight_coast = 5, - ao_flight_drogue = 6, - ao_flight_main = 7, - ao_flight_landed = 8, - ao_flight_invalid = 9, - ao_flight_test = 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 || HAS_MMA655X -#define HAS_SENSOR_ERRORS 1 -#endif - -#if HAS_SENSOR_ERRORS -extern __xdata uint8_t ao_sensor_errors; -#endif - -extern __pdata uint16_t ao_launch_time; -extern __pdata uint8_t ao_flight_force_idle; - -/* Flight thread */ -void -ao_flight(void); - -/* Initialize flight thread */ -void -ao_flight_init(void); - -/* - * ao_flight_nano.c - */ - -void -ao_flight_nano_init(void); - -#endif /* _AO_FLIGHT_H_ */ diff --git a/src/core/ao_flight_nano.c b/src/core/ao_flight_nano.c deleted file mode 100644 index 406d81ad..00000000 --- a/src/core/ao_flight_nano.c +++ /dev/null @@ -1,120 +0,0 @@ -/* - * 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" - -/* Main flight thread. */ - -__pdata enum ao_flight_state ao_flight_state; /* current flight state */ -__pdata uint16_t ao_launch_tick; /* time of launch detect */ - -/* - * track min/max data over a long interval to detect - * resting - */ -__pdata uint16_t ao_interval_end; -__pdata alt_t ao_interval_min_height; -__pdata alt_t ao_interval_max_height; - -__pdata uint8_t ao_flight_force_idle; - -/* Landing is detected by getting constant readings from both pressure and accelerometer - * for a fairly long time (AO_INTERVAL_TICKS) - */ -#define AO_INTERVAL_TICKS AO_SEC_TO_TICKS(5) - -static void -ao_flight_nano(void) -{ - ao_sample_init(); - ao_flight_state = ao_flight_startup; - - for (;;) { - /* - * Process ADC samples, just looping - * until the sensors are calibrated. - */ - if (!ao_sample()) - continue; - - switch (ao_flight_state) { - case ao_flight_startup: - if (ao_flight_force_idle) { - /* Set idle mode */ - ao_flight_state = ao_flight_idle; - } else { - ao_flight_state = ao_flight_pad; - /* Disable packet mode in pad state */ - ao_packet_slave_stop(); - - /* Turn on telemetry system */ - ao_rdf_set(1); - ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD); - } - /* signal successful initialization by turning off the LED */ - ao_led_off(AO_LED_RED); - - /* wakeup threads due to state change */ - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - break; - case ao_flight_pad: - if (ao_height> AO_M_TO_HEIGHT(20)) { - ao_flight_state = ao_flight_drogue; - ao_launch_tick = ao_sample_tick; - - /* start logging data */ - ao_log_start(); - - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - break; - case ao_flight_drogue: - /* drogue/main to land: - * - * barometer: altitude stable - */ - - if (ao_height < ao_interval_min_height) - ao_interval_min_height = ao_height; - if (ao_height > ao_interval_max_height) - ao_interval_max_height = ao_height; - - if ((int16_t) (ao_sample_tick - ao_interval_end) >= 0) { - if (ao_interval_max_height - ao_interval_min_height < AO_M_TO_HEIGHT(5)) - { - ao_flight_state = ao_flight_landed; - - /* turn off the ADC capture */ - ao_timer_set_adc_interval(0); - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - ao_interval_min_height = ao_interval_max_height = ao_height; - ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; - } - break; - } - } -} - -static __xdata struct ao_task flight_task; - -void -ao_flight_nano_init(void) -{ - ao_flight_state = ao_flight_startup; - ao_add_task(&flight_task, ao_flight_nano, "flight"); -} diff --git a/src/core/ao_freq.c b/src/core/ao_freq.c deleted file mode 100644 index 12496f6f..00000000 --- a/src/core/ao_freq.c +++ /dev/null @@ -1,55 +0,0 @@ -/* - * 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 - -/* - * The provided 'calibration' value is - * that needed to tune the radio to precisely 434550kHz. - * Use that to 'walk' to the target frequency by following - * a 'bresenham' line from 434550kHz to the target - * frequency, and updating the radio setting along the way - */ - -int32_t ao_freq_to_set(int32_t freq, int32_t cal) __reentrant -{ - static __pdata int32_t set; - static __pdata uint8_t neg; - static __pdata int32_t error; - - set = 0; - neg = 0; - error = -434550 / 2; - - if ((freq -= 434550) < 0) { - neg = 1; - freq = -freq; - } - for (;;) { - if (error > 0) { - error -= 434550; - set++; - } else { - error += cal; - if (--freq < 0) - break; - } - } - if (neg) - set = -set; - return cal + set; -} diff --git a/src/core/ao_gps_print.c b/src/core/ao_gps_print.c deleted file mode 100644 index 47c945d7..00000000 --- a/src/core/ao_gps_print.c +++ /dev/null @@ -1,112 +0,0 @@ -/* - * Copyright © 2009 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_GPS_TEST -#include "ao.h" -#endif -#include "ao_telem.h" - -void -ao_gps_print(__xdata struct ao_gps_orig *gps_data) __reentrant -{ - char state; - - if (gps_data->flags & AO_GPS_VALID) - state = AO_TELEM_GPS_STATE_LOCKED; - else if (gps_data->flags & AO_GPS_RUNNING) - state = AO_TELEM_GPS_STATE_UNLOCKED; - else - state = AO_TELEM_GPS_STATE_ERROR; - printf(AO_TELEM_GPS_STATE " %c " - AO_TELEM_GPS_NUM_SAT " %d ", - state, - (gps_data->flags & AO_GPS_NUM_SAT_MASK) >> AO_GPS_NUM_SAT_SHIFT); - if (!(gps_data->flags & AO_GPS_VALID)) - return; - printf(AO_TELEM_GPS_LATITUDE " %ld " - AO_TELEM_GPS_LONGITUDE " %ld " - AO_TELEM_GPS_ALTITUDE " %d ", - (long) gps_data->latitude, - (long) gps_data->longitude, - gps_data->altitude); - - if (gps_data->flags & AO_GPS_DATE_VALID) - printf(AO_TELEM_GPS_YEAR " %d " - AO_TELEM_GPS_MONTH " %d " - AO_TELEM_GPS_DAY " %d ", - gps_data->year, - gps_data->month, - gps_data->day); - - printf(AO_TELEM_GPS_HOUR " %d " - AO_TELEM_GPS_MINUTE " %d " - AO_TELEM_GPS_SECOND " %d ", - gps_data->hour, - gps_data->minute, - gps_data->second); - - printf(AO_TELEM_GPS_HDOP " %d ", - gps_data->hdop * 2); - - if (gps_data->flags & AO_GPS_COURSE_VALID) { - printf(AO_TELEM_GPS_HERROR " %d " - AO_TELEM_GPS_VERROR " %d " - AO_TELEM_GPS_VERTICAL_SPEED " %d " - AO_TELEM_GPS_HORIZONTAL_SPEED " %d " - AO_TELEM_GPS_COURSE " %d ", - gps_data->h_error, - gps_data->v_error, - gps_data->climb_rate, - gps_data->ground_speed, - (int) gps_data->course * 2); - } -} - -void -ao_gps_tracking_print(__xdata struct ao_gps_tracking_orig *gps_tracking_data) __reentrant -{ - uint8_t c, n, v; - __xdata struct ao_gps_sat_orig *sat; - - n = gps_tracking_data->channels; - if (n == 0) - return; - - sat = gps_tracking_data->sats; - v = 0; - for (c = 0; c < n; c++) { - if (sat->svid) - v++; - sat++; - } - - printf (AO_TELEM_SAT_NUM " %d ", - v); - - sat = gps_tracking_data->sats; - v = 0; - for (c = 0; c < n; c++) { - if (sat->svid) { - printf (AO_TELEM_SAT_SVID "%d %d " - AO_TELEM_SAT_C_N_0 "%d %d ", - v, sat->svid, - v, sat->c_n_1); - v++; - } - sat++; - } -} diff --git a/src/core/ao_gps_report.c b/src/core/ao_gps_report.c deleted file mode 100644 index 07201ac2..00000000 --- a/src/core/ao_gps_report.c +++ /dev/null @@ -1,89 +0,0 @@ -/* - * Copyright © 2009 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" - -void -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 (;;) { - while ((new = ao_gps_new) == 0) - ao_sleep(&ao_gps_new); - ao_mutex_get(&ao_gps_mutex); - 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 ((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); - } - } - if (new & AO_GPS_NEW_TRACKING) { - uint8_t c, n; - - 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; - -void -ao_gps_report_init(void) -{ - ao_add_task(&ao_gps_report_task, ao_gps_report, "gps_report"); -} diff --git a/src/core/ao_gps_report_mega.c b/src/core/ao_gps_report_mega.c deleted file mode 100644 index 5e3c71bf..00000000 --- a/src/core/ao_gps_report_mega.c +++ /dev/null @@ -1,90 +0,0 @@ -/* - * Copyright © 2009 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_log.h" - -void -ao_gps_report_mega(void) -{ - static __xdata struct ao_log_mega gps_log; - static __xdata struct ao_telemetry_location gps_data; - static __xdata struct ao_telemetry_satellite gps_tracking_data; - uint8_t new; - uint8_t c, n, i; - - for (;;) { - while (!(new = ao_gps_new)) - ao_sleep(&ao_gps_new); - ao_mutex_get(&ao_gps_mutex); - 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 ((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.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; - gps_log.u.gps.course = gps_data.course; - gps_log.u.gps.ground_speed = gps_data.ground_speed; - gps_log.u.gps.climb_rate = gps_data.climb_rate; - gps_log.u.gps.pdop = gps_data.pdop; - gps_log.u.gps.hdop = gps_data.hdop; - gps_log.u.gps.vdop = gps_data.vdop; - gps_log.u.gps.mode = gps_data.mode; - 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++; - if (i >= 12) - break; - } - gps_log.u.gps_sat.channels = i; - ao_log_mega(&gps_log); - } - } -} - -__xdata struct ao_task ao_gps_report_mega_task; - -void -ao_gps_report_mega_init(void) -{ - ao_add_task(&ao_gps_report_mega_task, - ao_gps_report_mega, - "gps_report"); -} diff --git a/src/core/ao_gps_report_metrum.c b/src/core/ao_gps_report_metrum.c deleted file mode 100644 index 696a833b..00000000 --- a/src/core/ao_gps_report_metrum.c +++ /dev/null @@ -1,96 +0,0 @@ -/* - * Copyright © 2009 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_log.h" - -void -ao_gps_report_metrum(void) -{ - static __xdata struct ao_log_metrum gps_log; - static __xdata struct ao_telemetry_location gps_data; - static __xdata struct ao_telemetry_satellite gps_tracking_data; - uint8_t c, n, i; - uint8_t svid; - uint8_t new; - - for (;;) { - while (!(new = ao_gps_new)) - ao_sleep(&ao_gps_new); - ao_mutex_get(&ao_gps_mutex); - 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 ((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_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++; - } - } - 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; - -void -ao_gps_report_metrum_init(void) -{ - ao_add_task(&ao_gps_report_metrum_task, - ao_gps_report_metrum, - "gps_report"); -} diff --git a/src/core/ao_gps_show.c b/src/core/ao_gps_show.c deleted file mode 100644 index 3a05e35a..00000000 --- a/src/core/ao_gps_show.c +++ /dev/null @@ -1,39 +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. - */ - -#ifndef AO_GPS_TEST -#include -#endif - -void -ao_gps_show(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); -} diff --git a/src/core/ao_host.h b/src/core/ao_host.h deleted file mode 100644 index 6eb752c9..00000000 --- a/src/core/ao_host.h +++ /dev/null @@ -1,135 +0,0 @@ -/* - * Copyright © 2009 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 - -#define AO_ADC_RING 64 -#define ao_adc_ring_next(n) (((n) + 1) & (AO_ADC_RING - 1)) -#define ao_adc_ring_prev(n) (((n) - 1) & (AO_ADC_RING - 1)) - -/* - * One set of samples read from the A/D converter - */ -struct ao_adc { - uint16_t tick; /* tick when the sample was read */ - 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 */ -}; - -#define __pdata -#define __data -#define __xdata -#define __code -#define __reentrant - -#define DATA_TO_XDATA(a) (a) -#define PDATA_TO_XDATA(a) (a) -#define CODE_TO_XDATA(a) (a) - -enum ao_flight_state { - ao_flight_startup = 0, - ao_flight_idle = 1, - ao_flight_pad = 2, - ao_flight_boost = 3, - ao_flight_fast = 4, - ao_flight_coast = 5, - ao_flight_drogue = 6, - ao_flight_main = 7, - ao_flight_landed = 8, - ao_flight_invalid = 9 -}; - -struct ao_adc ao_adc_ring[AO_ADC_RING]; -uint8_t ao_adc_head; - -#define ao_led_on(l) -#define ao_led_off(l) -#define ao_timer_set_adc_interval(i) -#define ao_wakeup(wchan) ao_dump_state(wchan) -#define ao_cmd_register(c) -#define ao_usb_disable() -#define ao_telemetry_set_interval(x) -#define ao_delay(x) - -enum ao_igniter { - ao_igniter_drogue = 0, - ao_igniter_main = 1 -}; - -void -ao_ignite(enum ao_igniter igniter) -{ - printf ("ignite %s\n", igniter == ao_igniter_drogue ? "drogue" : "main"); -} - -struct ao_task { - int dummy; -}; - -#define ao_add_task(t,f,n) - -#define ao_log_start() -#define ao_log_stop() - -#define AO_MS_TO_TICKS(ms) ((ms) / 10) -#define AO_SEC_TO_TICKS(s) ((s) * 100) - -#define AO_FLIGHT_TEST - -struct ao_adc ao_adc_static; - -FILE *emulator_in; - -void -ao_dump_state(void *wchan); - -void -ao_sleep(void *wchan); - -const char const * const ao_state_names[] = { - "startup", "idle", "pad", "boost", "fast", - "coast", "drogue", "main", "landed", "invalid" -}; - -struct ao_cmds { - void (*func)(void); - const char *help; -}; - - -struct ao_config { - uint16_t main_deploy; - int16_t accel_zero_g; -}; - -#define ao_config_get() - -struct ao_config ao_config = { 250, 16000 }; - -#define ao_xmemcpy(d,s,c) memcpy(d,s,c) -#define ao_xmemset(d,v,c) memset(d,v,c) -#define ao_xmemcmp(d,s,c) memcmp(d,s,c) diff --git a/src/core/ao_ignite.c b/src/core/ao_ignite.c deleted file mode 100644 index 823d003c..00000000 --- a/src/core/ao_ignite.c +++ /dev/null @@ -1,241 +0,0 @@ -/* - * Copyright © 2009 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 -#if AO_PYRO_NUM -#include -#endif - -#if HAS_IGNITE -__xdata struct ao_ignition ao_ignition[2]; - -void -ao_ignite(enum ao_igniter igniter) -{ - ao_arch_block_interrupts(); - ao_ignition[igniter].request = 1; - ao_wakeup(&ao_ignition); - ao_arch_release_interrupts(); -} - -#ifndef AO_SENSE_DROGUE -#define AO_SENSE_DROGUE(p) ((p)->adc.sense_d) -#define AO_SENSE_MAIN(p) ((p)->adc.sense_m) -#endif - -enum ao_igniter_status -ao_igniter_status(enum ao_igniter igniter) -{ - __xdata struct ao_data packet; - __pdata int16_t value; - __pdata uint8_t request, firing, fired; - - ao_arch_critical( - ao_data_get(&packet); - request = ao_ignition[igniter].request; - fired = ao_ignition[igniter].fired; - firing = ao_ignition[igniter].firing; - ); - if (firing || (request && !fired)) - return ao_igniter_active; - - value = (AO_IGNITER_CLOSED>>1); - switch (igniter) { - case ao_igniter_drogue: - value = AO_SENSE_DROGUE(&packet); - break; - case ao_igniter_main: - value = AO_SENSE_MAIN(&packet); - break; - } - if (value < AO_IGNITER_OPEN) - return ao_igniter_open; - else if (value > AO_IGNITER_CLOSED) - return ao_igniter_ready; - else - return ao_igniter_unknown; -} - -#ifndef AO_IGNITER_SET_DROGUE -#define AO_IGNITER_SET_DROGUE(v) AO_IGNITER_DROGUE = (v) -#define AO_IGNITER_SET_MAIN(v) AO_IGNITER_MAIN = (v) -#endif - -#ifndef AO_IGNITER_FIRE_TIME -#define AO_IGNITER_FIRE_TIME AO_MS_TO_TICKS(50) -#endif - -#ifndef AO_IGNITER_CHARGE_TIME -#define AO_IGNITER_CHARGE_TIME AO_MS_TO_TICKS(2000) -#endif - -void -ao_igniter_fire(enum ao_igniter igniter) -{ - ao_ignition[igniter].firing = 1; - switch(ao_config.ignite_mode) { - case AO_IGNITE_MODE_DUAL: - switch (igniter) { - case ao_igniter_drogue: - AO_IGNITER_SET_DROGUE(1); - ao_delay(AO_IGNITER_FIRE_TIME); - AO_IGNITER_SET_DROGUE(0); - break; - case ao_igniter_main: - AO_IGNITER_SET_MAIN(1); - ao_delay(AO_IGNITER_FIRE_TIME); - AO_IGNITER_SET_MAIN(0); - break; - } - break; - case AO_IGNITE_MODE_APOGEE: - switch (igniter) { - case ao_igniter_drogue: - AO_IGNITER_SET_DROGUE(1); - ao_delay(AO_IGNITER_FIRE_TIME); - AO_IGNITER_SET_DROGUE(0); - ao_delay(AO_IGNITER_CHARGE_TIME); - AO_IGNITER_SET_MAIN(1); - ao_delay(AO_IGNITER_FIRE_TIME); - AO_IGNITER_SET_MAIN(0); - break; - default: - break; - } - break; - case AO_IGNITE_MODE_MAIN: - switch (igniter) { - case ao_igniter_main: - AO_IGNITER_SET_DROGUE(1); - ao_delay(AO_IGNITER_FIRE_TIME); - AO_IGNITER_SET_DROGUE(0); - ao_delay(AO_IGNITER_CHARGE_TIME); - AO_IGNITER_SET_MAIN(1); - ao_delay(AO_IGNITER_FIRE_TIME); - AO_IGNITER_SET_MAIN(0); - break; - default: - break; - } - break; - } - ao_ignition[igniter].firing = 0; -} - -void -ao_igniter(void) -{ - __xdata enum ao_igniter igniter; - - ao_config_get(); - for (;;) { - ao_sleep(&ao_ignition); - for (igniter = ao_igniter_drogue; igniter <= ao_igniter_main; igniter++) { - if (ao_ignition[igniter].request && !ao_ignition[igniter].fired) { - if (igniter == ao_igniter_drogue && ao_config.apogee_delay) - ao_delay(AO_SEC_TO_TICKS(ao_config.apogee_delay)); - - ao_igniter_fire(igniter); - ao_delay(AO_IGNITER_CHARGE_TIME); - ao_ignition[igniter].fired = 1; - } - } - } -} - -#endif - -void -ao_ignite_manual(void) -{ - ao_cmd_white(); - if (!ao_match_word("DoIt")) - return; - ao_cmd_white(); -#if HAS_IGNITE - if (ao_cmd_lex_c == 'm' && ao_match_word("main")) { - ao_igniter_fire(ao_igniter_main); - return; - } - if (ao_cmd_lex_c == 'd' && ao_match_word("drogue")) { - ao_igniter_fire(ao_igniter_drogue); - return; - } -#endif -#if AO_PYRO_NUM - if ('0' <= ao_cmd_lex_c && ao_cmd_lex_c <= '9') { - ao_pyro_manual(ao_cmd_lex_c - '0'); - return; - } -#endif - ao_cmd_status = ao_cmd_syntax_error; -} - -__code char * __code ao_igniter_status_names[] = { - "unknown", "ready", "active", "open" -}; - -#if HAS_IGNITE -void -ao_ignite_print_status(enum ao_igniter igniter, __code char *name) __reentrant -{ - enum ao_igniter_status status = ao_igniter_status(igniter); - printf("Igniter: %6s Status: %s\n", - name, - ao_igniter_status_names[status]); -} -#endif - -void -ao_ignite_test(void) -{ -#if HAS_IGNITE - ao_ignite_print_status(ao_igniter_drogue, "drogue"); - ao_ignite_print_status(ao_igniter_main, "main"); -#endif -#if AO_PYRO_NUM - ao_pyro_print_status(); -#endif -} - -__code struct ao_cmds ao_ignite_cmds[] = { - { ao_ignite_manual, "i {main|drogue}\0Fire igniter. is doit with D&I" }, - { ao_ignite_test, "t\0Test igniter" }, - { 0, NULL }, -}; - -#if HAS_IGNITE -__xdata struct ao_task ao_igniter_task; - -void -ao_ignite_set_pins(void) -{ - ao_enable_output(AO_IGNITER_DROGUE_PORT, AO_IGNITER_DROGUE_PIN, AO_IGNITER_DROGUE, 0); - ao_enable_output(AO_IGNITER_MAIN_PORT, AO_IGNITER_MAIN_PIN, AO_IGNITER_MAIN, 0); -} -#endif - -void -ao_igniter_init(void) -{ -#if HAS_IGNITE - ao_ignite_set_pins(); - ao_add_task(&ao_igniter_task, ao_igniter, "igniter"); -#endif - ao_cmd_register(&ao_ignite_cmds[0]); -} diff --git a/src/core/ao_int64.c b/src/core/ao_int64.c deleted file mode 100644 index aa23dbe0..00000000 --- a/src/core/ao_int64.c +++ /dev/null @@ -1,158 +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 - -__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; - if (t < a->low) - r->high++; - r->low = 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; - if (t > a->low) - r->high--; - r->low = t; -} - -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) - r->low |= a->high << (32 - d); - r->high = (int32_t) a->high >> d; - } else { - d &= 0x1f; - r->low = (int32_t) a->high >> d; - r->high = 0; - } -} - -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) - r->high |= a->low >> (32 - d); - r->low = a->low << d; - } else { - d &= 0x1f; - r->high = a->low << d; - r->low = 0; - } -} - -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(__pdata ao_int64_t *r, __pdata ao_int64_t *a) __FATTR { - r->high = ~a->high; - if (!(r->low = ~a->low + 1)) - r->high++; -} - -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; - } - if (b < 0) { - b = -b; - --negative; - } - ao_umul64_32_32(r, a, b); - if (negative) - ao_neg64(r, r); -} - -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); - ao_umul64_32_32(r, a->low, b->low); - - r->high += r2.low + r3.low; -} - -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; - - if (ao_int64_negativep(a)) { - ao_neg64(&ap, a); - a = ≈ - ++negative; - } - if (ao_int64_negativep(b)) { - ao_neg64(&bp, b); - b = &bp; - --negative; - } - ao_umul64(r, a, b); - if (negative) - ao_neg64(r, r); -} - -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(__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++; - } else - ao_umul64_64_16(r, a, b); - if (negative) - ao_neg64(r, r); -} diff --git a/src/core/ao_int64.h b/src/core/ao_int64.h deleted file mode 100644 index b16db58c..00000000 --- a/src/core/ao_int64.h +++ /dev/null @@ -1,48 +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. - */ - -#ifndef _AO_INT64_H_ -#define _AO_INT64_H_ - -#include - -typedef struct { - uint32_t high; - uint32_t low; -} ao_int64_t; - -#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)) - -#define ao_cast64(a) (((int64_t) (a)->high << 32) | (a)->low) - -#define ao_int64_negativep(a) (((int32_t) (a)->high) < 0) - -#endif /* _AO_INT64_H_ */ diff --git a/src/core/ao_kalman.c b/src/core/ao_kalman.c deleted file mode 100644 index 9aea1f14..00000000 --- a/src/core/ao_kalman.c +++ /dev/null @@ -1,295 +0,0 @@ -/* - * 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. - */ - -#ifndef AO_FLIGHT_TEST -#include "ao.h" -#include "ao_flight.h" -#endif - -#include "ao_sample.h" -#include "ao_kalman.h" - - -static __pdata int32_t ao_k_height; -static __pdata int32_t ao_k_speed; -static __pdata int32_t ao_k_accel; - -#define AO_K_STEP_100 to_fix16(0.01) -#define AO_K_STEP_2_2_100 to_fix16(0.00005) - -#define AO_K_STEP_10 to_fix16(0.1) -#define AO_K_STEP_2_2_10 to_fix16(0.005) - -#define AO_K_STEP_1 to_fix16(1) -#define AO_K_STEP_2_2_1 to_fix16(0.5) - -__pdata int16_t ao_height; -__pdata int16_t ao_speed; -__pdata int16_t ao_accel; -__xdata int16_t ao_max_height; -static __pdata int32_t ao_avg_height_scaled; -__xdata int16_t ao_avg_height; - -__pdata int16_t ao_error_h; -__pdata int16_t ao_error_h_sq_avg; - -#if HAS_ACCEL -__pdata int16_t ao_error_a; -#endif - -static void -ao_kalman_predict(void) -{ -#ifdef AO_FLIGHT_TEST - if (ao_sample_tick - ao_sample_prev_tick > 50) { - ao_k_height += ((int32_t) ao_speed * AO_K_STEP_1 + - (int32_t) ao_accel * AO_K_STEP_2_2_1) >> 4; - ao_k_speed += (int32_t) ao_accel * AO_K_STEP_1; - - return; - } - if (ao_sample_tick - ao_sample_prev_tick > 5) { - ao_k_height += ((int32_t) ao_speed * AO_K_STEP_10 + - (int32_t) ao_accel * AO_K_STEP_2_2_10) >> 4; - ao_k_speed += (int32_t) ao_accel * AO_K_STEP_10; - - return; - } - if (ao_flight_debug) { - printf ("predict speed %g + (%g * %g) = %g\n", - ao_k_speed / (65536.0 * 16.0), ao_accel / 16.0, AO_K_STEP_100 / 65536.0, - (ao_k_speed + (int32_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0)); - } -#endif - ao_k_height += ((int32_t) ao_speed * AO_K_STEP_100 + - (int32_t) ao_accel * AO_K_STEP_2_2_100) >> 4; - ao_k_speed += (int32_t) ao_accel * AO_K_STEP_100; -} - -static void -ao_kalman_err_height(void) -{ - int16_t e; - int16_t height_distrust; -#if HAS_ACCEL - int16_t speed_distrust; -#endif - - ao_error_h = ao_sample_height - (int16_t) (ao_k_height >> 16); - - e = ao_error_h; - if (e < 0) - e = -e; - if (e > 127) - e = 127; -#if HAS_ACCEL - ao_error_h_sq_avg -= ao_error_h_sq_avg >> 2; - ao_error_h_sq_avg += (e * e) >> 2; -#else - ao_error_h_sq_avg -= ao_error_h_sq_avg >> 4; - ao_error_h_sq_avg += (e * e) >> 4; -#endif - - if (ao_flight_state >= ao_flight_drogue) - return; - height_distrust = ao_sample_alt - AO_MAX_BARO_HEIGHT; -#if HAS_ACCEL - /* speed is stored * 16, but we need to ramp between 200 and 328, so - * we want to multiply by 2. The result is a shift by 3. - */ - speed_distrust = (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) >> (4 - 1); - if (speed_distrust <= 0) - speed_distrust = 0; - else if (speed_distrust > height_distrust) - height_distrust = speed_distrust; -#endif - if (height_distrust > 0) { -#ifdef AO_FLIGHT_TEST - int old_ao_error_h = ao_error_h; -#endif - if (height_distrust > 0x100) - height_distrust = 0x100; - ao_error_h = (int16_t) (((int32_t) ao_error_h * (0x100 - height_distrust)) >> 8); -#ifdef AO_FLIGHT_TEST - if (ao_flight_debug) { - printf("over height %g over speed %g distrust: %g height: error %d -> %d\n", - (double) (ao_sample_alt - AO_MAX_BARO_HEIGHT), - (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) / 16.0, - height_distrust / 256.0, - old_ao_error_h, ao_error_h); - } -#endif - } -} - -static void -ao_kalman_correct_baro(void) -{ - ao_kalman_err_height(); -#ifdef AO_FLIGHT_TEST - if (ao_sample_tick - ao_sample_prev_tick > 50) { - ao_k_height += (int32_t) AO_BARO_K0_1 * ao_error_h; - ao_k_speed += (int32_t) AO_BARO_K1_1 * ao_error_h; - ao_k_accel += (int32_t) AO_BARO_K2_1 * ao_error_h; - return; - } - if (ao_sample_tick - ao_sample_prev_tick > 5) { - ao_k_height += (int32_t) AO_BARO_K0_10 * ao_error_h; - ao_k_speed += (int32_t) AO_BARO_K1_10 * ao_error_h; - ao_k_accel += (int32_t) AO_BARO_K2_10 * ao_error_h; - return; - } -#endif - ao_k_height += (int32_t) AO_BARO_K0_100 * ao_error_h; - ao_k_speed += (int32_t) AO_BARO_K1_100 * ao_error_h; - ao_k_accel += (int32_t) AO_BARO_K2_100 * ao_error_h; -} - -#if HAS_ACCEL - -static void -ao_kalman_err_accel(void) -{ - int32_t accel; - - accel = (ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale; - - /* Can't use ao_accel here as it is the pre-prediction value still */ - ao_error_a = (accel - ao_k_accel) >> 16; -} - -#ifndef FORCE_ACCEL -static void -ao_kalman_correct_both(void) -{ - ao_kalman_err_height(); - ao_kalman_err_accel(); - -#ifdef AO_FLIGHT_TEST - if (ao_sample_tick - ao_sample_prev_tick > 50) { - if (ao_flight_debug) { - printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n", - ao_k_speed / (65536.0 * 16.0), - (double) ao_error_h, AO_BOTH_K10_1 / 65536.0, - (double) ao_error_a, AO_BOTH_K11_1 / 65536.0, - (ao_k_speed + - (int32_t) AO_BOTH_K10_1 * ao_error_h + - (int32_t) AO_BOTH_K11_1 * ao_error_a) / (65536.0 * 16.0)); - } - ao_k_height += - (int32_t) AO_BOTH_K00_1 * ao_error_h + - (int32_t) AO_BOTH_K01_1 * ao_error_a; - ao_k_speed += - (int32_t) AO_BOTH_K10_1 * ao_error_h + - (int32_t) AO_BOTH_K11_1 * ao_error_a; - ao_k_accel += - (int32_t) AO_BOTH_K20_1 * ao_error_h + - (int32_t) AO_BOTH_K21_1 * ao_error_a; - return; - } - if (ao_sample_tick - ao_sample_prev_tick > 5) { - if (ao_flight_debug) { - printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n", - ao_k_speed / (65536.0 * 16.0), - (double) ao_error_h, AO_BOTH_K10_10 / 65536.0, - (double) ao_error_a, AO_BOTH_K11_10 / 65536.0, - (ao_k_speed + - (int32_t) AO_BOTH_K10_10 * ao_error_h + - (int32_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0)); - } - ao_k_height += - (int32_t) AO_BOTH_K00_10 * ao_error_h + - (int32_t) AO_BOTH_K01_10 * ao_error_a; - ao_k_speed += - (int32_t) AO_BOTH_K10_10 * ao_error_h + - (int32_t) AO_BOTH_K11_10 * ao_error_a; - ao_k_accel += - (int32_t) AO_BOTH_K20_10 * ao_error_h + - (int32_t) AO_BOTH_K21_10 * ao_error_a; - return; - } - if (ao_flight_debug) { - printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n", - ao_k_speed / (65536.0 * 16.0), - (double) ao_error_h, AO_BOTH_K10_100 / 65536.0, - (double) ao_error_a, AO_BOTH_K11_100 / 65536.0, - (ao_k_speed + - (int32_t) AO_BOTH_K10_100 * ao_error_h + - (int32_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0)); - } -#endif - ao_k_height += - (int32_t) AO_BOTH_K00_100 * ao_error_h + - (int32_t) AO_BOTH_K01_100 * ao_error_a; - ao_k_speed += - (int32_t) AO_BOTH_K10_100 * ao_error_h + - (int32_t) AO_BOTH_K11_100 * ao_error_a; - ao_k_accel += - (int32_t) AO_BOTH_K20_100 * ao_error_h + - (int32_t) AO_BOTH_K21_100 * ao_error_a; -} - -#else - -static void -ao_kalman_correct_accel(void) -{ - ao_kalman_err_accel(); - - if (ao_sample_tick - ao_sample_prev_tick > 5) { - ao_k_height +=(int32_t) AO_ACCEL_K0_10 * ao_error_a; - ao_k_speed += (int32_t) AO_ACCEL_K1_10 * ao_error_a; - ao_k_accel += (int32_t) AO_ACCEL_K2_10 * ao_error_a; - return; - } - ao_k_height += (int32_t) AO_ACCEL_K0_100 * ao_error_a; - ao_k_speed += (int32_t) AO_ACCEL_K1_100 * ao_error_a; - ao_k_accel += (int32_t) AO_ACCEL_K2_100 * ao_error_a; -} - -#endif /* else FORCE_ACCEL */ -#endif /* HAS_ACCEL */ - -void -ao_kalman(void) -{ - ao_kalman_predict(); -#if HAS_ACCEL - if (ao_flight_state <= ao_flight_coast) { -#ifdef FORCE_ACCEL - ao_kalman_correct_accel(); -#else - ao_kalman_correct_both(); -#endif - } else -#endif - ao_kalman_correct_baro(); - ao_height = from_fix(ao_k_height); - ao_speed = from_fix(ao_k_speed); - ao_accel = from_fix(ao_k_accel); - if (ao_height > ao_max_height) - ao_max_height = ao_height; - ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_sample_height; -#ifdef AO_FLIGHT_TEST - if (ao_sample_tick - ao_sample_prev_tick > 50) - ao_avg_height = (ao_avg_height_scaled + 1) >> 1; - else if (ao_sample_tick - ao_sample_prev_tick > 5) - ao_avg_height = (ao_avg_height_scaled + 7) >> 4; - else -#endif - ao_avg_height = (ao_avg_height_scaled + 63) >> 7; -} diff --git a/src/core/ao_lcd.h b/src/core/ao_lcd.h deleted file mode 100644 index f7e1391a..00000000 --- a/src/core/ao_lcd.h +++ /dev/null @@ -1,60 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_LCD_H_ -#define _AO_LCD_H_ - -/* ao_lcd.c */ - -void -ao_lcd_putchar(uint8_t d); - -void -ao_lcd_putstring(char *string); - -void -ao_lcd_contrast_set(uint8_t contrast); - -void -ao_lcd_clear(void); - -void -ao_lcd_cursor_on(void); - -void -ao_lcd_cursor_off(void); - -#define AO_LCD_ADDR(row,col) ((row << 6) | (col)) - -void -ao_lcd_goto(uint8_t addr); - -void -ao_lcd_start(void); - -void -ao_lcd_init(void); - -/* ao_lcd_port.c */ - -void -ao_lcd_port_put_nibble(uint8_t rs, uint8_t d); - -void -ao_lcd_port_init(void); - -#endif /* _AO_LCD_H_ */ diff --git a/src/core/ao_led.h b/src/core/ao_led.h deleted file mode 100644 index d9a0914a..00000000 --- a/src/core/ao_led.h +++ /dev/null @@ -1,59 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_LED_H_ -#define _AO_LED_H_ - -/* - * ao_led.c - */ - -#define AO_LED_NONE 0 - -#ifndef AO_LED_TYPE -#define AO_LED_TYPE uint8_t -#endif - -/* Turn on the specified LEDs */ -void -ao_led_on(AO_LED_TYPE colors); - -/* Turn off the specified LEDs */ -void -ao_led_off(AO_LED_TYPE colors); - -/* Set all of the LEDs to the specified state */ -void -ao_led_set(AO_LED_TYPE colors); - -/* Set all LEDs in 'mask' to the specified state */ -void -ao_led_set_mask(uint8_t colors, uint8_t mask); - -/* Toggle the specified LEDs */ -void -ao_led_toggle(AO_LED_TYPE colors); - -/* Turn on the specified LEDs for the indicated interval */ -void -ao_led_for(AO_LED_TYPE colors, uint16_t ticks) __reentrant; - -/* Initialize the LEDs */ -void -ao_led_init(AO_LED_TYPE enable); - -#endif /* _AO_LED_H_ */ diff --git a/src/core/ao_list.h b/src/core/ao_list.h deleted file mode 100644 index 8a6fa4d9..00000000 --- a/src/core/ao_list.h +++ /dev/null @@ -1,213 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_LIST_H_ -#define _AO_LIST_H_ - -#include - -struct ao_list { - struct ao_list *next, *prev; -}; - -static inline void -ao_list_init(struct ao_list *list) -{ - list->next = list->prev = list; -} - -static inline void -__ao_list_add(struct ao_list *list, struct ao_list *prev, struct ao_list *next) -{ - next->prev = list; - list->next = next; - list->prev = prev; - prev->next = list; -} - -/** - * Insert a new element after the given list head. The new element does not - * need to be initialised as empty list. - * The list changes from: - * head → some element → ... - * to - * head → new element → older element → ... - * - * Example: - * struct foo *newfoo = malloc(...); - * ao_list_add(&newfoo->entry, &bar->list_of_foos); - * - * @param entry The new element to prepend to the list. - * @param head The existing list. - */ -static inline void -ao_list_insert(struct ao_list *entry, struct ao_list *head) -{ - __ao_list_add(entry, head, head->next); -} - -/** - * Append a new element to the end of the list given with this list head. - * - * The list changes from: - * head → some element → ... → lastelement - * to - * head → some element → ... → lastelement → new element - * - * Example: - * struct foo *newfoo = malloc(...); - * ao_list_append(&newfoo->entry, &bar->list_of_foos); - * - * @param entry The new element to prepend to the list. - * @param head The existing list. - */ -static inline void -ao_list_append(struct ao_list *entry, struct ao_list *head) -{ - __ao_list_add(entry, head->prev, head); -} - -static inline void -__ao_list_del(struct ao_list *prev, struct ao_list *next) -{ - next->prev = prev; - prev->next = next; -} - -/** - * Remove the element from the list it is in. Using this function will reset - * the pointers to/from this element so it is removed from the list. It does - * NOT free the element itself or manipulate it otherwise. - * - * Using ao_list_del on a pure list head (like in the example at the top of - * this file) will NOT remove the first element from - * the list but rather reset the list as empty list. - * - * Example: - * ao_list_del(&foo->entry); - * - * @param entry The element to remove. - */ -static inline void -ao_list_del(struct ao_list *entry) -{ - __ao_list_del(entry->prev, entry->next); - ao_list_init(entry); -} - -/** - * Check if the list is empty. - * - * Example: - * ao_list_is_empty(&bar->list_of_foos); - * - * @return True if the list contains one or more elements or False otherwise. - */ -static inline uint8_t -ao_list_is_empty(struct ao_list *head) -{ - return head->next == head; -} - -/** - * Returns a pointer to the container of this list element. - * - * Example: - * struct foo* f; - * f = container_of(&foo->entry, struct foo, entry); - * assert(f == foo); - * - * @param ptr Pointer to the struct ao_list. - * @param type Data type of the list element. - * @param member Member name of the struct ao_list field in the list element. - * @return A pointer to the data struct containing the list head. - */ -#define ao_container_of(ptr, type, member) \ - ((type *)((char *)(ptr) - offsetof(type, member))) - -/** - * Alias of ao_container_of - */ -#define ao_list_entry(ptr, type, member) \ - ao_container_of(ptr, type, member) - -/** - * Retrieve the first list entry for the given list pointer. - * - * Example: - * struct foo *first; - * first = ao_list_first_entry(&bar->list_of_foos, struct foo, list_of_foos); - * - * @param ptr The list head - * @param type Data type of the list element to retrieve - * @param member Member name of the struct ao_list field in the list element. - * @return A pointer to the first list element. - */ -#define ao_list_first_entry(ptr, type, member) \ - ao_list_entry((ptr)->next, type, member) - -/** - * Retrieve the last list entry for the given listpointer. - * - * Example: - * struct foo *first; - * first = ao_list_last_entry(&bar->list_of_foos, struct foo, list_of_foos); - * - * @param ptr The list head - * @param type Data type of the list element to retrieve - * @param member Member name of the struct ao_list field in the list element. - * @return A pointer to the last list element. - */ -#define ao_list_last_entry(ptr, type, member) \ - ao_list_entry((ptr)->prev, type, member) - -/** - * Loop through the list given by head and set pos to struct in the list. - * - * Example: - * struct foo *iterator; - * ao_list_for_each_entry(iterator, &bar->list_of_foos, entry) { - * [modify iterator] - * } - * - * This macro is not safe for node deletion. Use ao_list_for_each_entry_safe - * instead. - * - * @param pos Iterator variable of the type of the list elements. - * @param head List head - * @param member Member name of the struct ao_list in the list elements. - * - */ -#define ao_list_for_each_entry(pos, head, type, member) \ - for (pos = ao_container_of((head)->next, type, member); \ - &pos->member != (head); \ - pos = ao_container_of(pos->member.next, type, member)) - -/** - * Loop through the list, keeping a backup pointer to the element. This - * macro allows for the deletion of a list element while looping through the - * list. - * - * See ao_list_for_each_entry for more details. - */ -#define ao_list_for_each_entry_safe(pos, tmp, head, type, member) \ - for ((pos = ao_container_of((head)->next, type, member)), \ - (tmp = ao_container_of(pos->member.next, type, member)); \ - &pos->member != (head); \ - (pos = tmp), (tmp = ao_container_of(pos->member.next, type, member))) - -#endif /* _AO_LIST_H_ */ diff --git a/src/core/ao_log.c b/src/core/ao_log.c deleted file mode 100644 index 20febefe..00000000 --- a/src/core/ao_log.c +++ /dev/null @@ -1,291 +0,0 @@ -/* - * Copyright © 2009 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 -#include - -__pdata uint32_t ao_log_current_pos; -__pdata uint32_t ao_log_end_pos; -__pdata uint32_t ao_log_start_pos; -__xdata uint8_t ao_log_running; -__pdata enum ao_flight_state ao_log_state; -__xdata uint16_t ao_flight_number; - -void -ao_log_flush(void) -{ - ao_storage_flush(); -} - -/* - * When erasing a flight log, make sure the config block - * has an up-to-date version of the current flight number - */ - -struct ao_log_erase { - uint8_t unused; - uint16_t flight; -}; - -static __xdata struct ao_log_erase erase; - -#define LOG_MAX_ERASE 16 - -static uint32_t -ao_log_erase_pos(uint8_t i) -{ - return i * sizeof (struct ao_log_erase) + AO_CONFIG_MAX_SIZE; -} - -void -ao_log_write_erase(uint8_t pos) -{ - erase.unused = 0x00; - erase.flight = ao_flight_number; - ao_config_write(ao_log_erase_pos(pos), &erase, sizeof (erase)); - ao_config_flush(); -} - -static void -ao_log_read_erase(uint8_t pos) -{ - ao_config_read(ao_log_erase_pos(pos), &erase, sizeof (erase)); -} - - -static void -ao_log_erase_mark(void) -{ - uint8_t i; - - for (i = 0; i < LOG_MAX_ERASE; i++) { - ao_log_read_erase(i); - if (erase.unused == 0 && erase.flight == ao_flight_number) - return; - if (erase.unused == 0xff) { - ao_log_write_erase(i); - return; - } - } - ao_config_put(); -} - -static uint8_t -ao_log_slots() -{ - return (uint8_t) (ao_storage_log_max / ao_config.flight_log_max); -} - -uint32_t -ao_log_pos(uint8_t slot) -{ - return ((slot) * ao_config.flight_log_max); -} - -static uint16_t -ao_log_max_flight(void) -{ - uint8_t log_slot; - uint8_t log_slots; - uint16_t log_flight; - uint16_t max_flight = 0; - - /* Scan the log space looking for the biggest flight number */ - log_slots = ao_log_slots(); - for (log_slot = 0; log_slot < log_slots; log_slot++) { - log_flight = ao_log_flight(log_slot); - if (!log_flight) - continue; - if (max_flight == 0 || (int16_t) (log_flight - max_flight) > 0) - max_flight = log_flight; - } - return max_flight; -} - -void -ao_log_scan(void) __reentrant -{ - uint8_t log_slot; - uint8_t log_slots; - uint8_t log_want; - - ao_config_get(); - - ao_flight_number = ao_log_max_flight(); - if (ao_flight_number) - if (++ao_flight_number == 0) - ao_flight_number = 1; - - /* Now look through the log of flight numbers from erase operations and - * see if the last one is bigger than what we found above - */ - for (log_slot = LOG_MAX_ERASE; log_slot-- > 0;) { - ao_log_read_erase(log_slot); - if (erase.unused == 0) { - if (ao_flight_number == 0 || - (int16_t) (erase.flight - ao_flight_number) > 0) - ao_flight_number = erase.flight; - break; - } - } - if (ao_flight_number == 0) - ao_flight_number = 1; - - /* With a flight number in hand, find a place to write a new log, - * use the target flight number to index the available log slots so - * that we write logs to each spot about the same number of times. - */ - - /* Find a log slot for the next flight, if available */ - ao_log_current_pos = ao_log_end_pos = 0; - log_slots = ao_log_slots(); - log_want = (ao_flight_number - 1) % log_slots; - log_slot = log_want; - do { - if (ao_log_flight(log_slot) == 0) { - ao_log_current_pos = ao_log_pos(log_slot); - ao_log_end_pos = ao_log_current_pos + ao_config.flight_log_max; - break; - } - if (++log_slot >= log_slots) - log_slot = 0; - } while (log_slot != log_want); - - ao_wakeup(&ao_flight_number); -} - -void -ao_log_start(void) -{ - /* start logging */ - ao_log_running = 1; - ao_wakeup(&ao_log_running); -} - -void -ao_log_stop(void) -{ - ao_log_running = 0; - ao_log_flush(); -} - -uint8_t -ao_log_present(void) -{ - return ao_log_max_flight() != 0; -} - -uint8_t -ao_log_full(void) -{ - return ao_log_current_pos == ao_log_end_pos; -} - -#if HAS_ADC -static __xdata struct ao_task ao_log_task; -#endif - -void -ao_log_list(void) __reentrant -{ - uint8_t slot; - uint8_t slots; - uint16_t flight; - - slots = ao_log_slots(); - for (slot = 0; slot < slots; slot++) - { - flight = ao_log_flight(slot); - if (flight) - printf ("flight %d start %x end %x\n", - flight, - (uint16_t) (ao_log_pos(slot) >> 8), - (uint16_t) (ao_log_pos(slot+1) >> 8)); - } - printf ("done\n"); -} - -void -ao_log_delete(void) __reentrant -{ - uint8_t slot; - uint8_t slots; - - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - - slots = ao_log_slots(); - /* Look for the flight log matching the requested flight */ - if (ao_cmd_lex_i) { - for (slot = 0; slot < slots; slot++) { - if (ao_log_flight(slot) == ao_cmd_lex_i) { - ao_log_erase_mark(); - ao_log_current_pos = ao_log_pos(slot); - ao_log_end_pos = ao_log_current_pos + ao_config.flight_log_max; - while (ao_log_current_pos < ao_log_end_pos) { - uint8_t i; - static __xdata uint8_t b; - - /* - * Check to see if we've reached the end of - * the used memory to avoid re-erasing the same - * memory over and over again - */ - for (i = 0; i < 16; i++) { - if (ao_storage_read(ao_log_current_pos + i, &b, 1)) - if (b != 0xff) - break; - } - if (i == 16) - break; - ao_storage_erase(ao_log_current_pos); - ao_log_current_pos += ao_storage_block; - } - puts("Erased"); - return; - } - } - } - printf("No such flight: %d\n", ao_cmd_lex_i); -} - -__code struct ao_cmds ao_log_cmds[] = { - { ao_log_list, "l\0List logs" }, - { ao_log_delete, "d \0Delete flight" }, - { 0, NULL }, -}; - -void -ao_log_init(void) -{ - ao_log_running = 0; - - /* For now, just log the flight starting at the begining of eeprom */ - ao_log_state = ao_flight_invalid; - - ao_cmd_register(&ao_log_cmds[0]); - -#ifndef HAS_ADC -#error Define HAS_ADC for ao_log.c -#endif -#if HAS_ADC - /* Create a task to log events to eeprom */ - ao_add_task(&ao_log_task, ao_log, "log"); -#endif -} diff --git a/src/core/ao_log.h b/src/core/ao_log.h deleted file mode 100644 index 09f31188..00000000 --- a/src/core/ao_log.h +++ /dev/null @@ -1,386 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_LOG_H_ -#define _AO_LOG_H_ - -#include - -/* - * ao_log.c - */ - -/* We record flight numbers in the first record of - * the log. Tasks may wait for this to be initialized - * by sleeping on this variable. - */ -extern __xdata uint16_t ao_flight_number; - -extern __pdata uint32_t ao_log_current_pos; -extern __pdata uint32_t ao_log_end_pos; -extern __pdata uint32_t ao_log_start_pos; -extern __xdata uint8_t ao_log_running; -extern __pdata enum ao_flight_state ao_log_state; - -/* required functions from the underlying log system */ - -#define AO_LOG_FORMAT_UNKNOWN 0 /* unknown; altosui will have to guess */ -#define AO_LOG_FORMAT_FULL 1 /* 8 byte typed log records */ -#define AO_LOG_FORMAT_TINY 2 /* two byte state/baro records */ -#define AO_LOG_FORMAT_TELEMETRY 3 /* 32 byte ao_telemetry records */ -#define AO_LOG_FORMAT_TELESCIENCE 4 /* 32 byte typed telescience records */ -#define AO_LOG_FORMAT_TELEMEGA 5 /* 32 byte typed telemega records */ -#define AO_LOG_FORMAT_EASYMINI 6 /* 16-byte MS5607 baro only, 3.0V supply */ -#define AO_LOG_FORMAT_TELEMETRUM 7 /* 16-byte typed telemetrum records */ -#define AO_LOG_FORMAT_TELEMINI 8 /* 16-byte MS5607 baro only, 3.3V supply */ -#define AO_LOG_FORMAT_NONE 127 /* No log at all */ - -extern __code uint8_t ao_log_format; - -/* Return the flight number from the given log slot, 0 if none */ -uint16_t -ao_log_flight(uint8_t slot); - -/* Flush the log */ -void -ao_log_flush(void); - -/* Logging thread main routine */ -void -ao_log(void); - -/* functions provided in ao_log.c */ - -/* Figure out the current flight number */ -void -ao_log_scan(void) __reentrant; - -/* Return the position of the start of the given log slot */ -uint32_t -ao_log_pos(uint8_t slot); - -/* Start logging to eeprom */ -void -ao_log_start(void); - -/* Stop logging */ -void -ao_log_stop(void); - -/* Initialize the logging system */ -void -ao_log_init(void); - -/* Write out the current flight number to the erase log */ -void -ao_log_write_erase(uint8_t pos); - -/* Returns true if there are any logs stored in eeprom */ -uint8_t -ao_log_present(void); - -/* Returns true if there is no more storage space available */ -uint8_t -ao_log_full(void); - -/* - * ao_log_big.c - */ - -/* - * The data log is recorded in the eeprom as a sequence - * of data packets. - * - * Each packet starts with a 4-byte header that has the - * packet type, the packet checksum and the tick count. Then - * they all contain 2 16 bit values which hold packet-specific - * data. - * - * For each flight, the first packet - * is FLIGHT packet, indicating the serial number of the - * device and a unique number marking the number of flights - * recorded by this device. - * - * During flight, data from the accelerometer and barometer - * are recorded in SENSOR packets, using the raw 16-bit values - * read from the A/D converter. - * - * Also during flight, but at a lower rate, the deployment - * sensors are recorded in DEPLOY packets. The goal here is to - * detect failure in the deployment circuits. - * - * STATE packets hold state transitions as the flight computer - * transitions through different stages of the flight. - */ -#define AO_LOG_FLIGHT 'F' -#define AO_LOG_SENSOR 'A' -#define AO_LOG_TEMP_VOLT 'T' -#define AO_LOG_DEPLOY 'D' -#define AO_LOG_STATE 'S' -#define AO_LOG_GPS_TIME 'G' -#define AO_LOG_GPS_LAT 'N' -#define AO_LOG_GPS_LON 'W' -#define AO_LOG_GPS_ALT 'H' -#define AO_LOG_GPS_SAT 'V' -#define AO_LOG_GPS_DATE 'Y' -#define AO_LOG_GPS_POS 'P' - -#define AO_LOG_POS_NONE (~0UL) - -struct ao_log_record { - char type; /* 0 */ - uint8_t csum; /* 1 */ - uint16_t tick; /* 2 */ - union { - struct { - int16_t ground_accel; /* 4 */ - uint16_t flight; /* 6 */ - } flight; - struct { - int16_t accel; /* 4 */ - int16_t pres; /* 6 */ - } sensor; - struct { - int16_t temp; - int16_t v_batt; - } temp_volt; - struct { - int16_t drogue; - int16_t main; - } deploy; - struct { - uint16_t state; - uint16_t reason; - } state; - struct { - uint8_t hour; - uint8_t minute; - uint8_t second; - uint8_t flags; - } gps_time; - int32_t gps_latitude; - int32_t gps_longitude; - struct { - int16_t altitude; - uint16_t unused; - } gps_altitude; - struct { - uint16_t svid; - uint8_t unused; - uint8_t c_n; - } gps_sat; - struct { - uint8_t year; - uint8_t month; - uint8_t day; - uint8_t extra; - } gps_date; - struct { - uint16_t d0; - uint16_t d1; - } anon; - } u; -}; - -struct ao_log_mega { - char type; /* 0 */ - uint8_t csum; /* 1 */ - uint16_t tick; /* 2 */ - union { /* 4 */ - /* AO_LOG_FLIGHT */ - struct { - uint16_t flight; /* 4 */ - int16_t ground_accel; /* 6 */ - uint32_t ground_pres; /* 8 */ - int16_t ground_accel_along; /* 16 */ - int16_t ground_accel_across; /* 12 */ - int16_t ground_accel_through; /* 14 */ - int16_t ground_roll; /* 18 */ - int16_t ground_pitch; /* 20 */ - int16_t ground_yaw; /* 22 */ - } flight; /* 24 */ - /* AO_LOG_STATE */ - struct { - uint16_t state; - uint16_t reason; - } state; - /* AO_LOG_SENSOR */ - struct { - uint32_t pres; /* 4 */ - uint32_t temp; /* 8 */ - int16_t accel_x; /* 12 */ - int16_t accel_y; /* 14 */ - int16_t accel_z; /* 16 */ - int16_t gyro_x; /* 18 */ - int16_t gyro_y; /* 20 */ - int16_t gyro_z; /* 22 */ - int16_t mag_x; /* 24 */ - int16_t mag_y; /* 26 */ - int16_t mag_z; /* 28 */ - int16_t accel; /* 30 */ - } sensor; /* 32 */ - /* AO_LOG_TEMP_VOLT */ - struct { - int16_t v_batt; /* 4 */ - int16_t v_pbatt; /* 6 */ - int16_t n_sense; /* 8 */ - int16_t sense[10]; /* 10 */ - uint16_t pyro; /* 30 */ - } volt; /* 32 */ - /* AO_LOG_GPS_TIME */ - struct { - int32_t latitude; /* 4 */ - int32_t longitude; /* 8 */ - int16_t altitude; /* 12 */ - uint8_t hour; /* 14 */ - uint8_t minute; /* 15 */ - uint8_t second; /* 16 */ - uint8_t flags; /* 17 */ - uint8_t year; /* 18 */ - uint8_t month; /* 19 */ - uint8_t day; /* 20 */ - uint8_t course; /* 21 */ - uint16_t ground_speed; /* 22 */ - int16_t climb_rate; /* 24 */ - uint8_t pdop; /* 26 */ - uint8_t hdop; /* 27 */ - uint8_t vdop; /* 28 */ - uint8_t mode; /* 29 */ - } gps; /* 30 */ - /* AO_LOG_GPS_SAT */ - struct { - uint16_t channels; /* 4 */ - struct { - uint8_t svid; - uint8_t c_n; - } sats[12]; /* 6 */ - } gps_sat; /* 30 */ - } u; -}; - -struct ao_log_metrum { - char type; /* 0 */ - uint8_t csum; /* 1 */ - uint16_t tick; /* 2 */ - union { /* 4 */ - /* AO_LOG_FLIGHT */ - struct { - uint16_t flight; /* 4 */ - int16_t ground_accel; /* 6 */ - uint32_t ground_pres; /* 8 */ - uint32_t ground_temp; /* 12 */ - } flight; /* 16 */ - /* AO_LOG_STATE */ - struct { - uint16_t state; /* 4 */ - uint16_t reason; /* 6 */ - } state; /* 8 */ - /* AO_LOG_SENSOR */ - struct { - uint32_t pres; /* 4 */ - uint32_t temp; /* 8 */ - int16_t accel; /* 12 */ - } sensor; /* 14 */ - /* AO_LOG_TEMP_VOLT */ - struct { - int16_t v_batt; /* 4 */ - int16_t sense_a; /* 6 */ - int16_t sense_m; /* 8 */ - } volt; /* 10 */ - /* AO_LOG_GPS_POS */ - struct { - int32_t latitude; /* 4 */ - int32_t longitude; /* 8 */ - int16_t altitude; /* 12 */ - } gps; /* 14 */ - /* AO_LOG_GPS_TIME */ - struct { - uint8_t hour; /* 4 */ - uint8_t minute; /* 5 */ - uint8_t second; /* 6 */ - uint8_t flags; /* 7 */ - uint8_t year; /* 8 */ - uint8_t month; /* 9 */ - uint8_t day; /* 10 */ - uint8_t pad; /* 11 */ - } gps_time; /* 12 */ - /* AO_LOG_GPS_SAT (up to three packets) */ - struct { - uint8_t channels; /* 4 */ - uint8_t more; /* 5 */ - struct { - uint8_t svid; - uint8_t c_n; - } sats[4]; /* 6 */ - } gps_sat; /* 14 */ - uint8_t raw[12]; /* 4 */ - } u; /* 16 */ -}; - -struct ao_log_mini { - char type; /* 0 */ - uint8_t csum; /* 1 */ - uint16_t tick; /* 2 */ - union { /* 4 */ - /* AO_LOG_FLIGHT */ - struct { - uint16_t flight; /* 4 */ - uint16_t r6; - uint32_t ground_pres; /* 8 */ - } flight; - /* AO_LOG_STATE */ - struct { - uint16_t state; /* 4 */ - uint16_t reason; /* 6 */ - } state; - /* AO_LOG_SENSOR */ - struct { - uint8_t pres[3]; /* 4 */ - uint8_t temp[3]; /* 7 */ - int16_t sense_a; /* 10 */ - int16_t sense_m; /* 12 */ - int16_t v_batt; /* 14 */ - } sensor; /* 16 */ - } u; /* 16 */ -}; /* 16 */ - -#define ao_log_pack24(dst,value) do { \ - (dst)[0] = (value); \ - (dst)[1] = (value) >> 8; \ - (dst)[2] = (value) >> 16; \ - } while (0) - -/* Write a record to the eeprom log */ -uint8_t -ao_log_data(__xdata struct ao_log_record *log) __reentrant; - -uint8_t -ao_log_mega(__xdata struct ao_log_mega *log) __reentrant; - -uint8_t -ao_log_metrum(__xdata struct ao_log_metrum *log) __reentrant; - -uint8_t -ao_log_mini(__xdata struct ao_log_mini *log) __reentrant; - -void -ao_log_flush(void); - -void -ao_gps_report_metrum_init(void); - -#endif /* _AO_LOG_H_ */ diff --git a/src/core/ao_log_big.c b/src/core/ao_log_big.c deleted file mode 100644 index db01f46c..00000000 --- a/src/core/ao_log_big.c +++ /dev/null @@ -1,162 +0,0 @@ -/* - * 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" - -static __xdata uint8_t ao_log_mutex; -static __xdata struct ao_log_record log; - -__code uint8_t ao_log_format = AO_LOG_FORMAT_FULL; - -static uint8_t -ao_log_csum(__xdata uint8_t *b) __reentrant -{ - uint8_t sum = 0x5a; - uint8_t i; - - for (i = 0; i < sizeof (struct ao_log_record); i++) - sum += *b++; - return -sum; -} - -uint8_t -ao_log_data(__xdata struct ao_log_record *log) __reentrant -{ - uint8_t wrote = 0; - /* set checksum */ - log->csum = 0; - log->csum = ao_log_csum((__xdata uint8_t *) log); - ao_mutex_get(&ao_log_mutex); { - if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) - ao_log_stop(); - if (ao_log_running) { - wrote = 1; - ao_storage_write(ao_log_current_pos, - log, - sizeof (struct ao_log_record)); - ao_log_current_pos += sizeof (struct ao_log_record); - } - } ao_mutex_put(&ao_log_mutex); - return wrote; -} - -static uint8_t -ao_log_dump_check_data(void) -{ - if (ao_log_csum((uint8_t *) &log) != 0) - return 0; - return 1; -} - -static __data uint8_t ao_log_data_pos; - -/* a hack to make sure that ao_log_records fill the eeprom block in even units */ -typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_record))] ; - -#ifndef AO_SENSOR_INTERVAL_ASCENT -#define AO_SENSOR_INTERVAL_ASCENT 1 -#define AO_SENSOR_INTERVAL_DESCENT 10 -#define AO_OTHER_INTERVAL 32 -#endif - -void -ao_log(void) -{ - __pdata uint16_t next_sensor, next_other; - - ao_storage_setup(); - - ao_log_scan(); - - while (!ao_log_running) - ao_sleep(&ao_log_running); - - log.type = AO_LOG_FLIGHT; - log.tick = ao_sample_tick; -#if HAS_ACCEL - log.u.flight.ground_accel = ao_ground_accel; -#endif - log.u.flight.flight = ao_flight_number; - ao_log_data(&log); - - /* Write the whole contents of the ring to the log - * when starting up. - */ - ao_log_data_pos = ao_data_ring_next(ao_sample_data); - next_other = next_sensor = ao_data_ring[ao_log_data_pos].tick; - ao_log_state = ao_flight_startup; - for (;;) { - /* Write samples to EEPROM */ - while (ao_log_data_pos != ao_sample_data) { - log.tick = ao_data_ring[ao_log_data_pos].tick; - if ((int16_t) (log.tick - next_sensor) >= 0) { - log.type = AO_LOG_SENSOR; - log.u.sensor.accel = ao_data_ring[ao_log_data_pos].adc.accel; - log.u.sensor.pres = ao_data_ring[ao_log_data_pos].adc.pres; - ao_log_data(&log); - if (ao_log_state <= ao_flight_coast) - next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT; - else - next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT; - } - if ((int16_t) (log.tick - next_other) >= 0) { - log.type = AO_LOG_TEMP_VOLT; - log.u.temp_volt.temp = ao_data_ring[ao_log_data_pos].adc.temp; - log.u.temp_volt.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; - ao_log_data(&log); - log.type = AO_LOG_DEPLOY; - log.u.deploy.drogue = ao_data_ring[ao_log_data_pos].adc.sense_d; - log.u.deploy.main = ao_data_ring[ao_log_data_pos].adc.sense_m; - ao_log_data(&log); - next_other = log.tick + AO_OTHER_INTERVAL; - } - ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); - } - /* Write state change to EEPROM */ - if (ao_flight_state != ao_log_state) { - ao_log_state = ao_flight_state; - log.type = AO_LOG_STATE; - log.tick = ao_sample_tick; - log.u.state.state = ao_log_state; - log.u.state.reason = 0; - ao_log_data(&log); - - if (ao_log_state == ao_flight_landed) - ao_log_stop(); - } - - /* Wait for a while */ - ao_delay(AO_MS_TO_TICKS(100)); - - /* Stop logging when told to */ - while (!ao_log_running) - ao_sleep(&ao_log_running); - } -} - -uint16_t -ao_log_flight(uint8_t slot) -{ - if (!ao_storage_read(ao_log_pos(slot), - &log, - sizeof (struct ao_log_record))) - return 0; - - if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) - return log.u.flight.flight; - return 0; -} diff --git a/src/core/ao_log_mega.c b/src/core/ao_log_mega.c deleted file mode 100644 index 768947d5..00000000 --- a/src/core/ao_log_mega.c +++ /dev/null @@ -1,199 +0,0 @@ -/* - * 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 "ao.h" -#include -#include -#include - -static __xdata uint8_t ao_log_mutex; -static __xdata struct ao_log_mega log; - -__code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMEGA; - -static uint8_t -ao_log_csum(__xdata uint8_t *b) __reentrant -{ - uint8_t sum = 0x5a; - uint8_t i; - - for (i = 0; i < sizeof (struct ao_log_mega); i++) - sum += *b++; - return -sum; -} - -uint8_t -ao_log_mega(__xdata struct ao_log_mega *log) __reentrant -{ - uint8_t wrote = 0; - /* set checksum */ - log->csum = 0; - log->csum = ao_log_csum((__xdata uint8_t *) log); - ao_mutex_get(&ao_log_mutex); { - if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) - ao_log_stop(); - if (ao_log_running) { - wrote = 1; - ao_storage_write(ao_log_current_pos, - log, - sizeof (struct ao_log_mega)); - ao_log_current_pos += sizeof (struct ao_log_mega); - } - } ao_mutex_put(&ao_log_mutex); - return wrote; -} - -static uint8_t -ao_log_dump_check_data(void) -{ - if (ao_log_csum((uint8_t *) &log) != 0) - return 0; - return 1; -} - -#if HAS_ADC -static __data uint8_t ao_log_data_pos; - -/* a hack to make sure that ao_log_megas fill the eeprom block in even units */ -typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_mega))] ; - -#ifndef AO_SENSOR_INTERVAL_ASCENT -#define AO_SENSOR_INTERVAL_ASCENT 1 -#define AO_SENSOR_INTERVAL_DESCENT 10 -#define AO_OTHER_INTERVAL 32 -#endif - -void -ao_log(void) -{ - __pdata uint16_t next_sensor, next_other; - uint8_t i; - - ao_storage_setup(); - - ao_log_scan(); - - while (!ao_log_running) - ao_sleep(&ao_log_running); - -#if HAS_FLIGHT - log.type = AO_LOG_FLIGHT; - log.tick = ao_sample_tick; -#if HAS_ACCEL - log.u.flight.ground_accel = ao_ground_accel; -#endif -#if HAS_GYRO - log.u.flight.ground_accel_along = ao_ground_accel_along; - log.u.flight.ground_accel_across = ao_ground_accel_across; - log.u.flight.ground_accel_through = ao_ground_accel_through; - log.u.flight.ground_pitch = ao_ground_pitch; - log.u.flight.ground_yaw = ao_ground_yaw; - log.u.flight.ground_roll = ao_ground_roll; -#endif - log.u.flight.ground_pres = ao_ground_pres; - log.u.flight.flight = ao_flight_number; - ao_log_mega(&log); -#endif - - /* Write the whole contents of the ring to the log - * when starting up. - */ - ao_log_data_pos = ao_data_ring_next(ao_data_head); - next_other = next_sensor = ao_data_ring[ao_log_data_pos].tick; - ao_log_state = ao_flight_startup; - for (;;) { - /* Write samples to EEPROM */ - while (ao_log_data_pos != ao_data_head) { - log.tick = ao_data_ring[ao_log_data_pos].tick; - if ((int16_t) (log.tick - next_sensor) >= 0) { - log.type = AO_LOG_SENSOR; -#if HAS_MS5607 - log.u.sensor.pres = ao_data_ring[ao_log_data_pos].ms5607_raw.pres; - log.u.sensor.temp = ao_data_ring[ao_log_data_pos].ms5607_raw.temp; -#endif -#if HAS_MPU6000 - log.u.sensor.accel_x = ao_data_ring[ao_log_data_pos].mpu6000.accel_x; - log.u.sensor.accel_y = ao_data_ring[ao_log_data_pos].mpu6000.accel_y; - log.u.sensor.accel_z = ao_data_ring[ao_log_data_pos].mpu6000.accel_z; - log.u.sensor.gyro_x = ao_data_ring[ao_log_data_pos].mpu6000.gyro_x; - log.u.sensor.gyro_y = ao_data_ring[ao_log_data_pos].mpu6000.gyro_y; - log.u.sensor.gyro_z = ao_data_ring[ao_log_data_pos].mpu6000.gyro_z; -#endif -#if HAS_HMC5883 - log.u.sensor.mag_x = ao_data_ring[ao_log_data_pos].hmc5883.x; - log.u.sensor.mag_y = ao_data_ring[ao_log_data_pos].hmc5883.y; - log.u.sensor.mag_z = ao_data_ring[ao_log_data_pos].hmc5883.z; -#endif - log.u.sensor.accel = ao_data_accel(&ao_data_ring[ao_log_data_pos]); - ao_log_mega(&log); - if (ao_log_state <= ao_flight_coast) - next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT; - else - next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT; - } - if ((int16_t) (log.tick - next_other) >= 0) { - log.type = AO_LOG_TEMP_VOLT; - log.u.volt.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; - log.u.volt.v_pbatt = ao_data_ring[ao_log_data_pos].adc.v_pbatt; - log.u.volt.n_sense = AO_ADC_NUM_SENSE; - for (i = 0; i < AO_ADC_NUM_SENSE; i++) - log.u.volt.sense[i] = ao_data_ring[ao_log_data_pos].adc.sense[i]; - log.u.volt.pyro = ao_pyro_fired; - ao_log_mega(&log); - next_other = log.tick + AO_OTHER_INTERVAL; - } - ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); - } -#if HAS_FLIGHT - /* Write state change to EEPROM */ - if (ao_flight_state != ao_log_state) { - ao_log_state = ao_flight_state; - log.type = AO_LOG_STATE; - log.tick = ao_time(); - log.u.state.state = ao_log_state; - log.u.state.reason = 0; - ao_log_mega(&log); - - if (ao_log_state == ao_flight_landed) - ao_log_stop(); - } -#endif - - ao_log_flush(); - - /* Wait for a while */ - ao_delay(AO_MS_TO_TICKS(100)); - - /* Stop logging when told to */ - while (!ao_log_running) - ao_sleep(&ao_log_running); - } -} -#endif - -uint16_t -ao_log_flight(uint8_t slot) -{ - if (!ao_storage_read(ao_log_pos(slot), - &log, - sizeof (struct ao_log_mega))) - return 0; - - if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) - return log.u.flight.flight; - return 0; -} diff --git a/src/core/ao_log_metrum.c b/src/core/ao_log_metrum.c deleted file mode 100644 index 9b17adc2..00000000 --- a/src/core/ao_log_metrum.c +++ /dev/null @@ -1,176 +0,0 @@ -/* - * 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 "ao.h" -#include -#include -#include - -static __xdata uint8_t ao_log_mutex; -static __xdata struct ao_log_metrum log; - -__code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMETRUM; - -static uint8_t -ao_log_csum(__xdata uint8_t *b) __reentrant -{ - uint8_t sum = 0x5a; - uint8_t i; - - for (i = 0; i < sizeof (struct ao_log_metrum); i++) - sum += *b++; - return -sum; -} - -uint8_t -ao_log_metrum(__xdata struct ao_log_metrum *log) __reentrant -{ - uint8_t wrote = 0; - /* set checksum */ - log->csum = 0; - log->csum = ao_log_csum((__xdata uint8_t *) log); - ao_mutex_get(&ao_log_mutex); { - if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) - ao_log_stop(); - if (ao_log_running) { - wrote = 1; - ao_storage_write(ao_log_current_pos, - log, - sizeof (struct ao_log_metrum)); - ao_log_current_pos += sizeof (struct ao_log_metrum); - } - } ao_mutex_put(&ao_log_mutex); - return wrote; -} - -static uint8_t -ao_log_dump_check_data(void) -{ - if (ao_log_csum((uint8_t *) &log) != 0) - return 0; - return 1; -} - -#if HAS_ADC -static __data uint8_t ao_log_data_pos; - -/* a hack to make sure that ao_log_metrums fill the eeprom block in even units */ -typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_metrum))] ; - -#ifndef AO_SENSOR_INTERVAL_ASCENT -#define AO_SENSOR_INTERVAL_ASCENT 1 -#define AO_SENSOR_INTERVAL_DESCENT 10 -#define AO_OTHER_INTERVAL 32 -#endif - -void -ao_log(void) -{ - __pdata uint16_t next_sensor, next_other; - - ao_storage_setup(); - - ao_log_scan(); - - while (!ao_log_running) - ao_sleep(&ao_log_running); - -#if HAS_FLIGHT - log.type = AO_LOG_FLIGHT; - log.tick = ao_sample_tick; -#if HAS_ACCEL - log.u.flight.ground_accel = ao_ground_accel; -#endif - log.u.flight.ground_pres = ao_ground_pres; - log.u.flight.flight = ao_flight_number; - ao_log_metrum(&log); -#endif - - /* Write the whole contents of the ring to the log - * when starting up. - */ - ao_log_data_pos = ao_data_ring_next(ao_data_head); - next_other = next_sensor = ao_data_ring[ao_log_data_pos].tick; - ao_log_state = ao_flight_startup; - for (;;) { - /* Write samples to EEPROM */ - while (ao_log_data_pos != ao_data_head) { - log.tick = ao_data_ring[ao_log_data_pos].tick; - if ((int16_t) (log.tick - next_sensor) >= 0) { - log.type = AO_LOG_SENSOR; -#if HAS_MS5607 - log.u.sensor.pres = ao_data_ring[ao_log_data_pos].ms5607_raw.pres; - log.u.sensor.temp = ao_data_ring[ao_log_data_pos].ms5607_raw.temp; -#endif -#if HAS_ACCEL - log.u.sensor.accel = ao_data_accel(&ao_data_ring[ao_log_data_pos]); -#endif - ao_log_metrum(&log); - if (ao_log_state <= ao_flight_coast) - next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT; - else - next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT; - } - if ((int16_t) (log.tick - next_other) >= 0) { - log.type = AO_LOG_TEMP_VOLT; - log.u.volt.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; - log.u.volt.sense_a = ao_data_ring[ao_log_data_pos].adc.sense_a; - log.u.volt.sense_m = ao_data_ring[ao_log_data_pos].adc.sense_m; - ao_log_metrum(&log); - next_other = log.tick + AO_OTHER_INTERVAL; - } - ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); - } -#if HAS_FLIGHT - /* Write state change to EEPROM */ - if (ao_flight_state != ao_log_state) { - ao_log_state = ao_flight_state; - log.type = AO_LOG_STATE; - log.tick = ao_time(); - log.u.state.state = ao_log_state; - log.u.state.reason = 0; - ao_log_metrum(&log); - - if (ao_log_state == ao_flight_landed) - ao_log_stop(); - } -#endif - - ao_log_flush(); - - /* Wait for a while */ - ao_delay(AO_MS_TO_TICKS(100)); - - /* Stop logging when told to */ - while (!ao_log_running) - ao_sleep(&ao_log_running); - } -} -#endif - -uint16_t -ao_log_flight(uint8_t slot) -{ - if (!ao_storage_read(ao_log_pos(slot), - &log, - sizeof (struct ao_log_metrum))) - return 0; - - if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) - return log.u.flight.flight; - return 0; -} diff --git a/src/core/ao_log_micro.c b/src/core/ao_log_micro.c deleted file mode 100644 index d665efb5..00000000 --- a/src/core/ao_log_micro.c +++ /dev/null @@ -1,121 +0,0 @@ -/* - * 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 - -static uint16_t ao_log_offset = STARTING_LOG_OFFSET; - -void -ao_log_micro_save(void) -{ - uint16_t n_samples = (ao_log_offset - STARTING_LOG_OFFSET) / sizeof (uint16_t); - ao_eeprom_write(PA_GROUND_OFFSET, &pa_ground, sizeof (pa_ground)); - ao_eeprom_write(PA_MIN_OFFSET, &pa_min, sizeof (pa_min)); - ao_eeprom_write(N_SAMPLES_OFFSET, &n_samples, sizeof (n_samples)); -} - -void -ao_log_micro_restore(void) -{ - ao_eeprom_read(PA_GROUND_OFFSET, &pa_ground, sizeof (pa_ground)); - ao_eeprom_read(PA_MIN_OFFSET, &pa_min, sizeof (pa_min)); -} - -void -ao_log_micro_data(void) -{ - uint16_t low_bits = pa; - - if (ao_log_offset < MAX_LOG_OFFSET) { - ao_eeprom_write(ao_log_offset, &low_bits, sizeof (low_bits)); - ao_log_offset += sizeof (low_bits); - } -} - -#define POLY 0x8408 - -static uint16_t -ao_log_micro_crc(uint16_t crc, uint8_t byte) -{ - uint8_t i; - - for (i = 0; i < 8; i++) { - if ((crc & 0x0001) ^ (byte & 0x0001)) - crc = (crc >> 1) ^ POLY; - else - crc = crc >> 1; - byte >>= 1; - } - return crc; -} - -static void -ao_log_hex_nibble(uint8_t b) -{ - if (b < 10) - ao_async_byte('0' + b); - else - ao_async_byte('a' - 10 + b); -} - -static void -ao_log_hex(uint8_t b) -{ - ao_log_hex_nibble(b>>4); - ao_log_hex_nibble(b&0xf); -} - -static void -ao_log_newline(void) -{ - ao_async_byte('\r'); - ao_async_byte('\n'); -} - -void -ao_log_micro_dump(void) -{ - uint16_t n_samples; - uint16_t nbytes; - uint8_t byte; - uint16_t b; - uint16_t crc = 0xffff; - - ao_eeprom_read(N_SAMPLES_OFFSET, &n_samples, sizeof (n_samples)); - if (n_samples == 0xffff) - n_samples = 0; - nbytes = STARTING_LOG_OFFSET + sizeof (uint16_t) * n_samples; - ao_async_start(); - ao_async_byte('M'); - ao_async_byte('P'); - for (b = 0; b < nbytes; b++) { - if ((b & 0xf) == 0) - ao_log_newline(); - ao_eeprom_read(b, &byte, 1); - ao_log_hex(byte); - crc = ao_log_micro_crc(crc, byte); - } - ao_log_newline(); - crc = ~crc; - ao_log_hex(crc >> 8); - ao_log_hex(crc); - ao_log_newline(); - ao_async_stop(); -} diff --git a/src/core/ao_log_micro.h b/src/core/ao_log_micro.h deleted file mode 100644 index 976852ee..00000000 --- a/src/core/ao_log_micro.h +++ /dev/null @@ -1,39 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_LOG_MICRO_H_ -#define _AO_LOG_MICRO_H_ - -#define PA_GROUND_OFFSET 0 -#define PA_MIN_OFFSET 4 -#define N_SAMPLES_OFFSET 8 -#define STARTING_LOG_OFFSET 10 -#define MAX_LOG_OFFSET 512 - -void -ao_log_micro_save(void); - -void -ao_log_micro_restore(void); - -void -ao_log_micro_data(void); - -void -ao_log_micro_dump(void); - -#endif /* _AO_LOG_MICRO_H_ */ diff --git a/src/core/ao_log_mini.c b/src/core/ao_log_mini.c deleted file mode 100644 index 29e3bd9f..00000000 --- a/src/core/ao_log_mini.c +++ /dev/null @@ -1,162 +0,0 @@ -/* - * 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 "ao.h" -#include -#include -#include - -static __xdata uint8_t ao_log_mutex; -static __xdata struct ao_log_mini log; - -__code uint8_t ao_log_format = AO_LOG_FORMAT; - -static uint8_t -ao_log_csum(__xdata uint8_t *b) __reentrant -{ - uint8_t sum = 0x5a; - uint8_t i; - - for (i = 0; i < sizeof (struct ao_log_mini); i++) - sum += *b++; - return -sum; -} - -uint8_t -ao_log_mini(__xdata struct ao_log_mini *log) __reentrant -{ - uint8_t wrote = 0; - /* set checksum */ - log->csum = 0; - log->csum = ao_log_csum((__xdata uint8_t *) log); - ao_mutex_get(&ao_log_mutex); { - if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) - ao_log_stop(); - if (ao_log_running) { - wrote = 1; - ao_storage_write(ao_log_current_pos, - log, - sizeof (struct ao_log_mini)); - ao_log_current_pos += sizeof (struct ao_log_mini); - } - } ao_mutex_put(&ao_log_mutex); - return wrote; -} - -static uint8_t -ao_log_dump_check_data(void) -{ - if (ao_log_csum((uint8_t *) &log) != 0) - return 0; - return 1; -} - -static __data uint8_t ao_log_data_pos; - -/* a hack to make sure that ao_log_minis fill the eeprom block in even units */ -typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_mini))] ; - -#ifndef AO_SENSOR_INTERVAL_ASCENT -#define AO_SENSOR_INTERVAL_ASCENT 1 -#define AO_SENSOR_INTERVAL_DESCENT 10 -#endif - -void -ao_log(void) -{ - __pdata uint16_t next_sensor; - - ao_storage_setup(); - - ao_log_scan(); - - while (!ao_log_running) - ao_sleep(&ao_log_running); - -#if HAS_FLIGHT - log.type = AO_LOG_FLIGHT; - log.tick = ao_sample_tick; - log.u.flight.flight = ao_flight_number; - log.u.flight.ground_pres = ao_ground_pres; - ao_log_mini(&log); -#endif - - /* Write the whole contents of the ring to the log - * when starting up. - */ - ao_log_data_pos = ao_data_ring_next(ao_data_head); - next_sensor = ao_data_ring[ao_log_data_pos].tick; - ao_log_state = ao_flight_startup; - for (;;) { - /* Write samples to EEPROM */ - while (ao_log_data_pos != ao_data_head) { - log.tick = ao_data_ring[ao_log_data_pos].tick; - if ((int16_t) (log.tick - next_sensor) >= 0) { - log.type = AO_LOG_SENSOR; - ao_log_pack24(log.u.sensor.pres, - ao_data_ring[ao_log_data_pos].ms5607_raw.pres); - ao_log_pack24(log.u.sensor.temp, - ao_data_ring[ao_log_data_pos].ms5607_raw.temp); - log.u.sensor.sense_a = ao_data_ring[ao_log_data_pos].adc.sense_a; - log.u.sensor.sense_m = ao_data_ring[ao_log_data_pos].adc.sense_m; - log.u.sensor.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; - ao_log_mini(&log); - if (ao_log_state <= ao_flight_coast) - next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT; - else - next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT; - } - ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); - } -#if HAS_FLIGHT - /* Write state change to EEPROM */ - if (ao_flight_state != ao_log_state) { - ao_log_state = ao_flight_state; - log.type = AO_LOG_STATE; - log.tick = ao_time(); - log.u.state.state = ao_log_state; - log.u.state.reason = 0; - ao_log_mini(&log); - - if (ao_log_state == ao_flight_landed) - ao_log_stop(); - } -#endif - - ao_log_flush(); - - /* Wait for a while */ - ao_delay(AO_MS_TO_TICKS(100)); - - /* Stop logging when told to */ - while (!ao_log_running) - ao_sleep(&ao_log_running); - } -} - -uint16_t -ao_log_flight(uint8_t slot) -{ - if (!ao_storage_read(ao_log_pos(slot), - &log, - sizeof (struct ao_log_mini))) - return 0; - - if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) - return log.u.flight.flight; - return 0; -} diff --git a/src/core/ao_log_single.c b/src/core/ao_log_single.c deleted file mode 100644 index 3f6235a6..00000000 --- a/src/core/ao_log_single.c +++ /dev/null @@ -1,198 +0,0 @@ -/* - * 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. - */ - -/* - * ao_log_single.c - * - * Stores a sequence of fixed-size (32 byte) chunks - * without splitting memory up into separate flights - */ - -#include "ao.h" -#include "ao_product.h" - -static __xdata struct ao_task ao_log_single_task; - -__xdata uint8_t ao_log_running; -__xdata uint8_t ao_log_mutex; -__pdata uint32_t ao_log_start_pos; -__pdata uint32_t ao_log_end_pos; -__pdata uint32_t ao_log_current_pos; - -__xdata union ao_log_single ao_log_single_write_data; -__xdata union ao_log_single ao_log_single_read_data; - -uint8_t -ao_log_single_write(void) -{ - uint8_t wrote = 0; - - ao_mutex_get(&ao_log_mutex); { - if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) - ao_log_single_stop(); - if (ao_log_running) { - wrote = 1; - ao_storage_write(ao_log_current_pos, - &ao_log_single_write_data, - AO_LOG_SINGLE_SIZE); - ao_log_current_pos += AO_LOG_SINGLE_SIZE; - } - } ao_mutex_put(&ao_log_mutex); - return wrote; -} - -static uint8_t -ao_log_single_valid(void) -{ - __xdata uint8_t *d = ao_log_single_read_data.bytes; - uint8_t i; - for (i = 0; i < AO_LOG_SINGLE_SIZE; i++) - if (*d++ != 0xff) - return 1; - return 0; -} - -uint8_t -ao_log_single_read(uint32_t pos) -{ - if (!ao_storage_read(pos, &ao_log_single_read_data, AO_LOG_SINGLE_SIZE)) - return 0; - return ao_log_single_valid(); -} - -void -ao_log_single_start(void) -{ - if (!ao_log_running) { - ao_log_running = 1; - ao_wakeup(&ao_log_running); - } -} - -void -ao_log_single_stop(void) -{ - if (ao_log_running) { - ao_log_running = 0; - } -} - -void -ao_log_single_restart(void) -{ - /* Find end of data */ - ao_log_end_pos = ao_storage_config; - for (ao_log_current_pos = 0; - ao_log_current_pos < ao_storage_config; - ao_log_current_pos += ao_storage_block) - { - if (!ao_log_single_read(ao_log_current_pos)) - break; - } - if (ao_log_current_pos > 0) { - ao_log_current_pos -= ao_storage_block; - for (; ao_log_current_pos < ao_storage_config; - ao_log_current_pos += sizeof (struct ao_log_telescience)) - { - if (!ao_log_single_read(ao_log_current_pos)) - break; - } - } -} - -void -ao_log_single_set(void) -{ - printf("Logging currently %s\n", ao_log_running ? "on" : "off"); - ao_cmd_hex(); - if (ao_cmd_status == ao_cmd_success) { - if (ao_cmd_lex_i) { - printf("Logging from %ld to %ld\n", ao_log_current_pos, ao_log_end_pos); - ao_log_single_start(); - } else { - printf ("Log stopped at %ld\n", ao_log_current_pos); - ao_log_single_stop(); - } - } - ao_cmd_status = ao_cmd_success; -} - -void -ao_log_single_delete(void) -{ - uint32_t pos; - - ao_cmd_hex(); - if (ao_cmd_status != ao_cmd_success) - return; - if (ao_cmd_lex_i != 1) { - ao_cmd_status = ao_cmd_syntax_error; - printf("No such flight: %d\n", ao_cmd_lex_i); - return; - } - ao_log_single_stop(); - for (pos = 0; pos < ao_storage_config; pos += ao_storage_block) { - if (!ao_log_single_read(pos)) - break; - ao_storage_erase(pos); - } - ao_log_current_pos = ao_log_start_pos = 0; - if (pos == 0) - printf("No such flight: %d\n", ao_cmd_lex_i); - else - printf ("Erased\n"); -} - -uint8_t -ao_log_full(void) -{ - return ao_log_current_pos >= ao_log_end_pos; -} - -uint8_t -ao_log_present(void) -{ - return ao_log_single_read(0); -} - -static void -ao_log_single_query(void) -{ - printf("Logging enabled: %d\n", ao_log_running); - printf("Log start: %ld\n", ao_log_start_pos); - printf("Log cur: %ld\n", ao_log_current_pos); - printf("Log end: %ld\n", ao_log_end_pos); - ao_log_single_extra_query(); -} - -const struct ao_cmds ao_log_single_cmds[] = { - { ao_log_single_set, "L <0 off, 1 on>\0Set logging" }, - { ao_log_single_list, "l\0List stored logs" }, - { ao_log_single_delete, "d 1\0Delete all stored logs" }, - { ao_log_single_query, "q\0Query log status" }, - { 0, NULL }, -}; - -void -ao_log_single_init(void) -{ - ao_log_running = 0; - - ao_cmd_register(&ao_log_single_cmds[0]); - - ao_add_task(&ao_log_single_task, ao_log_single, "log"); -} diff --git a/src/core/ao_log_telem.c b/src/core/ao_log_telem.c deleted file mode 100644 index 095aca37..00000000 --- a/src/core/ao_log_telem.c +++ /dev/null @@ -1,131 +0,0 @@ -/* - * 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 -#include -#include - -__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; -__xdata int16_t ao_max_height; /* max of ao_height */ -__pdata int16_t sense_d, sense_m; -__pdata uint8_t ao_igniter_present; - -static void -ao_log_telem_track() { - if (ao_monitoring == sizeof (union ao_telemetry_all)) { - switch (ao_log_single_write_data.telemetry.generic.type) { - case AO_TELEMETRY_SENSOR_TELEMETRUM: - case AO_TELEMETRY_SENSOR_TELEMINI: - /* fall through ... */ - case AO_TELEMETRY_SENSOR_TELENANO: - if (ao_log_single_write_data.telemetry.generic.type == AO_TELEMETRY_SENSOR_TELENANO) { - ao_igniter_present = 0; - } else { - sense_d = ao_log_single_write_data.telemetry.sensor.sense_d; - sense_m = ao_log_single_write_data.telemetry.sensor.sense_m; - ao_igniter_present = 1; - } - if (ao_log_single_write_data.telemetry.sensor.height > ao_max_height) { - ao_max_height = ao_log_single_write_data.telemetry.sensor.height; - } - if (ao_log_single_write_data.telemetry.sensor.state != ao_flight_state) { - ao_flight_state = ao_log_single_write_data.telemetry.sensor.state; - if (ao_flight_state == ao_flight_pad) - ao_max_height = 0; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - } - } -} - -enum ao_igniter_status -ao_igniter_status(enum ao_igniter igniter) -{ - int16_t value; - - switch (igniter) { - case ao_igniter_drogue: - value = sense_d; - break; - case ao_igniter_main: - value = sense_m; - break; - default: - value = 0; - break; - } - if (value < AO_IGNITER_OPEN) - return ao_igniter_open; - else if (value > AO_IGNITER_CLOSED) - return ao_igniter_ready; - else - return ao_igniter_unknown; -} - -void -ao_log_single(void) -{ - ao_storage_setup(); - - /* This can take a while, so let the rest - * of the system finish booting before we start - */ - ao_delay(AO_SEC_TO_TICKS(2)); - - ao_log_running = 1; - ao_log_single_restart(); - ao_flight_state = ao_flight_startup; - ao_monitor_set(sizeof(struct ao_telemetry_generic)); - - for (;;) { - while (!ao_log_running) - ao_sleep(&ao_log_running); - - ao_log_monitor_pos = ao_monitor_head; - while (ao_log_running) { - /* Write samples to EEPROM */ - while (ao_log_monitor_pos != ao_monitor_head) { - 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(); - } - /* Wait for more telemetry data to arrive */ - ao_sleep(DATA_TO_XDATA(&ao_monitor_head)); - } - } -} - -void -ao_log_single_list(void) -{ - if (ao_log_current_pos != 0) - printf("flight 1 start %x end %x\n", - 0, - (uint16_t) ((ao_log_current_pos + 0xff) >> 8)); - printf ("done\n"); -} - -void -ao_log_single_extra_query(void) -{ -} diff --git a/src/core/ao_log_telescience.c b/src/core/ao_log_telescience.c deleted file mode 100644 index 002a10bd..00000000 --- a/src/core/ao_log_telescience.c +++ /dev/null @@ -1,119 +0,0 @@ -/* - * 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_product.h" -#include "ao_log.h" -#include "ao_companion.h" - -static uint8_t ao_log_data_pos; - -__code uint8_t ao_log_format = AO_LOG_FORMAT_TELESCIENCE; - -static void -ao_log_telescience_csum(void) __reentrant -{ - __xdata uint8_t *b = ao_log_single_write_data.bytes; - uint8_t sum = 0x5a; - uint8_t i; - - ao_log_single_write_data.telescience.csum = 0; - for (i = 0; i < sizeof (struct ao_log_telescience); i++) - sum += *b++; - ao_log_single_write_data.telescience.csum = -sum; -} - -void -ao_log_single(void) -{ - ao_storage_setup(); - - /* This can take a while, so let the rest - * of the system finish booting before we start - */ - ao_delay(AO_SEC_TO_TICKS(10)); - - ao_log_single_restart(); - for (;;) { - while (!ao_log_running) - ao_sleep(&ao_log_running); - - ao_log_start_pos = ao_log_current_pos; - ao_log_single_write_data.telescience.type = AO_LOG_TELESCIENCE_START; - ao_log_single_write_data.telescience.tick = ao_time(); - ao_log_single_write_data.telescience.adc[0] = ao_companion_command.serial; - ao_log_single_write_data.telescience.adc[1] = ao_companion_command.flight; - ao_log_telescience_csum(); - ao_log_single_write(); - /* Write the whole contents of the ring to the log - * when starting up. - */ - ao_log_data_pos = ao_data_ring_next(ao_data_head); - ao_log_single_write_data.telescience.type = AO_LOG_TELESCIENCE_DATA; - while (ao_log_running) { - /* Write samples to EEPROM */ - while (ao_log_data_pos != ao_data_head) { - ao_log_single_write_data.telescience.tick = ao_data_ring[ao_log_data_pos].tick; - memcpy(&ao_log_single_write_data.telescience.adc, (void *) ao_data_ring[ao_log_data_pos].adc.adc, - AO_LOG_TELESCIENCE_NUM_ADC * sizeof (uint16_t)); - ao_log_telescience_csum(); - ao_log_single_write(); - ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); - } - /* Wait for more ADC data to arrive */ - ao_sleep((void *) &ao_data_head); - } - memset(&ao_log_single_write_data.telescience.adc, '\0', sizeof (ao_log_single_write_data.telescience.adc)); - } -} - -void -ao_log_single_list(void) -{ - uint32_t pos; - uint32_t start = 0; - uint8_t flight = 0; - - for (pos = 0; ; pos += sizeof (struct ao_log_telescience)) { - if (pos >= ao_storage_config || - !ao_log_single_read(pos) || - ao_log_single_read_data.telescience.type == AO_LOG_TELESCIENCE_START) - { - if (pos != start) { - printf("flight %d start %x end %x\n", - flight, - (uint16_t) (start >> 8), - (uint16_t) ((pos + 0xff) >> 8)); flush(); - } - if (ao_log_single_read_data.telescience.type != AO_LOG_TELESCIENCE_START) - break; - start = pos; - flight++; - } - } - printf ("done\n"); -} - -void -ao_log_single_extra_query(void) -{ - printf("log data tick: %04x\n", ao_log_single_write_data.telescience.tick); - printf("TM data tick: %04x\n", ao_log_single_write_data.telescience.tm_tick); - printf("TM state: %d\n", ao_log_single_write_data.telescience.tm_state); - printf("TM serial: %d\n", ao_companion_command.serial); - printf("TM flight: %d\n", ao_companion_command.flight); -} diff --git a/src/core/ao_log_tiny.c b/src/core/ao_log_tiny.c deleted file mode 100644 index 67767dc9..00000000 --- a/src/core/ao_log_tiny.c +++ /dev/null @@ -1,161 +0,0 @@ -/* - * 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" - -static __data uint16_t ao_log_tiny_interval; - -#define AO_LOG_TINY_INTERVAL_DEFAULT AO_MS_TO_TICKS(1000) -#if USE_FAST_ASCENT_LOG -#define AO_LOG_TINY_INTERVAL_ASCENT AO_MS_TO_TICKS(100) -#define AO_PAD_RING 8 -#else -#define AO_LOG_TINY_INTERVAL_ASCENT AO_LOG_TINY_INTERVAL_DEFAULT -#define AO_PAD_RING 2 -#endif - -__code uint8_t ao_log_format = AO_LOG_FORMAT_TINY; - -void -ao_log_tiny_set_interval(uint16_t ticks) -{ - ao_log_tiny_interval = ticks; -} - - -static void ao_log_tiny_data(uint16_t d) -{ - if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) - ao_log_stop(); - if (ao_log_running) { - ao_storage_write(ao_log_current_pos, DATA_TO_XDATA(&d), 2); - ao_log_current_pos += 2; - } -} - -static __xdata uint16_t ao_log_pad_ring[AO_PAD_RING]; -static __pdata uint8_t ao_log_pad_ring_pos; - -#define ao_pad_ring_next(n) (((n) + 1) & (AO_PAD_RING - 1)) - -static void ao_log_tiny_queue(uint16_t d) -{ - ao_log_pad_ring[ao_log_pad_ring_pos] = d; - ao_log_pad_ring_pos = ao_pad_ring_next(ao_log_pad_ring_pos); -} - -static void ao_log_tiny_start(void) -{ - uint8_t p; - uint16_t d; - - ao_log_tiny_data(ao_flight_number); - ao_log_tiny_data(ao_ground_pres); - p = ao_log_pad_ring_pos; - do { - d = ao_log_pad_ring[p]; - /* - * ignore unwritten slots - */ - if (d) - ao_log_tiny_data(d); - p = ao_pad_ring_next(p); - } while (p != ao_log_pad_ring_pos); -} - -void -ao_log(void) -{ - uint16_t last_time; - uint16_t now; - enum ao_flight_state ao_log_tiny_state; - int32_t sum; - int16_t count; - uint8_t ao_log_data; - uint8_t ao_log_started = 0; - - ao_storage_setup(); - - ao_log_scan(); - - ao_log_tiny_state = ao_flight_invalid; - ao_log_tiny_interval = AO_LOG_TINY_INTERVAL_ASCENT; - sum = 0; - count = 0; - ao_log_data = ao_sample_data; - last_time = ao_time(); - for (;;) { - - /* - * Add in pending sample data - */ - ao_sleep(DATA_TO_XDATA(&ao_sample_data)); - while (ao_log_data != ao_sample_data) { - sum += ao_data_pres(&ao_data_ring[ao_log_data]); - count++; - ao_log_data = ao_data_ring_next(ao_log_data); - } - if (ao_log_running) { - if (!ao_log_started) { - ao_log_tiny_start(); - ao_log_started = 1; - } - if (ao_flight_state != ao_log_tiny_state) { - ao_log_tiny_data(ao_flight_state | 0x8000); - ao_log_tiny_state = ao_flight_state; - ao_log_tiny_interval = AO_LOG_TINY_INTERVAL_DEFAULT; -#if AO_LOG_TINY_INTERVAL_ASCENT != AO_LOG_TINY_INTERVAL_DEFAULT - if (ao_log_tiny_state <= ao_flight_coast) - ao_log_tiny_interval = AO_LOG_TINY_INTERVAL_ASCENT; -#endif - if (ao_log_tiny_state == ao_flight_landed) - ao_log_stop(); - } - } - - /* Stop logging when told to */ - if (!ao_log_running && ao_log_started) - ao_exit(); - - /* - * Write out the sample when finished - */ - now = ao_time(); - if ((int16_t) (now - (last_time + ao_log_tiny_interval)) >= 0 && count) { - count = sum / count; - if (ao_log_started) - ao_log_tiny_data(count); - else - ao_log_tiny_queue(count); - sum = 0; - count = 0; - last_time = now; - } - } -} - -uint16_t -ao_log_flight(uint8_t slot) -{ - static __xdata uint16_t flight; - - (void) slot; - ao_storage_read(0, &flight, 2); - if (flight == 0xffff) - flight = 0; - return flight; -} diff --git a/src/core/ao_microflight.c b/src/core/ao_microflight.c deleted file mode 100644 index f680e400..00000000 --- a/src/core/ao_microflight.c +++ /dev/null @@ -1,143 +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. - */ - -#ifndef AO_FLIGHT_TEST -#include -#endif -#include -#include - -uint32_t pa; -uint32_t pa_ground; -uint32_t pa_min; - -static void -ao_microsample(void) -{ - ao_pa_get(); - ao_microkalman_predict(); - ao_microkalman_correct(); -} - -#define NUM_PA_HIST (GROUND_AVG) - -#define SKIP_PA_HIST(i,j) (((i) + (j)) & (NUM_PA_HIST - 1)) - -static uint32_t pa_hist[NUM_PA_HIST]; - -void -ao_microflight(void) -{ - int16_t sample_count; - uint16_t time; - uint32_t pa_interval_min, pa_interval_max; - int32_t pa_diff; - uint8_t h, i; - uint8_t accel_lock = 0; - uint32_t pa_sum = 0; - - /* Wait for motion, averaging values to get ground pressure */ - - time = ao_time(); - ao_pa_get(); - ao_microkalman_init(); - pa_ground = pa; - sample_count = 0; - h = 0; - for (;;) { - time += SAMPLE_SLEEP; - if ((sample_count & 0x1f) == 0) - ao_led_on(AO_LED_REPORT); - ao_delay_until(time); - ao_microsample(); - if ((sample_count & 0x1f) == 0) - ao_led_off(AO_LED_REPORT); - pa_hist[h] = pa; - h = SKIP_PA_HIST(h,1); - pa_diff = pa_ground - ao_pa; - - /* Check for a significant pressure change */ - if (pa_diff > BOOST_DETECT) - break; - - if (sample_count < GROUND_AVG * 2) { - if (sample_count < GROUND_AVG) - pa_sum += pa; - ++sample_count; - } else { - pa_ground = pa_sum >> GROUND_AVG_SHIFT; - pa_sum = 0; - sample_count = 0; - } - } - - /* Go back and find the last sample close to the ground */ - pa_min = pa_ground - LAND_DETECT; - for (i = SKIP_PA_HIST(h,-2); i != SKIP_PA_HIST(h,2); i = SKIP_PA_HIST(i,-2)) { - if (pa_hist[i] >= pa_min) - break; - } - - /* Log the remaining samples so we get a complete history since leaving the ground */ - for (; i != h; i = SKIP_PA_HIST(i,2)) { - pa = pa_hist[i]; - ao_log_micro_data(); - } - - /* Now sit around until the pressure is stable again and record the max */ - - sample_count = 0; - pa_min = ao_pa; - pa_interval_min = ao_pa; - pa_interval_max = ao_pa; - for (;;) { - time += SAMPLE_SLEEP; - ao_delay_until(time); - if ((sample_count & 3) == 0) - ao_led_on(AO_LED_REPORT); - ao_microsample(); - if ((sample_count & 3) == 0) - ao_led_off(AO_LED_REPORT); - if (sample_count & 1) - ao_log_micro_data(); - - /* If accelerating upwards, don't look for min pressure */ - if (ao_pa_accel < ACCEL_LOCK_PA) - accel_lock = ACCEL_LOCK_TIME; - else if (accel_lock) - --accel_lock; - else if (ao_pa < pa_min) - pa_min = ao_pa; - - if (sample_count == (GROUND_AVG - 1)) { - pa_diff = pa_interval_max - pa_interval_min; - - /* Check to see if the pressure is now stable */ - if (pa_diff < LAND_DETECT) - break; - sample_count = 0; - pa_interval_min = ao_pa; - pa_interval_max = ao_pa; - } else { - if (ao_pa < pa_interval_min) - pa_interval_min = ao_pa; - if (ao_pa > pa_interval_max) - pa_interval_max = ao_pa; - ++sample_count; - } - } -} diff --git a/src/core/ao_microkalman.c b/src/core/ao_microkalman.c deleted file mode 100644 index 0684ea2b..00000000 --- a/src/core/ao_microkalman.c +++ /dev/null @@ -1,74 +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. - */ - -#ifndef AO_FLIGHT_TEST -#include -#endif -#include - -#define FIX_BITS 16 - -#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) -#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) -#define from_fix8(x) ((x) >> 8) -#define from_fix(x) ((x) >> 16) -#define fix8_to_fix16(x) ((x) << 8) -#define fix16_to_fix8(x) ((x) >> 8) - -#include - -/* Basic time step (96ms) */ -#define AO_MK_STEP to_fix16(0.096) -/* step ** 2 / 2 */ -#define AO_MK_STEP_2_2 to_fix16(0.004608) - -uint32_t ao_k_pa; /* 24.8 fixed point */ -int32_t ao_k_pa_speed; /* 16.16 fixed point */ -int32_t ao_k_pa_accel; /* 16.16 fixed point */ - -uint32_t ao_pa; /* integer portion */ -int16_t ao_pa_speed; /* integer portion */ -int16_t ao_pa_accel; /* integer portion */ - -void -ao_microkalman_init(void) -{ - ao_pa = pa; - ao_k_pa = pa << 8; -} - -void -ao_microkalman_predict(void) -{ - ao_k_pa += fix16_to_fix8((int32_t) ao_pa_speed * AO_MK_STEP + (int32_t) ao_pa_accel * AO_MK_STEP_2_2); - ao_k_pa_speed += (int32_t) ao_pa_accel * AO_MK_STEP; -} - -void -ao_microkalman_correct(void) -{ - int16_t e; /* Height error in Pa */ - - e = pa - from_fix8(ao_k_pa); - - ao_k_pa += fix16_to_fix8((int32_t) e * AO_MK_BARO_K0_10); - ao_k_pa_speed += (int32_t) e * AO_MK_BARO_K1_10; - ao_k_pa_accel += (int32_t) e * AO_MK_BARO_K2_10; - ao_pa = from_fix8(ao_k_pa); - ao_pa_speed = from_fix(ao_k_pa_speed); - ao_pa_accel = from_fix(ao_k_pa_accel); -} diff --git a/src/core/ao_monitor.c b/src/core/ao_monitor.c deleted file mode 100644 index 18f170b4..00000000 --- a/src/core/ao_monitor.c +++ /dev/null @@ -1,308 +0,0 @@ -/* - * Copyright © 2009 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_telem.h" -#include "ao_flight.h" - -#if !HAS_MONITOR -#error Must define HAS_MONITOR to 1 -#endif - -#ifndef LEGACY_MONITOR -#error Must define LEGACY_MONITOR -#endif - -#ifndef HAS_MONITOR_PUT -#define HAS_MONITOR_PUT 1 -#endif - -#ifndef AO_MONITOR_LED -#error Must define AO_MONITOR_LED -#endif - -__data uint8_t ao_monitoring; -static __data uint8_t ao_monitor_disabled; -static __data uint8_t ao_internal_monitoring; -static __data uint8_t ao_external_monitoring; - -__xdata union ao_monitor ao_monitor_ring[AO_MONITOR_RING]; - -__data uint8_t ao_monitor_head; - -static void -_ao_monitor_adjust(void) -{ - if (ao_monitoring) - ao_radio_recv_abort(); - if (ao_monitor_disabled) - ao_monitoring = 0; - else { - if (ao_external_monitoring) - ao_monitoring = ao_external_monitoring; - else - ao_monitoring = ao_internal_monitoring; - } - ao_wakeup(DATA_TO_XDATA(&ao_monitoring)); -} - -void -ao_monitor_get(void) -{ - uint8_t size; - - for (;;) { - switch (ao_monitoring) { - case 0: - ao_sleep(DATA_TO_XDATA(&ao_monitoring)); - continue; -#if LEGACY_MONITOR - case AO_MONITORING_ORIG: - size = sizeof (struct ao_telemetry_orig_recv); - break; -#endif - default: - if (ao_monitoring > AO_MAX_TELEMETRY) - ao_monitoring = AO_MAX_TELEMETRY; - size = ao_monitoring; - break; - } - if (!ao_radio_recv(&ao_monitor_ring[ao_monitor_head], size + 2, 0)) - continue; - ao_monitor_head = ao_monitor_ring_next(ao_monitor_head); - ao_wakeup(DATA_TO_XDATA(&ao_monitor_head)); - } -} - -#if AO_MONITOR_LED -__xdata struct ao_task ao_monitor_blink_task; - -void -ao_monitor_blink(void) -{ - for (;;) { - ao_sleep(DATA_TO_XDATA(&ao_monitor_head)); - ao_led_for(AO_MONITOR_LED, AO_MS_TO_TICKS(100)); - } -} -#endif - -#if HAS_MONITOR_PUT - -static const char xdigit[16] = { - '0', '1', '2', '3', '4', '5', '6', '7', - '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' -}; - -#define hex(c) do { putchar(xdigit[(c) >> 4]); putchar(xdigit[(c)&0xf]); } while (0) - -void -ao_monitor_put(void) -{ -#if LEGACY_MONITOR - __xdata char callsign[AO_MAX_CALLSIGN+1]; - int16_t rssi; -#endif - uint8_t ao_monitor_tail; - uint8_t state; - uint8_t sum, byte; - __xdata union ao_monitor *m; - -#define recv_raw ((m->raw)) -#define recv_orig ((m->orig)) -#define recv_tiny ((m->tiny)) - - ao_monitor_tail = ao_monitor_head; - for (;;) { - while (!ao_external_monitoring) - ao_sleep(DATA_TO_XDATA(&ao_external_monitoring)); - while (ao_monitor_tail == ao_monitor_head && ao_external_monitoring) - ao_sleep(DATA_TO_XDATA(&ao_monitor_head)); - if (!ao_external_monitoring) - continue; - m = &ao_monitor_ring[ao_monitor_tail]; - ao_monitor_tail = ao_monitor_ring_next(ao_monitor_tail); - switch (ao_monitoring) { - case 0: - break; -#if LEGACY_MONITOR - case AO_MONITORING_ORIG: - state = recv_orig.telemetry_orig.flight_state; - - rssi = (int16_t) AO_RSSI_FROM_RADIO(recv_orig.rssi); - ao_xmemcpy(callsign, recv_orig.telemetry_orig.callsign, AO_MAX_CALLSIGN); - if (state > ao_flight_invalid) - state = ao_flight_invalid; - if (recv_orig.status & PKT_APPEND_STATUS_1_CRC_OK) { - - /* General header fields */ - printf(AO_TELEM_VERSION " %d " - AO_TELEM_CALL " %s " - AO_TELEM_SERIAL " %d " - AO_TELEM_FLIGHT " %d " - AO_TELEM_RSSI " %d " - AO_TELEM_STATE " %s " - AO_TELEM_TICK " %d ", - AO_TELEMETRY_VERSION, - callsign, - recv_orig.telemetry_orig.serial, - recv_orig.telemetry_orig.flight, - rssi, - ao_state_names[state], - recv_orig.telemetry_orig.adc.tick); - - /* Raw sensor values */ - printf(AO_TELEM_RAW_ACCEL " %d " - AO_TELEM_RAW_BARO " %d " - AO_TELEM_RAW_THERMO " %d " - AO_TELEM_RAW_BATT " %d " - AO_TELEM_RAW_DROGUE " %d " - AO_TELEM_RAW_MAIN " %d ", - recv_orig.telemetry_orig.adc.accel, - recv_orig.telemetry_orig.adc.pres, - recv_orig.telemetry_orig.adc.temp, - recv_orig.telemetry_orig.adc.v_batt, - recv_orig.telemetry_orig.adc.sense_d, - recv_orig.telemetry_orig.adc.sense_m); - - /* Sensor calibration values */ - printf(AO_TELEM_CAL_ACCEL_GROUND " %d " - AO_TELEM_CAL_BARO_GROUND " %d " - AO_TELEM_CAL_ACCEL_PLUS " %d " - AO_TELEM_CAL_ACCEL_MINUS " %d ", - recv_orig.telemetry_orig.ground_accel, - recv_orig.telemetry_orig.ground_pres, - recv_orig.telemetry_orig.accel_plus_g, - recv_orig.telemetry_orig.accel_minus_g); - - if (recv_orig.telemetry_orig.u.k.unused == 0x8000) { - /* Kalman state values */ - printf(AO_TELEM_KALMAN_HEIGHT " %d " - AO_TELEM_KALMAN_SPEED " %d " - AO_TELEM_KALMAN_ACCEL " %d ", - recv_orig.telemetry_orig.height, - recv_orig.telemetry_orig.u.k.speed, - recv_orig.telemetry_orig.accel); - } else { - /* Ad-hoc flight values */ - printf(AO_TELEM_ADHOC_ACCEL " %d " - AO_TELEM_ADHOC_SPEED " %ld " - AO_TELEM_ADHOC_BARO " %d ", - recv_orig.telemetry_orig.accel, - recv_orig.telemetry_orig.u.flight_vel, - recv_orig.telemetry_orig.height); - } - ao_gps_print(&recv_orig.telemetry_orig.gps); - ao_gps_tracking_print(&recv_orig.telemetry_orig.gps_tracking); - putchar('\n'); -#if HAS_RSSI - ao_rssi_set(rssi); -#endif - } else { - printf("CRC INVALID RSSI %3d\n", rssi); - } - break; -#endif /* LEGACY_MONITOR */ - default: -#if AO_PROFILE - { - extern uint32_t ao_rx_start_tick, ao_rx_packet_tick, ao_rx_done_tick, ao_rx_last_done_tick; - extern uint32_t ao_fec_decode_start, ao_fec_decode_end; - - printf ("between packet: %d\n", ao_rx_start_tick - ao_rx_last_done_tick); - printf ("receive start delay: %d\n", ao_rx_packet_tick - ao_rx_start_tick); - printf ("decode time: %d\n", ao_fec_decode_end - ao_fec_decode_start); - printf ("rx cleanup: %d\n", ao_rx_done_tick - ao_fec_decode_end); - } -#endif - printf("TELEM "); - hex((uint8_t) (ao_monitoring + 2)); - sum = 0x5a; - for (state = 0; state < ao_monitoring + 2; state++) { - byte = recv_raw.packet[state]; - sum += byte; - hex(byte); - } - hex(sum); - putchar ('\n'); -#if HAS_RSSI - if (recv_raw.packet[ao_monitoring + 1] & PKT_APPEND_STATUS_1_CRC_OK) { - rssi = AO_RSSI_FROM_RADIO(recv_raw.packet[ao_monitoring]); - ao_rssi_set(rssi); - } -#endif - break; - } - ao_usb_flush(); - } -} - -__xdata struct ao_task ao_monitor_put_task; -#endif - -__xdata struct ao_task ao_monitor_get_task; - -void -ao_monitor_set(uint8_t monitoring) -{ - ao_internal_monitoring = monitoring; - _ao_monitor_adjust(); -} - -void -ao_monitor_disable(void) -{ - ++ao_monitor_disabled; - _ao_monitor_adjust(); -} - -void -ao_monitor_enable(void) -{ - --ao_monitor_disabled; - _ao_monitor_adjust(); -} - -#if HAS_MONITOR_PUT -static void -set_monitor(void) -{ - ao_cmd_hex(); - ao_external_monitoring = ao_cmd_lex_i; - ao_wakeup(DATA_TO_XDATA(&ao_external_monitoring)); - ao_wakeup(DATA_TO_XDATA(&ao_monitor_head)); - _ao_monitor_adjust(); -} - -__code struct ao_cmds ao_monitor_cmds[] = { - { set_monitor, "m <0 off, 1 old, 20 std>\0Set radio monitoring" }, - { 0, NULL }, -}; -#endif - -void -ao_monitor_init(void) __reentrant -{ -#if HAS_MONITOR_PUT - ao_cmd_register(&ao_monitor_cmds[0]); - ao_add_task(&ao_monitor_put_task, ao_monitor_put, "monitor_put"); -#endif - ao_add_task(&ao_monitor_get_task, ao_monitor_get, "monitor_get"); -#if AO_MONITOR_LED - ao_add_task(&ao_monitor_blink_task, ao_monitor_blink, "monitor_blink"); -#endif -} diff --git a/src/core/ao_mutex.c b/src/core/ao_mutex.c deleted file mode 100644 index 952ff462..00000000 --- a/src/core/ao_mutex.c +++ /dev/null @@ -1,41 +0,0 @@ -/* - * Copyright © 2009 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" - -void -ao_mutex_get(__xdata uint8_t *mutex) __reentrant -{ - if (*mutex == ao_cur_task->task_id) - ao_panic(AO_PANIC_MUTEX); - ao_arch_critical( - while (*mutex) - ao_sleep(mutex); - *mutex = ao_cur_task->task_id; - ); -} - -void -ao_mutex_put(__xdata uint8_t *mutex) __reentrant -{ - if (*mutex != ao_cur_task->task_id) - ao_panic(AO_PANIC_MUTEX); - ao_arch_critical( - *mutex = 0; - ao_wakeup(mutex); - ); -} diff --git a/src/core/ao_notask.c b/src/core/ao_notask.c deleted file mode 100644 index 6f967e6d..00000000 --- a/src/core/ao_notask.c +++ /dev/null @@ -1,46 +0,0 @@ -/* - * 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 - -static volatile void *ao_wchan; - -uint8_t -ao_sleep(__xdata void *wchan) -{ -#if 1 - ao_wchan = wchan; - ao_arch_wait_interrupt(); -#else - uint8_t sreg; - - ao_wchan = wchan; - asm("in %0,__SREG__" : "=&r" (sreg)); - sei(); - while (ao_wchan) - ao_arch_cpu_idle(); - asm("out __SREG__,%0" : : "r" (sreg)); -#endif - return 0; -} - -void -ao_wakeup(__xdata void *wchan) -{ - (void) wchan; - ao_wchan = 0; -} diff --git a/src/core/ao_notask.h b/src/core/ao_notask.h deleted file mode 100644 index 6b6b5bb8..00000000 --- a/src/core/ao_notask.h +++ /dev/null @@ -1,27 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_NOTASK_H_ -#define _AO_NOTASK_H_ - -uint8_t -ao_sleep(__xdata void *wchan); - -void -ao_wakeup(__xdata void *wchan); - -#endif /* _AO_NOTASK_H_ */ diff --git a/src/core/ao_packet.h b/src/core/ao_packet.h deleted file mode 100644 index b8426cf9..00000000 --- a/src/core/ao_packet.h +++ /dev/null @@ -1,91 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_PACKET_H_ -#define _AO_PACKET_H_ - -/* - * ao_packet.c - * - * Packet-based command interface - */ - -#define AO_PACKET_MAX 64 -#define AO_PACKET_SYN (uint8_t) 0xff - -struct ao_packet { - uint8_t addr; - uint8_t len; - uint8_t seq; - uint8_t ack; - uint8_t d[AO_PACKET_MAX]; - uint8_t callsign[AO_MAX_CALLSIGN]; -}; - -struct ao_packet_recv { - struct ao_packet packet; - int8_t rssi; - uint8_t status; -}; - -extern __xdata struct ao_packet_recv ao_rx_packet; -extern __xdata struct ao_packet ao_tx_packet; -extern __xdata struct ao_task ao_packet_task; -extern __xdata uint8_t ao_packet_enable; -extern __xdata uint8_t ao_packet_master_sleeping; -extern __pdata uint8_t ao_packet_rx_len, ao_packet_rx_used, ao_packet_tx_used; -extern __xdata uint8_t ao_packet_restart; - -void -ao_packet_send(void); - -uint8_t -ao_packet_recv(void); - -void -ao_packet_flush(void); - -void -ao_packet_putchar(char c) __reentrant; - -int -_ao_packet_pollchar(void); - -#if PACKET_HAS_MASTER -/* ao_packet_master.c */ - -extern __xdata int8_t ao_packet_last_rssi; - -void -ao_packet_master_init(void); -#endif - -#if PACKET_HAS_SLAVE -/* ao_packet_slave.c */ - -void -ao_packet_slave_start(void); - -void -ao_packet_slave_stop(void); - -void -ao_packet_slave_init(uint8_t enable); - -#endif - -#endif /* _AO_PACKET_H_ */ diff --git a/src/core/ao_panic.c b/src/core/ao_panic.c deleted file mode 100644 index c29cd8fe..00000000 --- a/src/core/ao_panic.c +++ /dev/null @@ -1,90 +0,0 @@ -/* - * Copyright © 2009 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" - -#ifndef HAS_BEEP -#error Please define HAS_BEEP -#endif - -#if !HAS_BEEP -#define ao_beep(x) -#endif -#if !LEDS_AVAILABLE -#define ao_led_on(x) -#define ao_led_off(x) -#endif - -#ifndef AO_LED_PANIC -#define AO_LED_PANIC AO_LED_RED -#endif - -static void -ao_panic_delay(uint8_t n) -{ - uint8_t i = 0, j = 0; - - while (n--) - while (--j) - while (--i) - ao_arch_nop(); -} - -void -ao_panic(uint8_t reason) -{ - uint8_t n; - -#if LOW_LEVEL_DEBUG - ao_cur_task = NULL; - printf ("panic %d\n", reason); -#endif - ao_arch_block_interrupts(); - for (;;) { - ao_panic_delay(20); - for (n = 0; n < 5; n++) { - ao_led_on(AO_LED_PANIC); - ao_beep(AO_BEEP_HIGH); - ao_panic_delay(1); - ao_led_off(AO_LED_PANIC); - ao_beep(AO_BEEP_LOW); - ao_panic_delay(1); - } - ao_beep(AO_BEEP_OFF); - ao_panic_delay(2); - -#ifdef SDCC -#pragma disable_warning 126 -#endif - if (reason & 0x40) { - ao_led_on(AO_LED_PANIC); - ao_beep(AO_BEEP_HIGH); - ao_panic_delay(40); - ao_led_off(AO_LED_PANIC); - ao_beep(AO_BEEP_OFF); - ao_panic_delay(10); - } - for (n = 0; n < (reason & 0x3f); n++) { - ao_led_on(AO_LED_PANIC); - ao_beep(AO_BEEP_MID); - ao_panic_delay(10); - ao_led_off(AO_LED_PANIC); - ao_beep(AO_BEEP_OFF); - ao_panic_delay(10); - } - } -} diff --git a/src/core/ao_product.c b/src/core/ao_product.c deleted file mode 100644 index b9327bac..00000000 --- a/src/core/ao_product.c +++ /dev/null @@ -1,161 +0,0 @@ -/* - * Copyright © 2009 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_product.h" - -/* Defines which mark this particular AltOS product */ - -const char ao_version[AO_MAX_VERSION] = AO_iVersion_STRING; -const char ao_manufacturer[] = AO_iManufacturer_STRING; -const char ao_product[] = AO_iProduct_STRING; - -#define LE_WORD(x) ((x)&0xFF),((uint8_t) (((uint16_t) (x))>>8)) - -#if HAS_USB - -/* Maximum power in mA */ -#ifndef AO_USB_MAX_POWER -#define AO_USB_MAX_POWER 100 -#endif - -#include "ao_usb.h" -/* USB descriptors in one giant block of bytes */ -AO_ROMCONFIG_SYMBOL(0x00aa) uint8_t ao_usb_descriptors [] = -{ - /* Device descriptor */ - 0x12, - AO_USB_DESC_DEVICE, - LE_WORD(0x0110), /* bcdUSB */ - 0x02, /* bDeviceClass */ - 0x00, /* bDeviceSubClass */ - 0x00, /* bDeviceProtocol */ - AO_USB_CONTROL_SIZE, /* bMaxPacketSize */ - LE_WORD(0xFFFE), /* idVendor */ - LE_WORD(AO_idProduct_NUMBER), /* idProduct */ - LE_WORD(0x0100), /* bcdDevice */ - 0x01, /* iManufacturer */ - 0x02, /* iProduct */ - 0x03, /* iSerialNumber */ - 0x01, /* bNumConfigurations */ - - /* Configuration descriptor */ - 0x09, - AO_USB_DESC_CONFIGURATION, - LE_WORD(67), /* wTotalLength */ - 0x02, /* bNumInterfaces */ - 0x01, /* bConfigurationValue */ - 0x00, /* iConfiguration */ - 0xC0, /* bmAttributes */ - AO_USB_MAX_POWER >> 1, /* bMaxPower, 2mA units */ - - /* Control class interface */ - 0x09, - AO_USB_DESC_INTERFACE, - 0x00, /* bInterfaceNumber */ - 0x00, /* bAlternateSetting */ - 0x01, /* bNumEndPoints */ - 0x02, /* bInterfaceClass */ - 0x02, /* bInterfaceSubClass */ - 0x01, /* bInterfaceProtocol, linux requires value of 1 for the cdc_acm module */ - 0x00, /* iInterface */ - - /* Header functional descriptor */ - 0x05, - AO_USB_CS_INTERFACE, - 0x00, /* bDescriptor SubType Header */ - LE_WORD(0x0110), /* CDC version 1.1 */ - - /* Call management functional descriptor */ - 0x05, - AO_USB_CS_INTERFACE, - 0x01, /* bDescriptor SubType Call Management */ - 0x01, /* bmCapabilities = device handles call management */ - 0x01, /* bDataInterface call management interface number */ - - /* ACM functional descriptor */ - 0x04, - AO_USB_CS_INTERFACE, - 0x02, /* bDescriptor SubType Abstract Control Management */ - 0x02, /* bmCapabilities = D1 (Set_line_Coding, Set_Control_Line_State, Get_Line_Coding and Serial_State) */ - - /* Union functional descriptor */ - 0x05, - AO_USB_CS_INTERFACE, - 0x06, /* bDescriptor SubType Union Functional descriptor */ - 0x00, /* bMasterInterface */ - 0x01, /* bSlaveInterface0 */ - - /* Notification EP */ - 0x07, - AO_USB_DESC_ENDPOINT, - AO_USB_INT_EP|0x80, /* bEndpointAddress */ - 0x03, /* bmAttributes = intr */ - LE_WORD(8), /* wMaxPacketSize */ - 0xff, /* bInterval */ - - /* Data class interface descriptor */ - 0x09, - AO_USB_DESC_INTERFACE, - 0x01, /* bInterfaceNumber */ - 0x00, /* bAlternateSetting */ - 0x02, /* bNumEndPoints */ - 0x0A, /* bInterfaceClass = data */ - 0x00, /* bInterfaceSubClass */ - 0x00, /* bInterfaceProtocol */ - 0x00, /* iInterface */ - - /* Data EP OUT */ - 0x07, - AO_USB_DESC_ENDPOINT, - AO_USB_OUT_EP, /* bEndpointAddress */ - 0x02, /* bmAttributes = bulk */ - LE_WORD(AO_USB_OUT_SIZE),/* wMaxPacketSize */ - 0x00, /* bInterval */ - - /* Data EP in */ - 0x07, - AO_USB_DESC_ENDPOINT, - AO_USB_IN_EP|0x80, /* bEndpointAddress */ - 0x02, /* bmAttributes = bulk */ - LE_WORD(AO_USB_IN_SIZE),/* wMaxPacketSize */ - 0x00, /* bInterval */ - - /* String descriptors */ - 0x04, - AO_USB_DESC_STRING, - LE_WORD(0x0409), - - /* iManufacturer */ - AO_iManufacturer_LEN, - AO_USB_DESC_STRING, - AO_iManufacturer_UCS2, - - /* iProduct */ - AO_iProduct_LEN, - AO_USB_DESC_STRING, - AO_iProduct_UCS2, - - /* iSerial */ - AO_iSerial_LEN, - AO_USB_DESC_STRING, - AO_iSerial_UCS2, - - /* Terminating zero */ - 0 -}; -#endif diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c deleted file mode 100644 index e59f5bc4..00000000 --- a/src/core/ao_pyro.c +++ /dev/null @@ -1,518 +0,0 @@ -/* - * 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. - */ - -#ifndef AO_FLIGHT_TEST -#include -#include -#include -#endif -#include - -#if IS_COMPANION -#include -#define ao_accel ao_companion_command.accel -#define ao_speed ao_companion_command.speed -#define ao_height ao_companion_command.height -#define ao_flight_state ao_companion_command.flight_state -#define ao_motor_number ao_companion_command.motor_number -#endif - -#define ao_lowbit(x) ((x) & (-x)) - -#ifndef AO_FLIGHT_TEST -enum ao_igniter_status -ao_pyro_status(uint8_t p) -{ - __xdata struct ao_data packet; - __pdata int16_t value; - - ao_arch_critical( - ao_data_get(&packet); - ); - - value = (AO_IGNITER_CLOSED>>1); - value = AO_SENSE_PYRO(&packet, p); - if (value < AO_IGNITER_OPEN) - return ao_igniter_open; - else if (value > AO_IGNITER_CLOSED) - return ao_igniter_ready; - else - return ao_igniter_unknown; -} - -void -ao_pyro_print_status(void) -{ - uint8_t p; - - for(p = 0; p < AO_PYRO_NUM; p++) { - enum ao_igniter_status status = ao_pyro_status(p); - printf("Igniter: %d Status: %s\n", - p, ao_igniter_status_names[status]); - } -} -#endif - -uint16_t ao_pyro_fired; - -/* - * Given a pyro structure, figure out - * if the current flight state satisfies all - * of the requirements - */ -static uint8_t -ao_pyro_ready(struct ao_pyro *pyro) -{ - enum ao_pyro_flag flag, flags; - - flags = pyro->flags; - while (flags != ao_pyro_none) { - flag = ao_lowbit(flags); - flags &= ~flag; - switch (flag) { - - case ao_pyro_accel_less: - if (ao_accel <= pyro->accel_less) - continue; - break; - case ao_pyro_accel_greater: - if (ao_accel >= pyro->accel_greater) - continue; - break; - - - case ao_pyro_speed_less: - if (ao_speed <= pyro->speed_less) - continue; - break; - case ao_pyro_speed_greater: - if (ao_speed >= pyro->speed_greater) - continue; - break; - - case ao_pyro_height_less: - if (ao_height <= pyro->height_less) - continue; - break; - case ao_pyro_height_greater: - if (ao_height >= pyro->height_greater) - continue; - break; - -#if HAS_GYRO - case ao_pyro_orient_less: - if (ao_sample_orient <= pyro->orient_less) - continue; - break; - case ao_pyro_orient_greater: - if (ao_sample_orient >= pyro->orient_greater) - continue; - break; -#endif - - case ao_pyro_time_less: - if ((int16_t) (ao_time() - ao_boost_tick) <= pyro->time_less) - continue; - break; - case ao_pyro_time_greater: - if ((int16_t) (ao_time() - ao_boost_tick) >= pyro->time_greater) - continue; - break; - - case ao_pyro_ascending: - if (ao_speed > 0) - continue; - break; - case ao_pyro_descending: - if (ao_speed < 0) - continue; - break; - - case ao_pyro_after_motor: - if (ao_motor_number == pyro->motor) - continue; - break; - - case ao_pyro_delay: - /* handled separately */ - continue; - - case ao_pyro_state_less: - if (ao_flight_state < pyro->state_less) - continue; - break; - case ao_pyro_state_greater_or_equal: - if (ao_flight_state >= pyro->state_greater_or_equal) - continue; - break; - - default: - continue; - } - return FALSE; - } - return TRUE; -} - -#ifndef AO_FLIGHT_TEST -static void -ao_pyro_pin_set(uint8_t p, uint8_t v) -{ - switch (p) { -#if AO_PYRO_NUM > 0 - case 0: ao_gpio_set(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0, v); break; -#endif -#if AO_PYRO_NUM > 1 - case 1: ao_gpio_set(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1, v); break; -#endif -#if AO_PYRO_NUM > 2 - case 2: ao_gpio_set(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2, v); break; -#endif -#if AO_PYRO_NUM > 3 - case 3: ao_gpio_set(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3, v); break; -#endif -#if AO_PYRO_NUM > 4 - case 4: ao_gpio_set(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4, v); break; -#endif -#if AO_PYRO_NUM > 5 - case 5: ao_gpio_set(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5, v); break; -#endif -#if AO_PYRO_NUM > 6 - case 6: ao_gpio_set(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6, v); break; -#endif -#if AO_PYRO_NUM > 7 - case 7: ao_gpio_set(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, v); break; -#endif - default: break; - } -} -#endif - -uint8_t ao_pyro_wakeup; - -static void -ao_pyro_pins_fire(uint16_t fire) -{ - uint8_t p; - - for (p = 0; p < AO_PYRO_NUM; p++) { - if (fire & (1 << p)) - ao_pyro_pin_set(p, 1); - } - ao_delay(AO_MS_TO_TICKS(50)); - for (p = 0; p < AO_PYRO_NUM; p++) { - if (fire & (1 << p)) { - ao_pyro_pin_set(p, 0); - ao_config.pyro[p].fired = 1; - ao_pyro_fired |= (1 << p); - } - } - ao_delay(AO_MS_TO_TICKS(50)); -} - -static uint8_t -ao_pyro_check(void) -{ - struct ao_pyro *pyro; - uint8_t p, any_waiting; - uint16_t fire = 0; - - any_waiting = 0; - for (p = 0; p < AO_PYRO_NUM; p++) { - pyro = &ao_config.pyro[p]; - - /* Ignore igniters which have already fired - */ - if (pyro->fired) - continue; - - /* Ignore disabled igniters - */ - if (!pyro->flags) - continue; - - any_waiting = 1; - /* Check pyro state to see if it should fire - */ - if (!pyro->delay_done) { - if (!ao_pyro_ready(pyro)) - continue; - - /* If there's a delay set, then remember when - * it expires - */ - if (pyro->flags & ao_pyro_delay) { - pyro->delay_done = ao_time() + pyro->delay; - if (!pyro->delay_done) - pyro->delay_done = 1; - } - } - - /* Check to see if we're just waiting for - * the delay to expire - */ - if (pyro->delay_done) { - if ((int16_t) (ao_time() - pyro->delay_done) < 0) - continue; - } - - fire |= (1 << p); - } - - if (fire) - ao_pyro_pins_fire(fire); - - return any_waiting; -} - -#define NO_VALUE 0xff - -#define AO_PYRO_NAME_LEN 3 - -#if !DISABLE_HELP -#define ENABLE_HELP 1 -#endif - -#if ENABLE_HELP -#define HELP(s) (s) -#else -#define HELP(s) -#endif - -const struct { - char name[AO_PYRO_NAME_LEN]; - enum ao_pyro_flag flag; - uint8_t offset; -#if ENABLE_HELP - char *help; -#endif -} ao_pyro_values[] = { - { "a<", ao_pyro_accel_less, offsetof(struct ao_pyro, accel_less), HELP("accel less (m/ss * 16)") }, - { "a>", ao_pyro_accel_greater, offsetof(struct ao_pyro, accel_greater), HELP("accel greater (m/ss * 16)") }, - - { "s<", ao_pyro_speed_less, offsetof(struct ao_pyro, speed_less), HELP("speed less (m/s * 16)") }, - { "s>", ao_pyro_speed_greater, offsetof(struct ao_pyro, speed_greater), HELP("speed greater (m/s * 16)") }, - - { "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_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 - - { "t<", ao_pyro_time_less, offsetof(struct ao_pyro, time_less), HELP("time less (s * 100)") }, - { "t>", ao_pyro_time_greater, offsetof(struct ao_pyro, time_greater), HELP("time greater (s * 100)") }, - - { "f<", ao_pyro_state_less, offsetof(struct ao_pyro, state_less), HELP("state less") }, - { "f>=",ao_pyro_state_greater_or_equal, offsetof(struct ao_pyro, state_greater_or_equal), HELP("state greater or equal") }, - - { "A", ao_pyro_ascending, NO_VALUE, HELP("ascending") }, - { "D", ao_pyro_descending, NO_VALUE, HELP("descending") }, - - { "m", ao_pyro_after_motor, offsetof(struct ao_pyro, motor), HELP("after motor") }, - - { "d", ao_pyro_delay, offsetof(struct ao_pyro, delay), HELP("delay before firing (s * 100)") }, - { "", ao_pyro_none, NO_VALUE, HELP(NULL) }, -}; - -#define NUM_PYRO_VALUES (sizeof ao_pyro_values / sizeof ao_pyro_values[0]) - -#ifndef AO_FLIGHT_TEST -static void -ao_pyro(void) -{ - uint8_t any_waiting; - - ao_config_get(); - while (ao_flight_state < ao_flight_boost) - ao_sleep(&ao_flight_state); - - for (;;) { - ao_alarm(AO_MS_TO_TICKS(100)); - ao_sleep(&ao_pyro_wakeup); - ao_clear_alarm(); - if (ao_flight_state >= ao_flight_landed) - break; - any_waiting = ao_pyro_check(); - if (!any_waiting) - break; - } - ao_exit(); -} - -__xdata struct ao_task ao_pyro_task; - - -static void -ao_pyro_print_name(uint8_t v) -{ - const char *s = ao_pyro_values[v].name; - printf ("%s%s", s, " " + strlen(s)); -} - -#if ENABLE_HELP -static void -ao_pyro_help(void) -{ - uint8_t v; - for (v = 0; ao_pyro_values[v].flag != ao_pyro_none; v++) { - ao_pyro_print_name(v); - if (ao_pyro_values[v].offset != NO_VALUE) - printf (" "); - else - printf (" "); - printf ("%s\n", ao_pyro_values[v].help); - } -} -#endif - -void -ao_pyro_show(void) -{ - uint8_t p; - uint8_t v; - struct ao_pyro *pyro; - - printf ("Pyro-count: %d\n", AO_PYRO_NUM); - for (p = 0; p < AO_PYRO_NUM; p++) { - printf ("Pyro %2d: ", p); - pyro = &ao_config.pyro[p]; - if (!pyro->flags) { - printf ("\n"); - continue; - } - for (v = 0; ao_pyro_values[v].flag != ao_pyro_none; v++) { - if (!(pyro->flags & ao_pyro_values[v].flag)) - continue; - ao_pyro_print_name(v); - if (ao_pyro_values[v].offset != NO_VALUE) { - int16_t value; - - value = *((int16_t *) ((char *) pyro + ao_pyro_values[v].offset)); - printf ("%6d ", value); - } else { - printf (" "); - } - } - printf ("\n"); - } -} - -void -ao_pyro_set(void) -{ - uint8_t p; - struct ao_pyro pyro_tmp; - char name[AO_PYRO_NAME_LEN]; - uint8_t c; - uint8_t v; - - ao_cmd_white(); - -#if ENABLE_HELP - switch (ao_cmd_lex_c) { - case '?': - ao_pyro_help(); - return; - } -#endif - - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - p = ao_cmd_lex_i; - if (AO_PYRO_NUM <= p) { - printf ("invalid pyro channel %d\n", p); - return; - } - pyro_tmp.flags = 0; - for (;;) { - ao_cmd_white(); - if (ao_cmd_lex_c == '\n') - break; - - for (c = 0; c < AO_PYRO_NAME_LEN - 1; c++) { - if (ao_cmd_is_white()) - break; - name[c] = ao_cmd_lex_c; - ao_cmd_lex(); - } - name[c] = '\0'; - for (v = 0; ao_pyro_values[v].flag != ao_pyro_none; v++) { - if (!strcmp (ao_pyro_values[v].name, name)) - break; - } - if (ao_pyro_values[v].flag == ao_pyro_none) { - printf ("invalid pyro field %s\n", name); - ao_cmd_status = ao_cmd_syntax_error; - return; - } - pyro_tmp.flags |= ao_pyro_values[v].flag; - if (ao_pyro_values[v].offset != NO_VALUE) { - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - *((int16_t *) ((char *) &pyro_tmp + ao_pyro_values[v].offset)) = ao_cmd_lex_i; - } - } - _ao_config_edit_start(); - ao_config.pyro[p] = pyro_tmp; - _ao_config_edit_finish(); -} - -void -ao_pyro_manual(uint8_t p) -{ - printf ("ao_pyro_manual %d\n", p); - if (p >= AO_PYRO_NUM) { - ao_cmd_status = ao_cmd_syntax_error; - return; - } - ao_pyro_pins_fire(1 << p); -} - -void -ao_pyro_init(void) -{ -#if AO_PYRO_NUM > 0 - ao_enable_output(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0, 0); -#endif -#if AO_PYRO_NUM > 1 - ao_enable_output(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1, 0); -#endif -#if AO_PYRO_NUM > 2 - ao_enable_output(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2, 0); -#endif -#if AO_PYRO_NUM > 3 - ao_enable_output(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3, 0); -#endif -#if AO_PYRO_NUM > 4 - ao_enable_output(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4, 0); -#endif -#if AO_PYRO_NUM > 5 - ao_enable_output(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5, 0); -#endif -#if AO_PYRO_NUM > 6 - ao_enable_output(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6, 0); -#endif -#if AO_PYRO_NUM > 7 - ao_enable_output(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, 0); -#endif - ao_add_task(&ao_pyro_task, ao_pyro, "pyro"); -} -#endif diff --git a/src/core/ao_pyro.h b/src/core/ao_pyro.h deleted file mode 100644 index 0c5642d6..00000000 --- a/src/core/ao_pyro.h +++ /dev/null @@ -1,83 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_PYRO_H_ -#define _AO_PYRO_H_ - -enum ao_pyro_flag { - ao_pyro_none = 0x00000000, - - ao_pyro_accel_less = 0x00000001, - ao_pyro_accel_greater = 0x00000002, - - ao_pyro_speed_less = 0x00000004, - ao_pyro_speed_greater = 0x00000008, - - ao_pyro_height_less = 0x00000010, - ao_pyro_height_greater = 0x00000020, - - ao_pyro_orient_less = 0x00000040, - ao_pyro_orient_greater = 0x00000080, - - ao_pyro_time_less = 0x00000100, - ao_pyro_time_greater = 0x00000200, - - ao_pyro_ascending = 0x00000400, - ao_pyro_descending = 0x00000800, - - ao_pyro_after_motor = 0x00001000, - - ao_pyro_delay = 0x00002000, - - ao_pyro_state_less = 0x00004000, - ao_pyro_state_greater_or_equal = 0x00008000, -}; - -struct ao_pyro { - enum ao_pyro_flag flags; - int16_t accel_less, accel_greater; - int16_t speed_less, speed_greater; - int16_t height_less, height_greater; - int16_t orient_less, orient_greater; - int16_t time_less, time_greater; - int16_t delay; - uint8_t state_less, state_greater_or_equal; - int16_t motor; - uint16_t delay_done; - uint8_t fired; -}; - -extern uint8_t ao_pyro_wakeup; - -extern uint16_t ao_pyro_fired; - -void -ao_pyro_set(void); - -void -ao_pyro_show(void); - -void -ao_pyro_init(void); - -void -ao_pyro_manual(uint8_t p); - -void -ao_pyro_print_status(void); - -#endif diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h deleted file mode 100644 index 044f1607..00000000 --- a/src/core/ao_quaternion.h +++ /dev/null @@ -1,249 +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. - */ - -#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, - const struct ao_quaternion *a, - const struct ao_quaternion *b) -{ - struct ao_quaternion t; -#define T(_a,_b) (((a)->_a) * ((b)->_b)) - -/* - * Quaternions - * - * ii = jj = kk = ijk = -1; - * - * kji = 1; - * - * ij = k; ji = -k; - * kj = -i; jk = i; - * ik = -j; ki = j; - * - * Multiplication p * q: - * - * (pr + ipx + jpy + kpz) (qr + iqx + jqy + kqz) = - * - * ( pr * qr + pr * iqx + pr * jqy + pr * kqz) + - * (ipx * qr + ipx * iqx + ipx * jqy + ipx * kqz) + - * (jpy * qr + jpy * iqx + jpy * jqy + jpy * kqz) + - * (kpz * qr + kpz * iqx + kpz * jqy + kpz * kqz) = - * - * - * (pr * qr) + i(pr * qx) + j(pr * qy) + k(pr * qz) + - * i(px * qr) - (px * qx) + k(px * qy) - j(px * qz) + - * j(py * qr) - k(py * qx) - (py * qy) + i(py * qz) + - * k(pz * qr) + j(pz * qx) - i(pz * qy) - (pz * qz) = - * - * 1 * ( (pr * qr) - (px * qx) - (py * qy) - (pz * qz) ) + - * i * ( (pr * qx) + (px * qr) + (py * qz) - (pz * qy) ) + - * j * ( (pr * qy) - (px * qz) + (py * qr) + (pz * qx) ) + - * k * ( (pr * qz) + (px * qy) - (py * qx) + (pz * qr); - */ - - 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, - const 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(const 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, - const 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, - const 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 float ao_quaternion_dot(const struct ao_quaternion *a, - const struct ao_quaternion *b) -{ -#define T(_a) (((a)->_a) * ((b)->_a)) - return T(r) + T(x) + T(y) + T(z); -#undef T -} - - -static inline void ao_quaternion_rotate(struct ao_quaternion *r, - const struct ao_quaternion *a, - const struct ao_quaternion *b) -{ - struct ao_quaternion c; - struct ao_quaternion t; - - ao_quaternion_multiply(&t, b, a); - ao_quaternion_conjugate(&c, b); - ao_quaternion_multiply(r, &t, &c); -} - -/* - * Compute a rotation quaternion between two vectors - * - * cos(θ) + u * sin(θ) - * - * where θ is the angle between the two vectors and u - * is a unit vector axis of rotation - */ - -static inline void ao_quaternion_vectors_to_rotation(struct ao_quaternion *r, - const struct ao_quaternion *a, - const struct ao_quaternion *b) -{ - /* - * The cross product will point orthogonally to the two - * vectors, forming our rotation axis. The length will be - * sin(θ), so these values are already multiplied by that. - */ - - float x = a->y * b->z - a->z * b->y; - float y = a->z * b->x - a->x * b->z; - float z = a->x * b->y - a->y * b->x; - - float s_2 = x*x + y*y + z*z; - float s = sqrtf(s_2); - - /* cos(θ) = a · b / (|a| |b|). - * - * a and b are both unit vectors, so the divisor is one - */ - float c = a->x*b->x + a->y*b->y + a->z*b->z; - - float c_half = sqrtf ((1 + c) / 2); - float s_half = sqrtf ((1 - c) / 2); - - /* - * Divide out the sine factor from the - * cross product, then multiply in the - * half sine factor needed for the quaternion - */ - float s_scale = s_half / s; - - r->x = x * s_scale; - r->y = y * s_scale; - r->z = z * s_scale; - - r->r = c_half; - - ao_quaternion_normalize(r, r); -} - -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; -} - -/* - * The sincosf from newlib just calls sinf and cosf. This is a bit - * faster, if slightly less precise - */ - -static inline void -ao_sincosf(float a, float *s, float *c) { - float _s = sinf(a); - *s = _s; - *c = sqrtf(1 - _s*_s); -} - -/* - * Initialize a quaternion from 1/2 euler rotation angles (in radians). - * - * Yes, it would be nicer if there were a faster way, but because we - * sample the gyros at only 100Hz, we end up getting angles too large - * to take advantage of sin(x) ≃ x. - * - * We might be able to use just a couple of elements of the sin taylor - * series though, instead of the whole sin function? - */ - -static inline void ao_quaternion_init_half_euler(struct ao_quaternion *r, - float x, float y, float z) -{ - float s_x, c_x; - float s_y, c_y; - float s_z, c_z; - - ao_sincosf(x, &s_x, &c_x); - ao_sincosf(y, &s_y, &c_y); - ao_sincosf(z, &s_z, &c_z); - - r->r = c_x * c_y * c_z + s_x * s_y * s_z; - r->x = s_x * c_y * c_z - c_x * s_y * s_z; - r->y = c_x * s_y * c_z + s_x * c_y * s_z; - r->z = c_x * c_y * s_z - s_x * s_y * c_z; -} - -#endif /* _AO_QUATERNION_H_ */ diff --git a/src/core/ao_radio_cmac.c b/src/core/ao_radio_cmac.c deleted file mode 100644 index bff848f6..00000000 --- a/src/core/ao_radio_cmac.c +++ /dev/null @@ -1,164 +0,0 @@ -/* - * 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 -#include - -static __xdata uint8_t ao_radio_cmac_mutex; -__pdata int8_t ao_radio_cmac_rssi; -static __xdata uint8_t cmac_data[AO_CMAC_MAX_LEN + AO_CMAC_KEY_LEN + 2 + AO_CMAC_KEY_LEN]; - -static uint8_t -round_len(uint8_t len) -{ - uint8_t rem; - - /* Make sure we transfer at least one packet, and - * then make sure every packet is full. Note that - * there is no length encoded, and that the receiver - * must deal with any extra bytes in the packet - */ - if (len < AO_CMAC_KEY_LEN) - len = AO_CMAC_KEY_LEN; - rem = len % AO_CMAC_KEY_LEN; - if (rem != 0) - len += (AO_CMAC_KEY_LEN - rem); - return len; -} - -/* - * Sign and deliver the data sitting in the cmac buffer - */ -static void -radio_cmac_send(uint8_t len) __reentrant -{ - uint8_t i; - - len = round_len(len); - /* Make sure the AES key is loaded */ - ao_config_get(); - -#if HAS_MONITOR - ao_monitor_set(0); -#endif - - ao_mutex_get(&ao_aes_mutex); - ao_aes_set_mode(ao_aes_mode_cbc_mac); - ao_aes_set_key(ao_config.aes_key); - ao_aes_zero_iv(); - for (i = 0; i < len; i += AO_CMAC_KEY_LEN) { - if (i + AO_CMAC_KEY_LEN < len) - ao_aes_run(&cmac_data[i], NULL); - else - ao_aes_run(&cmac_data[i], &cmac_data[len]); - } - ao_mutex_put(&ao_aes_mutex); - - ao_radio_send(cmac_data, len + AO_CMAC_KEY_LEN); -} - -/* - * Receive and validate an incoming packet - */ - -static int8_t -radio_cmac_recv(uint8_t len, uint16_t timeout) __reentrant -{ - uint8_t i; - - len = round_len(len); -#if HAS_MONITOR - ao_monitor_set(0); -#endif - i = ao_radio_recv(cmac_data, len + AO_CMAC_KEY_LEN + 2, timeout); - - if (!i) { - ao_radio_cmac_rssi = 0; - return AO_RADIO_CMAC_TIMEOUT; - } - - ao_radio_cmac_rssi = ao_radio_rssi; - if (!(cmac_data[len + AO_CMAC_KEY_LEN +1] & AO_RADIO_STATUS_CRC_OK)) - return AO_RADIO_CMAC_CRC_ERROR; - - ao_config_get(); - - /* Compute the packet signature - */ - ao_mutex_get(&ao_aes_mutex); - ao_aes_set_mode(ao_aes_mode_cbc_mac); - ao_aes_set_key(ao_config.aes_key); - ao_aes_zero_iv(); - for (i = 0; i < len; i += AO_CMAC_KEY_LEN) { - if (i + AO_CMAC_KEY_LEN < len) - ao_aes_run(&cmac_data[i], NULL); - else - ao_aes_run(&cmac_data[i], &cmac_data[len + AO_CMAC_KEY_LEN + 2]); - } - ao_mutex_put(&ao_aes_mutex); - - /* Check the packet signature against the signature provided - * over the link - */ - - if (memcmp(&cmac_data[len], - &cmac_data[len + AO_CMAC_KEY_LEN + 2], - AO_CMAC_KEY_LEN) != 0) { - return AO_RADIO_CMAC_MAC_ERROR; - } - - return AO_RADIO_CMAC_OK; -} - -int8_t -ao_radio_cmac_send(__xdata void *packet, uint8_t len) __reentrant -{ - if (len > AO_CMAC_MAX_LEN) - return AO_RADIO_CMAC_LEN_ERROR; - ao_mutex_get(&ao_radio_cmac_mutex); - ao_xmemcpy(cmac_data, packet, len); -#if AO_LED_TX - ao_led_on(AO_LED_TX); -#endif - radio_cmac_send(len); -#if AO_LED_TX - ao_led_off(AO_LED_TX); -#endif - ao_mutex_put(&ao_radio_cmac_mutex); - return AO_RADIO_CMAC_OK; -} - -int8_t -ao_radio_cmac_recv(__xdata void *packet, uint8_t len, uint16_t timeout) __reentrant -{ - int8_t i; - if (len > AO_CMAC_MAX_LEN) - return AO_RADIO_CMAC_LEN_ERROR; - ao_mutex_get(&ao_radio_cmac_mutex); -#if AO_LED_RX - ao_led_on(AO_LED_RX); -#endif - i = radio_cmac_recv(len, timeout); -#if AO_LED_RX - ao_led_off(AO_LED_RX); -#endif - if (i == AO_RADIO_CMAC_OK) - ao_xmemcpy(packet, cmac_data, len); - ao_mutex_put(&ao_radio_cmac_mutex); - return i; -} - diff --git a/src/core/ao_radio_cmac.h b/src/core/ao_radio_cmac.h deleted file mode 100644 index e86f31e9..00000000 --- a/src/core/ao_radio_cmac.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_RADIO_CMAC_H_ -#define _AO_RADIO_CMAC_H_ - -#include - -#define AO_CMAC_KEY_LEN AO_AES_LEN -#define AO_CMAC_MAX_LEN (128 - AO_CMAC_KEY_LEN) - -extern __pdata int8_t ao_radio_cmac_rssi; - -int8_t -ao_radio_cmac_send(__xdata void *packet, uint8_t len) __reentrant; - -#define AO_RADIO_CMAC_OK 0 -#define AO_RADIO_CMAC_LEN_ERROR -1 -#define AO_RADIO_CMAC_CRC_ERROR -2 -#define AO_RADIO_CMAC_MAC_ERROR -3 -#define AO_RADIO_CMAC_TIMEOUT -4 - -int8_t -ao_radio_cmac_recv(__xdata void *packet, uint8_t len, uint16_t timeout) __reentrant; - -void -ao_radio_cmac_init(void); - -#endif /* _AO_RADIO_CMAC_H_ */ diff --git a/src/core/ao_radio_cmac_cmd.c b/src/core/ao_radio_cmac_cmd.c deleted file mode 100644 index 64410921..00000000 --- a/src/core/ao_radio_cmac_cmd.c +++ /dev/null @@ -1,104 +0,0 @@ -/* - * 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 - -static __xdata uint8_t cmac_data[AO_CMAC_MAX_LEN]; - -static uint8_t -getnibble(void) -{ - int8_t b; - - b = ao_cmd_hexchar(getchar()); - if (b < 0) { - ao_cmd_status = ao_cmd_lex_error; - return 0; - } - return (uint8_t) b; -} - -static uint8_t -getbyte(void) -{ - uint8_t b; - b = getnibble() << 4; - b |= getnibble(); - return b; -} - -static void -radio_cmac_send_cmd(void) __reentrant -{ - uint8_t i; - uint8_t len; - - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - len = ao_cmd_lex_i; - if (len > AO_CMAC_MAX_LEN) { - ao_cmd_status = ao_cmd_syntax_error; - return; - } - flush(); - len = ao_cmd_lex_i; - for (i = 0; i < len; i++) { - cmac_data[i] = getbyte(); - if (ao_cmd_status != ao_cmd_success) - return; - } - ao_radio_cmac_send(cmac_data, len); -} - -static void -radio_cmac_recv_cmd(void) __reentrant -{ - uint8_t len, i; - uint16_t timeout; - - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - len = ao_cmd_lex_i; - ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - timeout = AO_MS_TO_TICKS(ao_cmd_lex_i); - i = ao_radio_cmac_recv(cmac_data, len, timeout); - if (i == AO_RADIO_CMAC_OK) { - printf ("PACKET "); - for (i = 0; i < len; i++) - printf("%02x", cmac_data[i]); - printf (" %d\n", ao_radio_cmac_rssi); - } else - printf ("ERROR %d %d\n", i, ao_radio_cmac_rssi); -} - -static __code struct ao_cmds ao_radio_cmac_cmds[] = { - { radio_cmac_send_cmd, "s \0Send AES-CMAC packet. Bytes to send follow on next line" }, - { radio_cmac_recv_cmd, "S \0Receive AES-CMAC packet. Timeout in ms" }, - { 0, NULL }, -}; - -void -ao_radio_cmac_cmd_init(void) -{ - ao_cmd_register(&ao_radio_cmac_cmds[0]); -} diff --git a/src/core/ao_radio_cmac_cmd.h b/src/core/ao_radio_cmac_cmd.h deleted file mode 100644 index 6b8782de..00000000 --- a/src/core/ao_radio_cmac_cmd.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_RADIO_CMAC_CMD_H_ -#define _AO_RADIO_CMAC_CMD_H_ - -void -ao_radio_cmac_cmd_init(void); - -#endif /* _AO_RADIO_CMAC_CMD_H_ */ diff --git a/src/core/ao_report.c b/src/core/ao_report.c deleted file mode 100644 index 1104cd82..00000000 --- a/src/core/ao_report.c +++ /dev/null @@ -1,198 +0,0 @@ -/* - * Copyright © 2009 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 -#include - -#define BIT(i,x) ((x) ? (1 << (i)) : 0) -#define MORSE1(a) (1 | BIT(3,a)) -#define MORSE2(a,b) (2 | BIT(3,a) | BIT(4,b)) -#define MORSE3(a,b,c) (3 | BIT(3,a) | BIT(4,b) | BIT(5,c)) -#define MORSE4(a,b,c,d) (4 | BIT(3,a) | BIT(4,b) | BIT(5,c) | BIT(6,d)) -#define MORSE5(a,b,c,d,e) (5 | BIT(3,a) | BIT(4,b) | BIT(5,c) | BIT(6,d) | BIT(7,e)) - -static const uint8_t flight_reports[] = { - MORSE3(0,0,0), /* startup, 'S' */ - MORSE2(0,0), /* idle 'I' */ - MORSE4(0,1,1,0), /* pad 'P' */ - MORSE4(1,0,0,0), /* boost 'B' */ - MORSE4(0,0,1,0), /* fast 'F' */ - MORSE4(1,0,1,0), /* coast 'C' */ - MORSE3(1,0,0), /* drogue 'D' */ - MORSE2(1,1), /* main 'M' */ - MORSE4(0,1,0,0), /* landed 'L' */ - MORSE4(1,0,0,1), /* invalid 'X' */ -}; - -#if HAS_BEEP -#define low(time) ao_beep_for(AO_BEEP_LOW, time) -#define mid(time) ao_beep_for(AO_BEEP_MID, time) -#define high(time) ao_beep_for(AO_BEEP_HIGH, time) -#else -#define low(time) ao_led_for(AO_LED_GREEN, time) -#define mid(time) ao_led_for(AO_LED_RED, time) -#define high(time) ao_led_for(AO_LED_GREEN|AO_LED_RED, time) -#endif -#define pause(time) ao_delay(time) - -static __pdata enum ao_flight_state ao_report_state; - -static void -ao_report_beep(void) __reentrant -{ - uint8_t r = flight_reports[ao_flight_state]; - uint8_t l = r & 7; - - if (!r) - return; - while (l--) { - if (r & 8) - mid(AO_MS_TO_TICKS(600)); - else - mid(AO_MS_TO_TICKS(200)); - pause(AO_MS_TO_TICKS(200)); - r >>= 1; - } - pause(AO_MS_TO_TICKS(400)); -} - -static void -ao_report_digit(uint8_t digit) __reentrant -{ - if (!digit) { - mid(AO_MS_TO_TICKS(500)); - pause(AO_MS_TO_TICKS(200)); - } else { - while (digit--) { - mid(AO_MS_TO_TICKS(200)); - pause(AO_MS_TO_TICKS(200)); - } - } - pause(AO_MS_TO_TICKS(300)); -} - -static void -ao_report_altitude(void) -{ - __pdata int16_t agl = ao_max_height; - __xdata uint8_t digits[10]; - __pdata uint8_t ndigits, i; - - if (agl < 0) - agl = 0; - ndigits = 0; - do { - digits[ndigits++] = agl % 10; - agl /= 10; - } while (agl); - - i = ndigits; - do - ao_report_digit(digits[--i]); - while (i != 0); -} - -#if HAS_IGNITE_REPORT -static uint8_t -ao_report_igniter_ready(enum ao_igniter igniter) -{ - return ao_igniter_status(igniter) == ao_igniter_ready ? 1 : 0; -} - -uint8_t -ao_report_igniter(void) -{ - return (ao_report_igniter_ready(ao_igniter_drogue) | - (ao_report_igniter_ready(ao_igniter_main) << 1)); -} - -static void -ao_report_continuity(void) __reentrant -{ - uint8_t c; - -#if !HAS_IGNITE - if (!ao_igniter_present) - return; -#endif - c = ao_report_igniter(); - if (c) { - while (c--) { - high(AO_MS_TO_TICKS(25)); - pause(AO_MS_TO_TICKS(100)); - } - } else { - c = 10; - while (c--) { - high(AO_MS_TO_TICKS(20)); - low(AO_MS_TO_TICKS(20)); - } - } -#if HAS_LOG - if (ao_log_full()) { - pause(AO_MS_TO_TICKS(100)); - c = 2; - while (c--) { - low(AO_MS_TO_TICKS(100)); - mid(AO_MS_TO_TICKS(100)); - high(AO_MS_TO_TICKS(100)); - mid(AO_MS_TO_TICKS(100)); - } - } -#endif -} -#endif - -void -ao_report(void) -{ - ao_report_state = ao_flight_state; - for(;;) { - ao_report_beep(); - if (ao_flight_state == ao_flight_landed) { - ao_report_altitude(); -#if HAS_FLIGHT - ao_delay(AO_SEC_TO_TICKS(5)); - continue; -#endif - } -#if HAS_IGNITE_REPORT - if (ao_flight_state == ao_flight_idle) - ao_report_continuity(); - while (ao_flight_state == ao_flight_pad) { - uint8_t c; - ao_report_continuity(); - c = 50; - while (c-- && ao_flight_state == ao_flight_pad) - pause(AO_MS_TO_TICKS(100)); - } -#endif - - while (ao_report_state == ao_flight_state) - ao_sleep(DATA_TO_XDATA(&ao_flight_state)); - ao_report_state = ao_flight_state; - } -} - -static __xdata struct ao_task ao_report_task; - -void -ao_report_init(void) -{ - ao_add_task(&ao_report_task, ao_report, "report"); -} diff --git a/src/core/ao_report_micro.c b/src/core/ao_report_micro.c deleted file mode 100644 index 0e8e287f..00000000 --- a/src/core/ao_report_micro.c +++ /dev/null @@ -1,57 +0,0 @@ -/* - * 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 - -#define mid(time) ao_led_for(AO_LED_REPORT, time) -#define pause(time) ao_delay(time) - -static void -ao_report_digit(uint8_t digit) __reentrant -{ - if (!digit) { - mid(AO_MS_TO_TICKS(1000)); - pause(AO_MS_TO_TICKS(300)); - } else { - while (digit--) { - mid(AO_MS_TO_TICKS(300)); - pause(AO_MS_TO_TICKS(300)); - } - } - pause(AO_MS_TO_TICKS(1000)); -} - -void -ao_report_altitude(void) -{ - __pdata alt_t agl = ao_max_height; - static __xdata uint8_t digits[11]; - __pdata uint8_t ndigits, i; - - if (agl < 0) - agl = 0; - ndigits = 0; - do { - digits[ndigits++] = agl % 10; - agl /= 10; - } while (agl); - - i = ndigits; - do - ao_report_digit(digits[--i]); - while (i != 0); -} diff --git a/src/core/ao_rssi.c b/src/core/ao_rssi.c deleted file mode 100644 index 244a84fe..00000000 --- a/src/core/ao_rssi.c +++ /dev/null @@ -1,53 +0,0 @@ -/* - * Copyright © 2009 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" - -static __xdata uint16_t ao_rssi_time; -static __pdata uint16_t ao_rssi_delay; -static __pdata uint8_t ao_rssi_led; - -void -ao_rssi(void) -{ - for (;;) { - while ((int16_t) (ao_time() - ao_rssi_time) > AO_SEC_TO_TICKS(3)) - ao_sleep(&ao_rssi_time); - ao_led_for(ao_rssi_led, AO_MS_TO_TICKS(100)); - ao_delay(ao_rssi_delay); - } -} - -void -ao_rssi_set(int rssi_value) -{ - if (rssi_value > 0) - rssi_value = 0; - ao_rssi_delay = AO_MS_TO_TICKS((-rssi_value) * 5); - ao_rssi_time = ao_time(); - ao_wakeup(&ao_rssi_time); -} - -__xdata struct ao_task ao_rssi_task; - -void -ao_rssi_init(uint8_t rssi_led) -{ - ao_rssi_led = rssi_led; - ao_rssi_delay = 0; - ao_add_task(&ao_rssi_task, ao_rssi, "rssi"); -} diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c deleted file mode 100644 index 34658951..00000000 --- a/src/core/ao_sample.c +++ /dev/null @@ -1,372 +0,0 @@ -/* - * 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. - */ - -#ifndef AO_FLIGHT_TEST -#include "ao.h" -#include -#endif - -#if HAS_GYRO -#include -#endif - -/* - * Current sensor values - */ - -#ifndef PRES_TYPE -#define PRES_TYPE int32_t -#define ALT_TYPE int32_t -#define ACCEL_TYPE int16_t -#endif - -__pdata uint16_t ao_sample_tick; /* time of last data */ -__pdata pres_t ao_sample_pres; -__pdata alt_t ao_sample_alt; -__pdata alt_t ao_sample_height; -#if HAS_ACCEL -__pdata accel_t ao_sample_accel; -#endif -#if HAS_GYRO -__pdata accel_t ao_sample_accel_along; -__pdata accel_t ao_sample_accel_across; -__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_orient; -#endif - -__data uint8_t ao_sample_data; - -/* - * Sensor calibration values - */ - -__pdata pres_t ao_ground_pres; /* startup pressure */ -__pdata alt_t ao_ground_height; /* MSL of ao_ground_pres */ - -#if HAS_ACCEL -__pdata accel_t ao_ground_accel; /* startup acceleration */ -__pdata accel_t ao_accel_2g; /* factory accel calibration */ -__pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */ -#endif - -#if HAS_GYRO -__pdata accel_t ao_ground_accel_along; -__pdata accel_t ao_ground_accel_across; -__pdata accel_t ao_ground_accel_through; -__pdata int32_t ao_ground_pitch; -__pdata int32_t ao_ground_yaw; -__pdata int32_t ao_ground_roll; -#endif - -static __pdata uint8_t ao_preflight; /* in preflight mode */ - -static __pdata uint16_t nsamples; -__pdata int32_t ao_sample_pres_sum; -#if HAS_ACCEL -__pdata int32_t ao_sample_accel_sum; -#endif -#if HAS_GYRO -__pdata int32_t ao_sample_accel_along_sum; -__pdata int32_t ao_sample_accel_across_sum; -__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; -#endif - -#if HAS_FLIGHT_DEBUG -extern uint8_t ao_orient_test; -#endif - -static void -ao_sample_preflight_add(void) -{ -#if HAS_ACCEL - ao_sample_accel_sum += ao_sample_accel; -#endif - ao_sample_pres_sum += ao_sample_pres; -#if HAS_GYRO - ao_sample_accel_along_sum += ao_sample_accel_along; - ao_sample_accel_across_sum += ao_sample_accel_across; - ao_sample_accel_through_sum += ao_sample_accel_through; - ao_sample_pitch_sum += ao_sample_pitch; - ao_sample_yaw_sum += ao_sample_yaw; - ao_sample_roll_sum += ao_sample_roll; -#endif - ++nsamples; -} - -static void -ao_sample_preflight_set(void) -{ -#if HAS_ACCEL - ao_ground_accel = ao_sample_accel_sum >> 9; - ao_sample_accel_sum = 0; -#endif - ao_ground_pres = ao_sample_pres_sum >> 9; - ao_ground_height = pres_to_altitude(ao_ground_pres); - ao_sample_pres_sum = 0; -#if HAS_GYRO - ao_ground_accel_along = ao_sample_accel_along_sum >> 9; - ao_ground_accel_across = ao_sample_accel_across_sum >> 9; - ao_ground_accel_through = ao_sample_accel_through_sum >> 9; - ao_ground_pitch = ao_sample_pitch_sum; - ao_ground_yaw = ao_sample_yaw_sum; - ao_ground_roll = ao_sample_roll_sum; - ao_sample_accel_along_sum = 0; - ao_sample_accel_across_sum = 0; - ao_sample_accel_through_sum = 0; - ao_sample_pitch_sum = 0; - ao_sample_yaw_sum = 0; - ao_sample_roll_sum = 0; - ao_sample_orient = 0; - - struct ao_quaternion orient; - - /* Take the pad IMU acceleration values and compute our current direction - */ - - ao_quaternion_init_vector(&orient, - (ao_ground_accel_across - ao_config.accel_zero_across), - (ao_ground_accel_through - ao_config.accel_zero_through), - (ao_ground_accel_along - ao_config.accel_zero_along)); - - ao_quaternion_normalize(&orient, - &orient); - - /* Here's up */ - - struct ao_quaternion up = { .r = 0, .x = 0, .y = 0, .z = 1 }; - - if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP) - up.z = -1; - - /* Compute rotation to get from up to our current orientation, set - * that as the current rotation vector - */ - ao_quaternion_vectors_to_rotation(&ao_rotation, &up, &orient); -#if HAS_FLIGHT_DEBUG - if (ao_orient_test) - printf("\n\treset\n"); -#endif -#endif - nsamples = 0; -} - -#if HAS_GYRO - -#define TIME_DIV 200.0f - -static void -ao_sample_rotate(void) -{ -#ifdef AO_FLIGHT_TEST - float dt = (ao_sample_tick - ao_sample_prev_tick) / TIME_DIV; -#else - static const float dt = 1/TIME_DIV; -#endif - float x = ao_mpu6000_gyro((float) ((ao_sample_pitch << 9) - ao_ground_pitch) / 512.0f) * dt; - float y = ao_mpu6000_gyro((float) ((ao_sample_yaw << 9) - ao_ground_yaw) / 512.0f) * dt; - float z = ao_mpu6000_gyro((float) ((ao_sample_roll << 9) - ao_ground_roll) / 512.0f) * dt; - struct ao_quaternion rot; - - ao_quaternion_init_half_euler(&rot, x, y, z); - ao_quaternion_multiply(&ao_rotation, &rot, &ao_rotation); - - /* 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. - * - * rot = ao_rotation * vertical * ao_rotation° - * rot = ao_rotation * (0,0,0,1) * ao_rotation° - * = ((a.z, a.y, -a.x, a.r) * (a.r, -a.x, -a.y, -a.z)) .z - * - * = (-a.z * -a.z) + (a.y * -a.y) - (-a.x * -a.x) + (a.r * a.r) - * = a.z² - a.y² - a.x² + a.r² - * - * rot = ao_rotation * (0, 0, 0, -1) * ao_rotation° - * = ((-a.z, -a.y, a.x, -a.r) * (a.r, -a.x, -a.y, -a.z)) .z - * - * = (a.z * -a.z) + (-a.y * -a.y) - (a.x * -a.x) + (-a.r * a.r) - * = -a.z² + a.y² + a.x² - a.r² - */ - - float rotz; - rotz = ao_rotation.z * ao_rotation.z - ao_rotation.y * ao_rotation.y - ao_rotation.x * ao_rotation.x + ao_rotation.r * ao_rotation.r; - - ao_sample_orient = acosf(rotz) * (float) (180.0/M_PI); - -#if HAS_FLIGHT_DEBUG - if (ao_orient_test) { - printf ("rot %d %d %d orient %d \r", - (int) (x * 1000), - (int) (y * 1000), - (int) (z * 1000), - ao_sample_orient); - } -#endif - -} -#endif - -static void -ao_sample_preflight(void) -{ - /* startup state: - * - * Collect 512 samples of acceleration and pressure - * data and average them to find the resting values - */ - if (nsamples < 512) { - ao_sample_preflight_add(); - } else { -#if HAS_ACCEL - ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g; - ao_accel_scale = to_fix32(GRAVITY * 2 * 16) / ao_accel_2g; -#endif - ao_sample_preflight_set(); - ao_preflight = FALSE; - } -} - -/* - * While in pad mode, constantly update the ground state by - * re-averaging the data. This tracks changes in orientation, which - * might be caused by adjustments to the rocket on the pad and - * pressure, which might be caused by changes in the weather. - */ - -static void -ao_sample_preflight_update(void) -{ - if (nsamples < 512) - ao_sample_preflight_add(); - else if (nsamples < 1024) - ++nsamples; - else - ao_sample_preflight_set(); -} - -#if 0 -#if HAS_GYRO -static int32_t p_filt; -static int32_t y_filt; - -static gyro_t inline ao_gyro(void) { - gyro_t p = ao_sample_pitch - ao_ground_pitch; - gyro_t y = ao_sample_yaw - ao_ground_yaw; - - p_filt = p_filt - (p_filt >> 6) + p; - y_filt = y_filt - (y_filt >> 6) + y; - - p = p_filt >> 6; - y = y_filt >> 6; - return ao_sqrt(p*p + y*y); -} -#endif -#endif - -uint8_t -ao_sample(void) -{ - ao_wakeup(DATA_TO_XDATA(&ao_sample_data)); - ao_sleep((void *) DATA_TO_XDATA(&ao_data_head)); - while (ao_sample_data != ao_data_head) { - __xdata struct ao_data *ao_data; - - /* Capture a sample */ - ao_data = (struct ao_data *) &ao_data_ring[ao_sample_data]; - ao_sample_tick = ao_data->tick; - -#if HAS_BARO - ao_data_pres_cook(ao_data); - ao_sample_pres = ao_data_pres(ao_data); - ao_sample_alt = pres_to_altitude(ao_sample_pres); - ao_sample_height = ao_sample_alt - ao_ground_height; -#endif - -#if HAS_ACCEL - ao_sample_accel = ao_data_accel_cook(ao_data); - if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP) - ao_sample_accel = ao_data_accel_invert(ao_sample_accel); - ao_data_set_accel(ao_data, ao_sample_accel); -#endif -#if HAS_GYRO - ao_sample_accel_along = ao_data_along(ao_data); - ao_sample_accel_across = ao_data_across(ao_data); - ao_sample_accel_through = ao_data_through(ao_data); - ao_sample_pitch = ao_data_pitch(ao_data); - ao_sample_yaw = ao_data_yaw(ao_data); - ao_sample_roll = ao_data_roll(ao_data); -#endif - - if (ao_preflight) - ao_sample_preflight(); - else { - if (ao_flight_state < ao_flight_boost) - ao_sample_preflight_update(); - ao_kalman(); -#if HAS_GYRO - 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; -} - -void -ao_sample_init(void) -{ - ao_config_get(); - nsamples = 0; - ao_sample_pres_sum = 0; - ao_sample_pres = 0; -#if HAS_ACCEL - ao_sample_accel_sum = 0; - ao_sample_accel = 0; -#endif -#if HAS_GYRO - ao_sample_accel_along_sum = 0; - ao_sample_accel_across_sum = 0; - ao_sample_accel_through_sum = 0; - ao_sample_accel_along = 0; - ao_sample_accel_across = 0; - ao_sample_accel_through = 0; - ao_sample_pitch_sum = 0; - ao_sample_yaw_sum = 0; - ao_sample_roll_sum = 0; - ao_sample_pitch = 0; - ao_sample_yaw = 0; - ao_sample_roll = 0; - ao_sample_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 deleted file mode 100644 index 16d4c507..00000000 --- a/src/core/ao_sample.h +++ /dev/null @@ -1,156 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_SAMPLE_H_ -#define _AO_SAMPLE_H_ - -#include - -/* - * ao_sample.c - */ - -/* - * Barometer calibration - * - * We directly sample the barometer. The specs say: - * - * Pressure range: 15-115 kPa - * Voltage at 115kPa: 2.82 - * Output scale: 27mV/kPa - * - * If we want to detect launch with the barometer, we need - * a large enough bump to not be fooled by noise. At typical - * launch elevations (0-2000m), a 200Pa pressure change cooresponds - * to about a 20m elevation change. This is 5.4mV, or about 3LSB. - * As all of our calculations are done in 16 bits, we'll actually see a change - * of 16 times this though - * - * 27 mV/kPa * 32767 / 3300 counts/mV = 268.1 counts/kPa - */ - -/* Accelerometer calibration - * - * We're sampling the accelerometer through a resistor divider which - * consists of 5k and 10k resistors. This multiplies the values by 2/3. - * That goes into the cc1111 A/D converter, which is running at 11 bits - * of precision with the bits in the MSB of the 16 bit value. Only positive - * values are used, so values should range from 0-32752 for 0-3.3V. The - * specs say we should see 40mV/g (uncalibrated), multiply by 2/3 for what - * the A/D converter sees (26.67 mV/g). We should see 32752/3300 counts/mV, - * for a final computation of: - * - * 26.67 mV/g * 32767/3300 counts/mV = 264.8 counts/g - * - * Zero g was measured at 16000 (we would expect 16384). - * Note that this value is only require to tell if the - * rocket is standing upright. Once that is determined, - * the value of the accelerometer is averaged for 100 samples - * to find the resting accelerometer value, which is used - * for all further flight computations - */ - -/* - * Above this height, the baro sensor doesn't work - */ -#if HAS_MS5607 -#define AO_MAX_BARO_HEIGHT 30000 -#else -#define AO_MAX_BARO_HEIGHT 12000 -#endif - -/* - * Above this speed, baro measurements are unreliable - */ -#define AO_MAX_BARO_SPEED 200 - -#define ACCEL_NOSE_UP (ao_accel_2g >> 2) - -/* - * Speed and acceleration are scaled by 16 to provide a bit more - * resolution while still having reasonable range. Note that this - * limits speed to 2047m/s (around mach 6) and acceleration to - * 2047m/s² (over 200g) - */ - -#define AO_M_TO_HEIGHT(m) ((int16_t) (m)) -#define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16)) -#define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16)) - -extern __pdata uint16_t ao_sample_tick; /* time of last data */ -extern __data uint8_t ao_sample_adc; /* Ring position of last processed sample */ -extern __data uint8_t ao_sample_data; /* Ring position of last processed sample */ - -#if HAS_BARO -extern __pdata pres_t ao_sample_pres; /* most recent pressure sensor reading */ -extern __pdata alt_t ao_sample_alt; /* MSL of ao_sample_pres */ -extern __pdata alt_t ao_sample_height; /* AGL of ao_sample_pres */ -extern __pdata pres_t ao_ground_pres; /* startup pressure */ -extern __pdata alt_t ao_ground_height; /* MSL of ao_ground_pres */ -#endif - -#if HAS_ACCEL -extern __pdata accel_t ao_sample_accel; /* most recent accel sensor reading */ -extern __pdata accel_t ao_ground_accel; /* startup acceleration */ -extern __pdata accel_t ao_accel_2g; /* factory accel calibration */ -extern __pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */ -#endif -#if HAS_GYRO -extern __pdata accel_t ao_ground_accel_along; -extern __pdata accel_t ao_ground_accel_across; -extern __pdata accel_t ao_ground_accel_through; -extern __pdata int32_t ao_ground_pitch; /* * 512 */ -extern __pdata int32_t ao_ground_yaw; /* * 512 */ -extern __pdata int32_t ao_ground_roll; /* * 512 */ -extern __pdata accel_t ao_sample_accel_along; -extern __pdata accel_t ao_sample_accel_across; -extern __pdata accel_t ao_sample_accel_through; -extern __pdata gyro_t ao_sample_roll; -extern __pdata gyro_t ao_sample_pitch; -extern __pdata gyro_t ao_sample_yaw; -extern __pdata angle_t ao_sample_orient; -#endif - -void ao_sample_init(void); - -/* returns FALSE in preflight mode, TRUE in flight mode */ -uint8_t ao_sample(void); - -/* - * ao_kalman.c - */ - -#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) -#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) -#define from_fix(x) ((x) >> 16) - -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 __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; - -#if HAS_ACCEL -extern __pdata int16_t ao_error_a; -#endif - -void ao_kalman(void); - -#endif /* _AO_SAMPLE_H_ */ diff --git a/src/core/ao_sample_profile.c b/src/core/ao_sample_profile.c deleted file mode 100644 index d3743d12..00000000 --- a/src/core/ao_sample_profile.c +++ /dev/null @@ -1,173 +0,0 @@ -/* - * 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 - -#ifndef AO_SAMPLE_PROFILE_LOW_PC -#define AO_SAMPLE_PROFILE_LOW_PC 0x08002000 -#endif - -#ifndef AO_SAMPLE_PROFILE_HIGH_PC -#define AO_SAMPLE_PROFILE_HIGH_PC 0x0800f000 -#endif - -#ifndef AO_SAMPLE_PROFILE_SHIFT -#define AO_SAMPLE_PROFILE_SHIFT 6 -#endif - -#define AO_SAMPLE_PROFILE_RANGE (AO_SAMPLE_PROFILE_HIGH_PC - AO_SAMPLE_PROFILE_LOW_PC) -#define AO_SAMPLE_PROFILE_NUM (AO_SAMPLE_PROFILE_RANGE >> AO_SAMPLE_PROFILE_SHIFT) - -static uint16_t prev_tick; -static uint16_t samples[AO_SAMPLE_PROFILE_NUM]; -static uint8_t missed[AO_SAMPLE_PROFILE_NUM/8]; -static uint16_t max_miss; -static uint32_t task, isr, os, idle; - -extern uint8_t ao_idle_loc; - -void -ao_sample_profile_point(uint32_t pc, uint16_t tick, uint8_t in_isr) -{ - uint16_t delta = tick - prev_tick; - - if (pc < AO_SAMPLE_PROFILE_LOW_PC) - return; - if (pc >= AO_SAMPLE_PROFILE_HIGH_PC) - return; - if (ao_cur_task) { - uint8_t *sp; - int32_t sp_delta; - - asm("mov %0,sp" : "=&r" (sp)); - sp_delta = sp - (uint8_t *) ao_cur_task->stack; - if (-96 < sp_delta && sp_delta < 16) - ao_panic(AO_PANIC_STACK); - } - - if (in_isr) - isr += delta; - else if (ao_cur_task) { - ao_cur_task->ticks += delta; - task += delta; - } else if (pc == (uint32_t) &ao_idle_loc) - idle += delta; - else - os += delta; - - pc -= AO_SAMPLE_PROFILE_LOW_PC; - pc >>= AO_SAMPLE_PROFILE_SHIFT; - samples[pc] += delta; - - if (delta > 1) - missed[pc >> 3] |= (1 << (pc & 7)); - if (delta > max_miss) - max_miss = delta; - prev_tick = tick; -} - -static void -ao_sample_profile_start(void) -{ - prev_tick = ao_sample_profile_timer_start(); -} - -static void -ao_sample_profile_stop(void) -{ - ao_sample_profile_timer_stop(); -} - -static void -ao_sample_profile_dump(void) -{ - uint16_t a; - uint8_t t; - - printf ("task %6d\n", task); - printf ("isr %6d\n", isr); - printf ("os %6d\n", os); - printf ("idle %6d\n", idle); - printf ("irq blocked %d\n", max_miss); - for (t = 0; t < ao_num_tasks; t++) - printf ("task %6d %6d %6d %s\n", - ao_tasks[t]->ticks, - ao_tasks[t]->yields, - ao_tasks[t]->max_run, - ao_tasks[t]->name); - for (a = 0; a < AO_SAMPLE_PROFILE_NUM; a++) { - if (samples[a]) - printf ("%04x %c %u\n", - (a << AO_SAMPLE_PROFILE_SHIFT) + AO_SAMPLE_PROFILE_LOW_PC, - missed[a >> 3] & (1 << (a & 7)) ? '*' : ' ', - samples[a]); - } -} - -static void -ao_sample_profile_clear(void) -{ - int t; - - task = isr = os = idle = 0; - max_miss = 0; - memset(samples, '\0', sizeof (samples)); - memset(missed, '\0', sizeof (missed)); - for (t = 0; t < ao_num_tasks; t++) { - ao_tasks[t]->ticks = 0; - ao_tasks[t]->yields = 0; - ao_tasks[t]->max_run = 0; - } -} - -static void -ao_sample_profile_cmd(void) -{ - ao_cmd_white(); - switch (ao_cmd_lex_c) { - case '1': - ao_sample_profile_start(); - break; - case '0': - ao_sample_profile_stop(); - break; - case 'd': - ao_sample_profile_dump(); - break; - case 'c': - ao_sample_profile_clear(); - break; - default: - ao_cmd_status = ao_cmd_syntax_error; - break; - } -} - -static __code struct ao_cmds ao_sample_profile_cmds[] = { - { ao_sample_profile_cmd, "S <1 start,0 stop, d dump,c clear>\0Sample profile" }, - { 0, NULL } -}; - -void -ao_sample_profile_init(void) -{ - ao_sample_profile_timer_init(); - ao_cmd_register(&ao_sample_profile_cmds[0]); - ao_sample_profile_clear(); -} diff --git a/src/core/ao_sample_profile.h b/src/core/ao_sample_profile.h deleted file mode 100644 index dbc29d3d..00000000 --- a/src/core/ao_sample_profile.h +++ /dev/null @@ -1,29 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_SAMPLE_PROFILE_H_ -#define _AO_SAMPLE_PROFILE_H_ - -#include - -void -ao_sample_profile_point(uint32_t pc, uint16_t tick, uint8_t in_isr); - -void -ao_sample_profile_init(void); - -#endif /* _AO_SAMPLE_PROFILE_H_ */ diff --git a/src/core/ao_send_packet.c b/src/core/ao_send_packet.c deleted file mode 100644 index 66315d22..00000000 --- a/src/core/ao_send_packet.c +++ /dev/null @@ -1,58 +0,0 @@ -/* - * 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 "ao.h" - -#define AO_MAX_SEND 128 - -static __xdata uint8_t ao_send[AO_MAX_SEND]; - -static void -ao_send_packet(void) -{ - __pdata uint16_t count; - uint8_t b; - __pdata uint8_t i; - - ao_cmd_hex(); - count = ao_cmd_lex_i; - if (ao_cmd_status != ao_cmd_success) - return; - if (count > AO_MAX_SEND - 2) { - ao_cmd_status = ao_cmd_syntax_error; - return; - } - for (i = 0; i < count; i++) { - b = ao_getnibble() << 4; - b |= ao_getnibble(); - if (ao_cmd_status != ao_cmd_success) - return; - ao_send[i] = b; - } - ao_radio_send(ao_send, count); -} - -static __code struct ao_cmds ao_send_packet_cmds[] = { - { ao_send_packet, "S \0Send packet. Data on next line" }, - { 0, NULL } -}; - -void -ao_send_packet_init(void) -{ - ao_cmd_register(&ao_send_packet_cmds[0]); -} diff --git a/src/core/ao_send_packet.h b/src/core/ao_send_packet.h deleted file mode 100644 index 526f7b55..00000000 --- a/src/core/ao_send_packet.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_SEND_PACKET_H_ -#define _AO_SEND_PACKET_H_ - -void -ao_send_packet_init(void); - -#endif /* _AO_SEND_PACKET_H_ */ diff --git a/src/core/ao_serial.h b/src/core/ao_serial.h deleted file mode 100644 index baf213c0..00000000 --- a/src/core/ao_serial.h +++ /dev/null @@ -1,110 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_SERIAL_H_ -#define _AO_SERIAL_H_ - -#define AO_SERIAL_SPEED_4800 0 -#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; -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); - -void -ao_serial0_drain(void); - -void -ao_serial0_set_speed(uint8_t speed); -#endif - -#if HAS_SERIAL_1 -extern volatile __xdata struct ao_fifo ao_serial1_rx_fifo; -extern volatile __xdata struct ao_fifo ao_serial1_tx_fifo; - -char -ao_serial1_getchar(void); - -int -_ao_serial1_pollchar(void); - -void -ao_serial1_putchar(char c); - -void -ao_serial1_drain(void); - -void -ao_serial1_set_speed(uint8_t speed); -#endif - -#if HAS_SERIAL_2 -extern volatile __xdata struct ao_fifo ao_serial2_rx_fifo; -extern volatile __xdata struct ao_fifo ao_serial2_tx_fifo; - -char -ao_serial2_getchar(void); - -int -_ao_serial2_pollchar(void); - -void -ao_serial2_putchar(char c); - -void -ao_serial2_drain(void); - -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); - -#endif /* _AO_SERIAL_H_ */ diff --git a/src/core/ao_sqrt.c b/src/core/ao_sqrt.c deleted file mode 100644 index 3a550eaa..00000000 --- a/src/core/ao_sqrt.c +++ /dev/null @@ -1,48 +0,0 @@ -/* - * 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. - */ - -#ifndef AO_FLIGHT_TEST -#include "ao.h" -#endif - -/* Adapted from int_sqrt.c in the linux kernel, which is licensed GPLv2 */ -/** - * int_sqrt - rough approximation to sqrt - * @x: integer of which to calculate the sqrt - * - * A very rough approximation to the sqrt() function. - */ - -uint32_t -ao_sqrt(uint32_t op) -{ - uint32_t res = 0; - uint32_t one = 1UL << (sizeof (one) * 8 - 2); - - while (one > op) - one >>= 2; - - while (one != 0) { - if (op >= res + one) { - op = op - (res + one); - res = res + 2 * one; - } - res /= 2; - one /= 4; - } - return res; -} diff --git a/src/core/ao_state.c b/src/core/ao_state.c deleted file mode 100644 index ed197aa5..00000000 --- a/src/core/ao_state.c +++ /dev/null @@ -1,23 +0,0 @@ -/* - * Copyright © 2009 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" - -const char const * const ao_state_names[] = { - "startup", "idle", "pad", "boost", "fast", - "coast", "drogue", "main", "landed", "invalid" -}; diff --git a/src/core/ao_stdio.c b/src/core/ao_stdio.c deleted file mode 100644 index 99118137..00000000 --- a/src/core/ao_stdio.c +++ /dev/null @@ -1,158 +0,0 @@ -/* - * Copyright © 2009 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" - -/* - * Basic I/O functions to support SDCC stdio package - */ - -#ifndef USE_SERIAL_0_STDIN -#define USE_SERIAL_0_STDIN 0 -#endif -#ifndef USE_SERIAL_1_STDIN -#define USE_SERIAL_1_STDIN 0 -#endif -#ifndef USE_SERIAL_2_STDIN -#define USE_SERIAL_2_STDIN 0 -#endif -#ifndef USE_SERIAL_3_STDIN -#define USE_SERIAL_3_STDIN 0 -#endif -#ifndef USE_SERIAL_4_STDIN -#define USE_SERIAL_4_STDIN 0 -#endif -#ifndef USE_SERIAL_5_STDIN -#define USE_SERIAL_5_STDIN 0 -#endif -#ifndef USE_SERIAL_6_STDIN -#define USE_SERIAL_6_STDIN 0 -#endif -#ifndef USE_SERIAL_7_STDIN -#define USE_SERIAL_7_STDIN 0 -#endif -#ifndef USE_SERIAL_8_STDIN -#define USE_SERIAL_8_STDIN 0 -#endif -#ifndef USE_SERIAL_9_STDIN -#define USE_SERIAL_9_STDIN 0 -#endif -#ifndef PACKET_HAS_SLAVE -#define PACKET_HAS_SLAVE 0 -#endif - -#define USE_SERIAL_STDIN (USE_SERIAL_0_STDIN + \ - USE_SERIAL_1_STDIN + \ - USE_SERIAL_2_STDIN + \ - USE_SERIAL_3_STDIN + \ - USE_SERIAL_4_STDIN + \ - USE_SERIAL_5_STDIN + \ - USE_SERIAL_6_STDIN + \ - USE_SERIAL_7_STDIN + \ - USE_SERIAL_8_STDIN + \ - USE_SERIAL_9_STDIN) - -#define AO_NUM_STDIOS (HAS_USB + PACKET_HAS_SLAVE + USE_SERIAL_STDIN) - -__xdata struct ao_stdio ao_stdios[AO_NUM_STDIOS]; - -#if AO_NUM_STDIOS > 1 -__pdata int8_t ao_cur_stdio; -__pdata int8_t ao_num_stdios; -#else -__pdata int8_t ao_cur_stdio; -#define ao_cur_stdio 0 -#define ao_num_stdios 0 -#endif - -void -putchar(char c) -{ -#if LOW_LEVEL_DEBUG - if (!ao_cur_task) { - extern void ao_debug_out(char c); - if (c == '\n') - ao_debug_out('\r'); - ao_debug_out(c); - return; - } -#endif - if (c == '\n') - (*ao_stdios[ao_cur_stdio].putchar)('\r'); - (*ao_stdios[ao_cur_stdio].putchar)(c); -} - -void -flush(void) -{ - if (ao_stdios[ao_cur_stdio].flush) - ao_stdios[ao_cur_stdio].flush(); -} - -__xdata uint8_t ao_stdin_ready; - -char -getchar(void) __reentrant -{ - int c; - int8_t stdio; - - ao_arch_block_interrupts(); - stdio = ao_cur_stdio; - for (;;) { - c = ao_stdios[stdio]._pollchar(); - if (c != AO_READ_AGAIN) - break; -#if AO_NUM_STDIOS > 1 - if (++stdio == ao_num_stdios) - stdio = 0; - if (stdio == ao_cur_stdio) -#endif - ao_sleep(&ao_stdin_ready); - } -#if AO_NUM_STDIOS > 1 - ao_cur_stdio = stdio; -#endif - ao_arch_release_interrupts(); - return c; -} - -uint8_t -ao_echo(void) -{ - return ao_stdios[ao_cur_stdio].echo; -} - -int8_t -ao_add_stdio(int (*_pollchar)(void), - void (*putchar)(char), - void (*flush)(void)) __reentrant -{ -#if AO_NUM_STDIOS > 1 - if (ao_num_stdios == AO_NUM_STDIOS) - ao_panic(AO_PANIC_STDIO); -#endif - ao_stdios[ao_num_stdios]._pollchar = _pollchar; - ao_stdios[ao_num_stdios].putchar = putchar; - ao_stdios[ao_num_stdios].flush = flush; - ao_stdios[ao_num_stdios].echo = 1; -#if AO_NUM_STDIOS > 1 - return ao_num_stdios++; -#else - return 0; -#endif -} diff --git a/src/core/ao_storage.c b/src/core/ao_storage.c deleted file mode 100644 index 6eddae7f..00000000 --- a/src/core/ao_storage.c +++ /dev/null @@ -1,186 +0,0 @@ -/* - * 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 -#include - -uint8_t -ao_storage_read(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant -{ - uint16_t this_len; - uint16_t this_off; - - ao_storage_setup(); - if (pos >= ao_storage_total || pos + len > ao_storage_total) - return 0; - while (len) { - - /* Compute portion of transfer within - * a single block - */ - this_off = (uint16_t) pos & (ao_storage_unit - 1); - this_len = ao_storage_unit - this_off; - if (this_len > len) - this_len = len; - - if (!ao_storage_device_read(pos, buf, this_len)) - return 0; - - /* See how much is left */ - buf += this_len; - len -= this_len; - pos += this_len; - } - return 1; -} - -uint8_t -ao_storage_write(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant -{ - uint16_t this_len; - uint16_t this_off; - - ao_storage_setup(); - if (pos >= ao_storage_total || pos + len > ao_storage_total) - return 0; - while (len) { - - /* Compute portion of transfer within - * a single block - */ - this_off = (uint16_t) pos & (ao_storage_unit - 1); - this_len = ao_storage_unit - this_off; - if (this_len > len) - this_len = len; - - if (!ao_storage_device_write(pos, buf, this_len)) - return 0; - - /* See how much is left */ - buf += this_len; - len -= this_len; - pos += this_len; - } - return 1; -} - -static __xdata uint8_t storage_data[8]; - -static void -ao_storage_dump(void) __reentrant -{ - uint8_t i, j; - - ao_cmd_hex(); - if (ao_cmd_status != ao_cmd_success) - return; - for (i = 0; ; i += 8) { - if (ao_storage_read(((uint32_t) (ao_cmd_lex_i) << 8) + i, - storage_data, - 8)) { - ao_cmd_put16((uint16_t) i); - for (j = 0; j < 8; j++) { - putchar(' '); - ao_cmd_put8(storage_data[j]); - } - putchar ('\n'); - } - if (i == 248) - break; - } -} - -#if HAS_STORAGE_DEBUG - -/* not enough space for this today - */ -static void -ao_storage_store(void) __reentrant -{ - uint16_t block; - uint8_t i; - uint16_t len; - static __xdata uint8_t b; - uint32_t addr; - - ao_cmd_hex(); - block = ao_cmd_lex_i; - ao_cmd_hex(); - i = ao_cmd_lex_i; - addr = ((uint32_t) block << 8) | i; - ao_cmd_hex(); - len = ao_cmd_lex_i; - if (ao_cmd_status != ao_cmd_success) - return; - while (len--) { - ao_cmd_hex(); - if (ao_cmd_status != ao_cmd_success) - return; - b = ao_cmd_lex_i; - ao_storage_write(addr, &b, 1); - addr++; - } -} -#endif - -void -ao_storage_zap(void) __reentrant -{ - ao_cmd_hex(); - if (ao_cmd_status != ao_cmd_success) - return; - ao_storage_erase((uint32_t) ao_cmd_lex_i << 8); -} - -void -ao_storage_zapall(void) __reentrant -{ - uint32_t pos; - - ao_cmd_white(); - if (!ao_match_word("DoIt")) - return; - for (pos = 0; pos < ao_storage_log_max; pos += ao_storage_block) - ao_storage_erase(pos); -} - -void -ao_storage_info(void) __reentrant -{ - ao_storage_setup(); - printf("Storage size: %ld\n", (long) ao_storage_total); - printf("Storage erase unit: %ld\n", (long) ao_storage_block); - ao_storage_device_info(); -} - -__code struct ao_cmds ao_storage_cmds[] = { - { ao_storage_info, "f\0Show storage" }, - { ao_storage_dump, "e \0Dump flash" }, -#if HAS_STORAGE_DEBUG - { ao_storage_store, "w ...\0Write data to flash" }, -#endif - { ao_storage_zap, "z \0Erase " }, - { ao_storage_zapall,"Z \0Erase all. is doit with D&I" }, - { 0, NULL }, -}; - -void -ao_storage_init(void) -{ - ao_storage_device_init(); - ao_cmd_register(&ao_storage_cmds[0]); -} diff --git a/src/core/ao_storage.h b/src/core/ao_storage.h deleted file mode 100644 index 6cc6fcb7..00000000 --- a/src/core/ao_storage.h +++ /dev/null @@ -1,98 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_STORAGE_H_ -#define _AO_STORAGE_H_ - -/* - * Storage interface, provided by one of the eeprom or flash - * drivers - */ - -#ifndef ao_storage_pos_t -#define ao_storage_pos_t uint32_t -#endif - -typedef ao_storage_pos_t ao_pos_t; - -/* Total bytes of available storage */ -extern __pdata ao_pos_t ao_storage_total; - -/* Block size - device is erased in these units. At least 256 bytes */ -extern __pdata ao_pos_t ao_storage_block; - -#ifndef USE_STORAGE_CONFIG -#define USE_STORAGE_CONFIG 1 -#endif - -#if USE_STORAGE_CONFIG -/* Byte offset of config block. Will be ao_storage_block bytes long */ -extern __pdata ao_pos_t ao_storage_config; - -#define ao_storage_log_max ao_storage_config -#else -#define ao_storage_log_max ao_storage_total -#endif - -/* Storage unit size - device reads and writes must be within blocks of this size. Usually 256 bytes. */ -extern __pdata uint16_t ao_storage_unit; - -/* Initialize above values. Can only be called once the OS is running */ -void -ao_storage_setup(void) __reentrant; - -/* Write data. Returns 0 on failure, 1 on success */ -uint8_t -ao_storage_write(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant; - -/* Read data. Returns 0 on failure, 1 on success */ -uint8_t -ao_storage_read(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant; - -/* Erase a block of storage. This always clears ao_storage_block bytes */ -uint8_t -ao_storage_erase(ao_pos_t pos) __reentrant; - -/* Flush any pending writes to stable storage */ -void -ao_storage_flush(void) __reentrant; - -/* Initialize the storage code */ -void -ao_storage_init(void); - -/* - * Low-level functions wrapped by ao_storage.c - */ - -/* Read data within a storage unit */ -uint8_t -ao_storage_device_read(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant; - -/* Write data within a storage unit */ -uint8_t -ao_storage_device_write(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant; - -/* Initialize low-level device bits */ -void -ao_storage_device_init(void); - -/* Print out information about flash chips */ -void -ao_storage_device_info(void) __reentrant; - -#endif /* _AO_STORAGE_H_ */ diff --git a/src/core/ao_task.c b/src/core/ao_task.c deleted file mode 100644 index bafb4943..00000000 --- a/src/core/ao_task.c +++ /dev/null @@ -1,548 +0,0 @@ -/* - * Copyright © 2009 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 -#if HAS_SAMPLE_PROFILE -#include -#endif -#if HAS_STACK_GUARD -#include -#endif - -#define DEBUG 0 - -#define AO_NO_TASK_INDEX 0xff - -__xdata struct ao_task * __xdata ao_tasks[AO_NUM_TASKS]; -__data uint8_t ao_num_tasks; -__xdata struct ao_task *__data ao_cur_task; - -#if !HAS_TASK_QUEUE -static __data uint8_t ao_cur_task_index; -#endif - -#ifdef ao_arch_task_globals -ao_arch_task_globals -#endif - -#define AO_CHECK_STACK 0 - -#if AO_CHECK_STACK -static uint8_t in_yield; - -static inline void ao_check_stack(void) { - uint8_t q; - if (!in_yield && ao_cur_task && &q < &ao_cur_task->stack[0]) - ao_panic(AO_PANIC_STACK); -} -#else -#define ao_check_stack() -#endif - -#if HAS_TASK_QUEUE - -#define SLEEP_HASH_SIZE 17 - -static struct ao_list run_queue; -static struct ao_list alarm_queue; -static struct ao_list sleep_queue[SLEEP_HASH_SIZE]; - -static void -ao_task_to_run_queue(struct ao_task *task) -{ - ao_list_del(&task->queue); - ao_list_append(&task->queue, &run_queue); -} - -static struct ao_list * -ao_task_sleep_queue(void *wchan) -{ - return &sleep_queue[(uintptr_t) wchan % SLEEP_HASH_SIZE]; -} - -static void -ao_task_to_sleep_queue(struct ao_task *task, void *wchan) -{ - ao_list_del(&task->queue); - ao_list_append(&task->queue, ao_task_sleep_queue(wchan)); -} - -#if DEBUG -static void -ao_task_validate_alarm_queue(void) -{ - struct ao_task *alarm, *prev = NULL; - int i; - - if (ao_list_is_empty(&alarm_queue)) - return; - ao_list_for_each_entry(alarm, &alarm_queue, struct ao_task, alarm_queue) { - if (prev) { - if ((int16_t) (alarm->alarm - prev->alarm) < 0) { - ao_panic(1); - } - } - prev = alarm; - } - for (i = 0; i < ao_num_tasks; i++) { - alarm = ao_tasks[i]; - if (alarm->alarm) { - if (ao_list_is_empty(&alarm->alarm_queue)) - ao_panic(2); - } else { - if (!ao_list_is_empty(&alarm->alarm_queue)) - ao_panic(3); - } - } - if (ao_task_alarm_tick != ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm) - ao_panic(4); -} -#else -#define ao_task_validate_alarm_queue() -#endif - -uint16_t ao_task_alarm_tick; - -static void -ao_task_to_alarm_queue(struct ao_task *task) -{ - struct ao_task *alarm; - ao_list_for_each_entry(alarm, &alarm_queue, struct ao_task, alarm_queue) { - if ((int16_t) (alarm->alarm - task->alarm) >= 0) { - ao_list_insert(&task->alarm_queue, alarm->alarm_queue.prev); - ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm; - ao_task_validate_alarm_queue(); - return; - } - } - ao_list_append(&task->alarm_queue, &alarm_queue); - ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm; - ao_task_validate_alarm_queue(); -} - -static void -ao_task_from_alarm_queue(struct ao_task *task) -{ - ao_list_del(&task->alarm_queue); - if (ao_list_is_empty(&alarm_queue)) - ao_task_alarm_tick = 0; - else - ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm; - ao_task_validate_alarm_queue(); -} - -static void -ao_task_init_queue(struct ao_task *task) -{ - ao_list_init(&task->queue); - ao_list_init(&task->alarm_queue); -} - -static void -ao_task_exit_queue(struct ao_task *task) -{ - ao_list_del(&task->queue); - ao_list_del(&task->alarm_queue); -} - -void -ao_task_check_alarm(uint16_t tick) -{ - struct ao_task *alarm, *next; - - ao_list_for_each_entry_safe(alarm, next, &alarm_queue, struct ao_task, alarm_queue) { - if ((int16_t) (tick - alarm->alarm) < 0) - break; - alarm->alarm = 0; - ao_task_from_alarm_queue(alarm); - ao_task_to_run_queue(alarm); - } -} - -void -ao_task_init(void) -{ - uint8_t i; - ao_list_init(&run_queue); - ao_list_init(&alarm_queue); - ao_task_alarm_tick = 0; - for (i = 0; i < SLEEP_HASH_SIZE; i++) - ao_list_init(&sleep_queue[i]); -} - -#if DEBUG -static uint8_t -ao_task_validate_queue(struct ao_task *task) -{ - uint32_t flags; - struct ao_task *m; - uint8_t ret = 0; - struct ao_list *queue; - - flags = ao_arch_irqsave(); - if (task->wchan) { - queue = ao_task_sleep_queue(task->wchan); - ret |= 2; - } else { - queue = &run_queue; - ret |= 4; - } - ao_list_for_each_entry(m, queue, struct ao_task, queue) { - if (m == task) { - ret |= 1; - break; - } - } - ao_arch_irqrestore(flags); - return ret; -} - -static uint8_t -ao_task_validate_alarm(struct ao_task *task) -{ - uint32_t flags; - struct ao_task *m; - uint8_t ret = 0; - - flags = ao_arch_irqsave(); - if (task->alarm == 0) - return 0xff; - ao_list_for_each_entry(m, &alarm_queue, struct ao_task, alarm_queue) { - if (m == task) - ret |= 1; - else { - if (!(ret&1)) { - if ((int16_t) (m->alarm - task->alarm) > 0) - ret |= 2; - } else { - if ((int16_t) (task->alarm - m->alarm) > 0) - ret |= 4; - } - } - } - ao_arch_irqrestore(flags); - return ret; -} - - -static void -ao_task_validate(void) -{ - uint8_t i; - struct ao_task *task; - uint8_t ret; - - for (i = 0; i < ao_num_tasks; i++) { - task = ao_tasks[i]; - ret = ao_task_validate_queue(task); - if (!(ret & 1)) { - if (ret & 2) - printf ("sleeping task not on sleep queue %s %08x\n", - task->name, task->wchan); - else - printf ("running task not on run queue %s\n", - task->name); - } - ret = ao_task_validate_alarm(task); - if (ret != 0xff) { - if (!(ret & 1)) - printf ("alarm task not on alarm queue %s %d\n", - task->name, task->alarm); - if (ret & 2) - printf ("alarm queue has sooner entries after %s %d\n", - task->name, task->alarm); - if (ret & 4) - printf ("alarm queue has later entries before %s %d\n", - task->name, task->alarm); - } - } -} -#endif /* DEBUG */ - -#endif /* HAS_TASK_QUEUE */ - -void -ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *name) __reentrant -{ - uint8_t task_id; - uint8_t t; - if (ao_num_tasks == AO_NUM_TASKS) - ao_panic(AO_PANIC_NO_TASK); - for (task_id = 1; task_id != 0; task_id++) { - for (t = 0; t < ao_num_tasks; t++) - if (ao_tasks[t]->task_id == task_id) - break; - if (t == ao_num_tasks) - break; - } - task->task_id = task_id; - task->name = name; - task->wchan = NULL; - /* - * Construct a stack frame so that it will 'return' - * to the start of the task - */ - ao_arch_init_stack(task, start); - ao_arch_critical( -#if HAS_TASK_QUEUE - ao_task_init_queue(task); - ao_task_to_run_queue(task); -#endif - ao_tasks[ao_num_tasks] = task; - ao_num_tasks++; - ); -} - -__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 -{ - ao_arch_save_regs(); - -#if HAS_TASK_QUEUE - if (ao_cur_task == NULL) - ao_cur_task = ao_tasks[ao_num_tasks-1]; -#else - if (ao_cur_task_index == AO_NO_TASK_INDEX) - ao_cur_task_index = ao_num_tasks-1; -#endif - else - { -#if HAS_SAMPLE_PROFILE - uint16_t tick = ao_sample_profile_timer_value(); - uint16_t run = tick - ao_cur_task->start; - if (run > ao_cur_task->max_run) - ao_cur_task->max_run = run; - ++ao_cur_task->yields; -#endif - ao_arch_save_stack(); - } - - ao_arch_isr_stack(); -#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; -#endif - /* Find a task to run. If there isn't any runnable task, - * this loop will run forever, which is just fine - */ -#if HAS_TASK_QUEUE - /* If the current task is running, move it to the - * end of the queue to allow other tasks a chance - */ - if (ao_cur_task->wchan == NULL) - ao_task_to_run_queue(ao_cur_task); - ao_cur_task = NULL; - for (;;) { - ao_arch_memory_barrier(); - if (!ao_list_is_empty(&run_queue)) - break; - /* Wait for interrupts when there's nothing ready */ - ao_arch_wait_interrupt(); - } - ao_cur_task = ao_list_first_entry(&run_queue, struct ao_task, queue); -#else - { - __pdata uint8_t ao_last_task_index = ao_cur_task_index; - for (;;) { - ++ao_cur_task_index; - if (ao_cur_task_index == ao_num_tasks) - ao_cur_task_index = 0; - - ao_cur_task = ao_tasks[ao_cur_task_index]; - - /* Check for ready task */ - if (ao_cur_task->wchan == NULL) - break; - - /* Check if the alarm is set for a time which has passed */ - if (ao_cur_task->alarm && - (int16_t) (ao_time() - ao_cur_task->alarm) >= 0) - break; - - /* Wait for interrupts when there's nothing ready */ - if (ao_cur_task_index == ao_last_task_index && !ao_task_minimize_latency) - ao_arch_wait_interrupt(); - } - } -#endif -#if HAS_SAMPLE_PROFILE - ao_cur_task->start = ao_sample_profile_timer_value(); -#endif -#if HAS_STACK_GUARD - ao_mpu_stack_guard(ao_cur_task->stack); -#endif -#if AO_CHECK_STACK - in_yield = 0; -#endif - ao_arch_restore_stack(); -} - -uint8_t -ao_sleep(__xdata void *wchan) -{ -#if HAS_TASK_QUEUE - uint32_t flags; - flags = ao_arch_irqsave(); -#endif - ao_cur_task->wchan = wchan; -#if HAS_TASK_QUEUE - ao_task_to_sleep_queue(ao_cur_task, wchan); - ao_arch_irqrestore(flags); -#endif - ao_yield(); - if (ao_cur_task->wchan) { - ao_cur_task->wchan = NULL; - ao_cur_task->alarm = 0; - return 1; - } - return 0; -} - -void -ao_wakeup(__xdata void *wchan) __reentrant -{ -#if HAS_TASK_QUEUE - struct ao_task *sleep, *next; - struct ao_list *sleep_queue; - uint32_t flags; - - if (ao_num_tasks == 0) - return; - sleep_queue = ao_task_sleep_queue(wchan); - flags = ao_arch_irqsave(); - ao_list_for_each_entry_safe(sleep, next, sleep_queue, struct ao_task, queue) { - if (sleep->wchan == wchan) { - sleep->wchan = NULL; - ao_task_to_run_queue(sleep); - } - } - ao_arch_irqrestore(flags); -#else - uint8_t i; - for (i = 0; i < ao_num_tasks; i++) - if (ao_tasks[i]->wchan == wchan) - ao_tasks[i]->wchan = NULL; -#endif - ao_check_stack(); -} - -void -ao_alarm(uint16_t delay) -{ -#if HAS_TASK_QUEUE - uint32_t flags; - /* Make sure we sleep *at least* delay ticks, which means adding - * one to account for the fact that we may be close to the next tick - */ - flags = ao_arch_irqsave(); -#endif - if (!(ao_cur_task->alarm = ao_time() + delay + 1)) - ao_cur_task->alarm = 1; -#if HAS_TASK_QUEUE - ao_task_to_alarm_queue(ao_cur_task); - ao_arch_irqrestore(flags); -#endif -} - -void -ao_clear_alarm(void) -{ -#if HAS_TASK_QUEUE - uint32_t flags; - - flags = ao_arch_irqsave(); -#endif - ao_cur_task->alarm = 0; -#if HAS_TASK_QUEUE - ao_task_from_alarm_queue(ao_cur_task); - ao_arch_irqrestore(flags); -#endif -} - -static __xdata uint8_t ao_forever; - -void -ao_delay(uint16_t ticks) -{ - ao_alarm(ticks); - ao_sleep(&ao_forever); - ao_clear_alarm(); -} - -void -ao_exit(void) -{ - uint8_t i; - ao_arch_block_interrupts(); - ao_num_tasks--; -#if HAS_TASK_QUEUE - for (i = 0; i < ao_num_tasks; i++) - if (ao_tasks[i] == ao_cur_task) - break; - ao_task_exit_queue(ao_cur_task); -#else - i = ao_cur_task_index; - ao_cur_task_index = AO_NO_TASK_INDEX; -#endif - for (; i < ao_num_tasks; i++) - ao_tasks[i] = ao_tasks[i+1]; - ao_cur_task = NULL; - ao_yield(); - /* we'll never get back here */ -} - -#if HAS_TASK_INFO -void -ao_task_info(void) -{ - uint8_t i; - __xdata struct ao_task *task; - - for (i = 0; i < ao_num_tasks; i++) { - task = ao_tasks[i]; - printf("%12s: wchan %04x\n", - task->name, - (int) task->wchan); - } -#if HAS_TASK_QUEUE && DEBUG - ao_task_validate(); -#endif -} -#endif - -void -ao_start_scheduler(void) -{ -#if !HAS_TASK_QUEUE - ao_cur_task_index = AO_NO_TASK_INDEX; -#endif - ao_cur_task = NULL; -#if HAS_ARCH_START_SCHEDULER - ao_arch_start_scheduler(); -#endif - ao_yield(); -} diff --git a/src/core/ao_task.h b/src/core/ao_task.h deleted file mode 100644 index 9c56b480..00000000 --- a/src/core/ao_task.h +++ /dev/null @@ -1,117 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_TASK_H_ -#define _AO_TASK_H_ -#if HAS_TASK_QUEUE -#include -#endif - -#ifndef HAS_TASK_INFO -#define HAS_TASK_INFO 1 -#endif - -/* An AltOS task */ -struct ao_task { - __xdata void *wchan; /* current wait channel (NULL if running) */ - uint16_t alarm; /* abort ao_sleep time */ - ao_arch_task_members /* any architecture-specific fields */ - uint8_t task_id; /* unique id */ - __code char *name; /* task name */ -#if HAS_TASK_QUEUE - struct ao_list queue; - struct ao_list alarm_queue; -#endif - uint8_t stack[AO_STACK_SIZE]; /* saved stack */ -#if HAS_SAMPLE_PROFILE - uint32_t ticks; - uint32_t yields; - uint16_t start; - uint16_t max_run; -#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]; -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 - */ - -/* Suspend the current task until wchan is awoken. - * returns: - * 0 on normal wake - * 1 on alarm - */ -uint8_t -ao_sleep(__xdata void *wchan); - -/* Wake all tasks sleeping on wchan */ -void -ao_wakeup(__xdata void *wchan) __reentrant; - -/* set an alarm to go off in 'delay' ticks */ -void -ao_alarm(uint16_t delay); - -/* Clear any pending alarm */ -void -ao_clear_alarm(void); - -/* Yield the processor to another task */ -void -ao_yield(void) ao_arch_naked_declare; - -/* Add a task to the run queue */ -void -ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *name) __reentrant; - -#if HAS_TASK_QUEUE -/* Called on timer interrupt to check alarms */ -extern uint16_t ao_task_alarm_tick; -void -ao_task_check_alarm(uint16_t tick); -#endif - -/* Terminate the current task */ -void -ao_exit(void); - -/* Dump task info to console */ -void -ao_task_info(void); - -/* Start the scheduler. This will not return */ -void -ao_start_scheduler(void); - -#if HAS_TASK_QUEUE -void -ao_task_init(void); -#else -#define ao_task_init() -#endif - -#endif diff --git a/src/core/ao_telem.h b/src/core/ao_telem.h deleted file mode 100644 index 1a8da291..00000000 --- a/src/core/ao_telem.h +++ /dev/null @@ -1,172 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_TELEM_H_ -#define _AO_TELEM_H_ - -#define AO_TELEMETRY_VERSION 4 - -/* - * Telemetry version 4 and higher format: - * - * General header fields - * - * Name Value - * - * VERSION Telemetry version number (4 or more). Must be first. - * c Callsign (string, no spaces allowed) - * n Flight unit serial number (integer) - * f Flight number (integer) - * r Packet RSSI value (integer) - * s Flight computer state (string, no spaces allowed) - * t Flight computer clock (integer in centiseconds) - */ - -#define AO_TELEM_VERSION "VERSION" -#define AO_TELEM_CALL "c" -#define AO_TELEM_SERIAL "n" -#define AO_TELEM_FLIGHT "f" -#define AO_TELEM_RSSI "r" -#define AO_TELEM_STATE "s" -#define AO_TELEM_TICK "t" - -/* - * Raw sensor values - * - * Name Value - * r_a Accelerometer reading (integer) - * r_b Barometer reading (integer) - * r_t Thermometer reading (integer) - * r_v Battery reading (integer) - * r_d Drogue continuity (integer) - * r_m Main continuity (integer) - */ - -#define AO_TELEM_RAW_ACCEL "r_a" -#define AO_TELEM_RAW_BARO "r_b" -#define AO_TELEM_RAW_THERMO "r_t" -#define AO_TELEM_RAW_BATT "r_v" -#define AO_TELEM_RAW_DROGUE "r_d" -#define AO_TELEM_RAW_MAIN "r_m" - -/* - * Sensor calibration values - * - * Name Value - * c_a Ground accelerometer reading (integer) - * c_b Ground barometer reading (integer) - * c_p Accelerometer reading for +1g - * c_m Accelerometer reading for -1g - */ - -#define AO_TELEM_CAL_ACCEL_GROUND "c_a" -#define AO_TELEM_CAL_BARO_GROUND "c_b" -#define AO_TELEM_CAL_ACCEL_PLUS "c_p" -#define AO_TELEM_CAL_ACCEL_MINUS "c_m" - -/* - * Kalman state values - * - * Name Value - * k_h Height above pad (integer, meters) - * k_s Vertical speeed (integer, m/s * 16) - * k_a Vertical acceleration (integer, m/s² * 16) - */ - -#define AO_TELEM_KALMAN_HEIGHT "k_h" -#define AO_TELEM_KALMAN_SPEED "k_s" -#define AO_TELEM_KALMAN_ACCEL "k_a" - -/* - * Ad-hoc flight values - * - * Name Value - * a_a Acceleration (integer, sensor units) - * a_s Speed (integer, integrated acceleration value) - * a_b Barometer reading (integer, sensor units) - */ - -#define AO_TELEM_ADHOC_ACCEL "a_a" -#define AO_TELEM_ADHOC_SPEED "a_s" -#define AO_TELEM_ADHOC_BARO "a_b" - -/* - * GPS values - * - * Name Value - * g GPS state (string): - * l locked - * u unlocked - * e error (missing or broken) - * g_n Number of sats used in solution - * g_ns Latitude (degrees * 10e7) - * g_ew Longitude (degrees * 10e7) - * g_a Altitude (integer meters) - * g_Y GPS year (integer) - * g_M GPS month (integer - 1-12) - * g_D GPS day (integer - 1-31) - * g_h GPS hour (integer - 0-23) - * g_m GPS minute (integer - 0-59) - * g_s GPS second (integer - 0-59) - * g_v GPS vertical speed (integer, cm/sec) - * g_g GPS horizontal speed (integer, cm/sec) - * g_c GPS course (integer, 0-359) - * g_hd GPS hdop (integer * 10) - * g_vd GPS vdop (integer * 10) - * g_he GPS h error (integer) - * g_ve GPS v error (integer) - */ - -#define AO_TELEM_GPS_STATE "g" -#define AO_TELEM_GPS_STATE_LOCKED 'l' -#define AO_TELEM_GPS_STATE_UNLOCKED 'u' -#define AO_TELEM_GPS_STATE_ERROR 'e' -#define AO_TELEM_GPS_NUM_SAT "g_n" -#define AO_TELEM_GPS_LATITUDE "g_ns" -#define AO_TELEM_GPS_LONGITUDE "g_ew" -#define AO_TELEM_GPS_ALTITUDE "g_a" -#define AO_TELEM_GPS_YEAR "g_Y" -#define AO_TELEM_GPS_MONTH "g_M" -#define AO_TELEM_GPS_DAY "g_D" -#define AO_TELEM_GPS_HOUR "g_h" -#define AO_TELEM_GPS_MINUTE "g_m" -#define AO_TELEM_GPS_SECOND "g_s" -#define AO_TELEM_GPS_VERTICAL_SPEED "g_v" -#define AO_TELEM_GPS_HORIZONTAL_SPEED "g_g" -#define AO_TELEM_GPS_COURSE "g_c" -#define AO_TELEM_GPS_HDOP "g_hd" -#define AO_TELEM_GPS_VDOP "g_vd" -#define AO_TELEM_GPS_HERROR "g_he" -#define AO_TELEM_GPS_VERROR "g_ve" - -/* - * GPS satellite values - * - * Name Value - * s_n Number of satellites reported (integer) - * s_v0 Space vehicle ID (integer) for report 0 - * s_c0 C/N0 number (integer) for report 0 - * s_v1 Space vehicle ID (integer) for report 1 - * s_c1 C/N0 number (integer) for report 1 - * ... - */ - -#define AO_TELEM_SAT_NUM "s_n" -#define AO_TELEM_SAT_SVID "s_v" -#define AO_TELEM_SAT_C_N_0 "s_c" - -#endif /* _AO_TELEM_H_ */ diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c deleted file mode 100644 index a1c19185..00000000 --- a/src/core/ao_telemetry.c +++ /dev/null @@ -1,555 +0,0 @@ -/* - * 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_log.h" -#include "ao_product.h" - -#ifndef HAS_RDF -#define HAS_RDF 1 -#endif - -static __pdata uint16_t ao_telemetry_interval; - -#if HAS_RDF -static __pdata uint8_t ao_rdf = 0; -static __pdata uint16_t ao_rdf_time; -#endif - -#if HAS_APRS -static __pdata uint16_t ao_aprs_time; - -#include -#endif - -#if defined(TELEMEGA) -#define AO_SEND_MEGA 1 -#endif - -#if defined (TELEMETRUM_V_2_0) -#define AO_SEND_METRUM 1 -#endif - -#if defined(TELEMETRUM_V_0_1) || defined(TELEMETRUM_V_0_2) || defined(TELEMETRUM_V_1_0) || defined(TELEMETRUM_V_1_1) || defined(TELEBALLOON_V_1_1) || defined(TELEMETRUM_V_1_2) -#define AO_TELEMETRY_SENSOR AO_TELEMETRY_SENSOR_TELEMETRUM -#endif - -#if defined(TELEMINI_V_1_0) -#define AO_TELEMETRY_SENSOR AO_TELEMETRY_SENSOR_TELEMINI -#endif - -#if defined(TELENANO_V_0_1) -#define AO_TELEMETRY_SENSOR AO_TELEMETRY_SENSOR_TELENANO -#endif - -static __xdata union ao_telemetry_all telemetry; - -#if defined AO_TELEMETRY_SENSOR -/* Send sensor packet */ -static void -ao_send_sensor(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_SENSOR; - - telemetry.sensor.state = ao_flight_state; -#if HAS_ACCEL - telemetry.sensor.accel = packet->adc.accel; -#else - telemetry.sensor.accel = 0; -#endif - telemetry.sensor.pres = ao_data_pres(packet); - telemetry.sensor.temp = packet->adc.temp; - telemetry.sensor.v_batt = packet->adc.v_batt; -#if HAS_IGNITE - telemetry.sensor.sense_d = packet->adc.sense_d; - telemetry.sensor.sense_m = packet->adc.sense_m; -#else - telemetry.sensor.sense_d = 0; - telemetry.sensor.sense_m = 0; -#endif - - telemetry.sensor.acceleration = ao_accel; - telemetry.sensor.speed = ao_speed; - telemetry.sensor.height = ao_height; - - telemetry.sensor.ground_pres = ao_ground_pres; -#if HAS_ACCEL - telemetry.sensor.ground_accel = ao_ground_accel; - telemetry.sensor.accel_plus_g = ao_config.accel_plus_g; - telemetry.sensor.accel_minus_g = ao_config.accel_minus_g; -#else - telemetry.sensor.ground_accel = 0; - telemetry.sensor.accel_plus_g = 0; - telemetry.sensor.accel_minus_g = 0; -#endif - - ao_radio_send(&telemetry, sizeof (telemetry)); -} -#endif - - -#ifdef AO_SEND_MEGA -/* Send mega sensor packet */ -static void -ao_send_mega_sensor(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_MEGA_SENSOR; - - telemetry.mega_sensor.orient = ao_sample_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); - -#if HAS_MPU6000 - telemetry.mega_sensor.accel_x = packet->mpu6000.accel_x; - telemetry.mega_sensor.accel_y = packet->mpu6000.accel_y; - telemetry.mega_sensor.accel_z = packet->mpu6000.accel_z; - - telemetry.mega_sensor.gyro_x = packet->mpu6000.gyro_x; - telemetry.mega_sensor.gyro_y = packet->mpu6000.gyro_y; - telemetry.mega_sensor.gyro_z = packet->mpu6000.gyro_z; -#endif - -#if HAS_HMC5883 - telemetry.mega_sensor.mag_x = packet->hmc5883.x; - telemetry.mega_sensor.mag_y = packet->hmc5883.y; - telemetry.mega_sensor.mag_z = packet->hmc5883.z; -#endif - - ao_radio_send(&telemetry, sizeof (telemetry)); -} - -static __pdata int8_t ao_telemetry_mega_data_max; -static __pdata int8_t ao_telemetry_mega_data_cur; - -/* Send mega data packet */ -static void -ao_send_mega_data(void) -{ - if (--ao_telemetry_mega_data_cur <= 0) { - __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; - uint8_t i; - - telemetry.generic.tick = packet->tick; - telemetry.generic.type = AO_TELEMETRY_MEGA_DATA; - - telemetry.mega_data.state = ao_flight_state; - telemetry.mega_data.v_batt = packet->adc.v_batt; - telemetry.mega_data.v_pyro = packet->adc.v_pbatt; - - /* ADC range is 0-4095, so shift by four to save the high 8 bits */ - for (i = 0; i < AO_ADC_NUM_SENSE; i++) - telemetry.mega_data.sense[i] = packet->adc.sense[i] >> 4; - - telemetry.mega_data.ground_pres = ao_ground_pres; - telemetry.mega_data.ground_accel = ao_ground_accel; - telemetry.mega_data.accel_plus_g = ao_config.accel_plus_g; - telemetry.mega_data.accel_minus_g = ao_config.accel_minus_g; - - telemetry.mega_data.acceleration = ao_accel; - telemetry.mega_data.speed = ao_speed; - telemetry.mega_data.height = ao_height; - - ao_radio_send(&telemetry, sizeof (telemetry)); - ao_telemetry_mega_data_cur = ao_telemetry_mega_data_max; - } -} -#endif /* AO_SEND_MEGA */ - -#ifdef AO_SEND_METRUM -/* Send telemetrum sensor packet */ -static void -ao_send_metrum_sensor(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_METRUM_SENSOR; - - telemetry.metrum_sensor.state = ao_flight_state; -#if HAS_ACCEL - telemetry.metrum_sensor.accel = ao_data_accel(packet); -#endif - telemetry.metrum_sensor.pres = ao_data_pres(packet); - telemetry.metrum_sensor.temp = ao_data_temp(packet); - - telemetry.metrum_sensor.acceleration = ao_accel; - telemetry.metrum_sensor.speed = ao_speed; - telemetry.metrum_sensor.height = ao_height; - - telemetry.metrum_sensor.v_batt = packet->adc.v_batt; - telemetry.metrum_sensor.sense_a = packet->adc.sense_a; - telemetry.metrum_sensor.sense_m = packet->adc.sense_m; - - ao_radio_send(&telemetry, sizeof (telemetry)); -} - -static __pdata int8_t ao_telemetry_metrum_data_max; -static __pdata int8_t ao_telemetry_metrum_data_cur; - -/* Send telemetrum data packet */ -static void -ao_send_metrum_data(void) -{ - if (--ao_telemetry_metrum_data_cur <= 0) { - __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_METRUM_DATA; - - telemetry.metrum_data.ground_pres = ao_ground_pres; -#if HAS_ACCEL - telemetry.metrum_data.ground_accel = ao_ground_accel; - telemetry.metrum_data.accel_plus_g = ao_config.accel_plus_g; - telemetry.metrum_data.accel_minus_g = ao_config.accel_minus_g; -#else - telemetry.metrum_data.ground_accel = 1; - telemetry.metrum_data.accel_plus_g = 0; - telemetry.metrum_data.accel_minus_g = 2; -#endif - - ao_radio_send(&telemetry, sizeof (telemetry)); - ao_telemetry_metrum_data_cur = ao_telemetry_metrum_data_max; - } -} -#endif /* AO_SEND_METRUM */ - -#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 /* AO_SEND_MINI */ - -#ifdef AO_SEND_ALL_BARO -static uint8_t ao_baro_sample; - -static void -ao_send_baro(void) -{ - uint8_t sample = ao_sample_data; - uint8_t samples = (sample - ao_baro_sample) & (AO_DATA_RING - 1); - - if (samples > 12) { - ao_baro_sample = (ao_baro_sample + (samples - 12)) & (AO_DATA_RING - 1); - samples = 12; - } - telemetry.generic.tick = ao_data_ring[sample].tick; - telemetry.generic.type = AO_TELEMETRY_BARO; - telemetry.baro.samples = samples; - for (sample = 0; sample < samples; sample++) { - telemetry.baro.baro[sample] = ao_data_ring[ao_baro_sample].adc.pres; - ao_baro_sample = ao_data_ring_next(ao_baro_sample); - } - ao_radio_send(&telemetry, sizeof (telemetry)); -} -#endif - -static __pdata int8_t ao_telemetry_config_max; -static __pdata int8_t ao_telemetry_config_cur; - -static void -ao_send_configuration(void) -{ - if (--ao_telemetry_config_cur <= 0) - { - telemetry.generic.type = AO_TELEMETRY_CONFIGURATION; - telemetry.configuration.device = AO_idProduct_NUMBER; -#if HAS_LOG - telemetry.configuration.flight = ao_log_full() ? 0 : ao_flight_number; -#else - telemetry.configuration.flight = ao_flight_number; -#endif - telemetry.configuration.config_major = AO_CONFIG_MAJOR; - telemetry.configuration.config_minor = AO_CONFIG_MINOR; - telemetry.configuration.apogee_delay = ao_config.apogee_delay; - telemetry.configuration.main_deploy = ao_config.main_deploy; - telemetry.configuration.flight_log_max = ao_config.flight_log_max >> 10; - ao_xmemcpy (telemetry.configuration.callsign, - ao_config.callsign, - AO_MAX_CALLSIGN); - ao_xmemcpy (telemetry.configuration.version, - CODE_TO_XDATA(ao_version), - AO_MAX_VERSION); - ao_radio_send(&telemetry, sizeof (telemetry)); - ao_telemetry_config_cur = ao_telemetry_config_max; - } -} - -#if HAS_GPS - -static __pdata int8_t ao_telemetry_loc_cur; -static __pdata int8_t ao_telemetry_sat_cur; - -static void -ao_send_location(void) -{ - if (--ao_telemetry_loc_cur <= 0) - { - telemetry.generic.type = AO_TELEMETRY_LOCATION; - ao_mutex_get(&ao_gps_mutex); - ao_xmemcpy(&telemetry.location.flags, - &ao_gps_data.flags, - 26); - telemetry.location.tick = ao_gps_tick; - ao_mutex_put(&ao_gps_mutex); - ao_radio_send(&telemetry, sizeof (telemetry)); - ao_telemetry_loc_cur = ao_telemetry_config_max; - } -} - -static void -ao_send_satellite(void) -{ - if (--ao_telemetry_sat_cur <= 0) - { - telemetry.generic.type = AO_TELEMETRY_SATELLITE; - ao_mutex_get(&ao_gps_mutex); - telemetry.satellite.channels = ao_gps_tracking_data.channels; - ao_xmemcpy(&telemetry.satellite.sats, - &ao_gps_tracking_data.sats, - AO_MAX_GPS_TRACKING * sizeof (struct ao_telemetry_satellite_info)); - ao_mutex_put(&ao_gps_mutex); - ao_radio_send(&telemetry, sizeof (telemetry)); - ao_telemetry_sat_cur = ao_telemetry_config_max; - } -} -#endif - -#if HAS_COMPANION - -static __pdata int8_t ao_telemetry_companion_max; -static __pdata int8_t ao_telemetry_companion_cur; - -static void -ao_send_companion(void) -{ - if (--ao_telemetry_companion_cur <= 0) { - telemetry.generic.type = AO_TELEMETRY_COMPANION; - telemetry.companion.board_id = ao_companion_setup.board_id; - telemetry.companion.update_period = ao_companion_setup.update_period; - telemetry.companion.channels = ao_companion_setup.channels; - ao_mutex_get(&ao_companion_mutex); - ao_xmemcpy(&telemetry.companion.companion_data, - ao_companion_data, - ao_companion_setup.channels * 2); - ao_mutex_put(&ao_companion_mutex); - ao_radio_send(&telemetry, sizeof (telemetry)); - ao_telemetry_companion_cur = ao_telemetry_companion_max; - } -} -#endif - -void -ao_telemetry(void) -{ - uint16_t time; - int16_t delay; - - ao_config_get(); - if (!ao_config.radio_enable) - ao_exit(); - while (!ao_flight_number) - ao_sleep(&ao_flight_number); - - telemetry.generic.serial = ao_serial_number; - for (;;) { - while (ao_telemetry_interval == 0) - ao_sleep(&telemetry); - time = ao_time(); -#if HAS_RDF - ao_rdf_time = time; -#endif -#if HAS_APRS - ao_aprs_time = time; -#endif - while (ao_telemetry_interval) { -#if HAS_APRS - if (!(ao_config.radio_enable & AO_RADIO_DISABLE_TELEMETRY)) -#endif - { -#ifdef AO_SEND_ALL_BARO - ao_send_baro(); -#endif - -#if HAS_FLIGHT -# ifdef AO_SEND_MEGA - ao_send_mega_sensor(); - ao_send_mega_data(); -# endif -# ifdef AO_SEND_METRUM - ao_send_metrum_sensor(); - ao_send_metrum_data(); -# endif -# ifdef AO_SEND_MINI - ao_send_mini(); -# endif -# ifdef AO_TELEMETRY_SENSOR - ao_send_sensor(); -# endif -#endif /* HAS_FLIGHT */ - -#if HAS_COMPANION - if (ao_companion_running) - ao_send_companion(); -#endif - ao_send_configuration(); -#if HAS_GPS - ao_send_location(); - ao_send_satellite(); -#endif - } -#ifndef AO_SEND_ALL_BARO -#if HAS_RDF - if (ao_rdf && -#if HAS_APRS - !(ao_config.radio_enable & AO_RADIO_DISABLE_RDF) && -#endif /* HAS_APRS */ - (int16_t) (ao_time() - ao_rdf_time) >= 0) - { -#if HAS_IGNITE_REPORT - uint8_t c; -#endif /* HAS_IGNITE_REPORT */ - ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS; -#if HAS_IGNITE_REPORT - if (ao_flight_state == ao_flight_pad && (c = ao_report_igniter())) - ao_radio_continuity(c); - else -#endif /* HAS_IGNITE_REPORT*/ - ao_radio_rdf(); - } -#endif /* HAS_RDF */ -#if HAS_APRS - if (ao_config.aprs_interval != 0 && - (int16_t) (ao_time() - ao_aprs_time) >= 0) - { - ao_aprs_time = ao_time() + AO_SEC_TO_TICKS(ao_config.aprs_interval); - ao_aprs_send(); - } -#endif /* HAS_APRS */ -#endif /* !AO_SEND_ALL_BARO */ - time += ao_telemetry_interval; - delay = time - ao_time(); - if (delay > 0) { - ao_alarm(delay); - ao_sleep(&telemetry); - ao_clear_alarm(); - } - else - time = ao_time(); - } - } -} - -void -ao_telemetry_set_interval(uint16_t interval) -{ - int8_t cur = 0; - ao_telemetry_interval = interval; - -#if AO_SEND_MEGA - if (interval > 1) - ao_telemetry_mega_data_max = 1; - else - ao_telemetry_mega_data_max = 2; - if (ao_telemetry_mega_data_max > cur) - cur++; - ao_telemetry_mega_data_cur = cur; -#endif -#if AO_SEND_METRUM - ao_telemetry_metrum_data_max = AO_SEC_TO_TICKS(1) / interval; - if (ao_telemetry_metrum_data_max > cur) - cur++; - ao_telemetry_metrum_data_cur = cur; -#endif - -#if HAS_COMPANION - if (!ao_companion_setup.update_period) - ao_companion_setup.update_period = AO_SEC_TO_TICKS(1); - ao_telemetry_companion_max = ao_companion_setup.update_period / interval; - if (ao_telemetry_companion_max > cur) - cur++; - ao_telemetry_companion_cur = cur; -#endif - - ao_telemetry_config_max = AO_SEC_TO_TICKS(1) / interval; -#if HAS_COMPANION - if (ao_telemetry_config_max > cur) - cur++; - ao_telemetry_config_cur = cur; -#endif - -#if HAS_GPS - if (ao_telemetry_config_max > cur) - cur++; - ao_telemetry_loc_cur = cur; - if (ao_telemetry_config_max > cur) - cur++; - ao_telemetry_sat_cur = cur; -#endif - ao_wakeup(&telemetry); -} - -#if HAS_RDF -void -ao_rdf_set(uint8_t rdf) -{ - ao_rdf = rdf; - if (rdf == 0) - ao_radio_rdf_abort(); - else { - ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS; - } -} -#endif - -__xdata struct ao_task ao_telemetry_task; - -void -ao_telemetry_init() -{ - ao_add_task(&ao_telemetry_task, ao_telemetry, "telemetry"); -} diff --git a/src/core/ao_telemetry.h b/src/core/ao_telemetry.h deleted file mode 100644 index 237a35ab..00000000 --- a/src/core/ao_telemetry.h +++ /dev/null @@ -1,321 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_TELEMETRY_H_ -#define _AO_TELEMETRY_H_ - -/* - * ao_telemetry.c - */ -#define AO_MAX_CALLSIGN 8 -#define AO_MAX_VERSION 8 -#if LEGACY_MONITOR -#define AO_MAX_TELEMETRY 128 -#else -#define AO_MAX_TELEMETRY 32 -#endif - -struct ao_telemetry_generic { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - uint8_t payload[27]; /* 5 */ - /* 32 */ -}; - -#define AO_TELEMETRY_SENSOR_TELEMETRUM 0x01 -#define AO_TELEMETRY_SENSOR_TELEMINI 0x02 -#define AO_TELEMETRY_SENSOR_TELENANO 0x03 - -struct ao_telemetry_sensor { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t state; /* 5 flight state */ - int16_t accel; /* 6 accelerometer (TM only) */ - int16_t pres; /* 8 pressure sensor */ - int16_t temp; /* 10 temperature sensor */ - int16_t v_batt; /* 12 battery voltage */ - int16_t sense_d; /* 14 drogue continuity sense (TM/Tm) */ - int16_t sense_m; /* 16 main continuity sense (TM/Tm) */ - - int16_t acceleration; /* 18 m/s² * 16 */ - int16_t speed; /* 20 m/s * 16 */ - int16_t height; /* 22 m */ - - int16_t ground_pres; /* 24 average pres on pad */ - int16_t ground_accel; /* 26 average accel on pad */ - int16_t accel_plus_g; /* 28 accel calibration at +1g */ - int16_t accel_minus_g; /* 30 accel calibration at -1g */ - /* 32 */ -}; - -#define AO_TELEMETRY_CONFIGURATION 0x04 - -struct ao_telemetry_configuration { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t device; /* 5 device type */ - uint16_t flight; /* 6 flight number */ - uint8_t config_major; /* 8 Config major version */ - uint8_t config_minor; /* 9 Config minor version */ - uint16_t apogee_delay; /* 10 Apogee deploy delay in seconds */ - uint16_t main_deploy; /* 12 Main deploy alt in meters */ - uint16_t flight_log_max; /* 14 Maximum flight log size in kB */ - char callsign[AO_MAX_CALLSIGN]; /* 16 Radio operator identity */ - char version[AO_MAX_VERSION]; /* 24 Software version */ - /* 32 */ -}; - -#define AO_TELEMETRY_LOCATION 0x05 - -#define AO_GPS_MODE_NOT_VALID 'N' -#define AO_GPS_MODE_AUTONOMOUS 'A' -#define AO_GPS_MODE_DIFFERENTIAL 'D' -#define AO_GPS_MODE_ESTIMATED 'E' -#define AO_GPS_MODE_MANUAL 'M' -#define AO_GPS_MODE_SIMULATED 'S' - -struct ao_telemetry_location { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t flags; /* 5 Number of sats and other flags */ - int16_t altitude; /* 6 GPS reported altitude (m) */ - int32_t latitude; /* 8 latitude (degrees * 10⁷) */ - int32_t longitude; /* 12 longitude (degrees * 10⁷) */ - uint8_t year; /* 16 (- 2000) */ - uint8_t month; /* 17 (1-12) */ - uint8_t day; /* 18 (1-31) */ - uint8_t hour; /* 19 (0-23) */ - uint8_t minute; /* 20 (0-59) */ - uint8_t second; /* 21 (0-59) */ - uint8_t pdop; /* 22 (m * 5) */ - uint8_t hdop; /* 23 (m * 5) */ - uint8_t vdop; /* 24 (m * 5) */ - uint8_t mode; /* 25 */ - uint16_t ground_speed; /* 26 cm/s */ - int16_t climb_rate; /* 28 cm/s */ - uint8_t course; /* 30 degrees / 2 */ - uint8_t unused[1]; /* 31 */ - /* 32 */ -}; - -#define AO_TELEMETRY_SATELLITE 0x06 - -struct ao_telemetry_satellite_info { - uint8_t svid; - uint8_t c_n_1; -}; - -#define AO_TELEMETRY_SATELLITE_MAX_SAT 12 - -struct ao_telemetry_satellite { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - uint8_t channels; /* 5 number of reported sats */ - - struct ao_telemetry_satellite_info sats[AO_TELEMETRY_SATELLITE_MAX_SAT]; /* 6 */ - uint8_t unused[2]; /* 30 */ - /* 32 */ -}; - -#define AO_TELEMETRY_COMPANION 0x07 - -#define AO_COMPANION_MAX_CHANNELS 12 - -struct ao_telemetry_companion { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - uint8_t board_id; /* 5 */ - - uint8_t update_period; /* 6 */ - uint8_t channels; /* 7 */ - uint16_t companion_data[AO_COMPANION_MAX_CHANNELS]; /* 8 */ - /* 32 */ -}; - -#define AO_TELEMETRY_MEGA_SENSOR 0x08 - -struct ao_telemetry_mega_sensor { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t orient; /* 5 angle from vertical */ - int16_t accel; /* 6 Z axis */ - - int32_t pres; /* 8 Pa * 10 */ - int16_t temp; /* 12 °C * 100 */ - - int16_t accel_x; /* 14 */ - int16_t accel_y; /* 16 */ - int16_t accel_z; /* 18 */ - - int16_t gyro_x; /* 20 */ - int16_t gyro_y; /* 22 */ - int16_t gyro_z; /* 24 */ - - int16_t mag_x; /* 26 */ - int16_t mag_y; /* 28 */ - int16_t mag_z; /* 30 */ - /* 32 */ -}; - -#define AO_TELEMETRY_MEGA_DATA 0x09 - -struct ao_telemetry_mega_data { - 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 v_pyro; /* 8 pyro battery voltage */ - int8_t sense[6]; /* 10 continuity sense */ - - int32_t ground_pres; /* 16 average pres on pad */ - int16_t ground_accel; /* 20 average accel on pad */ - int16_t accel_plus_g; /* 22 accel calibration at +1g */ - int16_t accel_minus_g; /* 24 accel calibration at -1g */ - - int16_t acceleration; /* 26 m/s² * 16 */ - int16_t speed; /* 28 m/s * 16 */ - int16_t height; /* 30 m */ - /* 32 */ -}; - - -#define AO_TELEMETRY_METRUM_SENSOR 0x0A - -struct ao_telemetry_metrum_sensor { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t state; /* 5 flight state */ - int16_t accel; /* 6 Z axis */ - - int32_t pres; /* 8 Pa * 10 */ - int16_t temp; /* 12 °C * 100 */ - - int16_t acceleration; /* 14 m/s² * 16 */ - int16_t speed; /* 16 m/s * 16 */ - int16_t height; /* 18 m */ - - int16_t v_batt; /* 20 battery voltage */ - int16_t sense_a; /* 22 apogee continuity sense */ - int16_t sense_m; /* 24 main continuity sense */ - - uint8_t pad[6]; /* 26 */ - /* 32 */ -}; - -#define AO_TELEMETRY_METRUM_DATA 0x0B - -struct ao_telemetry_metrum_data { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - int32_t ground_pres; /* 8 average pres on pad */ - int16_t ground_accel; /* 12 average accel on pad */ - int16_t accel_plus_g; /* 14 accel calibration at +1g */ - int16_t accel_minus_g; /* 16 accel calibration at -1g */ - - uint8_t pad[14]; /* 18 */ - /* 32 */ -}; - -#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 - -/* - * This packet allows the full sampling rate baro - * data to be captured over the RF link so that the - * flight software can be tested using 'real' data. - * - * Along with this telemetry packet, the flight - * code is modified to send full-rate telemetry all the time - * and never send an RDF tone; this ensure that the full radio - * link is available. - */ -struct ao_telemetry_baro { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - uint8_t samples; /* 5 number samples */ - - int16_t baro[12]; /* 6 samples */ - /* 32 */ -}; - -union ao_telemetry_all { - struct ao_telemetry_generic generic; - struct ao_telemetry_sensor sensor; - struct ao_telemetry_configuration configuration; - struct ao_telemetry_location location; - struct ao_telemetry_satellite satellite; - struct ao_telemetry_companion companion; - struct ao_telemetry_mega_sensor mega_sensor; - struct ao_telemetry_mega_data mega_data; - struct ao_telemetry_metrum_sensor metrum_sensor; - struct ao_telemetry_metrum_data metrum_data; - struct ao_telemetry_mini mini; - struct ao_telemetry_baro baro; -}; - -struct ao_telemetry_all_recv { - union ao_telemetry_all telemetry; - int8_t rssi; - uint8_t status; -}; - -#endif /* _AO_TELEMETRY_H_ */ diff --git a/src/core/ao_usb.h b/src/core/ao_usb.h deleted file mode 100644 index 1ce4f82f..00000000 --- a/src/core/ao_usb.h +++ /dev/null @@ -1,142 +0,0 @@ -/* - * 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. - */ - -#ifndef _AO_USB_H_ -#define _AO_USB_H_ - -/* - * ao_usb.c - */ - -/* Put one character to the USB output queue */ -void -ao_usb_putchar(char c); - -/* Get one character from the USB input queue */ -char -ao_usb_getchar(void); - -/* Poll for a charcter on the USB input queue. - * returns AO_READ_AGAIN if none are available - */ -int -ao_usb_pollchar(void); - -/* Flush the USB output queue */ -void -ao_usb_flush(void); - -/* Enable the USB controller */ -void -ao_usb_enable(void); - -/* Disable the USB controller */ -void -ao_usb_disable(void); - -/* Initialize the USB system */ -void -ao_usb_init(void); - -extern __code __at (0x00aa) uint8_t ao_usb_descriptors []; - -#define AO_USB_SETUP_DIR_MASK (0x01 << 7) -#define AO_USB_SETUP_TYPE_MASK (0x03 << 5) -#define AO_USB_SETUP_RECIP_MASK (0x1f) - -#define AO_USB_DIR_OUT 0 -#define AO_USB_DIR_IN (1 << 7) - -#define AO_USB_TYPE_STANDARD 0 -#define AO_USB_TYPE_CLASS (1 << 5) -#define AO_USB_TYPE_VENDOR (2 << 5) -#define AO_USB_TYPE_RESERVED (3 << 5) - -#define AO_USB_RECIP_DEVICE 0 -#define AO_USB_RECIP_INTERFACE 1 -#define AO_USB_RECIP_ENDPOINT 2 -#define AO_USB_RECIP_OTHER 3 - -/* standard requests */ -#define AO_USB_REQ_GET_STATUS 0x00 -#define AO_USB_REQ_CLEAR_FEATURE 0x01 -#define AO_USB_REQ_SET_FEATURE 0x03 -#define AO_USB_REQ_SET_ADDRESS 0x05 -#define AO_USB_REQ_GET_DESCRIPTOR 0x06 -#define AO_USB_REQ_SET_DESCRIPTOR 0x07 -#define AO_USB_REQ_GET_CONFIGURATION 0x08 -#define AO_USB_REQ_SET_CONFIGURATION 0x09 -#define AO_USB_REQ_GET_INTERFACE 0x0A -#define AO_USB_REQ_SET_INTERFACE 0x0B -#define AO_USB_REQ_SYNCH_FRAME 0x0C - -#define AO_USB_DESC_DEVICE 1 -#define AO_USB_DESC_CONFIGURATION 2 -#define AO_USB_DESC_STRING 3 -#define AO_USB_DESC_INTERFACE 4 -#define AO_USB_DESC_ENDPOINT 5 -#define AO_USB_DESC_DEVICE_QUALIFIER 6 -#define AO_USB_DESC_OTHER_SPEED 7 -#define AO_USB_DESC_INTERFACE_POWER 8 - -#define AO_USB_GET_DESC_TYPE(x) (((x)>>8)&0xFF) -#define AO_USB_GET_DESC_INDEX(x) ((x)&0xFF) - -#define AO_USB_CONTROL_EP 0 -#define AO_USB_CONTROL_SIZE 32 - -#define AO_USB_INT_EP 1 -#define AO_USB_INT_SIZE 8 - -#ifndef AO_USB_OUT_EP -#define AO_USB_OUT_EP 4 -#define AO_USB_IN_EP 5 -#endif - -/* - * USB bulk packets can only come in 8, 16, 32 and 64 - * byte sizes, so we'll use 64 for everything - */ -#define AO_USB_IN_SIZE 64 -#define AO_USB_OUT_SIZE 64 - -#define AO_USB_EP0_IDLE 0 -#define AO_USB_EP0_DATA_IN 1 -#define AO_USB_EP0_DATA_OUT 2 -#define AO_USB_EP0_STALL 3 - -#define LE_WORD(x) ((x)&0xFF),((uint8_t) (((uint16_t) (x))>>8)) - -/* CDC definitions */ -#define AO_USB_CS_INTERFACE 0x24 -#define AO_USB_CS_ENDPOINT 0x25 - -#define AO_USB_SET_LINE_CODING 0x20 -#define AO_USB_GET_LINE_CODING 0x21 -#define AO_USB_SET_CONTROL_LINE_STATE 0x22 - -/* Data structure for GET_LINE_CODING / SET_LINE_CODING class requests */ -struct ao_usb_line_coding { - uint32_t rate; - uint8_t char_format; - uint8_t parity; - uint8_t data_bits; -} ; - -extern __pdata uint8_t ao_usb_running; - -#endif /* _AO_USB_H_ */ diff --git a/src/kernel/altitude.h b/src/kernel/altitude.h new file mode 100644 index 00000000..a278bbc6 --- /dev/null +++ b/src/kernel/altitude.h @@ -0,0 +1,132 @@ +/*max error 3.197865153490684 at 0.782%. Average error 0.260150920474668*/ +#define NALT 129 +#define ALT_FRAC_BITS 8 + 15835, /* 10.56 kPa 0.000% */ + 15332, /* 11.42 kPa 0.781% */ + 14868, /* 12.29 kPa 1.563% */ + 14435, /* 13.16 kPa 2.344% */ + 14030, /* 14.02 kPa 3.125% */ + 13649, /* 14.90 kPa 3.906% */ + 13290, /* 15.76 kPa 4.688% */ + 12950, /* 16.63 kPa 5.469% */ + 12627, /* 17.50 kPa 6.250% */ + 12320, /* 18.37 kPa 7.031% */ + 12027, /* 19.24 kPa 7.813% */ + 11747, /* 20.10 kPa 8.594% */ + 11479, /* 20.97 kPa 9.375% */ + 11222, /* 21.84 kPa 10.156% */ + 10975, /* 22.71 kPa 10.938% */ + 10736, /* 23.58 kPa 11.719% */ + 10504, /* 24.44 kPa 12.500% */ + 10278, /* 25.31 kPa 13.281% */ + 10059, /* 26.18 kPa 14.063% */ + 9846, /* 27.05 kPa 14.844% */ + 9638, /* 27.91 kPa 15.625% */ + 9435, /* 28.78 kPa 16.406% */ + 9237, /* 29.65 kPa 17.188% */ + 9044, /* 30.52 kPa 17.969% */ + 8855, /* 31.39 kPa 18.750% */ + 8670, /* 32.26 kPa 19.531% */ + 8490, /* 33.13 kPa 20.313% */ + 8313, /* 33.99 kPa 21.094% */ + 8140, /* 34.86 kPa 21.875% */ + 7970, /* 35.73 kPa 22.656% */ + 7803, /* 36.60 kPa 23.438% */ + 7640, /* 37.47 kPa 24.219% */ + 7480, /* 38.33 kPa 25.000% */ + 7322, /* 39.20 kPa 25.781% */ + 7168, /* 40.07 kPa 26.563% */ + 7016, /* 40.94 kPa 27.344% */ + 6867, /* 41.80 kPa 28.125% */ + 6720, /* 42.67 kPa 28.906% */ + 6575, /* 43.54 kPa 29.688% */ + 6433, /* 44.41 kPa 30.469% */ + 6294, /* 45.28 kPa 31.250% */ + 6156, /* 46.15 kPa 32.031% */ + 6020, /* 47.01 kPa 32.813% */ + 5887, /* 47.88 kPa 33.594% */ + 5755, /* 48.75 kPa 34.375% */ + 5625, /* 49.62 kPa 35.156% */ + 5497, /* 50.49 kPa 35.938% */ + 5371, /* 51.35 kPa 36.719% */ + 5247, /* 52.22 kPa 37.500% */ + 5124, /* 53.09 kPa 38.281% */ + 5003, /* 53.96 kPa 39.063% */ + 4883, /* 54.83 kPa 39.844% */ + 4765, /* 55.69 kPa 40.625% */ + 4648, /* 56.56 kPa 41.406% */ + 4533, /* 57.43 kPa 42.188% */ + 4419, /* 58.30 kPa 42.969% */ + 4307, /* 59.17 kPa 43.750% */ + 4196, /* 60.03 kPa 44.531% */ + 4086, /* 60.90 kPa 45.313% */ + 3977, /* 61.77 kPa 46.094% */ + 3870, /* 62.63 kPa 46.875% */ + 3764, /* 63.51 kPa 47.656% */ + 3659, /* 64.38 kPa 48.438% */ + 3555, /* 65.24 kPa 49.219% */ + 3453, /* 66.11 kPa 50.000% */ + 3351, /* 66.98 kPa 50.781% */ + 3250, /* 67.85 kPa 51.563% */ + 3151, /* 68.72 kPa 52.344% */ + 3052, /* 69.58 kPa 53.125% */ + 2955, /* 70.45 kPa 53.906% */ + 2858, /* 71.32 kPa 54.688% */ + 2763, /* 72.19 kPa 55.469% */ + 2668, /* 73.06 kPa 56.250% */ + 2574, /* 73.92 kPa 57.031% */ + 2482, /* 74.79 kPa 57.813% */ + 2390, /* 75.66 kPa 58.594% */ + 2298, /* 76.52 kPa 59.375% */ + 2208, /* 77.40 kPa 60.156% */ + 2119, /* 78.26 kPa 60.938% */ + 2030, /* 79.13 kPa 61.719% */ + 1942, /* 80.00 kPa 62.500% */ + 1855, /* 80.87 kPa 63.281% */ + 1769, /* 81.74 kPa 64.063% */ + 1683, /* 82.60 kPa 64.844% */ + 1598, /* 83.47 kPa 65.625% */ + 1514, /* 84.34 kPa 66.406% */ + 1430, /* 85.21 kPa 67.188% */ + 1347, /* 86.08 kPa 67.969% */ + 1265, /* 86.94 kPa 68.750% */ + 1184, /* 87.81 kPa 69.531% */ + 1103, /* 88.68 kPa 70.313% */ + 1023, /* 89.55 kPa 71.094% */ + 943, /* 90.41 kPa 71.875% */ + 864, /* 91.28 kPa 72.656% */ + 786, /* 92.15 kPa 73.438% */ + 708, /* 93.02 kPa 74.219% */ + 631, /* 93.89 kPa 75.000% */ + 554, /* 94.76 kPa 75.781% */ + 478, /* 95.63 kPa 76.563% */ + 403, /* 96.49 kPa 77.344% */ + 328, /* 97.36 kPa 78.125% */ + 254, /* 98.23 kPa 78.906% */ + 180, /* 99.10 kPa 79.688% */ + 106, /* 99.97 kPa 80.469% */ + 34, /* 100.83 kPa 81.250% */ + -39, /* 101.70 kPa 82.031% */ + -111, /* 102.57 kPa 82.813% */ + -182, /* 103.44 kPa 83.594% */ + -253, /* 104.30 kPa 84.375% */ + -323, /* 105.17 kPa 85.156% */ + -393, /* 106.04 kPa 85.938% */ + -462, /* 106.91 kPa 86.719% */ + -531, /* 107.78 kPa 87.500% */ + -600, /* 108.65 kPa 88.281% */ + -668, /* 109.51 kPa 89.063% */ + -736, /* 110.38 kPa 89.844% */ + -803, /* 111.25 kPa 90.625% */ + -870, /* 112.12 kPa 91.406% */ + -936, /* 112.99 kPa 92.188% */ + -1002, /* 113.85 kPa 92.969% */ + -1068, /* 114.72 kPa 93.750% */ + -1133, /* 115.59 kPa 94.531% */ + -1198, /* 116.46 kPa 95.313% */ + -1262, /* 117.33 kPa 96.094% */ + -1326, /* 118.19 kPa 96.875% */ + -1389, /* 119.06 kPa 97.656% */ + -1453, /* 119.93 kPa 98.438% */ + -1516, /* 120.80 kPa 99.219% */ + -1578, /* 121.67 kPa 100.000% */ diff --git a/src/kernel/ao.h b/src/kernel/ao.h new file mode 100644 index 00000000..29ad2603 --- /dev/null +++ b/src/kernel/ao.h @@ -0,0 +1,1041 @@ +/* + * Copyright © 2009 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_H_ +#define _AO_H_ + +#include +#include +#include +#include +#include +#include + +#define TRUE 1 +#define FALSE 0 + +/* Convert a __data pointer into an __xdata pointer */ +#ifndef DATA_TO_XDATA +#define DATA_TO_XDATA(a) (a) +#endif +#ifndef PDATA_TO_XDATA +#define PDATA_TO_XDATA(a) (a) +#endif +#ifndef CODE_TO_XDATA +#define CODE_TO_XDATA(a) (a) +#endif + +#ifndef HAS_TASK +#define HAS_TASK 1 +#endif + +#ifndef AO_PORT_TYPE +#define AO_PORT_TYPE uint8_t +#endif + +typedef AO_PORT_TYPE ao_port_t; + +#if HAS_TASK +#include +#else +#include +#endif + +/* + * ao_panic.c + */ + +#define AO_PANIC_NO_TASK 1 /* AO_NUM_TASKS is not large enough */ +#define AO_PANIC_DMA 2 /* Attempt to start DMA while active */ +#define AO_PANIC_MUTEX 3 /* Mis-using mutex API */ +#define AO_PANIC_EE 4 /* Mis-using eeprom API */ +#define AO_PANIC_LOG 5 /* Failing to read/write log data */ +#define AO_PANIC_CMD 6 /* Too many command sets registered */ +#define AO_PANIC_STDIO 7 /* Too many stdio handlers registered */ +#define AO_PANIC_REBOOT 8 /* Reboot failed */ +#define AO_PANIC_FLASH 9 /* Invalid flash part (or wrong blocksize) */ +#define AO_PANIC_USB 10 /* Trying to send USB packet while busy */ +#define AO_PANIC_BT 11 /* Communications with bluetooth device failed */ +#define AO_PANIC_STACK 12 /* Stack overflow */ +#define AO_PANIC_SPI 13 /* SPI communication failure */ +#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 */ +#define AO_PANIC_SELF_TEST_MS5607 0x40 | 4 /* Self test failure */ + +/* Stop the operating system, beeping and blinking the reason */ +void +ao_panic(uint8_t reason); + +/* + * ao_timer.c + */ + +#ifndef AO_TICK_TYPE +#define AO_TICK_TYPE uint16_t +#define AO_TICK_SIGNED int16_t +#endif + +extern volatile __data AO_TICK_TYPE ao_tick_count; + +/* Our timer runs at 100Hz */ +#ifndef AO_HERTZ +#define AO_HERTZ 100 +#endif +#define AO_MS_TO_TICKS(ms) ((ms) / (1000 / AO_HERTZ)) +#define AO_SEC_TO_TICKS(s) ((s) * AO_HERTZ) + +/* Returns the current time in ticks */ +AO_TICK_TYPE +ao_time(void); + +/* Suspend the current task until ticks time has passed */ +void +ao_delay(uint16_t ticks); + +/* Set the ADC interval */ +void +ao_timer_set_adc_interval(uint8_t interval); + +/* Timer interrupt */ +void +ao_timer_isr(void) ao_arch_interrupt(9); + +/* Initialize the timer */ +void +ao_timer_init(void); + +/* Initialize the hardware clock. Must be called first */ +void +ao_clock_init(void); + +/* + * ao_mutex.c + */ + +#ifndef ao_mutex_get +void +ao_mutex_get(__xdata uint8_t *ao_mutex) __reentrant; + +void +ao_mutex_put(__xdata uint8_t *ao_mutex) __reentrant; +#endif + +/* + * ao_cmd.c + */ + +enum ao_cmd_status { + ao_cmd_success = 0, + ao_cmd_lex_error = 1, + ao_cmd_syntax_error = 2, +}; + +extern __pdata uint16_t ao_cmd_lex_i; +extern __pdata uint32_t ao_cmd_lex_u32; +extern __pdata char ao_cmd_lex_c; +extern __pdata enum ao_cmd_status ao_cmd_status; + +void +ao_put_string(__code char *s); + +void +ao_cmd_lex(void); + +void +ao_cmd_put8(uint8_t v); + +void +ao_cmd_put16(uint16_t v); + +uint8_t +ao_cmd_is_white(void); + +void +ao_cmd_white(void); + +int8_t +ao_cmd_hexchar(char c); + +void +ao_cmd_hexbyte(void); + +void +ao_cmd_hex(void); + +void +ao_cmd_decimal(void) __reentrant; + +/* Read a single hex nibble off stdin. */ +uint8_t +ao_getnibble(void); + +uint8_t +ao_match_word(__code char *word); + +struct ao_cmds { + void (*func)(void); + __code char *help; +}; + +void +ao_cmd_register(const __code struct ao_cmds *cmds); + +void +ao_cmd_init(void); + +#if HAS_CMD_FILTER +/* + * Provided by an external module to filter raw command lines + */ +uint8_t +ao_cmd_filter(void); +#endif + +/* + * Various drivers + */ +#if HAS_ADC +#include +#endif + +#if HAS_BEEP +#include +#endif + +#if LEDS_AVAILABLE +#include +#endif + +#if HAS_USB +#include +#endif + +#if HAS_EEPROM +#include +#endif + +#if HAS_LOG +#include +#endif + +#if HAS_FLIGHT +#include +#include +#endif + +/* + * ao_report.c + */ + +#define AO_RDF_INTERVAL_TICKS AO_SEC_TO_TICKS(5) +#define AO_RDF_LENGTH_MS 500 +#define AO_RDF_CONTINUITY_MS 32 +#define AO_RDF_CONTINUITY_PAUSE 96 +#define AO_RDF_CONTINUITY_TOTAL ((AO_RDF_CONTINUITY_PAUSE + AO_RDF_CONTINUITY_MS) * 3 + AO_RDF_CONTINUITY_PAUSE) + +/* This assumes that we're generating a 1kHz tone, which + * modulates the carrier at 2kbps, or 250kBps + */ +#define AO_MS_TO_RDF_LEN(ms) ((ms) / 4) + +#define AO_RADIO_RDF_LEN AO_MS_TO_RDF_LEN(AO_RDF_LENGTH_MS) +#define AO_RADIO_CONT_TONE_LEN AO_MS_TO_RDF_LEN(AO_RDF_CONTINUITY_MS) +#define AO_RADIO_CONT_PAUSE_LEN AO_MS_TO_RDF_LEN(AO_RDF_CONTINUITY_PAUSE) +#define AO_RADIO_CONT_TOTAL_LEN AO_MS_TO_RDF_LEN(AO_RDF_CONTINUITY_TOTAL) + +/* returns a value 0-3 to indicate igniter continuity */ +uint8_t +ao_report_igniter(void); + +void +ao_report_init(void); + +/* + * ao_convert.c + * + * Given raw data, convert to SI units + */ + +/* pressure from the sensor to altitude in meters */ +int16_t +ao_pres_to_altitude(int16_t pres) __reentrant; + +int16_t +ao_altitude_to_pres(int16_t alt) __reentrant; + +int16_t +ao_temp_to_dC(int16_t temp) __reentrant; + +/* + * ao_convert_pa.c + * + * Convert between pressure in Pa and altitude in meters + */ + +#include + +alt_t +ao_pa_to_altitude(int32_t pa); + +int32_t +ao_altitude_to_pa(alt_t alt); + +#if HAS_DBG +#include +#endif + +#if HAS_SERIAL_0 || HAS_SERIAL_1 || HAS_SERIAL_2 || HAS_SERIAL_3 +#include +#endif + +/* + * ao_convert_volt.c + * + * Convert ADC readings to decivolts + */ + +int16_t +ao_battery_decivolt(int16_t adc); + +int16_t +ao_ignite_decivolt(int16_t adc); + +/* + * ao_spi_slave.c + */ + +uint8_t +ao_spi_slave_recv(void *buf, uint16_t len); + +void +ao_spi_slave_send(void *buf, uint16_t len); + +void +ao_spi_slave_init(void); + +/* This must be defined by the product; it will get called when chip + * select goes low, at which point it should use ao_spi_read and + * ao_spi_write to deal with the request + */ + +void +ao_spi_slave(void); + +#include +/* + * ao_gps.c + */ + +#define AO_GPS_NUM_SAT_MASK (0xf << 0) +#define AO_GPS_NUM_SAT_SHIFT (0) + +#define AO_GPS_VALID (1 << 4) +#define AO_GPS_RUNNING (1 << 5) +#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; +extern __xdata struct ao_telemetry_satellite ao_gps_tracking_data; + +struct ao_gps_orig { + uint8_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t minute; + uint8_t second; + uint8_t flags; + int32_t latitude; /* degrees * 10⁷ */ + int32_t longitude; /* degrees * 10⁷ */ + int16_t altitude; /* m */ + uint16_t ground_speed; /* cm/s */ + uint8_t course; /* degrees / 2 */ + uint8_t hdop; /* * 5 */ + int16_t climb_rate; /* cm/s */ + uint16_t h_error; /* m */ + uint16_t v_error; /* m */ +}; + +struct ao_gps_sat_orig { + uint8_t svid; + uint8_t c_n_1; +}; + +#define AO_MAX_GPS_TRACKING 12 + +struct ao_gps_tracking_orig { + uint8_t channels; + struct ao_gps_sat_orig sats[AO_MAX_GPS_TRACKING]; +}; + +void +ao_gps(void); + +void +ao_gps_print(__xdata struct ao_gps_orig *gps_data); + +void +ao_gps_tracking_print(__xdata struct ao_gps_tracking_orig *gps_tracking_data); + +void +ao_gps_show(void) __reentrant; + +void +ao_gps_init(void); + +/* + * ao_gps_report.c + */ + +void +ao_gps_report(void); + +void +ao_gps_report_init(void); + +/* + * ao_gps_report_mega.c + */ + +void +ao_gps_report_mega(void); + +void +ao_gps_report_mega_init(void); + +/* + * ao_telemetry_orig.c + */ + +#if LEGACY_MONITOR +struct ao_adc_orig { + uint16_t tick; /* tick when the sample was read */ + 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 */ +}; + +struct ao_telemetry_orig { + uint16_t serial; + uint16_t flight; + uint8_t flight_state; + int16_t accel; + int16_t ground_accel; + union { + struct { + int16_t speed; + int16_t unused; + } k; + int32_t flight_vel; + } u; + int16_t height; + int16_t ground_pres; + int16_t accel_plus_g; + int16_t accel_minus_g; + struct ao_adc_orig adc; + struct ao_gps_orig gps; + char callsign[AO_MAX_CALLSIGN]; + struct ao_gps_tracking_orig gps_tracking; +}; + +struct ao_telemetry_tiny { + uint16_t serial; + uint16_t flight; + uint8_t flight_state; + int16_t height; /* AGL in meters */ + int16_t speed; /* in m/s * 16 */ + int16_t accel; /* in m/s² * 16 */ + int16_t ground_pres; /* sensor units */ + struct ao_adc adc; /* raw ADC readings */ + char callsign[AO_MAX_CALLSIGN]; +}; + +struct ao_telemetry_orig_recv { + struct ao_telemetry_orig telemetry_orig; + int8_t rssi; + uint8_t status; +}; + +struct ao_telemetry_tiny_recv { + struct ao_telemetry_tiny telemetry_tiny; + int8_t rssi; + uint8_t status; +}; + +#endif /* LEGACY_MONITOR */ + +/* Unfortunately, we've exposed the CC1111 rssi units as the 'usual' method + * for reporting RSSI. So, now we use these values everywhere + */ +#define AO_RSSI_FROM_RADIO(radio) ((int16_t) ((int8_t) (radio) >> 1) - 74) +#define AO_RADIO_FROM_RSSI(rssi) (((int8_t) (rssi) + 74) << 1) + +/* + * ao_radio_recv tacks on rssi and status bytes + */ + +struct ao_telemetry_raw_recv { + uint8_t packet[AO_MAX_TELEMETRY + 2]; +}; + +/* Set delay between telemetry reports (0 to disable) */ + +#ifdef AO_SEND_ALL_BARO +#define AO_TELEMETRY_INTERVAL_PAD AO_MS_TO_TICKS(100) +#define AO_TELEMETRY_INTERVAL_FLIGHT AO_MS_TO_TICKS(100) +#define AO_TELEMETRY_INTERVAL_RECOVER AO_MS_TO_TICKS(100) +#else +#define AO_TELEMETRY_INTERVAL_PAD AO_MS_TO_TICKS(1000) +#define AO_TELEMETRY_INTERVAL_FLIGHT AO_MS_TO_TICKS(100) +#define AO_TELEMETRY_INTERVAL_RECOVER AO_MS_TO_TICKS(1000) +#endif + +void +ao_telemetry_set_interval(uint16_t interval); + +void +ao_rdf_set(uint8_t rdf); + +void +ao_telemetry_init(void); + +void +ao_telemetry_orig_init(void); + +void +ao_telemetry_tiny_init(void); + +/* + * ao_radio.c + */ + +extern __xdata uint8_t ao_radio_dma; + +extern __xdata int8_t ao_radio_rssi; + +#ifdef PKT_APPEND_STATUS_1_CRC_OK +#define AO_RADIO_STATUS_CRC_OK PKT_APPEND_STATUS_1_CRC_OK +#else +#include +#define AO_RADIO_STATUS_CRC_OK AO_FEC_DECODE_CRC_OK +#endif + +#ifndef HAS_RADIO_RECV +#define HAS_RADIO_RECV HAS_RADIO +#endif +#ifndef HAS_RADIO_XMIT +#define HAS_RADIO_XMIT HAS_RADIO +#endif + +void +ao_radio_general_isr(void) ao_arch_interrupt(16); + +#if HAS_RADIO_XMIT +void +ao_radio_send(const __xdata void *d, uint8_t size) __reentrant; +#endif + +#if HAS_RADIO_RECV +uint8_t +ao_radio_recv(__xdata void *d, uint8_t size, uint8_t timeout) __reentrant; + +void +ao_radio_recv_abort(void); +#endif + +void +ao_radio_test(uint8_t on); + +typedef int16_t (*ao_radio_fill_func)(uint8_t *buffer, int16_t len); + +void +ao_radio_send_aprs(ao_radio_fill_func fill); + +/* + * ao_radio_pa + */ + +#if HAS_RADIO_AMP +void +ao_radio_pa_on(void); + +void +ao_radio_pa_off(void); + +void +ao_radio_pa_init(void); +#else +#define ao_radio_pa_on() +#define ao_radio_pa_off() +#define ao_radio_pa_init() +#endif + +/* + * Compute the packet length as follows: + * + * 2000 bps (for a 1kHz tone) + * so, for 'ms' milliseconds, we need + * 2 * ms bits, or ms / 4 bytes + */ + +void +ao_radio_rdf(void); + +void +ao_radio_continuity(uint8_t c); + +void +ao_radio_rdf_abort(void); + +void +ao_radio_init(void); + +/* + * ao_monitor.c + */ + +#if HAS_MONITOR + +extern const char const * const ao_state_names[]; + +#define AO_MONITOR_RING 8 + +union ao_monitor { + struct ao_telemetry_raw_recv raw; + struct ao_telemetry_all_recv all; +#if LEGACY_MONITOR + struct ao_telemetry_orig_recv orig; + struct ao_telemetry_tiny_recv tiny; +#endif +}; + +extern __xdata union ao_monitor ao_monitor_ring[AO_MONITOR_RING]; + +#define ao_monitor_ring_next(n) (((n) + 1) & (AO_MONITOR_RING - 1)) + +extern __data uint8_t ao_monitoring; +extern __data uint8_t ao_monitor_head; + +void +ao_monitor(void); + +#define AO_MONITORING_OFF 0 +#define AO_MONITORING_ORIG 1 + +void +ao_monitor_set(uint8_t monitoring); + +void +ao_monitor_disable(void); + +void +ao_monitor_enable(void); + +void +ao_monitor_init(void) __reentrant; + +#endif + +/* + * ao_stdio.c + */ + +#define AO_READ_AGAIN (-1) + +struct ao_stdio { + int (*_pollchar)(void); /* Called with interrupts blocked */ + void (*putchar)(char c) __reentrant; + void (*flush)(void); + uint8_t echo; +}; + +extern __xdata struct ao_stdio ao_stdios[]; +extern __pdata int8_t ao_cur_stdio; +extern __pdata int8_t ao_num_stdios; + +void +flush(void); + +extern __xdata uint8_t ao_stdin_ready; + +uint8_t +ao_echo(void); + +int8_t +ao_add_stdio(int (*pollchar)(void), + void (*putchar)(char) __reentrant, + void (*flush)(void)) __reentrant; + +/* + * ao_ignite.c + */ + +enum ao_igniter { + ao_igniter_drogue = 0, + ao_igniter_main = 1 +}; + +void +ao_ignite(enum ao_igniter igniter); + +enum ao_igniter_status { + ao_igniter_unknown, /* unknown status (ambiguous voltage) */ + ao_igniter_ready, /* continuity detected */ + ao_igniter_active, /* igniter firing */ + ao_igniter_open, /* open circuit detected */ +}; + +struct ao_ignition { + uint8_t request; + uint8_t fired; + uint8_t firing; +}; + +extern __code char * __code ao_igniter_status_names[]; + +extern __xdata struct ao_ignition ao_ignition[2]; + +enum ao_igniter_status +ao_igniter_status(enum ao_igniter igniter); + +extern __pdata uint8_t ao_igniter_present; + +void +ao_ignite_set_pins(void); + +void +ao_igniter_init(void); + +/* + * ao_config.c + */ + +#if AO_PYRO_NUM +#include +#endif + +#if HAS_FORCE_FREQ +/* + * Set this to force the frequency to 434.550MHz + */ +extern __xdata uint8_t ao_force_freq; +#endif + +#define AO_CONFIG_MAJOR 1 +#define AO_CONFIG_MINOR 15 + +#define AO_AES_LEN 16 + +extern __xdata uint8_t ao_config_aes_seq; + +struct ao_config { + uint8_t major; + uint8_t minor; + uint16_t main_deploy; + int16_t accel_plus_g; /* changed for minor version 2 */ + uint8_t _legacy_radio_channel; + char callsign[AO_MAX_CALLSIGN + 1]; + uint8_t apogee_delay; /* minor version 1 */ + int16_t accel_minus_g; /* minor version 2 */ + uint32_t radio_cal; /* minor version 3 */ + uint32_t flight_log_max; /* minor version 4 */ + uint8_t ignite_mode; /* minor version 5 */ + uint8_t pad_orientation; /* minor version 6 */ + uint32_t radio_setting; /* minor version 7 */ + uint8_t radio_enable; /* minor version 8 */ + uint8_t aes_key[AO_AES_LEN]; /* minor version 9 */ + uint32_t frequency; /* minor version 10 */ + uint16_t apogee_lockout; /* minor version 11 */ +#if AO_PYRO_NUM + struct ao_pyro pyro[AO_PYRO_NUM]; /* minor version 12 */ +#endif + uint16_t aprs_interval; /* minor version 13 */ +#if HAS_RADIO_POWER + uint8_t radio_power; /* minor version 14 */ +#endif +#if HAS_RADIO_AMP + uint8_t radio_amp; /* minor version 14 */ +#endif +#if HAS_GYRO + int16_t accel_zero_along; /* minor version 15 */ + int16_t accel_zero_across; /* minor version 15 */ + int16_t accel_zero_through; /* minor version 15 */ +#endif +}; + +#define AO_IGNITE_MODE_DUAL 0 +#define AO_IGNITE_MODE_APOGEE 1 +#define AO_IGNITE_MODE_MAIN 2 + +#define AO_RADIO_ENABLE_CORE 1 +#define AO_RADIO_DISABLE_TELEMETRY 2 +#define AO_RADIO_DISABLE_RDF 4 + +#define AO_PAD_ORIENTATION_ANTENNA_UP 0 +#define AO_PAD_ORIENTATION_ANTENNA_DOWN 1 + +extern __xdata struct ao_config ao_config; + +#define AO_CONFIG_MAX_SIZE 128 + +void +_ao_config_edit_start(void); + +void +_ao_config_edit_finish(void); + +void +ao_config_get(void); + +void +ao_config_put(void); + +void +ao_config_set_radio(void); + +void +ao_config_init(void); + +/* + * ao_rssi.c + */ + +void +ao_rssi_set(int rssi_value); + +void +ao_rssi_init(uint8_t rssi_led); + +/* + * ao_product.c + * + * values which need to be defined for + * each instance of a product + */ + +extern const char ao_version[]; +extern const char ao_manufacturer[]; +extern const char ao_product[]; + +/* + * Fifos + */ + +#define AO_FIFO_SIZE 32 + +struct ao_fifo { + uint8_t insert; + uint8_t remove; + char fifo[AO_FIFO_SIZE]; +}; + +#define ao_fifo_insert(f,c) do { \ + (f).fifo[(f).insert] = (c); \ + (f).insert = ((f).insert + 1) & (AO_FIFO_SIZE-1); \ +} while(0) + +#define ao_fifo_remove(f,c) do {\ + c = (f).fifo[(f).remove]; \ + (f).remove = ((f).remove + 1) & (AO_FIFO_SIZE-1); \ +} while(0) + +#define ao_fifo_full(f) ((((f).insert + 1) & (AO_FIFO_SIZE-1)) == (f).remove) +#define ao_fifo_empty(f) ((f).insert == (f).remove) + +#if PACKET_HAS_MASTER || PACKET_HAS_SLAVE +#include +#endif + +#if HAS_BTM +#include +#endif + +#if HAS_COMPANION +#include +#endif + +#if HAS_LCD +#include +#endif + +#if HAS_AES +#include +#endif + +/* ao_launch.c */ + +struct ao_launch_command { + uint16_t tick; + uint16_t serial; + uint8_t cmd; + uint8_t channel; + uint16_t unused; +}; + +#define AO_LAUNCH_QUERY 1 + +struct ao_launch_query { + uint16_t tick; + uint16_t serial; + uint8_t channel; + uint8_t valid; + uint8_t arm_status; + uint8_t igniter_status; +}; + +#define AO_LAUNCH_ARM 2 +#define AO_LAUNCH_FIRE 3 + +void +ao_launch_init(void); + +/* + * ao_log_single.c + */ + +#define AO_LOG_TELESCIENCE_START ((uint8_t) 's') +#define AO_LOG_TELESCIENCE_DATA ((uint8_t) 'd') + +#define AO_LOG_TELESCIENCE_NUM_ADC 12 + +struct ao_log_telescience { + uint8_t type; + uint8_t csum; + uint16_t tick; + uint16_t tm_tick; + uint8_t tm_state; + uint8_t unused; + uint16_t adc[AO_LOG_TELESCIENCE_NUM_ADC]; +}; + +#define AO_LOG_SINGLE_SIZE 32 + +union ao_log_single { + struct ao_log_telescience telescience; + union ao_telemetry_all telemetry; + uint8_t bytes[AO_LOG_SINGLE_SIZE]; +}; + +extern __xdata union ao_log_single ao_log_single_write_data; +extern __xdata union ao_log_single ao_log_single_read_data; + +void +ao_log_single_extra_query(void); + +void +ao_log_single_list(void); + +void +ao_log_single_main(void); + +uint8_t +ao_log_single_write(void); + +uint8_t +ao_log_single_read(uint32_t pos); + +void +ao_log_single_start(void); + +void +ao_log_single_stop(void); + +void +ao_log_single_restart(void); + +void +ao_log_single_set(void); + +void +ao_log_single_delete(void); + +void +ao_log_single_init(void); + +void +ao_log_single(void); + +/* + * ao_pyro_slave.c + */ + +#define AO_TELEPYRO_NUM_ADC 9 + +#ifndef ao_xmemcpy +#define ao_xmemcpy(d,s,c) memcpy(d,s,c) +#define ao_xmemset(d,v,c) memset(d,v,c) +#define ao_xmemcmp(d,s,c) memcmp(d,s,c) +#endif + +/* + * ao_terraui.c + */ + +void +ao_terraui_init(void); + +/* + * ao_battery.c + */ + +#ifdef BATTERY_PIN +void +ao_battery_isr(void) ao_arch_interrupt(1); + +uint16_t +ao_battery_get(void); + +void +ao_battery_init(void); +#endif /* BATTERY_PIN */ + +/* + * ao_sqrt.c + */ + +uint32_t +ao_sqrt(uint32_t op); + +/* + * ao_freq.c + */ + +int32_t ao_freq_to_set(int32_t freq, int32_t cal) __reentrant; + +/* + * ao_ms5607.c + */ + +void ao_ms5607_init(void); + +#include + +#endif /* _AO_H_ */ diff --git a/src/kernel/ao_adc.h b/src/kernel/ao_adc.h new file mode 100644 index 00000000..373db1c4 --- /dev/null +++ b/src/kernel/ao_adc.h @@ -0,0 +1,35 @@ +/* + * 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. + */ + +#ifndef _AO_ADC_H_ +#define _AO_ADC_H_ + +#include + +/* Trigger a conversion sequence (called from the timer interrupt) */ +void +ao_adc_poll(void); + +/* Suspend the current task until another A/D sample is converted */ +void +ao_adc_sleep(void); + +/* Initialize the A/D converter */ +void +ao_adc_init(void); + +#endif /* _AO_ADC_H_ */ diff --git a/src/kernel/ao_aes.h b/src/kernel/ao_aes.h new file mode 100644 index 00000000..c47bc2db --- /dev/null +++ b/src/kernel/ao_aes.h @@ -0,0 +1,54 @@ +/* + * 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. + */ + +#ifndef _AO_AES_H_ +#define _AO_AES_H_ + +/* ao_aes.c */ + +extern __xdata uint8_t ao_aes_mutex; + +/* AES keys and blocks are 128 bits */ + +enum ao_aes_mode { + ao_aes_mode_cbc_mac +}; + +#if HAS_AES +#ifdef SDCC +void +ao_aes_isr(void) __interrupt 4; +#endif +#endif + +void +ao_aes_set_mode(enum ao_aes_mode mode); + +void +ao_aes_set_key(__xdata uint8_t *in); + +void +ao_aes_zero_iv(void); + +void +ao_aes_run(__xdata uint8_t *in, + __xdata uint8_t *out); + +void +ao_aes_init(void); + +#endif /* _AO_AES_H_ */ diff --git a/src/kernel/ao_balloon.c b/src/kernel/ao_balloon.c new file mode 100644 index 00000000..904a9c08 --- /dev/null +++ b/src/kernel/ao_balloon.c @@ -0,0 +1,136 @@ +/* + * Copyright © 2009 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_FLIGHT_TEST +#include "ao.h" +#endif + +#ifndef HAS_ACCEL +#error Please define HAS_ACCEL +#endif + +#ifndef HAS_GPS +#error Please define HAS_GPS +#endif + +#ifndef HAS_USB +#error Please define HAS_USB +#endif + +#if HAS_SENSOR_ERRORS +/* Any sensor can set this to mark the flight computer as 'broken' */ +__xdata uint8_t ao_sensor_errors; +#endif + +__pdata uint16_t ao_motor_number; /* number of motors burned so far */ + +/* Main flight thread. */ + +__pdata enum ao_flight_state ao_flight_state; /* current flight state */ + +__pdata uint8_t ao_flight_force_idle; + +void +ao_flight(void) +{ + ao_sample_init(); + ao_flight_state = ao_flight_startup; + for (;;) { + + /* + * Process ADC samples, just looping + * until the sensors are calibrated. + */ + if (!ao_sample()) + continue; + + switch (ao_flight_state) { + case ao_flight_startup: + + /* Check to see what mode we should go to. + * - Invalid mode if accel cal appears to be out + * - pad mode if we're upright, + * - idle mode otherwise + */ + if (!ao_flight_force_idle) + { + /* Set pad mode - we can fly! */ + ao_flight_state = ao_flight_pad; +#if HAS_USB + /* Disable the USB controller in flight mode + * to save power + */ + if (!ao_usb_running) + ao_usb_disable(); +#endif + + /* Disable packet mode in pad state */ + ao_packet_slave_stop(); + + /* Turn on telemetry system */ + ao_rdf_set(1); + ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_BALLOON); + + /* signal successful initialization by turning off the LED */ + ao_led_off(AO_LED_RED); + } else { + /* Set idle mode */ + ao_flight_state = ao_flight_idle; + + /* signal successful initialization by turning off the LED */ + ao_led_off(AO_LED_RED); + } + /* wakeup threads due to state change */ + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + + break; + case ao_flight_pad: + + /* pad to coast: + * + * barometer: > 20m vertical motion + */ + if (ao_height > AO_M_TO_HEIGHT(20)) + { + ao_flight_state = ao_flight_drogue; + + /* start logging data */ + ao_log_start(); + +#if HAS_GPS + /* Record current GPS position by waking up GPS log tasks */ + 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)); + } + break; + default: + break; + } + } +} + +static __xdata struct ao_task flight_task; + +void +ao_flight_init(void) +{ + ao_flight_state = ao_flight_startup; + ao_add_task(&flight_task, ao_flight, "flight"); +} diff --git a/src/kernel/ao_beep.h b/src/kernel/ao_beep.h new file mode 100644 index 00000000..55f61171 --- /dev/null +++ b/src/kernel/ao_beep.h @@ -0,0 +1,74 @@ +/* + * 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. + */ + +#ifndef _AO_BEEP_H_ +#define _AO_BEEP_H_ + +/* + * ao_beep.c + */ + +/* + * Various pre-defined beep frequencies + * + * frequency = 1/2 (24e6/32) / beep + */ + +#define AO_BEEP_LOW 150 /* 2500Hz */ +#define AO_BEEP_MID 94 /* 3989Hz */ +#define AO_BEEP_HIGH 75 /* 5000Hz */ +#define AO_BEEP_OFF 0 /* off */ + +#define AO_BEEP_g 240 /* 1562.5Hz */ +#define AO_BEEP_gs 227 /* 1652Hz (1655Hz) */ +#define AO_BEEP_aa 214 /* 1752Hz (1754Hz) */ +#define AO_BEEP_bbf 202 /* 1856Hz (1858Hz) */ +#define AO_BEEP_bb 190 /* 1974Hz (1969Hz) */ +#define AO_BEEP_cc 180 /* 2083Hz (2086Hz) */ +#define AO_BEEP_ccs 170 /* 2205Hz (2210Hz) */ +#define AO_BEEP_dd 160 /* 2344Hz (2341Hz) */ +#define AO_BEEP_eef 151 /* 2483Hz (2480Hz) */ +#define AO_BEEP_ee 143 /* 2622Hz (2628Hz) */ +#define AO_BEEP_ff 135 /* 2778Hz (2784Hz) */ +#define AO_BEEP_ffs 127 /* 2953Hz (2950Hz) */ +#define AO_BEEP_gg 120 /* 3125Hz */ +#define AO_BEEP_ggs 113 /* 3319Hz (3311Hz) */ +#define AO_BEEP_aaa 107 /* 3504Hz (3508Hz) */ +#define AO_BEEP_bbbf 101 /* 3713Hz (3716Hz) */ +#define AO_BEEP_bbb 95 /* 3947Hz (3937Hz) */ +#define AO_BEEP_ccc 90 /* 4167Hz (4171Hz) */ +#define AO_BEEP_cccs 85 /* 4412Hz (4419Hz) */ +#define AO_BEEP_ddd 80 /* 4688Hz (4682Hz) */ +#define AO_BEEP_eeef 76 /* 4934Hz (4961Hz) */ +#define AO_BEEP_eee 71 /* 5282Hz (5256Hz) */ +#define AO_BEEP_fff 67 /* 5597Hz (5568Hz) */ +#define AO_BEEP_fffs 64 /* 5859Hz (5899Hz) */ +#define AO_BEEP_ggg 60 /* 6250Hz */ + +/* Set the beeper to the specified tone */ +void +ao_beep(uint8_t beep); + +/* Turn on the beeper for the specified time */ +void +ao_beep_for(uint8_t beep, uint16_t ticks) __reentrant; + +/* Initialize the beeper */ +void +ao_beep_init(void); + +#endif /* _AO_BEEP_H_ */ diff --git a/src/kernel/ao_btm.h b/src/kernel/ao_btm.h new file mode 100644 index 00000000..484e5d7f --- /dev/null +++ b/src/kernel/ao_btm.h @@ -0,0 +1,36 @@ +/* + * 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. + */ + +#ifndef _AO_BTM_H_ +#define _AO_BTM_H_ + +/* ao_btm.c */ + +/* If bt_link is on P2, this interrupt is shared by USB, so the USB + * code calls this function. Otherwise, it's a regular ISR. + */ + +void +ao_btm_isr(void) +#if BT_LINK_ON_P1 + __interrupt 15 +#endif + ; +void +ao_btm_init(void); + +#endif /* _AO_BTM_H_ */ diff --git a/src/kernel/ao_cmd.c b/src/kernel/ao_cmd.c new file mode 100644 index 00000000..4ebaa607 --- /dev/null +++ b/src/kernel/ao_cmd.c @@ -0,0 +1,420 @@ +/* + * Copyright © 2009 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_task.h" + +__pdata uint16_t ao_cmd_lex_i; +__pdata uint32_t ao_cmd_lex_u32; +__pdata char ao_cmd_lex_c; +__pdata enum ao_cmd_status ao_cmd_status; + +#define CMD_LEN 48 + +static __xdata char cmd_line[CMD_LEN]; +static __pdata uint8_t cmd_len; +static __pdata uint8_t cmd_i; + +void +ao_put_string(__code char *s) +{ + char c; + while ((c = *s++)) + putchar(c); +} + +static void +backspace(void) +{ + ao_put_string ("\010 \010"); +} + +static void +readline(void) +{ + char c; + if (ao_echo()) + ao_put_string("> "); + cmd_len = 0; + for (;;) { + flush(); + c = getchar(); + /* backspace/delete */ + if (c == '\010' || c == '\177') { + if (cmd_len != 0) { + if (ao_echo()) + backspace(); + --cmd_len; + } + continue; + } + + /* ^U */ + if (c == '\025') { + while (cmd_len != 0) { + if (ao_echo()) + backspace(); + --cmd_len; + } + continue; + } + + /* map CR to NL */ + if (c == '\r') + c = '\n'; + + if (c == '\n') { + if (ao_echo()) + putchar('\n'); + break; + } + + if (cmd_len >= CMD_LEN - 2) + continue; + cmd_line[cmd_len++] = c; + if (ao_echo()) + putchar(c); + } + cmd_line[cmd_len++] = '\n'; + cmd_line[cmd_len++] = '\0'; + cmd_i = 0; +} + +void +ao_cmd_lex(void) +{ + ao_cmd_lex_c = '\n'; + if (cmd_i < cmd_len) + ao_cmd_lex_c = cmd_line[cmd_i++]; +} + +static void +putnibble(uint8_t v) +{ + if (v < 10) + putchar(v + '0'); + else + 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) +{ + ao_cmd_put8(v >> 8); + ao_cmd_put8(v); +} + +void +ao_cmd_put8(uint8_t v) +{ + putnibble((v >> 4) & 0xf); + putnibble(v & 0xf); +} + +uint8_t +ao_cmd_is_white(void) +{ + return ao_cmd_lex_c == ' ' || ao_cmd_lex_c == '\t'; +} + +void +ao_cmd_white(void) +{ + while (ao_cmd_is_white()) + ao_cmd_lex(); +} + +int8_t +ao_cmd_hexchar(char c) +{ + 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); + return -1; +} + +void +ao_cmd_hexbyte(void) +{ + uint8_t i; + int8_t n; + + ao_cmd_lex_i = 0; + ao_cmd_white(); + for (i = 0; i < 2; i++) { + n = ao_cmd_hexchar(ao_cmd_lex_c); + if (n < 0) { + ao_cmd_status = ao_cmd_syntax_error; + break; + } + ao_cmd_lex_i = (ao_cmd_lex_i << 4) | n; + ao_cmd_lex(); + } +} + +void +ao_cmd_hex(void) +{ + __pdata uint8_t r = ao_cmd_lex_error; + int8_t n; + + ao_cmd_lex_i = 0; + ao_cmd_white(); + for(;;) { + n = ao_cmd_hexchar(ao_cmd_lex_c); + if (n < 0) + break; + ao_cmd_lex_i = (ao_cmd_lex_i << 4) | n; + r = ao_cmd_success; + ao_cmd_lex(); + } + if (r != ao_cmd_success) + ao_cmd_status = r; +} + +void +ao_cmd_decimal(void) __reentrant +{ + uint8_t r = ao_cmd_lex_error; + + ao_cmd_lex_u32 = 0; + ao_cmd_white(); + for(;;) { + if ('0' <= ao_cmd_lex_c && ao_cmd_lex_c <= '9') + ao_cmd_lex_u32 = (ao_cmd_lex_u32 * 10) + (ao_cmd_lex_c - '0'); + else + break; + r = ao_cmd_success; + ao_cmd_lex(); + } + if (r != ao_cmd_success) + ao_cmd_status = r; + ao_cmd_lex_i = (uint16_t) ao_cmd_lex_u32; +} + +uint8_t +ao_match_word(__code char *word) +{ + while (*word) { + if (ao_cmd_lex_c != *word) { + ao_cmd_status = ao_cmd_syntax_error; + return 0; + } + word++; + ao_cmd_lex(); + } + return 1; +} + +static void +echo(void) +{ + ao_cmd_hex(); + if (ao_cmd_status == ao_cmd_success) + ao_stdios[ao_cur_stdio].echo = ao_cmd_lex_i != 0; +} + +static void +ao_reboot(void) +{ + ao_cmd_white(); + if (!ao_match_word("eboot")) + return; + /* Delay waiting for the packet master to be turned off + * so that we don't end up back in idle mode because we + * received a packet after boot. + */ + flush(); + ao_delay(AO_SEC_TO_TICKS(1)); + ao_arch_reboot(); + ao_panic(AO_PANIC_REBOOT); +} + +#ifndef HAS_VERSION +#define HAS_VERSION 1 +#endif + +#if HAS_VERSION +static void +version(void) +{ + 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 + , ao_log_format +#endif + ); + printf("software-version %s\n", ao_version); +} +#endif + +#ifndef NUM_CMDS +#define NUM_CMDS 11 +#endif + +static __code struct ao_cmds *__xdata (ao_cmds[NUM_CMDS]); +static __pdata uint8_t ao_ncmds; + +static void +help(void) +{ + __pdata uint8_t cmds; + __pdata uint8_t cmd; + __code struct ao_cmds * __pdata cs; + __code const char *h; + uint8_t e; + + for (cmds = 0; cmds < ao_ncmds; cmds++) { + cs = ao_cmds[cmds]; + for (cmd = 0; cs[cmd].func; cmd++) { + h = cs[cmd].help; + ao_put_string(h); + e = strlen(h); + h += e + 1; + e = 45 - e; + while (e--) + putchar(' '); + ao_put_string(h); + putchar('\n'); + } + } +} + +static void +report(void) +{ + switch(ao_cmd_status) { + case ao_cmd_lex_error: + case ao_cmd_syntax_error: + puts("Syntax error"); + ao_cmd_status = 0; + default: + break; + } +} + +void +ao_cmd_register(__code struct ao_cmds *cmds) +{ + if (ao_ncmds >= NUM_CMDS) + ao_panic(AO_PANIC_CMD); + ao_cmds[ao_ncmds++] = cmds; +} + +void +ao_cmd(void) +{ + __pdata char c; + uint8_t cmd, cmds; + __code struct ao_cmds * __xdata cs; + void (*__xdata func)(void); + + for (;;) { + readline(); + ao_cmd_lex(); + ao_cmd_white(); + c = ao_cmd_lex_c; + ao_cmd_lex(); + if (c == '\r' || c == '\n') + continue; + func = (void (*)(void)) NULL; + for (cmds = 0; cmds < ao_ncmds; cmds++) { + cs = ao_cmds[cmds]; + for (cmd = 0; cs[cmd].func; cmd++) + if (cs[cmd].help[0] == c) { + func = cs[cmd].func; + break; + } + if (func) + break; + } + if (func) + (*func)(); + else + ao_cmd_status = ao_cmd_syntax_error; + report(); + } +} + +#if HAS_BOOT_LOADER + +#include + +static void +ao_loader(void) +{ + flush(); + ao_boot_loader(); +} +#endif + +__xdata struct ao_task ao_cmd_task; + +__code struct ao_cmds ao_base_cmds[] = { + { help, "?\0Help" }, +#if HAS_TASK_INFO + { ao_task_info, "T\0Tasks" }, +#endif + { echo, "E <0 off, 1 on>\0Echo" }, + { ao_reboot, "r eboot\0Reboot" }, +#if HAS_VERSION + { version, "v\0Version" }, +#endif +#if HAS_BOOT_LOADER + { ao_loader, "X\0Switch to boot loader" }, +#endif + { 0, NULL }, +}; + +void +ao_cmd_init(void) +{ + ao_cmd_register(&ao_base_cmds[0]); + ao_add_task(&ao_cmd_task, ao_cmd, "cmd"); +} diff --git a/src/kernel/ao_companion.h b/src/kernel/ao_companion.h new file mode 100644 index 00000000..035325a3 --- /dev/null +++ b/src/kernel/ao_companion.h @@ -0,0 +1,55 @@ +/* + * 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. + */ + +#ifndef _AO_COMPANION_H_ +#define _AO_COMPANION_H_ + +/* ao_companion.c */ + +#define AO_COMPANION_SETUP 1 +#define AO_COMPANION_FETCH 2 +#define AO_COMPANION_NOTIFY 3 + +struct ao_companion_command { + uint8_t command; + uint8_t flight_state; + uint16_t tick; + uint16_t serial; + uint16_t flight; + int16_t accel; + int16_t speed; + int16_t height; + int16_t motor_number; +}; + +struct ao_companion_setup { + uint16_t board_id; + uint16_t board_id_inverse; + uint8_t update_period; + uint8_t channels; +}; + +extern __pdata uint8_t ao_companion_running; +extern __xdata uint8_t ao_companion_mutex; +extern __xdata struct ao_companion_command ao_companion_command; +extern __xdata struct ao_companion_setup ao_companion_setup; +extern __xdata uint16_t ao_companion_data[AO_COMPANION_MAX_CHANNELS]; + +void +ao_companion_init(void); + +#endif /* _AO_COMPANION_H_ */ diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c new file mode 100644 index 00000000..4482f673 --- /dev/null +++ b/src/kernel/ao_config.c @@ -0,0 +1,805 @@ +/* + * Copyright © 2009 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_log.h" +#include +#if HAS_FLIGHT +#include +#include +#endif + +__xdata struct ao_config ao_config; +__pdata uint8_t ao_config_loaded; +__pdata uint8_t ao_config_dirty; +__xdata uint8_t ao_config_mutex; + +#ifndef AO_CONFIG_DEFAULT_APRS_INTERVAL +#define AO_CONFIG_DEFAULT_APRS_INTERVAL 0 +#endif +#define AO_CONFIG_DEFAULT_MAIN_DEPLOY 250 +#define AO_CONFIG_DEFAULT_RADIO_CHANNEL 0 +#define AO_CONFIG_DEFAULT_CALLSIGN "N0CALL" +#define AO_CONFIG_DEFAULT_ACCEL_ZERO_G 16000 +#define AO_CONFIG_DEFAULT_APOGEE_DELAY 0 +#define AO_CONFIG_DEFAULT_IGNITE_MODE AO_IGNITE_MODE_DUAL +#define AO_CONFIG_DEFAULT_PAD_ORIENTATION AO_PAD_ORIENTATION_ANTENNA_UP +#if HAS_EEPROM +#ifndef USE_INTERNAL_FLASH +#error Please define USE_INTERNAL_FLASH +#endif +#endif +#ifndef AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX +#if USE_INTERNAL_FLASH +#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX ao_storage_config +#else +#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX ((uint32_t) 192 * (uint32_t) 1024) +#endif +#endif +#ifndef AO_CONFIG_DEFAULT_RADIO_POWER +#define AO_CONFIG_DEFAULT_RADIO_POWER 0x60 +#endif +#define AO_CONFIG_DEFAULT_RADIO_AMP 0 + +#if HAS_EEPROM +static void +_ao_config_put(void) +{ + ao_config_setup(); + ao_config_erase(); + ao_config_write(0, &ao_config, sizeof (ao_config)); +#if HAS_FLIGHT + ao_log_write_erase(0); +#endif + ao_config_flush(); +} + +void +ao_config_put(void) +{ + ao_mutex_get(&ao_config_mutex); + _ao_config_put(); + ao_mutex_put(&ao_config_mutex); +} +#endif + +#if HAS_RADIO +void +ao_config_set_radio(void) +{ + ao_config.radio_setting = ao_freq_to_set(ao_config.frequency, ao_config.radio_cal); +} +#endif /* HAS_RADIO */ + +static void +_ao_config_get(void) +{ + uint8_t minor; + + if (ao_config_loaded) + return; +#if HAS_EEPROM + /* Yes, I know ao_storage_read calls ao_storage_setup, + * but ao_storage_setup *also* sets ao_storage_config, which we + * need before calling ao_storage_read here + */ + ao_config_setup(); + ao_config_read(0, &ao_config, sizeof (ao_config)); +#endif + if (ao_config.major != AO_CONFIG_MAJOR) { + ao_config.major = AO_CONFIG_MAJOR; + ao_config.minor = 0; + + /* Version 0 stuff */ + ao_config.main_deploy = AO_CONFIG_DEFAULT_MAIN_DEPLOY; + ao_xmemset(&ao_config.callsign, '\0', sizeof (ao_config.callsign)); + ao_xmemcpy(&ao_config.callsign, CODE_TO_XDATA(AO_CONFIG_DEFAULT_CALLSIGN), + sizeof(AO_CONFIG_DEFAULT_CALLSIGN) - 1); + ao_config._legacy_radio_channel = 0; + } + minor = ao_config.minor; + if (minor != AO_CONFIG_MINOR) { + /* Fixups for minor version 1 */ + if (minor < 1) + ao_config.apogee_delay = AO_CONFIG_DEFAULT_APOGEE_DELAY; + /* Fixups for minor version 2 */ + if (minor < 2) { + ao_config.accel_plus_g = 0; + ao_config.accel_minus_g = 0; + } + /* Fixups for minor version 3 */ +#if HAS_RADIO + if (minor < 3) + ao_config.radio_cal = ao_radio_cal; +#endif + /* Fixups for minor version 4 */ +#if HAS_FLIGHT + if (minor < 4) + ao_config.flight_log_max = AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX; +#endif + /* Fixupes for minor version 5 */ + if (minor < 5) + ao_config.ignite_mode = AO_CONFIG_DEFAULT_IGNITE_MODE; + if (minor < 6) + ao_config.pad_orientation = AO_CONFIG_DEFAULT_PAD_ORIENTATION; + if (minor < 8) + ao_config.radio_enable = AO_RADIO_ENABLE_CORE; + if (minor < 9) + ao_xmemset(&ao_config.aes_key, '\0', AO_AES_LEN); + if (minor < 10) + ao_config.frequency = 434550 + ao_config._legacy_radio_channel * 100; + if (minor < 11) + ao_config.apogee_lockout = 0; +#if AO_PYRO_NUM + if (minor < 12) + memset(&ao_config.pyro, '\0', sizeof (ao_config.pyro)); +#endif + if (minor < 13) + ao_config.aprs_interval = AO_CONFIG_DEFAULT_APRS_INTERVAL; +#if HAS_RADIO_POWER + if (minor < 14) + ao_config.radio_power = AO_CONFIG_DEFAULT_RADIO_POWER; + #endif +#if HAS_RADIO_AMP + if (minor < 14) + ao_config.radio_amp = AO_CONFIG_DEFAULT_RADIO_AMP; +#endif +#if HAS_GYRO + if (minor < 15) { + ao_config.accel_zero_along = 0; + ao_config.accel_zero_across = 0; + ao_config.accel_zero_through = 0; + + /* Reset the main accel offsets to force + * re-calibration + */ + ao_config.accel_plus_g = 0; + ao_config.accel_minus_g = 0; + } +#endif + ao_config.minor = AO_CONFIG_MINOR; + ao_config_dirty = 1; + } +#if HAS_RADIO +#if HAS_FORCE_FREQ + if (ao_force_freq) { + ao_config.frequency = 434550; + ao_config.radio_cal = ao_radio_cal; + ao_xmemcpy(&ao_config.callsign, CODE_TO_XDATA(AO_CONFIG_DEFAULT_CALLSIGN), + sizeof(AO_CONFIG_DEFAULT_CALLSIGN) - 1); + } +#endif + ao_config_set_radio(); +#endif + ao_config_loaded = 1; +} + +void +_ao_config_edit_start(void) +{ + ao_mutex_get(&ao_config_mutex); + _ao_config_get(); +} + +void +_ao_config_edit_finish(void) +{ + ao_config_dirty = 1; + ao_mutex_put(&ao_config_mutex); +} + +void +ao_config_get(void) +{ + _ao_config_edit_start(); + ao_mutex_put(&ao_config_mutex); +} + +void +ao_config_callsign_show(void) +{ + printf ("Callsign: \"%s\"\n", ao_config.callsign); +} + +void +ao_config_callsign_set(void) __reentrant +{ + uint8_t c; + static __xdata char callsign[AO_MAX_CALLSIGN + 1]; + + ao_xmemset(callsign, '\0', sizeof callsign); + ao_cmd_white(); + c = 0; + while (ao_cmd_lex_c != '\n') { + if (c < AO_MAX_CALLSIGN) + callsign[c++] = ao_cmd_lex_c; + else + ao_cmd_status = ao_cmd_lex_error; + ao_cmd_lex(); + } + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_xmemcpy(&ao_config.callsign, &callsign, + AO_MAX_CALLSIGN + 1); + _ao_config_edit_finish(); +} + +#if HAS_RADIO + +void +ao_config_frequency_show(void) __reentrant +{ + printf("Frequency: %ld\n", + ao_config.frequency); +} + +void +ao_config_frequency_set(void) __reentrant +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.frequency = ao_cmd_lex_u32; + ao_config_set_radio(); + _ao_config_edit_finish(); +#if HAS_RADIO_RECV + ao_radio_recv_abort(); +#endif +} +#endif + +#if HAS_FLIGHT + +void +ao_config_main_deploy_show(void) __reentrant +{ + printf("Main deploy: %d meters\n", + ao_config.main_deploy); +} + +void +ao_config_main_deploy_set(void) __reentrant +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.main_deploy = ao_cmd_lex_i; + _ao_config_edit_finish(); +} + +#if HAS_ACCEL +void +ao_config_accel_calibrate_show(void) __reentrant +{ + printf("Accel cal +1g: %d -1g: %d\n", + ao_config.accel_plus_g, ao_config.accel_minus_g); +#if HAS_GYRO + printf ("IMU cal along %d across %d through %d\n", + ao_config.accel_zero_along, + ao_config.accel_zero_across, + ao_config.accel_zero_through); +#endif +} + +#define ACCEL_CALIBRATE_SAMPLES 1024 +#define ACCEL_CALIBRATE_SHIFT 10 + +#if HAS_GYRO +static int16_t accel_cal_along; +static int16_t accel_cal_across; +static int16_t accel_cal_through; +#endif + +static int16_t +ao_config_accel_calibrate_auto(char *orientation) __reentrant +{ + uint16_t i; + int32_t accel_total; + uint8_t cal_data_ring; +#if HAS_GYRO + int32_t accel_along_total = 0; + int32_t accel_across_total = 0; + int32_t accel_through_total = 0; +#endif + + printf("Orient antenna %s and press a key...", orientation); + flush(); + (void) getchar(); + puts("\r\n"); flush(); + puts("Calibrating..."); flush(); + i = ACCEL_CALIBRATE_SAMPLES; + accel_total = 0; + cal_data_ring = ao_sample_data; + while (i) { + ao_sleep(DATA_TO_XDATA(&ao_sample_data)); + while (i && cal_data_ring != ao_sample_data) { + accel_total += (int32_t) ao_data_accel(&ao_data_ring[cal_data_ring]); +#if HAS_GYRO + accel_along_total += (int32_t) ao_data_along(&ao_data_ring[cal_data_ring]); + accel_across_total += (int32_t) ao_data_across(&ao_data_ring[cal_data_ring]); + accel_through_total += (int32_t) ao_data_through(&ao_data_ring[cal_data_ring]); +#endif + cal_data_ring = ao_data_ring_next(cal_data_ring); + i--; + } + } +#if HAS_GYRO + accel_cal_along = accel_along_total >> ACCEL_CALIBRATE_SHIFT; + accel_cal_across = accel_across_total >> ACCEL_CALIBRATE_SHIFT; + accel_cal_through = accel_through_total >> ACCEL_CALIBRATE_SHIFT; +#endif + return accel_total >> ACCEL_CALIBRATE_SHIFT; +} + +void +ao_config_accel_calibrate_set(void) __reentrant +{ + int16_t up, down; +#if HAS_GYRO + int16_t accel_along_up = 0, accel_along_down = 0; + int16_t accel_across_up = 0, accel_across_down = 0; + int16_t accel_through_up = 0, accel_through_down = 0; +#endif + + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + if (ao_cmd_lex_i == 0) { + up = ao_config_accel_calibrate_auto("up"); +#if HAS_GYRO + accel_along_up = accel_cal_along; + accel_across_up = accel_cal_across; + accel_through_up = accel_cal_through; +#endif + down = ao_config_accel_calibrate_auto("down"); +#if HAS_GYRO + accel_along_down = accel_cal_along; + accel_across_down = accel_cal_across; + accel_through_down = accel_cal_through; +#endif + } else { + up = ao_cmd_lex_i; + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + down = ao_cmd_lex_i; + } + if (up >= down) { + printf("Invalid accel: up (%d) down (%d)\n", + up, down); + return; + } + _ao_config_edit_start(); + ao_config.accel_plus_g = up; + ao_config.accel_minus_g = down; +#if HAS_GYRO + if (ao_cmd_lex_i == 0) { + ao_config.accel_zero_along = (accel_along_up + accel_along_down) / 2; + ao_config.accel_zero_across = (accel_across_up + accel_across_down) / 2; + ao_config.accel_zero_through = (accel_through_up + accel_through_down) / 2; + } +#endif + _ao_config_edit_finish(); +} +#endif /* HAS_ACCEL */ + +void +ao_config_apogee_delay_show(void) __reentrant +{ + printf("Apogee delay: %d seconds\n", + ao_config.apogee_delay); +} + +void +ao_config_apogee_delay_set(void) __reentrant +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.apogee_delay = ao_cmd_lex_i; + _ao_config_edit_finish(); +} + +void +ao_config_apogee_lockout_show(void) __reentrant +{ + printf ("Apogee lockout: %d seconds\n", + ao_config.apogee_lockout); +} + +void +ao_config_apogee_lockout_set(void) __reentrant +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.apogee_lockout = ao_cmd_lex_i; + _ao_config_edit_finish(); +} + +#endif /* HAS_FLIGHT */ + +#if HAS_RADIO +void +ao_config_radio_cal_show(void) __reentrant +{ + printf("Radio cal: %ld\n", ao_config.radio_cal); +} + +void +ao_config_radio_cal_set(void) __reentrant +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.radio_cal = ao_cmd_lex_u32; + ao_config_set_radio(); + _ao_config_edit_finish(); +} +#endif + +#if HAS_LOG +void +ao_config_log_show(void) __reentrant +{ + printf("Max flight log: %d kB\n", (int16_t) (ao_config.flight_log_max >> 10)); +} + +void +ao_config_log_set(void) __reentrant +{ + uint16_t block = (uint16_t) (ao_storage_block >> 10); + uint16_t log_max = (uint16_t) (ao_storage_log_max >> 10); + + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + if (ao_log_present()) + printf("Storage must be empty before changing log size\n"); + else if (block > 1024 && (ao_cmd_lex_i & (block - 1))) + printf("Flight log size must be multiple of %d kB\n", block); + else if (ao_cmd_lex_i > log_max) + printf("Flight log max %d kB\n", log_max); + else { + _ao_config_edit_start(); + ao_config.flight_log_max = (uint32_t) ao_cmd_lex_i << 10; + _ao_config_edit_finish(); + } +} +#endif /* HAS_LOG */ + +#if HAS_IGNITE +void +ao_config_ignite_mode_show(void) __reentrant +{ + printf("Ignite mode: %d\n", ao_config.ignite_mode); +} + +void +ao_config_ignite_mode_set(void) __reentrant +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.ignite_mode = ao_cmd_lex_i; + _ao_config_edit_finish(); +} +#endif + +#if HAS_ACCEL +void +ao_config_pad_orientation_show(void) __reentrant +{ + printf("Pad orientation: %d\n", ao_config.pad_orientation); +} + +#ifndef AO_ACCEL_INVERT +#define AO_ACCEL_INVERT 0x7fff +#endif + +void +ao_config_pad_orientation_set(void) __reentrant +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_cmd_lex_i &= 1; + if (ao_config.pad_orientation != ao_cmd_lex_i) { + int16_t t; + t = ao_config.accel_plus_g; + ao_config.accel_plus_g = AO_ACCEL_INVERT - ao_config.accel_minus_g; + ao_config.accel_minus_g = AO_ACCEL_INVERT - t; + } + ao_config.pad_orientation = ao_cmd_lex_i; + _ao_config_edit_finish(); +} +#endif + +#if HAS_RADIO +void +ao_config_radio_enable_show(void) __reentrant +{ + printf("Radio enable: %d\n", ao_config.radio_enable); +} + +void +ao_config_radio_enable_set(void) __reentrant +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.radio_enable = ao_cmd_lex_i; + _ao_config_edit_finish(); +} +#endif /* HAS_RADIO */ + +#if HAS_AES + +__xdata uint8_t ao_config_aes_seq = 1; + +void +ao_config_key_show(void) __reentrant +{ + uint8_t i; + printf("AES key: "); + for (i = 0; i < AO_AES_LEN; i++) + printf ("%02x", ao_config.aes_key[i]); + printf("\n"); +} + +void +ao_config_key_set(void) __reentrant +{ + uint8_t i; + + _ao_config_edit_start(); + for (i = 0; i < AO_AES_LEN; i++) { + ao_cmd_hexbyte(); + if (ao_cmd_status != ao_cmd_success) + break; + ao_config.aes_key[i] = ao_cmd_lex_i; + } + ++ao_config_aes_seq; + _ao_config_edit_finish(); +} +#endif + +#if HAS_APRS + +void +ao_config_aprs_show(void) +{ + printf ("APRS interval: %d\n", ao_config.aprs_interval); +} + +void +ao_config_aprs_set(void) +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.aprs_interval = ao_cmd_lex_i; + _ao_config_edit_finish(); +} + +#endif /* HAS_APRS */ + +#if HAS_RADIO_AMP + +void +ao_config_radio_amp_show(void) +{ + printf ("Radio amp setting: %d\n", ao_config.radio_amp); +} + +void +ao_config_radio_amp_set(void) +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.radio_amp = ao_cmd_lex_i; + _ao_config_edit_finish(); +} + +#endif + +#if HAS_RADIO_POWER + +void +ao_config_radio_power_show(void) +{ + printf ("Radio power setting: %d\n", ao_config.radio_power); +} + +void +ao_config_radio_power_set(void) +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.radio_power = ao_cmd_lex_i; + _ao_config_edit_finish(); +} + +#endif + +struct ao_config_var { + __code char *str; + void (*set)(void) __reentrant; + void (*show)(void) __reentrant; +}; + +static void +ao_config_help(void) __reentrant; + +static void +ao_config_show(void) __reentrant; + +#if HAS_EEPROM +static void +ao_config_save(void) __reentrant; +#endif + +__code struct ao_config_var ao_config_vars[] = { +#if HAS_FLIGHT + { "m \0Main deploy (m)", + ao_config_main_deploy_set, ao_config_main_deploy_show, }, + { "d \0Apogee delay (s)", + ao_config_apogee_delay_set, ao_config_apogee_delay_show }, + { "L \0Apogee detect lockout (s)", + ao_config_apogee_lockout_set, ao_config_apogee_lockout_show, }, +#endif /* HAS_FLIGHT */ +#if HAS_RADIO + { "F \0Frequency (kHz)", + ao_config_frequency_set, ao_config_frequency_show }, + { "c \0Callsign (8 char max)", + 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 \0Radio calib (cal = rf/(xtal/2^16))", + ao_config_radio_cal_set, ao_config_radio_cal_show }, +#if HAS_RADIO_POWER + { "p \0Radio power setting (0-255)", + ao_config_radio_power_set, ao_config_radio_power_show }, +#endif +#if HAS_RADIO_AMP + { "d \0Radio amplifier setting (0-3)", + ao_config_radio_amp_set, ao_config_radio_amp_show }, +#endif +#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_LOG + { "l \0Flight log size (kB)", + ao_config_log_set, ao_config_log_show }, +#endif +#if HAS_IGNITE + { "i <0 dual, 1 apogee, 2 main>\0Set igniter mode", + ao_config_ignite_mode_set, ao_config_ignite_mode_show }, +#endif +#if HAS_AES + { "k <32 hex digits>\0Set AES encryption key", + ao_config_key_set, ao_config_key_show }, +#endif +#if AO_PYRO_NUM + { "P \0Configure pyro channels", + ao_pyro_set, ao_pyro_show }, +#endif +#if HAS_APRS + { "A \0APRS packet interval (0 disable)", + ao_config_aprs_set, ao_config_aprs_show }, +#endif + { "s\0Show", + ao_config_show, 0 }, +#if HAS_EEPROM + { "w\0Write to eeprom", + ao_config_save, 0 }, +#endif + { "?\0Help", + ao_config_help, 0 }, + { 0, 0, 0 } +}; + +void +ao_config_set(void) +{ + char c; + uint8_t cmd; + + ao_cmd_white(); + c = ao_cmd_lex_c; + ao_cmd_lex(); + for (cmd = 0; ao_config_vars[cmd].str != NULL; cmd++) + if (ao_config_vars[cmd].str[0] == c) { + (*ao_config_vars[cmd].set)(); + return; + } + ao_cmd_status = ao_cmd_syntax_error; +} + +static void +ao_config_help(void) __reentrant +{ + uint8_t cmd; + for (cmd = 0; ao_config_vars[cmd].str != NULL; cmd++) + printf("%-20s %s\n", + ao_config_vars[cmd].str, + ao_config_vars[cmd].str+1+ + strlen(ao_config_vars[cmd].str)); +} + +static void +ao_config_show(void) __reentrant +{ + uint8_t cmd; + ao_config_get(); + printf("Config version: %d.%d\n", + ao_config.major, ao_config.minor); + for (cmd = 0; ao_config_vars[cmd].str != NULL; cmd++) + if (ao_config_vars[cmd].show) + (*ao_config_vars[cmd].show)(); +#if HAS_MS5607 + ao_ms5607_info(); +#endif +} + +#if HAS_EEPROM +static void +ao_config_save(void) __reentrant +{ + uint8_t saved = 0; + ao_mutex_get(&ao_config_mutex); + if (ao_config_dirty) { + _ao_config_put(); + ao_config_dirty = 0; + saved = 1; + } + ao_mutex_put(&ao_config_mutex); + if (saved) + puts("Saved"); + else + puts("Nothing to save"); +} +#endif + +__code struct ao_cmds ao_config_cmds[] = { + { ao_config_set, "c \0Set config (? for help, s to show)" }, + { 0, NULL }, +}; + +void +ao_config_init(void) +{ + ao_cmd_register(&ao_config_cmds[0]); +} diff --git a/src/kernel/ao_config.h b/src/kernel/ao_config.h new file mode 100644 index 00000000..e101af8e --- /dev/null +++ b/src/kernel/ao_config.h @@ -0,0 +1,53 @@ +/* + * 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_CONFIG_H_ +#define _AO_CONFIG_H_ + +#ifndef USE_STORAGE_CONFIG +#define USE_STORAGE_CONFIG 1 +#endif + +#ifndef USE_EEPROM_CONFIG +#define USE_EEPROM_CONFIG 0 +#endif + +#if USE_STORAGE_CONFIG + +#include + +#define ao_config_setup() ao_storage_setup() +#define ao_config_erase() ao_storage_erase(ao_storage_config) +#define ao_config_write(pos,bytes, len) ao_storage_write(ao_storage_config+(pos), bytes, len) +#define ao_config_read(pos,bytes, len) ao_storage_read(ao_storage_config+(pos), bytes, len) +#define ao_config_flush() ao_storage_flush() + +#endif + +#if USE_EEPROM_CONFIG + +#include + +#define ao_config_setup() +#define ao_config_erase() +#define ao_config_write(pos,bytes, len) ao_eeprom_write(pos, bytes, len) +#define ao_config_read(pos,bytes, len) ao_eeprom_read(pos, bytes, len) +#define ao_config_flush() + +#endif + +#endif /* _AO_CONFIG_H_ */ diff --git a/src/kernel/ao_convert.c b/src/kernel/ao_convert.c new file mode 100644 index 00000000..aa9b5f48 --- /dev/null +++ b/src/kernel/ao_convert.c @@ -0,0 +1,89 @@ +/* + * Copyright © 2009 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. + */ + +#if !defined(AO_CONVERT_TEST) && !defined(AO_FLIGHT_TEST) +#include "ao.h" +#endif + +static const int16_t altitude_table[] = { +#include "altitude.h" +}; + +#define ALT_FRAC_SCALE (1 << ALT_FRAC_BITS) +#define ALT_FRAC_MASK (ALT_FRAC_SCALE - 1) + +int16_t +ao_pres_to_altitude(int16_t pres) __reentrant +{ + uint8_t o; + int16_t part; + + if (pres < 0) + pres = 0; + o = pres >> ALT_FRAC_BITS; + part = pres & ALT_FRAC_MASK; + + return ((int32_t) altitude_table[o] * (ALT_FRAC_SCALE - part) + + (int32_t) altitude_table[o+1] * part + (ALT_FRAC_SCALE >> 1)) >> ALT_FRAC_BITS; +} + +#if AO_NEED_ALTITUDE_TO_PRES +int16_t +ao_altitude_to_pres(int16_t alt) __reentrant +{ + int16_t span, sub_span; + uint8_t l, h, m; + int32_t pres; + + l = 0; + h = NALT - 1; + while ((h - l) != 1) { + m = (l + h) >> 1; + if (altitude_table[m] < alt) + h = m; + else + l = m; + } + span = altitude_table[l] - altitude_table[h]; + sub_span = altitude_table[l] - alt; + pres = ((((int32_t) l * (span - sub_span) + (int32_t) h * sub_span) << ALT_FRAC_BITS) + (span >> 1)) / span; + if (pres > 32767) + pres = 32767; + if (pres < 0) + pres = 0; + return (int16_t) pres; +} +#endif + +#if 0 +int16_t +ao_temp_to_dC(int16_t temp) __reentrant +{ + int16_t ret; + + /* Output voltage at 0°C = 0.755V + * Coefficient = 0.00247V/°C + * Reference voltage = 1.25V + * + * temp = ((value / 32767) * 1.25 - 0.755) / 0.00247 + * = (value - 19791.268) / 32768 * 1.25 / 0.00247 + * ≃ (value - 19791) * 1012 / 65536 + */ + ret = ((temp - 19791) * 1012L) >> 16; + return ret; +} +#endif diff --git a/src/kernel/ao_convert_pa.c b/src/kernel/ao_convert_pa.c new file mode 100644 index 00000000..fe6e0ef6 --- /dev/null +++ b/src/kernel/ao_convert_pa.c @@ -0,0 +1,82 @@ +/* + * 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. + */ + +#if !defined(AO_CONVERT_TEST) && !defined(AO_FLIGHT_TEST) +#include "ao.h" +#endif + +#ifndef AO_CONST_ATTRIB +#define AO_CONST_ATTRIB +#endif + +static const alt_t altitude_table[] AO_CONST_ATTRIB = { +#include "altitude-pa.h" +}; + +#ifndef FETCH_ALT +#define FETCH_ALT(o) altitude_table[o] +#endif + +#define ALT_SCALE (1 << ALT_SHIFT) +#define ALT_MASK (ALT_SCALE - 1) + +alt_t +ao_pa_to_altitude(int32_t pa) +{ + int16_t o; + int16_t part; + int32_t low, high; + + if (pa < 0) + pa = 0; + if (pa > 120000L) + pa = 120000L; + o = pa >> ALT_SHIFT; + part = pa & ALT_MASK; + + low = (int32_t) FETCH_ALT(o) * (ALT_SCALE - part); + high = (int32_t) FETCH_ALT(o+1) * part + (ALT_SCALE >> 1); + return (low + high) >> ALT_SHIFT; +} + +#ifdef AO_CONVERT_TEST +int32_t +ao_altitude_to_pa(int32_t alt) +{ + int32_t span, sub_span; + uint16_t l, h, m; + int32_t pa; + + l = 0; + h = NALT - 1; + while ((h - l) != 1) { + m = (l + h) >> 1; + if (altitude_table[m] < alt) + h = m; + else + l = m; + } + span = altitude_table[l] - altitude_table[h]; + sub_span = altitude_table[l] - alt; + pa = ((((int32_t) l * (span - sub_span) + (int32_t) h * sub_span) << ALT_SHIFT) + (span >> 1)) / span; + if (pa > 120000) + pa = 120000; + if (pa < 0) + pa = 0; + return pa; +} +#endif diff --git a/src/kernel/ao_convert_pa_test.c b/src/kernel/ao_convert_pa_test.c new file mode 100644 index 00000000..7d5b1922 --- /dev/null +++ b/src/kernel/ao_convert_pa_test.c @@ -0,0 +1,76 @@ +/* + * 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 +#define AO_CONVERT_TEST +typedef int32_t alt_t; +#include "ao_host.h" +#include "ao_convert_pa.c" + +#define STEP_P 1 +#define STEP_A 1 + +static inline int i_abs(int i) { return i < 0 ? -i : i; } + +int +main (int argc, char **argv) +{ + int i; + int32_t p_to_a, p_to_a_to_p; + int32_t a_to_p, a_to_p_to_a; + int max_p_error = 0, max_p_error_p = -1; + int max_a_error = 0, max_a_error_a = -1; + int p_error; + int a_error; + int ret = 0; + + for (i = 0; i < 120000 + STEP_P; i += STEP_P) { + if (i > 120000) + i = 120000; + p_to_a = ao_pa_to_altitude(i); + p_to_a_to_p = ao_altitude_to_pa(p_to_a); + p_error = i_abs(p_to_a_to_p - i); + if (p_error > max_p_error) { + max_p_error = p_error; + max_p_error_p = i; + } +// printf ("pa %d alt %d pa %d\n", +// i, p_to_a, p_to_a_to_p); + } + for (i = -1450; i < 40000 + STEP_A; i += STEP_A) { + a_to_p = ao_altitude_to_pa(i); + a_to_p_to_a = ao_pa_to_altitude(a_to_p); + a_error = i_abs(a_to_p_to_a - i); + if (a_error > max_a_error) { + max_a_error = a_error; + max_a_error_a = i; + } +// printf ("alt %d pa %d alt %d\n", +// i, a_to_p, a_to_p_to_a); + } + if (max_p_error > 2) { + printf ("max p error %d at %d\n", max_p_error, + max_p_error_p); + ret++; + } + if (max_a_error > 1) { + printf ("max a error %d at %d\n", max_a_error, + max_a_error_a); + ret++; + } + return ret; +} diff --git a/src/kernel/ao_convert_test.c b/src/kernel/ao_convert_test.c new file mode 100644 index 00000000..87e76841 --- /dev/null +++ b/src/kernel/ao_convert_test.c @@ -0,0 +1,76 @@ +/* + * 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. + */ + +#include +#define AO_CONVERT_TEST +#define AO_NEED_ALTITUDE_TO_PRES 1 +#include "ao_host.h" +#include "ao_convert.c" + +#define STEP 1 + +static inline int i_abs(int i) { return i < 0 ? -i : i; } + +int main (int argc, char **argv) +{ + int i; + int16_t p_to_a, p_to_a_to_p; + int16_t a_to_p, a_to_p_to_a; + int max_p_error = 0, max_p_error_p = -1; + int max_a_error = 0, max_a_error_a = -1; + int p_error; + int a_error; + int ret = 0; + + for (i = 0; i < 32767 + STEP; i += STEP) { + if (i > 32767) + i = 32767; + p_to_a = ao_pres_to_altitude(i); + p_to_a_to_p = ao_altitude_to_pres(p_to_a); + p_error = i_abs(p_to_a_to_p - i); + if (p_error > max_p_error) { + max_p_error = p_error; + max_p_error_p = i; + } +// printf ("pres %d alt %d pres %d\n", +// i, p_to_a, p_to_a_to_p); + } + for (i = -1578; i < 15835 + STEP; i += STEP) { + if (i > 15835) + i = 15835; + a_to_p = ao_altitude_to_pres(i); + a_to_p_to_a = ao_pres_to_altitude(a_to_p); + a_error = i_abs(a_to_p_to_a - i); + if (a_error > max_a_error) { + max_a_error = a_error; + max_a_error_a = i; + } +// printf ("alt %d pres %d alt %d\n", +// i, a_to_p, a_to_p_to_a); + } + if (max_p_error > 2) { + printf ("max p error %d at %d\n", max_p_error, + max_p_error_p); + ret++; + } + if (max_a_error > 1) { + printf ("max a error %d at %d\n", max_a_error, + max_a_error_a); + ret++; + } + return ret; +} diff --git a/src/kernel/ao_convert_volt.c b/src/kernel/ao_convert_volt.c new file mode 100644 index 00000000..8556d423 --- /dev/null +++ b/src/kernel/ao_convert_volt.c @@ -0,0 +1,33 @@ +/* + * Copyright © 2014 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" + +#define scale(v,p,m) ((int32_t) (v) * (AO_ADC_REFERENCE_DV * ((p) + (m))) / (AO_ADC_MAX * (m))) + +int16_t +ao_battery_decivolt(int16_t adc) +{ + return scale(adc, AO_BATTERY_DIV_PLUS, AO_BATTERY_DIV_MINUS); +} + +int16_t +ao_ignite_decivolt(int16_t adc) +{ + return scale(adc, AO_IGNITE_DIV_PLUS, AO_IGNITE_DIV_MINUS); +} + diff --git a/src/kernel/ao_data.c b/src/kernel/ao_data.c new file mode 100644 index 00000000..6a3d02a1 --- /dev/null +++ b/src/kernel/ao_data.c @@ -0,0 +1,36 @@ +/* + * 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 + +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) +{ +#if HAS_FLIGHT + uint8_t i = ao_data_ring_prev(ao_sample_data); +#else + uint8_t i = ao_data_ring_prev(ao_data_head); +#endif + memcpy(packet, (void *) &ao_data_ring[i], sizeof (struct ao_data)); +} +#endif diff --git a/src/kernel/ao_data.h b/src/kernel/ao_data.h new file mode 100644 index 00000000..c4b062fd --- /dev/null +++ b/src/kernel/ao_data.h @@ -0,0 +1,338 @@ +/* + * 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. + */ + +#ifndef _AO_DATA_H_ +#define _AO_DATA_H_ + +#define GRAVITY 9.80665 + +#if HAS_ADC +#define AO_DATA_ADC (1 << 0) +#else +#define AO_DATA_ADC 0 +#endif + +#if HAS_MS5607 +#include +#define AO_DATA_MS5607 (1 << 1) +#else +#define AO_DATA_MS5607 0 +#endif + +#if HAS_MPU6000 +#include +#define AO_DATA_MPU6000 (1 << 2) +#else +#define AO_DATA_MPU6000 0 +#endif + +#if HAS_HMC5883 +#include +#define AO_DATA_HMC5883 (1 << 3) +#else +#define AO_DATA_HMC5883 0 +#endif + +#if HAS_MMA655X +#include +#define AO_DATA_MMA655X (1 << 4) +#else +#define AO_DATA_MMA655X 0 +#endif + +#ifdef AO_DATA_RING + +#define AO_DATA_ALL (AO_DATA_ADC|AO_DATA_MS5607|AO_DATA_MPU6000|AO_DATA_HMC5883|AO_DATA_MMA655X) + +struct ao_data { + uint16_t tick; +#if HAS_ADC + struct ao_adc adc; +#endif +#if HAS_MS5607 + struct ao_ms5607_sample ms5607_raw; + struct ao_ms5607_value ms5607_cooked; +#endif +#if HAS_MPU6000 + struct ao_mpu6000_sample mpu6000; +#if !HAS_MMA655X + int16_t z_accel; +#endif +#endif +#if HAS_HMC5883 + struct ao_hmc5883_sample hmc5883; +#endif +#if HAS_MMA655X + uint16_t mma655x; +#endif +}; + +#define ao_data_ring_next(n) (((n) + 1) & (AO_DATA_RING - 1)) +#define ao_data_ring_prev(n) (((n) - 1) & (AO_DATA_RING - 1)) + +/* Get a copy of the last complete sample set */ +void +ao_data_get(__xdata struct ao_data *packet); + +extern volatile __xdata struct ao_data ao_data_ring[AO_DATA_RING]; +extern volatile __data uint8_t ao_data_head; +extern volatile __data uint8_t ao_data_present; +extern volatile __data uint8_t ao_data_count; + +/* + * Mark a section of data as ready, check for data complete + */ +#define AO_DATA_PRESENT(bit) (ao_data_present |= (bit)) + +/* + * Wait until it is time to write a sensor sample; this is + * signaled by the timer tick + */ +#define AO_DATA_WAIT() do { \ + ao_sleep(DATA_TO_XDATA ((void *) &ao_data_count)); \ + } while (0) + +#endif /* AO_DATA_RING */ + +#if !HAS_BARO && HAS_MS5607 + +/* Either an MS5607 or an MS5611 hooked to a SPI port + */ + +#define HAS_BARO 1 + +typedef int32_t pres_t; + +#ifndef AO_ALT_TYPE +#define AO_ALT_TYPE int32_t +#endif + +typedef AO_ALT_TYPE alt_t; + +#define ao_data_pres_cook(packet) ao_ms5607_convert(&packet->ms5607_raw, &packet->ms5607_cooked) + +#define ao_data_pres(packet) ((packet)->ms5607_cooked.pres) +#define ao_data_temp(packet) ((packet)->ms5607_cooked.temp) + +#define pres_to_altitude(p) ao_pa_to_altitude(p) + +#endif + +#if !HAS_BARO && HAS_ADC + +#define HAS_BARO 1 + +typedef int16_t pres_t; +typedef int16_t alt_t; + +#define ao_data_pres(packet) ((packet)->adc.pres) +#define ao_data_temp(packet) ((packet)->adc.temp) +#define pres_to_altitude(p) ao_pres_to_altitude(p) +#define ao_data_pres_cook(p) + +#endif + +#if !HAS_BARO +typedef int16_t alt_t; +#endif + +/* + * Need a few macros to pull data from the sensors: + * + * ao_data_accel_sample - pull raw sensor and convert to normalized values + * ao_data_accel - pull normalized value (lives in the same memory) + * ao_data_set_accel - store normalized value back in the sensor location + * ao_data_accel_invert - flip rocket ends for positive acceleration + */ + +#if HAS_ACCEL + +/* This section is for an analog accelerometer hooked to one of the ADC pins. As + * those are 5V parts, this also requires that the 5V supply be hooked to to anothe ADC + * pin so that the both can be measured to correct for changes between the 3.3V and 5V rails + */ + +typedef int16_t accel_t; +#define ao_data_accel(packet) ((packet)->adc.accel) +#define ao_data_set_accel(packet, a) ((packet)->adc.accel = (a)) +#define ao_data_accel_invert(a) (0x7fff -(a)) + +/* + * Ok, the math here is a bit tricky. + * + * ao_sample_accel: ADC output for acceleration + * ao_accel_ref: ADC output for the 5V reference. + * ao_cook_accel: Corrected acceleration value + * Vcc: 3.3V supply to the CC1111 + * Vac: 5V supply to the accelerometer + * accel: input voltage to accelerometer ADC pin + * ref: input voltage to 5V reference ADC pin + * + * + * Measured acceleration is ratiometric to Vcc: + * + * ao_sample_accel accel + * ------------ = ----- + * 32767 Vcc + * + * Measured 5v reference is also ratiometric to Vcc: + * + * ao_accel_ref ref + * ------------ = ----- + * 32767 Vcc + * + * + * ao_accel_ref = 32767 * (ref / Vcc) + * + * Acceleration is measured ratiometric to the 5V supply, + * so what we want is: + * + * ao_cook_accel accel + * ------------- = ----- + * 32767 ref + * + * + * accel Vcc + * = ----- * --- + * Vcc ref + * + * ao_sample_accel 32767 + * = ------------ * ------------ + * 32767 ao_accel_ref + * + * Multiply through by 32767: + * + * ao_sample_accel * 32767 + * ao_cook_accel = -------------------- + * ao_accel_ref + * + * Now, the tricky part. Getting this to compile efficiently + * and keeping all of the values in-range. + * + * First off, we need to use a shift of 16 instead of * 32767 as SDCC + * does the obvious optimizations for byte-granularity shifts: + * + * ao_cook_accel = (ao_sample_accel << 16) / ao_accel_ref + * + * Next, lets check our input ranges: + * + * 0 <= ao_sample_accel <= 0x7fff (singled ended ADC conversion) + * 0x7000 <= ao_accel_ref <= 0x7fff (the 5V ref value is close to 0x7fff) + * + * Plugging in our input ranges, we get an output range of 0 - 0x12490, + * which is 17 bits. That won't work. If we take the accel ref and shift + * by a bit, we'll change its range: + * + * 0xe000 <= ao_accel_ref<<1 <= 0xfffe + * + * ao_cook_accel = (ao_sample_accel << 16) / (ao_accel_ref << 1) + * + * Now the output range is 0 - 0x9248, which nicely fits in 16 bits. It + * is, however, one bit too large for our signed computations. So, we + * take the result and shift that by a bit: + * + * ao_cook_accel = ((ao_sample_accel << 16) / (ao_accel_ref << 1)) >> 1 + * + * This finally creates an output range of 0 - 0x4924. As the ADC only + * provides 11 bits of data, we haven't actually lost any precision, + * just dropped a bit of noise off the low end. + */ + +#if HAS_ACCEL_REF + +#define ao_data_accel_cook(packet) \ + ((uint16_t) ((((uint32_t) (packet)->adc.accel << 16) / ((packet)->adc.accel_ref << 1))) >> 1) + +#else + +#define ao_data_accel_cook(packet) ((packet)->adc.accel) + +#endif /* HAS_ACCEL_REF */ + +#endif /* HAS_ACCEL */ + +#if !HAS_ACCEL && HAS_MMA655X + +#define HAS_ACCEL 1 + +typedef int16_t accel_t; + +/* MMA655X is hooked up so that positive values represent negative acceleration */ + +#define AO_ACCEL_INVERT 4095 + +#define ao_data_accel(packet) ((packet)->mma655x) +#if AO_MMA655X_INVERT +#define ao_data_accel_cook(packet) (AO_ACCEL_INVERT - (packet)->mma655x) +#else +#define ao_data_accel_cook(packet) ((packet)->mma655x) +#endif +#define ao_data_set_accel(packet, accel) ((packet)->mma655x = (accel)) +#define ao_data_accel_invert(accel) (AO_ACCEL_INVERT - (accel)) + +#endif + +#if !HAS_ACCEL && HAS_MPU6000 + +#define HAS_ACCEL 1 + +#define AO_ACCEL_INVERT 0 + +typedef int16_t accel_t; + +/* MPU6000 is hooked up so that positive y is positive acceleration */ +#define ao_data_accel(packet) ((packet)->z_accel) +#define ao_data_accel_cook(packet) (-(packet)->mpu6000.accel_y) +#define ao_data_set_accel(packet, accel) ((packet)->z_accel = (accel)) +#define ao_data_accel_invert(a) (-(a)) + +#endif + +#if !HAS_GYRO && HAS_MPU6000 + +#define HAS_GYRO 1 + +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) */ +/* Z axis is aligned perpendicular to the board (through) */ + +#define ao_data_along(packet) ((packet)->mpu6000.accel_y) +#define ao_data_across(packet) ((packet)->mpu6000.accel_x) +#define ao_data_through(packet) ((packet)->mpu6000.accel_z) + +#define ao_data_roll(packet) ((packet)->mpu6000.gyro_y) +#define ao_data_pitch(packet) ((packet)->mpu6000.gyro_x) +#define ao_data_yaw(packet) ((packet)->mpu6000.gyro_z) + +#endif + +#if !HAS_MAG && HAS_HMC5883 + +#define HAS_MAG 1 + +typedef int16_t ao_mag_t; /* in raw sample units */ + +#define ao_data_mag_along(packet) ((packet)->hmc5883.x) +#define ao_data_mag_across(packet) ((packet)->hmc5883.y) +#define ao_data_mag_through(packet) ((packet)->hmc5883.z) + +#endif + +#endif /* _AO_DATA_H_ */ diff --git a/src/kernel/ao_dbg.h b/src/kernel/ao_dbg.h new file mode 100644 index 00000000..181e6ec2 --- /dev/null +++ b/src/kernel/ao_dbg.h @@ -0,0 +1,62 @@ +/* + * 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. + */ + +#ifndef _AO_DBG_H_ +#define _AO_DBG_H_ + +/* + * ao_dbg.c + * + * debug another telemetrum board + */ + +/* Send a byte to the dbg target */ +void +ao_dbg_send_byte(uint8_t byte); + +/* Receive a byte from the dbg target */ +uint8_t +ao_dbg_recv_byte(void); + +/* Start a bulk transfer to/from dbg target memory */ +void +ao_dbg_start_transfer(uint16_t addr); + +/* End a bulk transfer to/from dbg target memory */ +void +ao_dbg_end_transfer(void); + +/* Write a byte to dbg target memory */ +void +ao_dbg_write_byte(uint8_t byte); + +/* Read a byte from dbg target memory */ +uint8_t +ao_dbg_read_byte(void); + +/* Enable dbg mode, switching use of the pins */ +void +ao_dbg_debug_mode(void); + +/* Reset the dbg target */ +void +ao_dbg_reset(void); + +void +ao_dbg_init(void); + +#endif /* _AO_DBG_H_ */ diff --git a/src/kernel/ao_debounce.c b/src/kernel/ao_debounce.c new file mode 100644 index 00000000..b9d67729 --- /dev/null +++ b/src/kernel/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/kernel/ao_debounce.h b/src/kernel/ao_debounce.h new file mode 100644 index 00000000..19c620f5 --- /dev/null +++ b/src/kernel/ao_debounce.h @@ -0,0 +1,74 @@ +/* + * 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 */ + 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 */ + uint8_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); +}; + +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); + +void +_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/kernel/ao_ee_fake.c b/src/kernel/ao_ee_fake.c new file mode 100644 index 00000000..7fcfcab0 --- /dev/null +++ b/src/kernel/ao_ee_fake.c @@ -0,0 +1,37 @@ +/* + * Copyright © 2009 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" + +/* + * For hardware without eeprom, the config code still + * wants to call these functions + */ +uint8_t +ao_ee_write_config(uint8_t *buf, uint16_t len) __reentrant +{ + (void) buf; + (void) len; + return 1; +} + +uint8_t +ao_ee_read_config(uint8_t *buf, uint16_t len) __reentrant +{ + ao_xmemset(buf, '\0', len); + return 1; +} diff --git a/src/kernel/ao_eeprom.h b/src/kernel/ao_eeprom.h new file mode 100644 index 00000000..915522bf --- /dev/null +++ b/src/kernel/ao_eeprom.h @@ -0,0 +1,43 @@ +/* + * 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_EEPROM_H_ +#define _AO_EEPROM_H_ + +extern const ao_pos_t ao_eeprom_total; + +/* + * Write to eeprom + */ + +uint8_t +ao_eeprom_write(ao_pos_t pos32, __xdata void *v, uint16_t len); + +/* + * Read from eeprom + */ +uint8_t +ao_eeprom_read(ao_pos_t pos, __xdata void *v, uint16_t len); + +/* + * Initialize eeprom + */ + +void +ao_eeprom_init(void); + +#endif /* _AO_EEPROM_H_ */ diff --git a/src/kernel/ao_fec.h b/src/kernel/ao_fec.h new file mode 100644 index 00000000..618756c1 --- /dev/null +++ b/src/kernel/ao_fec.h @@ -0,0 +1,84 @@ +/* + * 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. + */ + +#ifndef _AO_FEC_H_ +#define _AO_FEC_H_ + +#include + +#define AO_FEC_CRC_INIT 0xffff +#define AO_FEC_TRELLIS_TERMINATOR 0x0b +#define AO_FEC_PREPARE_EXTRA 4 + +extern const uint8_t ao_fec_whiten_table[]; + +#if AO_FEC_DEBUG +void +ao_fec_dump_bytes(const uint8_t *bytes, uint16_t len, const char *name); +#endif + +static inline uint16_t +ao_fec_crc_byte(uint8_t byte, uint16_t crc) +{ + uint8_t bit; + + for (bit = 0; bit < 8; bit++) { + if (((crc & 0x8000) >> 8) ^ (byte & 0x80)) + crc = (crc << 1) ^ 0x8005; + else + crc = (crc << 1); + byte <<= 1; + } + return crc; +} + +uint16_t +ao_fec_crc(const uint8_t *bytes, uint8_t len); + +/* + * 'len' is the length of the original data; 'bytes' + * must be four bytes longer than that, and the first + * two after 'len' must be the received crc + */ +uint8_t +ao_fec_check_crc(const uint8_t *bytes, uint8_t len); + +/* + * Compute CRC, whiten, convolve and interleave data. 'out' must be (len + 4) * 2 bytes long + */ +uint8_t +ao_fec_encode(const uint8_t *in, uint8_t len, uint8_t *out); + +/* + * Decode data. 'in' is one byte per bit, soft decision + * 'out' must be len/8 bytes long + */ + +#define AO_FEC_DECODE_BLOCK (32) /* callback must return multiples of this many bits */ + +#define AO_FEC_DECODE_CRC_OK 0x80 /* stored in out[out_len-1] */ + +uint8_t +ao_fec_decode(const uint8_t *in, uint16_t in_len, uint8_t *out, uint8_t out_len, uint16_t (*callback)(void)); + +/* + * Interleave data packed in bytes. 'out' must be 'len' bytes long. + */ +uint16_t +ao_fec_interleave_bytes(uint8_t *in, uint16_t len, uint8_t *out); + +#endif /* _AO_FEC_H_ */ diff --git a/src/kernel/ao_fec_rx.c b/src/kernel/ao_fec_rx.c new file mode 100644 index 00000000..c4f5559a --- /dev/null +++ b/src/kernel/ao_fec_rx.c @@ -0,0 +1,318 @@ +/* + * 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 + +#ifdef TELEMEGA +#include +#endif + +#if AO_PROFILE +#include + +uint32_t ao_fec_decode_start, ao_fec_decode_end; +#endif + +/* + * byte order repeats through 3 2 1 0 + * + * bit-pair order repeats through + * + * 1/0 3/2 5/4 7/6 + * + * So, the over all order is: + * + * 3,1/0 2,1/0 1,1/0 0,1/0 + * 3,3/2 2,3/2 1,3/2 0,3/2 + * 3,5/4 2,5/4 1,5/4 0,5/4 + * 3,7/6 2,7/6 1,7/6 0,7/6 + * + * The raw bit order is thus + * + * 1e/1f 16/17 0e/0f 06/07 + * 1c/1d 14/15 0c/0d 04/05 + * 1a/1b 12/13 0a/0b 02/03 + * 18/19 10/11 08/09 00/01 + */ + +static const uint8_t ao_interleave_order[] = { + 0x1e, 0x16, 0x0e, 0x06, + 0x1c, 0x14, 0x0c, 0x04, + 0x1a, 0x12, 0x0a, 0x02, + 0x18, 0x10, 0x08, 0x00 +}; + +static inline uint16_t ao_interleave_index(uint16_t i) { + return (i & ~0x1e) | ao_interleave_order[(i & 0x1e) >> 1]; +} + +#define NUM_STATE 8 +#define NUM_HIST 24 + +typedef uint32_t bits_t; + +#define V_0 0xff +#define V_1 0x00 + +/* + * These are just the 'zero' states; the 'one' states mirror them + */ +static const uint8_t ao_fec_decode_table[NUM_STATE*2] = { + V_0, V_0, /* 000 */ + V_0, V_1, /* 001 */ + V_1, V_1, /* 010 */ + V_1, V_0, /* 011 */ + V_1, V_1, /* 100 */ + V_1, V_0, /* 101 */ + V_0, V_0, /* 110 */ + V_0, V_1 /* 111 */ +}; + +static inline uint8_t +ao_next_state(uint8_t state, uint8_t bit) +{ + return ((state << 1) | bit) & 0x7; +} + +/* + * 'in' is 8-bits per symbol soft decision data + * 'len' is input byte length. 'out' must be + * 'len'/16 bytes long + */ + +uint8_t +ao_fec_decode(const uint8_t *in, uint16_t len, uint8_t *out, uint8_t out_len, uint16_t (*callback)(void)) +{ + static uint32_t cost[2][NUM_STATE]; /* path cost */ + static bits_t bits[2][NUM_STATE]; /* save bits to quickly output them */ + + uint16_t i; /* input byte index */ + uint16_t b; /* encoded symbol index (bytes/2) */ + uint16_t o; /* output bit index */ + uint8_t p; /* previous cost/bits index */ + uint8_t n; /* next cost/bits index */ + uint8_t state; /* state index */ + const uint8_t *whiten = ao_fec_whiten_table; + uint16_t interleave; /* input byte array index */ + uint8_t s0, s1; + uint16_t avail; + uint16_t crc = AO_FEC_CRC_INIT; +#if AO_PROFILE + uint32_t start_tick; +#endif + + p = 0; + for (state = 0; state < NUM_STATE; state++) { + cost[0][state] = 0x7fffffff; + bits[0][state] = 0; + } + cost[0][0] = 0; + + if (callback) + avail = 0; + else + avail = len; + +#if AO_PROFILE + if (!avail) { + avail = callback(); + if (!avail) + return 0; + } + start_tick = ao_profile_tick(); +#endif + o = 0; + for (i = 0; i < len; i += 2) { + b = i/2; + n = p ^ 1; + + if (!avail) { + avail = callback(); + if (!avail) + return 0; + } + + /* Fetch one pair of input bytes, de-interleaving + * the input. + */ + interleave = ao_interleave_index(i); + s0 = in[interleave]; + s1 = in[interleave+1]; + + avail -= 2; + + /* Compute path costs and accumulate output bit path + * for each state and encoded bit value. Unrolling + * this loop is worth about > 30% performance boost. + * Decoding 76-byte remote access packets is reduced + * from 14.700ms to 9.3ms. Redoing the loop to + * directly compare the two pasts for each future state + * reduces this down to 5.7ms + */ + + /* Ok, of course this is tricky, it's optimized. + * + * First, it's important to realize that we have 8 + * states representing the combinations of the three + * most recent bits from the encoder. Flipping any + * of these three bits flips both output bits. + * + * 'state<<1' represents the target state for a new + * bit value of 0. '(state<<1)+1' represents the + * target state for a new bit value of 1. + * + * 'state' is the previous state with an oldest bit + * value of 0. 'state + 4' is the previous state with + * an oldest bit value of 1. These two states will + * either lead to 'state<<1' or '(state<<1)+1', depending + * on whether the next encoded bit was a zero or a one. + * + * m0 and m1 are the cost of coming to 'state<<1' from + * one of the two possible previous states 'state' and + * 'state + 4'. + * + * Because we know the expected values of each + * received bit are flipped between these two previous + * states: + * + * bitcost(state+4) = 510 - bitcost(state) + * + * With those two total costs in hand, we then pick + * the lower as the cost of the 'state<<1', and compute + * the path of bits leading to that state. + * + * Then, do the same for '(state<<1) + 1'. This time, + * instead of computing the m0 and m1 values from + * scratch, because the only difference is that we're + * expecting a one bit instead of a zero bit, we just + * flip the bitcost values around to match the + * expected transmitted bits with some tricky + * arithmetic which is equivalent to: + * + * m0 = cost[p][state] + (510 - bitcost); + * m1 = cost[p][state+4] + bitcost + * + * Then, the lowest cost and bit trace of the new state + * is saved. + */ + +#define DO_STATE(state) { \ + uint32_t bitcost; \ + \ + uint32_t m0; \ + uint32_t m1; \ + uint32_t bit; \ + \ + bitcost = ((uint32_t) (s0 ^ ao_fec_decode_table[(state<<1)]) + \ + (uint32_t) (s1 ^ ao_fec_decode_table[(state<<1)|1])); \ + \ + m0 = cost[p][state] + bitcost; \ + m1 = cost[p][state+4] + (510 - bitcost); \ + bit = m0 > m1; \ + cost[n][state<<1] = bit ? m1 : m0; \ + bits[n][state<<1] = (bits[p][state + (bit<<2)] << 1) | (state&1); \ + \ + m0 -= (bitcost+bitcost-510); \ + m1 += (bitcost+bitcost-510); \ + bit = m0 > m1; \ + cost[n][(state<<1)+1] = bit ? m1 : m0; \ + bits[n][(state<<1)+1] = (bits[p][state + (bit<<2)] << 1) | (state&1); \ + } + + DO_STATE(0); + DO_STATE(1); + DO_STATE(2); + DO_STATE(3); + +#if 0 + printf ("bit %3d symbol %2x %2x:", i/2, s0, s1); + for (state = 0; state < NUM_STATE; state++) { + printf (" %8u(%08x)", cost[n][state], bits[n][state]); + } + printf ("\n"); +#endif + p = n; + + /* A loop is needed to handle the last output byte. It + * won't have any bits of future data to perform full + * error correction, but we might as well give the + * best possible answer anyways. + */ + while ((b - o) >= (8 + NUM_HIST) || (i + 2 >= len && b > o)) { + + /* Compute number of bits to the end of the + * last full byte of data. This is generally + * NUM_HIST, unless we've reached + * the end of the input, in which case + * it will be seven. + */ + int8_t dist = b - (o + 8); /* distance to last ready-for-writing bit */ + uint32_t min_cost; /* lowest cost */ + uint8_t min_state; /* lowest cost state */ + uint8_t byte; + + /* Find the best fit at the current point + * of the decode. + */ + min_cost = cost[p][0]; + min_state = 0; + for (state = 1; state < NUM_STATE; state++) { + if (cost[p][state] < min_cost) { + min_cost = cost[p][state]; + min_state = state; + } + } + + /* The very last byte of data has the very last bit + * of data left in the state value; just smash the + * bits value in place and reset the 'dist' from + * -1 to 0 so that the full byte is read out + */ + if (dist < 0) { + bits[p][min_state] = (bits[p][min_state] << 1) | (min_state & 1); + dist = 0; + } + +#if 0 + printf ("\tbit %3d min_cost %5d old bit %3d old_state %x bits %02x whiten %0x\n", + i/2, min_cost, o + 8, min_state, (bits[p][min_state] >> dist) & 0xff, *whiten); +#endif + byte = (bits[p][min_state] >> dist) ^ *whiten++; + *out++ = byte; + if (out_len > 2) + crc = ao_fec_crc_byte(byte, crc); + + if (!--out_len) { + if ((out[-2] == (uint8_t) (crc >> 8)) && + out[-1] == (uint8_t) crc) + out[-1] = AO_FEC_DECODE_CRC_OK; + else + out[-1] = 0; + out[-2] = 0; + goto done; + } + o += 8; + } + } +done: +#if AO_PROFILE + ao_fec_decode_start = start_tick; + ao_fec_decode_end = ao_profile_tick(); +#endif + return 1; +} diff --git a/src/kernel/ao_fec_tx.c b/src/kernel/ao_fec_tx.c new file mode 100644 index 00000000..4941d745 --- /dev/null +++ b/src/kernel/ao_fec_tx.c @@ -0,0 +1,134 @@ +/* + * 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 + +#if AO_FEC_DEBUG +void +ao_fec_dump_bytes(const uint8_t *bytes, uint16_t len, const char *name) +{ + uint16_t i; + + printf ("%s (%d):", name, len); + for (i = 0; i < len; i++) { + if ((i & 7) == 0) + printf ("\n\t%02x:", i); + printf(" %02x", bytes[i]); + } + printf ("\n"); +} +#endif + +uint16_t +ao_fec_crc(const uint8_t *bytes, uint8_t len) +{ + uint16_t crc = AO_FEC_CRC_INIT; + + while (len--) + crc = ao_fec_crc_byte(*bytes++, crc); + return crc; +} + +/* + * len is the length of the data; the crc will be + * the fist two bytes after that + */ + +uint8_t +ao_fec_check_crc(const uint8_t *bytes, uint8_t len) +{ + uint16_t computed_crc = ao_fec_crc(bytes, len); + uint16_t received_crc = (bytes[len] << 8) | (bytes[len+1]); + + return computed_crc == received_crc; +} + +/* + * Compute CRC and trellis-terminator/interleave-pad bytes + */ +static uint8_t +ao_fec_prepare(const uint8_t *in, uint8_t len, uint8_t *extra) +{ + uint16_t crc = ao_fec_crc (in, len); + uint8_t i = 0; + uint8_t num_fec; + + /* Append CRC */ + extra[i++] = crc >> 8; + extra[i++] = crc; + + /* Append FEC -- 1 byte if odd, two bytes if even */ + num_fec = 2 - (i & 1); + while (num_fec--) + extra[i++] = AO_FEC_TRELLIS_TERMINATOR; + return i; +} + +const uint8_t ao_fec_whiten_table[] = { +#include "ao_whiten.h" +}; + +static const uint8_t ao_fec_encode_table[16] = { +/* next 0 1 state */ + 0, 3, /* 000 */ + 1, 2, /* 001 */ + 3, 0, /* 010 */ + 2, 1, /* 011 */ + 3, 0, /* 100 */ + 2, 1, /* 101 */ + 0, 3, /* 110 */ + 1, 2 /* 111 */ +}; + +uint8_t +ao_fec_encode(const uint8_t *in, uint8_t len, uint8_t *out) +{ + uint8_t extra[AO_FEC_PREPARE_EXTRA]; + uint8_t extra_len; + uint32_t encode, interleave; + uint8_t pair, byte, bit; + uint16_t fec = 0; + const uint8_t *whiten = ao_fec_whiten_table; + + extra_len = ao_fec_prepare(in, len, extra); + for (pair = 0; pair < len + extra_len; pair += 2) { + encode = 0; + for (byte = 0; byte < 2; byte++) { + if (pair + byte == len) + in = extra; + fec |= *in++ ^ *whiten++; + for (bit = 0; bit < 8; bit++) { + encode = encode << 2 | ao_fec_encode_table[fec >> 7]; + fec = (fec << 1) & 0x7ff; + } + } + + interleave = 0; + for (bit = 0; bit < 4 * 4; bit++) { + uint8_t byte_shift = (bit & 0x3) << 3; + uint8_t bit_shift = (bit & 0xc) >> 1; + + interleave = (interleave << 2) | ((encode >> (byte_shift + bit_shift)) & 0x3); + } + *out++ = interleave >> 24; + *out++ = interleave >> 16; + *out++ = interleave >> 8; + *out++ = interleave >> 0; + } + return (len + extra_len) * 2; +} diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c new file mode 100644 index 00000000..24099347 --- /dev/null +++ b/src/kernel/ao_flight.c @@ -0,0 +1,472 @@ +/* + * Copyright © 2009 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_FLIGHT_TEST +#include "ao.h" +#include +#endif + +#if HAS_MPU6000 +#include +#endif + +#ifndef HAS_ACCEL +#error Please define HAS_ACCEL +#endif + +#ifndef HAS_GPS +#error Please define HAS_GPS +#endif + +#ifndef HAS_USB +#error Please define HAS_USB +#endif + +#ifndef HAS_TELEMETRY +#define HAS_TELEMETRY HAS_RADIO +#endif + +/* Main flight thread. */ + +__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_SENSOR_ERRORS +/* 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 + */ +static __data uint16_t ao_interval_end; +static __data int16_t ao_interval_min_height; +static __data int16_t ao_interval_max_height; +#if HAS_ACCEL +static __data int16_t ao_coast_avg_accel; +#endif + +__pdata uint8_t ao_flight_force_idle; + +/* We also have a clock, which can be used to sanity check things in + * case of other failures + */ + +#define BOOST_TICKS_MAX AO_SEC_TO_TICKS(15) + +/* Landing is detected by getting constant readings from both pressure and accelerometer + * for a fairly long time (AO_INTERVAL_TICKS) + */ +#define AO_INTERVAL_TICKS AO_SEC_TO_TICKS(10) + +#define abs(a) ((a) < 0 ? -(a) : (a)) + +void +ao_flight(void) +{ + ao_sample_init(); + ao_flight_state = ao_flight_startup; + for (;;) { + + /* + * Process ADC samples, just looping + * until the sensors are calibrated. + */ + if (!ao_sample()) + continue; + + switch (ao_flight_state) { + case ao_flight_startup: + + /* Check to see what mode we should go to. + * - Invalid mode if accel cal appears to be out + * - pad mode if we're upright, + * - idle mode otherwise + */ +#if HAS_ACCEL + if (ao_config.accel_plus_g == 0 || + 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 || + ao_ground_height < -1000 || + ao_ground_height > 7000) + { + /* Detected an accel value outside -1.5g to 1.5g + * (or uncalibrated values), so we go into invalid mode + */ + ao_flight_state = ao_flight_invalid; + +#if HAS_RADIO && PACKET_HAS_SLAVE + /* Turn on packet system in invalid mode on TeleMetrum */ + ao_packet_slave_start(); +#endif + } else +#endif + if (!ao_flight_force_idle +#if HAS_ACCEL + && ao_ground_accel < ao_config.accel_plus_g + ACCEL_NOSE_UP +#endif + ) + { + /* Set pad mode - we can fly! */ + ao_flight_state = ao_flight_pad; +#if HAS_USB && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE + /* Disable the USB controller in flight mode + * to save power + */ + ao_usb_disable(); +#endif + +#if !HAS_ACCEL && PACKET_HAS_SLAVE + /* Disable packet mode in pad state on TeleMini */ + ao_packet_slave_stop(); +#endif + +#if HAS_TELEMETRY + /* Turn on telemetry system */ + ao_rdf_set(1); + ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD); +#endif +#if AO_LED_RED + /* signal successful initialization by turning off the LED */ + ao_led_off(AO_LED_RED); +#endif + } else { + /* Set idle mode */ + ao_flight_state = ao_flight_idle; +#if HAS_SENSOR_ERRORS + if (ao_sensor_errors) + ao_flight_state = ao_flight_invalid; +#endif + +#if HAS_ACCEL && HAS_RADIO && PACKET_HAS_SLAVE + /* Turn on packet system in idle mode on TeleMetrum */ + ao_packet_slave_start(); +#endif + +#if AO_LED_RED + /* signal successful initialization by turning off the LED */ + ao_led_off(AO_LED_RED); +#endif + } + /* wakeup threads due to state change */ + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + + break; + case ao_flight_pad: + + /* pad to boost: + * + * barometer: > 20m vertical motion + * OR + * accelerometer: > 2g AND velocity > 5m/s + * + * The accelerometer should always detect motion before + * the barometer, but we use both to make sure this + * transition is detected. If the device + * doesn't have an accelerometer, then ignore the + * speed and acceleration as they are quite noisy + * on the pad. + */ + if (ao_height > AO_M_TO_HEIGHT(20) +#if HAS_ACCEL + || (ao_accel > AO_MSS_TO_ACCEL(20) && + ao_speed > AO_MS_TO_SPEED(5)) +#endif + ) + { + ao_flight_state = ao_flight_boost; + ao_boost_tick = ao_sample_tick; + + /* start logging data */ + ao_log_start(); + +#if HAS_TELEMETRY + /* Increase telemetry rate */ + ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_FLIGHT); + + /* disable RDF beacon */ + ao_rdf_set(0); +#endif + +#if HAS_GPS + /* Record current GPS position by waking up GPS log tasks */ + 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)); + } + break; + case ao_flight_boost: + + /* boost to fast: + * + * accelerometer: start to fall at > 1/4 G + * OR + * time: boost for more than 15 seconds + * + * Detects motor burn out by the switch from acceleration to + * deceleration, or by waiting until the maximum burn duration + * (15 seconds) has past. + */ + if ((ao_accel < AO_MSS_TO_ACCEL(-2.5) && ao_height > AO_M_TO_HEIGHT(100)) || + (int16_t) (ao_sample_tick - ao_boost_tick) > BOOST_TICKS_MAX) + { +#if HAS_ACCEL + ao_flight_state = ao_flight_fast; + ao_coast_avg_accel = ao_accel; +#else + ao_flight_state = ao_flight_coast; +#endif + ++ao_motor_number; + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } + break; +#if HAS_ACCEL + case ao_flight_fast: + /* + * This is essentially the same as coast, + * but the barometer is being ignored as + * it may be unreliable. + */ + if (ao_speed < AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) + { + ao_flight_state = ao_flight_coast; + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } else + goto check_re_boost; + break; +#endif + case ao_flight_coast: + + /* + * By customer request - allow the user + * to lock out apogee detection for a specified + * number of seconds. + */ + if (ao_config.apogee_lockout) { + if ((ao_sample_tick - ao_boost_tick) < + AO_SEC_TO_TICKS(ao_config.apogee_lockout)) + break; + } + + /* apogee detect: coast to drogue deploy: + * + * speed: < 0 + * + * Also make sure the model altitude is tracking + * the measured altitude reasonably closely; otherwise + * we're probably transsonic. + */ + if (ao_speed < 0 +#if !HAS_ACCEL + && (ao_sample_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg < 100) +#endif + ) + { +#if HAS_IGNITE + /* ignite the drogue charge */ + ao_ignite(ao_igniter_drogue); +#endif + +#if HAS_TELEMETRY + /* slow down the telemetry system */ + ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_RECOVER); + + /* Turn the RDF beacon back on */ + ao_rdf_set(1); +#endif + + /* and enter drogue state */ + ao_flight_state = ao_flight_drogue; + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } +#if HAS_ACCEL + else { + check_re_boost: + ao_coast_avg_accel = ao_coast_avg_accel - (ao_coast_avg_accel >> 6) + (ao_accel >> 6); + if (ao_coast_avg_accel > AO_MSS_TO_ACCEL(20)) { + ao_boost_tick = ao_sample_tick; + ao_flight_state = ao_flight_boost; + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } + } +#endif + + break; + case ao_flight_drogue: + + /* drogue to main deploy: + * + * barometer: reach main deploy altitude + * + * Would like to use the accelerometer for this test, but + * the orientation of the flight computer is unknown after + * drogue deploy, so we ignore it. Could also detect + * high descent rate using the pressure sensor to + * recognize drogue deploy failure and eject the main + * at that point. Perhaps also use the drogue sense lines + * to notice continutity? + */ + if (ao_height <= ao_config.main_deploy) + { +#if HAS_IGNITE + ao_ignite(ao_igniter_main); +#endif + + /* + * Start recording min/max height + * to figure out when the rocket has landed + */ + + /* initialize interval values */ + ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; + + ao_interval_min_height = ao_interval_max_height = ao_avg_height; + + ao_flight_state = ao_flight_main; + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } + break; + + /* fall through... */ + case ao_flight_main: + + /* main to land: + * + * barometer: altitude stable + */ + + if (ao_avg_height < ao_interval_min_height) + ao_interval_min_height = ao_avg_height; + if (ao_avg_height > ao_interval_max_height) + ao_interval_max_height = ao_avg_height; + + if ((int16_t) (ao_sample_tick - ao_interval_end) >= 0) { + if (ao_interval_max_height - ao_interval_min_height <= AO_M_TO_HEIGHT(4)) + { + ao_flight_state = ao_flight_landed; + + /* turn off the ADC capture */ + ao_timer_set_adc_interval(0); + + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } + ao_interval_min_height = ao_interval_max_height = ao_avg_height; + ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; + } + break; +#if HAS_FLIGHT_DEBUG + case ao_flight_test: +#if HAS_GYRO + printf ("angle %4d pitch %7d yaw %7d roll %7d\n", + ao_sample_orient, + ((ao_sample_pitch << 9) - ao_ground_pitch) >> 9, + ((ao_sample_yaw << 9) - ao_ground_yaw) >> 9, + ((ao_sample_roll << 9) - ao_ground_roll) >> 9); +#endif + flush(); + break; +#endif /* HAS_FLIGHT_DEBUG */ + default: + break; + } + } +} + +#if HAS_FLIGHT_DEBUG +static inline int int_part(int16_t i) { return i >> 4; } +static inline int frac_part(int16_t i) { return ((i & 0xf) * 100 + 8) / 16; } + +static void +ao_flight_dump(void) +{ +#if HAS_ACCEL + int16_t accel; + + accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16; +#endif + + printf ("sample:\n"); + printf (" tick %d\n", ao_sample_tick); + printf (" raw pres %d\n", ao_sample_pres); +#if HAS_ACCEL + printf (" raw accel %d\n", ao_sample_accel); +#endif + printf (" ground pres %d\n", ao_ground_pres); + printf (" ground alt %d\n", ao_ground_height); +#if HAS_ACCEL + printf (" raw accel %d\n", ao_sample_accel); + printf (" groundaccel %d\n", ao_ground_accel); + printf (" accel_2g %d\n", ao_accel_2g); +#endif + + printf (" alt %d\n", ao_sample_alt); + printf (" height %d\n", ao_sample_height); +#if HAS_ACCEL + printf (" accel %d.%02d\n", int_part(accel), frac_part(accel)); +#endif + + + printf ("kalman:\n"); + printf (" height %d\n", ao_height); + printf (" speed %d.%02d\n", int_part(ao_speed), frac_part(ao_speed)); + printf (" accel %d.%02d\n", int_part(ao_accel), frac_part(ao_accel)); + printf (" max_height %d\n", ao_max_height); + printf (" avg_height %d\n", ao_avg_height); + printf (" error_h %d\n", ao_error_h); + printf (" error_avg %d\n", ao_error_h_sq_avg); +} + +static void +ao_gyro_test(void) +{ + ao_flight_state = ao_flight_test; + ao_getchar(); + ao_flight_state = ao_flight_idle; +} + +uint8_t ao_orient_test; + +static void +ao_orient_test_select(void) +{ + ao_orient_test = !ao_orient_test; +} + +__code struct ao_cmds ao_flight_cmds[] = { + { ao_flight_dump, "F\0Dump flight status" }, + { ao_gyro_test, "G\0Test gyro code" }, + { ao_orient_test_select,"O\0Test orientation code" }, + { 0, NULL }, +}; +#endif + +static __xdata struct ao_task flight_task; + +void +ao_flight_init(void) +{ + ao_flight_state = ao_flight_startup; +#if HAS_FLIGHT_DEBUG + ao_cmd_register(&ao_flight_cmds[0]); +#endif + ao_add_task(&flight_task, ao_flight, "flight"); +} diff --git a/src/kernel/ao_flight.h b/src/kernel/ao_flight.h new file mode 100644 index 00000000..01d21c11 --- /dev/null +++ b/src/kernel/ao_flight.h @@ -0,0 +1,70 @@ +/* + * 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. + */ + +#ifndef _AO_FLIGHT_H_ +#define _AO_FLIGHT_H_ + + +/* + * ao_flight.c + */ + +enum ao_flight_state { + ao_flight_startup = 0, + ao_flight_idle = 1, + ao_flight_pad = 2, + ao_flight_boost = 3, + ao_flight_fast = 4, + ao_flight_coast = 5, + ao_flight_drogue = 6, + ao_flight_main = 7, + ao_flight_landed = 8, + ao_flight_invalid = 9, + ao_flight_test = 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 || HAS_MMA655X +#define HAS_SENSOR_ERRORS 1 +#endif + +#if HAS_SENSOR_ERRORS +extern __xdata uint8_t ao_sensor_errors; +#endif + +extern __pdata uint16_t ao_launch_time; +extern __pdata uint8_t ao_flight_force_idle; + +/* Flight thread */ +void +ao_flight(void); + +/* Initialize flight thread */ +void +ao_flight_init(void); + +/* + * ao_flight_nano.c + */ + +void +ao_flight_nano_init(void); + +#endif /* _AO_FLIGHT_H_ */ diff --git a/src/kernel/ao_flight_nano.c b/src/kernel/ao_flight_nano.c new file mode 100644 index 00000000..406d81ad --- /dev/null +++ b/src/kernel/ao_flight_nano.c @@ -0,0 +1,120 @@ +/* + * 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" + +/* Main flight thread. */ + +__pdata enum ao_flight_state ao_flight_state; /* current flight state */ +__pdata uint16_t ao_launch_tick; /* time of launch detect */ + +/* + * track min/max data over a long interval to detect + * resting + */ +__pdata uint16_t ao_interval_end; +__pdata alt_t ao_interval_min_height; +__pdata alt_t ao_interval_max_height; + +__pdata uint8_t ao_flight_force_idle; + +/* Landing is detected by getting constant readings from both pressure and accelerometer + * for a fairly long time (AO_INTERVAL_TICKS) + */ +#define AO_INTERVAL_TICKS AO_SEC_TO_TICKS(5) + +static void +ao_flight_nano(void) +{ + ao_sample_init(); + ao_flight_state = ao_flight_startup; + + for (;;) { + /* + * Process ADC samples, just looping + * until the sensors are calibrated. + */ + if (!ao_sample()) + continue; + + switch (ao_flight_state) { + case ao_flight_startup: + if (ao_flight_force_idle) { + /* Set idle mode */ + ao_flight_state = ao_flight_idle; + } else { + ao_flight_state = ao_flight_pad; + /* Disable packet mode in pad state */ + ao_packet_slave_stop(); + + /* Turn on telemetry system */ + ao_rdf_set(1); + ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD); + } + /* signal successful initialization by turning off the LED */ + ao_led_off(AO_LED_RED); + + /* wakeup threads due to state change */ + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + break; + case ao_flight_pad: + if (ao_height> AO_M_TO_HEIGHT(20)) { + ao_flight_state = ao_flight_drogue; + ao_launch_tick = ao_sample_tick; + + /* start logging data */ + ao_log_start(); + + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } + break; + case ao_flight_drogue: + /* drogue/main to land: + * + * barometer: altitude stable + */ + + if (ao_height < ao_interval_min_height) + ao_interval_min_height = ao_height; + if (ao_height > ao_interval_max_height) + ao_interval_max_height = ao_height; + + if ((int16_t) (ao_sample_tick - ao_interval_end) >= 0) { + if (ao_interval_max_height - ao_interval_min_height < AO_M_TO_HEIGHT(5)) + { + ao_flight_state = ao_flight_landed; + + /* turn off the ADC capture */ + ao_timer_set_adc_interval(0); + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } + ao_interval_min_height = ao_interval_max_height = ao_height; + ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; + } + break; + } + } +} + +static __xdata struct ao_task flight_task; + +void +ao_flight_nano_init(void) +{ + ao_flight_state = ao_flight_startup; + ao_add_task(&flight_task, ao_flight_nano, "flight"); +} diff --git a/src/kernel/ao_freq.c b/src/kernel/ao_freq.c new file mode 100644 index 00000000..12496f6f --- /dev/null +++ b/src/kernel/ao_freq.c @@ -0,0 +1,55 @@ +/* + * 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 + +/* + * The provided 'calibration' value is + * that needed to tune the radio to precisely 434550kHz. + * Use that to 'walk' to the target frequency by following + * a 'bresenham' line from 434550kHz to the target + * frequency, and updating the radio setting along the way + */ + +int32_t ao_freq_to_set(int32_t freq, int32_t cal) __reentrant +{ + static __pdata int32_t set; + static __pdata uint8_t neg; + static __pdata int32_t error; + + set = 0; + neg = 0; + error = -434550 / 2; + + if ((freq -= 434550) < 0) { + neg = 1; + freq = -freq; + } + for (;;) { + if (error > 0) { + error -= 434550; + set++; + } else { + error += cal; + if (--freq < 0) + break; + } + } + if (neg) + set = -set; + return cal + set; +} diff --git a/src/kernel/ao_gps_print.c b/src/kernel/ao_gps_print.c new file mode 100644 index 00000000..47c945d7 --- /dev/null +++ b/src/kernel/ao_gps_print.c @@ -0,0 +1,112 @@ +/* + * Copyright © 2009 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_GPS_TEST +#include "ao.h" +#endif +#include "ao_telem.h" + +void +ao_gps_print(__xdata struct ao_gps_orig *gps_data) __reentrant +{ + char state; + + if (gps_data->flags & AO_GPS_VALID) + state = AO_TELEM_GPS_STATE_LOCKED; + else if (gps_data->flags & AO_GPS_RUNNING) + state = AO_TELEM_GPS_STATE_UNLOCKED; + else + state = AO_TELEM_GPS_STATE_ERROR; + printf(AO_TELEM_GPS_STATE " %c " + AO_TELEM_GPS_NUM_SAT " %d ", + state, + (gps_data->flags & AO_GPS_NUM_SAT_MASK) >> AO_GPS_NUM_SAT_SHIFT); + if (!(gps_data->flags & AO_GPS_VALID)) + return; + printf(AO_TELEM_GPS_LATITUDE " %ld " + AO_TELEM_GPS_LONGITUDE " %ld " + AO_TELEM_GPS_ALTITUDE " %d ", + (long) gps_data->latitude, + (long) gps_data->longitude, + gps_data->altitude); + + if (gps_data->flags & AO_GPS_DATE_VALID) + printf(AO_TELEM_GPS_YEAR " %d " + AO_TELEM_GPS_MONTH " %d " + AO_TELEM_GPS_DAY " %d ", + gps_data->year, + gps_data->month, + gps_data->day); + + printf(AO_TELEM_GPS_HOUR " %d " + AO_TELEM_GPS_MINUTE " %d " + AO_TELEM_GPS_SECOND " %d ", + gps_data->hour, + gps_data->minute, + gps_data->second); + + printf(AO_TELEM_GPS_HDOP " %d ", + gps_data->hdop * 2); + + if (gps_data->flags & AO_GPS_COURSE_VALID) { + printf(AO_TELEM_GPS_HERROR " %d " + AO_TELEM_GPS_VERROR " %d " + AO_TELEM_GPS_VERTICAL_SPEED " %d " + AO_TELEM_GPS_HORIZONTAL_SPEED " %d " + AO_TELEM_GPS_COURSE " %d ", + gps_data->h_error, + gps_data->v_error, + gps_data->climb_rate, + gps_data->ground_speed, + (int) gps_data->course * 2); + } +} + +void +ao_gps_tracking_print(__xdata struct ao_gps_tracking_orig *gps_tracking_data) __reentrant +{ + uint8_t c, n, v; + __xdata struct ao_gps_sat_orig *sat; + + n = gps_tracking_data->channels; + if (n == 0) + return; + + sat = gps_tracking_data->sats; + v = 0; + for (c = 0; c < n; c++) { + if (sat->svid) + v++; + sat++; + } + + printf (AO_TELEM_SAT_NUM " %d ", + v); + + sat = gps_tracking_data->sats; + v = 0; + for (c = 0; c < n; c++) { + if (sat->svid) { + printf (AO_TELEM_SAT_SVID "%d %d " + AO_TELEM_SAT_C_N_0 "%d %d ", + v, sat->svid, + v, sat->c_n_1); + v++; + } + sat++; + } +} diff --git a/src/kernel/ao_gps_report.c b/src/kernel/ao_gps_report.c new file mode 100644 index 00000000..07201ac2 --- /dev/null +++ b/src/kernel/ao_gps_report.c @@ -0,0 +1,89 @@ +/* + * Copyright © 2009 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" + +void +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 (;;) { + while ((new = ao_gps_new) == 0) + ao_sleep(&ao_gps_new); + ao_mutex_get(&ao_gps_mutex); + 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 ((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); + } + } + if (new & AO_GPS_NEW_TRACKING) { + uint8_t c, n; + + 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; + +void +ao_gps_report_init(void) +{ + ao_add_task(&ao_gps_report_task, ao_gps_report, "gps_report"); +} diff --git a/src/kernel/ao_gps_report_mega.c b/src/kernel/ao_gps_report_mega.c new file mode 100644 index 00000000..5e3c71bf --- /dev/null +++ b/src/kernel/ao_gps_report_mega.c @@ -0,0 +1,90 @@ +/* + * Copyright © 2009 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_log.h" + +void +ao_gps_report_mega(void) +{ + static __xdata struct ao_log_mega gps_log; + static __xdata struct ao_telemetry_location gps_data; + static __xdata struct ao_telemetry_satellite gps_tracking_data; + uint8_t new; + uint8_t c, n, i; + + for (;;) { + while (!(new = ao_gps_new)) + ao_sleep(&ao_gps_new); + ao_mutex_get(&ao_gps_mutex); + 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 ((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.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; + gps_log.u.gps.course = gps_data.course; + gps_log.u.gps.ground_speed = gps_data.ground_speed; + gps_log.u.gps.climb_rate = gps_data.climb_rate; + gps_log.u.gps.pdop = gps_data.pdop; + gps_log.u.gps.hdop = gps_data.hdop; + gps_log.u.gps.vdop = gps_data.vdop; + gps_log.u.gps.mode = gps_data.mode; + 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++; + if (i >= 12) + break; + } + gps_log.u.gps_sat.channels = i; + ao_log_mega(&gps_log); + } + } +} + +__xdata struct ao_task ao_gps_report_mega_task; + +void +ao_gps_report_mega_init(void) +{ + ao_add_task(&ao_gps_report_mega_task, + ao_gps_report_mega, + "gps_report"); +} diff --git a/src/kernel/ao_gps_report_metrum.c b/src/kernel/ao_gps_report_metrum.c new file mode 100644 index 00000000..696a833b --- /dev/null +++ b/src/kernel/ao_gps_report_metrum.c @@ -0,0 +1,96 @@ +/* + * Copyright © 2009 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_log.h" + +void +ao_gps_report_metrum(void) +{ + static __xdata struct ao_log_metrum gps_log; + static __xdata struct ao_telemetry_location gps_data; + static __xdata struct ao_telemetry_satellite gps_tracking_data; + uint8_t c, n, i; + uint8_t svid; + uint8_t new; + + for (;;) { + while (!(new = ao_gps_new)) + ao_sleep(&ao_gps_new); + ao_mutex_get(&ao_gps_mutex); + 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 ((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_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++; + } + } + 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; + +void +ao_gps_report_metrum_init(void) +{ + ao_add_task(&ao_gps_report_metrum_task, + ao_gps_report_metrum, + "gps_report"); +} diff --git a/src/kernel/ao_gps_show.c b/src/kernel/ao_gps_show.c new file mode 100644 index 00000000..3a05e35a --- /dev/null +++ b/src/kernel/ao_gps_show.c @@ -0,0 +1,39 @@ +/* + * 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_GPS_TEST +#include +#endif + +void +ao_gps_show(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); +} diff --git a/src/kernel/ao_host.h b/src/kernel/ao_host.h new file mode 100644 index 00000000..6eb752c9 --- /dev/null +++ b/src/kernel/ao_host.h @@ -0,0 +1,135 @@ +/* + * Copyright © 2009 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 + +#define AO_ADC_RING 64 +#define ao_adc_ring_next(n) (((n) + 1) & (AO_ADC_RING - 1)) +#define ao_adc_ring_prev(n) (((n) - 1) & (AO_ADC_RING - 1)) + +/* + * One set of samples read from the A/D converter + */ +struct ao_adc { + uint16_t tick; /* tick when the sample was read */ + 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 */ +}; + +#define __pdata +#define __data +#define __xdata +#define __code +#define __reentrant + +#define DATA_TO_XDATA(a) (a) +#define PDATA_TO_XDATA(a) (a) +#define CODE_TO_XDATA(a) (a) + +enum ao_flight_state { + ao_flight_startup = 0, + ao_flight_idle = 1, + ao_flight_pad = 2, + ao_flight_boost = 3, + ao_flight_fast = 4, + ao_flight_coast = 5, + ao_flight_drogue = 6, + ao_flight_main = 7, + ao_flight_landed = 8, + ao_flight_invalid = 9 +}; + +struct ao_adc ao_adc_ring[AO_ADC_RING]; +uint8_t ao_adc_head; + +#define ao_led_on(l) +#define ao_led_off(l) +#define ao_timer_set_adc_interval(i) +#define ao_wakeup(wchan) ao_dump_state(wchan) +#define ao_cmd_register(c) +#define ao_usb_disable() +#define ao_telemetry_set_interval(x) +#define ao_delay(x) + +enum ao_igniter { + ao_igniter_drogue = 0, + ao_igniter_main = 1 +}; + +void +ao_ignite(enum ao_igniter igniter) +{ + printf ("ignite %s\n", igniter == ao_igniter_drogue ? "drogue" : "main"); +} + +struct ao_task { + int dummy; +}; + +#define ao_add_task(t,f,n) + +#define ao_log_start() +#define ao_log_stop() + +#define AO_MS_TO_TICKS(ms) ((ms) / 10) +#define AO_SEC_TO_TICKS(s) ((s) * 100) + +#define AO_FLIGHT_TEST + +struct ao_adc ao_adc_static; + +FILE *emulator_in; + +void +ao_dump_state(void *wchan); + +void +ao_sleep(void *wchan); + +const char const * const ao_state_names[] = { + "startup", "idle", "pad", "boost", "fast", + "coast", "drogue", "main", "landed", "invalid" +}; + +struct ao_cmds { + void (*func)(void); + const char *help; +}; + + +struct ao_config { + uint16_t main_deploy; + int16_t accel_zero_g; +}; + +#define ao_config_get() + +struct ao_config ao_config = { 250, 16000 }; + +#define ao_xmemcpy(d,s,c) memcpy(d,s,c) +#define ao_xmemset(d,v,c) memset(d,v,c) +#define ao_xmemcmp(d,s,c) memcmp(d,s,c) diff --git a/src/kernel/ao_ignite.c b/src/kernel/ao_ignite.c new file mode 100644 index 00000000..823d003c --- /dev/null +++ b/src/kernel/ao_ignite.c @@ -0,0 +1,241 @@ +/* + * Copyright © 2009 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 +#if AO_PYRO_NUM +#include +#endif + +#if HAS_IGNITE +__xdata struct ao_ignition ao_ignition[2]; + +void +ao_ignite(enum ao_igniter igniter) +{ + ao_arch_block_interrupts(); + ao_ignition[igniter].request = 1; + ao_wakeup(&ao_ignition); + ao_arch_release_interrupts(); +} + +#ifndef AO_SENSE_DROGUE +#define AO_SENSE_DROGUE(p) ((p)->adc.sense_d) +#define AO_SENSE_MAIN(p) ((p)->adc.sense_m) +#endif + +enum ao_igniter_status +ao_igniter_status(enum ao_igniter igniter) +{ + __xdata struct ao_data packet; + __pdata int16_t value; + __pdata uint8_t request, firing, fired; + + ao_arch_critical( + ao_data_get(&packet); + request = ao_ignition[igniter].request; + fired = ao_ignition[igniter].fired; + firing = ao_ignition[igniter].firing; + ); + if (firing || (request && !fired)) + return ao_igniter_active; + + value = (AO_IGNITER_CLOSED>>1); + switch (igniter) { + case ao_igniter_drogue: + value = AO_SENSE_DROGUE(&packet); + break; + case ao_igniter_main: + value = AO_SENSE_MAIN(&packet); + break; + } + if (value < AO_IGNITER_OPEN) + return ao_igniter_open; + else if (value > AO_IGNITER_CLOSED) + return ao_igniter_ready; + else + return ao_igniter_unknown; +} + +#ifndef AO_IGNITER_SET_DROGUE +#define AO_IGNITER_SET_DROGUE(v) AO_IGNITER_DROGUE = (v) +#define AO_IGNITER_SET_MAIN(v) AO_IGNITER_MAIN = (v) +#endif + +#ifndef AO_IGNITER_FIRE_TIME +#define AO_IGNITER_FIRE_TIME AO_MS_TO_TICKS(50) +#endif + +#ifndef AO_IGNITER_CHARGE_TIME +#define AO_IGNITER_CHARGE_TIME AO_MS_TO_TICKS(2000) +#endif + +void +ao_igniter_fire(enum ao_igniter igniter) +{ + ao_ignition[igniter].firing = 1; + switch(ao_config.ignite_mode) { + case AO_IGNITE_MODE_DUAL: + switch (igniter) { + case ao_igniter_drogue: + AO_IGNITER_SET_DROGUE(1); + ao_delay(AO_IGNITER_FIRE_TIME); + AO_IGNITER_SET_DROGUE(0); + break; + case ao_igniter_main: + AO_IGNITER_SET_MAIN(1); + ao_delay(AO_IGNITER_FIRE_TIME); + AO_IGNITER_SET_MAIN(0); + break; + } + break; + case AO_IGNITE_MODE_APOGEE: + switch (igniter) { + case ao_igniter_drogue: + AO_IGNITER_SET_DROGUE(1); + ao_delay(AO_IGNITER_FIRE_TIME); + AO_IGNITER_SET_DROGUE(0); + ao_delay(AO_IGNITER_CHARGE_TIME); + AO_IGNITER_SET_MAIN(1); + ao_delay(AO_IGNITER_FIRE_TIME); + AO_IGNITER_SET_MAIN(0); + break; + default: + break; + } + break; + case AO_IGNITE_MODE_MAIN: + switch (igniter) { + case ao_igniter_main: + AO_IGNITER_SET_DROGUE(1); + ao_delay(AO_IGNITER_FIRE_TIME); + AO_IGNITER_SET_DROGUE(0); + ao_delay(AO_IGNITER_CHARGE_TIME); + AO_IGNITER_SET_MAIN(1); + ao_delay(AO_IGNITER_FIRE_TIME); + AO_IGNITER_SET_MAIN(0); + break; + default: + break; + } + break; + } + ao_ignition[igniter].firing = 0; +} + +void +ao_igniter(void) +{ + __xdata enum ao_igniter igniter; + + ao_config_get(); + for (;;) { + ao_sleep(&ao_ignition); + for (igniter = ao_igniter_drogue; igniter <= ao_igniter_main; igniter++) { + if (ao_ignition[igniter].request && !ao_ignition[igniter].fired) { + if (igniter == ao_igniter_drogue && ao_config.apogee_delay) + ao_delay(AO_SEC_TO_TICKS(ao_config.apogee_delay)); + + ao_igniter_fire(igniter); + ao_delay(AO_IGNITER_CHARGE_TIME); + ao_ignition[igniter].fired = 1; + } + } + } +} + +#endif + +void +ao_ignite_manual(void) +{ + ao_cmd_white(); + if (!ao_match_word("DoIt")) + return; + ao_cmd_white(); +#if HAS_IGNITE + if (ao_cmd_lex_c == 'm' && ao_match_word("main")) { + ao_igniter_fire(ao_igniter_main); + return; + } + if (ao_cmd_lex_c == 'd' && ao_match_word("drogue")) { + ao_igniter_fire(ao_igniter_drogue); + return; + } +#endif +#if AO_PYRO_NUM + if ('0' <= ao_cmd_lex_c && ao_cmd_lex_c <= '9') { + ao_pyro_manual(ao_cmd_lex_c - '0'); + return; + } +#endif + ao_cmd_status = ao_cmd_syntax_error; +} + +__code char * __code ao_igniter_status_names[] = { + "unknown", "ready", "active", "open" +}; + +#if HAS_IGNITE +void +ao_ignite_print_status(enum ao_igniter igniter, __code char *name) __reentrant +{ + enum ao_igniter_status status = ao_igniter_status(igniter); + printf("Igniter: %6s Status: %s\n", + name, + ao_igniter_status_names[status]); +} +#endif + +void +ao_ignite_test(void) +{ +#if HAS_IGNITE + ao_ignite_print_status(ao_igniter_drogue, "drogue"); + ao_ignite_print_status(ao_igniter_main, "main"); +#endif +#if AO_PYRO_NUM + ao_pyro_print_status(); +#endif +} + +__code struct ao_cmds ao_ignite_cmds[] = { + { ao_ignite_manual, "i {main|drogue}\0Fire igniter. is doit with D&I" }, + { ao_ignite_test, "t\0Test igniter" }, + { 0, NULL }, +}; + +#if HAS_IGNITE +__xdata struct ao_task ao_igniter_task; + +void +ao_ignite_set_pins(void) +{ + ao_enable_output(AO_IGNITER_DROGUE_PORT, AO_IGNITER_DROGUE_PIN, AO_IGNITER_DROGUE, 0); + ao_enable_output(AO_IGNITER_MAIN_PORT, AO_IGNITER_MAIN_PIN, AO_IGNITER_MAIN, 0); +} +#endif + +void +ao_igniter_init(void) +{ +#if HAS_IGNITE + ao_ignite_set_pins(); + ao_add_task(&ao_igniter_task, ao_igniter, "igniter"); +#endif + ao_cmd_register(&ao_ignite_cmds[0]); +} diff --git a/src/kernel/ao_int64.c b/src/kernel/ao_int64.c new file mode 100644 index 00000000..aa23dbe0 --- /dev/null +++ b/src/kernel/ao_int64.c @@ -0,0 +1,158 @@ +/* + * 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 + +__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; + if (t < a->low) + r->high++; + r->low = 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; + if (t > a->low) + r->high--; + r->low = t; +} + +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) + r->low |= a->high << (32 - d); + r->high = (int32_t) a->high >> d; + } else { + d &= 0x1f; + r->low = (int32_t) a->high >> d; + r->high = 0; + } +} + +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) + r->high |= a->low >> (32 - d); + r->low = a->low << d; + } else { + d &= 0x1f; + r->high = a->low << d; + r->low = 0; + } +} + +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(__pdata ao_int64_t *r, __pdata ao_int64_t *a) __FATTR { + r->high = ~a->high; + if (!(r->low = ~a->low + 1)) + r->high++; +} + +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; + } + if (b < 0) { + b = -b; + --negative; + } + ao_umul64_32_32(r, a, b); + if (negative) + ao_neg64(r, r); +} + +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); + ao_umul64_32_32(r, a->low, b->low); + + r->high += r2.low + r3.low; +} + +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; + + if (ao_int64_negativep(a)) { + ao_neg64(&ap, a); + a = ≈ + ++negative; + } + if (ao_int64_negativep(b)) { + ao_neg64(&bp, b); + b = &bp; + --negative; + } + ao_umul64(r, a, b); + if (negative) + ao_neg64(r, r); +} + +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(__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++; + } else + ao_umul64_64_16(r, a, b); + if (negative) + ao_neg64(r, r); +} diff --git a/src/kernel/ao_int64.h b/src/kernel/ao_int64.h new file mode 100644 index 00000000..b16db58c --- /dev/null +++ b/src/kernel/ao_int64.h @@ -0,0 +1,48 @@ +/* + * 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_INT64_H_ +#define _AO_INT64_H_ + +#include + +typedef struct { + uint32_t high; + uint32_t low; +} ao_int64_t; + +#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)) + +#define ao_cast64(a) (((int64_t) (a)->high << 32) | (a)->low) + +#define ao_int64_negativep(a) (((int32_t) (a)->high) < 0) + +#endif /* _AO_INT64_H_ */ diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c new file mode 100644 index 00000000..9aea1f14 --- /dev/null +++ b/src/kernel/ao_kalman.c @@ -0,0 +1,295 @@ +/* + * 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. + */ + +#ifndef AO_FLIGHT_TEST +#include "ao.h" +#include "ao_flight.h" +#endif + +#include "ao_sample.h" +#include "ao_kalman.h" + + +static __pdata int32_t ao_k_height; +static __pdata int32_t ao_k_speed; +static __pdata int32_t ao_k_accel; + +#define AO_K_STEP_100 to_fix16(0.01) +#define AO_K_STEP_2_2_100 to_fix16(0.00005) + +#define AO_K_STEP_10 to_fix16(0.1) +#define AO_K_STEP_2_2_10 to_fix16(0.005) + +#define AO_K_STEP_1 to_fix16(1) +#define AO_K_STEP_2_2_1 to_fix16(0.5) + +__pdata int16_t ao_height; +__pdata int16_t ao_speed; +__pdata int16_t ao_accel; +__xdata int16_t ao_max_height; +static __pdata int32_t ao_avg_height_scaled; +__xdata int16_t ao_avg_height; + +__pdata int16_t ao_error_h; +__pdata int16_t ao_error_h_sq_avg; + +#if HAS_ACCEL +__pdata int16_t ao_error_a; +#endif + +static void +ao_kalman_predict(void) +{ +#ifdef AO_FLIGHT_TEST + if (ao_sample_tick - ao_sample_prev_tick > 50) { + ao_k_height += ((int32_t) ao_speed * AO_K_STEP_1 + + (int32_t) ao_accel * AO_K_STEP_2_2_1) >> 4; + ao_k_speed += (int32_t) ao_accel * AO_K_STEP_1; + + return; + } + if (ao_sample_tick - ao_sample_prev_tick > 5) { + ao_k_height += ((int32_t) ao_speed * AO_K_STEP_10 + + (int32_t) ao_accel * AO_K_STEP_2_2_10) >> 4; + ao_k_speed += (int32_t) ao_accel * AO_K_STEP_10; + + return; + } + if (ao_flight_debug) { + printf ("predict speed %g + (%g * %g) = %g\n", + ao_k_speed / (65536.0 * 16.0), ao_accel / 16.0, AO_K_STEP_100 / 65536.0, + (ao_k_speed + (int32_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0)); + } +#endif + ao_k_height += ((int32_t) ao_speed * AO_K_STEP_100 + + (int32_t) ao_accel * AO_K_STEP_2_2_100) >> 4; + ao_k_speed += (int32_t) ao_accel * AO_K_STEP_100; +} + +static void +ao_kalman_err_height(void) +{ + int16_t e; + int16_t height_distrust; +#if HAS_ACCEL + int16_t speed_distrust; +#endif + + ao_error_h = ao_sample_height - (int16_t) (ao_k_height >> 16); + + e = ao_error_h; + if (e < 0) + e = -e; + if (e > 127) + e = 127; +#if HAS_ACCEL + ao_error_h_sq_avg -= ao_error_h_sq_avg >> 2; + ao_error_h_sq_avg += (e * e) >> 2; +#else + ao_error_h_sq_avg -= ao_error_h_sq_avg >> 4; + ao_error_h_sq_avg += (e * e) >> 4; +#endif + + if (ao_flight_state >= ao_flight_drogue) + return; + height_distrust = ao_sample_alt - AO_MAX_BARO_HEIGHT; +#if HAS_ACCEL + /* speed is stored * 16, but we need to ramp between 200 and 328, so + * we want to multiply by 2. The result is a shift by 3. + */ + speed_distrust = (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) >> (4 - 1); + if (speed_distrust <= 0) + speed_distrust = 0; + else if (speed_distrust > height_distrust) + height_distrust = speed_distrust; +#endif + if (height_distrust > 0) { +#ifdef AO_FLIGHT_TEST + int old_ao_error_h = ao_error_h; +#endif + if (height_distrust > 0x100) + height_distrust = 0x100; + ao_error_h = (int16_t) (((int32_t) ao_error_h * (0x100 - height_distrust)) >> 8); +#ifdef AO_FLIGHT_TEST + if (ao_flight_debug) { + printf("over height %g over speed %g distrust: %g height: error %d -> %d\n", + (double) (ao_sample_alt - AO_MAX_BARO_HEIGHT), + (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) / 16.0, + height_distrust / 256.0, + old_ao_error_h, ao_error_h); + } +#endif + } +} + +static void +ao_kalman_correct_baro(void) +{ + ao_kalman_err_height(); +#ifdef AO_FLIGHT_TEST + if (ao_sample_tick - ao_sample_prev_tick > 50) { + ao_k_height += (int32_t) AO_BARO_K0_1 * ao_error_h; + ao_k_speed += (int32_t) AO_BARO_K1_1 * ao_error_h; + ao_k_accel += (int32_t) AO_BARO_K2_1 * ao_error_h; + return; + } + if (ao_sample_tick - ao_sample_prev_tick > 5) { + ao_k_height += (int32_t) AO_BARO_K0_10 * ao_error_h; + ao_k_speed += (int32_t) AO_BARO_K1_10 * ao_error_h; + ao_k_accel += (int32_t) AO_BARO_K2_10 * ao_error_h; + return; + } +#endif + ao_k_height += (int32_t) AO_BARO_K0_100 * ao_error_h; + ao_k_speed += (int32_t) AO_BARO_K1_100 * ao_error_h; + ao_k_accel += (int32_t) AO_BARO_K2_100 * ao_error_h; +} + +#if HAS_ACCEL + +static void +ao_kalman_err_accel(void) +{ + int32_t accel; + + accel = (ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale; + + /* Can't use ao_accel here as it is the pre-prediction value still */ + ao_error_a = (accel - ao_k_accel) >> 16; +} + +#ifndef FORCE_ACCEL +static void +ao_kalman_correct_both(void) +{ + ao_kalman_err_height(); + ao_kalman_err_accel(); + +#ifdef AO_FLIGHT_TEST + if (ao_sample_tick - ao_sample_prev_tick > 50) { + if (ao_flight_debug) { + printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n", + ao_k_speed / (65536.0 * 16.0), + (double) ao_error_h, AO_BOTH_K10_1 / 65536.0, + (double) ao_error_a, AO_BOTH_K11_1 / 65536.0, + (ao_k_speed + + (int32_t) AO_BOTH_K10_1 * ao_error_h + + (int32_t) AO_BOTH_K11_1 * ao_error_a) / (65536.0 * 16.0)); + } + ao_k_height += + (int32_t) AO_BOTH_K00_1 * ao_error_h + + (int32_t) AO_BOTH_K01_1 * ao_error_a; + ao_k_speed += + (int32_t) AO_BOTH_K10_1 * ao_error_h + + (int32_t) AO_BOTH_K11_1 * ao_error_a; + ao_k_accel += + (int32_t) AO_BOTH_K20_1 * ao_error_h + + (int32_t) AO_BOTH_K21_1 * ao_error_a; + return; + } + if (ao_sample_tick - ao_sample_prev_tick > 5) { + if (ao_flight_debug) { + printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n", + ao_k_speed / (65536.0 * 16.0), + (double) ao_error_h, AO_BOTH_K10_10 / 65536.0, + (double) ao_error_a, AO_BOTH_K11_10 / 65536.0, + (ao_k_speed + + (int32_t) AO_BOTH_K10_10 * ao_error_h + + (int32_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0)); + } + ao_k_height += + (int32_t) AO_BOTH_K00_10 * ao_error_h + + (int32_t) AO_BOTH_K01_10 * ao_error_a; + ao_k_speed += + (int32_t) AO_BOTH_K10_10 * ao_error_h + + (int32_t) AO_BOTH_K11_10 * ao_error_a; + ao_k_accel += + (int32_t) AO_BOTH_K20_10 * ao_error_h + + (int32_t) AO_BOTH_K21_10 * ao_error_a; + return; + } + if (ao_flight_debug) { + printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n", + ao_k_speed / (65536.0 * 16.0), + (double) ao_error_h, AO_BOTH_K10_100 / 65536.0, + (double) ao_error_a, AO_BOTH_K11_100 / 65536.0, + (ao_k_speed + + (int32_t) AO_BOTH_K10_100 * ao_error_h + + (int32_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0)); + } +#endif + ao_k_height += + (int32_t) AO_BOTH_K00_100 * ao_error_h + + (int32_t) AO_BOTH_K01_100 * ao_error_a; + ao_k_speed += + (int32_t) AO_BOTH_K10_100 * ao_error_h + + (int32_t) AO_BOTH_K11_100 * ao_error_a; + ao_k_accel += + (int32_t) AO_BOTH_K20_100 * ao_error_h + + (int32_t) AO_BOTH_K21_100 * ao_error_a; +} + +#else + +static void +ao_kalman_correct_accel(void) +{ + ao_kalman_err_accel(); + + if (ao_sample_tick - ao_sample_prev_tick > 5) { + ao_k_height +=(int32_t) AO_ACCEL_K0_10 * ao_error_a; + ao_k_speed += (int32_t) AO_ACCEL_K1_10 * ao_error_a; + ao_k_accel += (int32_t) AO_ACCEL_K2_10 * ao_error_a; + return; + } + ao_k_height += (int32_t) AO_ACCEL_K0_100 * ao_error_a; + ao_k_speed += (int32_t) AO_ACCEL_K1_100 * ao_error_a; + ao_k_accel += (int32_t) AO_ACCEL_K2_100 * ao_error_a; +} + +#endif /* else FORCE_ACCEL */ +#endif /* HAS_ACCEL */ + +void +ao_kalman(void) +{ + ao_kalman_predict(); +#if HAS_ACCEL + if (ao_flight_state <= ao_flight_coast) { +#ifdef FORCE_ACCEL + ao_kalman_correct_accel(); +#else + ao_kalman_correct_both(); +#endif + } else +#endif + ao_kalman_correct_baro(); + ao_height = from_fix(ao_k_height); + ao_speed = from_fix(ao_k_speed); + ao_accel = from_fix(ao_k_accel); + if (ao_height > ao_max_height) + ao_max_height = ao_height; + ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_sample_height; +#ifdef AO_FLIGHT_TEST + if (ao_sample_tick - ao_sample_prev_tick > 50) + ao_avg_height = (ao_avg_height_scaled + 1) >> 1; + else if (ao_sample_tick - ao_sample_prev_tick > 5) + ao_avg_height = (ao_avg_height_scaled + 7) >> 4; + else +#endif + ao_avg_height = (ao_avg_height_scaled + 63) >> 7; +} diff --git a/src/kernel/ao_lcd.h b/src/kernel/ao_lcd.h new file mode 100644 index 00000000..f7e1391a --- /dev/null +++ b/src/kernel/ao_lcd.h @@ -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. + */ + +#ifndef _AO_LCD_H_ +#define _AO_LCD_H_ + +/* ao_lcd.c */ + +void +ao_lcd_putchar(uint8_t d); + +void +ao_lcd_putstring(char *string); + +void +ao_lcd_contrast_set(uint8_t contrast); + +void +ao_lcd_clear(void); + +void +ao_lcd_cursor_on(void); + +void +ao_lcd_cursor_off(void); + +#define AO_LCD_ADDR(row,col) ((row << 6) | (col)) + +void +ao_lcd_goto(uint8_t addr); + +void +ao_lcd_start(void); + +void +ao_lcd_init(void); + +/* ao_lcd_port.c */ + +void +ao_lcd_port_put_nibble(uint8_t rs, uint8_t d); + +void +ao_lcd_port_init(void); + +#endif /* _AO_LCD_H_ */ diff --git a/src/kernel/ao_led.h b/src/kernel/ao_led.h new file mode 100644 index 00000000..d9a0914a --- /dev/null +++ b/src/kernel/ao_led.h @@ -0,0 +1,59 @@ +/* + * 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. + */ + +#ifndef _AO_LED_H_ +#define _AO_LED_H_ + +/* + * ao_led.c + */ + +#define AO_LED_NONE 0 + +#ifndef AO_LED_TYPE +#define AO_LED_TYPE uint8_t +#endif + +/* Turn on the specified LEDs */ +void +ao_led_on(AO_LED_TYPE colors); + +/* Turn off the specified LEDs */ +void +ao_led_off(AO_LED_TYPE colors); + +/* Set all of the LEDs to the specified state */ +void +ao_led_set(AO_LED_TYPE colors); + +/* Set all LEDs in 'mask' to the specified state */ +void +ao_led_set_mask(uint8_t colors, uint8_t mask); + +/* Toggle the specified LEDs */ +void +ao_led_toggle(AO_LED_TYPE colors); + +/* Turn on the specified LEDs for the indicated interval */ +void +ao_led_for(AO_LED_TYPE colors, uint16_t ticks) __reentrant; + +/* Initialize the LEDs */ +void +ao_led_init(AO_LED_TYPE enable); + +#endif /* _AO_LED_H_ */ diff --git a/src/kernel/ao_list.h b/src/kernel/ao_list.h new file mode 100644 index 00000000..8a6fa4d9 --- /dev/null +++ b/src/kernel/ao_list.h @@ -0,0 +1,213 @@ +/* + * 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. + */ + +#ifndef _AO_LIST_H_ +#define _AO_LIST_H_ + +#include + +struct ao_list { + struct ao_list *next, *prev; +}; + +static inline void +ao_list_init(struct ao_list *list) +{ + list->next = list->prev = list; +} + +static inline void +__ao_list_add(struct ao_list *list, struct ao_list *prev, struct ao_list *next) +{ + next->prev = list; + list->next = next; + list->prev = prev; + prev->next = list; +} + +/** + * Insert a new element after the given list head. The new element does not + * need to be initialised as empty list. + * The list changes from: + * head → some element → ... + * to + * head → new element → older element → ... + * + * Example: + * struct foo *newfoo = malloc(...); + * ao_list_add(&newfoo->entry, &bar->list_of_foos); + * + * @param entry The new element to prepend to the list. + * @param head The existing list. + */ +static inline void +ao_list_insert(struct ao_list *entry, struct ao_list *head) +{ + __ao_list_add(entry, head, head->next); +} + +/** + * Append a new element to the end of the list given with this list head. + * + * The list changes from: + * head → some element → ... → lastelement + * to + * head → some element → ... → lastelement → new element + * + * Example: + * struct foo *newfoo = malloc(...); + * ao_list_append(&newfoo->entry, &bar->list_of_foos); + * + * @param entry The new element to prepend to the list. + * @param head The existing list. + */ +static inline void +ao_list_append(struct ao_list *entry, struct ao_list *head) +{ + __ao_list_add(entry, head->prev, head); +} + +static inline void +__ao_list_del(struct ao_list *prev, struct ao_list *next) +{ + next->prev = prev; + prev->next = next; +} + +/** + * Remove the element from the list it is in. Using this function will reset + * the pointers to/from this element so it is removed from the list. It does + * NOT free the element itself or manipulate it otherwise. + * + * Using ao_list_del on a pure list head (like in the example at the top of + * this file) will NOT remove the first element from + * the list but rather reset the list as empty list. + * + * Example: + * ao_list_del(&foo->entry); + * + * @param entry The element to remove. + */ +static inline void +ao_list_del(struct ao_list *entry) +{ + __ao_list_del(entry->prev, entry->next); + ao_list_init(entry); +} + +/** + * Check if the list is empty. + * + * Example: + * ao_list_is_empty(&bar->list_of_foos); + * + * @return True if the list contains one or more elements or False otherwise. + */ +static inline uint8_t +ao_list_is_empty(struct ao_list *head) +{ + return head->next == head; +} + +/** + * Returns a pointer to the container of this list element. + * + * Example: + * struct foo* f; + * f = container_of(&foo->entry, struct foo, entry); + * assert(f == foo); + * + * @param ptr Pointer to the struct ao_list. + * @param type Data type of the list element. + * @param member Member name of the struct ao_list field in the list element. + * @return A pointer to the data struct containing the list head. + */ +#define ao_container_of(ptr, type, member) \ + ((type *)((char *)(ptr) - offsetof(type, member))) + +/** + * Alias of ao_container_of + */ +#define ao_list_entry(ptr, type, member) \ + ao_container_of(ptr, type, member) + +/** + * Retrieve the first list entry for the given list pointer. + * + * Example: + * struct foo *first; + * first = ao_list_first_entry(&bar->list_of_foos, struct foo, list_of_foos); + * + * @param ptr The list head + * @param type Data type of the list element to retrieve + * @param member Member name of the struct ao_list field in the list element. + * @return A pointer to the first list element. + */ +#define ao_list_first_entry(ptr, type, member) \ + ao_list_entry((ptr)->next, type, member) + +/** + * Retrieve the last list entry for the given listpointer. + * + * Example: + * struct foo *first; + * first = ao_list_last_entry(&bar->list_of_foos, struct foo, list_of_foos); + * + * @param ptr The list head + * @param type Data type of the list element to retrieve + * @param member Member name of the struct ao_list field in the list element. + * @return A pointer to the last list element. + */ +#define ao_list_last_entry(ptr, type, member) \ + ao_list_entry((ptr)->prev, type, member) + +/** + * Loop through the list given by head and set pos to struct in the list. + * + * Example: + * struct foo *iterator; + * ao_list_for_each_entry(iterator, &bar->list_of_foos, entry) { + * [modify iterator] + * } + * + * This macro is not safe for node deletion. Use ao_list_for_each_entry_safe + * instead. + * + * @param pos Iterator variable of the type of the list elements. + * @param head List head + * @param member Member name of the struct ao_list in the list elements. + * + */ +#define ao_list_for_each_entry(pos, head, type, member) \ + for (pos = ao_container_of((head)->next, type, member); \ + &pos->member != (head); \ + pos = ao_container_of(pos->member.next, type, member)) + +/** + * Loop through the list, keeping a backup pointer to the element. This + * macro allows for the deletion of a list element while looping through the + * list. + * + * See ao_list_for_each_entry for more details. + */ +#define ao_list_for_each_entry_safe(pos, tmp, head, type, member) \ + for ((pos = ao_container_of((head)->next, type, member)), \ + (tmp = ao_container_of(pos->member.next, type, member)); \ + &pos->member != (head); \ + (pos = tmp), (tmp = ao_container_of(pos->member.next, type, member))) + +#endif /* _AO_LIST_H_ */ diff --git a/src/kernel/ao_log.c b/src/kernel/ao_log.c new file mode 100644 index 00000000..20febefe --- /dev/null +++ b/src/kernel/ao_log.c @@ -0,0 +1,291 @@ +/* + * Copyright © 2009 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 +#include + +__pdata uint32_t ao_log_current_pos; +__pdata uint32_t ao_log_end_pos; +__pdata uint32_t ao_log_start_pos; +__xdata uint8_t ao_log_running; +__pdata enum ao_flight_state ao_log_state; +__xdata uint16_t ao_flight_number; + +void +ao_log_flush(void) +{ + ao_storage_flush(); +} + +/* + * When erasing a flight log, make sure the config block + * has an up-to-date version of the current flight number + */ + +struct ao_log_erase { + uint8_t unused; + uint16_t flight; +}; + +static __xdata struct ao_log_erase erase; + +#define LOG_MAX_ERASE 16 + +static uint32_t +ao_log_erase_pos(uint8_t i) +{ + return i * sizeof (struct ao_log_erase) + AO_CONFIG_MAX_SIZE; +} + +void +ao_log_write_erase(uint8_t pos) +{ + erase.unused = 0x00; + erase.flight = ao_flight_number; + ao_config_write(ao_log_erase_pos(pos), &erase, sizeof (erase)); + ao_config_flush(); +} + +static void +ao_log_read_erase(uint8_t pos) +{ + ao_config_read(ao_log_erase_pos(pos), &erase, sizeof (erase)); +} + + +static void +ao_log_erase_mark(void) +{ + uint8_t i; + + for (i = 0; i < LOG_MAX_ERASE; i++) { + ao_log_read_erase(i); + if (erase.unused == 0 && erase.flight == ao_flight_number) + return; + if (erase.unused == 0xff) { + ao_log_write_erase(i); + return; + } + } + ao_config_put(); +} + +static uint8_t +ao_log_slots() +{ + return (uint8_t) (ao_storage_log_max / ao_config.flight_log_max); +} + +uint32_t +ao_log_pos(uint8_t slot) +{ + return ((slot) * ao_config.flight_log_max); +} + +static uint16_t +ao_log_max_flight(void) +{ + uint8_t log_slot; + uint8_t log_slots; + uint16_t log_flight; + uint16_t max_flight = 0; + + /* Scan the log space looking for the biggest flight number */ + log_slots = ao_log_slots(); + for (log_slot = 0; log_slot < log_slots; log_slot++) { + log_flight = ao_log_flight(log_slot); + if (!log_flight) + continue; + if (max_flight == 0 || (int16_t) (log_flight - max_flight) > 0) + max_flight = log_flight; + } + return max_flight; +} + +void +ao_log_scan(void) __reentrant +{ + uint8_t log_slot; + uint8_t log_slots; + uint8_t log_want; + + ao_config_get(); + + ao_flight_number = ao_log_max_flight(); + if (ao_flight_number) + if (++ao_flight_number == 0) + ao_flight_number = 1; + + /* Now look through the log of flight numbers from erase operations and + * see if the last one is bigger than what we found above + */ + for (log_slot = LOG_MAX_ERASE; log_slot-- > 0;) { + ao_log_read_erase(log_slot); + if (erase.unused == 0) { + if (ao_flight_number == 0 || + (int16_t) (erase.flight - ao_flight_number) > 0) + ao_flight_number = erase.flight; + break; + } + } + if (ao_flight_number == 0) + ao_flight_number = 1; + + /* With a flight number in hand, find a place to write a new log, + * use the target flight number to index the available log slots so + * that we write logs to each spot about the same number of times. + */ + + /* Find a log slot for the next flight, if available */ + ao_log_current_pos = ao_log_end_pos = 0; + log_slots = ao_log_slots(); + log_want = (ao_flight_number - 1) % log_slots; + log_slot = log_want; + do { + if (ao_log_flight(log_slot) == 0) { + ao_log_current_pos = ao_log_pos(log_slot); + ao_log_end_pos = ao_log_current_pos + ao_config.flight_log_max; + break; + } + if (++log_slot >= log_slots) + log_slot = 0; + } while (log_slot != log_want); + + ao_wakeup(&ao_flight_number); +} + +void +ao_log_start(void) +{ + /* start logging */ + ao_log_running = 1; + ao_wakeup(&ao_log_running); +} + +void +ao_log_stop(void) +{ + ao_log_running = 0; + ao_log_flush(); +} + +uint8_t +ao_log_present(void) +{ + return ao_log_max_flight() != 0; +} + +uint8_t +ao_log_full(void) +{ + return ao_log_current_pos == ao_log_end_pos; +} + +#if HAS_ADC +static __xdata struct ao_task ao_log_task; +#endif + +void +ao_log_list(void) __reentrant +{ + uint8_t slot; + uint8_t slots; + uint16_t flight; + + slots = ao_log_slots(); + for (slot = 0; slot < slots; slot++) + { + flight = ao_log_flight(slot); + if (flight) + printf ("flight %d start %x end %x\n", + flight, + (uint16_t) (ao_log_pos(slot) >> 8), + (uint16_t) (ao_log_pos(slot+1) >> 8)); + } + printf ("done\n"); +} + +void +ao_log_delete(void) __reentrant +{ + uint8_t slot; + uint8_t slots; + + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + + slots = ao_log_slots(); + /* Look for the flight log matching the requested flight */ + if (ao_cmd_lex_i) { + for (slot = 0; slot < slots; slot++) { + if (ao_log_flight(slot) == ao_cmd_lex_i) { + ao_log_erase_mark(); + ao_log_current_pos = ao_log_pos(slot); + ao_log_end_pos = ao_log_current_pos + ao_config.flight_log_max; + while (ao_log_current_pos < ao_log_end_pos) { + uint8_t i; + static __xdata uint8_t b; + + /* + * Check to see if we've reached the end of + * the used memory to avoid re-erasing the same + * memory over and over again + */ + for (i = 0; i < 16; i++) { + if (ao_storage_read(ao_log_current_pos + i, &b, 1)) + if (b != 0xff) + break; + } + if (i == 16) + break; + ao_storage_erase(ao_log_current_pos); + ao_log_current_pos += ao_storage_block; + } + puts("Erased"); + return; + } + } + } + printf("No such flight: %d\n", ao_cmd_lex_i); +} + +__code struct ao_cmds ao_log_cmds[] = { + { ao_log_list, "l\0List logs" }, + { ao_log_delete, "d \0Delete flight" }, + { 0, NULL }, +}; + +void +ao_log_init(void) +{ + ao_log_running = 0; + + /* For now, just log the flight starting at the begining of eeprom */ + ao_log_state = ao_flight_invalid; + + ao_cmd_register(&ao_log_cmds[0]); + +#ifndef HAS_ADC +#error Define HAS_ADC for ao_log.c +#endif +#if HAS_ADC + /* Create a task to log events to eeprom */ + ao_add_task(&ao_log_task, ao_log, "log"); +#endif +} diff --git a/src/kernel/ao_log.h b/src/kernel/ao_log.h new file mode 100644 index 00000000..09f31188 --- /dev/null +++ b/src/kernel/ao_log.h @@ -0,0 +1,386 @@ +/* + * 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. + */ + +#ifndef _AO_LOG_H_ +#define _AO_LOG_H_ + +#include + +/* + * ao_log.c + */ + +/* We record flight numbers in the first record of + * the log. Tasks may wait for this to be initialized + * by sleeping on this variable. + */ +extern __xdata uint16_t ao_flight_number; + +extern __pdata uint32_t ao_log_current_pos; +extern __pdata uint32_t ao_log_end_pos; +extern __pdata uint32_t ao_log_start_pos; +extern __xdata uint8_t ao_log_running; +extern __pdata enum ao_flight_state ao_log_state; + +/* required functions from the underlying log system */ + +#define AO_LOG_FORMAT_UNKNOWN 0 /* unknown; altosui will have to guess */ +#define AO_LOG_FORMAT_FULL 1 /* 8 byte typed log records */ +#define AO_LOG_FORMAT_TINY 2 /* two byte state/baro records */ +#define AO_LOG_FORMAT_TELEMETRY 3 /* 32 byte ao_telemetry records */ +#define AO_LOG_FORMAT_TELESCIENCE 4 /* 32 byte typed telescience records */ +#define AO_LOG_FORMAT_TELEMEGA 5 /* 32 byte typed telemega records */ +#define AO_LOG_FORMAT_EASYMINI 6 /* 16-byte MS5607 baro only, 3.0V supply */ +#define AO_LOG_FORMAT_TELEMETRUM 7 /* 16-byte typed telemetrum records */ +#define AO_LOG_FORMAT_TELEMINI 8 /* 16-byte MS5607 baro only, 3.3V supply */ +#define AO_LOG_FORMAT_NONE 127 /* No log at all */ + +extern __code uint8_t ao_log_format; + +/* Return the flight number from the given log slot, 0 if none */ +uint16_t +ao_log_flight(uint8_t slot); + +/* Flush the log */ +void +ao_log_flush(void); + +/* Logging thread main routine */ +void +ao_log(void); + +/* functions provided in ao_log.c */ + +/* Figure out the current flight number */ +void +ao_log_scan(void) __reentrant; + +/* Return the position of the start of the given log slot */ +uint32_t +ao_log_pos(uint8_t slot); + +/* Start logging to eeprom */ +void +ao_log_start(void); + +/* Stop logging */ +void +ao_log_stop(void); + +/* Initialize the logging system */ +void +ao_log_init(void); + +/* Write out the current flight number to the erase log */ +void +ao_log_write_erase(uint8_t pos); + +/* Returns true if there are any logs stored in eeprom */ +uint8_t +ao_log_present(void); + +/* Returns true if there is no more storage space available */ +uint8_t +ao_log_full(void); + +/* + * ao_log_big.c + */ + +/* + * The data log is recorded in the eeprom as a sequence + * of data packets. + * + * Each packet starts with a 4-byte header that has the + * packet type, the packet checksum and the tick count. Then + * they all contain 2 16 bit values which hold packet-specific + * data. + * + * For each flight, the first packet + * is FLIGHT packet, indicating the serial number of the + * device and a unique number marking the number of flights + * recorded by this device. + * + * During flight, data from the accelerometer and barometer + * are recorded in SENSOR packets, using the raw 16-bit values + * read from the A/D converter. + * + * Also during flight, but at a lower rate, the deployment + * sensors are recorded in DEPLOY packets. The goal here is to + * detect failure in the deployment circuits. + * + * STATE packets hold state transitions as the flight computer + * transitions through different stages of the flight. + */ +#define AO_LOG_FLIGHT 'F' +#define AO_LOG_SENSOR 'A' +#define AO_LOG_TEMP_VOLT 'T' +#define AO_LOG_DEPLOY 'D' +#define AO_LOG_STATE 'S' +#define AO_LOG_GPS_TIME 'G' +#define AO_LOG_GPS_LAT 'N' +#define AO_LOG_GPS_LON 'W' +#define AO_LOG_GPS_ALT 'H' +#define AO_LOG_GPS_SAT 'V' +#define AO_LOG_GPS_DATE 'Y' +#define AO_LOG_GPS_POS 'P' + +#define AO_LOG_POS_NONE (~0UL) + +struct ao_log_record { + char type; /* 0 */ + uint8_t csum; /* 1 */ + uint16_t tick; /* 2 */ + union { + struct { + int16_t ground_accel; /* 4 */ + uint16_t flight; /* 6 */ + } flight; + struct { + int16_t accel; /* 4 */ + int16_t pres; /* 6 */ + } sensor; + struct { + int16_t temp; + int16_t v_batt; + } temp_volt; + struct { + int16_t drogue; + int16_t main; + } deploy; + struct { + uint16_t state; + uint16_t reason; + } state; + struct { + uint8_t hour; + uint8_t minute; + uint8_t second; + uint8_t flags; + } gps_time; + int32_t gps_latitude; + int32_t gps_longitude; + struct { + int16_t altitude; + uint16_t unused; + } gps_altitude; + struct { + uint16_t svid; + uint8_t unused; + uint8_t c_n; + } gps_sat; + struct { + uint8_t year; + uint8_t month; + uint8_t day; + uint8_t extra; + } gps_date; + struct { + uint16_t d0; + uint16_t d1; + } anon; + } u; +}; + +struct ao_log_mega { + char type; /* 0 */ + uint8_t csum; /* 1 */ + uint16_t tick; /* 2 */ + union { /* 4 */ + /* AO_LOG_FLIGHT */ + struct { + uint16_t flight; /* 4 */ + int16_t ground_accel; /* 6 */ + uint32_t ground_pres; /* 8 */ + int16_t ground_accel_along; /* 16 */ + int16_t ground_accel_across; /* 12 */ + int16_t ground_accel_through; /* 14 */ + int16_t ground_roll; /* 18 */ + int16_t ground_pitch; /* 20 */ + int16_t ground_yaw; /* 22 */ + } flight; /* 24 */ + /* AO_LOG_STATE */ + struct { + uint16_t state; + uint16_t reason; + } state; + /* AO_LOG_SENSOR */ + struct { + uint32_t pres; /* 4 */ + uint32_t temp; /* 8 */ + int16_t accel_x; /* 12 */ + int16_t accel_y; /* 14 */ + int16_t accel_z; /* 16 */ + int16_t gyro_x; /* 18 */ + int16_t gyro_y; /* 20 */ + int16_t gyro_z; /* 22 */ + int16_t mag_x; /* 24 */ + int16_t mag_y; /* 26 */ + int16_t mag_z; /* 28 */ + int16_t accel; /* 30 */ + } sensor; /* 32 */ + /* AO_LOG_TEMP_VOLT */ + struct { + int16_t v_batt; /* 4 */ + int16_t v_pbatt; /* 6 */ + int16_t n_sense; /* 8 */ + int16_t sense[10]; /* 10 */ + uint16_t pyro; /* 30 */ + } volt; /* 32 */ + /* AO_LOG_GPS_TIME */ + struct { + int32_t latitude; /* 4 */ + int32_t longitude; /* 8 */ + int16_t altitude; /* 12 */ + uint8_t hour; /* 14 */ + uint8_t minute; /* 15 */ + uint8_t second; /* 16 */ + uint8_t flags; /* 17 */ + uint8_t year; /* 18 */ + uint8_t month; /* 19 */ + uint8_t day; /* 20 */ + uint8_t course; /* 21 */ + uint16_t ground_speed; /* 22 */ + int16_t climb_rate; /* 24 */ + uint8_t pdop; /* 26 */ + uint8_t hdop; /* 27 */ + uint8_t vdop; /* 28 */ + uint8_t mode; /* 29 */ + } gps; /* 30 */ + /* AO_LOG_GPS_SAT */ + struct { + uint16_t channels; /* 4 */ + struct { + uint8_t svid; + uint8_t c_n; + } sats[12]; /* 6 */ + } gps_sat; /* 30 */ + } u; +}; + +struct ao_log_metrum { + char type; /* 0 */ + uint8_t csum; /* 1 */ + uint16_t tick; /* 2 */ + union { /* 4 */ + /* AO_LOG_FLIGHT */ + struct { + uint16_t flight; /* 4 */ + int16_t ground_accel; /* 6 */ + uint32_t ground_pres; /* 8 */ + uint32_t ground_temp; /* 12 */ + } flight; /* 16 */ + /* AO_LOG_STATE */ + struct { + uint16_t state; /* 4 */ + uint16_t reason; /* 6 */ + } state; /* 8 */ + /* AO_LOG_SENSOR */ + struct { + uint32_t pres; /* 4 */ + uint32_t temp; /* 8 */ + int16_t accel; /* 12 */ + } sensor; /* 14 */ + /* AO_LOG_TEMP_VOLT */ + struct { + int16_t v_batt; /* 4 */ + int16_t sense_a; /* 6 */ + int16_t sense_m; /* 8 */ + } volt; /* 10 */ + /* AO_LOG_GPS_POS */ + struct { + int32_t latitude; /* 4 */ + int32_t longitude; /* 8 */ + int16_t altitude; /* 12 */ + } gps; /* 14 */ + /* AO_LOG_GPS_TIME */ + struct { + uint8_t hour; /* 4 */ + uint8_t minute; /* 5 */ + uint8_t second; /* 6 */ + uint8_t flags; /* 7 */ + uint8_t year; /* 8 */ + uint8_t month; /* 9 */ + uint8_t day; /* 10 */ + uint8_t pad; /* 11 */ + } gps_time; /* 12 */ + /* AO_LOG_GPS_SAT (up to three packets) */ + struct { + uint8_t channels; /* 4 */ + uint8_t more; /* 5 */ + struct { + uint8_t svid; + uint8_t c_n; + } sats[4]; /* 6 */ + } gps_sat; /* 14 */ + uint8_t raw[12]; /* 4 */ + } u; /* 16 */ +}; + +struct ao_log_mini { + char type; /* 0 */ + uint8_t csum; /* 1 */ + uint16_t tick; /* 2 */ + union { /* 4 */ + /* AO_LOG_FLIGHT */ + struct { + uint16_t flight; /* 4 */ + uint16_t r6; + uint32_t ground_pres; /* 8 */ + } flight; + /* AO_LOG_STATE */ + struct { + uint16_t state; /* 4 */ + uint16_t reason; /* 6 */ + } state; + /* AO_LOG_SENSOR */ + struct { + uint8_t pres[3]; /* 4 */ + uint8_t temp[3]; /* 7 */ + int16_t sense_a; /* 10 */ + int16_t sense_m; /* 12 */ + int16_t v_batt; /* 14 */ + } sensor; /* 16 */ + } u; /* 16 */ +}; /* 16 */ + +#define ao_log_pack24(dst,value) do { \ + (dst)[0] = (value); \ + (dst)[1] = (value) >> 8; \ + (dst)[2] = (value) >> 16; \ + } while (0) + +/* Write a record to the eeprom log */ +uint8_t +ao_log_data(__xdata struct ao_log_record *log) __reentrant; + +uint8_t +ao_log_mega(__xdata struct ao_log_mega *log) __reentrant; + +uint8_t +ao_log_metrum(__xdata struct ao_log_metrum *log) __reentrant; + +uint8_t +ao_log_mini(__xdata struct ao_log_mini *log) __reentrant; + +void +ao_log_flush(void); + +void +ao_gps_report_metrum_init(void); + +#endif /* _AO_LOG_H_ */ diff --git a/src/kernel/ao_log_big.c b/src/kernel/ao_log_big.c new file mode 100644 index 00000000..db01f46c --- /dev/null +++ b/src/kernel/ao_log_big.c @@ -0,0 +1,162 @@ +/* + * 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" + +static __xdata uint8_t ao_log_mutex; +static __xdata struct ao_log_record log; + +__code uint8_t ao_log_format = AO_LOG_FORMAT_FULL; + +static uint8_t +ao_log_csum(__xdata uint8_t *b) __reentrant +{ + uint8_t sum = 0x5a; + uint8_t i; + + for (i = 0; i < sizeof (struct ao_log_record); i++) + sum += *b++; + return -sum; +} + +uint8_t +ao_log_data(__xdata struct ao_log_record *log) __reentrant +{ + uint8_t wrote = 0; + /* set checksum */ + log->csum = 0; + log->csum = ao_log_csum((__xdata uint8_t *) log); + ao_mutex_get(&ao_log_mutex); { + if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) + ao_log_stop(); + if (ao_log_running) { + wrote = 1; + ao_storage_write(ao_log_current_pos, + log, + sizeof (struct ao_log_record)); + ao_log_current_pos += sizeof (struct ao_log_record); + } + } ao_mutex_put(&ao_log_mutex); + return wrote; +} + +static uint8_t +ao_log_dump_check_data(void) +{ + if (ao_log_csum((uint8_t *) &log) != 0) + return 0; + return 1; +} + +static __data uint8_t ao_log_data_pos; + +/* a hack to make sure that ao_log_records fill the eeprom block in even units */ +typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_record))] ; + +#ifndef AO_SENSOR_INTERVAL_ASCENT +#define AO_SENSOR_INTERVAL_ASCENT 1 +#define AO_SENSOR_INTERVAL_DESCENT 10 +#define AO_OTHER_INTERVAL 32 +#endif + +void +ao_log(void) +{ + __pdata uint16_t next_sensor, next_other; + + ao_storage_setup(); + + ao_log_scan(); + + while (!ao_log_running) + ao_sleep(&ao_log_running); + + log.type = AO_LOG_FLIGHT; + log.tick = ao_sample_tick; +#if HAS_ACCEL + log.u.flight.ground_accel = ao_ground_accel; +#endif + log.u.flight.flight = ao_flight_number; + ao_log_data(&log); + + /* Write the whole contents of the ring to the log + * when starting up. + */ + ao_log_data_pos = ao_data_ring_next(ao_sample_data); + next_other = next_sensor = ao_data_ring[ao_log_data_pos].tick; + ao_log_state = ao_flight_startup; + for (;;) { + /* Write samples to EEPROM */ + while (ao_log_data_pos != ao_sample_data) { + log.tick = ao_data_ring[ao_log_data_pos].tick; + if ((int16_t) (log.tick - next_sensor) >= 0) { + log.type = AO_LOG_SENSOR; + log.u.sensor.accel = ao_data_ring[ao_log_data_pos].adc.accel; + log.u.sensor.pres = ao_data_ring[ao_log_data_pos].adc.pres; + ao_log_data(&log); + if (ao_log_state <= ao_flight_coast) + next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT; + else + next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT; + } + if ((int16_t) (log.tick - next_other) >= 0) { + log.type = AO_LOG_TEMP_VOLT; + log.u.temp_volt.temp = ao_data_ring[ao_log_data_pos].adc.temp; + log.u.temp_volt.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; + ao_log_data(&log); + log.type = AO_LOG_DEPLOY; + log.u.deploy.drogue = ao_data_ring[ao_log_data_pos].adc.sense_d; + log.u.deploy.main = ao_data_ring[ao_log_data_pos].adc.sense_m; + ao_log_data(&log); + next_other = log.tick + AO_OTHER_INTERVAL; + } + ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); + } + /* Write state change to EEPROM */ + if (ao_flight_state != ao_log_state) { + ao_log_state = ao_flight_state; + log.type = AO_LOG_STATE; + log.tick = ao_sample_tick; + log.u.state.state = ao_log_state; + log.u.state.reason = 0; + ao_log_data(&log); + + if (ao_log_state == ao_flight_landed) + ao_log_stop(); + } + + /* Wait for a while */ + ao_delay(AO_MS_TO_TICKS(100)); + + /* Stop logging when told to */ + while (!ao_log_running) + ao_sleep(&ao_log_running); + } +} + +uint16_t +ao_log_flight(uint8_t slot) +{ + if (!ao_storage_read(ao_log_pos(slot), + &log, + sizeof (struct ao_log_record))) + return 0; + + if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) + return log.u.flight.flight; + return 0; +} diff --git a/src/kernel/ao_log_mega.c b/src/kernel/ao_log_mega.c new file mode 100644 index 00000000..768947d5 --- /dev/null +++ b/src/kernel/ao_log_mega.c @@ -0,0 +1,199 @@ +/* + * 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 "ao.h" +#include +#include +#include + +static __xdata uint8_t ao_log_mutex; +static __xdata struct ao_log_mega log; + +__code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMEGA; + +static uint8_t +ao_log_csum(__xdata uint8_t *b) __reentrant +{ + uint8_t sum = 0x5a; + uint8_t i; + + for (i = 0; i < sizeof (struct ao_log_mega); i++) + sum += *b++; + return -sum; +} + +uint8_t +ao_log_mega(__xdata struct ao_log_mega *log) __reentrant +{ + uint8_t wrote = 0; + /* set checksum */ + log->csum = 0; + log->csum = ao_log_csum((__xdata uint8_t *) log); + ao_mutex_get(&ao_log_mutex); { + if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) + ao_log_stop(); + if (ao_log_running) { + wrote = 1; + ao_storage_write(ao_log_current_pos, + log, + sizeof (struct ao_log_mega)); + ao_log_current_pos += sizeof (struct ao_log_mega); + } + } ao_mutex_put(&ao_log_mutex); + return wrote; +} + +static uint8_t +ao_log_dump_check_data(void) +{ + if (ao_log_csum((uint8_t *) &log) != 0) + return 0; + return 1; +} + +#if HAS_ADC +static __data uint8_t ao_log_data_pos; + +/* a hack to make sure that ao_log_megas fill the eeprom block in even units */ +typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_mega))] ; + +#ifndef AO_SENSOR_INTERVAL_ASCENT +#define AO_SENSOR_INTERVAL_ASCENT 1 +#define AO_SENSOR_INTERVAL_DESCENT 10 +#define AO_OTHER_INTERVAL 32 +#endif + +void +ao_log(void) +{ + __pdata uint16_t next_sensor, next_other; + uint8_t i; + + ao_storage_setup(); + + ao_log_scan(); + + while (!ao_log_running) + ao_sleep(&ao_log_running); + +#if HAS_FLIGHT + log.type = AO_LOG_FLIGHT; + log.tick = ao_sample_tick; +#if HAS_ACCEL + log.u.flight.ground_accel = ao_ground_accel; +#endif +#if HAS_GYRO + log.u.flight.ground_accel_along = ao_ground_accel_along; + log.u.flight.ground_accel_across = ao_ground_accel_across; + log.u.flight.ground_accel_through = ao_ground_accel_through; + log.u.flight.ground_pitch = ao_ground_pitch; + log.u.flight.ground_yaw = ao_ground_yaw; + log.u.flight.ground_roll = ao_ground_roll; +#endif + log.u.flight.ground_pres = ao_ground_pres; + log.u.flight.flight = ao_flight_number; + ao_log_mega(&log); +#endif + + /* Write the whole contents of the ring to the log + * when starting up. + */ + ao_log_data_pos = ao_data_ring_next(ao_data_head); + next_other = next_sensor = ao_data_ring[ao_log_data_pos].tick; + ao_log_state = ao_flight_startup; + for (;;) { + /* Write samples to EEPROM */ + while (ao_log_data_pos != ao_data_head) { + log.tick = ao_data_ring[ao_log_data_pos].tick; + if ((int16_t) (log.tick - next_sensor) >= 0) { + log.type = AO_LOG_SENSOR; +#if HAS_MS5607 + log.u.sensor.pres = ao_data_ring[ao_log_data_pos].ms5607_raw.pres; + log.u.sensor.temp = ao_data_ring[ao_log_data_pos].ms5607_raw.temp; +#endif +#if HAS_MPU6000 + log.u.sensor.accel_x = ao_data_ring[ao_log_data_pos].mpu6000.accel_x; + log.u.sensor.accel_y = ao_data_ring[ao_log_data_pos].mpu6000.accel_y; + log.u.sensor.accel_z = ao_data_ring[ao_log_data_pos].mpu6000.accel_z; + log.u.sensor.gyro_x = ao_data_ring[ao_log_data_pos].mpu6000.gyro_x; + log.u.sensor.gyro_y = ao_data_ring[ao_log_data_pos].mpu6000.gyro_y; + log.u.sensor.gyro_z = ao_data_ring[ao_log_data_pos].mpu6000.gyro_z; +#endif +#if HAS_HMC5883 + log.u.sensor.mag_x = ao_data_ring[ao_log_data_pos].hmc5883.x; + log.u.sensor.mag_y = ao_data_ring[ao_log_data_pos].hmc5883.y; + log.u.sensor.mag_z = ao_data_ring[ao_log_data_pos].hmc5883.z; +#endif + log.u.sensor.accel = ao_data_accel(&ao_data_ring[ao_log_data_pos]); + ao_log_mega(&log); + if (ao_log_state <= ao_flight_coast) + next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT; + else + next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT; + } + if ((int16_t) (log.tick - next_other) >= 0) { + log.type = AO_LOG_TEMP_VOLT; + log.u.volt.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; + log.u.volt.v_pbatt = ao_data_ring[ao_log_data_pos].adc.v_pbatt; + log.u.volt.n_sense = AO_ADC_NUM_SENSE; + for (i = 0; i < AO_ADC_NUM_SENSE; i++) + log.u.volt.sense[i] = ao_data_ring[ao_log_data_pos].adc.sense[i]; + log.u.volt.pyro = ao_pyro_fired; + ao_log_mega(&log); + next_other = log.tick + AO_OTHER_INTERVAL; + } + ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); + } +#if HAS_FLIGHT + /* Write state change to EEPROM */ + if (ao_flight_state != ao_log_state) { + ao_log_state = ao_flight_state; + log.type = AO_LOG_STATE; + log.tick = ao_time(); + log.u.state.state = ao_log_state; + log.u.state.reason = 0; + ao_log_mega(&log); + + if (ao_log_state == ao_flight_landed) + ao_log_stop(); + } +#endif + + ao_log_flush(); + + /* Wait for a while */ + ao_delay(AO_MS_TO_TICKS(100)); + + /* Stop logging when told to */ + while (!ao_log_running) + ao_sleep(&ao_log_running); + } +} +#endif + +uint16_t +ao_log_flight(uint8_t slot) +{ + if (!ao_storage_read(ao_log_pos(slot), + &log, + sizeof (struct ao_log_mega))) + return 0; + + if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) + return log.u.flight.flight; + return 0; +} diff --git a/src/kernel/ao_log_metrum.c b/src/kernel/ao_log_metrum.c new file mode 100644 index 00000000..9b17adc2 --- /dev/null +++ b/src/kernel/ao_log_metrum.c @@ -0,0 +1,176 @@ +/* + * 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 "ao.h" +#include +#include +#include + +static __xdata uint8_t ao_log_mutex; +static __xdata struct ao_log_metrum log; + +__code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMETRUM; + +static uint8_t +ao_log_csum(__xdata uint8_t *b) __reentrant +{ + uint8_t sum = 0x5a; + uint8_t i; + + for (i = 0; i < sizeof (struct ao_log_metrum); i++) + sum += *b++; + return -sum; +} + +uint8_t +ao_log_metrum(__xdata struct ao_log_metrum *log) __reentrant +{ + uint8_t wrote = 0; + /* set checksum */ + log->csum = 0; + log->csum = ao_log_csum((__xdata uint8_t *) log); + ao_mutex_get(&ao_log_mutex); { + if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) + ao_log_stop(); + if (ao_log_running) { + wrote = 1; + ao_storage_write(ao_log_current_pos, + log, + sizeof (struct ao_log_metrum)); + ao_log_current_pos += sizeof (struct ao_log_metrum); + } + } ao_mutex_put(&ao_log_mutex); + return wrote; +} + +static uint8_t +ao_log_dump_check_data(void) +{ + if (ao_log_csum((uint8_t *) &log) != 0) + return 0; + return 1; +} + +#if HAS_ADC +static __data uint8_t ao_log_data_pos; + +/* a hack to make sure that ao_log_metrums fill the eeprom block in even units */ +typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_metrum))] ; + +#ifndef AO_SENSOR_INTERVAL_ASCENT +#define AO_SENSOR_INTERVAL_ASCENT 1 +#define AO_SENSOR_INTERVAL_DESCENT 10 +#define AO_OTHER_INTERVAL 32 +#endif + +void +ao_log(void) +{ + __pdata uint16_t next_sensor, next_other; + + ao_storage_setup(); + + ao_log_scan(); + + while (!ao_log_running) + ao_sleep(&ao_log_running); + +#if HAS_FLIGHT + log.type = AO_LOG_FLIGHT; + log.tick = ao_sample_tick; +#if HAS_ACCEL + log.u.flight.ground_accel = ao_ground_accel; +#endif + log.u.flight.ground_pres = ao_ground_pres; + log.u.flight.flight = ao_flight_number; + ao_log_metrum(&log); +#endif + + /* Write the whole contents of the ring to the log + * when starting up. + */ + ao_log_data_pos = ao_data_ring_next(ao_data_head); + next_other = next_sensor = ao_data_ring[ao_log_data_pos].tick; + ao_log_state = ao_flight_startup; + for (;;) { + /* Write samples to EEPROM */ + while (ao_log_data_pos != ao_data_head) { + log.tick = ao_data_ring[ao_log_data_pos].tick; + if ((int16_t) (log.tick - next_sensor) >= 0) { + log.type = AO_LOG_SENSOR; +#if HAS_MS5607 + log.u.sensor.pres = ao_data_ring[ao_log_data_pos].ms5607_raw.pres; + log.u.sensor.temp = ao_data_ring[ao_log_data_pos].ms5607_raw.temp; +#endif +#if HAS_ACCEL + log.u.sensor.accel = ao_data_accel(&ao_data_ring[ao_log_data_pos]); +#endif + ao_log_metrum(&log); + if (ao_log_state <= ao_flight_coast) + next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT; + else + next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT; + } + if ((int16_t) (log.tick - next_other) >= 0) { + log.type = AO_LOG_TEMP_VOLT; + log.u.volt.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; + log.u.volt.sense_a = ao_data_ring[ao_log_data_pos].adc.sense_a; + log.u.volt.sense_m = ao_data_ring[ao_log_data_pos].adc.sense_m; + ao_log_metrum(&log); + next_other = log.tick + AO_OTHER_INTERVAL; + } + ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); + } +#if HAS_FLIGHT + /* Write state change to EEPROM */ + if (ao_flight_state != ao_log_state) { + ao_log_state = ao_flight_state; + log.type = AO_LOG_STATE; + log.tick = ao_time(); + log.u.state.state = ao_log_state; + log.u.state.reason = 0; + ao_log_metrum(&log); + + if (ao_log_state == ao_flight_landed) + ao_log_stop(); + } +#endif + + ao_log_flush(); + + /* Wait for a while */ + ao_delay(AO_MS_TO_TICKS(100)); + + /* Stop logging when told to */ + while (!ao_log_running) + ao_sleep(&ao_log_running); + } +} +#endif + +uint16_t +ao_log_flight(uint8_t slot) +{ + if (!ao_storage_read(ao_log_pos(slot), + &log, + sizeof (struct ao_log_metrum))) + return 0; + + if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) + return log.u.flight.flight; + return 0; +} diff --git a/src/kernel/ao_log_micro.c b/src/kernel/ao_log_micro.c new file mode 100644 index 00000000..d665efb5 --- /dev/null +++ b/src/kernel/ao_log_micro.c @@ -0,0 +1,121 @@ +/* + * 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 + +static uint16_t ao_log_offset = STARTING_LOG_OFFSET; + +void +ao_log_micro_save(void) +{ + uint16_t n_samples = (ao_log_offset - STARTING_LOG_OFFSET) / sizeof (uint16_t); + ao_eeprom_write(PA_GROUND_OFFSET, &pa_ground, sizeof (pa_ground)); + ao_eeprom_write(PA_MIN_OFFSET, &pa_min, sizeof (pa_min)); + ao_eeprom_write(N_SAMPLES_OFFSET, &n_samples, sizeof (n_samples)); +} + +void +ao_log_micro_restore(void) +{ + ao_eeprom_read(PA_GROUND_OFFSET, &pa_ground, sizeof (pa_ground)); + ao_eeprom_read(PA_MIN_OFFSET, &pa_min, sizeof (pa_min)); +} + +void +ao_log_micro_data(void) +{ + uint16_t low_bits = pa; + + if (ao_log_offset < MAX_LOG_OFFSET) { + ao_eeprom_write(ao_log_offset, &low_bits, sizeof (low_bits)); + ao_log_offset += sizeof (low_bits); + } +} + +#define POLY 0x8408 + +static uint16_t +ao_log_micro_crc(uint16_t crc, uint8_t byte) +{ + uint8_t i; + + for (i = 0; i < 8; i++) { + if ((crc & 0x0001) ^ (byte & 0x0001)) + crc = (crc >> 1) ^ POLY; + else + crc = crc >> 1; + byte >>= 1; + } + return crc; +} + +static void +ao_log_hex_nibble(uint8_t b) +{ + if (b < 10) + ao_async_byte('0' + b); + else + ao_async_byte('a' - 10 + b); +} + +static void +ao_log_hex(uint8_t b) +{ + ao_log_hex_nibble(b>>4); + ao_log_hex_nibble(b&0xf); +} + +static void +ao_log_newline(void) +{ + ao_async_byte('\r'); + ao_async_byte('\n'); +} + +void +ao_log_micro_dump(void) +{ + uint16_t n_samples; + uint16_t nbytes; + uint8_t byte; + uint16_t b; + uint16_t crc = 0xffff; + + ao_eeprom_read(N_SAMPLES_OFFSET, &n_samples, sizeof (n_samples)); + if (n_samples == 0xffff) + n_samples = 0; + nbytes = STARTING_LOG_OFFSET + sizeof (uint16_t) * n_samples; + ao_async_start(); + ao_async_byte('M'); + ao_async_byte('P'); + for (b = 0; b < nbytes; b++) { + if ((b & 0xf) == 0) + ao_log_newline(); + ao_eeprom_read(b, &byte, 1); + ao_log_hex(byte); + crc = ao_log_micro_crc(crc, byte); + } + ao_log_newline(); + crc = ~crc; + ao_log_hex(crc >> 8); + ao_log_hex(crc); + ao_log_newline(); + ao_async_stop(); +} diff --git a/src/kernel/ao_log_micro.h b/src/kernel/ao_log_micro.h new file mode 100644 index 00000000..976852ee --- /dev/null +++ b/src/kernel/ao_log_micro.h @@ -0,0 +1,39 @@ +/* + * 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. + */ + +#ifndef _AO_LOG_MICRO_H_ +#define _AO_LOG_MICRO_H_ + +#define PA_GROUND_OFFSET 0 +#define PA_MIN_OFFSET 4 +#define N_SAMPLES_OFFSET 8 +#define STARTING_LOG_OFFSET 10 +#define MAX_LOG_OFFSET 512 + +void +ao_log_micro_save(void); + +void +ao_log_micro_restore(void); + +void +ao_log_micro_data(void); + +void +ao_log_micro_dump(void); + +#endif /* _AO_LOG_MICRO_H_ */ diff --git a/src/kernel/ao_log_mini.c b/src/kernel/ao_log_mini.c new file mode 100644 index 00000000..29e3bd9f --- /dev/null +++ b/src/kernel/ao_log_mini.c @@ -0,0 +1,162 @@ +/* + * 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 "ao.h" +#include +#include +#include + +static __xdata uint8_t ao_log_mutex; +static __xdata struct ao_log_mini log; + +__code uint8_t ao_log_format = AO_LOG_FORMAT; + +static uint8_t +ao_log_csum(__xdata uint8_t *b) __reentrant +{ + uint8_t sum = 0x5a; + uint8_t i; + + for (i = 0; i < sizeof (struct ao_log_mini); i++) + sum += *b++; + return -sum; +} + +uint8_t +ao_log_mini(__xdata struct ao_log_mini *log) __reentrant +{ + uint8_t wrote = 0; + /* set checksum */ + log->csum = 0; + log->csum = ao_log_csum((__xdata uint8_t *) log); + ao_mutex_get(&ao_log_mutex); { + if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) + ao_log_stop(); + if (ao_log_running) { + wrote = 1; + ao_storage_write(ao_log_current_pos, + log, + sizeof (struct ao_log_mini)); + ao_log_current_pos += sizeof (struct ao_log_mini); + } + } ao_mutex_put(&ao_log_mutex); + return wrote; +} + +static uint8_t +ao_log_dump_check_data(void) +{ + if (ao_log_csum((uint8_t *) &log) != 0) + return 0; + return 1; +} + +static __data uint8_t ao_log_data_pos; + +/* a hack to make sure that ao_log_minis fill the eeprom block in even units */ +typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_mini))] ; + +#ifndef AO_SENSOR_INTERVAL_ASCENT +#define AO_SENSOR_INTERVAL_ASCENT 1 +#define AO_SENSOR_INTERVAL_DESCENT 10 +#endif + +void +ao_log(void) +{ + __pdata uint16_t next_sensor; + + ao_storage_setup(); + + ao_log_scan(); + + while (!ao_log_running) + ao_sleep(&ao_log_running); + +#if HAS_FLIGHT + log.type = AO_LOG_FLIGHT; + log.tick = ao_sample_tick; + log.u.flight.flight = ao_flight_number; + log.u.flight.ground_pres = ao_ground_pres; + ao_log_mini(&log); +#endif + + /* Write the whole contents of the ring to the log + * when starting up. + */ + ao_log_data_pos = ao_data_ring_next(ao_data_head); + next_sensor = ao_data_ring[ao_log_data_pos].tick; + ao_log_state = ao_flight_startup; + for (;;) { + /* Write samples to EEPROM */ + while (ao_log_data_pos != ao_data_head) { + log.tick = ao_data_ring[ao_log_data_pos].tick; + if ((int16_t) (log.tick - next_sensor) >= 0) { + log.type = AO_LOG_SENSOR; + ao_log_pack24(log.u.sensor.pres, + ao_data_ring[ao_log_data_pos].ms5607_raw.pres); + ao_log_pack24(log.u.sensor.temp, + ao_data_ring[ao_log_data_pos].ms5607_raw.temp); + log.u.sensor.sense_a = ao_data_ring[ao_log_data_pos].adc.sense_a; + log.u.sensor.sense_m = ao_data_ring[ao_log_data_pos].adc.sense_m; + log.u.sensor.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; + ao_log_mini(&log); + if (ao_log_state <= ao_flight_coast) + next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT; + else + next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT; + } + ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); + } +#if HAS_FLIGHT + /* Write state change to EEPROM */ + if (ao_flight_state != ao_log_state) { + ao_log_state = ao_flight_state; + log.type = AO_LOG_STATE; + log.tick = ao_time(); + log.u.state.state = ao_log_state; + log.u.state.reason = 0; + ao_log_mini(&log); + + if (ao_log_state == ao_flight_landed) + ao_log_stop(); + } +#endif + + ao_log_flush(); + + /* Wait for a while */ + ao_delay(AO_MS_TO_TICKS(100)); + + /* Stop logging when told to */ + while (!ao_log_running) + ao_sleep(&ao_log_running); + } +} + +uint16_t +ao_log_flight(uint8_t slot) +{ + if (!ao_storage_read(ao_log_pos(slot), + &log, + sizeof (struct ao_log_mini))) + return 0; + + if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) + return log.u.flight.flight; + return 0; +} diff --git a/src/kernel/ao_log_single.c b/src/kernel/ao_log_single.c new file mode 100644 index 00000000..3f6235a6 --- /dev/null +++ b/src/kernel/ao_log_single.c @@ -0,0 +1,198 @@ +/* + * 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. + */ + +/* + * ao_log_single.c + * + * Stores a sequence of fixed-size (32 byte) chunks + * without splitting memory up into separate flights + */ + +#include "ao.h" +#include "ao_product.h" + +static __xdata struct ao_task ao_log_single_task; + +__xdata uint8_t ao_log_running; +__xdata uint8_t ao_log_mutex; +__pdata uint32_t ao_log_start_pos; +__pdata uint32_t ao_log_end_pos; +__pdata uint32_t ao_log_current_pos; + +__xdata union ao_log_single ao_log_single_write_data; +__xdata union ao_log_single ao_log_single_read_data; + +uint8_t +ao_log_single_write(void) +{ + uint8_t wrote = 0; + + ao_mutex_get(&ao_log_mutex); { + if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) + ao_log_single_stop(); + if (ao_log_running) { + wrote = 1; + ao_storage_write(ao_log_current_pos, + &ao_log_single_write_data, + AO_LOG_SINGLE_SIZE); + ao_log_current_pos += AO_LOG_SINGLE_SIZE; + } + } ao_mutex_put(&ao_log_mutex); + return wrote; +} + +static uint8_t +ao_log_single_valid(void) +{ + __xdata uint8_t *d = ao_log_single_read_data.bytes; + uint8_t i; + for (i = 0; i < AO_LOG_SINGLE_SIZE; i++) + if (*d++ != 0xff) + return 1; + return 0; +} + +uint8_t +ao_log_single_read(uint32_t pos) +{ + if (!ao_storage_read(pos, &ao_log_single_read_data, AO_LOG_SINGLE_SIZE)) + return 0; + return ao_log_single_valid(); +} + +void +ao_log_single_start(void) +{ + if (!ao_log_running) { + ao_log_running = 1; + ao_wakeup(&ao_log_running); + } +} + +void +ao_log_single_stop(void) +{ + if (ao_log_running) { + ao_log_running = 0; + } +} + +void +ao_log_single_restart(void) +{ + /* Find end of data */ + ao_log_end_pos = ao_storage_config; + for (ao_log_current_pos = 0; + ao_log_current_pos < ao_storage_config; + ao_log_current_pos += ao_storage_block) + { + if (!ao_log_single_read(ao_log_current_pos)) + break; + } + if (ao_log_current_pos > 0) { + ao_log_current_pos -= ao_storage_block; + for (; ao_log_current_pos < ao_storage_config; + ao_log_current_pos += sizeof (struct ao_log_telescience)) + { + if (!ao_log_single_read(ao_log_current_pos)) + break; + } + } +} + +void +ao_log_single_set(void) +{ + printf("Logging currently %s\n", ao_log_running ? "on" : "off"); + ao_cmd_hex(); + if (ao_cmd_status == ao_cmd_success) { + if (ao_cmd_lex_i) { + printf("Logging from %ld to %ld\n", ao_log_current_pos, ao_log_end_pos); + ao_log_single_start(); + } else { + printf ("Log stopped at %ld\n", ao_log_current_pos); + ao_log_single_stop(); + } + } + ao_cmd_status = ao_cmd_success; +} + +void +ao_log_single_delete(void) +{ + uint32_t pos; + + ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + if (ao_cmd_lex_i != 1) { + ao_cmd_status = ao_cmd_syntax_error; + printf("No such flight: %d\n", ao_cmd_lex_i); + return; + } + ao_log_single_stop(); + for (pos = 0; pos < ao_storage_config; pos += ao_storage_block) { + if (!ao_log_single_read(pos)) + break; + ao_storage_erase(pos); + } + ao_log_current_pos = ao_log_start_pos = 0; + if (pos == 0) + printf("No such flight: %d\n", ao_cmd_lex_i); + else + printf ("Erased\n"); +} + +uint8_t +ao_log_full(void) +{ + return ao_log_current_pos >= ao_log_end_pos; +} + +uint8_t +ao_log_present(void) +{ + return ao_log_single_read(0); +} + +static void +ao_log_single_query(void) +{ + printf("Logging enabled: %d\n", ao_log_running); + printf("Log start: %ld\n", ao_log_start_pos); + printf("Log cur: %ld\n", ao_log_current_pos); + printf("Log end: %ld\n", ao_log_end_pos); + ao_log_single_extra_query(); +} + +const struct ao_cmds ao_log_single_cmds[] = { + { ao_log_single_set, "L <0 off, 1 on>\0Set logging" }, + { ao_log_single_list, "l\0List stored logs" }, + { ao_log_single_delete, "d 1\0Delete all stored logs" }, + { ao_log_single_query, "q\0Query log status" }, + { 0, NULL }, +}; + +void +ao_log_single_init(void) +{ + ao_log_running = 0; + + ao_cmd_register(&ao_log_single_cmds[0]); + + ao_add_task(&ao_log_single_task, ao_log_single, "log"); +} diff --git a/src/kernel/ao_log_telem.c b/src/kernel/ao_log_telem.c new file mode 100644 index 00000000..095aca37 --- /dev/null +++ b/src/kernel/ao_log_telem.c @@ -0,0 +1,131 @@ +/* + * 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 +#include +#include + +__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; +__xdata int16_t ao_max_height; /* max of ao_height */ +__pdata int16_t sense_d, sense_m; +__pdata uint8_t ao_igniter_present; + +static void +ao_log_telem_track() { + if (ao_monitoring == sizeof (union ao_telemetry_all)) { + switch (ao_log_single_write_data.telemetry.generic.type) { + case AO_TELEMETRY_SENSOR_TELEMETRUM: + case AO_TELEMETRY_SENSOR_TELEMINI: + /* fall through ... */ + case AO_TELEMETRY_SENSOR_TELENANO: + if (ao_log_single_write_data.telemetry.generic.type == AO_TELEMETRY_SENSOR_TELENANO) { + ao_igniter_present = 0; + } else { + sense_d = ao_log_single_write_data.telemetry.sensor.sense_d; + sense_m = ao_log_single_write_data.telemetry.sensor.sense_m; + ao_igniter_present = 1; + } + if (ao_log_single_write_data.telemetry.sensor.height > ao_max_height) { + ao_max_height = ao_log_single_write_data.telemetry.sensor.height; + } + if (ao_log_single_write_data.telemetry.sensor.state != ao_flight_state) { + ao_flight_state = ao_log_single_write_data.telemetry.sensor.state; + if (ao_flight_state == ao_flight_pad) + ao_max_height = 0; + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } + } + } +} + +enum ao_igniter_status +ao_igniter_status(enum ao_igniter igniter) +{ + int16_t value; + + switch (igniter) { + case ao_igniter_drogue: + value = sense_d; + break; + case ao_igniter_main: + value = sense_m; + break; + default: + value = 0; + break; + } + if (value < AO_IGNITER_OPEN) + return ao_igniter_open; + else if (value > AO_IGNITER_CLOSED) + return ao_igniter_ready; + else + return ao_igniter_unknown; +} + +void +ao_log_single(void) +{ + ao_storage_setup(); + + /* This can take a while, so let the rest + * of the system finish booting before we start + */ + ao_delay(AO_SEC_TO_TICKS(2)); + + ao_log_running = 1; + ao_log_single_restart(); + ao_flight_state = ao_flight_startup; + ao_monitor_set(sizeof(struct ao_telemetry_generic)); + + for (;;) { + while (!ao_log_running) + ao_sleep(&ao_log_running); + + ao_log_monitor_pos = ao_monitor_head; + while (ao_log_running) { + /* Write samples to EEPROM */ + while (ao_log_monitor_pos != ao_monitor_head) { + 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(); + } + /* Wait for more telemetry data to arrive */ + ao_sleep(DATA_TO_XDATA(&ao_monitor_head)); + } + } +} + +void +ao_log_single_list(void) +{ + if (ao_log_current_pos != 0) + printf("flight 1 start %x end %x\n", + 0, + (uint16_t) ((ao_log_current_pos + 0xff) >> 8)); + printf ("done\n"); +} + +void +ao_log_single_extra_query(void) +{ +} diff --git a/src/kernel/ao_log_telescience.c b/src/kernel/ao_log_telescience.c new file mode 100644 index 00000000..002a10bd --- /dev/null +++ b/src/kernel/ao_log_telescience.c @@ -0,0 +1,119 @@ +/* + * 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_product.h" +#include "ao_log.h" +#include "ao_companion.h" + +static uint8_t ao_log_data_pos; + +__code uint8_t ao_log_format = AO_LOG_FORMAT_TELESCIENCE; + +static void +ao_log_telescience_csum(void) __reentrant +{ + __xdata uint8_t *b = ao_log_single_write_data.bytes; + uint8_t sum = 0x5a; + uint8_t i; + + ao_log_single_write_data.telescience.csum = 0; + for (i = 0; i < sizeof (struct ao_log_telescience); i++) + sum += *b++; + ao_log_single_write_data.telescience.csum = -sum; +} + +void +ao_log_single(void) +{ + ao_storage_setup(); + + /* This can take a while, so let the rest + * of the system finish booting before we start + */ + ao_delay(AO_SEC_TO_TICKS(10)); + + ao_log_single_restart(); + for (;;) { + while (!ao_log_running) + ao_sleep(&ao_log_running); + + ao_log_start_pos = ao_log_current_pos; + ao_log_single_write_data.telescience.type = AO_LOG_TELESCIENCE_START; + ao_log_single_write_data.telescience.tick = ao_time(); + ao_log_single_write_data.telescience.adc[0] = ao_companion_command.serial; + ao_log_single_write_data.telescience.adc[1] = ao_companion_command.flight; + ao_log_telescience_csum(); + ao_log_single_write(); + /* Write the whole contents of the ring to the log + * when starting up. + */ + ao_log_data_pos = ao_data_ring_next(ao_data_head); + ao_log_single_write_data.telescience.type = AO_LOG_TELESCIENCE_DATA; + while (ao_log_running) { + /* Write samples to EEPROM */ + while (ao_log_data_pos != ao_data_head) { + ao_log_single_write_data.telescience.tick = ao_data_ring[ao_log_data_pos].tick; + memcpy(&ao_log_single_write_data.telescience.adc, (void *) ao_data_ring[ao_log_data_pos].adc.adc, + AO_LOG_TELESCIENCE_NUM_ADC * sizeof (uint16_t)); + ao_log_telescience_csum(); + ao_log_single_write(); + ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); + } + /* Wait for more ADC data to arrive */ + ao_sleep((void *) &ao_data_head); + } + memset(&ao_log_single_write_data.telescience.adc, '\0', sizeof (ao_log_single_write_data.telescience.adc)); + } +} + +void +ao_log_single_list(void) +{ + uint32_t pos; + uint32_t start = 0; + uint8_t flight = 0; + + for (pos = 0; ; pos += sizeof (struct ao_log_telescience)) { + if (pos >= ao_storage_config || + !ao_log_single_read(pos) || + ao_log_single_read_data.telescience.type == AO_LOG_TELESCIENCE_START) + { + if (pos != start) { + printf("flight %d start %x end %x\n", + flight, + (uint16_t) (start >> 8), + (uint16_t) ((pos + 0xff) >> 8)); flush(); + } + if (ao_log_single_read_data.telescience.type != AO_LOG_TELESCIENCE_START) + break; + start = pos; + flight++; + } + } + printf ("done\n"); +} + +void +ao_log_single_extra_query(void) +{ + printf("log data tick: %04x\n", ao_log_single_write_data.telescience.tick); + printf("TM data tick: %04x\n", ao_log_single_write_data.telescience.tm_tick); + printf("TM state: %d\n", ao_log_single_write_data.telescience.tm_state); + printf("TM serial: %d\n", ao_companion_command.serial); + printf("TM flight: %d\n", ao_companion_command.flight); +} diff --git a/src/kernel/ao_log_tiny.c b/src/kernel/ao_log_tiny.c new file mode 100644 index 00000000..67767dc9 --- /dev/null +++ b/src/kernel/ao_log_tiny.c @@ -0,0 +1,161 @@ +/* + * 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" + +static __data uint16_t ao_log_tiny_interval; + +#define AO_LOG_TINY_INTERVAL_DEFAULT AO_MS_TO_TICKS(1000) +#if USE_FAST_ASCENT_LOG +#define AO_LOG_TINY_INTERVAL_ASCENT AO_MS_TO_TICKS(100) +#define AO_PAD_RING 8 +#else +#define AO_LOG_TINY_INTERVAL_ASCENT AO_LOG_TINY_INTERVAL_DEFAULT +#define AO_PAD_RING 2 +#endif + +__code uint8_t ao_log_format = AO_LOG_FORMAT_TINY; + +void +ao_log_tiny_set_interval(uint16_t ticks) +{ + ao_log_tiny_interval = ticks; +} + + +static void ao_log_tiny_data(uint16_t d) +{ + if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) + ao_log_stop(); + if (ao_log_running) { + ao_storage_write(ao_log_current_pos, DATA_TO_XDATA(&d), 2); + ao_log_current_pos += 2; + } +} + +static __xdata uint16_t ao_log_pad_ring[AO_PAD_RING]; +static __pdata uint8_t ao_log_pad_ring_pos; + +#define ao_pad_ring_next(n) (((n) + 1) & (AO_PAD_RING - 1)) + +static void ao_log_tiny_queue(uint16_t d) +{ + ao_log_pad_ring[ao_log_pad_ring_pos] = d; + ao_log_pad_ring_pos = ao_pad_ring_next(ao_log_pad_ring_pos); +} + +static void ao_log_tiny_start(void) +{ + uint8_t p; + uint16_t d; + + ao_log_tiny_data(ao_flight_number); + ao_log_tiny_data(ao_ground_pres); + p = ao_log_pad_ring_pos; + do { + d = ao_log_pad_ring[p]; + /* + * ignore unwritten slots + */ + if (d) + ao_log_tiny_data(d); + p = ao_pad_ring_next(p); + } while (p != ao_log_pad_ring_pos); +} + +void +ao_log(void) +{ + uint16_t last_time; + uint16_t now; + enum ao_flight_state ao_log_tiny_state; + int32_t sum; + int16_t count; + uint8_t ao_log_data; + uint8_t ao_log_started = 0; + + ao_storage_setup(); + + ao_log_scan(); + + ao_log_tiny_state = ao_flight_invalid; + ao_log_tiny_interval = AO_LOG_TINY_INTERVAL_ASCENT; + sum = 0; + count = 0; + ao_log_data = ao_sample_data; + last_time = ao_time(); + for (;;) { + + /* + * Add in pending sample data + */ + ao_sleep(DATA_TO_XDATA(&ao_sample_data)); + while (ao_log_data != ao_sample_data) { + sum += ao_data_pres(&ao_data_ring[ao_log_data]); + count++; + ao_log_data = ao_data_ring_next(ao_log_data); + } + if (ao_log_running) { + if (!ao_log_started) { + ao_log_tiny_start(); + ao_log_started = 1; + } + if (ao_flight_state != ao_log_tiny_state) { + ao_log_tiny_data(ao_flight_state | 0x8000); + ao_log_tiny_state = ao_flight_state; + ao_log_tiny_interval = AO_LOG_TINY_INTERVAL_DEFAULT; +#if AO_LOG_TINY_INTERVAL_ASCENT != AO_LOG_TINY_INTERVAL_DEFAULT + if (ao_log_tiny_state <= ao_flight_coast) + ao_log_tiny_interval = AO_LOG_TINY_INTERVAL_ASCENT; +#endif + if (ao_log_tiny_state == ao_flight_landed) + ao_log_stop(); + } + } + + /* Stop logging when told to */ + if (!ao_log_running && ao_log_started) + ao_exit(); + + /* + * Write out the sample when finished + */ + now = ao_time(); + if ((int16_t) (now - (last_time + ao_log_tiny_interval)) >= 0 && count) { + count = sum / count; + if (ao_log_started) + ao_log_tiny_data(count); + else + ao_log_tiny_queue(count); + sum = 0; + count = 0; + last_time = now; + } + } +} + +uint16_t +ao_log_flight(uint8_t slot) +{ + static __xdata uint16_t flight; + + (void) slot; + ao_storage_read(0, &flight, 2); + if (flight == 0xffff) + flight = 0; + return flight; +} diff --git a/src/kernel/ao_microflight.c b/src/kernel/ao_microflight.c new file mode 100644 index 00000000..f680e400 --- /dev/null +++ b/src/kernel/ao_microflight.c @@ -0,0 +1,143 @@ +/* + * 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_FLIGHT_TEST +#include +#endif +#include +#include + +uint32_t pa; +uint32_t pa_ground; +uint32_t pa_min; + +static void +ao_microsample(void) +{ + ao_pa_get(); + ao_microkalman_predict(); + ao_microkalman_correct(); +} + +#define NUM_PA_HIST (GROUND_AVG) + +#define SKIP_PA_HIST(i,j) (((i) + (j)) & (NUM_PA_HIST - 1)) + +static uint32_t pa_hist[NUM_PA_HIST]; + +void +ao_microflight(void) +{ + int16_t sample_count; + uint16_t time; + uint32_t pa_interval_min, pa_interval_max; + int32_t pa_diff; + uint8_t h, i; + uint8_t accel_lock = 0; + uint32_t pa_sum = 0; + + /* Wait for motion, averaging values to get ground pressure */ + + time = ao_time(); + ao_pa_get(); + ao_microkalman_init(); + pa_ground = pa; + sample_count = 0; + h = 0; + for (;;) { + time += SAMPLE_SLEEP; + if ((sample_count & 0x1f) == 0) + ao_led_on(AO_LED_REPORT); + ao_delay_until(time); + ao_microsample(); + if ((sample_count & 0x1f) == 0) + ao_led_off(AO_LED_REPORT); + pa_hist[h] = pa; + h = SKIP_PA_HIST(h,1); + pa_diff = pa_ground - ao_pa; + + /* Check for a significant pressure change */ + if (pa_diff > BOOST_DETECT) + break; + + if (sample_count < GROUND_AVG * 2) { + if (sample_count < GROUND_AVG) + pa_sum += pa; + ++sample_count; + } else { + pa_ground = pa_sum >> GROUND_AVG_SHIFT; + pa_sum = 0; + sample_count = 0; + } + } + + /* Go back and find the last sample close to the ground */ + pa_min = pa_ground - LAND_DETECT; + for (i = SKIP_PA_HIST(h,-2); i != SKIP_PA_HIST(h,2); i = SKIP_PA_HIST(i,-2)) { + if (pa_hist[i] >= pa_min) + break; + } + + /* Log the remaining samples so we get a complete history since leaving the ground */ + for (; i != h; i = SKIP_PA_HIST(i,2)) { + pa = pa_hist[i]; + ao_log_micro_data(); + } + + /* Now sit around until the pressure is stable again and record the max */ + + sample_count = 0; + pa_min = ao_pa; + pa_interval_min = ao_pa; + pa_interval_max = ao_pa; + for (;;) { + time += SAMPLE_SLEEP; + ao_delay_until(time); + if ((sample_count & 3) == 0) + ao_led_on(AO_LED_REPORT); + ao_microsample(); + if ((sample_count & 3) == 0) + ao_led_off(AO_LED_REPORT); + if (sample_count & 1) + ao_log_micro_data(); + + /* If accelerating upwards, don't look for min pressure */ + if (ao_pa_accel < ACCEL_LOCK_PA) + accel_lock = ACCEL_LOCK_TIME; + else if (accel_lock) + --accel_lock; + else if (ao_pa < pa_min) + pa_min = ao_pa; + + if (sample_count == (GROUND_AVG - 1)) { + pa_diff = pa_interval_max - pa_interval_min; + + /* Check to see if the pressure is now stable */ + if (pa_diff < LAND_DETECT) + break; + sample_count = 0; + pa_interval_min = ao_pa; + pa_interval_max = ao_pa; + } else { + if (ao_pa < pa_interval_min) + pa_interval_min = ao_pa; + if (ao_pa > pa_interval_max) + pa_interval_max = ao_pa; + ++sample_count; + } + } +} diff --git a/src/kernel/ao_microkalman.c b/src/kernel/ao_microkalman.c new file mode 100644 index 00000000..0684ea2b --- /dev/null +++ b/src/kernel/ao_microkalman.c @@ -0,0 +1,74 @@ +/* + * 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_FLIGHT_TEST +#include +#endif +#include + +#define FIX_BITS 16 + +#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) +#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) +#define from_fix8(x) ((x) >> 8) +#define from_fix(x) ((x) >> 16) +#define fix8_to_fix16(x) ((x) << 8) +#define fix16_to_fix8(x) ((x) >> 8) + +#include + +/* Basic time step (96ms) */ +#define AO_MK_STEP to_fix16(0.096) +/* step ** 2 / 2 */ +#define AO_MK_STEP_2_2 to_fix16(0.004608) + +uint32_t ao_k_pa; /* 24.8 fixed point */ +int32_t ao_k_pa_speed; /* 16.16 fixed point */ +int32_t ao_k_pa_accel; /* 16.16 fixed point */ + +uint32_t ao_pa; /* integer portion */ +int16_t ao_pa_speed; /* integer portion */ +int16_t ao_pa_accel; /* integer portion */ + +void +ao_microkalman_init(void) +{ + ao_pa = pa; + ao_k_pa = pa << 8; +} + +void +ao_microkalman_predict(void) +{ + ao_k_pa += fix16_to_fix8((int32_t) ao_pa_speed * AO_MK_STEP + (int32_t) ao_pa_accel * AO_MK_STEP_2_2); + ao_k_pa_speed += (int32_t) ao_pa_accel * AO_MK_STEP; +} + +void +ao_microkalman_correct(void) +{ + int16_t e; /* Height error in Pa */ + + e = pa - from_fix8(ao_k_pa); + + ao_k_pa += fix16_to_fix8((int32_t) e * AO_MK_BARO_K0_10); + ao_k_pa_speed += (int32_t) e * AO_MK_BARO_K1_10; + ao_k_pa_accel += (int32_t) e * AO_MK_BARO_K2_10; + ao_pa = from_fix8(ao_k_pa); + ao_pa_speed = from_fix(ao_k_pa_speed); + ao_pa_accel = from_fix(ao_k_pa_accel); +} diff --git a/src/kernel/ao_monitor.c b/src/kernel/ao_monitor.c new file mode 100644 index 00000000..18f170b4 --- /dev/null +++ b/src/kernel/ao_monitor.c @@ -0,0 +1,308 @@ +/* + * Copyright © 2009 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_telem.h" +#include "ao_flight.h" + +#if !HAS_MONITOR +#error Must define HAS_MONITOR to 1 +#endif + +#ifndef LEGACY_MONITOR +#error Must define LEGACY_MONITOR +#endif + +#ifndef HAS_MONITOR_PUT +#define HAS_MONITOR_PUT 1 +#endif + +#ifndef AO_MONITOR_LED +#error Must define AO_MONITOR_LED +#endif + +__data uint8_t ao_monitoring; +static __data uint8_t ao_monitor_disabled; +static __data uint8_t ao_internal_monitoring; +static __data uint8_t ao_external_monitoring; + +__xdata union ao_monitor ao_monitor_ring[AO_MONITOR_RING]; + +__data uint8_t ao_monitor_head; + +static void +_ao_monitor_adjust(void) +{ + if (ao_monitoring) + ao_radio_recv_abort(); + if (ao_monitor_disabled) + ao_monitoring = 0; + else { + if (ao_external_monitoring) + ao_monitoring = ao_external_monitoring; + else + ao_monitoring = ao_internal_monitoring; + } + ao_wakeup(DATA_TO_XDATA(&ao_monitoring)); +} + +void +ao_monitor_get(void) +{ + uint8_t size; + + for (;;) { + switch (ao_monitoring) { + case 0: + ao_sleep(DATA_TO_XDATA(&ao_monitoring)); + continue; +#if LEGACY_MONITOR + case AO_MONITORING_ORIG: + size = sizeof (struct ao_telemetry_orig_recv); + break; +#endif + default: + if (ao_monitoring > AO_MAX_TELEMETRY) + ao_monitoring = AO_MAX_TELEMETRY; + size = ao_monitoring; + break; + } + if (!ao_radio_recv(&ao_monitor_ring[ao_monitor_head], size + 2, 0)) + continue; + ao_monitor_head = ao_monitor_ring_next(ao_monitor_head); + ao_wakeup(DATA_TO_XDATA(&ao_monitor_head)); + } +} + +#if AO_MONITOR_LED +__xdata struct ao_task ao_monitor_blink_task; + +void +ao_monitor_blink(void) +{ + for (;;) { + ao_sleep(DATA_TO_XDATA(&ao_monitor_head)); + ao_led_for(AO_MONITOR_LED, AO_MS_TO_TICKS(100)); + } +} +#endif + +#if HAS_MONITOR_PUT + +static const char xdigit[16] = { + '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' +}; + +#define hex(c) do { putchar(xdigit[(c) >> 4]); putchar(xdigit[(c)&0xf]); } while (0) + +void +ao_monitor_put(void) +{ +#if LEGACY_MONITOR + __xdata char callsign[AO_MAX_CALLSIGN+1]; + int16_t rssi; +#endif + uint8_t ao_monitor_tail; + uint8_t state; + uint8_t sum, byte; + __xdata union ao_monitor *m; + +#define recv_raw ((m->raw)) +#define recv_orig ((m->orig)) +#define recv_tiny ((m->tiny)) + + ao_monitor_tail = ao_monitor_head; + for (;;) { + while (!ao_external_monitoring) + ao_sleep(DATA_TO_XDATA(&ao_external_monitoring)); + while (ao_monitor_tail == ao_monitor_head && ao_external_monitoring) + ao_sleep(DATA_TO_XDATA(&ao_monitor_head)); + if (!ao_external_monitoring) + continue; + m = &ao_monitor_ring[ao_monitor_tail]; + ao_monitor_tail = ao_monitor_ring_next(ao_monitor_tail); + switch (ao_monitoring) { + case 0: + break; +#if LEGACY_MONITOR + case AO_MONITORING_ORIG: + state = recv_orig.telemetry_orig.flight_state; + + rssi = (int16_t) AO_RSSI_FROM_RADIO(recv_orig.rssi); + ao_xmemcpy(callsign, recv_orig.telemetry_orig.callsign, AO_MAX_CALLSIGN); + if (state > ao_flight_invalid) + state = ao_flight_invalid; + if (recv_orig.status & PKT_APPEND_STATUS_1_CRC_OK) { + + /* General header fields */ + printf(AO_TELEM_VERSION " %d " + AO_TELEM_CALL " %s " + AO_TELEM_SERIAL " %d " + AO_TELEM_FLIGHT " %d " + AO_TELEM_RSSI " %d " + AO_TELEM_STATE " %s " + AO_TELEM_TICK " %d ", + AO_TELEMETRY_VERSION, + callsign, + recv_orig.telemetry_orig.serial, + recv_orig.telemetry_orig.flight, + rssi, + ao_state_names[state], + recv_orig.telemetry_orig.adc.tick); + + /* Raw sensor values */ + printf(AO_TELEM_RAW_ACCEL " %d " + AO_TELEM_RAW_BARO " %d " + AO_TELEM_RAW_THERMO " %d " + AO_TELEM_RAW_BATT " %d " + AO_TELEM_RAW_DROGUE " %d " + AO_TELEM_RAW_MAIN " %d ", + recv_orig.telemetry_orig.adc.accel, + recv_orig.telemetry_orig.adc.pres, + recv_orig.telemetry_orig.adc.temp, + recv_orig.telemetry_orig.adc.v_batt, + recv_orig.telemetry_orig.adc.sense_d, + recv_orig.telemetry_orig.adc.sense_m); + + /* Sensor calibration values */ + printf(AO_TELEM_CAL_ACCEL_GROUND " %d " + AO_TELEM_CAL_BARO_GROUND " %d " + AO_TELEM_CAL_ACCEL_PLUS " %d " + AO_TELEM_CAL_ACCEL_MINUS " %d ", + recv_orig.telemetry_orig.ground_accel, + recv_orig.telemetry_orig.ground_pres, + recv_orig.telemetry_orig.accel_plus_g, + recv_orig.telemetry_orig.accel_minus_g); + + if (recv_orig.telemetry_orig.u.k.unused == 0x8000) { + /* Kalman state values */ + printf(AO_TELEM_KALMAN_HEIGHT " %d " + AO_TELEM_KALMAN_SPEED " %d " + AO_TELEM_KALMAN_ACCEL " %d ", + recv_orig.telemetry_orig.height, + recv_orig.telemetry_orig.u.k.speed, + recv_orig.telemetry_orig.accel); + } else { + /* Ad-hoc flight values */ + printf(AO_TELEM_ADHOC_ACCEL " %d " + AO_TELEM_ADHOC_SPEED " %ld " + AO_TELEM_ADHOC_BARO " %d ", + recv_orig.telemetry_orig.accel, + recv_orig.telemetry_orig.u.flight_vel, + recv_orig.telemetry_orig.height); + } + ao_gps_print(&recv_orig.telemetry_orig.gps); + ao_gps_tracking_print(&recv_orig.telemetry_orig.gps_tracking); + putchar('\n'); +#if HAS_RSSI + ao_rssi_set(rssi); +#endif + } else { + printf("CRC INVALID RSSI %3d\n", rssi); + } + break; +#endif /* LEGACY_MONITOR */ + default: +#if AO_PROFILE + { + extern uint32_t ao_rx_start_tick, ao_rx_packet_tick, ao_rx_done_tick, ao_rx_last_done_tick; + extern uint32_t ao_fec_decode_start, ao_fec_decode_end; + + printf ("between packet: %d\n", ao_rx_start_tick - ao_rx_last_done_tick); + printf ("receive start delay: %d\n", ao_rx_packet_tick - ao_rx_start_tick); + printf ("decode time: %d\n", ao_fec_decode_end - ao_fec_decode_start); + printf ("rx cleanup: %d\n", ao_rx_done_tick - ao_fec_decode_end); + } +#endif + printf("TELEM "); + hex((uint8_t) (ao_monitoring + 2)); + sum = 0x5a; + for (state = 0; state < ao_monitoring + 2; state++) { + byte = recv_raw.packet[state]; + sum += byte; + hex(byte); + } + hex(sum); + putchar ('\n'); +#if HAS_RSSI + if (recv_raw.packet[ao_monitoring + 1] & PKT_APPEND_STATUS_1_CRC_OK) { + rssi = AO_RSSI_FROM_RADIO(recv_raw.packet[ao_monitoring]); + ao_rssi_set(rssi); + } +#endif + break; + } + ao_usb_flush(); + } +} + +__xdata struct ao_task ao_monitor_put_task; +#endif + +__xdata struct ao_task ao_monitor_get_task; + +void +ao_monitor_set(uint8_t monitoring) +{ + ao_internal_monitoring = monitoring; + _ao_monitor_adjust(); +} + +void +ao_monitor_disable(void) +{ + ++ao_monitor_disabled; + _ao_monitor_adjust(); +} + +void +ao_monitor_enable(void) +{ + --ao_monitor_disabled; + _ao_monitor_adjust(); +} + +#if HAS_MONITOR_PUT +static void +set_monitor(void) +{ + ao_cmd_hex(); + ao_external_monitoring = ao_cmd_lex_i; + ao_wakeup(DATA_TO_XDATA(&ao_external_monitoring)); + ao_wakeup(DATA_TO_XDATA(&ao_monitor_head)); + _ao_monitor_adjust(); +} + +__code struct ao_cmds ao_monitor_cmds[] = { + { set_monitor, "m <0 off, 1 old, 20 std>\0Set radio monitoring" }, + { 0, NULL }, +}; +#endif + +void +ao_monitor_init(void) __reentrant +{ +#if HAS_MONITOR_PUT + ao_cmd_register(&ao_monitor_cmds[0]); + ao_add_task(&ao_monitor_put_task, ao_monitor_put, "monitor_put"); +#endif + ao_add_task(&ao_monitor_get_task, ao_monitor_get, "monitor_get"); +#if AO_MONITOR_LED + ao_add_task(&ao_monitor_blink_task, ao_monitor_blink, "monitor_blink"); +#endif +} diff --git a/src/kernel/ao_mutex.c b/src/kernel/ao_mutex.c new file mode 100644 index 00000000..952ff462 --- /dev/null +++ b/src/kernel/ao_mutex.c @@ -0,0 +1,41 @@ +/* + * Copyright © 2009 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" + +void +ao_mutex_get(__xdata uint8_t *mutex) __reentrant +{ + if (*mutex == ao_cur_task->task_id) + ao_panic(AO_PANIC_MUTEX); + ao_arch_critical( + while (*mutex) + ao_sleep(mutex); + *mutex = ao_cur_task->task_id; + ); +} + +void +ao_mutex_put(__xdata uint8_t *mutex) __reentrant +{ + if (*mutex != ao_cur_task->task_id) + ao_panic(AO_PANIC_MUTEX); + ao_arch_critical( + *mutex = 0; + ao_wakeup(mutex); + ); +} diff --git a/src/kernel/ao_notask.c b/src/kernel/ao_notask.c new file mode 100644 index 00000000..6f967e6d --- /dev/null +++ b/src/kernel/ao_notask.c @@ -0,0 +1,46 @@ +/* + * 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 + +static volatile void *ao_wchan; + +uint8_t +ao_sleep(__xdata void *wchan) +{ +#if 1 + ao_wchan = wchan; + ao_arch_wait_interrupt(); +#else + uint8_t sreg; + + ao_wchan = wchan; + asm("in %0,__SREG__" : "=&r" (sreg)); + sei(); + while (ao_wchan) + ao_arch_cpu_idle(); + asm("out __SREG__,%0" : : "r" (sreg)); +#endif + return 0; +} + +void +ao_wakeup(__xdata void *wchan) +{ + (void) wchan; + ao_wchan = 0; +} diff --git a/src/kernel/ao_notask.h b/src/kernel/ao_notask.h new file mode 100644 index 00000000..6b6b5bb8 --- /dev/null +++ b/src/kernel/ao_notask.h @@ -0,0 +1,27 @@ +/* + * 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. + */ + +#ifndef _AO_NOTASK_H_ +#define _AO_NOTASK_H_ + +uint8_t +ao_sleep(__xdata void *wchan); + +void +ao_wakeup(__xdata void *wchan); + +#endif /* _AO_NOTASK_H_ */ diff --git a/src/kernel/ao_packet.h b/src/kernel/ao_packet.h new file mode 100644 index 00000000..b8426cf9 --- /dev/null +++ b/src/kernel/ao_packet.h @@ -0,0 +1,91 @@ +/* + * 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. + */ + +#ifndef _AO_PACKET_H_ +#define _AO_PACKET_H_ + +/* + * ao_packet.c + * + * Packet-based command interface + */ + +#define AO_PACKET_MAX 64 +#define AO_PACKET_SYN (uint8_t) 0xff + +struct ao_packet { + uint8_t addr; + uint8_t len; + uint8_t seq; + uint8_t ack; + uint8_t d[AO_PACKET_MAX]; + uint8_t callsign[AO_MAX_CALLSIGN]; +}; + +struct ao_packet_recv { + struct ao_packet packet; + int8_t rssi; + uint8_t status; +}; + +extern __xdata struct ao_packet_recv ao_rx_packet; +extern __xdata struct ao_packet ao_tx_packet; +extern __xdata struct ao_task ao_packet_task; +extern __xdata uint8_t ao_packet_enable; +extern __xdata uint8_t ao_packet_master_sleeping; +extern __pdata uint8_t ao_packet_rx_len, ao_packet_rx_used, ao_packet_tx_used; +extern __xdata uint8_t ao_packet_restart; + +void +ao_packet_send(void); + +uint8_t +ao_packet_recv(void); + +void +ao_packet_flush(void); + +void +ao_packet_putchar(char c) __reentrant; + +int +_ao_packet_pollchar(void); + +#if PACKET_HAS_MASTER +/* ao_packet_master.c */ + +extern __xdata int8_t ao_packet_last_rssi; + +void +ao_packet_master_init(void); +#endif + +#if PACKET_HAS_SLAVE +/* ao_packet_slave.c */ + +void +ao_packet_slave_start(void); + +void +ao_packet_slave_stop(void); + +void +ao_packet_slave_init(uint8_t enable); + +#endif + +#endif /* _AO_PACKET_H_ */ diff --git a/src/kernel/ao_panic.c b/src/kernel/ao_panic.c new file mode 100644 index 00000000..c29cd8fe --- /dev/null +++ b/src/kernel/ao_panic.c @@ -0,0 +1,90 @@ +/* + * Copyright © 2009 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" + +#ifndef HAS_BEEP +#error Please define HAS_BEEP +#endif + +#if !HAS_BEEP +#define ao_beep(x) +#endif +#if !LEDS_AVAILABLE +#define ao_led_on(x) +#define ao_led_off(x) +#endif + +#ifndef AO_LED_PANIC +#define AO_LED_PANIC AO_LED_RED +#endif + +static void +ao_panic_delay(uint8_t n) +{ + uint8_t i = 0, j = 0; + + while (n--) + while (--j) + while (--i) + ao_arch_nop(); +} + +void +ao_panic(uint8_t reason) +{ + uint8_t n; + +#if LOW_LEVEL_DEBUG + ao_cur_task = NULL; + printf ("panic %d\n", reason); +#endif + ao_arch_block_interrupts(); + for (;;) { + ao_panic_delay(20); + for (n = 0; n < 5; n++) { + ao_led_on(AO_LED_PANIC); + ao_beep(AO_BEEP_HIGH); + ao_panic_delay(1); + ao_led_off(AO_LED_PANIC); + ao_beep(AO_BEEP_LOW); + ao_panic_delay(1); + } + ao_beep(AO_BEEP_OFF); + ao_panic_delay(2); + +#ifdef SDCC +#pragma disable_warning 126 +#endif + if (reason & 0x40) { + ao_led_on(AO_LED_PANIC); + ao_beep(AO_BEEP_HIGH); + ao_panic_delay(40); + ao_led_off(AO_LED_PANIC); + ao_beep(AO_BEEP_OFF); + ao_panic_delay(10); + } + for (n = 0; n < (reason & 0x3f); n++) { + ao_led_on(AO_LED_PANIC); + ao_beep(AO_BEEP_MID); + ao_panic_delay(10); + ao_led_off(AO_LED_PANIC); + ao_beep(AO_BEEP_OFF); + ao_panic_delay(10); + } + } +} diff --git a/src/kernel/ao_product.c b/src/kernel/ao_product.c new file mode 100644 index 00000000..b9327bac --- /dev/null +++ b/src/kernel/ao_product.c @@ -0,0 +1,161 @@ +/* + * Copyright © 2009 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_product.h" + +/* Defines which mark this particular AltOS product */ + +const char ao_version[AO_MAX_VERSION] = AO_iVersion_STRING; +const char ao_manufacturer[] = AO_iManufacturer_STRING; +const char ao_product[] = AO_iProduct_STRING; + +#define LE_WORD(x) ((x)&0xFF),((uint8_t) (((uint16_t) (x))>>8)) + +#if HAS_USB + +/* Maximum power in mA */ +#ifndef AO_USB_MAX_POWER +#define AO_USB_MAX_POWER 100 +#endif + +#include "ao_usb.h" +/* USB descriptors in one giant block of bytes */ +AO_ROMCONFIG_SYMBOL(0x00aa) uint8_t ao_usb_descriptors [] = +{ + /* Device descriptor */ + 0x12, + AO_USB_DESC_DEVICE, + LE_WORD(0x0110), /* bcdUSB */ + 0x02, /* bDeviceClass */ + 0x00, /* bDeviceSubClass */ + 0x00, /* bDeviceProtocol */ + AO_USB_CONTROL_SIZE, /* bMaxPacketSize */ + LE_WORD(0xFFFE), /* idVendor */ + LE_WORD(AO_idProduct_NUMBER), /* idProduct */ + LE_WORD(0x0100), /* bcdDevice */ + 0x01, /* iManufacturer */ + 0x02, /* iProduct */ + 0x03, /* iSerialNumber */ + 0x01, /* bNumConfigurations */ + + /* Configuration descriptor */ + 0x09, + AO_USB_DESC_CONFIGURATION, + LE_WORD(67), /* wTotalLength */ + 0x02, /* bNumInterfaces */ + 0x01, /* bConfigurationValue */ + 0x00, /* iConfiguration */ + 0xC0, /* bmAttributes */ + AO_USB_MAX_POWER >> 1, /* bMaxPower, 2mA units */ + + /* Control class interface */ + 0x09, + AO_USB_DESC_INTERFACE, + 0x00, /* bInterfaceNumber */ + 0x00, /* bAlternateSetting */ + 0x01, /* bNumEndPoints */ + 0x02, /* bInterfaceClass */ + 0x02, /* bInterfaceSubClass */ + 0x01, /* bInterfaceProtocol, linux requires value of 1 for the cdc_acm module */ + 0x00, /* iInterface */ + + /* Header functional descriptor */ + 0x05, + AO_USB_CS_INTERFACE, + 0x00, /* bDescriptor SubType Header */ + LE_WORD(0x0110), /* CDC version 1.1 */ + + /* Call management functional descriptor */ + 0x05, + AO_USB_CS_INTERFACE, + 0x01, /* bDescriptor SubType Call Management */ + 0x01, /* bmCapabilities = device handles call management */ + 0x01, /* bDataInterface call management interface number */ + + /* ACM functional descriptor */ + 0x04, + AO_USB_CS_INTERFACE, + 0x02, /* bDescriptor SubType Abstract Control Management */ + 0x02, /* bmCapabilities = D1 (Set_line_Coding, Set_Control_Line_State, Get_Line_Coding and Serial_State) */ + + /* Union functional descriptor */ + 0x05, + AO_USB_CS_INTERFACE, + 0x06, /* bDescriptor SubType Union Functional descriptor */ + 0x00, /* bMasterInterface */ + 0x01, /* bSlaveInterface0 */ + + /* Notification EP */ + 0x07, + AO_USB_DESC_ENDPOINT, + AO_USB_INT_EP|0x80, /* bEndpointAddress */ + 0x03, /* bmAttributes = intr */ + LE_WORD(8), /* wMaxPacketSize */ + 0xff, /* bInterval */ + + /* Data class interface descriptor */ + 0x09, + AO_USB_DESC_INTERFACE, + 0x01, /* bInterfaceNumber */ + 0x00, /* bAlternateSetting */ + 0x02, /* bNumEndPoints */ + 0x0A, /* bInterfaceClass = data */ + 0x00, /* bInterfaceSubClass */ + 0x00, /* bInterfaceProtocol */ + 0x00, /* iInterface */ + + /* Data EP OUT */ + 0x07, + AO_USB_DESC_ENDPOINT, + AO_USB_OUT_EP, /* bEndpointAddress */ + 0x02, /* bmAttributes = bulk */ + LE_WORD(AO_USB_OUT_SIZE),/* wMaxPacketSize */ + 0x00, /* bInterval */ + + /* Data EP in */ + 0x07, + AO_USB_DESC_ENDPOINT, + AO_USB_IN_EP|0x80, /* bEndpointAddress */ + 0x02, /* bmAttributes = bulk */ + LE_WORD(AO_USB_IN_SIZE),/* wMaxPacketSize */ + 0x00, /* bInterval */ + + /* String descriptors */ + 0x04, + AO_USB_DESC_STRING, + LE_WORD(0x0409), + + /* iManufacturer */ + AO_iManufacturer_LEN, + AO_USB_DESC_STRING, + AO_iManufacturer_UCS2, + + /* iProduct */ + AO_iProduct_LEN, + AO_USB_DESC_STRING, + AO_iProduct_UCS2, + + /* iSerial */ + AO_iSerial_LEN, + AO_USB_DESC_STRING, + AO_iSerial_UCS2, + + /* Terminating zero */ + 0 +}; +#endif diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c new file mode 100644 index 00000000..e59f5bc4 --- /dev/null +++ b/src/kernel/ao_pyro.c @@ -0,0 +1,518 @@ +/* + * 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. + */ + +#ifndef AO_FLIGHT_TEST +#include +#include +#include +#endif +#include + +#if IS_COMPANION +#include +#define ao_accel ao_companion_command.accel +#define ao_speed ao_companion_command.speed +#define ao_height ao_companion_command.height +#define ao_flight_state ao_companion_command.flight_state +#define ao_motor_number ao_companion_command.motor_number +#endif + +#define ao_lowbit(x) ((x) & (-x)) + +#ifndef AO_FLIGHT_TEST +enum ao_igniter_status +ao_pyro_status(uint8_t p) +{ + __xdata struct ao_data packet; + __pdata int16_t value; + + ao_arch_critical( + ao_data_get(&packet); + ); + + value = (AO_IGNITER_CLOSED>>1); + value = AO_SENSE_PYRO(&packet, p); + if (value < AO_IGNITER_OPEN) + return ao_igniter_open; + else if (value > AO_IGNITER_CLOSED) + return ao_igniter_ready; + else + return ao_igniter_unknown; +} + +void +ao_pyro_print_status(void) +{ + uint8_t p; + + for(p = 0; p < AO_PYRO_NUM; p++) { + enum ao_igniter_status status = ao_pyro_status(p); + printf("Igniter: %d Status: %s\n", + p, ao_igniter_status_names[status]); + } +} +#endif + +uint16_t ao_pyro_fired; + +/* + * Given a pyro structure, figure out + * if the current flight state satisfies all + * of the requirements + */ +static uint8_t +ao_pyro_ready(struct ao_pyro *pyro) +{ + enum ao_pyro_flag flag, flags; + + flags = pyro->flags; + while (flags != ao_pyro_none) { + flag = ao_lowbit(flags); + flags &= ~flag; + switch (flag) { + + case ao_pyro_accel_less: + if (ao_accel <= pyro->accel_less) + continue; + break; + case ao_pyro_accel_greater: + if (ao_accel >= pyro->accel_greater) + continue; + break; + + + case ao_pyro_speed_less: + if (ao_speed <= pyro->speed_less) + continue; + break; + case ao_pyro_speed_greater: + if (ao_speed >= pyro->speed_greater) + continue; + break; + + case ao_pyro_height_less: + if (ao_height <= pyro->height_less) + continue; + break; + case ao_pyro_height_greater: + if (ao_height >= pyro->height_greater) + continue; + break; + +#if HAS_GYRO + case ao_pyro_orient_less: + if (ao_sample_orient <= pyro->orient_less) + continue; + break; + case ao_pyro_orient_greater: + if (ao_sample_orient >= pyro->orient_greater) + continue; + break; +#endif + + case ao_pyro_time_less: + if ((int16_t) (ao_time() - ao_boost_tick) <= pyro->time_less) + continue; + break; + case ao_pyro_time_greater: + if ((int16_t) (ao_time() - ao_boost_tick) >= pyro->time_greater) + continue; + break; + + case ao_pyro_ascending: + if (ao_speed > 0) + continue; + break; + case ao_pyro_descending: + if (ao_speed < 0) + continue; + break; + + case ao_pyro_after_motor: + if (ao_motor_number == pyro->motor) + continue; + break; + + case ao_pyro_delay: + /* handled separately */ + continue; + + case ao_pyro_state_less: + if (ao_flight_state < pyro->state_less) + continue; + break; + case ao_pyro_state_greater_or_equal: + if (ao_flight_state >= pyro->state_greater_or_equal) + continue; + break; + + default: + continue; + } + return FALSE; + } + return TRUE; +} + +#ifndef AO_FLIGHT_TEST +static void +ao_pyro_pin_set(uint8_t p, uint8_t v) +{ + switch (p) { +#if AO_PYRO_NUM > 0 + case 0: ao_gpio_set(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0, v); break; +#endif +#if AO_PYRO_NUM > 1 + case 1: ao_gpio_set(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1, v); break; +#endif +#if AO_PYRO_NUM > 2 + case 2: ao_gpio_set(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2, v); break; +#endif +#if AO_PYRO_NUM > 3 + case 3: ao_gpio_set(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3, v); break; +#endif +#if AO_PYRO_NUM > 4 + case 4: ao_gpio_set(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4, v); break; +#endif +#if AO_PYRO_NUM > 5 + case 5: ao_gpio_set(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5, v); break; +#endif +#if AO_PYRO_NUM > 6 + case 6: ao_gpio_set(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6, v); break; +#endif +#if AO_PYRO_NUM > 7 + case 7: ao_gpio_set(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, v); break; +#endif + default: break; + } +} +#endif + +uint8_t ao_pyro_wakeup; + +static void +ao_pyro_pins_fire(uint16_t fire) +{ + uint8_t p; + + for (p = 0; p < AO_PYRO_NUM; p++) { + if (fire & (1 << p)) + ao_pyro_pin_set(p, 1); + } + ao_delay(AO_MS_TO_TICKS(50)); + for (p = 0; p < AO_PYRO_NUM; p++) { + if (fire & (1 << p)) { + ao_pyro_pin_set(p, 0); + ao_config.pyro[p].fired = 1; + ao_pyro_fired |= (1 << p); + } + } + ao_delay(AO_MS_TO_TICKS(50)); +} + +static uint8_t +ao_pyro_check(void) +{ + struct ao_pyro *pyro; + uint8_t p, any_waiting; + uint16_t fire = 0; + + any_waiting = 0; + for (p = 0; p < AO_PYRO_NUM; p++) { + pyro = &ao_config.pyro[p]; + + /* Ignore igniters which have already fired + */ + if (pyro->fired) + continue; + + /* Ignore disabled igniters + */ + if (!pyro->flags) + continue; + + any_waiting = 1; + /* Check pyro state to see if it should fire + */ + if (!pyro->delay_done) { + if (!ao_pyro_ready(pyro)) + continue; + + /* If there's a delay set, then remember when + * it expires + */ + if (pyro->flags & ao_pyro_delay) { + pyro->delay_done = ao_time() + pyro->delay; + if (!pyro->delay_done) + pyro->delay_done = 1; + } + } + + /* Check to see if we're just waiting for + * the delay to expire + */ + if (pyro->delay_done) { + if ((int16_t) (ao_time() - pyro->delay_done) < 0) + continue; + } + + fire |= (1 << p); + } + + if (fire) + ao_pyro_pins_fire(fire); + + return any_waiting; +} + +#define NO_VALUE 0xff + +#define AO_PYRO_NAME_LEN 3 + +#if !DISABLE_HELP +#define ENABLE_HELP 1 +#endif + +#if ENABLE_HELP +#define HELP(s) (s) +#else +#define HELP(s) +#endif + +const struct { + char name[AO_PYRO_NAME_LEN]; + enum ao_pyro_flag flag; + uint8_t offset; +#if ENABLE_HELP + char *help; +#endif +} ao_pyro_values[] = { + { "a<", ao_pyro_accel_less, offsetof(struct ao_pyro, accel_less), HELP("accel less (m/ss * 16)") }, + { "a>", ao_pyro_accel_greater, offsetof(struct ao_pyro, accel_greater), HELP("accel greater (m/ss * 16)") }, + + { "s<", ao_pyro_speed_less, offsetof(struct ao_pyro, speed_less), HELP("speed less (m/s * 16)") }, + { "s>", ao_pyro_speed_greater, offsetof(struct ao_pyro, speed_greater), HELP("speed greater (m/s * 16)") }, + + { "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_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 + + { "t<", ao_pyro_time_less, offsetof(struct ao_pyro, time_less), HELP("time less (s * 100)") }, + { "t>", ao_pyro_time_greater, offsetof(struct ao_pyro, time_greater), HELP("time greater (s * 100)") }, + + { "f<", ao_pyro_state_less, offsetof(struct ao_pyro, state_less), HELP("state less") }, + { "f>=",ao_pyro_state_greater_or_equal, offsetof(struct ao_pyro, state_greater_or_equal), HELP("state greater or equal") }, + + { "A", ao_pyro_ascending, NO_VALUE, HELP("ascending") }, + { "D", ao_pyro_descending, NO_VALUE, HELP("descending") }, + + { "m", ao_pyro_after_motor, offsetof(struct ao_pyro, motor), HELP("after motor") }, + + { "d", ao_pyro_delay, offsetof(struct ao_pyro, delay), HELP("delay before firing (s * 100)") }, + { "", ao_pyro_none, NO_VALUE, HELP(NULL) }, +}; + +#define NUM_PYRO_VALUES (sizeof ao_pyro_values / sizeof ao_pyro_values[0]) + +#ifndef AO_FLIGHT_TEST +static void +ao_pyro(void) +{ + uint8_t any_waiting; + + ao_config_get(); + while (ao_flight_state < ao_flight_boost) + ao_sleep(&ao_flight_state); + + for (;;) { + ao_alarm(AO_MS_TO_TICKS(100)); + ao_sleep(&ao_pyro_wakeup); + ao_clear_alarm(); + if (ao_flight_state >= ao_flight_landed) + break; + any_waiting = ao_pyro_check(); + if (!any_waiting) + break; + } + ao_exit(); +} + +__xdata struct ao_task ao_pyro_task; + + +static void +ao_pyro_print_name(uint8_t v) +{ + const char *s = ao_pyro_values[v].name; + printf ("%s%s", s, " " + strlen(s)); +} + +#if ENABLE_HELP +static void +ao_pyro_help(void) +{ + uint8_t v; + for (v = 0; ao_pyro_values[v].flag != ao_pyro_none; v++) { + ao_pyro_print_name(v); + if (ao_pyro_values[v].offset != NO_VALUE) + printf (" "); + else + printf (" "); + printf ("%s\n", ao_pyro_values[v].help); + } +} +#endif + +void +ao_pyro_show(void) +{ + uint8_t p; + uint8_t v; + struct ao_pyro *pyro; + + printf ("Pyro-count: %d\n", AO_PYRO_NUM); + for (p = 0; p < AO_PYRO_NUM; p++) { + printf ("Pyro %2d: ", p); + pyro = &ao_config.pyro[p]; + if (!pyro->flags) { + printf ("\n"); + continue; + } + for (v = 0; ao_pyro_values[v].flag != ao_pyro_none; v++) { + if (!(pyro->flags & ao_pyro_values[v].flag)) + continue; + ao_pyro_print_name(v); + if (ao_pyro_values[v].offset != NO_VALUE) { + int16_t value; + + value = *((int16_t *) ((char *) pyro + ao_pyro_values[v].offset)); + printf ("%6d ", value); + } else { + printf (" "); + } + } + printf ("\n"); + } +} + +void +ao_pyro_set(void) +{ + uint8_t p; + struct ao_pyro pyro_tmp; + char name[AO_PYRO_NAME_LEN]; + uint8_t c; + uint8_t v; + + ao_cmd_white(); + +#if ENABLE_HELP + switch (ao_cmd_lex_c) { + case '?': + ao_pyro_help(); + return; + } +#endif + + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + p = ao_cmd_lex_i; + if (AO_PYRO_NUM <= p) { + printf ("invalid pyro channel %d\n", p); + return; + } + pyro_tmp.flags = 0; + for (;;) { + ao_cmd_white(); + if (ao_cmd_lex_c == '\n') + break; + + for (c = 0; c < AO_PYRO_NAME_LEN - 1; c++) { + if (ao_cmd_is_white()) + break; + name[c] = ao_cmd_lex_c; + ao_cmd_lex(); + } + name[c] = '\0'; + for (v = 0; ao_pyro_values[v].flag != ao_pyro_none; v++) { + if (!strcmp (ao_pyro_values[v].name, name)) + break; + } + if (ao_pyro_values[v].flag == ao_pyro_none) { + printf ("invalid pyro field %s\n", name); + ao_cmd_status = ao_cmd_syntax_error; + return; + } + pyro_tmp.flags |= ao_pyro_values[v].flag; + if (ao_pyro_values[v].offset != NO_VALUE) { + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + *((int16_t *) ((char *) &pyro_tmp + ao_pyro_values[v].offset)) = ao_cmd_lex_i; + } + } + _ao_config_edit_start(); + ao_config.pyro[p] = pyro_tmp; + _ao_config_edit_finish(); +} + +void +ao_pyro_manual(uint8_t p) +{ + printf ("ao_pyro_manual %d\n", p); + if (p >= AO_PYRO_NUM) { + ao_cmd_status = ao_cmd_syntax_error; + return; + } + ao_pyro_pins_fire(1 << p); +} + +void +ao_pyro_init(void) +{ +#if AO_PYRO_NUM > 0 + ao_enable_output(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0, 0); +#endif +#if AO_PYRO_NUM > 1 + ao_enable_output(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1, 0); +#endif +#if AO_PYRO_NUM > 2 + ao_enable_output(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2, 0); +#endif +#if AO_PYRO_NUM > 3 + ao_enable_output(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3, 0); +#endif +#if AO_PYRO_NUM > 4 + ao_enable_output(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4, 0); +#endif +#if AO_PYRO_NUM > 5 + ao_enable_output(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5, 0); +#endif +#if AO_PYRO_NUM > 6 + ao_enable_output(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6, 0); +#endif +#if AO_PYRO_NUM > 7 + ao_enable_output(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, 0); +#endif + ao_add_task(&ao_pyro_task, ao_pyro, "pyro"); +} +#endif diff --git a/src/kernel/ao_pyro.h b/src/kernel/ao_pyro.h new file mode 100644 index 00000000..0c5642d6 --- /dev/null +++ b/src/kernel/ao_pyro.h @@ -0,0 +1,83 @@ +/* + * 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. + */ + +#ifndef _AO_PYRO_H_ +#define _AO_PYRO_H_ + +enum ao_pyro_flag { + ao_pyro_none = 0x00000000, + + ao_pyro_accel_less = 0x00000001, + ao_pyro_accel_greater = 0x00000002, + + ao_pyro_speed_less = 0x00000004, + ao_pyro_speed_greater = 0x00000008, + + ao_pyro_height_less = 0x00000010, + ao_pyro_height_greater = 0x00000020, + + ao_pyro_orient_less = 0x00000040, + ao_pyro_orient_greater = 0x00000080, + + ao_pyro_time_less = 0x00000100, + ao_pyro_time_greater = 0x00000200, + + ao_pyro_ascending = 0x00000400, + ao_pyro_descending = 0x00000800, + + ao_pyro_after_motor = 0x00001000, + + ao_pyro_delay = 0x00002000, + + ao_pyro_state_less = 0x00004000, + ao_pyro_state_greater_or_equal = 0x00008000, +}; + +struct ao_pyro { + enum ao_pyro_flag flags; + int16_t accel_less, accel_greater; + int16_t speed_less, speed_greater; + int16_t height_less, height_greater; + int16_t orient_less, orient_greater; + int16_t time_less, time_greater; + int16_t delay; + uint8_t state_less, state_greater_or_equal; + int16_t motor; + uint16_t delay_done; + uint8_t fired; +}; + +extern uint8_t ao_pyro_wakeup; + +extern uint16_t ao_pyro_fired; + +void +ao_pyro_set(void); + +void +ao_pyro_show(void); + +void +ao_pyro_init(void); + +void +ao_pyro_manual(uint8_t p); + +void +ao_pyro_print_status(void); + +#endif diff --git a/src/kernel/ao_quaternion.h b/src/kernel/ao_quaternion.h new file mode 100644 index 00000000..044f1607 --- /dev/null +++ b/src/kernel/ao_quaternion.h @@ -0,0 +1,249 @@ +/* + * 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, + const struct ao_quaternion *a, + const struct ao_quaternion *b) +{ + struct ao_quaternion t; +#define T(_a,_b) (((a)->_a) * ((b)->_b)) + +/* + * Quaternions + * + * ii = jj = kk = ijk = -1; + * + * kji = 1; + * + * ij = k; ji = -k; + * kj = -i; jk = i; + * ik = -j; ki = j; + * + * Multiplication p * q: + * + * (pr + ipx + jpy + kpz) (qr + iqx + jqy + kqz) = + * + * ( pr * qr + pr * iqx + pr * jqy + pr * kqz) + + * (ipx * qr + ipx * iqx + ipx * jqy + ipx * kqz) + + * (jpy * qr + jpy * iqx + jpy * jqy + jpy * kqz) + + * (kpz * qr + kpz * iqx + kpz * jqy + kpz * kqz) = + * + * + * (pr * qr) + i(pr * qx) + j(pr * qy) + k(pr * qz) + + * i(px * qr) - (px * qx) + k(px * qy) - j(px * qz) + + * j(py * qr) - k(py * qx) - (py * qy) + i(py * qz) + + * k(pz * qr) + j(pz * qx) - i(pz * qy) - (pz * qz) = + * + * 1 * ( (pr * qr) - (px * qx) - (py * qy) - (pz * qz) ) + + * i * ( (pr * qx) + (px * qr) + (py * qz) - (pz * qy) ) + + * j * ( (pr * qy) - (px * qz) + (py * qr) + (pz * qx) ) + + * k * ( (pr * qz) + (px * qy) - (py * qx) + (pz * qr); + */ + + 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, + const 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(const 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, + const 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, + const 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 float ao_quaternion_dot(const struct ao_quaternion *a, + const struct ao_quaternion *b) +{ +#define T(_a) (((a)->_a) * ((b)->_a)) + return T(r) + T(x) + T(y) + T(z); +#undef T +} + + +static inline void ao_quaternion_rotate(struct ao_quaternion *r, + const struct ao_quaternion *a, + const struct ao_quaternion *b) +{ + struct ao_quaternion c; + struct ao_quaternion t; + + ao_quaternion_multiply(&t, b, a); + ao_quaternion_conjugate(&c, b); + ao_quaternion_multiply(r, &t, &c); +} + +/* + * Compute a rotation quaternion between two vectors + * + * cos(θ) + u * sin(θ) + * + * where θ is the angle between the two vectors and u + * is a unit vector axis of rotation + */ + +static inline void ao_quaternion_vectors_to_rotation(struct ao_quaternion *r, + const struct ao_quaternion *a, + const struct ao_quaternion *b) +{ + /* + * The cross product will point orthogonally to the two + * vectors, forming our rotation axis. The length will be + * sin(θ), so these values are already multiplied by that. + */ + + float x = a->y * b->z - a->z * b->y; + float y = a->z * b->x - a->x * b->z; + float z = a->x * b->y - a->y * b->x; + + float s_2 = x*x + y*y + z*z; + float s = sqrtf(s_2); + + /* cos(θ) = a · b / (|a| |b|). + * + * a and b are both unit vectors, so the divisor is one + */ + float c = a->x*b->x + a->y*b->y + a->z*b->z; + + float c_half = sqrtf ((1 + c) / 2); + float s_half = sqrtf ((1 - c) / 2); + + /* + * Divide out the sine factor from the + * cross product, then multiply in the + * half sine factor needed for the quaternion + */ + float s_scale = s_half / s; + + r->x = x * s_scale; + r->y = y * s_scale; + r->z = z * s_scale; + + r->r = c_half; + + ao_quaternion_normalize(r, r); +} + +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; +} + +/* + * The sincosf from newlib just calls sinf and cosf. This is a bit + * faster, if slightly less precise + */ + +static inline void +ao_sincosf(float a, float *s, float *c) { + float _s = sinf(a); + *s = _s; + *c = sqrtf(1 - _s*_s); +} + +/* + * Initialize a quaternion from 1/2 euler rotation angles (in radians). + * + * Yes, it would be nicer if there were a faster way, but because we + * sample the gyros at only 100Hz, we end up getting angles too large + * to take advantage of sin(x) ≃ x. + * + * We might be able to use just a couple of elements of the sin taylor + * series though, instead of the whole sin function? + */ + +static inline void ao_quaternion_init_half_euler(struct ao_quaternion *r, + float x, float y, float z) +{ + float s_x, c_x; + float s_y, c_y; + float s_z, c_z; + + ao_sincosf(x, &s_x, &c_x); + ao_sincosf(y, &s_y, &c_y); + ao_sincosf(z, &s_z, &c_z); + + r->r = c_x * c_y * c_z + s_x * s_y * s_z; + r->x = s_x * c_y * c_z - c_x * s_y * s_z; + r->y = c_x * s_y * c_z + s_x * c_y * s_z; + r->z = c_x * c_y * s_z - s_x * s_y * c_z; +} + +#endif /* _AO_QUATERNION_H_ */ diff --git a/src/kernel/ao_radio_cmac.c b/src/kernel/ao_radio_cmac.c new file mode 100644 index 00000000..bff848f6 --- /dev/null +++ b/src/kernel/ao_radio_cmac.c @@ -0,0 +1,164 @@ +/* + * 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 +#include + +static __xdata uint8_t ao_radio_cmac_mutex; +__pdata int8_t ao_radio_cmac_rssi; +static __xdata uint8_t cmac_data[AO_CMAC_MAX_LEN + AO_CMAC_KEY_LEN + 2 + AO_CMAC_KEY_LEN]; + +static uint8_t +round_len(uint8_t len) +{ + uint8_t rem; + + /* Make sure we transfer at least one packet, and + * then make sure every packet is full. Note that + * there is no length encoded, and that the receiver + * must deal with any extra bytes in the packet + */ + if (len < AO_CMAC_KEY_LEN) + len = AO_CMAC_KEY_LEN; + rem = len % AO_CMAC_KEY_LEN; + if (rem != 0) + len += (AO_CMAC_KEY_LEN - rem); + return len; +} + +/* + * Sign and deliver the data sitting in the cmac buffer + */ +static void +radio_cmac_send(uint8_t len) __reentrant +{ + uint8_t i; + + len = round_len(len); + /* Make sure the AES key is loaded */ + ao_config_get(); + +#if HAS_MONITOR + ao_monitor_set(0); +#endif + + ao_mutex_get(&ao_aes_mutex); + ao_aes_set_mode(ao_aes_mode_cbc_mac); + ao_aes_set_key(ao_config.aes_key); + ao_aes_zero_iv(); + for (i = 0; i < len; i += AO_CMAC_KEY_LEN) { + if (i + AO_CMAC_KEY_LEN < len) + ao_aes_run(&cmac_data[i], NULL); + else + ao_aes_run(&cmac_data[i], &cmac_data[len]); + } + ao_mutex_put(&ao_aes_mutex); + + ao_radio_send(cmac_data, len + AO_CMAC_KEY_LEN); +} + +/* + * Receive and validate an incoming packet + */ + +static int8_t +radio_cmac_recv(uint8_t len, uint16_t timeout) __reentrant +{ + uint8_t i; + + len = round_len(len); +#if HAS_MONITOR + ao_monitor_set(0); +#endif + i = ao_radio_recv(cmac_data, len + AO_CMAC_KEY_LEN + 2, timeout); + + if (!i) { + ao_radio_cmac_rssi = 0; + return AO_RADIO_CMAC_TIMEOUT; + } + + ao_radio_cmac_rssi = ao_radio_rssi; + if (!(cmac_data[len + AO_CMAC_KEY_LEN +1] & AO_RADIO_STATUS_CRC_OK)) + return AO_RADIO_CMAC_CRC_ERROR; + + ao_config_get(); + + /* Compute the packet signature + */ + ao_mutex_get(&ao_aes_mutex); + ao_aes_set_mode(ao_aes_mode_cbc_mac); + ao_aes_set_key(ao_config.aes_key); + ao_aes_zero_iv(); + for (i = 0; i < len; i += AO_CMAC_KEY_LEN) { + if (i + AO_CMAC_KEY_LEN < len) + ao_aes_run(&cmac_data[i], NULL); + else + ao_aes_run(&cmac_data[i], &cmac_data[len + AO_CMAC_KEY_LEN + 2]); + } + ao_mutex_put(&ao_aes_mutex); + + /* Check the packet signature against the signature provided + * over the link + */ + + if (memcmp(&cmac_data[len], + &cmac_data[len + AO_CMAC_KEY_LEN + 2], + AO_CMAC_KEY_LEN) != 0) { + return AO_RADIO_CMAC_MAC_ERROR; + } + + return AO_RADIO_CMAC_OK; +} + +int8_t +ao_radio_cmac_send(__xdata void *packet, uint8_t len) __reentrant +{ + if (len > AO_CMAC_MAX_LEN) + return AO_RADIO_CMAC_LEN_ERROR; + ao_mutex_get(&ao_radio_cmac_mutex); + ao_xmemcpy(cmac_data, packet, len); +#if AO_LED_TX + ao_led_on(AO_LED_TX); +#endif + radio_cmac_send(len); +#if AO_LED_TX + ao_led_off(AO_LED_TX); +#endif + ao_mutex_put(&ao_radio_cmac_mutex); + return AO_RADIO_CMAC_OK; +} + +int8_t +ao_radio_cmac_recv(__xdata void *packet, uint8_t len, uint16_t timeout) __reentrant +{ + int8_t i; + if (len > AO_CMAC_MAX_LEN) + return AO_RADIO_CMAC_LEN_ERROR; + ao_mutex_get(&ao_radio_cmac_mutex); +#if AO_LED_RX + ao_led_on(AO_LED_RX); +#endif + i = radio_cmac_recv(len, timeout); +#if AO_LED_RX + ao_led_off(AO_LED_RX); +#endif + if (i == AO_RADIO_CMAC_OK) + ao_xmemcpy(packet, cmac_data, len); + ao_mutex_put(&ao_radio_cmac_mutex); + return i; +} + diff --git a/src/kernel/ao_radio_cmac.h b/src/kernel/ao_radio_cmac.h new file mode 100644 index 00000000..e86f31e9 --- /dev/null +++ b/src/kernel/ao_radio_cmac.h @@ -0,0 +1,43 @@ +/* + * 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. + */ + +#ifndef _AO_RADIO_CMAC_H_ +#define _AO_RADIO_CMAC_H_ + +#include + +#define AO_CMAC_KEY_LEN AO_AES_LEN +#define AO_CMAC_MAX_LEN (128 - AO_CMAC_KEY_LEN) + +extern __pdata int8_t ao_radio_cmac_rssi; + +int8_t +ao_radio_cmac_send(__xdata void *packet, uint8_t len) __reentrant; + +#define AO_RADIO_CMAC_OK 0 +#define AO_RADIO_CMAC_LEN_ERROR -1 +#define AO_RADIO_CMAC_CRC_ERROR -2 +#define AO_RADIO_CMAC_MAC_ERROR -3 +#define AO_RADIO_CMAC_TIMEOUT -4 + +int8_t +ao_radio_cmac_recv(__xdata void *packet, uint8_t len, uint16_t timeout) __reentrant; + +void +ao_radio_cmac_init(void); + +#endif /* _AO_RADIO_CMAC_H_ */ diff --git a/src/kernel/ao_radio_cmac_cmd.c b/src/kernel/ao_radio_cmac_cmd.c new file mode 100644 index 00000000..64410921 --- /dev/null +++ b/src/kernel/ao_radio_cmac_cmd.c @@ -0,0 +1,104 @@ +/* + * 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 + +static __xdata uint8_t cmac_data[AO_CMAC_MAX_LEN]; + +static uint8_t +getnibble(void) +{ + int8_t b; + + b = ao_cmd_hexchar(getchar()); + if (b < 0) { + ao_cmd_status = ao_cmd_lex_error; + return 0; + } + return (uint8_t) b; +} + +static uint8_t +getbyte(void) +{ + uint8_t b; + b = getnibble() << 4; + b |= getnibble(); + return b; +} + +static void +radio_cmac_send_cmd(void) __reentrant +{ + uint8_t i; + uint8_t len; + + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + len = ao_cmd_lex_i; + if (len > AO_CMAC_MAX_LEN) { + ao_cmd_status = ao_cmd_syntax_error; + return; + } + flush(); + len = ao_cmd_lex_i; + for (i = 0; i < len; i++) { + cmac_data[i] = getbyte(); + if (ao_cmd_status != ao_cmd_success) + return; + } + ao_radio_cmac_send(cmac_data, len); +} + +static void +radio_cmac_recv_cmd(void) __reentrant +{ + uint8_t len, i; + uint16_t timeout; + + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + len = ao_cmd_lex_i; + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + timeout = AO_MS_TO_TICKS(ao_cmd_lex_i); + i = ao_radio_cmac_recv(cmac_data, len, timeout); + if (i == AO_RADIO_CMAC_OK) { + printf ("PACKET "); + for (i = 0; i < len; i++) + printf("%02x", cmac_data[i]); + printf (" %d\n", ao_radio_cmac_rssi); + } else + printf ("ERROR %d %d\n", i, ao_radio_cmac_rssi); +} + +static __code struct ao_cmds ao_radio_cmac_cmds[] = { + { radio_cmac_send_cmd, "s \0Send AES-CMAC packet. Bytes to send follow on next line" }, + { radio_cmac_recv_cmd, "S \0Receive AES-CMAC packet. Timeout in ms" }, + { 0, NULL }, +}; + +void +ao_radio_cmac_cmd_init(void) +{ + ao_cmd_register(&ao_radio_cmac_cmds[0]); +} diff --git a/src/kernel/ao_radio_cmac_cmd.h b/src/kernel/ao_radio_cmac_cmd.h new file mode 100644 index 00000000..6b8782de --- /dev/null +++ b/src/kernel/ao_radio_cmac_cmd.h @@ -0,0 +1,24 @@ +/* + * 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. + */ + +#ifndef _AO_RADIO_CMAC_CMD_H_ +#define _AO_RADIO_CMAC_CMD_H_ + +void +ao_radio_cmac_cmd_init(void); + +#endif /* _AO_RADIO_CMAC_CMD_H_ */ diff --git a/src/kernel/ao_report.c b/src/kernel/ao_report.c new file mode 100644 index 00000000..1104cd82 --- /dev/null +++ b/src/kernel/ao_report.c @@ -0,0 +1,198 @@ +/* + * Copyright © 2009 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 +#include + +#define BIT(i,x) ((x) ? (1 << (i)) : 0) +#define MORSE1(a) (1 | BIT(3,a)) +#define MORSE2(a,b) (2 | BIT(3,a) | BIT(4,b)) +#define MORSE3(a,b,c) (3 | BIT(3,a) | BIT(4,b) | BIT(5,c)) +#define MORSE4(a,b,c,d) (4 | BIT(3,a) | BIT(4,b) | BIT(5,c) | BIT(6,d)) +#define MORSE5(a,b,c,d,e) (5 | BIT(3,a) | BIT(4,b) | BIT(5,c) | BIT(6,d) | BIT(7,e)) + +static const uint8_t flight_reports[] = { + MORSE3(0,0,0), /* startup, 'S' */ + MORSE2(0,0), /* idle 'I' */ + MORSE4(0,1,1,0), /* pad 'P' */ + MORSE4(1,0,0,0), /* boost 'B' */ + MORSE4(0,0,1,0), /* fast 'F' */ + MORSE4(1,0,1,0), /* coast 'C' */ + MORSE3(1,0,0), /* drogue 'D' */ + MORSE2(1,1), /* main 'M' */ + MORSE4(0,1,0,0), /* landed 'L' */ + MORSE4(1,0,0,1), /* invalid 'X' */ +}; + +#if HAS_BEEP +#define low(time) ao_beep_for(AO_BEEP_LOW, time) +#define mid(time) ao_beep_for(AO_BEEP_MID, time) +#define high(time) ao_beep_for(AO_BEEP_HIGH, time) +#else +#define low(time) ao_led_for(AO_LED_GREEN, time) +#define mid(time) ao_led_for(AO_LED_RED, time) +#define high(time) ao_led_for(AO_LED_GREEN|AO_LED_RED, time) +#endif +#define pause(time) ao_delay(time) + +static __pdata enum ao_flight_state ao_report_state; + +static void +ao_report_beep(void) __reentrant +{ + uint8_t r = flight_reports[ao_flight_state]; + uint8_t l = r & 7; + + if (!r) + return; + while (l--) { + if (r & 8) + mid(AO_MS_TO_TICKS(600)); + else + mid(AO_MS_TO_TICKS(200)); + pause(AO_MS_TO_TICKS(200)); + r >>= 1; + } + pause(AO_MS_TO_TICKS(400)); +} + +static void +ao_report_digit(uint8_t digit) __reentrant +{ + if (!digit) { + mid(AO_MS_TO_TICKS(500)); + pause(AO_MS_TO_TICKS(200)); + } else { + while (digit--) { + mid(AO_MS_TO_TICKS(200)); + pause(AO_MS_TO_TICKS(200)); + } + } + pause(AO_MS_TO_TICKS(300)); +} + +static void +ao_report_altitude(void) +{ + __pdata int16_t agl = ao_max_height; + __xdata uint8_t digits[10]; + __pdata uint8_t ndigits, i; + + if (agl < 0) + agl = 0; + ndigits = 0; + do { + digits[ndigits++] = agl % 10; + agl /= 10; + } while (agl); + + i = ndigits; + do + ao_report_digit(digits[--i]); + while (i != 0); +} + +#if HAS_IGNITE_REPORT +static uint8_t +ao_report_igniter_ready(enum ao_igniter igniter) +{ + return ao_igniter_status(igniter) == ao_igniter_ready ? 1 : 0; +} + +uint8_t +ao_report_igniter(void) +{ + return (ao_report_igniter_ready(ao_igniter_drogue) | + (ao_report_igniter_ready(ao_igniter_main) << 1)); +} + +static void +ao_report_continuity(void) __reentrant +{ + uint8_t c; + +#if !HAS_IGNITE + if (!ao_igniter_present) + return; +#endif + c = ao_report_igniter(); + if (c) { + while (c--) { + high(AO_MS_TO_TICKS(25)); + pause(AO_MS_TO_TICKS(100)); + } + } else { + c = 10; + while (c--) { + high(AO_MS_TO_TICKS(20)); + low(AO_MS_TO_TICKS(20)); + } + } +#if HAS_LOG + if (ao_log_full()) { + pause(AO_MS_TO_TICKS(100)); + c = 2; + while (c--) { + low(AO_MS_TO_TICKS(100)); + mid(AO_MS_TO_TICKS(100)); + high(AO_MS_TO_TICKS(100)); + mid(AO_MS_TO_TICKS(100)); + } + } +#endif +} +#endif + +void +ao_report(void) +{ + ao_report_state = ao_flight_state; + for(;;) { + ao_report_beep(); + if (ao_flight_state == ao_flight_landed) { + ao_report_altitude(); +#if HAS_FLIGHT + ao_delay(AO_SEC_TO_TICKS(5)); + continue; +#endif + } +#if HAS_IGNITE_REPORT + if (ao_flight_state == ao_flight_idle) + ao_report_continuity(); + while (ao_flight_state == ao_flight_pad) { + uint8_t c; + ao_report_continuity(); + c = 50; + while (c-- && ao_flight_state == ao_flight_pad) + pause(AO_MS_TO_TICKS(100)); + } +#endif + + while (ao_report_state == ao_flight_state) + ao_sleep(DATA_TO_XDATA(&ao_flight_state)); + ao_report_state = ao_flight_state; + } +} + +static __xdata struct ao_task ao_report_task; + +void +ao_report_init(void) +{ + ao_add_task(&ao_report_task, ao_report, "report"); +} diff --git a/src/kernel/ao_report_micro.c b/src/kernel/ao_report_micro.c new file mode 100644 index 00000000..0e8e287f --- /dev/null +++ b/src/kernel/ao_report_micro.c @@ -0,0 +1,57 @@ +/* + * 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 + +#define mid(time) ao_led_for(AO_LED_REPORT, time) +#define pause(time) ao_delay(time) + +static void +ao_report_digit(uint8_t digit) __reentrant +{ + if (!digit) { + mid(AO_MS_TO_TICKS(1000)); + pause(AO_MS_TO_TICKS(300)); + } else { + while (digit--) { + mid(AO_MS_TO_TICKS(300)); + pause(AO_MS_TO_TICKS(300)); + } + } + pause(AO_MS_TO_TICKS(1000)); +} + +void +ao_report_altitude(void) +{ + __pdata alt_t agl = ao_max_height; + static __xdata uint8_t digits[11]; + __pdata uint8_t ndigits, i; + + if (agl < 0) + agl = 0; + ndigits = 0; + do { + digits[ndigits++] = agl % 10; + agl /= 10; + } while (agl); + + i = ndigits; + do + ao_report_digit(digits[--i]); + while (i != 0); +} diff --git a/src/kernel/ao_rssi.c b/src/kernel/ao_rssi.c new file mode 100644 index 00000000..244a84fe --- /dev/null +++ b/src/kernel/ao_rssi.c @@ -0,0 +1,53 @@ +/* + * Copyright © 2009 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" + +static __xdata uint16_t ao_rssi_time; +static __pdata uint16_t ao_rssi_delay; +static __pdata uint8_t ao_rssi_led; + +void +ao_rssi(void) +{ + for (;;) { + while ((int16_t) (ao_time() - ao_rssi_time) > AO_SEC_TO_TICKS(3)) + ao_sleep(&ao_rssi_time); + ao_led_for(ao_rssi_led, AO_MS_TO_TICKS(100)); + ao_delay(ao_rssi_delay); + } +} + +void +ao_rssi_set(int rssi_value) +{ + if (rssi_value > 0) + rssi_value = 0; + ao_rssi_delay = AO_MS_TO_TICKS((-rssi_value) * 5); + ao_rssi_time = ao_time(); + ao_wakeup(&ao_rssi_time); +} + +__xdata struct ao_task ao_rssi_task; + +void +ao_rssi_init(uint8_t rssi_led) +{ + ao_rssi_led = rssi_led; + ao_rssi_delay = 0; + ao_add_task(&ao_rssi_task, ao_rssi, "rssi"); +} diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c new file mode 100644 index 00000000..34658951 --- /dev/null +++ b/src/kernel/ao_sample.c @@ -0,0 +1,372 @@ +/* + * 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. + */ + +#ifndef AO_FLIGHT_TEST +#include "ao.h" +#include +#endif + +#if HAS_GYRO +#include +#endif + +/* + * Current sensor values + */ + +#ifndef PRES_TYPE +#define PRES_TYPE int32_t +#define ALT_TYPE int32_t +#define ACCEL_TYPE int16_t +#endif + +__pdata uint16_t ao_sample_tick; /* time of last data */ +__pdata pres_t ao_sample_pres; +__pdata alt_t ao_sample_alt; +__pdata alt_t ao_sample_height; +#if HAS_ACCEL +__pdata accel_t ao_sample_accel; +#endif +#if HAS_GYRO +__pdata accel_t ao_sample_accel_along; +__pdata accel_t ao_sample_accel_across; +__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_orient; +#endif + +__data uint8_t ao_sample_data; + +/* + * Sensor calibration values + */ + +__pdata pres_t ao_ground_pres; /* startup pressure */ +__pdata alt_t ao_ground_height; /* MSL of ao_ground_pres */ + +#if HAS_ACCEL +__pdata accel_t ao_ground_accel; /* startup acceleration */ +__pdata accel_t ao_accel_2g; /* factory accel calibration */ +__pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */ +#endif + +#if HAS_GYRO +__pdata accel_t ao_ground_accel_along; +__pdata accel_t ao_ground_accel_across; +__pdata accel_t ao_ground_accel_through; +__pdata int32_t ao_ground_pitch; +__pdata int32_t ao_ground_yaw; +__pdata int32_t ao_ground_roll; +#endif + +static __pdata uint8_t ao_preflight; /* in preflight mode */ + +static __pdata uint16_t nsamples; +__pdata int32_t ao_sample_pres_sum; +#if HAS_ACCEL +__pdata int32_t ao_sample_accel_sum; +#endif +#if HAS_GYRO +__pdata int32_t ao_sample_accel_along_sum; +__pdata int32_t ao_sample_accel_across_sum; +__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; +#endif + +#if HAS_FLIGHT_DEBUG +extern uint8_t ao_orient_test; +#endif + +static void +ao_sample_preflight_add(void) +{ +#if HAS_ACCEL + ao_sample_accel_sum += ao_sample_accel; +#endif + ao_sample_pres_sum += ao_sample_pres; +#if HAS_GYRO + ao_sample_accel_along_sum += ao_sample_accel_along; + ao_sample_accel_across_sum += ao_sample_accel_across; + ao_sample_accel_through_sum += ao_sample_accel_through; + ao_sample_pitch_sum += ao_sample_pitch; + ao_sample_yaw_sum += ao_sample_yaw; + ao_sample_roll_sum += ao_sample_roll; +#endif + ++nsamples; +} + +static void +ao_sample_preflight_set(void) +{ +#if HAS_ACCEL + ao_ground_accel = ao_sample_accel_sum >> 9; + ao_sample_accel_sum = 0; +#endif + ao_ground_pres = ao_sample_pres_sum >> 9; + ao_ground_height = pres_to_altitude(ao_ground_pres); + ao_sample_pres_sum = 0; +#if HAS_GYRO + ao_ground_accel_along = ao_sample_accel_along_sum >> 9; + ao_ground_accel_across = ao_sample_accel_across_sum >> 9; + ao_ground_accel_through = ao_sample_accel_through_sum >> 9; + ao_ground_pitch = ao_sample_pitch_sum; + ao_ground_yaw = ao_sample_yaw_sum; + ao_ground_roll = ao_sample_roll_sum; + ao_sample_accel_along_sum = 0; + ao_sample_accel_across_sum = 0; + ao_sample_accel_through_sum = 0; + ao_sample_pitch_sum = 0; + ao_sample_yaw_sum = 0; + ao_sample_roll_sum = 0; + ao_sample_orient = 0; + + struct ao_quaternion orient; + + /* Take the pad IMU acceleration values and compute our current direction + */ + + ao_quaternion_init_vector(&orient, + (ao_ground_accel_across - ao_config.accel_zero_across), + (ao_ground_accel_through - ao_config.accel_zero_through), + (ao_ground_accel_along - ao_config.accel_zero_along)); + + ao_quaternion_normalize(&orient, + &orient); + + /* Here's up */ + + struct ao_quaternion up = { .r = 0, .x = 0, .y = 0, .z = 1 }; + + if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP) + up.z = -1; + + /* Compute rotation to get from up to our current orientation, set + * that as the current rotation vector + */ + ao_quaternion_vectors_to_rotation(&ao_rotation, &up, &orient); +#if HAS_FLIGHT_DEBUG + if (ao_orient_test) + printf("\n\treset\n"); +#endif +#endif + nsamples = 0; +} + +#if HAS_GYRO + +#define TIME_DIV 200.0f + +static void +ao_sample_rotate(void) +{ +#ifdef AO_FLIGHT_TEST + float dt = (ao_sample_tick - ao_sample_prev_tick) / TIME_DIV; +#else + static const float dt = 1/TIME_DIV; +#endif + float x = ao_mpu6000_gyro((float) ((ao_sample_pitch << 9) - ao_ground_pitch) / 512.0f) * dt; + float y = ao_mpu6000_gyro((float) ((ao_sample_yaw << 9) - ao_ground_yaw) / 512.0f) * dt; + float z = ao_mpu6000_gyro((float) ((ao_sample_roll << 9) - ao_ground_roll) / 512.0f) * dt; + struct ao_quaternion rot; + + ao_quaternion_init_half_euler(&rot, x, y, z); + ao_quaternion_multiply(&ao_rotation, &rot, &ao_rotation); + + /* 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. + * + * rot = ao_rotation * vertical * ao_rotation° + * rot = ao_rotation * (0,0,0,1) * ao_rotation° + * = ((a.z, a.y, -a.x, a.r) * (a.r, -a.x, -a.y, -a.z)) .z + * + * = (-a.z * -a.z) + (a.y * -a.y) - (-a.x * -a.x) + (a.r * a.r) + * = a.z² - a.y² - a.x² + a.r² + * + * rot = ao_rotation * (0, 0, 0, -1) * ao_rotation° + * = ((-a.z, -a.y, a.x, -a.r) * (a.r, -a.x, -a.y, -a.z)) .z + * + * = (a.z * -a.z) + (-a.y * -a.y) - (a.x * -a.x) + (-a.r * a.r) + * = -a.z² + a.y² + a.x² - a.r² + */ + + float rotz; + rotz = ao_rotation.z * ao_rotation.z - ao_rotation.y * ao_rotation.y - ao_rotation.x * ao_rotation.x + ao_rotation.r * ao_rotation.r; + + ao_sample_orient = acosf(rotz) * (float) (180.0/M_PI); + +#if HAS_FLIGHT_DEBUG + if (ao_orient_test) { + printf ("rot %d %d %d orient %d \r", + (int) (x * 1000), + (int) (y * 1000), + (int) (z * 1000), + ao_sample_orient); + } +#endif + +} +#endif + +static void +ao_sample_preflight(void) +{ + /* startup state: + * + * Collect 512 samples of acceleration and pressure + * data and average them to find the resting values + */ + if (nsamples < 512) { + ao_sample_preflight_add(); + } else { +#if HAS_ACCEL + ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g; + ao_accel_scale = to_fix32(GRAVITY * 2 * 16) / ao_accel_2g; +#endif + ao_sample_preflight_set(); + ao_preflight = FALSE; + } +} + +/* + * While in pad mode, constantly update the ground state by + * re-averaging the data. This tracks changes in orientation, which + * might be caused by adjustments to the rocket on the pad and + * pressure, which might be caused by changes in the weather. + */ + +static void +ao_sample_preflight_update(void) +{ + if (nsamples < 512) + ao_sample_preflight_add(); + else if (nsamples < 1024) + ++nsamples; + else + ao_sample_preflight_set(); +} + +#if 0 +#if HAS_GYRO +static int32_t p_filt; +static int32_t y_filt; + +static gyro_t inline ao_gyro(void) { + gyro_t p = ao_sample_pitch - ao_ground_pitch; + gyro_t y = ao_sample_yaw - ao_ground_yaw; + + p_filt = p_filt - (p_filt >> 6) + p; + y_filt = y_filt - (y_filt >> 6) + y; + + p = p_filt >> 6; + y = y_filt >> 6; + return ao_sqrt(p*p + y*y); +} +#endif +#endif + +uint8_t +ao_sample(void) +{ + ao_wakeup(DATA_TO_XDATA(&ao_sample_data)); + ao_sleep((void *) DATA_TO_XDATA(&ao_data_head)); + while (ao_sample_data != ao_data_head) { + __xdata struct ao_data *ao_data; + + /* Capture a sample */ + ao_data = (struct ao_data *) &ao_data_ring[ao_sample_data]; + ao_sample_tick = ao_data->tick; + +#if HAS_BARO + ao_data_pres_cook(ao_data); + ao_sample_pres = ao_data_pres(ao_data); + ao_sample_alt = pres_to_altitude(ao_sample_pres); + ao_sample_height = ao_sample_alt - ao_ground_height; +#endif + +#if HAS_ACCEL + ao_sample_accel = ao_data_accel_cook(ao_data); + if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP) + ao_sample_accel = ao_data_accel_invert(ao_sample_accel); + ao_data_set_accel(ao_data, ao_sample_accel); +#endif +#if HAS_GYRO + ao_sample_accel_along = ao_data_along(ao_data); + ao_sample_accel_across = ao_data_across(ao_data); + ao_sample_accel_through = ao_data_through(ao_data); + ao_sample_pitch = ao_data_pitch(ao_data); + ao_sample_yaw = ao_data_yaw(ao_data); + ao_sample_roll = ao_data_roll(ao_data); +#endif + + if (ao_preflight) + ao_sample_preflight(); + else { + if (ao_flight_state < ao_flight_boost) + ao_sample_preflight_update(); + ao_kalman(); +#if HAS_GYRO + 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; +} + +void +ao_sample_init(void) +{ + ao_config_get(); + nsamples = 0; + ao_sample_pres_sum = 0; + ao_sample_pres = 0; +#if HAS_ACCEL + ao_sample_accel_sum = 0; + ao_sample_accel = 0; +#endif +#if HAS_GYRO + ao_sample_accel_along_sum = 0; + ao_sample_accel_across_sum = 0; + ao_sample_accel_through_sum = 0; + ao_sample_accel_along = 0; + ao_sample_accel_across = 0; + ao_sample_accel_through = 0; + ao_sample_pitch_sum = 0; + ao_sample_yaw_sum = 0; + ao_sample_roll_sum = 0; + ao_sample_pitch = 0; + ao_sample_yaw = 0; + ao_sample_roll = 0; + ao_sample_orient = 0; +#endif + ao_sample_data = ao_data_head; + ao_preflight = TRUE; +} diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h new file mode 100644 index 00000000..16d4c507 --- /dev/null +++ b/src/kernel/ao_sample.h @@ -0,0 +1,156 @@ +/* + * 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. + */ + +#ifndef _AO_SAMPLE_H_ +#define _AO_SAMPLE_H_ + +#include + +/* + * ao_sample.c + */ + +/* + * Barometer calibration + * + * We directly sample the barometer. The specs say: + * + * Pressure range: 15-115 kPa + * Voltage at 115kPa: 2.82 + * Output scale: 27mV/kPa + * + * If we want to detect launch with the barometer, we need + * a large enough bump to not be fooled by noise. At typical + * launch elevations (0-2000m), a 200Pa pressure change cooresponds + * to about a 20m elevation change. This is 5.4mV, or about 3LSB. + * As all of our calculations are done in 16 bits, we'll actually see a change + * of 16 times this though + * + * 27 mV/kPa * 32767 / 3300 counts/mV = 268.1 counts/kPa + */ + +/* Accelerometer calibration + * + * We're sampling the accelerometer through a resistor divider which + * consists of 5k and 10k resistors. This multiplies the values by 2/3. + * That goes into the cc1111 A/D converter, which is running at 11 bits + * of precision with the bits in the MSB of the 16 bit value. Only positive + * values are used, so values should range from 0-32752 for 0-3.3V. The + * specs say we should see 40mV/g (uncalibrated), multiply by 2/3 for what + * the A/D converter sees (26.67 mV/g). We should see 32752/3300 counts/mV, + * for a final computation of: + * + * 26.67 mV/g * 32767/3300 counts/mV = 264.8 counts/g + * + * Zero g was measured at 16000 (we would expect 16384). + * Note that this value is only require to tell if the + * rocket is standing upright. Once that is determined, + * the value of the accelerometer is averaged for 100 samples + * to find the resting accelerometer value, which is used + * for all further flight computations + */ + +/* + * Above this height, the baro sensor doesn't work + */ +#if HAS_MS5607 +#define AO_MAX_BARO_HEIGHT 30000 +#else +#define AO_MAX_BARO_HEIGHT 12000 +#endif + +/* + * Above this speed, baro measurements are unreliable + */ +#define AO_MAX_BARO_SPEED 200 + +#define ACCEL_NOSE_UP (ao_accel_2g >> 2) + +/* + * Speed and acceleration are scaled by 16 to provide a bit more + * resolution while still having reasonable range. Note that this + * limits speed to 2047m/s (around mach 6) and acceleration to + * 2047m/s² (over 200g) + */ + +#define AO_M_TO_HEIGHT(m) ((int16_t) (m)) +#define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16)) +#define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16)) + +extern __pdata uint16_t ao_sample_tick; /* time of last data */ +extern __data uint8_t ao_sample_adc; /* Ring position of last processed sample */ +extern __data uint8_t ao_sample_data; /* Ring position of last processed sample */ + +#if HAS_BARO +extern __pdata pres_t ao_sample_pres; /* most recent pressure sensor reading */ +extern __pdata alt_t ao_sample_alt; /* MSL of ao_sample_pres */ +extern __pdata alt_t ao_sample_height; /* AGL of ao_sample_pres */ +extern __pdata pres_t ao_ground_pres; /* startup pressure */ +extern __pdata alt_t ao_ground_height; /* MSL of ao_ground_pres */ +#endif + +#if HAS_ACCEL +extern __pdata accel_t ao_sample_accel; /* most recent accel sensor reading */ +extern __pdata accel_t ao_ground_accel; /* startup acceleration */ +extern __pdata accel_t ao_accel_2g; /* factory accel calibration */ +extern __pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */ +#endif +#if HAS_GYRO +extern __pdata accel_t ao_ground_accel_along; +extern __pdata accel_t ao_ground_accel_across; +extern __pdata accel_t ao_ground_accel_through; +extern __pdata int32_t ao_ground_pitch; /* * 512 */ +extern __pdata int32_t ao_ground_yaw; /* * 512 */ +extern __pdata int32_t ao_ground_roll; /* * 512 */ +extern __pdata accel_t ao_sample_accel_along; +extern __pdata accel_t ao_sample_accel_across; +extern __pdata accel_t ao_sample_accel_through; +extern __pdata gyro_t ao_sample_roll; +extern __pdata gyro_t ao_sample_pitch; +extern __pdata gyro_t ao_sample_yaw; +extern __pdata angle_t ao_sample_orient; +#endif + +void ao_sample_init(void); + +/* returns FALSE in preflight mode, TRUE in flight mode */ +uint8_t ao_sample(void); + +/* + * ao_kalman.c + */ + +#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) +#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) +#define from_fix(x) ((x) >> 16) + +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 __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; + +#if HAS_ACCEL +extern __pdata int16_t ao_error_a; +#endif + +void ao_kalman(void); + +#endif /* _AO_SAMPLE_H_ */ diff --git a/src/kernel/ao_sample_profile.c b/src/kernel/ao_sample_profile.c new file mode 100644 index 00000000..d3743d12 --- /dev/null +++ b/src/kernel/ao_sample_profile.c @@ -0,0 +1,173 @@ +/* + * 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 + +#ifndef AO_SAMPLE_PROFILE_LOW_PC +#define AO_SAMPLE_PROFILE_LOW_PC 0x08002000 +#endif + +#ifndef AO_SAMPLE_PROFILE_HIGH_PC +#define AO_SAMPLE_PROFILE_HIGH_PC 0x0800f000 +#endif + +#ifndef AO_SAMPLE_PROFILE_SHIFT +#define AO_SAMPLE_PROFILE_SHIFT 6 +#endif + +#define AO_SAMPLE_PROFILE_RANGE (AO_SAMPLE_PROFILE_HIGH_PC - AO_SAMPLE_PROFILE_LOW_PC) +#define AO_SAMPLE_PROFILE_NUM (AO_SAMPLE_PROFILE_RANGE >> AO_SAMPLE_PROFILE_SHIFT) + +static uint16_t prev_tick; +static uint16_t samples[AO_SAMPLE_PROFILE_NUM]; +static uint8_t missed[AO_SAMPLE_PROFILE_NUM/8]; +static uint16_t max_miss; +static uint32_t task, isr, os, idle; + +extern uint8_t ao_idle_loc; + +void +ao_sample_profile_point(uint32_t pc, uint16_t tick, uint8_t in_isr) +{ + uint16_t delta = tick - prev_tick; + + if (pc < AO_SAMPLE_PROFILE_LOW_PC) + return; + if (pc >= AO_SAMPLE_PROFILE_HIGH_PC) + return; + if (ao_cur_task) { + uint8_t *sp; + int32_t sp_delta; + + asm("mov %0,sp" : "=&r" (sp)); + sp_delta = sp - (uint8_t *) ao_cur_task->stack; + if (-96 < sp_delta && sp_delta < 16) + ao_panic(AO_PANIC_STACK); + } + + if (in_isr) + isr += delta; + else if (ao_cur_task) { + ao_cur_task->ticks += delta; + task += delta; + } else if (pc == (uint32_t) &ao_idle_loc) + idle += delta; + else + os += delta; + + pc -= AO_SAMPLE_PROFILE_LOW_PC; + pc >>= AO_SAMPLE_PROFILE_SHIFT; + samples[pc] += delta; + + if (delta > 1) + missed[pc >> 3] |= (1 << (pc & 7)); + if (delta > max_miss) + max_miss = delta; + prev_tick = tick; +} + +static void +ao_sample_profile_start(void) +{ + prev_tick = ao_sample_profile_timer_start(); +} + +static void +ao_sample_profile_stop(void) +{ + ao_sample_profile_timer_stop(); +} + +static void +ao_sample_profile_dump(void) +{ + uint16_t a; + uint8_t t; + + printf ("task %6d\n", task); + printf ("isr %6d\n", isr); + printf ("os %6d\n", os); + printf ("idle %6d\n", idle); + printf ("irq blocked %d\n", max_miss); + for (t = 0; t < ao_num_tasks; t++) + printf ("task %6d %6d %6d %s\n", + ao_tasks[t]->ticks, + ao_tasks[t]->yields, + ao_tasks[t]->max_run, + ao_tasks[t]->name); + for (a = 0; a < AO_SAMPLE_PROFILE_NUM; a++) { + if (samples[a]) + printf ("%04x %c %u\n", + (a << AO_SAMPLE_PROFILE_SHIFT) + AO_SAMPLE_PROFILE_LOW_PC, + missed[a >> 3] & (1 << (a & 7)) ? '*' : ' ', + samples[a]); + } +} + +static void +ao_sample_profile_clear(void) +{ + int t; + + task = isr = os = idle = 0; + max_miss = 0; + memset(samples, '\0', sizeof (samples)); + memset(missed, '\0', sizeof (missed)); + for (t = 0; t < ao_num_tasks; t++) { + ao_tasks[t]->ticks = 0; + ao_tasks[t]->yields = 0; + ao_tasks[t]->max_run = 0; + } +} + +static void +ao_sample_profile_cmd(void) +{ + ao_cmd_white(); + switch (ao_cmd_lex_c) { + case '1': + ao_sample_profile_start(); + break; + case '0': + ao_sample_profile_stop(); + break; + case 'd': + ao_sample_profile_dump(); + break; + case 'c': + ao_sample_profile_clear(); + break; + default: + ao_cmd_status = ao_cmd_syntax_error; + break; + } +} + +static __code struct ao_cmds ao_sample_profile_cmds[] = { + { ao_sample_profile_cmd, "S <1 start,0 stop, d dump,c clear>\0Sample profile" }, + { 0, NULL } +}; + +void +ao_sample_profile_init(void) +{ + ao_sample_profile_timer_init(); + ao_cmd_register(&ao_sample_profile_cmds[0]); + ao_sample_profile_clear(); +} diff --git a/src/kernel/ao_sample_profile.h b/src/kernel/ao_sample_profile.h new file mode 100644 index 00000000..dbc29d3d --- /dev/null +++ b/src/kernel/ao_sample_profile.h @@ -0,0 +1,29 @@ +/* + * 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. + */ + +#ifndef _AO_SAMPLE_PROFILE_H_ +#define _AO_SAMPLE_PROFILE_H_ + +#include + +void +ao_sample_profile_point(uint32_t pc, uint16_t tick, uint8_t in_isr); + +void +ao_sample_profile_init(void); + +#endif /* _AO_SAMPLE_PROFILE_H_ */ diff --git a/src/kernel/ao_send_packet.c b/src/kernel/ao_send_packet.c new file mode 100644 index 00000000..66315d22 --- /dev/null +++ b/src/kernel/ao_send_packet.c @@ -0,0 +1,58 @@ +/* + * 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 "ao.h" + +#define AO_MAX_SEND 128 + +static __xdata uint8_t ao_send[AO_MAX_SEND]; + +static void +ao_send_packet(void) +{ + __pdata uint16_t count; + uint8_t b; + __pdata uint8_t i; + + ao_cmd_hex(); + count = ao_cmd_lex_i; + if (ao_cmd_status != ao_cmd_success) + return; + if (count > AO_MAX_SEND - 2) { + ao_cmd_status = ao_cmd_syntax_error; + return; + } + for (i = 0; i < count; i++) { + b = ao_getnibble() << 4; + b |= ao_getnibble(); + if (ao_cmd_status != ao_cmd_success) + return; + ao_send[i] = b; + } + ao_radio_send(ao_send, count); +} + +static __code struct ao_cmds ao_send_packet_cmds[] = { + { ao_send_packet, "S \0Send packet. Data on next line" }, + { 0, NULL } +}; + +void +ao_send_packet_init(void) +{ + ao_cmd_register(&ao_send_packet_cmds[0]); +} diff --git a/src/kernel/ao_send_packet.h b/src/kernel/ao_send_packet.h new file mode 100644 index 00000000..526f7b55 --- /dev/null +++ b/src/kernel/ao_send_packet.h @@ -0,0 +1,24 @@ +/* + * 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. + */ + +#ifndef _AO_SEND_PACKET_H_ +#define _AO_SEND_PACKET_H_ + +void +ao_send_packet_init(void); + +#endif /* _AO_SEND_PACKET_H_ */ diff --git a/src/kernel/ao_serial.h b/src/kernel/ao_serial.h new file mode 100644 index 00000000..baf213c0 --- /dev/null +++ b/src/kernel/ao_serial.h @@ -0,0 +1,110 @@ +/* + * 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. + */ + +#ifndef _AO_SERIAL_H_ +#define _AO_SERIAL_H_ + +#define AO_SERIAL_SPEED_4800 0 +#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; +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); + +void +ao_serial0_drain(void); + +void +ao_serial0_set_speed(uint8_t speed); +#endif + +#if HAS_SERIAL_1 +extern volatile __xdata struct ao_fifo ao_serial1_rx_fifo; +extern volatile __xdata struct ao_fifo ao_serial1_tx_fifo; + +char +ao_serial1_getchar(void); + +int +_ao_serial1_pollchar(void); + +void +ao_serial1_putchar(char c); + +void +ao_serial1_drain(void); + +void +ao_serial1_set_speed(uint8_t speed); +#endif + +#if HAS_SERIAL_2 +extern volatile __xdata struct ao_fifo ao_serial2_rx_fifo; +extern volatile __xdata struct ao_fifo ao_serial2_tx_fifo; + +char +ao_serial2_getchar(void); + +int +_ao_serial2_pollchar(void); + +void +ao_serial2_putchar(char c); + +void +ao_serial2_drain(void); + +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); + +#endif /* _AO_SERIAL_H_ */ diff --git a/src/kernel/ao_sqrt.c b/src/kernel/ao_sqrt.c new file mode 100644 index 00000000..3a550eaa --- /dev/null +++ b/src/kernel/ao_sqrt.c @@ -0,0 +1,48 @@ +/* + * 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. + */ + +#ifndef AO_FLIGHT_TEST +#include "ao.h" +#endif + +/* Adapted from int_sqrt.c in the linux kernel, which is licensed GPLv2 */ +/** + * int_sqrt - rough approximation to sqrt + * @x: integer of which to calculate the sqrt + * + * A very rough approximation to the sqrt() function. + */ + +uint32_t +ao_sqrt(uint32_t op) +{ + uint32_t res = 0; + uint32_t one = 1UL << (sizeof (one) * 8 - 2); + + while (one > op) + one >>= 2; + + while (one != 0) { + if (op >= res + one) { + op = op - (res + one); + res = res + 2 * one; + } + res /= 2; + one /= 4; + } + return res; +} diff --git a/src/kernel/ao_state.c b/src/kernel/ao_state.c new file mode 100644 index 00000000..ed197aa5 --- /dev/null +++ b/src/kernel/ao_state.c @@ -0,0 +1,23 @@ +/* + * Copyright © 2009 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" + +const char const * const ao_state_names[] = { + "startup", "idle", "pad", "boost", "fast", + "coast", "drogue", "main", "landed", "invalid" +}; diff --git a/src/kernel/ao_stdio.c b/src/kernel/ao_stdio.c new file mode 100644 index 00000000..99118137 --- /dev/null +++ b/src/kernel/ao_stdio.c @@ -0,0 +1,158 @@ +/* + * Copyright © 2009 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" + +/* + * Basic I/O functions to support SDCC stdio package + */ + +#ifndef USE_SERIAL_0_STDIN +#define USE_SERIAL_0_STDIN 0 +#endif +#ifndef USE_SERIAL_1_STDIN +#define USE_SERIAL_1_STDIN 0 +#endif +#ifndef USE_SERIAL_2_STDIN +#define USE_SERIAL_2_STDIN 0 +#endif +#ifndef USE_SERIAL_3_STDIN +#define USE_SERIAL_3_STDIN 0 +#endif +#ifndef USE_SERIAL_4_STDIN +#define USE_SERIAL_4_STDIN 0 +#endif +#ifndef USE_SERIAL_5_STDIN +#define USE_SERIAL_5_STDIN 0 +#endif +#ifndef USE_SERIAL_6_STDIN +#define USE_SERIAL_6_STDIN 0 +#endif +#ifndef USE_SERIAL_7_STDIN +#define USE_SERIAL_7_STDIN 0 +#endif +#ifndef USE_SERIAL_8_STDIN +#define USE_SERIAL_8_STDIN 0 +#endif +#ifndef USE_SERIAL_9_STDIN +#define USE_SERIAL_9_STDIN 0 +#endif +#ifndef PACKET_HAS_SLAVE +#define PACKET_HAS_SLAVE 0 +#endif + +#define USE_SERIAL_STDIN (USE_SERIAL_0_STDIN + \ + USE_SERIAL_1_STDIN + \ + USE_SERIAL_2_STDIN + \ + USE_SERIAL_3_STDIN + \ + USE_SERIAL_4_STDIN + \ + USE_SERIAL_5_STDIN + \ + USE_SERIAL_6_STDIN + \ + USE_SERIAL_7_STDIN + \ + USE_SERIAL_8_STDIN + \ + USE_SERIAL_9_STDIN) + +#define AO_NUM_STDIOS (HAS_USB + PACKET_HAS_SLAVE + USE_SERIAL_STDIN) + +__xdata struct ao_stdio ao_stdios[AO_NUM_STDIOS]; + +#if AO_NUM_STDIOS > 1 +__pdata int8_t ao_cur_stdio; +__pdata int8_t ao_num_stdios; +#else +__pdata int8_t ao_cur_stdio; +#define ao_cur_stdio 0 +#define ao_num_stdios 0 +#endif + +void +putchar(char c) +{ +#if LOW_LEVEL_DEBUG + if (!ao_cur_task) { + extern void ao_debug_out(char c); + if (c == '\n') + ao_debug_out('\r'); + ao_debug_out(c); + return; + } +#endif + if (c == '\n') + (*ao_stdios[ao_cur_stdio].putchar)('\r'); + (*ao_stdios[ao_cur_stdio].putchar)(c); +} + +void +flush(void) +{ + if (ao_stdios[ao_cur_stdio].flush) + ao_stdios[ao_cur_stdio].flush(); +} + +__xdata uint8_t ao_stdin_ready; + +char +getchar(void) __reentrant +{ + int c; + int8_t stdio; + + ao_arch_block_interrupts(); + stdio = ao_cur_stdio; + for (;;) { + c = ao_stdios[stdio]._pollchar(); + if (c != AO_READ_AGAIN) + break; +#if AO_NUM_STDIOS > 1 + if (++stdio == ao_num_stdios) + stdio = 0; + if (stdio == ao_cur_stdio) +#endif + ao_sleep(&ao_stdin_ready); + } +#if AO_NUM_STDIOS > 1 + ao_cur_stdio = stdio; +#endif + ao_arch_release_interrupts(); + return c; +} + +uint8_t +ao_echo(void) +{ + return ao_stdios[ao_cur_stdio].echo; +} + +int8_t +ao_add_stdio(int (*_pollchar)(void), + void (*putchar)(char), + void (*flush)(void)) __reentrant +{ +#if AO_NUM_STDIOS > 1 + if (ao_num_stdios == AO_NUM_STDIOS) + ao_panic(AO_PANIC_STDIO); +#endif + ao_stdios[ao_num_stdios]._pollchar = _pollchar; + ao_stdios[ao_num_stdios].putchar = putchar; + ao_stdios[ao_num_stdios].flush = flush; + ao_stdios[ao_num_stdios].echo = 1; +#if AO_NUM_STDIOS > 1 + return ao_num_stdios++; +#else + return 0; +#endif +} diff --git a/src/kernel/ao_storage.c b/src/kernel/ao_storage.c new file mode 100644 index 00000000..6eddae7f --- /dev/null +++ b/src/kernel/ao_storage.c @@ -0,0 +1,186 @@ +/* + * 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 +#include + +uint8_t +ao_storage_read(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant +{ + uint16_t this_len; + uint16_t this_off; + + ao_storage_setup(); + if (pos >= ao_storage_total || pos + len > ao_storage_total) + return 0; + while (len) { + + /* Compute portion of transfer within + * a single block + */ + this_off = (uint16_t) pos & (ao_storage_unit - 1); + this_len = ao_storage_unit - this_off; + if (this_len > len) + this_len = len; + + if (!ao_storage_device_read(pos, buf, this_len)) + return 0; + + /* See how much is left */ + buf += this_len; + len -= this_len; + pos += this_len; + } + return 1; +} + +uint8_t +ao_storage_write(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant +{ + uint16_t this_len; + uint16_t this_off; + + ao_storage_setup(); + if (pos >= ao_storage_total || pos + len > ao_storage_total) + return 0; + while (len) { + + /* Compute portion of transfer within + * a single block + */ + this_off = (uint16_t) pos & (ao_storage_unit - 1); + this_len = ao_storage_unit - this_off; + if (this_len > len) + this_len = len; + + if (!ao_storage_device_write(pos, buf, this_len)) + return 0; + + /* See how much is left */ + buf += this_len; + len -= this_len; + pos += this_len; + } + return 1; +} + +static __xdata uint8_t storage_data[8]; + +static void +ao_storage_dump(void) __reentrant +{ + uint8_t i, j; + + ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + for (i = 0; ; i += 8) { + if (ao_storage_read(((uint32_t) (ao_cmd_lex_i) << 8) + i, + storage_data, + 8)) { + ao_cmd_put16((uint16_t) i); + for (j = 0; j < 8; j++) { + putchar(' '); + ao_cmd_put8(storage_data[j]); + } + putchar ('\n'); + } + if (i == 248) + break; + } +} + +#if HAS_STORAGE_DEBUG + +/* not enough space for this today + */ +static void +ao_storage_store(void) __reentrant +{ + uint16_t block; + uint8_t i; + uint16_t len; + static __xdata uint8_t b; + uint32_t addr; + + ao_cmd_hex(); + block = ao_cmd_lex_i; + ao_cmd_hex(); + i = ao_cmd_lex_i; + addr = ((uint32_t) block << 8) | i; + ao_cmd_hex(); + len = ao_cmd_lex_i; + if (ao_cmd_status != ao_cmd_success) + return; + while (len--) { + ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + b = ao_cmd_lex_i; + ao_storage_write(addr, &b, 1); + addr++; + } +} +#endif + +void +ao_storage_zap(void) __reentrant +{ + ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + ao_storage_erase((uint32_t) ao_cmd_lex_i << 8); +} + +void +ao_storage_zapall(void) __reentrant +{ + uint32_t pos; + + ao_cmd_white(); + if (!ao_match_word("DoIt")) + return; + for (pos = 0; pos < ao_storage_log_max; pos += ao_storage_block) + ao_storage_erase(pos); +} + +void +ao_storage_info(void) __reentrant +{ + ao_storage_setup(); + printf("Storage size: %ld\n", (long) ao_storage_total); + printf("Storage erase unit: %ld\n", (long) ao_storage_block); + ao_storage_device_info(); +} + +__code struct ao_cmds ao_storage_cmds[] = { + { ao_storage_info, "f\0Show storage" }, + { ao_storage_dump, "e \0Dump flash" }, +#if HAS_STORAGE_DEBUG + { ao_storage_store, "w ...\0Write data to flash" }, +#endif + { ao_storage_zap, "z \0Erase " }, + { ao_storage_zapall,"Z \0Erase all. is doit with D&I" }, + { 0, NULL }, +}; + +void +ao_storage_init(void) +{ + ao_storage_device_init(); + ao_cmd_register(&ao_storage_cmds[0]); +} diff --git a/src/kernel/ao_storage.h b/src/kernel/ao_storage.h new file mode 100644 index 00000000..6cc6fcb7 --- /dev/null +++ b/src/kernel/ao_storage.h @@ -0,0 +1,98 @@ +/* + * 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. + */ + +#ifndef _AO_STORAGE_H_ +#define _AO_STORAGE_H_ + +/* + * Storage interface, provided by one of the eeprom or flash + * drivers + */ + +#ifndef ao_storage_pos_t +#define ao_storage_pos_t uint32_t +#endif + +typedef ao_storage_pos_t ao_pos_t; + +/* Total bytes of available storage */ +extern __pdata ao_pos_t ao_storage_total; + +/* Block size - device is erased in these units. At least 256 bytes */ +extern __pdata ao_pos_t ao_storage_block; + +#ifndef USE_STORAGE_CONFIG +#define USE_STORAGE_CONFIG 1 +#endif + +#if USE_STORAGE_CONFIG +/* Byte offset of config block. Will be ao_storage_block bytes long */ +extern __pdata ao_pos_t ao_storage_config; + +#define ao_storage_log_max ao_storage_config +#else +#define ao_storage_log_max ao_storage_total +#endif + +/* Storage unit size - device reads and writes must be within blocks of this size. Usually 256 bytes. */ +extern __pdata uint16_t ao_storage_unit; + +/* Initialize above values. Can only be called once the OS is running */ +void +ao_storage_setup(void) __reentrant; + +/* Write data. Returns 0 on failure, 1 on success */ +uint8_t +ao_storage_write(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant; + +/* Read data. Returns 0 on failure, 1 on success */ +uint8_t +ao_storage_read(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant; + +/* Erase a block of storage. This always clears ao_storage_block bytes */ +uint8_t +ao_storage_erase(ao_pos_t pos) __reentrant; + +/* Flush any pending writes to stable storage */ +void +ao_storage_flush(void) __reentrant; + +/* Initialize the storage code */ +void +ao_storage_init(void); + +/* + * Low-level functions wrapped by ao_storage.c + */ + +/* Read data within a storage unit */ +uint8_t +ao_storage_device_read(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant; + +/* Write data within a storage unit */ +uint8_t +ao_storage_device_write(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant; + +/* Initialize low-level device bits */ +void +ao_storage_device_init(void); + +/* Print out information about flash chips */ +void +ao_storage_device_info(void) __reentrant; + +#endif /* _AO_STORAGE_H_ */ diff --git a/src/kernel/ao_task.c b/src/kernel/ao_task.c new file mode 100644 index 00000000..bafb4943 --- /dev/null +++ b/src/kernel/ao_task.c @@ -0,0 +1,548 @@ +/* + * Copyright © 2009 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 +#if HAS_SAMPLE_PROFILE +#include +#endif +#if HAS_STACK_GUARD +#include +#endif + +#define DEBUG 0 + +#define AO_NO_TASK_INDEX 0xff + +__xdata struct ao_task * __xdata ao_tasks[AO_NUM_TASKS]; +__data uint8_t ao_num_tasks; +__xdata struct ao_task *__data ao_cur_task; + +#if !HAS_TASK_QUEUE +static __data uint8_t ao_cur_task_index; +#endif + +#ifdef ao_arch_task_globals +ao_arch_task_globals +#endif + +#define AO_CHECK_STACK 0 + +#if AO_CHECK_STACK +static uint8_t in_yield; + +static inline void ao_check_stack(void) { + uint8_t q; + if (!in_yield && ao_cur_task && &q < &ao_cur_task->stack[0]) + ao_panic(AO_PANIC_STACK); +} +#else +#define ao_check_stack() +#endif + +#if HAS_TASK_QUEUE + +#define SLEEP_HASH_SIZE 17 + +static struct ao_list run_queue; +static struct ao_list alarm_queue; +static struct ao_list sleep_queue[SLEEP_HASH_SIZE]; + +static void +ao_task_to_run_queue(struct ao_task *task) +{ + ao_list_del(&task->queue); + ao_list_append(&task->queue, &run_queue); +} + +static struct ao_list * +ao_task_sleep_queue(void *wchan) +{ + return &sleep_queue[(uintptr_t) wchan % SLEEP_HASH_SIZE]; +} + +static void +ao_task_to_sleep_queue(struct ao_task *task, void *wchan) +{ + ao_list_del(&task->queue); + ao_list_append(&task->queue, ao_task_sleep_queue(wchan)); +} + +#if DEBUG +static void +ao_task_validate_alarm_queue(void) +{ + struct ao_task *alarm, *prev = NULL; + int i; + + if (ao_list_is_empty(&alarm_queue)) + return; + ao_list_for_each_entry(alarm, &alarm_queue, struct ao_task, alarm_queue) { + if (prev) { + if ((int16_t) (alarm->alarm - prev->alarm) < 0) { + ao_panic(1); + } + } + prev = alarm; + } + for (i = 0; i < ao_num_tasks; i++) { + alarm = ao_tasks[i]; + if (alarm->alarm) { + if (ao_list_is_empty(&alarm->alarm_queue)) + ao_panic(2); + } else { + if (!ao_list_is_empty(&alarm->alarm_queue)) + ao_panic(3); + } + } + if (ao_task_alarm_tick != ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm) + ao_panic(4); +} +#else +#define ao_task_validate_alarm_queue() +#endif + +uint16_t ao_task_alarm_tick; + +static void +ao_task_to_alarm_queue(struct ao_task *task) +{ + struct ao_task *alarm; + ao_list_for_each_entry(alarm, &alarm_queue, struct ao_task, alarm_queue) { + if ((int16_t) (alarm->alarm - task->alarm) >= 0) { + ao_list_insert(&task->alarm_queue, alarm->alarm_queue.prev); + ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm; + ao_task_validate_alarm_queue(); + return; + } + } + ao_list_append(&task->alarm_queue, &alarm_queue); + ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm; + ao_task_validate_alarm_queue(); +} + +static void +ao_task_from_alarm_queue(struct ao_task *task) +{ + ao_list_del(&task->alarm_queue); + if (ao_list_is_empty(&alarm_queue)) + ao_task_alarm_tick = 0; + else + ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm; + ao_task_validate_alarm_queue(); +} + +static void +ao_task_init_queue(struct ao_task *task) +{ + ao_list_init(&task->queue); + ao_list_init(&task->alarm_queue); +} + +static void +ao_task_exit_queue(struct ao_task *task) +{ + ao_list_del(&task->queue); + ao_list_del(&task->alarm_queue); +} + +void +ao_task_check_alarm(uint16_t tick) +{ + struct ao_task *alarm, *next; + + ao_list_for_each_entry_safe(alarm, next, &alarm_queue, struct ao_task, alarm_queue) { + if ((int16_t) (tick - alarm->alarm) < 0) + break; + alarm->alarm = 0; + ao_task_from_alarm_queue(alarm); + ao_task_to_run_queue(alarm); + } +} + +void +ao_task_init(void) +{ + uint8_t i; + ao_list_init(&run_queue); + ao_list_init(&alarm_queue); + ao_task_alarm_tick = 0; + for (i = 0; i < SLEEP_HASH_SIZE; i++) + ao_list_init(&sleep_queue[i]); +} + +#if DEBUG +static uint8_t +ao_task_validate_queue(struct ao_task *task) +{ + uint32_t flags; + struct ao_task *m; + uint8_t ret = 0; + struct ao_list *queue; + + flags = ao_arch_irqsave(); + if (task->wchan) { + queue = ao_task_sleep_queue(task->wchan); + ret |= 2; + } else { + queue = &run_queue; + ret |= 4; + } + ao_list_for_each_entry(m, queue, struct ao_task, queue) { + if (m == task) { + ret |= 1; + break; + } + } + ao_arch_irqrestore(flags); + return ret; +} + +static uint8_t +ao_task_validate_alarm(struct ao_task *task) +{ + uint32_t flags; + struct ao_task *m; + uint8_t ret = 0; + + flags = ao_arch_irqsave(); + if (task->alarm == 0) + return 0xff; + ao_list_for_each_entry(m, &alarm_queue, struct ao_task, alarm_queue) { + if (m == task) + ret |= 1; + else { + if (!(ret&1)) { + if ((int16_t) (m->alarm - task->alarm) > 0) + ret |= 2; + } else { + if ((int16_t) (task->alarm - m->alarm) > 0) + ret |= 4; + } + } + } + ao_arch_irqrestore(flags); + return ret; +} + + +static void +ao_task_validate(void) +{ + uint8_t i; + struct ao_task *task; + uint8_t ret; + + for (i = 0; i < ao_num_tasks; i++) { + task = ao_tasks[i]; + ret = ao_task_validate_queue(task); + if (!(ret & 1)) { + if (ret & 2) + printf ("sleeping task not on sleep queue %s %08x\n", + task->name, task->wchan); + else + printf ("running task not on run queue %s\n", + task->name); + } + ret = ao_task_validate_alarm(task); + if (ret != 0xff) { + if (!(ret & 1)) + printf ("alarm task not on alarm queue %s %d\n", + task->name, task->alarm); + if (ret & 2) + printf ("alarm queue has sooner entries after %s %d\n", + task->name, task->alarm); + if (ret & 4) + printf ("alarm queue has later entries before %s %d\n", + task->name, task->alarm); + } + } +} +#endif /* DEBUG */ + +#endif /* HAS_TASK_QUEUE */ + +void +ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *name) __reentrant +{ + uint8_t task_id; + uint8_t t; + if (ao_num_tasks == AO_NUM_TASKS) + ao_panic(AO_PANIC_NO_TASK); + for (task_id = 1; task_id != 0; task_id++) { + for (t = 0; t < ao_num_tasks; t++) + if (ao_tasks[t]->task_id == task_id) + break; + if (t == ao_num_tasks) + break; + } + task->task_id = task_id; + task->name = name; + task->wchan = NULL; + /* + * Construct a stack frame so that it will 'return' + * to the start of the task + */ + ao_arch_init_stack(task, start); + ao_arch_critical( +#if HAS_TASK_QUEUE + ao_task_init_queue(task); + ao_task_to_run_queue(task); +#endif + ao_tasks[ao_num_tasks] = task; + ao_num_tasks++; + ); +} + +__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 +{ + ao_arch_save_regs(); + +#if HAS_TASK_QUEUE + if (ao_cur_task == NULL) + ao_cur_task = ao_tasks[ao_num_tasks-1]; +#else + if (ao_cur_task_index == AO_NO_TASK_INDEX) + ao_cur_task_index = ao_num_tasks-1; +#endif + else + { +#if HAS_SAMPLE_PROFILE + uint16_t tick = ao_sample_profile_timer_value(); + uint16_t run = tick - ao_cur_task->start; + if (run > ao_cur_task->max_run) + ao_cur_task->max_run = run; + ++ao_cur_task->yields; +#endif + ao_arch_save_stack(); + } + + ao_arch_isr_stack(); +#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; +#endif + /* Find a task to run. If there isn't any runnable task, + * this loop will run forever, which is just fine + */ +#if HAS_TASK_QUEUE + /* If the current task is running, move it to the + * end of the queue to allow other tasks a chance + */ + if (ao_cur_task->wchan == NULL) + ao_task_to_run_queue(ao_cur_task); + ao_cur_task = NULL; + for (;;) { + ao_arch_memory_barrier(); + if (!ao_list_is_empty(&run_queue)) + break; + /* Wait for interrupts when there's nothing ready */ + ao_arch_wait_interrupt(); + } + ao_cur_task = ao_list_first_entry(&run_queue, struct ao_task, queue); +#else + { + __pdata uint8_t ao_last_task_index = ao_cur_task_index; + for (;;) { + ++ao_cur_task_index; + if (ao_cur_task_index == ao_num_tasks) + ao_cur_task_index = 0; + + ao_cur_task = ao_tasks[ao_cur_task_index]; + + /* Check for ready task */ + if (ao_cur_task->wchan == NULL) + break; + + /* Check if the alarm is set for a time which has passed */ + if (ao_cur_task->alarm && + (int16_t) (ao_time() - ao_cur_task->alarm) >= 0) + break; + + /* Wait for interrupts when there's nothing ready */ + if (ao_cur_task_index == ao_last_task_index && !ao_task_minimize_latency) + ao_arch_wait_interrupt(); + } + } +#endif +#if HAS_SAMPLE_PROFILE + ao_cur_task->start = ao_sample_profile_timer_value(); +#endif +#if HAS_STACK_GUARD + ao_mpu_stack_guard(ao_cur_task->stack); +#endif +#if AO_CHECK_STACK + in_yield = 0; +#endif + ao_arch_restore_stack(); +} + +uint8_t +ao_sleep(__xdata void *wchan) +{ +#if HAS_TASK_QUEUE + uint32_t flags; + flags = ao_arch_irqsave(); +#endif + ao_cur_task->wchan = wchan; +#if HAS_TASK_QUEUE + ao_task_to_sleep_queue(ao_cur_task, wchan); + ao_arch_irqrestore(flags); +#endif + ao_yield(); + if (ao_cur_task->wchan) { + ao_cur_task->wchan = NULL; + ao_cur_task->alarm = 0; + return 1; + } + return 0; +} + +void +ao_wakeup(__xdata void *wchan) __reentrant +{ +#if HAS_TASK_QUEUE + struct ao_task *sleep, *next; + struct ao_list *sleep_queue; + uint32_t flags; + + if (ao_num_tasks == 0) + return; + sleep_queue = ao_task_sleep_queue(wchan); + flags = ao_arch_irqsave(); + ao_list_for_each_entry_safe(sleep, next, sleep_queue, struct ao_task, queue) { + if (sleep->wchan == wchan) { + sleep->wchan = NULL; + ao_task_to_run_queue(sleep); + } + } + ao_arch_irqrestore(flags); +#else + uint8_t i; + for (i = 0; i < ao_num_tasks; i++) + if (ao_tasks[i]->wchan == wchan) + ao_tasks[i]->wchan = NULL; +#endif + ao_check_stack(); +} + +void +ao_alarm(uint16_t delay) +{ +#if HAS_TASK_QUEUE + uint32_t flags; + /* Make sure we sleep *at least* delay ticks, which means adding + * one to account for the fact that we may be close to the next tick + */ + flags = ao_arch_irqsave(); +#endif + if (!(ao_cur_task->alarm = ao_time() + delay + 1)) + ao_cur_task->alarm = 1; +#if HAS_TASK_QUEUE + ao_task_to_alarm_queue(ao_cur_task); + ao_arch_irqrestore(flags); +#endif +} + +void +ao_clear_alarm(void) +{ +#if HAS_TASK_QUEUE + uint32_t flags; + + flags = ao_arch_irqsave(); +#endif + ao_cur_task->alarm = 0; +#if HAS_TASK_QUEUE + ao_task_from_alarm_queue(ao_cur_task); + ao_arch_irqrestore(flags); +#endif +} + +static __xdata uint8_t ao_forever; + +void +ao_delay(uint16_t ticks) +{ + ao_alarm(ticks); + ao_sleep(&ao_forever); + ao_clear_alarm(); +} + +void +ao_exit(void) +{ + uint8_t i; + ao_arch_block_interrupts(); + ao_num_tasks--; +#if HAS_TASK_QUEUE + for (i = 0; i < ao_num_tasks; i++) + if (ao_tasks[i] == ao_cur_task) + break; + ao_task_exit_queue(ao_cur_task); +#else + i = ao_cur_task_index; + ao_cur_task_index = AO_NO_TASK_INDEX; +#endif + for (; i < ao_num_tasks; i++) + ao_tasks[i] = ao_tasks[i+1]; + ao_cur_task = NULL; + ao_yield(); + /* we'll never get back here */ +} + +#if HAS_TASK_INFO +void +ao_task_info(void) +{ + uint8_t i; + __xdata struct ao_task *task; + + for (i = 0; i < ao_num_tasks; i++) { + task = ao_tasks[i]; + printf("%12s: wchan %04x\n", + task->name, + (int) task->wchan); + } +#if HAS_TASK_QUEUE && DEBUG + ao_task_validate(); +#endif +} +#endif + +void +ao_start_scheduler(void) +{ +#if !HAS_TASK_QUEUE + ao_cur_task_index = AO_NO_TASK_INDEX; +#endif + ao_cur_task = NULL; +#if HAS_ARCH_START_SCHEDULER + ao_arch_start_scheduler(); +#endif + ao_yield(); +} diff --git a/src/kernel/ao_task.h b/src/kernel/ao_task.h new file mode 100644 index 00000000..9c56b480 --- /dev/null +++ b/src/kernel/ao_task.h @@ -0,0 +1,117 @@ +/* + * 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. + */ + +#ifndef _AO_TASK_H_ +#define _AO_TASK_H_ +#if HAS_TASK_QUEUE +#include +#endif + +#ifndef HAS_TASK_INFO +#define HAS_TASK_INFO 1 +#endif + +/* An AltOS task */ +struct ao_task { + __xdata void *wchan; /* current wait channel (NULL if running) */ + uint16_t alarm; /* abort ao_sleep time */ + ao_arch_task_members /* any architecture-specific fields */ + uint8_t task_id; /* unique id */ + __code char *name; /* task name */ +#if HAS_TASK_QUEUE + struct ao_list queue; + struct ao_list alarm_queue; +#endif + uint8_t stack[AO_STACK_SIZE]; /* saved stack */ +#if HAS_SAMPLE_PROFILE + uint32_t ticks; + uint32_t yields; + uint16_t start; + uint16_t max_run; +#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]; +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 + */ + +/* Suspend the current task until wchan is awoken. + * returns: + * 0 on normal wake + * 1 on alarm + */ +uint8_t +ao_sleep(__xdata void *wchan); + +/* Wake all tasks sleeping on wchan */ +void +ao_wakeup(__xdata void *wchan) __reentrant; + +/* set an alarm to go off in 'delay' ticks */ +void +ao_alarm(uint16_t delay); + +/* Clear any pending alarm */ +void +ao_clear_alarm(void); + +/* Yield the processor to another task */ +void +ao_yield(void) ao_arch_naked_declare; + +/* Add a task to the run queue */ +void +ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *name) __reentrant; + +#if HAS_TASK_QUEUE +/* Called on timer interrupt to check alarms */ +extern uint16_t ao_task_alarm_tick; +void +ao_task_check_alarm(uint16_t tick); +#endif + +/* Terminate the current task */ +void +ao_exit(void); + +/* Dump task info to console */ +void +ao_task_info(void); + +/* Start the scheduler. This will not return */ +void +ao_start_scheduler(void); + +#if HAS_TASK_QUEUE +void +ao_task_init(void); +#else +#define ao_task_init() +#endif + +#endif diff --git a/src/kernel/ao_telem.h b/src/kernel/ao_telem.h new file mode 100644 index 00000000..1a8da291 --- /dev/null +++ b/src/kernel/ao_telem.h @@ -0,0 +1,172 @@ +/* + * 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. + */ + +#ifndef _AO_TELEM_H_ +#define _AO_TELEM_H_ + +#define AO_TELEMETRY_VERSION 4 + +/* + * Telemetry version 4 and higher format: + * + * General header fields + * + * Name Value + * + * VERSION Telemetry version number (4 or more). Must be first. + * c Callsign (string, no spaces allowed) + * n Flight unit serial number (integer) + * f Flight number (integer) + * r Packet RSSI value (integer) + * s Flight computer state (string, no spaces allowed) + * t Flight computer clock (integer in centiseconds) + */ + +#define AO_TELEM_VERSION "VERSION" +#define AO_TELEM_CALL "c" +#define AO_TELEM_SERIAL "n" +#define AO_TELEM_FLIGHT "f" +#define AO_TELEM_RSSI "r" +#define AO_TELEM_STATE "s" +#define AO_TELEM_TICK "t" + +/* + * Raw sensor values + * + * Name Value + * r_a Accelerometer reading (integer) + * r_b Barometer reading (integer) + * r_t Thermometer reading (integer) + * r_v Battery reading (integer) + * r_d Drogue continuity (integer) + * r_m Main continuity (integer) + */ + +#define AO_TELEM_RAW_ACCEL "r_a" +#define AO_TELEM_RAW_BARO "r_b" +#define AO_TELEM_RAW_THERMO "r_t" +#define AO_TELEM_RAW_BATT "r_v" +#define AO_TELEM_RAW_DROGUE "r_d" +#define AO_TELEM_RAW_MAIN "r_m" + +/* + * Sensor calibration values + * + * Name Value + * c_a Ground accelerometer reading (integer) + * c_b Ground barometer reading (integer) + * c_p Accelerometer reading for +1g + * c_m Accelerometer reading for -1g + */ + +#define AO_TELEM_CAL_ACCEL_GROUND "c_a" +#define AO_TELEM_CAL_BARO_GROUND "c_b" +#define AO_TELEM_CAL_ACCEL_PLUS "c_p" +#define AO_TELEM_CAL_ACCEL_MINUS "c_m" + +/* + * Kalman state values + * + * Name Value + * k_h Height above pad (integer, meters) + * k_s Vertical speeed (integer, m/s * 16) + * k_a Vertical acceleration (integer, m/s² * 16) + */ + +#define AO_TELEM_KALMAN_HEIGHT "k_h" +#define AO_TELEM_KALMAN_SPEED "k_s" +#define AO_TELEM_KALMAN_ACCEL "k_a" + +/* + * Ad-hoc flight values + * + * Name Value + * a_a Acceleration (integer, sensor units) + * a_s Speed (integer, integrated acceleration value) + * a_b Barometer reading (integer, sensor units) + */ + +#define AO_TELEM_ADHOC_ACCEL "a_a" +#define AO_TELEM_ADHOC_SPEED "a_s" +#define AO_TELEM_ADHOC_BARO "a_b" + +/* + * GPS values + * + * Name Value + * g GPS state (string): + * l locked + * u unlocked + * e error (missing or broken) + * g_n Number of sats used in solution + * g_ns Latitude (degrees * 10e7) + * g_ew Longitude (degrees * 10e7) + * g_a Altitude (integer meters) + * g_Y GPS year (integer) + * g_M GPS month (integer - 1-12) + * g_D GPS day (integer - 1-31) + * g_h GPS hour (integer - 0-23) + * g_m GPS minute (integer - 0-59) + * g_s GPS second (integer - 0-59) + * g_v GPS vertical speed (integer, cm/sec) + * g_g GPS horizontal speed (integer, cm/sec) + * g_c GPS course (integer, 0-359) + * g_hd GPS hdop (integer * 10) + * g_vd GPS vdop (integer * 10) + * g_he GPS h error (integer) + * g_ve GPS v error (integer) + */ + +#define AO_TELEM_GPS_STATE "g" +#define AO_TELEM_GPS_STATE_LOCKED 'l' +#define AO_TELEM_GPS_STATE_UNLOCKED 'u' +#define AO_TELEM_GPS_STATE_ERROR 'e' +#define AO_TELEM_GPS_NUM_SAT "g_n" +#define AO_TELEM_GPS_LATITUDE "g_ns" +#define AO_TELEM_GPS_LONGITUDE "g_ew" +#define AO_TELEM_GPS_ALTITUDE "g_a" +#define AO_TELEM_GPS_YEAR "g_Y" +#define AO_TELEM_GPS_MONTH "g_M" +#define AO_TELEM_GPS_DAY "g_D" +#define AO_TELEM_GPS_HOUR "g_h" +#define AO_TELEM_GPS_MINUTE "g_m" +#define AO_TELEM_GPS_SECOND "g_s" +#define AO_TELEM_GPS_VERTICAL_SPEED "g_v" +#define AO_TELEM_GPS_HORIZONTAL_SPEED "g_g" +#define AO_TELEM_GPS_COURSE "g_c" +#define AO_TELEM_GPS_HDOP "g_hd" +#define AO_TELEM_GPS_VDOP "g_vd" +#define AO_TELEM_GPS_HERROR "g_he" +#define AO_TELEM_GPS_VERROR "g_ve" + +/* + * GPS satellite values + * + * Name Value + * s_n Number of satellites reported (integer) + * s_v0 Space vehicle ID (integer) for report 0 + * s_c0 C/N0 number (integer) for report 0 + * s_v1 Space vehicle ID (integer) for report 1 + * s_c1 C/N0 number (integer) for report 1 + * ... + */ + +#define AO_TELEM_SAT_NUM "s_n" +#define AO_TELEM_SAT_SVID "s_v" +#define AO_TELEM_SAT_C_N_0 "s_c" + +#endif /* _AO_TELEM_H_ */ diff --git a/src/kernel/ao_telemetry.c b/src/kernel/ao_telemetry.c new file mode 100644 index 00000000..a1c19185 --- /dev/null +++ b/src/kernel/ao_telemetry.c @@ -0,0 +1,555 @@ +/* + * 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_log.h" +#include "ao_product.h" + +#ifndef HAS_RDF +#define HAS_RDF 1 +#endif + +static __pdata uint16_t ao_telemetry_interval; + +#if HAS_RDF +static __pdata uint8_t ao_rdf = 0; +static __pdata uint16_t ao_rdf_time; +#endif + +#if HAS_APRS +static __pdata uint16_t ao_aprs_time; + +#include +#endif + +#if defined(TELEMEGA) +#define AO_SEND_MEGA 1 +#endif + +#if defined (TELEMETRUM_V_2_0) +#define AO_SEND_METRUM 1 +#endif + +#if defined(TELEMETRUM_V_0_1) || defined(TELEMETRUM_V_0_2) || defined(TELEMETRUM_V_1_0) || defined(TELEMETRUM_V_1_1) || defined(TELEBALLOON_V_1_1) || defined(TELEMETRUM_V_1_2) +#define AO_TELEMETRY_SENSOR AO_TELEMETRY_SENSOR_TELEMETRUM +#endif + +#if defined(TELEMINI_V_1_0) +#define AO_TELEMETRY_SENSOR AO_TELEMETRY_SENSOR_TELEMINI +#endif + +#if defined(TELENANO_V_0_1) +#define AO_TELEMETRY_SENSOR AO_TELEMETRY_SENSOR_TELENANO +#endif + +static __xdata union ao_telemetry_all telemetry; + +#if defined AO_TELEMETRY_SENSOR +/* Send sensor packet */ +static void +ao_send_sensor(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_SENSOR; + + telemetry.sensor.state = ao_flight_state; +#if HAS_ACCEL + telemetry.sensor.accel = packet->adc.accel; +#else + telemetry.sensor.accel = 0; +#endif + telemetry.sensor.pres = ao_data_pres(packet); + telemetry.sensor.temp = packet->adc.temp; + telemetry.sensor.v_batt = packet->adc.v_batt; +#if HAS_IGNITE + telemetry.sensor.sense_d = packet->adc.sense_d; + telemetry.sensor.sense_m = packet->adc.sense_m; +#else + telemetry.sensor.sense_d = 0; + telemetry.sensor.sense_m = 0; +#endif + + telemetry.sensor.acceleration = ao_accel; + telemetry.sensor.speed = ao_speed; + telemetry.sensor.height = ao_height; + + telemetry.sensor.ground_pres = ao_ground_pres; +#if HAS_ACCEL + telemetry.sensor.ground_accel = ao_ground_accel; + telemetry.sensor.accel_plus_g = ao_config.accel_plus_g; + telemetry.sensor.accel_minus_g = ao_config.accel_minus_g; +#else + telemetry.sensor.ground_accel = 0; + telemetry.sensor.accel_plus_g = 0; + telemetry.sensor.accel_minus_g = 0; +#endif + + ao_radio_send(&telemetry, sizeof (telemetry)); +} +#endif + + +#ifdef AO_SEND_MEGA +/* Send mega sensor packet */ +static void +ao_send_mega_sensor(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_MEGA_SENSOR; + + telemetry.mega_sensor.orient = ao_sample_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); + +#if HAS_MPU6000 + telemetry.mega_sensor.accel_x = packet->mpu6000.accel_x; + telemetry.mega_sensor.accel_y = packet->mpu6000.accel_y; + telemetry.mega_sensor.accel_z = packet->mpu6000.accel_z; + + telemetry.mega_sensor.gyro_x = packet->mpu6000.gyro_x; + telemetry.mega_sensor.gyro_y = packet->mpu6000.gyro_y; + telemetry.mega_sensor.gyro_z = packet->mpu6000.gyro_z; +#endif + +#if HAS_HMC5883 + telemetry.mega_sensor.mag_x = packet->hmc5883.x; + telemetry.mega_sensor.mag_y = packet->hmc5883.y; + telemetry.mega_sensor.mag_z = packet->hmc5883.z; +#endif + + ao_radio_send(&telemetry, sizeof (telemetry)); +} + +static __pdata int8_t ao_telemetry_mega_data_max; +static __pdata int8_t ao_telemetry_mega_data_cur; + +/* Send mega data packet */ +static void +ao_send_mega_data(void) +{ + if (--ao_telemetry_mega_data_cur <= 0) { + __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; + uint8_t i; + + telemetry.generic.tick = packet->tick; + telemetry.generic.type = AO_TELEMETRY_MEGA_DATA; + + telemetry.mega_data.state = ao_flight_state; + telemetry.mega_data.v_batt = packet->adc.v_batt; + telemetry.mega_data.v_pyro = packet->adc.v_pbatt; + + /* ADC range is 0-4095, so shift by four to save the high 8 bits */ + for (i = 0; i < AO_ADC_NUM_SENSE; i++) + telemetry.mega_data.sense[i] = packet->adc.sense[i] >> 4; + + telemetry.mega_data.ground_pres = ao_ground_pres; + telemetry.mega_data.ground_accel = ao_ground_accel; + telemetry.mega_data.accel_plus_g = ao_config.accel_plus_g; + telemetry.mega_data.accel_minus_g = ao_config.accel_minus_g; + + telemetry.mega_data.acceleration = ao_accel; + telemetry.mega_data.speed = ao_speed; + telemetry.mega_data.height = ao_height; + + ao_radio_send(&telemetry, sizeof (telemetry)); + ao_telemetry_mega_data_cur = ao_telemetry_mega_data_max; + } +} +#endif /* AO_SEND_MEGA */ + +#ifdef AO_SEND_METRUM +/* Send telemetrum sensor packet */ +static void +ao_send_metrum_sensor(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_METRUM_SENSOR; + + telemetry.metrum_sensor.state = ao_flight_state; +#if HAS_ACCEL + telemetry.metrum_sensor.accel = ao_data_accel(packet); +#endif + telemetry.metrum_sensor.pres = ao_data_pres(packet); + telemetry.metrum_sensor.temp = ao_data_temp(packet); + + telemetry.metrum_sensor.acceleration = ao_accel; + telemetry.metrum_sensor.speed = ao_speed; + telemetry.metrum_sensor.height = ao_height; + + telemetry.metrum_sensor.v_batt = packet->adc.v_batt; + telemetry.metrum_sensor.sense_a = packet->adc.sense_a; + telemetry.metrum_sensor.sense_m = packet->adc.sense_m; + + ao_radio_send(&telemetry, sizeof (telemetry)); +} + +static __pdata int8_t ao_telemetry_metrum_data_max; +static __pdata int8_t ao_telemetry_metrum_data_cur; + +/* Send telemetrum data packet */ +static void +ao_send_metrum_data(void) +{ + if (--ao_telemetry_metrum_data_cur <= 0) { + __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_METRUM_DATA; + + telemetry.metrum_data.ground_pres = ao_ground_pres; +#if HAS_ACCEL + telemetry.metrum_data.ground_accel = ao_ground_accel; + telemetry.metrum_data.accel_plus_g = ao_config.accel_plus_g; + telemetry.metrum_data.accel_minus_g = ao_config.accel_minus_g; +#else + telemetry.metrum_data.ground_accel = 1; + telemetry.metrum_data.accel_plus_g = 0; + telemetry.metrum_data.accel_minus_g = 2; +#endif + + ao_radio_send(&telemetry, sizeof (telemetry)); + ao_telemetry_metrum_data_cur = ao_telemetry_metrum_data_max; + } +} +#endif /* AO_SEND_METRUM */ + +#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 /* AO_SEND_MINI */ + +#ifdef AO_SEND_ALL_BARO +static uint8_t ao_baro_sample; + +static void +ao_send_baro(void) +{ + uint8_t sample = ao_sample_data; + uint8_t samples = (sample - ao_baro_sample) & (AO_DATA_RING - 1); + + if (samples > 12) { + ao_baro_sample = (ao_baro_sample + (samples - 12)) & (AO_DATA_RING - 1); + samples = 12; + } + telemetry.generic.tick = ao_data_ring[sample].tick; + telemetry.generic.type = AO_TELEMETRY_BARO; + telemetry.baro.samples = samples; + for (sample = 0; sample < samples; sample++) { + telemetry.baro.baro[sample] = ao_data_ring[ao_baro_sample].adc.pres; + ao_baro_sample = ao_data_ring_next(ao_baro_sample); + } + ao_radio_send(&telemetry, sizeof (telemetry)); +} +#endif + +static __pdata int8_t ao_telemetry_config_max; +static __pdata int8_t ao_telemetry_config_cur; + +static void +ao_send_configuration(void) +{ + if (--ao_telemetry_config_cur <= 0) + { + telemetry.generic.type = AO_TELEMETRY_CONFIGURATION; + telemetry.configuration.device = AO_idProduct_NUMBER; +#if HAS_LOG + telemetry.configuration.flight = ao_log_full() ? 0 : ao_flight_number; +#else + telemetry.configuration.flight = ao_flight_number; +#endif + telemetry.configuration.config_major = AO_CONFIG_MAJOR; + telemetry.configuration.config_minor = AO_CONFIG_MINOR; + telemetry.configuration.apogee_delay = ao_config.apogee_delay; + telemetry.configuration.main_deploy = ao_config.main_deploy; + telemetry.configuration.flight_log_max = ao_config.flight_log_max >> 10; + ao_xmemcpy (telemetry.configuration.callsign, + ao_config.callsign, + AO_MAX_CALLSIGN); + ao_xmemcpy (telemetry.configuration.version, + CODE_TO_XDATA(ao_version), + AO_MAX_VERSION); + ao_radio_send(&telemetry, sizeof (telemetry)); + ao_telemetry_config_cur = ao_telemetry_config_max; + } +} + +#if HAS_GPS + +static __pdata int8_t ao_telemetry_loc_cur; +static __pdata int8_t ao_telemetry_sat_cur; + +static void +ao_send_location(void) +{ + if (--ao_telemetry_loc_cur <= 0) + { + telemetry.generic.type = AO_TELEMETRY_LOCATION; + ao_mutex_get(&ao_gps_mutex); + ao_xmemcpy(&telemetry.location.flags, + &ao_gps_data.flags, + 26); + telemetry.location.tick = ao_gps_tick; + ao_mutex_put(&ao_gps_mutex); + ao_radio_send(&telemetry, sizeof (telemetry)); + ao_telemetry_loc_cur = ao_telemetry_config_max; + } +} + +static void +ao_send_satellite(void) +{ + if (--ao_telemetry_sat_cur <= 0) + { + telemetry.generic.type = AO_TELEMETRY_SATELLITE; + ao_mutex_get(&ao_gps_mutex); + telemetry.satellite.channels = ao_gps_tracking_data.channels; + ao_xmemcpy(&telemetry.satellite.sats, + &ao_gps_tracking_data.sats, + AO_MAX_GPS_TRACKING * sizeof (struct ao_telemetry_satellite_info)); + ao_mutex_put(&ao_gps_mutex); + ao_radio_send(&telemetry, sizeof (telemetry)); + ao_telemetry_sat_cur = ao_telemetry_config_max; + } +} +#endif + +#if HAS_COMPANION + +static __pdata int8_t ao_telemetry_companion_max; +static __pdata int8_t ao_telemetry_companion_cur; + +static void +ao_send_companion(void) +{ + if (--ao_telemetry_companion_cur <= 0) { + telemetry.generic.type = AO_TELEMETRY_COMPANION; + telemetry.companion.board_id = ao_companion_setup.board_id; + telemetry.companion.update_period = ao_companion_setup.update_period; + telemetry.companion.channels = ao_companion_setup.channels; + ao_mutex_get(&ao_companion_mutex); + ao_xmemcpy(&telemetry.companion.companion_data, + ao_companion_data, + ao_companion_setup.channels * 2); + ao_mutex_put(&ao_companion_mutex); + ao_radio_send(&telemetry, sizeof (telemetry)); + ao_telemetry_companion_cur = ao_telemetry_companion_max; + } +} +#endif + +void +ao_telemetry(void) +{ + uint16_t time; + int16_t delay; + + ao_config_get(); + if (!ao_config.radio_enable) + ao_exit(); + while (!ao_flight_number) + ao_sleep(&ao_flight_number); + + telemetry.generic.serial = ao_serial_number; + for (;;) { + while (ao_telemetry_interval == 0) + ao_sleep(&telemetry); + time = ao_time(); +#if HAS_RDF + ao_rdf_time = time; +#endif +#if HAS_APRS + ao_aprs_time = time; +#endif + while (ao_telemetry_interval) { +#if HAS_APRS + if (!(ao_config.radio_enable & AO_RADIO_DISABLE_TELEMETRY)) +#endif + { +#ifdef AO_SEND_ALL_BARO + ao_send_baro(); +#endif + +#if HAS_FLIGHT +# ifdef AO_SEND_MEGA + ao_send_mega_sensor(); + ao_send_mega_data(); +# endif +# ifdef AO_SEND_METRUM + ao_send_metrum_sensor(); + ao_send_metrum_data(); +# endif +# ifdef AO_SEND_MINI + ao_send_mini(); +# endif +# ifdef AO_TELEMETRY_SENSOR + ao_send_sensor(); +# endif +#endif /* HAS_FLIGHT */ + +#if HAS_COMPANION + if (ao_companion_running) + ao_send_companion(); +#endif + ao_send_configuration(); +#if HAS_GPS + ao_send_location(); + ao_send_satellite(); +#endif + } +#ifndef AO_SEND_ALL_BARO +#if HAS_RDF + if (ao_rdf && +#if HAS_APRS + !(ao_config.radio_enable & AO_RADIO_DISABLE_RDF) && +#endif /* HAS_APRS */ + (int16_t) (ao_time() - ao_rdf_time) >= 0) + { +#if HAS_IGNITE_REPORT + uint8_t c; +#endif /* HAS_IGNITE_REPORT */ + ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS; +#if HAS_IGNITE_REPORT + if (ao_flight_state == ao_flight_pad && (c = ao_report_igniter())) + ao_radio_continuity(c); + else +#endif /* HAS_IGNITE_REPORT*/ + ao_radio_rdf(); + } +#endif /* HAS_RDF */ +#if HAS_APRS + if (ao_config.aprs_interval != 0 && + (int16_t) (ao_time() - ao_aprs_time) >= 0) + { + ao_aprs_time = ao_time() + AO_SEC_TO_TICKS(ao_config.aprs_interval); + ao_aprs_send(); + } +#endif /* HAS_APRS */ +#endif /* !AO_SEND_ALL_BARO */ + time += ao_telemetry_interval; + delay = time - ao_time(); + if (delay > 0) { + ao_alarm(delay); + ao_sleep(&telemetry); + ao_clear_alarm(); + } + else + time = ao_time(); + } + } +} + +void +ao_telemetry_set_interval(uint16_t interval) +{ + int8_t cur = 0; + ao_telemetry_interval = interval; + +#if AO_SEND_MEGA + if (interval > 1) + ao_telemetry_mega_data_max = 1; + else + ao_telemetry_mega_data_max = 2; + if (ao_telemetry_mega_data_max > cur) + cur++; + ao_telemetry_mega_data_cur = cur; +#endif +#if AO_SEND_METRUM + ao_telemetry_metrum_data_max = AO_SEC_TO_TICKS(1) / interval; + if (ao_telemetry_metrum_data_max > cur) + cur++; + ao_telemetry_metrum_data_cur = cur; +#endif + +#if HAS_COMPANION + if (!ao_companion_setup.update_period) + ao_companion_setup.update_period = AO_SEC_TO_TICKS(1); + ao_telemetry_companion_max = ao_companion_setup.update_period / interval; + if (ao_telemetry_companion_max > cur) + cur++; + ao_telemetry_companion_cur = cur; +#endif + + ao_telemetry_config_max = AO_SEC_TO_TICKS(1) / interval; +#if HAS_COMPANION + if (ao_telemetry_config_max > cur) + cur++; + ao_telemetry_config_cur = cur; +#endif + +#if HAS_GPS + if (ao_telemetry_config_max > cur) + cur++; + ao_telemetry_loc_cur = cur; + if (ao_telemetry_config_max > cur) + cur++; + ao_telemetry_sat_cur = cur; +#endif + ao_wakeup(&telemetry); +} + +#if HAS_RDF +void +ao_rdf_set(uint8_t rdf) +{ + ao_rdf = rdf; + if (rdf == 0) + ao_radio_rdf_abort(); + else { + ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS; + } +} +#endif + +__xdata struct ao_task ao_telemetry_task; + +void +ao_telemetry_init() +{ + ao_add_task(&ao_telemetry_task, ao_telemetry, "telemetry"); +} diff --git a/src/kernel/ao_telemetry.h b/src/kernel/ao_telemetry.h new file mode 100644 index 00000000..237a35ab --- /dev/null +++ b/src/kernel/ao_telemetry.h @@ -0,0 +1,321 @@ +/* + * 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. + */ + +#ifndef _AO_TELEMETRY_H_ +#define _AO_TELEMETRY_H_ + +/* + * ao_telemetry.c + */ +#define AO_MAX_CALLSIGN 8 +#define AO_MAX_VERSION 8 +#if LEGACY_MONITOR +#define AO_MAX_TELEMETRY 128 +#else +#define AO_MAX_TELEMETRY 32 +#endif + +struct ao_telemetry_generic { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + uint8_t payload[27]; /* 5 */ + /* 32 */ +}; + +#define AO_TELEMETRY_SENSOR_TELEMETRUM 0x01 +#define AO_TELEMETRY_SENSOR_TELEMINI 0x02 +#define AO_TELEMETRY_SENSOR_TELENANO 0x03 + +struct ao_telemetry_sensor { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + + uint8_t state; /* 5 flight state */ + int16_t accel; /* 6 accelerometer (TM only) */ + int16_t pres; /* 8 pressure sensor */ + int16_t temp; /* 10 temperature sensor */ + int16_t v_batt; /* 12 battery voltage */ + int16_t sense_d; /* 14 drogue continuity sense (TM/Tm) */ + int16_t sense_m; /* 16 main continuity sense (TM/Tm) */ + + int16_t acceleration; /* 18 m/s² * 16 */ + int16_t speed; /* 20 m/s * 16 */ + int16_t height; /* 22 m */ + + int16_t ground_pres; /* 24 average pres on pad */ + int16_t ground_accel; /* 26 average accel on pad */ + int16_t accel_plus_g; /* 28 accel calibration at +1g */ + int16_t accel_minus_g; /* 30 accel calibration at -1g */ + /* 32 */ +}; + +#define AO_TELEMETRY_CONFIGURATION 0x04 + +struct ao_telemetry_configuration { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + + uint8_t device; /* 5 device type */ + uint16_t flight; /* 6 flight number */ + uint8_t config_major; /* 8 Config major version */ + uint8_t config_minor; /* 9 Config minor version */ + uint16_t apogee_delay; /* 10 Apogee deploy delay in seconds */ + uint16_t main_deploy; /* 12 Main deploy alt in meters */ + uint16_t flight_log_max; /* 14 Maximum flight log size in kB */ + char callsign[AO_MAX_CALLSIGN]; /* 16 Radio operator identity */ + char version[AO_MAX_VERSION]; /* 24 Software version */ + /* 32 */ +}; + +#define AO_TELEMETRY_LOCATION 0x05 + +#define AO_GPS_MODE_NOT_VALID 'N' +#define AO_GPS_MODE_AUTONOMOUS 'A' +#define AO_GPS_MODE_DIFFERENTIAL 'D' +#define AO_GPS_MODE_ESTIMATED 'E' +#define AO_GPS_MODE_MANUAL 'M' +#define AO_GPS_MODE_SIMULATED 'S' + +struct ao_telemetry_location { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + + uint8_t flags; /* 5 Number of sats and other flags */ + int16_t altitude; /* 6 GPS reported altitude (m) */ + int32_t latitude; /* 8 latitude (degrees * 10⁷) */ + int32_t longitude; /* 12 longitude (degrees * 10⁷) */ + uint8_t year; /* 16 (- 2000) */ + uint8_t month; /* 17 (1-12) */ + uint8_t day; /* 18 (1-31) */ + uint8_t hour; /* 19 (0-23) */ + uint8_t minute; /* 20 (0-59) */ + uint8_t second; /* 21 (0-59) */ + uint8_t pdop; /* 22 (m * 5) */ + uint8_t hdop; /* 23 (m * 5) */ + uint8_t vdop; /* 24 (m * 5) */ + uint8_t mode; /* 25 */ + uint16_t ground_speed; /* 26 cm/s */ + int16_t climb_rate; /* 28 cm/s */ + uint8_t course; /* 30 degrees / 2 */ + uint8_t unused[1]; /* 31 */ + /* 32 */ +}; + +#define AO_TELEMETRY_SATELLITE 0x06 + +struct ao_telemetry_satellite_info { + uint8_t svid; + uint8_t c_n_1; +}; + +#define AO_TELEMETRY_SATELLITE_MAX_SAT 12 + +struct ao_telemetry_satellite { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + uint8_t channels; /* 5 number of reported sats */ + + struct ao_telemetry_satellite_info sats[AO_TELEMETRY_SATELLITE_MAX_SAT]; /* 6 */ + uint8_t unused[2]; /* 30 */ + /* 32 */ +}; + +#define AO_TELEMETRY_COMPANION 0x07 + +#define AO_COMPANION_MAX_CHANNELS 12 + +struct ao_telemetry_companion { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + uint8_t board_id; /* 5 */ + + uint8_t update_period; /* 6 */ + uint8_t channels; /* 7 */ + uint16_t companion_data[AO_COMPANION_MAX_CHANNELS]; /* 8 */ + /* 32 */ +}; + +#define AO_TELEMETRY_MEGA_SENSOR 0x08 + +struct ao_telemetry_mega_sensor { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + + uint8_t orient; /* 5 angle from vertical */ + int16_t accel; /* 6 Z axis */ + + int32_t pres; /* 8 Pa * 10 */ + int16_t temp; /* 12 °C * 100 */ + + int16_t accel_x; /* 14 */ + int16_t accel_y; /* 16 */ + int16_t accel_z; /* 18 */ + + int16_t gyro_x; /* 20 */ + int16_t gyro_y; /* 22 */ + int16_t gyro_z; /* 24 */ + + int16_t mag_x; /* 26 */ + int16_t mag_y; /* 28 */ + int16_t mag_z; /* 30 */ + /* 32 */ +}; + +#define AO_TELEMETRY_MEGA_DATA 0x09 + +struct ao_telemetry_mega_data { + 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 v_pyro; /* 8 pyro battery voltage */ + int8_t sense[6]; /* 10 continuity sense */ + + int32_t ground_pres; /* 16 average pres on pad */ + int16_t ground_accel; /* 20 average accel on pad */ + int16_t accel_plus_g; /* 22 accel calibration at +1g */ + int16_t accel_minus_g; /* 24 accel calibration at -1g */ + + int16_t acceleration; /* 26 m/s² * 16 */ + int16_t speed; /* 28 m/s * 16 */ + int16_t height; /* 30 m */ + /* 32 */ +}; + + +#define AO_TELEMETRY_METRUM_SENSOR 0x0A + +struct ao_telemetry_metrum_sensor { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + + uint8_t state; /* 5 flight state */ + int16_t accel; /* 6 Z axis */ + + int32_t pres; /* 8 Pa * 10 */ + int16_t temp; /* 12 °C * 100 */ + + int16_t acceleration; /* 14 m/s² * 16 */ + int16_t speed; /* 16 m/s * 16 */ + int16_t height; /* 18 m */ + + int16_t v_batt; /* 20 battery voltage */ + int16_t sense_a; /* 22 apogee continuity sense */ + int16_t sense_m; /* 24 main continuity sense */ + + uint8_t pad[6]; /* 26 */ + /* 32 */ +}; + +#define AO_TELEMETRY_METRUM_DATA 0x0B + +struct ao_telemetry_metrum_data { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + + int32_t ground_pres; /* 8 average pres on pad */ + int16_t ground_accel; /* 12 average accel on pad */ + int16_t accel_plus_g; /* 14 accel calibration at +1g */ + int16_t accel_minus_g; /* 16 accel calibration at -1g */ + + uint8_t pad[14]; /* 18 */ + /* 32 */ +}; + +#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 + +/* + * This packet allows the full sampling rate baro + * data to be captured over the RF link so that the + * flight software can be tested using 'real' data. + * + * Along with this telemetry packet, the flight + * code is modified to send full-rate telemetry all the time + * and never send an RDF tone; this ensure that the full radio + * link is available. + */ +struct ao_telemetry_baro { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + uint8_t samples; /* 5 number samples */ + + int16_t baro[12]; /* 6 samples */ + /* 32 */ +}; + +union ao_telemetry_all { + struct ao_telemetry_generic generic; + struct ao_telemetry_sensor sensor; + struct ao_telemetry_configuration configuration; + struct ao_telemetry_location location; + struct ao_telemetry_satellite satellite; + struct ao_telemetry_companion companion; + struct ao_telemetry_mega_sensor mega_sensor; + struct ao_telemetry_mega_data mega_data; + struct ao_telemetry_metrum_sensor metrum_sensor; + struct ao_telemetry_metrum_data metrum_data; + struct ao_telemetry_mini mini; + struct ao_telemetry_baro baro; +}; + +struct ao_telemetry_all_recv { + union ao_telemetry_all telemetry; + int8_t rssi; + uint8_t status; +}; + +#endif /* _AO_TELEMETRY_H_ */ diff --git a/src/kernel/ao_usb.h b/src/kernel/ao_usb.h new file mode 100644 index 00000000..1ce4f82f --- /dev/null +++ b/src/kernel/ao_usb.h @@ -0,0 +1,142 @@ +/* + * 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. + */ + +#ifndef _AO_USB_H_ +#define _AO_USB_H_ + +/* + * ao_usb.c + */ + +/* Put one character to the USB output queue */ +void +ao_usb_putchar(char c); + +/* Get one character from the USB input queue */ +char +ao_usb_getchar(void); + +/* Poll for a charcter on the USB input queue. + * returns AO_READ_AGAIN if none are available + */ +int +ao_usb_pollchar(void); + +/* Flush the USB output queue */ +void +ao_usb_flush(void); + +/* Enable the USB controller */ +void +ao_usb_enable(void); + +/* Disable the USB controller */ +void +ao_usb_disable(void); + +/* Initialize the USB system */ +void +ao_usb_init(void); + +extern __code __at (0x00aa) uint8_t ao_usb_descriptors []; + +#define AO_USB_SETUP_DIR_MASK (0x01 << 7) +#define AO_USB_SETUP_TYPE_MASK (0x03 << 5) +#define AO_USB_SETUP_RECIP_MASK (0x1f) + +#define AO_USB_DIR_OUT 0 +#define AO_USB_DIR_IN (1 << 7) + +#define AO_USB_TYPE_STANDARD 0 +#define AO_USB_TYPE_CLASS (1 << 5) +#define AO_USB_TYPE_VENDOR (2 << 5) +#define AO_USB_TYPE_RESERVED (3 << 5) + +#define AO_USB_RECIP_DEVICE 0 +#define AO_USB_RECIP_INTERFACE 1 +#define AO_USB_RECIP_ENDPOINT 2 +#define AO_USB_RECIP_OTHER 3 + +/* standard requests */ +#define AO_USB_REQ_GET_STATUS 0x00 +#define AO_USB_REQ_CLEAR_FEATURE 0x01 +#define AO_USB_REQ_SET_FEATURE 0x03 +#define AO_USB_REQ_SET_ADDRESS 0x05 +#define AO_USB_REQ_GET_DESCRIPTOR 0x06 +#define AO_USB_REQ_SET_DESCRIPTOR 0x07 +#define AO_USB_REQ_GET_CONFIGURATION 0x08 +#define AO_USB_REQ_SET_CONFIGURATION 0x09 +#define AO_USB_REQ_GET_INTERFACE 0x0A +#define AO_USB_REQ_SET_INTERFACE 0x0B +#define AO_USB_REQ_SYNCH_FRAME 0x0C + +#define AO_USB_DESC_DEVICE 1 +#define AO_USB_DESC_CONFIGURATION 2 +#define AO_USB_DESC_STRING 3 +#define AO_USB_DESC_INTERFACE 4 +#define AO_USB_DESC_ENDPOINT 5 +#define AO_USB_DESC_DEVICE_QUALIFIER 6 +#define AO_USB_DESC_OTHER_SPEED 7 +#define AO_USB_DESC_INTERFACE_POWER 8 + +#define AO_USB_GET_DESC_TYPE(x) (((x)>>8)&0xFF) +#define AO_USB_GET_DESC_INDEX(x) ((x)&0xFF) + +#define AO_USB_CONTROL_EP 0 +#define AO_USB_CONTROL_SIZE 32 + +#define AO_USB_INT_EP 1 +#define AO_USB_INT_SIZE 8 + +#ifndef AO_USB_OUT_EP +#define AO_USB_OUT_EP 4 +#define AO_USB_IN_EP 5 +#endif + +/* + * USB bulk packets can only come in 8, 16, 32 and 64 + * byte sizes, so we'll use 64 for everything + */ +#define AO_USB_IN_SIZE 64 +#define AO_USB_OUT_SIZE 64 + +#define AO_USB_EP0_IDLE 0 +#define AO_USB_EP0_DATA_IN 1 +#define AO_USB_EP0_DATA_OUT 2 +#define AO_USB_EP0_STALL 3 + +#define LE_WORD(x) ((x)&0xFF),((uint8_t) (((uint16_t) (x))>>8)) + +/* CDC definitions */ +#define AO_USB_CS_INTERFACE 0x24 +#define AO_USB_CS_ENDPOINT 0x25 + +#define AO_USB_SET_LINE_CODING 0x20 +#define AO_USB_GET_LINE_CODING 0x21 +#define AO_USB_SET_CONTROL_LINE_STATE 0x22 + +/* Data structure for GET_LINE_CODING / SET_LINE_CODING class requests */ +struct ao_usb_line_coding { + uint32_t rate; + uint8_t char_format; + uint8_t parity; + uint8_t data_bits; +} ; + +extern __pdata uint8_t ao_usb_running; + +#endif /* _AO_USB_H_ */ diff --git a/src/lpc/Makefile-lpc.defs b/src/lpc/Makefile-lpc.defs index fecb9135..bccea5bc 100644 --- a/src/lpc/Makefile-lpc.defs +++ b/src/lpc/Makefile-lpc.defs @@ -4,7 +4,7 @@ endif include $(TOPDIR)/Makedefs -vpath % $(TOPDIR)/lpc:$(TOPDIR)/product:$(TOPDIR)/drivers:$(TOPDIR)/core:$(TOPDIR)/util:$(TOPDIR)/kalman:$(TOPDIR/aes):$(TOPDIR):$(TOPDIR)/math +vpath % $(TOPDIR)/lpc:$(TOPDIR)/product:$(TOPDIR)/drivers:$(TOPDIR)/kernel:$(TOPDIR)/util:$(TOPDIR)/kalman:$(TOPDIR/aes):$(TOPDIR):$(TOPDIR)/math vpath make-altitude $(TOPDIR)/util vpath make-kalman $(TOPDIR)/util vpath kalman.5c $(TOPDIR)/kalman @@ -28,7 +28,7 @@ CC=$(ARM_CC) WARN_FLAGS=-Wall -Wextra -Werror -AO_CFLAGS=-I. -I$(TOPDIR)/lpc -I$(TOPDIR)/core -I$(TOPDIR)/drivers -I$(TOPDIR)/product -I$(TOPDIR) -I$(TOPDIR)/math -I$(TOPDIR) $(PDCLIB_INCLUDES) +AO_CFLAGS=-I. -I$(TOPDIR)/lpc -I$(TOPDIR)/kernel -I$(TOPDIR)/drivers -I$(TOPDIR)/product -I$(TOPDIR) -I$(TOPDIR)/math -I$(TOPDIR) $(PDCLIB_INCLUDES) LPC_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m0 -mthumb\ -ffreestanding -nostdlib $(AO_CFLAGS) $(WARN_FLAGS) diff --git a/src/micropeak/Makefile b/src/micropeak/Makefile index dcc32874..f4c57076 100644 --- a/src/micropeak/Makefile +++ b/src/micropeak/Makefile @@ -2,7 +2,7 @@ # Tiny AltOS build # # -vpath % ../attiny:../drivers:../core:../product:.. +vpath % ../attiny:../drivers:../kernel:../product:.. vpath ao-make-product.5c ../util vpath make-altitude-pa ../util @@ -47,7 +47,7 @@ INC=\ IDPRODUCT=0 PRODUCT=MicroPeak-v0.1 PRODUCT_DEF=-DMICROPEAK -CFLAGS = $(PRODUCT_DEF) -I. -I../attiny -I../core -I.. -I../drivers -I../product +CFLAGS = $(PRODUCT_DEF) -I. -I../attiny -I../kernel -I.. -I../drivers -I../product CFLAGS += -g -mmcu=$(MCU) -Wall -Wstrict-prototypes -O2 -mcall-prologues -DATTINY NICKLE=nickle diff --git a/src/nanopeak-v0.1/Makefile b/src/nanopeak-v0.1/Makefile index 04eea902..d3779594 100644 --- a/src/nanopeak-v0.1/Makefile +++ b/src/nanopeak-v0.1/Makefile @@ -2,7 +2,7 @@ # Tiny AltOS build # # -vpath % ../attiny:../drivers:../core:../product:.. +vpath % ../attiny:../drivers:../kernel:../product:.. vpath ao-make-product.5c ../util vpath make-altitude-pa ../util @@ -47,7 +47,7 @@ INC=\ IDPRODUCT=0 PRODUCT=NanoPeak-v0.1 PRODUCT_DEF=-DNANOPEAK -CFLAGS = $(PRODUCT_DEF) -I. -I../attiny -I../core -I.. -I../drivers -I../product +CFLAGS = $(PRODUCT_DEF) -I. -I../attiny -I../kernel -I.. -I../drivers -I../product CFLAGS += -g -mmcu=$(MCU) -Wall -Wstrict-prototypes -O2 -mcall-prologues -DATTINY NICKLE=nickle diff --git a/src/product/Makefile.teledongle b/src/product/Makefile.teledongle index da9bcba0..81151364 100644 --- a/src/product/Makefile.teledongle +++ b/src/product/Makefile.teledongle @@ -7,8 +7,8 @@ # TD_VER, TD_DEF and include # this file -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/product/Makefile.telelaunch b/src/product/Makefile.telelaunch index a5e2eb7f..90fe7833 100644 --- a/src/product/Makefile.telelaunch +++ b/src/product/Makefile.telelaunch @@ -4,8 +4,8 @@ # define TELELAUNCH_VER, TELELAUNCH_DEF # this file -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/product/Makefile.telemetrum b/src/product/Makefile.telemetrum index c740a483..dbbf57d8 100644 --- a/src/product/Makefile.telemetrum +++ b/src/product/Makefile.telemetrum @@ -7,8 +7,8 @@ # TM_VER, TM_DEF, TM_INC and TM_SRC and include # this file -vpath %.c .:..:../core:../cc1111:../drivers:../product -vpath %.h .:..:../core:../cc1111:../drivers:../product +vpath %.c .:..:../kernel:../cc1111:../drivers:../product +vpath %.h .:..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/product/Makefile.telemini b/src/product/Makefile.telemini index 0884079e..ff8b9d56 100644 --- a/src/product/Makefile.telemini +++ b/src/product/Makefile.telemini @@ -4,8 +4,8 @@ # Define TELEMINI_VER and TELEMINI_DEF and then # include this file -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/product/Makefile.telenano b/src/product/Makefile.telenano index c31989ee..d2fcb6d8 100644 --- a/src/product/Makefile.telenano +++ b/src/product/Makefile.telenano @@ -4,8 +4,8 @@ # Define TELENANO_VER and TELENANO_DEF and then # include this file -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/spiradio-v0.1/.sdcdbrc b/src/spiradio-v0.1/.sdcdbrc index b9f6129c..2c77e32b 100644 --- a/src/spiradio-v0.1/.sdcdbrc +++ b/src/spiradio-v0.1/.sdcdbrc @@ -1,2 +1,2 @@ ---directory=../cc1111:../product:../core:../drivers:. +--directory=../cc1111:../product:../kernel:../drivers:. diff --git a/src/spiradio-v0.1/Makefile b/src/spiradio-v0.1/Makefile index e644bc49..cd7a9cde 100644 --- a/src/spiradio-v0.1/Makefile +++ b/src/spiradio-v0.1/Makefile @@ -5,8 +5,8 @@ SPIRADIO_VER=0.1 SPIRADIO_DEF=0_1 -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/stm-bringup/Makefile b/src/stm-bringup/Makefile index 797df2d6..b0943e56 100644 --- a/src/stm-bringup/Makefile +++ b/src/stm-bringup/Makefile @@ -1,4 +1,4 @@ -vpath % ..:../core:../product:../drivers:../stm +vpath % ..:../kernel:../product:../drivers:../stm vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/stm/Makefile-flash.defs b/src/stm/Makefile-flash.defs index 3890eff1..dde51a68 100644 --- a/src/stm/Makefile-flash.defs +++ b/src/stm/Makefile-flash.defs @@ -1,4 +1,4 @@ -vpath % $(TOPDIR)/stm:$(TOPDIR)/product:$(TOPDIR)/drivers:$(TOPDIR)/core:$(TOPDIR)/util:$(TOPDIR) +vpath % $(TOPDIR)/stm:$(TOPDIR)/product:$(TOPDIR)/drivers:$(TOPDIR)/kernel:$(TOPDIR)/util:$(TOPDIR) vpath ao-make-product.5c $(TOPDIR)/util .SUFFIXES: .elf .ihx @@ -14,7 +14,7 @@ include $(TOPDIR)/Makedefs CC=$(ARM_CC) LIBS=$(PDCLIB_LIBS_M3) -lgcc -AO_CFLAGS=-I. -I$(TOPDIR)/stm -I$(TOPDIR)/core -I$(TOPDIR)/drivers -I$(TOPDIR)/product -I$(TOPDIR) $(PDCLIB_INCLUDES) +AO_CFLAGS=-I. -I$(TOPDIR)/stm -I$(TOPDIR)/kernel -I$(TOPDIR)/drivers -I$(TOPDIR)/product -I$(TOPDIR) $(PDCLIB_INCLUDES) STM_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m3 -mthumb -ffreestanding -nostdlib $(AO_CFLAGS) $(SAT_CFLAGS) LDFLAGS=-L$(TOPDIR)/stm -Wl,-Taltos-loader.ld diff --git a/src/stm/Makefile.defs b/src/stm/Makefile.defs index 42adfd09..c3d2707f 100644 --- a/src/stm/Makefile.defs +++ b/src/stm/Makefile.defs @@ -1,4 +1,4 @@ -vpath % ../stm:../product:../drivers:../core:../util:../kalman:../aes:../math:.. +vpath % ../stm:../product:../drivers:../kernel:../util:../kalman:../aes:../math:.. vpath make-altitude ../util vpath make-kalman ../util vpath kalman.5c ../kalman @@ -26,7 +26,7 @@ LIBS=$(PDCLIB_LIBS_M3) -lgcc WARN_FLAGS=-Wall -Wextra -Werror -AO_CFLAGS=-I. -I../stm -I../core -I../drivers -I../math -I.. $(PDCLIB_INCLUDES) +AO_CFLAGS=-I. -I../stm -I../kernel -I../drivers -I../math -I.. $(PDCLIB_INCLUDES) STM_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m3 -mthumb \ -ffreestanding -nostdlib $(AO_CFLAGS) $(WARN_FLAGS) diff --git a/src/teleballoon-v1.1/Makefile b/src/teleballoon-v1.1/Makefile index 6ff076a9..c6f6345a 100644 --- a/src/teleballoon-v1.1/Makefile +++ b/src/teleballoon-v1.1/Makefile @@ -17,8 +17,8 @@ TELEBALLOON_SRC = \ ao_gps_skytraq.c \ ao_m25.c -vpath %.c ..:../core:../cc1111:../drivers:../product:. -vpath %.h ..:../core:../cc1111:../drivers:../product:. +vpath %.c ..:../kernel:../cc1111:../drivers:../product:. +vpath %.h ..:../kernel:../cc1111:../drivers:../product:. vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/telebt-v1.0/.sdcdbrc b/src/telebt-v1.0/.sdcdbrc index b9f6129c..2c77e32b 100644 --- a/src/telebt-v1.0/.sdcdbrc +++ b/src/telebt-v1.0/.sdcdbrc @@ -1,2 +1,2 @@ ---directory=../cc1111:../product:../core:../drivers:. +--directory=../cc1111:../product:../kernel:../drivers:. diff --git a/src/telebt-v1.0/Makefile b/src/telebt-v1.0/Makefile index 40853fc3..a7797499 100644 --- a/src/telebt-v1.0/Makefile +++ b/src/telebt-v1.0/Makefile @@ -5,8 +5,8 @@ TELEBT_VER=1.0 TELEBT_DEF=1_0 -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/telefire-v0.1/.sdcdbrc b/src/telefire-v0.1/.sdcdbrc index b9f6129c..2c77e32b 100644 --- a/src/telefire-v0.1/.sdcdbrc +++ b/src/telefire-v0.1/.sdcdbrc @@ -1,2 +1,2 @@ ---directory=../cc1111:../product:../core:../drivers:. +--directory=../cc1111:../product:../kernel:../drivers:. diff --git a/src/telefire-v0.1/Makefile b/src/telefire-v0.1/Makefile index f9e11698..25267268 100644 --- a/src/telefire-v0.1/Makefile +++ b/src/telefire-v0.1/Makefile @@ -5,8 +5,8 @@ TELEFIRE_VER=0.1 TELEFIRE_DEF=0_1 -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/telefire-v0.2/.sdcdbrc b/src/telefire-v0.2/.sdcdbrc index b9f6129c..2c77e32b 100644 --- a/src/telefire-v0.2/.sdcdbrc +++ b/src/telefire-v0.2/.sdcdbrc @@ -1,2 +1,2 @@ ---directory=../cc1111:../product:../core:../drivers:. +--directory=../cc1111:../product:../kernel:../drivers:. diff --git a/src/telefire-v0.2/Makefile b/src/telefire-v0.2/Makefile index a820990a..ad5065c1 100644 --- a/src/telefire-v0.2/Makefile +++ b/src/telefire-v0.2/Makefile @@ -5,8 +5,8 @@ TELEFIRE_VER=0.2 TELEFIRE_DEF=0_2 -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/telemetrum-v1.0/.sdcdbrc b/src/telemetrum-v1.0/.sdcdbrc index fbe9a599..e9a51ea6 100644 --- a/src/telemetrum-v1.0/.sdcdbrc +++ b/src/telemetrum-v1.0/.sdcdbrc @@ -1 +1 @@ ---directory=../cc1111:../product:../core:../drivers:. +--directory=../cc1111:../product:../kernel:../drivers:. diff --git a/src/telemetrum-v1.1/.sdcdbrc b/src/telemetrum-v1.1/.sdcdbrc index fbe9a599..e9a51ea6 100644 --- a/src/telemetrum-v1.1/.sdcdbrc +++ b/src/telemetrum-v1.1/.sdcdbrc @@ -1 +1 @@ ---directory=../cc1111:../product:../core:../drivers:. +--directory=../cc1111:../product:../kernel:../drivers:. diff --git a/src/telemini-v1.0/.sdcdbrc b/src/telemini-v1.0/.sdcdbrc index b9f6129c..2c77e32b 100644 --- a/src/telemini-v1.0/.sdcdbrc +++ b/src/telemini-v1.0/.sdcdbrc @@ -1,2 +1,2 @@ ---directory=../cc1111:../product:../core:../drivers:. +--directory=../cc1111:../product:../kernel:../drivers:. diff --git a/src/telemini-v2.0/.sdcdbrc b/src/telemini-v2.0/.sdcdbrc index b9f6129c..2c77e32b 100644 --- a/src/telemini-v2.0/.sdcdbrc +++ b/src/telemini-v2.0/.sdcdbrc @@ -1,2 +1,2 @@ ---directory=../cc1111:../product:../core:../drivers:. +--directory=../cc1111:../product:../kernel:../drivers:. diff --git a/src/telemini-v2.0/Makefile b/src/telemini-v2.0/Makefile index fcac2c48..606b4f3b 100644 --- a/src/telemini-v2.0/Makefile +++ b/src/telemini-v2.0/Makefile @@ -5,8 +5,8 @@ TELEMINI_VER=2.0 TELEMINI_DEF=2_0 -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/telepyro-v0.1/Makefile b/src/telepyro-v0.1/Makefile index 025b324a..dcac03dc 100644 --- a/src/telepyro-v0.1/Makefile +++ b/src/telepyro-v0.1/Makefile @@ -2,7 +2,7 @@ # AltOS build # # -vpath % .:..:../core:../product:../drivers:../avr +vpath % .:..:../kernel:../product:../drivers:../avr vpath ao-make-product.5c ../util include ../avr/Makefile.defs @@ -45,7 +45,7 @@ PRODUCT=TelePyro-v0.1 MCU=atmega32u4 PRODUCT_DEF=-DTELEPYRO IDPRODUCT=0x0011 -CFLAGS = $(PRODUCT_DEF) -I. -I../avr -I../core -I.. +CFLAGS = $(PRODUCT_DEF) -I. -I../avr -I../kernel -I.. CFLAGS += -mmcu=$(MCU) -Wall -Wstrict-prototypes -O3 -mcall-prologues -DAVR NICKLE=nickle diff --git a/src/telescience-pwm/Makefile b/src/telescience-pwm/Makefile index de81b8d7..493bd480 100644 --- a/src/telescience-pwm/Makefile +++ b/src/telescience-pwm/Makefile @@ -2,7 +2,7 @@ # AltOS build # # -vpath % ..:../core:../product:../drivers:../avr +vpath % ..:../kernel:../product:../drivers:../avr vpath ao-make-product.5c ../util include ../avr/Makefile.defs @@ -57,7 +57,7 @@ PRODUCT=TeleScience-PWM MCU=atmega32u4 PRODUCT_DEF=-DTELESCIENCE -DTELESCIENCE_PWM IDPRODUCT=0x0011 -CFLAGS = $(PRODUCT_DEF) -I. -I../avr -I../core -I.. +CFLAGS = $(PRODUCT_DEF) -I. -I../avr -I../kernel -I.. CFLAGS += -g -mmcu=$(MCU) -Wall -Wstrict-prototypes -O3 -mcall-prologues -DAVR NICKLE=nickle diff --git a/src/telescience-v0.1/Makefile b/src/telescience-v0.1/Makefile index 6e4eb6de..c55c48e2 100644 --- a/src/telescience-v0.1/Makefile +++ b/src/telescience-v0.1/Makefile @@ -2,7 +2,7 @@ # AltOS build # # -vpath % ..:../core:../product:../drivers:../avr +vpath % ..:../kernel:../product:../drivers:../avr vpath ao-make-product.5c ../util include ../avr/Makefile.defs @@ -56,7 +56,7 @@ PRODUCT=TeleScience-v0.1 MCU=atmega32u4 PRODUCT_DEF=-DTELESCIENCE IDPRODUCT=0x0011 -CFLAGS = $(PRODUCT_DEF) -I. -I../avr -I../core -I.. +CFLAGS = $(PRODUCT_DEF) -I. -I../avr -I../kernel -I.. CFLAGS += -g -mmcu=$(MCU) -Wall -Wstrict-prototypes -O3 -mcall-prologues -DAVR NICKLE=nickle diff --git a/src/teleshield-v0.1/Makefile b/src/teleshield-v0.1/Makefile index e8b262ef..f54488a2 100644 --- a/src/teleshield-v0.1/Makefile +++ b/src/teleshield-v0.1/Makefile @@ -17,8 +17,8 @@ TELESHIELD_SRC = \ ao_btm.c \ ao_spi.c -vpath %.c ..:../core:../cc1111:../drivers:../product:. -vpath %.h ..:../core:../cc1111:../drivers:../product:. +vpath %.c ..:../kernel:../cc1111:../drivers:../product:. +vpath %.h ..:../kernel:../cc1111:../drivers:../product:. vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/teleterra-v0.2/.sdcdbrc b/src/teleterra-v0.2/.sdcdbrc index fbe9a599..e9a51ea6 100644 --- a/src/teleterra-v0.2/.sdcdbrc +++ b/src/teleterra-v0.2/.sdcdbrc @@ -1 +1 @@ ---directory=../cc1111:../product:../core:../drivers:. +--directory=../cc1111:../product:../kernel:../drivers:. diff --git a/src/teleterra-v0.2/Makefile b/src/teleterra-v0.2/Makefile index 88637360..826c52e5 100644 --- a/src/teleterra-v0.2/Makefile +++ b/src/teleterra-v0.2/Makefile @@ -2,8 +2,8 @@ # TeleTerra build file # -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION diff --git a/src/test/Makefile b/src/test/Makefile index c6025219..7cb86140 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -1,4 +1,4 @@ -vpath % ..:../core:../drivers:../util:../micropeak:../aes:../product +vpath % ..:../kernel:../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 \ @@ -9,7 +9,7 @@ INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h ao_quat KALMAN=make-kalman -CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -I../product -O0 -g -Wall +CFLAGS=-I.. -I. -I../kernel -I../drivers -I../micropeak -I../product -O0 -g -Wall all: $(PROGS) ao_aprs_data.wav @@ -52,7 +52,7 @@ ao_kalman.h: $(KALMAN) (cd .. && make ao_kalman.h) ao_fec_test: ao_fec_test.c ao_fec_tx.c ao_fec_rx.c - cc $(CFLAGS) -DAO_FEC_DEBUG=1 -o $@ ao_fec_test.c ../core/ao_fec_tx.c ../core/ao_fec_rx.c -lm + cc $(CFLAGS) -DAO_FEC_DEBUG=1 -o $@ ao_fec_test.c ../kernel/ao_fec_tx.c ../kernel/ao_fec_rx.c -lm ao_aprs_test: ao_aprs_test.c ao_aprs.c cc $(CFLAGS) -o $@ ao_aprs_test.c diff --git a/src/tidongle/Makefile b/src/tidongle/Makefile index b2ba537b..0e90d744 100644 --- a/src/tidongle/Makefile +++ b/src/tidongle/Makefile @@ -2,8 +2,8 @@ # TIDongle build file # -vpath %.c ..:../core:../cc1111:../drivers:../product -vpath %.h ..:../core:../cc1111:../drivers:../product +vpath %.c ..:../kernel:../cc1111:../drivers:../product +vpath %.h ..:../kernel:../cc1111:../drivers:../product vpath ao-make-product.5c ../util ifndef VERSION -- cgit v1.2.3