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 --- 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 +++++ 92 files changed, 14747 insertions(+) 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/kernel') 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_ */ -- cgit v1.2.3 From 0d367fc24bfd0377db6f3b00a888a18245616767 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 5 Apr 2014 00:18:57 -0700 Subject: altos: Report battery voltage instead of S at startup This works on everything with a beeper except TeleMetrum v1.0 which just doesn't have enough flash space for the code. Signed-off-by: Keith Packard --- src/cc1111/ao_arch.h | 2 ++ src/cc1111/ao_pins.h | 19 +++++++++++++++++++ src/easymega-v1.0/ao_pins.h | 1 + src/easymini-v1.0/Makefile | 1 + src/easymini-v1.0/ao_pins.h | 20 +++++++++++++++++++- src/kernel/ao_convert_volt.c | 10 ++++++++-- src/kernel/ao_report.c | 41 +++++++++++++++++++++++++++++++++-------- src/lpc/ao_arch.h | 4 ++++ src/teleballoon-v1.1/ao_pins.h | 1 + src/teleballoon-v2.0/ao_pins.h | 1 + src/telemega-v0.1/ao_pins.h | 1 + src/telemega-v1.0/ao_pins.h | 1 + src/telemetrum-v1.1/Makefile | 1 + src/telemetrum-v1.2/Makefile | 1 + src/telemetrum-v2.0/ao_pins.h | 1 + src/telemini-v2.0/Makefile | 1 + src/telemini-v2.0/ao_pins.h | 18 ++++++++++++++++++ 17 files changed, 113 insertions(+), 11 deletions(-) (limited to 'src/kernel') diff --git a/src/cc1111/ao_arch.h b/src/cc1111/ao_arch.h index 34235b08..fcac331b 100644 --- a/src/cc1111/ao_arch.h +++ b/src/cc1111/ao_arch.h @@ -326,4 +326,6 @@ void ao_p0_isr(void) __interrupt(13); #endif +#define AO_ADC_MAX 32767 + #endif /* _AO_ARCH_H_ */ diff --git a/src/cc1111/ao_pins.h b/src/cc1111/ao_pins.h index 2f0e2884..5b1586e0 100644 --- a/src/cc1111/ao_pins.h +++ b/src/cc1111/ao_pins.h @@ -58,6 +58,7 @@ #define HAS_FLIGHT 1 #define HAS_USB 1 #define HAS_BEEP 1 + #define HAS_BATTERY_REPORT 1 #define HAS_GPS 1 #define HAS_SERIAL_1 1 #define HAS_ADC 1 @@ -94,6 +95,7 @@ #define HAS_FLIGHT 1 #define HAS_USB 1 #define HAS_BEEP 1 + #define HAS_BATTERY_REPORT 1 #define HAS_GPS 1 #define HAS_SERIAL_1 1 #define HAS_ADC 1 @@ -572,4 +574,21 @@ struct ao_adc { #endif }; +/* + * Voltage divider on ADC battery sampler + */ +#define AO_BATTERY_DIV_PLUS 5 /* 5k */ +#define AO_BATTERY_DIV_MINUS 10 /* 10k */ + +/* + * Voltage divider on ADC igniter samplers + */ +#define AO_IGNITE_DIV_PLUS 100 /* 100k */ +#define AO_IGNITE_DIV_MINUS 27 /* 27k */ + +/* + * ADC reference in decivolts + */ +#define AO_ADC_REFERENCE_DV 33 + #endif /* _AO_PINS_H_ */ diff --git a/src/easymega-v1.0/ao_pins.h b/src/easymega-v1.0/ao_pins.h index 0b6c0f07..fb6e05b1 100644 --- a/src/easymega-v1.0/ao_pins.h +++ b/src/easymega-v1.0/ao_pins.h @@ -66,6 +66,7 @@ #define USE_STORAGE_CONFIG 0 #define HAS_USB 1 #define HAS_BEEP 1 +#define HAS_BATTERY_REPORT 1 #define HAS_RADIO 0 #define HAS_TELEMETRY 0 #define HAS_APRS 0 diff --git a/src/easymini-v1.0/Makefile b/src/easymini-v1.0/Makefile index 7dae2692..654be22b 100644 --- a/src/easymini-v1.0/Makefile +++ b/src/easymini-v1.0/Makefile @@ -32,6 +32,7 @@ ALTOS_SRC = \ ao_sample.c \ ao_data.c \ ao_convert_pa.c \ + ao_convert_volt.c \ ao_task.c \ ao_log.c \ ao_log_mini.c \ diff --git a/src/easymini-v1.0/ao_pins.h b/src/easymini-v1.0/ao_pins.h index 6757047f..0edde5a2 100644 --- a/src/easymini-v1.0/ao_pins.h +++ b/src/easymini-v1.0/ao_pins.h @@ -15,7 +15,8 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. */ -#define HAS_BEEP 1 +#define HAS_BEEP 1 +#define HAS_BATTERY_REPORT 1 #define AO_STACK_SIZE 384 @@ -133,3 +134,20 @@ struct ao_adc { #define AO_ADC_DUMP(p) \ printf("tick: %5u apogee: %5d main: %5d batt: %5d\n", \ (p)->tick, (p)->adc.sense_a, (p)->adc.sense_m, (p)->adc.v_batt) + +/* + * Voltage divider on ADC battery sampler + */ +#define AO_BATTERY_DIV_PLUS 100 /* 100k */ +#define AO_BATTERY_DIV_MINUS 27 /* 27k */ + +/* + * Voltage divider on ADC igniter samplers + */ +#define AO_IGNITE_DIV_PLUS 100 /* 100k */ +#define AO_IGNITE_DIV_MINUS 27 /* 27k */ + +/* + * ADC reference in decivolts + */ +#define AO_ADC_REFERENCE_DV 33 diff --git a/src/kernel/ao_convert_volt.c b/src/kernel/ao_convert_volt.c index 8556d423..7afc34bf 100644 --- a/src/kernel/ao_convert_volt.c +++ b/src/kernel/ao_convert_volt.c @@ -17,17 +17,23 @@ #include "ao.h" -#define scale(v,p,m) ((int32_t) (v) * (AO_ADC_REFERENCE_DV * ((p) + (m))) / (AO_ADC_MAX * (m))) +#define MUL(p,m) ((int32_t) AO_ADC_REFERENCE_DV * ((p) + (m))) +#define ADD(p,m) (MUL(p,m)/2) +#define DIV(p,m) ((int32_t) AO_ADC_MAX * (m)) +#define scale(v,p,m) (((int32_t) (v) * MUL(p,m) + ADD(p,m)) / DIV(p,m)) +#if HAS_APRS || HAS_BATTERY_REPORT int16_t ao_battery_decivolt(int16_t adc) { return scale(adc, AO_BATTERY_DIV_PLUS, AO_BATTERY_DIV_MINUS); } +#endif +#if HAS_APRS int16_t ao_ignite_decivolt(int16_t adc) { return scale(adc, AO_IGNITE_DIV_PLUS, AO_IGNITE_DIV_MINUS); } - +#endif diff --git a/src/kernel/ao_report.c b/src/kernel/ao_report.c index 1104cd82..1a0e9e16 100644 --- a/src/kernel/ao_report.c +++ b/src/kernel/ao_report.c @@ -87,19 +87,18 @@ ao_report_digit(uint8_t digit) __reentrant } static void -ao_report_altitude(void) +ao_report_number(int16_t n) { - __pdata int16_t agl = ao_max_height; __xdata uint8_t digits[10]; __pdata uint8_t ndigits, i; - if (agl < 0) - agl = 0; + if (n < 0) + n = 0; ndigits = 0; do { - digits[ndigits++] = agl % 10; - agl /= 10; - } while (agl); + digits[ndigits++] = n % 10; + n /= 10; + } while (n); i = ndigits; do @@ -107,6 +106,27 @@ ao_report_altitude(void) while (i != 0); } +static void +ao_report_altitude(void) +{ + ao_report_number(ao_max_height); +} + +#if HAS_BATTERY_REPORT +static void +ao_report_battery(void) +{ + __xdata struct ao_data packet; + for (;;) { + ao_data_get(&packet); + if (packet.adc.v_batt != 0) + break; + ao_sleep(DATA_TO_XDATA(&ao_sample_data)); + } + ao_report_number(ao_battery_decivolt(packet.adc.v_batt)); +} +#endif + #if HAS_IGNITE_REPORT static uint8_t ao_report_igniter_ready(enum ao_igniter igniter) @@ -163,7 +183,12 @@ ao_report(void) { ao_report_state = ao_flight_state; for(;;) { - ao_report_beep(); +#if HAS_BATTERY_REPORT + if (ao_flight_state == ao_flight_startup) + ao_report_battery(); + else +#endif + ao_report_beep(); if (ao_flight_state == ao_flight_landed) { ao_report_altitude(); #if HAS_FLIGHT diff --git a/src/lpc/ao_arch.h b/src/lpc/ao_arch.h index 9dcfadc0..dd0debd4 100644 --- a/src/lpc/ao_arch.h +++ b/src/lpc/ao_arch.h @@ -143,4 +143,8 @@ ao_serial_init(void); #define AO_BOOT_LOADER_BASE ((uint32_t *) 0x00000000) #define HAS_BOOT_LOADER 1 +/* ADC definitions */ + +#define AO_ADC_MAX 32767 + #endif /* _AO_ARCH_H_ */ diff --git a/src/teleballoon-v1.1/ao_pins.h b/src/teleballoon-v1.1/ao_pins.h index 7ba48c96..4c23ca88 100644 --- a/src/teleballoon-v1.1/ao_pins.h +++ b/src/teleballoon-v1.1/ao_pins.h @@ -28,6 +28,7 @@ #define HAS_FLIGHT 1 #define HAS_USB 1 #define HAS_BEEP 1 + #define HAS_BATTERY_REPORT 1 #define HAS_GPS 1 #define HAS_SERIAL_1 1 #define HAS_ADC 1 diff --git a/src/teleballoon-v2.0/ao_pins.h b/src/teleballoon-v2.0/ao_pins.h index 22551d9e..f2515521 100644 --- a/src/teleballoon-v2.0/ao_pins.h +++ b/src/teleballoon-v2.0/ao_pins.h @@ -66,6 +66,7 @@ #define USE_STORAGE_CONFIG 0 #define HAS_USB 1 #define HAS_BEEP 1 +#define HAS_BATTERY_REPORT 1 #define BEEPER_CHANNEL 4 #define HAS_RADIO 1 #define HAS_TELEMETRY 1 diff --git a/src/telemega-v0.1/ao_pins.h b/src/telemega-v0.1/ao_pins.h index db397c66..a63c76a0 100644 --- a/src/telemega-v0.1/ao_pins.h +++ b/src/telemega-v0.1/ao_pins.h @@ -71,6 +71,7 @@ #define USE_STORAGE_CONFIG 0 #define HAS_USB 1 #define HAS_BEEP 1 +#define HAS_BATTERY_REPORT 1 #define HAS_RADIO 1 #define HAS_TELEMETRY 1 #define HAS_APRS 1 diff --git a/src/telemega-v1.0/ao_pins.h b/src/telemega-v1.0/ao_pins.h index fe97c684..7f0b1f94 100644 --- a/src/telemega-v1.0/ao_pins.h +++ b/src/telemega-v1.0/ao_pins.h @@ -71,6 +71,7 @@ #define USE_STORAGE_CONFIG 0 #define HAS_USB 1 #define HAS_BEEP 1 +#define HAS_BATTERY_REPORT 1 #define HAS_RADIO 1 #define HAS_TELEMETRY 1 #define HAS_APRS 1 diff --git a/src/telemetrum-v1.1/Makefile b/src/telemetrum-v1.1/Makefile index e44be7f9..61e9273b 100644 --- a/src/telemetrum-v1.1/Makefile +++ b/src/telemetrum-v1.1/Makefile @@ -12,6 +12,7 @@ TM_SRC = \ ao_companion.c \ ao_gps_skytraq.c \ ao_gps_show.c \ + ao_convert_volt.c \ ao_m25.c include ../product/Makefile.telemetrum diff --git a/src/telemetrum-v1.2/Makefile b/src/telemetrum-v1.2/Makefile index f2285fbe..38ba6d49 100644 --- a/src/telemetrum-v1.2/Makefile +++ b/src/telemetrum-v1.2/Makefile @@ -12,6 +12,7 @@ TM_SRC = \ ao_companion.c \ ao_gps_skytraq.c \ ao_gps_show.c \ + ao_convert_volt.c \ ao_m25.c include ../product/Makefile.telemetrum diff --git a/src/telemetrum-v2.0/ao_pins.h b/src/telemetrum-v2.0/ao_pins.h index 1b5cedc7..2ccac628 100644 --- a/src/telemetrum-v2.0/ao_pins.h +++ b/src/telemetrum-v2.0/ao_pins.h @@ -66,6 +66,7 @@ #define USE_STORAGE_CONFIG 0 #define HAS_USB 1 #define HAS_BEEP 1 +#define HAS_BATTERY_REPORT 1 #define BEEPER_CHANNEL 4 #define HAS_RADIO 1 #define HAS_TELEMETRY 1 diff --git a/src/telemini-v2.0/Makefile b/src/telemini-v2.0/Makefile index 606b4f3b..34f35cd9 100644 --- a/src/telemini-v2.0/Makefile +++ b/src/telemini-v2.0/Makefile @@ -59,6 +59,7 @@ CC1111_SRC = \ ao_spi.c \ ao_usb.c \ ao_convert_pa.c \ + ao_convert_volt.c \ ao_beep.c \ ao_timer.c \ ao_exti.c \ diff --git a/src/telemini-v2.0/ao_pins.h b/src/telemini-v2.0/ao_pins.h index c4681ee2..dc18aff7 100644 --- a/src/telemini-v2.0/ao_pins.h +++ b/src/telemini-v2.0/ao_pins.h @@ -24,6 +24,7 @@ #define HAS_USB 1 #define USB_FORCE_FLIGHT_IDLE 1 #define HAS_BEEP 1 +#define HAS_BATTERY_REPORT 1 #define HAS_GPS 0 #define HAS_SERIAL_1 0 #define HAS_EEPROM 1 @@ -158,4 +159,21 @@ struct ao_adc { ao_data_ring[ao_data_head].ms5607_raw.temp = ao_ms5607_current.temp; \ } while (0) +/* + * Voltage divider on ADC battery sampler + */ +#define AO_BATTERY_DIV_PLUS 100 /* 100k */ +#define AO_BATTERY_DIV_MINUS 27 /* 27k */ + +/* + * Voltage divider on ADC igniter samplers + */ +#define AO_IGNITE_DIV_PLUS 100 /* 100k */ +#define AO_IGNITE_DIV_MINUS 27 /* 27k */ + +/* + * ADC reference in decivolts + */ +#define AO_ADC_REFERENCE_DV 33 + #endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From 3b5c4d88671e6c511fbfb1ce6b046f558dd6c2bf Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 12 Apr 2014 17:46:34 -0700 Subject: altos: Switch beeping to farnsworth spacing Use 17wpm/12wpm farnsworth spacing for the state reports. Leave the numeric reports running slowly as those require counting. Signed-off-by: Keith Packard --- src/kernel/ao_report.c | 62 ++++++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 58 insertions(+), 4 deletions(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_report.c b/src/kernel/ao_report.c index 1a0e9e16..f2263154 100644 --- a/src/kernel/ao_report.c +++ b/src/kernel/ao_report.c @@ -52,6 +52,60 @@ static const uint8_t flight_reports[] = { static __pdata enum ao_flight_state ao_report_state; +/* + * Farnsworth spacing + * + * From: http://www.arrl.org/files/file/Technology/x9004008.pdf + * + * c: character rate in wpm + * s: overall rate in wpm + * u: unit rate (dit speed) + * + * dit: u + * dah: 3u + * intra-character-time: u + * + * u = 1.2/c + * + * Because our clock runs at 10ms, we'll round up to 70ms for u, which + * is about 17wpm + * + * Farnsworth adds space between characters and + * words: + * 60 c - 37.2 s + * Ta = ------------- + * sc + * + * 3 Ta + * Tc = ---- + * 19 + * + * 7 Ta + * Tw = ---- + * 19 + * + * Ta = total delay to add to the characters (31 units) + * of a standard 50-unit "word", in seconds + * + * Tc = period between characters, in seconds + * + * Tw = period between words, in seconds + * + * We'll use Farnsworth spacing with c=18 and s=12: + * + * u = 1.2/18 = 0.0667 + * + * Ta = (60 * 17 - 37.2 * 12) / (17 * 12) = 2.812 + * + * Tc = 3 * Ta / 19 = .444 + * + * Tw = 1.036 + * + * Note that the values below are all reduced by 10ms; that's because + * the timer always adds a tick to make sure the task actually sleeps + * at least as long as the argument. + */ + static void ao_report_beep(void) __reentrant { @@ -62,13 +116,13 @@ ao_report_beep(void) __reentrant 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)); + else + mid(AO_MS_TO_TICKS(60)); + pause(AO_MS_TO_TICKS(60)); r >>= 1; } - pause(AO_MS_TO_TICKS(400)); + pause(AO_MS_TO_TICKS(360)); } static void -- cgit v1.2.3 From 027b1470c7a2d007eaab5c8d49f772b0c7559b80 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 2 May 2014 12:26:07 -0700 Subject: altos: Add configurable beep tone This lets you directly set the mid-range beep tone; the high and low tones remain set off of that in the same ratio as before. Note that none of the cc1111 products get this feature as they don't have enough flash space anymore... Signed-off-by: Keith Packard --- src/cc1111/ao_pins.h | 5 +++++ src/kernel/ao.h | 5 ++++- src/kernel/ao_beep.h | 19 ++++++++++++++++--- src/kernel/ao_config.c | 35 +++++++++++++++++++++++++++++++++-- src/kernel/ao_telemetry.h | 6 +++--- src/telemini-v2.0/ao_pins.h | 1 + 6 files changed, 62 insertions(+), 9 deletions(-) (limited to 'src/kernel') diff --git a/src/cc1111/ao_pins.h b/src/cc1111/ao_pins.h index 5b1586e0..91bf81d2 100644 --- a/src/cc1111/ao_pins.h +++ b/src/cc1111/ao_pins.h @@ -24,6 +24,7 @@ #define HAS_FLIGHT 1 #define HAS_USB 1 #define HAS_BEEP 1 + #define HAS_BEEP_CONFIG 0 #define HAS_GPS 1 #define HAS_SERIAL_1 1 #define HAS_ADC 1 @@ -58,6 +59,7 @@ #define HAS_FLIGHT 1 #define HAS_USB 1 #define HAS_BEEP 1 + #define HAS_BEEP_CONFIG 0 #define HAS_BATTERY_REPORT 1 #define HAS_GPS 1 #define HAS_SERIAL_1 1 @@ -95,6 +97,7 @@ #define HAS_FLIGHT 1 #define HAS_USB 1 #define HAS_BEEP 1 + #define HAS_BEEP_CONFIG 0 #define HAS_BATTERY_REPORT 1 #define HAS_GPS 1 #define HAS_SERIAL_1 1 @@ -212,6 +215,7 @@ #define HAS_FLIGHT 1 #define HAS_USB 1 #define HAS_BEEP 1 + #define HAS_BEEP_CONFIG 0 #define HAS_GPS 1 #define HAS_SERIAL_1 1 #define HAS_ADC 1 @@ -333,6 +337,7 @@ #define HAS_FLIGHT 0 #define HAS_USB 1 #define HAS_BEEP 1 + #define HAS_BEEP_CONFIG 0 #define HAS_SERIAL_1 1 #define HAS_SERIAL_1_ALT_1 1 #define HAS_SERIAL_1_ALT_2 0 diff --git a/src/kernel/ao.h b/src/kernel/ao.h index 29ad2603..9169bd84 100644 --- a/src/kernel/ao.h +++ b/src/kernel/ao.h @@ -750,7 +750,7 @@ extern __xdata uint8_t ao_force_freq; #endif #define AO_CONFIG_MAJOR 1 -#define AO_CONFIG_MINOR 15 +#define AO_CONFIG_MINOR 16 #define AO_AES_LEN 16 @@ -789,6 +789,9 @@ struct ao_config { int16_t accel_zero_across; /* minor version 15 */ int16_t accel_zero_through; /* minor version 15 */ #endif +#if HAS_BEEP + uint8_t mid_beep; /* minor version 16 */ +#endif }; #define AO_IGNITE_MODE_DUAL 0 diff --git a/src/kernel/ao_beep.h b/src/kernel/ao_beep.h index 55f61171..9d6ecf27 100644 --- a/src/kernel/ao_beep.h +++ b/src/kernel/ao_beep.h @@ -18,6 +18,12 @@ #ifndef _AO_BEEP_H_ #define _AO_BEEP_H_ +#ifndef HAS_BEEP_CONFIG +#if defined(USE_EEPROM_CONFIG) && USE_EEPROM_CONFIG || HAS_EEPROM +#define HAS_BEEP_CONFIG 1 +#endif +#endif + /* * ao_beep.c */ @@ -28,9 +34,16 @@ * 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_MID_DEFAULT 94 /* 3989Hz */ + +#if HAS_BEEP_CONFIG +#define AO_BEEP_MID ao_config.mid_beep +#else +#define AO_BEEP_MID AO_BEEP_MID_DEFAULT +#endif +#define AO_BEEP_LOW AO_BEEP_MID * 150 / 94 /* 2500Hz */ +#define AO_BEEP_HIGH AO_BEEP_MID * 75 / 94 /* 5000Hz */ + #define AO_BEEP_OFF 0 /* off */ #define AO_BEEP_g 240 /* 1562.5Hz */ diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c index 4482f673..411399d7 100644 --- a/src/kernel/ao_config.c +++ b/src/kernel/ao_config.c @@ -22,6 +22,9 @@ #include #include #endif +#if HAS_BEEP +#include +#endif __xdata struct ao_config ao_config; __pdata uint8_t ao_config_loaded; @@ -170,6 +173,10 @@ _ao_config_get(void) ao_config.accel_plus_g = 0; ao_config.accel_minus_g = 0; } +#endif +#if HAS_BEEP_CONFIG + if (minor < 16) + ao_config.mid_beep = AO_BEEP_MID_DEFAULT; #endif ao_config.minor = AO_CONFIG_MINOR; ao_config_dirty = 1; @@ -357,7 +364,7 @@ ao_config_accel_calibrate_set(void) __reentrant 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; @@ -555,7 +562,7 @@ ao_config_radio_enable_set(void) __reentrant _ao_config_edit_finish(); } #endif /* HAS_RADIO */ - + #if HAS_AES __xdata uint8_t ao_config_aes_seq = 1; @@ -650,6 +657,26 @@ ao_config_radio_power_set(void) #endif +#if HAS_BEEP_CONFIG +void +ao_config_beep_show(void) +{ + printf ("Beeper setting: %d\n", ao_config.mid_beep); +} + +void +ao_config_beep_set(void) +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.mid_beep = ao_cmd_lex_i; + _ao_config_edit_finish(); +} +#endif + + struct ao_config_var { __code char *str; void (*set)(void) __reentrant; @@ -719,6 +746,10 @@ __code struct ao_config_var ao_config_vars[] = { #if HAS_APRS { "A \0APRS packet interval (0 disable)", ao_config_aprs_set, ao_config_aprs_show }, +#endif +#if HAS_BEEP_CONFIG + { "b \0Configure beeper tone (freq = 1/2 (24e6/32) / beep", + ao_config_beep_set, ao_config_beep_show }, #endif { "s\0Show", ao_config_show, 0 }, diff --git a/src/kernel/ao_telemetry.h b/src/kernel/ao_telemetry.h index 237a35ab..fe07c2af 100644 --- a/src/kernel/ao_telemetry.h +++ b/src/kernel/ao_telemetry.h @@ -154,7 +154,7 @@ struct ao_telemetry_companion { uint16_t companion_data[AO_COMPANION_MAX_CHANNELS]; /* 8 */ /* 32 */ }; - + #define AO_TELEMETRY_MEGA_SENSOR 0x08 struct ao_telemetry_mega_sensor { @@ -181,7 +181,7 @@ struct ao_telemetry_mega_sensor { int16_t mag_z; /* 30 */ /* 32 */ }; - + #define AO_TELEMETRY_MEGA_DATA 0x09 struct ao_telemetry_mega_data { @@ -231,7 +231,7 @@ struct ao_telemetry_metrum_sensor { uint8_t pad[6]; /* 26 */ /* 32 */ }; - + #define AO_TELEMETRY_METRUM_DATA 0x0B struct ao_telemetry_metrum_data { diff --git a/src/telemini-v2.0/ao_pins.h b/src/telemini-v2.0/ao_pins.h index dc18aff7..1b4f602f 100644 --- a/src/telemini-v2.0/ao_pins.h +++ b/src/telemini-v2.0/ao_pins.h @@ -24,6 +24,7 @@ #define HAS_USB 1 #define USB_FORCE_FLIGHT_IDLE 1 #define HAS_BEEP 1 +#define HAS_BEEP_CONFIG 0 #define HAS_BATTERY_REPORT 1 #define HAS_GPS 0 #define HAS_SERIAL_1 0 -- cgit v1.2.3 From d59d6787bfe26c3b18491ece602ad6cc5cf26c42 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 3 May 2014 10:58:31 -0700 Subject: altos: 8051 64 * 16 multiply function was broken for negative 64-bit It was jumping around the actual multiply when the 64-bit argument was negative. Signed-off-by: Keith Packard --- src/kernel/ao_int64.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_int64.c b/src/kernel/ao_int64.c index aa23dbe0..ca75751b 100644 --- a/src/kernel/ao_int64.c +++ b/src/kernel/ao_int64.c @@ -17,8 +17,6 @@ #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; @@ -151,8 +149,8 @@ void ao_mul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG uint16_t b) ao_neg64(&ap, a); a = ≈ negative++; - } else - ao_umul64_64_16(r, a, b); + } + ao_umul64_64_16(r, a, b); if (negative) ao_neg64(r, r); } -- cgit v1.2.3 From c8ad50495e2d81209a4882dd4f82c19d9ae2ac34 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 5 May 2014 23:45:30 -0700 Subject: altos: Fix byte offsets in the mega AO_LOG_FLIGHT packets Just comments, but even those should be correct Signed-off-by: Keith Packard --- src/kernel/ao_log.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_log.h b/src/kernel/ao_log.h index 09f31188..3ff9e811 100644 --- a/src/kernel/ao_log.h +++ b/src/kernel/ao_log.h @@ -206,9 +206,9 @@ struct ao_log_mega { 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_accel_along; /* 12 */ + int16_t ground_accel_across; /* 14 */ + int16_t ground_accel_through; /* 16 */ int16_t ground_roll; /* 18 */ int16_t ground_pitch; /* 20 */ int16_t ground_yaw; /* 22 */ -- cgit v1.2.3 From d1908101241b1002fbc582b0a2c27045065a6615 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 12 May 2014 22:51:16 -0700 Subject: altos: Report amount of program space available in the version command Signed-off-by: Keith Packard --- src/kernel/ao_cmd.c | 6 ++++++ 1 file changed, 6 insertions(+) (limited to 'src/kernel') diff --git a/src/kernel/ao_cmd.c b/src/kernel/ao_cmd.c index 4ebaa607..cd662314 100644 --- a/src/kernel/ao_cmd.c +++ b/src/kernel/ao_cmd.c @@ -279,6 +279,9 @@ version(void) #endif #if HAS_LOG "log-format %u\n" +#endif +#if defined(AO_BOOT_APPLICATION_BASE) && defined(AO_BOOT_APPLICATION_BOUND) + "program-space %u\n" #endif , ao_manufacturer , ao_product @@ -288,6 +291,9 @@ version(void) #endif #if HAS_LOG , ao_log_format +#endif +#if defined(AO_BOOT_APPLICATION_BASE) && defined(AO_BOOT_APPLICATION_BOUND) + , (uint32_t) AO_BOOT_APPLICATION_BOUND - (uint32_t) AO_BOOT_APPLICATION_BASE #endif ); printf("software-version %s\n", ao_version); -- cgit v1.2.3 From 6833e466d7d77765199bf4d21437c34a4eceb044 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 15 May 2014 23:46:41 -0600 Subject: altos: stm and lpc ao_boot.h were identical. move to kernel. These two files were absolutely identical, so share them by moving under kernel instead.x Signed-off-by: Keith Packard --- src/kernel/ao_boot.h | 41 +++++++++++++++++++++++++++++++++++++++++ src/lpc/ao_boot.h | 41 ----------------------------------------- src/stm/ao_boot.h | 41 ----------------------------------------- 3 files changed, 41 insertions(+), 82 deletions(-) create mode 100644 src/kernel/ao_boot.h delete mode 100644 src/lpc/ao_boot.h delete mode 100644 src/stm/ao_boot.h (limited to 'src/kernel') diff --git a/src/kernel/ao_boot.h b/src/kernel/ao_boot.h new file mode 100644 index 00000000..62392d25 --- /dev/null +++ b/src/kernel/ao_boot.h @@ -0,0 +1,41 @@ +/* + * 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_BOOT_H_ +#define _AO_BOOT_H_ + +void +ao_boot_chain(uint32_t *base); + +void +ao_boot_check_pin(void); + +/* Return true to switch to application (if present) */ +int +ao_boot_check_chain(void); + +void +ao_boot_reboot(uint32_t *base); + +#define AO_BOOT_FORCE_LOADER ((uint32_t *) 0) + +static inline void +ao_boot_loader(void) { + ao_boot_reboot(AO_BOOT_FORCE_LOADER); +} + +#endif /* _AO_BOOT_H_ */ diff --git a/src/lpc/ao_boot.h b/src/lpc/ao_boot.h deleted file mode 100644 index 62392d25..00000000 --- a/src/lpc/ao_boot.h +++ /dev/null @@ -1,41 +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_BOOT_H_ -#define _AO_BOOT_H_ - -void -ao_boot_chain(uint32_t *base); - -void -ao_boot_check_pin(void); - -/* Return true to switch to application (if present) */ -int -ao_boot_check_chain(void); - -void -ao_boot_reboot(uint32_t *base); - -#define AO_BOOT_FORCE_LOADER ((uint32_t *) 0) - -static inline void -ao_boot_loader(void) { - ao_boot_reboot(AO_BOOT_FORCE_LOADER); -} - -#endif /* _AO_BOOT_H_ */ diff --git a/src/stm/ao_boot.h b/src/stm/ao_boot.h deleted file mode 100644 index 62392d25..00000000 --- a/src/stm/ao_boot.h +++ /dev/null @@ -1,41 +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_BOOT_H_ -#define _AO_BOOT_H_ - -void -ao_boot_chain(uint32_t *base); - -void -ao_boot_check_pin(void); - -/* Return true to switch to application (if present) */ -int -ao_boot_check_chain(void); - -void -ao_boot_reboot(uint32_t *base); - -#define AO_BOOT_FORCE_LOADER ((uint32_t *) 0) - -static inline void -ao_boot_loader(void) { - ao_boot_reboot(AO_BOOT_FORCE_LOADER); -} - -#endif /* _AO_BOOT_H_ */ -- cgit v1.2.3 From af782e92c6a0c0a6b0fc2fa52519749a88ca8fb8 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 21 May 2014 01:36:40 -0700 Subject: altos: Expose ao_gps_set_rate from u-blox driver This lets applications set the desired GPS update rate to reduce power usage Signed-off-by: Keith Packard --- src/drivers/ao_gps_ublox.c | 11 +++++++++-- src/kernel/ao.h | 3 +++ 2 files changed, 12 insertions(+), 2 deletions(-) (limited to 'src/kernel') diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index 8676a670..077698a9 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -600,6 +600,14 @@ static const uint8_t ublox_enable_nav[] = { UBLOX_NAV_TIMEUTC }; +void +ao_gps_set_rate(uint8_t rate) +{ + uint8_t i; + for (i = 0; i < sizeof (ublox_enable_nav); i++) + ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], rate); +} + void ao_gps(void) __reentrant { @@ -616,8 +624,7 @@ ao_gps(void) __reentrant ao_ublox_set_message_rate(UBLOX_NAV, ublox_disable_nav[i], 0); /* Enable all of the messages we want */ - for (i = 0; i < sizeof (ublox_enable_nav); i++) - ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], 1); + ao_gps_set_rate(1); ao_ublox_set_navigation_settings((1 << UBLOX_CFG_NAV5_MASK_DYN) | (1 << UBLOX_CFG_NAV5_MASK_FIXMODE), UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_4G, diff --git a/src/kernel/ao.h b/src/kernel/ao.h index 9169bd84..b1f850ba 100644 --- a/src/kernel/ao.h +++ b/src/kernel/ao.h @@ -393,6 +393,9 @@ struct ao_gps_tracking_orig { struct ao_gps_sat_orig sats[AO_MAX_GPS_TRACKING]; }; +void +ao_gps_set_rate(uint8_t rate); + void ao_gps(void); -- cgit v1.2.3 From 2a3846df381a5eeac8ec3327c770af502aaf4e76 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 21 May 2014 01:37:57 -0700 Subject: altos: Don't define ao_ignite_decivolt without igniters Signed-off-by: Keith Packard --- src/kernel/ao_convert_volt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_convert_volt.c b/src/kernel/ao_convert_volt.c index 7afc34bf..f697e748 100644 --- a/src/kernel/ao_convert_volt.c +++ b/src/kernel/ao_convert_volt.c @@ -30,7 +30,7 @@ ao_battery_decivolt(int16_t adc) } #endif -#if HAS_APRS +#if HAS_APRS && defined(AO_IGNITE_DIV_PLUS) int16_t ao_ignite_decivolt(int16_t adc) { -- cgit v1.2.3 From 2625a464417c8475c66101757ca2c30cd6c74e0c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 21 May 2014 14:02:35 -0700 Subject: altos: Add config values for tracker start motion limits TeleGPS switches from 'pad' to 'drogue' states after the device moves a specified distance from the initial starting point. These values can be configured, and this is the configuration for them. Signed-off-by: Keith Packard --- src/kernel/ao.h | 6 +++++- src/kernel/ao_config.c | 50 ++++++++++++++++++++++++++++++++++++++++++++----- src/kernel/ao_tracker.h | 27 ++++++++++++++++++++++++++ 3 files changed, 77 insertions(+), 6 deletions(-) create mode 100644 src/kernel/ao_tracker.h (limited to 'src/kernel') diff --git a/src/kernel/ao.h b/src/kernel/ao.h index b1f850ba..0ffc647a 100644 --- a/src/kernel/ao.h +++ b/src/kernel/ao.h @@ -753,7 +753,7 @@ extern __xdata uint8_t ao_force_freq; #endif #define AO_CONFIG_MAJOR 1 -#define AO_CONFIG_MINOR 16 +#define AO_CONFIG_MINOR 17 #define AO_AES_LEN 16 @@ -795,6 +795,10 @@ struct ao_config { #if HAS_BEEP uint8_t mid_beep; /* minor version 16 */ #endif +#if HAS_TRACKER + uint16_t tracker_start_horiz; /* minor version 17 */ + uint16_t tracker_start_vert; /* minor version 17 */ +#endif }; #define AO_IGNITE_MODE_DUAL 0 diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c index 411399d7..e5f8efba 100644 --- a/src/kernel/ao_config.c +++ b/src/kernel/ao_config.c @@ -25,6 +25,9 @@ #if HAS_BEEP #include #endif +#if HAS_TRACKER +#include +#endif __xdata struct ao_config ao_config; __pdata uint8_t ao_config_loaded; @@ -177,6 +180,12 @@ _ao_config_get(void) #if HAS_BEEP_CONFIG if (minor < 16) ao_config.mid_beep = AO_BEEP_MID_DEFAULT; +#endif +#if HAS_TRACKER + if (minor < 17) { + ao_config.tracker_start_horiz = AO_CONFIG_DEFAULT_TRACKER_START_HORIZ; + ao_config.tracker_start_vert = AO_CONFIG_DEFAULT_TRACKER_START_VERT; + } #endif ao_config.minor = AO_CONFIG_MINOR; ao_config_dirty = 1; @@ -676,6 +685,33 @@ ao_config_beep_set(void) } #endif +#if HAS_TRACKER +void +ao_config_tracker_show(void) +{ + printf ("Tracker setting: %d %d\n", + ao_config.tracker_start_horiz, + ao_config.tracker_start_vert); +} + +void +ao_config_tracker_set(void) +{ + uint16_t h, v; + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + h = ao_cmd_lex_i; + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + v = ao_cmd_lex_i; + _ao_config_edit_start(); + ao_config.tracker_start_horiz = h; + ao_config.tracker_start_vert = v; + _ao_config_edit_finish(); +} +#endif /* HAS_TRACKER */ struct ao_config_var { __code char *str; @@ -724,7 +760,7 @@ __code struct ao_config_var ao_config_vars[] = { #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", + { "o <0 antenna up, 1 antenna down>\0Pad orientation", ao_config_pad_orientation_set,ao_config_pad_orientation_show }, #endif /* HAS_ACCEL */ #if HAS_LOG @@ -732,15 +768,15 @@ __code struct ao_config_var ao_config_vars[] = { ao_config_log_set, ao_config_log_show }, #endif #if HAS_IGNITE - { "i <0 dual, 1 apogee, 2 main>\0Set igniter mode", + { "i <0 dual, 1 apogee, 2 main>\0Igniter mode", ao_config_ignite_mode_set, ao_config_ignite_mode_show }, #endif #if HAS_AES - { "k <32 hex digits>\0Set AES encryption key", + { "k <32 hex digits>\0AES encryption key", ao_config_key_set, ao_config_key_show }, #endif #if AO_PYRO_NUM - { "P \0Configure pyro channels", + { "P \0Pyro channels", ao_pyro_set, ao_pyro_show }, #endif #if HAS_APRS @@ -748,8 +784,12 @@ __code struct ao_config_var ao_config_vars[] = { ao_config_aprs_set, ao_config_aprs_show }, #endif #if HAS_BEEP_CONFIG - { "b \0Configure beeper tone (freq = 1/2 (24e6/32) / beep", + { "b \0Beeper tone (freq = 1/2 (24e6/32) / beep", ao_config_beep_set, ao_config_beep_show }, +#endif +#if HAS_TRACKER + { "t \0Tracker start trigger distances", + ao_config_tracker_set, ao_config_tracker_show }, #endif { "s\0Show", ao_config_show, 0 }, diff --git a/src/kernel/ao_tracker.h b/src/kernel/ao_tracker.h new file mode 100644 index 00000000..43556965 --- /dev/null +++ b/src/kernel/ao_tracker.h @@ -0,0 +1,27 @@ +/* + * 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. + */ + +#ifndef _AO_TRACKER_H_ +#define _AO_TRACKER_H_ + +#define AO_CONFIG_DEFAULT_TRACKER_START_HORIZ 1000 +#define AO_CONFIG_DEFAULT_TRACKER_START_VERT 100 + +void +ao_tracker_init(void); + +#endif /* _AO_TRACKER_H_ */ -- cgit v1.2.3 From 1894b51daceaf9fb6b49a0625e09a366985d15b6 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 25 May 2014 21:12:29 -0700 Subject: altos: Move ao_config declarations to ao_config.h No sense leaving these in ao.h, and it's nice to make that file smaller Signed-off-by: Keith Packard --- src/kernel/ao.h | 83 +----------------------------------------------- src/kernel/ao_config.h | 85 ++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 86 insertions(+), 82 deletions(-) (limited to 'src/kernel') diff --git a/src/kernel/ao.h b/src/kernel/ao.h index 0ffc647a..5ff9b518 100644 --- a/src/kernel/ao.h +++ b/src/kernel/ao.h @@ -740,6 +740,7 @@ ao_igniter_init(void); /* * ao_config.c */ +#include #if AO_PYRO_NUM #include @@ -752,88 +753,6 @@ ao_igniter_init(void); extern __xdata uint8_t ao_force_freq; #endif -#define AO_CONFIG_MAJOR 1 -#define AO_CONFIG_MINOR 17 - -#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 -#if HAS_BEEP - uint8_t mid_beep; /* minor version 16 */ -#endif -#if HAS_TRACKER - uint16_t tracker_start_horiz; /* minor version 17 */ - uint16_t tracker_start_vert; /* minor version 17 */ -#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 */ diff --git a/src/kernel/ao_config.h b/src/kernel/ao_config.h index e101af8e..85673764 100644 --- a/src/kernel/ao_config.h +++ b/src/kernel/ao_config.h @@ -18,6 +18,8 @@ #ifndef _AO_CONFIG_H_ #define _AO_CONFIG_H_ +#include + #ifndef USE_STORAGE_CONFIG #define USE_STORAGE_CONFIG 1 #endif @@ -50,4 +52,87 @@ #endif +#define AO_CONFIG_MAJOR 1 +#define AO_CONFIG_MINOR 17 + +#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 +#if HAS_BEEP + uint8_t mid_beep; /* minor version 16 */ +#endif +#if HAS_TRACKER + uint16_t tracker_start_horiz; /* minor version 17 */ + uint16_t tracker_start_vert; /* minor version 17 */ +#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 + +#define AO_CONFIG_MAX_SIZE 128 + +extern __xdata struct ao_config ao_config; +extern __pdata uint8_t ao_config_loaded; + +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); + #endif /* _AO_CONFIG_H_ */ -- cgit v1.2.3 From c674a20432c2cb97e5bc2a3de891f78b9e172fe9 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 27 May 2014 11:05:02 -0700 Subject: altos: Fake flight code changes in kernel and stm Redirects data input from local sensors to USB sourced data, leaving USB enabled when the computer goes into pad mode. Signed-off-by: Keith Packard --- src/kernel/ao_fake_flight.c | 219 ++++++++++++++++++++++++++++++++++++++++++++ src/kernel/ao_fake_flight.h | 53 +++++++++++ src/kernel/ao_flight.c | 10 +- src/kernel/ao_pyro.h | 6 +- src/stm/ao_timer.c | 10 +- 5 files changed, 294 insertions(+), 4 deletions(-) create mode 100644 src/kernel/ao_fake_flight.c create mode 100644 src/kernel/ao_fake_flight.h (limited to 'src/kernel') diff --git a/src/kernel/ao_fake_flight.c b/src/kernel/ao_fake_flight.c new file mode 100644 index 00000000..11329bb9 --- /dev/null +++ b/src/kernel/ao_fake_flight.c @@ -0,0 +1,219 @@ +/* + * 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 +#include +#if HAS_MS5607 || HAS_MS5611 +#include +#endif + +uint8_t ao_fake_flight_active; + +static uint8_t ao_fake_has_cur; +static volatile uint8_t ao_fake_has_next; +static uint8_t ao_fake_has_offset; +static uint16_t ao_fake_tick_offset; +static struct ao_data ao_fake_cur, ao_fake_next; + +void +ao_fake_flight_poll(void) +{ + if (ao_fake_has_next && (ao_tick_count - ao_fake_next.tick) >= 0) { + ao_fake_cur = ao_fake_next; + ao_fake_has_next = 0; + ao_wakeup((void *) &ao_fake_has_next); + ao_fake_has_cur = 1; + } + if (!ao_fake_has_cur) + return; + ao_data_ring[ao_data_head] = ao_fake_cur; + ao_data_ring[ao_data_head].tick = ao_tick_count; + ao_data_head = ao_data_ring_next(ao_data_head); + ao_wakeup((void *) &ao_data_head); +} + +static uint8_t +ao_fake_data_read(void) +{ + uint8_t i; + uint8_t *d = (void *) &ao_fake_next; + + if (getchar() == 0) + return FALSE; + for (i = 0; i < sizeof (struct ao_data); i++) + *d++ = getchar(); + if (!ao_fake_has_offset) { + ao_fake_tick_offset = (ao_tick_count + 1000) - ao_fake_next.tick; + ao_fake_next.tick = ao_tick_count; + ao_fake_has_offset = 1; + } else + ao_fake_next.tick += ao_fake_tick_offset; + ao_fake_has_next = 1; + return TRUE; +} + +static void +ao_fake_calib_get(struct ao_fake_calib *calib) +{ +#if HAS_ACCEL + calib->accel_plus_g = ao_config.accel_plus_g; + calib->accel_minus_g = ao_config.accel_minus_g; +#endif +#if HAS_GYRO + calib->accel_zero_along = ao_config.accel_zero_along; + calib->accel_zero_across = ao_config.accel_zero_across; + calib->accel_zero_through = ao_config.accel_zero_through; +#endif +#if HAS_MS5607 || HAS_MS5611 + calib->ms5607_prom = ao_ms5607_prom; +#endif +} + +static void +ao_fake_calib_set(struct ao_fake_calib *calib) +{ +#if HAS_ACCEL + ao_config.accel_plus_g = calib->accel_plus_g; + ao_config.accel_minus_g = calib->accel_minus_g; +#endif +#if HAS_GYRO + ao_config.accel_zero_along = calib->accel_zero_along; + ao_config.accel_zero_across = calib->accel_zero_across; + ao_config.accel_zero_through = calib->accel_zero_through; +#endif +#if HAS_MS5607 || HAS_MS5611 + ao_ms5607_prom = calib->ms5607_prom; +#endif +} + +static uint8_t +ao_fake_calib_read(void) +{ + struct ao_fake_calib ao_calib; + uint8_t *d = (void *) &ao_calib; + uint16_t i; + + /* Read calibration data */ + for (i = 0; i < sizeof (struct ao_fake_calib); i++) + *d++ = getchar(); + if (ao_calib.major != AO_FAKE_CALIB_MAJOR +#if AO_FAKE_CALIB_MINOR != 0 + || ao_calib.minor < AO_FAKE_CALIB_MINOR +#endif + ) { + printf ("Calibration data major version mismatch %d.%d <= %d.%d\n", + ao_calib.major, ao_calib.minor, AO_FAKE_CALIB_MAJOR, AO_FAKE_CALIB_MINOR); + return FALSE; + } + ao_fake_calib_set(&ao_calib); + return TRUE; +} + +static void +ao_fake_flight(void) +{ + int16_t calib_size, data_size; + struct ao_fake_calib save_calib; + uint16_t my_pyro_fired = 0; + enum ao_flight_state my_state = ao_flight_invalid; + int i; + + ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + calib_size = ao_cmd_lex_i; + ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + data_size = ao_cmd_lex_i; + if ((unsigned) calib_size != sizeof (struct ao_fake_calib)) { + printf ("calib size %d larger than actual size %d\n", + calib_size, sizeof (struct ao_fake_calib)); + ao_cmd_status = ao_cmd_syntax_error; + return; + } + if (data_size != sizeof (struct ao_data)) { + printf ("data size %d doesn't match actual size %d\n", + data_size, sizeof (struct ao_data)); + ao_cmd_status = ao_cmd_syntax_error; + return; + } + ao_fake_calib_get(&save_calib); + if (!ao_fake_calib_read()) + return; + + ao_fake_has_next = 0; + ao_fake_has_cur = 0; + ao_fake_flight_active = 1; + ao_sample_init(); +#if PACKET_HAS_SLAVE + ao_packet_slave_stop(); +#endif +#if AO_LED_RED + /* Turn on the LED to indicate startup */ + ao_led_on(AO_LED_RED); +#endif + ao_flight_state = ao_flight_startup; + for (;;) { + if (my_state != ao_flight_state) { + printf("state %d\n", ao_flight_state); + my_state = ao_flight_state; + flush(); + } + if (my_pyro_fired != ao_pyro_fired) { + int pyro; + + for (pyro = 0; pyro < AO_PYRO_NUM; pyro++) { + uint16_t bit = (1 << pyro); + if (!(my_pyro_fired & bit) && (ao_pyro_fired & bit)) + printf ("fire %d\n", pyro); + } + my_pyro_fired = ao_pyro_fired; + } + while (ao_fake_has_next) + ao_sleep((void *) &ao_fake_has_next); + if (!ao_fake_data_read()) + break; + } + + /* Wait 20 seconds to see if we enter landed state */ + for (i = 0; i < 200; i++) + { + if (ao_flight_state == ao_flight_landed) + break; + ao_delay(AO_MS_TO_TICKS(100)); + } +#if AO_LED_RED + /* Turn on the LED to indicate startup */ + ao_led_on(AO_LED_RED); +#endif + ao_fake_flight_active = 0; + ao_flight_state = ao_flight_startup; + ao_sample_init(); + ao_fake_calib_set(&save_calib); +} + +static const struct ao_cmds ao_fake_flight_cmds[] = { + { ao_fake_flight, "F \0Start fake flight" }, + { 0, NULL } +}; + +void +ao_fake_flight_init(void) +{ + ao_cmd_register(&ao_fake_flight_cmds[0]); +} diff --git a/src/kernel/ao_fake_flight.h b/src/kernel/ao_fake_flight.h new file mode 100644 index 00000000..172fc589 --- /dev/null +++ b/src/kernel/ao_fake_flight.h @@ -0,0 +1,53 @@ +/* + * 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. + */ + +#ifndef _AO_FAKE_FLIGHT_H_ +#define _AO_FAKE_FLIGHT_H_ +#if HAS_MS5607 || HAS_MS5611 +#include +#endif + +extern uint8_t ao_fake_flight_active; + +#define AO_FAKE_CALIB_MAJOR 1 +#define AO_FAKE_CALIB_MINOR 0 + +struct ao_fake_calib { + uint16_t major; + uint16_t minor; +#if HAS_ACCEL + int16_t accel_plus_g; + int16_t accel_minus_g; +#endif +#if HAS_GYRO + int16_t accel_zero_along; + int16_t accel_zero_across; + int16_t accel_zero_through; + uint16_t pad30; +#endif +#if HAS_MS5607 || HAS_MS5611 + struct ao_ms5607_prom ms5607_prom; +#endif +}; + +void +ao_fake_flight_poll(void); + +void +ao_fake_flight_init(void); + +#endif /* _AO_FAKE_FLIGHT_H_ */ diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c index 24099347..2b433ee9 100644 --- a/src/kernel/ao_flight.c +++ b/src/kernel/ao_flight.c @@ -36,6 +36,10 @@ #error Please define HAS_USB #endif +#if HAS_FAKE_FLIGHT +#include +#endif + #ifndef HAS_TELEMETRY #define HAS_TELEMETRY HAS_RADIO #endif @@ -130,7 +134,10 @@ ao_flight(void) /* Disable the USB controller in flight mode * to save power */ - ao_usb_disable(); +#if HAS_FAKE_FLIGHT + if (!ao_fake_flight_active) +#endif + ao_usb_disable(); #endif #if !HAS_ACCEL && PACKET_HAS_SLAVE @@ -170,7 +177,6 @@ ao_flight(void) break; case ao_flight_pad: - /* pad to boost: * * barometer: > 20m vertical motion diff --git a/src/kernel/ao_pyro.h b/src/kernel/ao_pyro.h index 0c5642d6..34c99078 100644 --- a/src/kernel/ao_pyro.h +++ b/src/kernel/ao_pyro.h @@ -45,7 +45,11 @@ enum ao_pyro_flag { ao_pyro_state_less = 0x00004000, ao_pyro_state_greater_or_equal = 0x00008000, -}; +} +#ifdef __GNUC__ + __attribute__ ((packed)) +#endif + ; struct ao_pyro { enum ao_pyro_flag flags; diff --git a/src/stm/ao_timer.c b/src/stm/ao_timer.c index d93531fc..8db62e76 100644 --- a/src/stm/ao_timer.c +++ b/src/stm/ao_timer.c @@ -17,6 +17,9 @@ #include "ao.h" #include +#if HAS_FAKE_FLIGHT +#include +#endif #ifndef HAS_TICK #define HAS_TICK 1 @@ -47,7 +50,12 @@ void stm_systick_isr(void) #if AO_DATA_ALL if (++ao_data_count == ao_data_interval) { ao_data_count = 0; - ao_adc_poll(); +#if HAS_FAKE_FLIGHT + if (ao_fake_flight_active) + ao_fake_flight_poll(); + else +#endif + ao_adc_poll(); #if (AO_DATA_ALL & ~(AO_DATA_ADC)) ao_wakeup((void *) &ao_data_count); #endif -- cgit v1.2.3 From 177d3c0333fd4218f01e05c78cbc5f186c8e32c0 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 30 May 2014 17:27:10 -0700 Subject: altos: Allow sparse GPS data logging for TeleGPS When the device hasn't moved for a while, stop logging data. Start as soon as it moves again. Signed-off-by: Keith Packard --- src/kernel/ao_gps_report_mega.c | 44 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) (limited to 'src/kernel') diff --git a/src/kernel/ao_gps_report_mega.c b/src/kernel/ao_gps_report_mega.c index 5e3c71bf..cb0c0fd9 100644 --- a/src/kernel/ao_gps_report_mega.c +++ b/src/kernel/ao_gps_report_mega.c @@ -18,6 +18,43 @@ #include "ao.h" #include "ao_log.h" +#ifndef GPS_SPARSE_LOG +#define GPS_SPARSE_LOG 0 +#endif + +#if GPS_SPARSE_LOG +static int32_t prev_lat, prev_lon, int16_t prev_alt; +static uint8_t has_prev, unmoving; + +#define GPS_SPARSE_UNMOVING_REPORTS 10 +#define GPS_SPARSE_UNMOVING_GROUND 10 +#define GPS_SPARSE_UNMOVING_AIR 10 + +static uint8_t +ao_gps_sparse_should_log(int32_t lat, int32_t lon, int16_t alt) +{ + uint8_t ret = 1; + + if (has_prev && ao_log_running) { + uint32_t h = ao_distance(prev_lat, prev_lon, lat, lon); + uint16_t v = alt > prev_alt ? (alt - prev_alt) : (prev_alt - alt); + + if (h < GPS_SPARSE_UNMOVING_GROUND && v < GPS_SPARSE_UNMOVING_AIR) { + if (unmoving < GPS_SPARSE_UNMOVING_REPORTS) + ++unmoving; + } else + unmoving = 0; + } else + unmoving = 0; + + prev_lat = lat; + prev_lon = lon; + prev_alt = alt; + has_prev = 1; + return unmoving >= GPS_SPARSE_UNMOVING_REPORTS; +} +#endif + void ao_gps_report_mega(void) { @@ -38,7 +75,14 @@ ao_gps_report_mega(void) ao_gps_new = 0; ao_mutex_put(&ao_gps_mutex); +#if GPS_SPARSE_LOG + /* Don't log data if GPS has a fix and hasn't moved for a while */ + if ((gps_data.flags & AO_GPS_VALID) && + !ao_gps_sparse_should_log(gps_data.latitude, gps_data.longitude, gps_data.altitude)) + continue; +#endif 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; -- cgit v1.2.3 From 7385c76af46ff400b9e79a8540199be289cb57c0 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 2 Jun 2014 22:03:26 -0700 Subject: altos: Configuring pyro channels can use more than 48 characters Increase the command buffer from 48 to 128 bytes to hold the longest pyro configuration commands Signed-off-by: Keith Packard --- src/kernel/ao_cmd.c | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src/kernel') diff --git a/src/kernel/ao_cmd.c b/src/kernel/ao_cmd.c index cd662314..69372fed 100644 --- a/src/kernel/ao_cmd.c +++ b/src/kernel/ao_cmd.c @@ -23,7 +23,11 @@ __pdata uint32_t ao_cmd_lex_u32; __pdata char ao_cmd_lex_c; __pdata enum ao_cmd_status ao_cmd_status; +#if AO_PYRO_NUM +#define CMD_LEN 128 +#else #define CMD_LEN 48 +#endif static __xdata char cmd_line[CMD_LEN]; static __pdata uint8_t cmd_len; -- cgit v1.2.3 From b7abc063fb27da29cd7a717bbea15f92882bd205 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 2 Jun 2014 22:04:11 -0700 Subject: altos: Maximum pyro configuration parameter has 4 bytes in the name "f>=" needs four bytes, not just three to store the whole string. If we only store three, then we never manage to compare correctly as the null terminating byte is missing. Signed-off-by: Keith Packard --- src/kernel/ao_pyro.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c index e59f5bc4..56758fa4 100644 --- a/src/kernel/ao_pyro.c +++ b/src/kernel/ao_pyro.c @@ -281,7 +281,7 @@ ao_pyro_check(void) #define NO_VALUE 0xff -#define AO_PYRO_NAME_LEN 3 +#define AO_PYRO_NAME_LEN 4 #if !DISABLE_HELP #define ENABLE_HELP 1 -- cgit v1.2.3 From d20c608ce833fb8949dce527f92887775d216823 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 2 Jun 2014 22:05:11 -0700 Subject: altos: Fetch/store only 8 bits for pyro state values These fields are uint8_t, not int16_t. Fetching and storing 16 bits is a bad idea. Signed-off-by: Keith Packard --- src/kernel/ao_pyro.c | 10 ++++++++-- src/kernel/ao_pyro.h | 2 ++ 2 files changed, 10 insertions(+), 2 deletions(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c index 56758fa4..0ee7fbee 100644 --- a/src/kernel/ao_pyro.c +++ b/src/kernel/ao_pyro.c @@ -403,7 +403,10 @@ ao_pyro_show(void) if (ao_pyro_values[v].offset != NO_VALUE) { int16_t value; - value = *((int16_t *) ((char *) pyro + ao_pyro_values[v].offset)); + if (ao_pyro_values[v].flag & AO_PYRO_8_BIT_VALUE) + value = *((uint8_t *) ((char *) pyro + ao_pyro_values[v].offset)); + else + value = *((int16_t *) ((char *) pyro + ao_pyro_values[v].offset)); printf ("%6d ", value); } else { printf (" "); @@ -467,7 +470,10 @@ ao_pyro_set(void) 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; + if (ao_pyro_values[v].flag & AO_PYRO_8_BIT_VALUE) + *((uint8_t *) ((char *) &pyro_tmp + ao_pyro_values[v].offset)) = ao_cmd_lex_i; + else + *((int16_t *) ((char *) &pyro_tmp + ao_pyro_values[v].offset)) = ao_cmd_lex_i; } } _ao_config_edit_start(); diff --git a/src/kernel/ao_pyro.h b/src/kernel/ao_pyro.h index 34c99078..b37aaeb1 100644 --- a/src/kernel/ao_pyro.h +++ b/src/kernel/ao_pyro.h @@ -65,6 +65,8 @@ struct ao_pyro { uint8_t fired; }; +#define AO_PYRO_8_BIT_VALUE (ao_pyro_state_less|ao_pyro_state_greater_or_equal) + extern uint8_t ao_pyro_wakeup; extern uint16_t ao_pyro_fired; -- cgit v1.2.3 From 6e152dd5c0786a650aed8f0c09babdc93895bff1 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 5 Jun 2014 17:10:28 -0700 Subject: altos: Add ao_distance.c to compute cartesian distances on the globe This is not a great circle distance, but should be good enough for points reasonably close together Signed-off-by: Keith Packard --- src/kernel/ao_distance.c | 122 +++++++++++++++++++++++++++++++++++++++++++++++ src/kernel/ao_distance.h | 25 ++++++++++ 2 files changed, 147 insertions(+) create mode 100644 src/kernel/ao_distance.c create mode 100644 src/kernel/ao_distance.h (limited to 'src/kernel') diff --git a/src/kernel/ao_distance.c b/src/kernel/ao_distance.c new file mode 100644 index 00000000..ba7d59fe --- /dev/null +++ b/src/kernel/ao_distance.c @@ -0,0 +1,122 @@ +/* + * 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 +#include + +static uint32_t +ao_dist(int32_t a, int32_t b) +{ + int32_t d = a - b; + if (d < 0) + d = -d; + return (uint32_t) ((int64_t) d * 111198 / 10000000); +} + +static uint32_t +ao_lat_dist(int32_t lat_a, int32_t lat_b) +{ + return ao_dist(lat_a, lat_b); +} + +static const uint8_t cos_table[] = { + 0, /* 0 */ + 0, /* 1 */ + 0, /* 2 */ + 255, /* 3 */ + 254, /* 4 */ + 253, /* 5 */ + 252, /* 6 */ + 251, /* 7 */ + 249, /* 8 */ + 247, /* 9 */ + 245, /* 10 */ + 243, /* 11 */ + 240, /* 12 */ + 238, /* 13 */ + 235, /* 14 */ + 232, /* 15 */ + 228, /* 16 */ + 225, /* 17 */ + 221, /* 18 */ + 217, /* 19 */ + 213, /* 20 */ + 209, /* 21 */ + 205, /* 22 */ + 200, /* 23 */ + 195, /* 24 */ + 190, /* 25 */ + 185, /* 26 */ + 180, /* 27 */ + 175, /* 28 */ + 169, /* 29 */ + 163, /* 30 */ + 158, /* 31 */ + 152, /* 32 */ + 145, /* 33 */ + 139, /* 34 */ + 133, /* 35 */ + 126, /* 36 */ + 120, /* 37 */ + 113, /* 38 */ + 106, /* 39 */ + 100, /* 40 */ + 93, /* 41 */ + 86, /* 42 */ + 79, /* 43 */ + 71, /* 44 */ + 64, /* 45 */ + 57, /* 46 */ + 49, /* 47 */ + 42, /* 48 */ + 35, /* 49 */ + 27, /* 50 */ + 20, /* 51 */ + 12, /* 52 */ + 5, /* 53 */ + 1, /* 54 */ +}; + +static uint32_t +ao_lon_dist(int32_t lon_a, int32_t lon_b) +{ + uint8_t c = cos_table[lon_a >> 24]; + uint32_t lon_dist; + + /* check if it's shorter to go the other way around */ + if (lon_a < lon_b - 1800000000) + lon_a += 3600000000; + lon_dist = ao_dist(lon_a, lon_b); + if (c) { + if (lon_dist & 0x7f800000) + lon_dist = (lon_dist >> 8) * c; + else + lon_dist = (lon_dist * (int16_t) c) >> 8; + } + return lon_dist; +} + +static uint32_t sqr(uint32_t x) { return x * x; } + +uint32_t +ao_distance(int32_t lat_a, int32_t lon_a, int32_t lat_b, int32_t lon_b) +{ + uint32_t lat_dist = ao_lat_dist(lat_a, lat_b); + uint32_t lon_dist = ao_lon_dist(lon_a, lon_b); + + return ao_sqrt (sqr(lat_dist) + sqr(lon_dist)); +} diff --git a/src/kernel/ao_distance.h b/src/kernel/ao_distance.h new file mode 100644 index 00000000..6762434e --- /dev/null +++ b/src/kernel/ao_distance.h @@ -0,0 +1,25 @@ +/* + * 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. + */ + +#ifndef _AO_DISTANCE_H_ +#define _AO_DISTANCE_H_ +#include + +uint32_t +ao_distance(int32_t lat_a, int32_t lon_a, int32_t lat_b, int32_t lon_b); + +#endif /* _AO_DISTANCE_H_ */ -- cgit v1.2.3 From b8201bc9ba4a5f5f0522b68493cd5e7f013fd4bb Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 5 Jun 2014 17:14:28 -0700 Subject: altos: Include sensor logging task only on flight boards This lets TeleGPS use the logging infrastructure without wasting a task to log sensor data Signed-off-by: Keith Packard --- src/kernel/ao_log.c | 8 ++++++-- src/kernel/ao_log_mega.c | 6 +++--- 2 files changed, 9 insertions(+), 5 deletions(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_log.c b/src/kernel/ao_log.c index 20febefe..d60485e0 100644 --- a/src/kernel/ao_log.c +++ b/src/kernel/ao_log.c @@ -196,7 +196,11 @@ ao_log_full(void) return ao_log_current_pos == ao_log_end_pos; } -#if HAS_ADC +#ifndef LOG_ADC +#define LOG_ADC HAS_ADC +#endif + +#if LOG_ADC static __xdata struct ao_task ao_log_task; #endif @@ -284,7 +288,7 @@ ao_log_init(void) #ifndef HAS_ADC #error Define HAS_ADC for ao_log.c #endif -#if HAS_ADC +#if LOG_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_mega.c b/src/kernel/ao_log_mega.c index 768947d5..8997fd05 100644 --- a/src/kernel/ao_log_mega.c +++ b/src/kernel/ao_log_mega.c @@ -65,7 +65,7 @@ ao_log_dump_check_data(void) return 1; } -#if HAS_ADC +#if HAS_FLIGHT static __data uint8_t ao_log_data_pos; /* a hack to make sure that ao_log_megas fill the eeprom block in even units */ @@ -100,9 +100,9 @@ ao_log(void) 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_roll = ao_ground_roll; 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; @@ -183,7 +183,7 @@ ao_log(void) ao_sleep(&ao_log_running); } } -#endif +#endif /* HAS_FLIGHT */ uint16_t ao_log_flight(uint8_t slot) -- cgit v1.2.3 From d7df6e8c47df35c0d27f1a2559ecc305ef28d271 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 5 Jun 2014 18:31:06 -0700 Subject: altos: ao_distance was overflowing when checking for longitude wrap Need to shift everyone right one bit to fit in 32 bits Signed-off-by: Keith Packard --- src/kernel/ao_distance.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_distance.c b/src/kernel/ao_distance.c index ba7d59fe..5654182a 100644 --- a/src/kernel/ao_distance.c +++ b/src/kernel/ao_distance.c @@ -24,6 +24,7 @@ ao_dist(int32_t a, int32_t b) int32_t d = a - b; if (d < 0) d = -d; + return (uint32_t) ((int64_t) d * 111198 / 10000000); } @@ -98,7 +99,7 @@ ao_lon_dist(int32_t lon_a, int32_t lon_b) uint32_t lon_dist; /* check if it's shorter to go the other way around */ - if (lon_a < lon_b - 1800000000) + if ((lon_a >> 1) < (lon_b >> 1) - (1800000000 >> 1)) lon_a += 3600000000; lon_dist = ao_dist(lon_a, lon_b); if (c) { -- cgit v1.2.3 From 97dac0f66bc938940e6b49409d950a1736c92655 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 5 Jun 2014 18:44:26 -0700 Subject: altos: Stick flight state in GPS location packets Useful for TeleGPS Signed-off-by: Keith Packard --- src/kernel/ao_telemetry.c | 1 + src/kernel/ao_telemetry.h | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_telemetry.c b/src/kernel/ao_telemetry.c index a1c19185..7850f738 100644 --- a/src/kernel/ao_telemetry.c +++ b/src/kernel/ao_telemetry.c @@ -334,6 +334,7 @@ ao_send_location(void) ao_xmemcpy(&telemetry.location.flags, &ao_gps_data.flags, 26); + telemetry.location.state = ao_flight_state; telemetry.location.tick = ao_gps_tick; ao_mutex_put(&ao_gps_mutex); ao_radio_send(&telemetry, sizeof (telemetry)); diff --git a/src/kernel/ao_telemetry.h b/src/kernel/ao_telemetry.h index fe07c2af..1aeda264 100644 --- a/src/kernel/ao_telemetry.h +++ b/src/kernel/ao_telemetry.h @@ -115,7 +115,7 @@ struct ao_telemetry_location { 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 */ + uint8_t state; /* 31 flight state */ /* 32 */ }; -- cgit v1.2.3 From ec3de3ac461f2380d23c5c5d948333a9a210c400 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 5 Jun 2014 21:35:41 -0700 Subject: altos: Fix config to set default log size for all devices with log Not just devices with flight Signed-off-by: Keith Packard --- src/kernel/ao_config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c index e5f8efba..b4847cdd 100644 --- a/src/kernel/ao_config.c +++ b/src/kernel/ao_config.c @@ -133,7 +133,7 @@ _ao_config_get(void) ao_config.radio_cal = ao_radio_cal; #endif /* Fixups for minor version 4 */ -#if HAS_FLIGHT +#if HAS_LOG if (minor < 4) ao_config.flight_log_max = AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX; #endif -- cgit v1.2.3 From 1873d539a8f1a0e1e8ad539af5d49a77a129b928 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 7 Jun 2014 07:41:11 -0700 Subject: altos: Move ao_tracker.c to kernel Doesn't make sense to be in product Signed-off-by: Keith Packard --- src/kernel/ao_tracker.c | 212 +++++++++++++++++++++++++++++++++++++++++++++++ src/product/ao_tracker.c | 212 ----------------------------------------------- 2 files changed, 212 insertions(+), 212 deletions(-) create mode 100644 src/kernel/ao_tracker.c delete mode 100644 src/product/ao_tracker.c (limited to 'src/kernel') diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c new file mode 100644 index 00000000..b207c562 --- /dev/null +++ b/src/kernel/ao_tracker.c @@ -0,0 +1,212 @@ +/* + * 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 +#include +#include +#include + +enum ao_flight_state ao_flight_state; + +/* Speeds for the various modes, 2m/s seems reasonable for 'not moving' */ +#define AO_TRACKER_NOT_MOVING 200 + +static uint8_t ao_tracker_force_telem; +static uint8_t ao_tracker_force_launch; + +#if HAS_USB_CONNECT +static inline uint8_t +ao_usb_connected(void) +{ + return ao_gpio_get(AO_USB_CONNECT_PORT, AO_USB_CONNECT_PIN, AO_USB_CONNECT) != 0; +} +#else +#define ao_usb_connected() 1 +#endif + +#define STARTUP_AVERAGE 5 + +static void +ao_tracker_start_flight(void) +{ + struct ao_log_mega log; + ao_log_start(); + log.type = AO_LOG_FLIGHT; + log.tick = ao_time(); +#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_roll = ao_ground_roll; + log.u.flight.ground_pitch = ao_ground_pitch; + log.u.flight.ground_yaw = ao_ground_yaw; +#endif +#if HAS_FLIGHT + log.u.flight.ground_pres = ao_ground_pres; +#endif + log.u.flight.flight = ao_flight_number; + ao_log_mega(&log); +} + +static void +ao_tracker(void) +{ + uint16_t telem_rate = AO_SEC_TO_TICKS(1), new_telem_rate; + uint8_t gps_rate = 1, new_gps_rate; + uint8_t telem_enabled = 0, new_telem_enabled; + int32_t start_latitude = 0, start_longitude = 0; + int16_t start_altitude = 0; + uint32_t ground_distance; + int16_t height; + uint16_t speed; + int64_t lat_sum = 0, lon_sum = 0; + int32_t alt_sum = 0; + int nsamples = 0; + +#if HAS_ADC + ao_timer_set_adc_interval(100); +#endif + +#if !HAS_USB_CONNECT + ao_tracker_force_telem = 1; +#endif + + ao_log_scan(); + + ao_rdf_set(1); + + ao_telemetry_set_interval(0); + + ao_flight_state = ao_flight_startup; + for (;;) { + ao_sleep(&ao_gps_new); + + new_gps_rate = gps_rate; + new_telem_rate = telem_rate; + + new_telem_enabled = ao_tracker_force_telem || !ao_usb_connected(); + + ao_mutex_get(&ao_gps_mutex); + + /* Don't change anything if GPS isn't locked */ + if ((ao_gps_data.flags & (AO_GPS_VALID|AO_GPS_COURSE_VALID)) == + (AO_GPS_VALID|AO_GPS_COURSE_VALID)) + { + switch (ao_flight_state) { + case ao_flight_startup: + /* startup to pad when GPS locks */ + + lat_sum += ao_gps_data.latitude; + lon_sum += ao_gps_data.longitude; + alt_sum += ao_gps_data.altitude; + + if (++nsamples >= STARTUP_AVERAGE) { + ao_flight_state = ao_flight_pad; + ao_wakeup(&ao_flight_state); + start_latitude = lat_sum / nsamples; + start_longitude = lon_sum / nsamples; + start_altitude = alt_sum / nsamples; + } + break; + case ao_flight_pad: + ground_distance = ao_distance(ao_gps_data.latitude, + ao_gps_data.longitude, + start_latitude, + start_longitude); + height = ao_gps_data.altitude - start_altitude; + if (height < 0) + height = -height; + + if (ground_distance >= ao_config.tracker_start_horiz || + height >= ao_config.tracker_start_vert || + ao_tracker_force_launch) + { + ao_flight_state = ao_flight_drogue; + ao_wakeup(&ao_flight_state); + ao_log_start(); + ao_tracker_start_flight(); + } + break; + case ao_flight_drogue: + /* Modulate data rates based on speed (in cm/s) */ + if (ao_gps_data.climb_rate < 0) + speed = -ao_gps_data.climb_rate; + else + speed = ao_gps_data.climb_rate; + speed += ao_gps_data.ground_speed; + + if (speed < AO_TRACKER_NOT_MOVING) { + new_telem_rate = AO_SEC_TO_TICKS(10); + new_gps_rate = 10; + } else { + new_telem_rate = AO_SEC_TO_TICKS(1); + new_gps_rate = 1; + } + break; + default: + break; + } + } + ao_mutex_put(&ao_gps_mutex); + + if (new_telem_rate != telem_rate || new_telem_enabled != telem_enabled) { + if (new_telem_enabled) + ao_telemetry_set_interval(new_telem_rate); + else + ao_telemetry_set_interval(0); + telem_rate = new_telem_rate; + telem_enabled = new_telem_enabled; + } + + if (new_gps_rate != gps_rate) { + ao_gps_set_rate(new_gps_rate); + gps_rate = new_gps_rate; + } + } +} + +static struct ao_task ao_tracker_task; + +static void +ao_tracker_set_telem(void) +{ + ao_cmd_hex(); + if (ao_cmd_status == ao_cmd_success) { + ao_tracker_force_telem = (ao_cmd_lex_i & 1) != 0; + ao_tracker_force_launch = (ao_cmd_lex_i & 2) != 0; + } + printf ("flight %d force telem %d force launch %d\n", + ao_flight_number, ao_tracker_force_telem, ao_tracker_force_launch); +} + +static const struct ao_cmds ao_tracker_cmds[] = { + { ao_tracker_set_telem, "t \0Set telem on USB" }, + { 0, NULL }, +}; + +void +ao_tracker_init(void) +{ +#if HAS_USB_CONNECT + ao_enable_input(AO_USB_CONNECT_PORT, AO_USB_CONNECT_PIN, 0); +#endif + ao_cmd_register(&ao_tracker_cmds[0]); + ao_add_task(&ao_tracker_task, ao_tracker, "tracker"); +} diff --git a/src/product/ao_tracker.c b/src/product/ao_tracker.c deleted file mode 100644 index b207c562..00000000 --- a/src/product/ao_tracker.c +++ /dev/null @@ -1,212 +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 -#include -#include -#include - -enum ao_flight_state ao_flight_state; - -/* Speeds for the various modes, 2m/s seems reasonable for 'not moving' */ -#define AO_TRACKER_NOT_MOVING 200 - -static uint8_t ao_tracker_force_telem; -static uint8_t ao_tracker_force_launch; - -#if HAS_USB_CONNECT -static inline uint8_t -ao_usb_connected(void) -{ - return ao_gpio_get(AO_USB_CONNECT_PORT, AO_USB_CONNECT_PIN, AO_USB_CONNECT) != 0; -} -#else -#define ao_usb_connected() 1 -#endif - -#define STARTUP_AVERAGE 5 - -static void -ao_tracker_start_flight(void) -{ - struct ao_log_mega log; - ao_log_start(); - log.type = AO_LOG_FLIGHT; - log.tick = ao_time(); -#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_roll = ao_ground_roll; - log.u.flight.ground_pitch = ao_ground_pitch; - log.u.flight.ground_yaw = ao_ground_yaw; -#endif -#if HAS_FLIGHT - log.u.flight.ground_pres = ao_ground_pres; -#endif - log.u.flight.flight = ao_flight_number; - ao_log_mega(&log); -} - -static void -ao_tracker(void) -{ - uint16_t telem_rate = AO_SEC_TO_TICKS(1), new_telem_rate; - uint8_t gps_rate = 1, new_gps_rate; - uint8_t telem_enabled = 0, new_telem_enabled; - int32_t start_latitude = 0, start_longitude = 0; - int16_t start_altitude = 0; - uint32_t ground_distance; - int16_t height; - uint16_t speed; - int64_t lat_sum = 0, lon_sum = 0; - int32_t alt_sum = 0; - int nsamples = 0; - -#if HAS_ADC - ao_timer_set_adc_interval(100); -#endif - -#if !HAS_USB_CONNECT - ao_tracker_force_telem = 1; -#endif - - ao_log_scan(); - - ao_rdf_set(1); - - ao_telemetry_set_interval(0); - - ao_flight_state = ao_flight_startup; - for (;;) { - ao_sleep(&ao_gps_new); - - new_gps_rate = gps_rate; - new_telem_rate = telem_rate; - - new_telem_enabled = ao_tracker_force_telem || !ao_usb_connected(); - - ao_mutex_get(&ao_gps_mutex); - - /* Don't change anything if GPS isn't locked */ - if ((ao_gps_data.flags & (AO_GPS_VALID|AO_GPS_COURSE_VALID)) == - (AO_GPS_VALID|AO_GPS_COURSE_VALID)) - { - switch (ao_flight_state) { - case ao_flight_startup: - /* startup to pad when GPS locks */ - - lat_sum += ao_gps_data.latitude; - lon_sum += ao_gps_data.longitude; - alt_sum += ao_gps_data.altitude; - - if (++nsamples >= STARTUP_AVERAGE) { - ao_flight_state = ao_flight_pad; - ao_wakeup(&ao_flight_state); - start_latitude = lat_sum / nsamples; - start_longitude = lon_sum / nsamples; - start_altitude = alt_sum / nsamples; - } - break; - case ao_flight_pad: - ground_distance = ao_distance(ao_gps_data.latitude, - ao_gps_data.longitude, - start_latitude, - start_longitude); - height = ao_gps_data.altitude - start_altitude; - if (height < 0) - height = -height; - - if (ground_distance >= ao_config.tracker_start_horiz || - height >= ao_config.tracker_start_vert || - ao_tracker_force_launch) - { - ao_flight_state = ao_flight_drogue; - ao_wakeup(&ao_flight_state); - ao_log_start(); - ao_tracker_start_flight(); - } - break; - case ao_flight_drogue: - /* Modulate data rates based on speed (in cm/s) */ - if (ao_gps_data.climb_rate < 0) - speed = -ao_gps_data.climb_rate; - else - speed = ao_gps_data.climb_rate; - speed += ao_gps_data.ground_speed; - - if (speed < AO_TRACKER_NOT_MOVING) { - new_telem_rate = AO_SEC_TO_TICKS(10); - new_gps_rate = 10; - } else { - new_telem_rate = AO_SEC_TO_TICKS(1); - new_gps_rate = 1; - } - break; - default: - break; - } - } - ao_mutex_put(&ao_gps_mutex); - - if (new_telem_rate != telem_rate || new_telem_enabled != telem_enabled) { - if (new_telem_enabled) - ao_telemetry_set_interval(new_telem_rate); - else - ao_telemetry_set_interval(0); - telem_rate = new_telem_rate; - telem_enabled = new_telem_enabled; - } - - if (new_gps_rate != gps_rate) { - ao_gps_set_rate(new_gps_rate); - gps_rate = new_gps_rate; - } - } -} - -static struct ao_task ao_tracker_task; - -static void -ao_tracker_set_telem(void) -{ - ao_cmd_hex(); - if (ao_cmd_status == ao_cmd_success) { - ao_tracker_force_telem = (ao_cmd_lex_i & 1) != 0; - ao_tracker_force_launch = (ao_cmd_lex_i & 2) != 0; - } - printf ("flight %d force telem %d force launch %d\n", - ao_flight_number, ao_tracker_force_telem, ao_tracker_force_launch); -} - -static const struct ao_cmds ao_tracker_cmds[] = { - { ao_tracker_set_telem, "t \0Set telem on USB" }, - { 0, NULL }, -}; - -void -ao_tracker_init(void) -{ -#if HAS_USB_CONNECT - ao_enable_input(AO_USB_CONNECT_PORT, AO_USB_CONNECT_PIN, 0); -#endif - ao_cmd_register(&ao_tracker_cmds[0]); - ao_add_task(&ao_tracker_task, ao_tracker, "tracker"); -} -- cgit v1.2.3 From b8a29d65ec605a995de1d1ec8b110d620d2f7a87 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 7 Jun 2014 10:05:21 -0700 Subject: altos: Allow AO_CONFIG_MAX_SIZE to be configured. Validate it. TeleMega config is 200 bytes. AO_CONFIG_MAX_SIZE was 128. That didn't work out well when logging erased flight information. Allow TeleMega to use a larger value (1k), and then do a compiler hack to make sure the defined value is at least as large as the ao_config structure. Signed-off-by: Keith Packard --- src/kernel/ao_config.h | 5 +++++ 1 file changed, 5 insertions(+) (limited to 'src/kernel') diff --git a/src/kernel/ao_config.h b/src/kernel/ao_config.h index 85673764..7ad221c6 100644 --- a/src/kernel/ao_config.h +++ b/src/kernel/ao_config.h @@ -112,7 +112,12 @@ struct ao_config { #define AO_PAD_ORIENTATION_ANTENNA_UP 0 #define AO_PAD_ORIENTATION_ANTENNA_DOWN 1 +#ifndef AO_CONFIG_MAX_SIZE #define AO_CONFIG_MAX_SIZE 128 +#endif + +/* Make sure AO_CONFIG_MAX_SIZE is big enough */ +typedef uint8_t config_check_space[(int) (AO_CONFIG_MAX_SIZE - sizeof (struct ao_config))]; extern __xdata struct ao_config ao_config; extern __pdata uint8_t ao_config_loaded; -- cgit v1.2.3 From 1d6ca536c688d35b3cba0a829b04b93c5124b328 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 7 Jun 2014 10:09:51 -0700 Subject: altos: Allow value other than 0 for marking erased flights on-chip eeprom doesn't erase to 0xff, so let TeleMega use a different value. Signed-off-by: Keith Packard --- src/kernel/ao_log.c | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_log.c b/src/kernel/ao_log.c index d60485e0..76e0e68c 100644 --- a/src/kernel/ao_log.c +++ b/src/kernel/ao_log.c @@ -38,13 +38,22 @@ ao_log_flush(void) */ struct ao_log_erase { - uint8_t unused; + uint8_t mark; uint16_t flight; }; static __xdata struct ao_log_erase erase; +#ifndef LOG_MAX_ERASE #define LOG_MAX_ERASE 16 +#endif + +#ifndef LOG_ERASE_MARK +#if USE_EEPROM_CONFIG +#error "Must define LOG_ERASE_MARK with USE_EEPROM_CONFIG" +#endif +#define LOG_ERASE_MARK 0x00 +#endif static uint32_t ao_log_erase_pos(uint8_t i) @@ -55,7 +64,7 @@ ao_log_erase_pos(uint8_t i) void ao_log_write_erase(uint8_t pos) { - erase.unused = 0x00; + erase.mark = LOG_ERASE_MARK; erase.flight = ao_flight_number; ao_config_write(ao_log_erase_pos(pos), &erase, sizeof (erase)); ao_config_flush(); @@ -75,9 +84,9 @@ ao_log_erase_mark(void) for (i = 0; i < LOG_MAX_ERASE; i++) { ao_log_read_erase(i); - if (erase.unused == 0 && erase.flight == ao_flight_number) + if (erase.mark == LOG_ERASE_MARK && erase.flight == ao_flight_number) return; - if (erase.unused == 0xff) { + if (erase.mark != LOG_ERASE_MARK) { ao_log_write_erase(i); return; } @@ -136,7 +145,7 @@ ao_log_scan(void) __reentrant */ for (log_slot = LOG_MAX_ERASE; log_slot-- > 0;) { ao_log_read_erase(log_slot); - if (erase.unused == 0) { + if (erase.mark == LOG_ERASE_MARK) { if (ao_flight_number == 0 || (int16_t) (erase.flight - ao_flight_number) > 0) ao_flight_number = erase.flight; -- cgit v1.2.3 From 5d973570ef2324b21a64477eecb0a292652ff467 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 7 Jun 2014 10:54:14 -0700 Subject: altos: Clear out eeprom erase records when writing entry 0 When writing config/erase to eeprom, there's no 'erase' operation as on-chip eeprom is writable at a byte level. As such, we can't tell when the erase blocks get reset when the config gets written. When this happens, erase block 0 gets written explicitly, so just use that call to trigger explicit erasing of the data. Signed-off-by: Keith Packard --- src/kernel/ao_log.c | 12 ++++++++++++ 1 file changed, 12 insertions(+) (limited to 'src/kernel') diff --git a/src/kernel/ao_log.c b/src/kernel/ao_log.c index 76e0e68c..3cf85a33 100644 --- a/src/kernel/ao_log.c +++ b/src/kernel/ao_log.c @@ -67,6 +67,18 @@ ao_log_write_erase(uint8_t pos) erase.mark = LOG_ERASE_MARK; erase.flight = ao_flight_number; ao_config_write(ao_log_erase_pos(pos), &erase, sizeof (erase)); + +#if USE_EEPROM_CONFIG + if (pos == 0) { + uint8_t i; + for (i = 1; i < LOG_MAX_ERASE; i++) { + erase.mark = ~LOG_ERASE_MARK; + erase.flight = 0; + ao_config_write(ao_log_erase_pos(i), &erase, sizeof (erase)); + } + } +#endif + ao_config_flush(); } -- cgit v1.2.3 From 302842ccda46a0a3d58b60d5c7fc82e05f614b0b Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 7 Jun 2014 11:34:29 -0700 Subject: altos: Add TeleGPS logging format This is mostly like the mega format, but places the flight state in a spare byte of the GPS data and writes the gps starting location to the flight packet. Log data is written by the main tracker thread; there's no reason for a separate thread given the GPS update rate and the lack of flight controls. This means ao_log_gps has an API to be called from there, rather than a thread to run. Signed-off-by: Keith Packard --- src/kernel/ao_log.h | 48 ++++++++++++++ src/kernel/ao_log_gps.c | 173 ++++++++++++++++++++++++++++++++++++++++++++++++ src/kernel/ao_log_gps.h | 32 +++++++++ 3 files changed, 253 insertions(+) create mode 100644 src/kernel/ao_log_gps.c create mode 100644 src/kernel/ao_log_gps.h (limited to 'src/kernel') diff --git a/src/kernel/ao_log.h b/src/kernel/ao_log.h index 3ff9e811..87fa0d4d 100644 --- a/src/kernel/ao_log.h +++ b/src/kernel/ao_log.h @@ -47,6 +47,7 @@ extern __pdata enum ao_flight_state ao_log_state; #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_TELEGPS 9 /* 32 byte telegps records */ #define AO_LOG_FORMAT_NONE 127 /* No log at all */ extern __code uint8_t ao_log_format; @@ -364,6 +365,50 @@ struct ao_log_mini { (dst)[2] = (value) >> 16; \ } while (0) +struct ao_log_gps { + char type; /* 0 */ + uint8_t csum; /* 1 */ + uint16_t tick; /* 2 */ + union { /* 4 */ + /* AO_LOG_FLIGHT */ + struct { + uint16_t flight; /* 4 */ + int16_t start_altitude; /* 6 */ + int32_t start_latitude; /* 8 */ + int32_t start_longitude; /* 12 */ + } flight; /* 16 */ + /* 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 */ + uint8_t state; /* 30 */ + } gps; /* 31 */ + /* AO_LOG_GPS_SAT */ + struct { + uint16_t channels; /* 4 */ + struct { + uint8_t svid; + uint8_t c_n; + } sats[12]; /* 6 */ + } gps_sat; /* 30 */ + } u; +}; + /* Write a record to the eeprom log */ uint8_t ao_log_data(__xdata struct ao_log_record *log) __reentrant; @@ -377,6 +422,9 @@ ao_log_metrum(__xdata struct ao_log_metrum *log) __reentrant; uint8_t ao_log_mini(__xdata struct ao_log_mini *log) __reentrant; +uint8_t +ao_log_gps(__xdata struct ao_log_gps *log) __reentrant; + void ao_log_flush(void); diff --git a/src/kernel/ao_log_gps.c b/src/kernel/ao_log_gps.c new file mode 100644 index 00000000..e9e573a5 --- /dev/null +++ b/src/kernel/ao_log_gps.c @@ -0,0 +1,173 @@ +/* + * 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" +#include +#include +#include +#include +#include +#include + +static __xdata uint8_t ao_log_mutex; +static __xdata struct ao_log_gps log; + +__code uint8_t ao_log_format = AO_LOG_FORMAT_TELEGPS; + +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_gps); i++) + sum += *b++; + return -sum; +} + +uint8_t +ao_log_gps(__xdata struct ao_log_gps *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_gps)); + ao_log_current_pos += sizeof (struct ao_log_gps); + } + } ao_mutex_put(&ao_log_mutex); + return wrote; +} + + +static int32_t prev_lat, prev_lon; +static int16_t prev_alt; +static uint8_t has_prev, unmoving; + +#define GPS_SPARSE_UNMOVING_REPORTS 10 +#define GPS_SPARSE_UNMOVING_GROUND 10 +#define GPS_SPARSE_UNMOVING_AIR 10 + +uint8_t +ao_log_gps_should_log(int32_t lat, int32_t lon, int16_t alt) +{ + if (has_prev && ao_log_running) { + uint32_t h = ao_distance(prev_lat, prev_lon, lat, lon); + uint16_t v = alt > prev_alt ? (alt - prev_alt) : (prev_alt - alt); + + if (h < GPS_SPARSE_UNMOVING_GROUND && v < GPS_SPARSE_UNMOVING_AIR) { + if (unmoving < GPS_SPARSE_UNMOVING_REPORTS) + ++unmoving; + } else + unmoving = 0; + } else + unmoving = 0; + + prev_lat = lat; + prev_lon = lon; + prev_alt = alt; + has_prev = 1; + return unmoving >= GPS_SPARSE_UNMOVING_REPORTS; +} + +void +ao_log_gps_flight(void) +{ + log.type = AO_LOG_FLIGHT; + log.tick = ao_time(); + log.u.flight.flight = ao_flight_number; + log.u.flight.start_altitude = ao_tracker_start_altitude; + log.u.flight.start_latitude = ao_tracker_start_latitude; + log.u.flight.start_longitude = ao_tracker_start_longitude; + ao_log_gps(&log); +} + +void +ao_log_gps_data(uint16_t tick, uint8_t state, struct ao_telemetry_location *gps_data) +{ + log.tick = tick; + log.type = AO_LOG_GPS_TIME; + log.u.gps.latitude = gps_data->latitude; + log.u.gps.longitude = gps_data->longitude; + log.u.gps.altitude = gps_data->altitude; + + log.u.gps.hour = gps_data->hour; + log.u.gps.minute = gps_data->minute; + log.u.gps.second = gps_data->second; + log.u.gps.flags = gps_data->flags; + log.u.gps.year = gps_data->year; + log.u.gps.month = gps_data->month; + log.u.gps.day = gps_data->day; + log.u.gps.course = gps_data->course; + log.u.gps.ground_speed = gps_data->ground_speed; + log.u.gps.climb_rate = gps_data->climb_rate; + log.u.gps.pdop = gps_data->pdop; + log.u.gps.hdop = gps_data->hdop; + log.u.gps.vdop = gps_data->vdop; + log.u.gps.mode = gps_data->mode; + log.u.gps.state = state; + ao_log_gps(&log); +} + +void +ao_log_gps_tracking(uint16_t tick, struct ao_telemetry_satellite *gps_tracking_data) +{ + uint8_t c, n, i; + + log.tick = tick; + log.type = AO_LOG_GPS_SAT; + i = 0; + n = gps_tracking_data->channels; + for (c = 0; c < n; c++) + if ((log.u.gps_sat.sats[i].svid = gps_tracking_data->sats[c].svid)) + { + log.u.gps_sat.sats[i].c_n = gps_tracking_data->sats[c].c_n_1; + i++; + if (i >= 12) + break; + } + log.u.gps_sat.channels = i; + ao_log_gps(&log); +} + +static uint8_t +ao_log_dump_check_data(void) +{ + if (ao_log_csum((uint8_t *) &log) != 0) + return 0; + return 1; +} + +uint16_t +ao_log_flight(uint8_t slot) +{ + if (!ao_storage_read(ao_log_pos(slot), + &log, + sizeof (struct ao_log_gps))) + 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_gps.h b/src/kernel/ao_log_gps.h new file mode 100644 index 00000000..733db19b --- /dev/null +++ b/src/kernel/ao_log_gps.h @@ -0,0 +1,32 @@ +/* + * 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" +#include "ao_telemetry.h" + +uint8_t +ao_log_gps_should_log(int32_t lat, int32_t lon, int16_t alt); + +void +ao_log_gps_flight(void); + +void +ao_log_gps_data(uint16_t tick, uint8_t state, struct ao_telemetry_location *gps_data); + +void +ao_log_gps_tracking(uint16_t tick, struct ao_telemetry_satellite *gps_tracking_data); + -- cgit v1.2.3 From 394ab536257ab58de0190b3828dd3bb897ad4474 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 7 Jun 2014 11:40:41 -0700 Subject: altos: Write tracker logging from tracker thread directly Also, logs 8 pre-launch GPS packets so we can get the ground position. Signed-off-by: Keith Packard --- src/kernel/ao_tracker.c | 270 ++++++++++++++++++++++++++++-------------------- src/kernel/ao_tracker.h | 23 +++++ 2 files changed, 180 insertions(+), 113 deletions(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c index b207c562..4bc229b4 100644 --- a/src/kernel/ao_tracker.c +++ b/src/kernel/ao_tracker.c @@ -17,14 +17,14 @@ #include #include +#include +#include #include +#include #include enum ao_flight_state ao_flight_state; -/* Speeds for the various modes, 2m/s seems reasonable for 'not moving' */ -#define AO_TRACKER_NOT_MOVING 200 - static uint8_t ao_tracker_force_telem; static uint8_t ao_tracker_force_launch; @@ -40,45 +40,147 @@ ao_usb_connected(void) #define STARTUP_AVERAGE 5 +int32_t ao_tracker_start_latitude; +int32_t ao_tracker_start_longitude; +int16_t ao_tracker_start_altitude; + +struct ao_tracker_data ao_tracker_data[AO_TRACKER_RING]; +uint8_t ao_tracker_head; +static uint8_t ao_tracker_log_pos; + +static uint16_t telem_rate; +static uint8_t gps_rate; +static uint8_t telem_enabled; + +static int16_t lat_sum, lon_sum; +static int32_t alt_sum; +static int nsamples; + static void -ao_tracker_start_flight(void) +ao_tracker_state_update(struct ao_tracker_data *tracker) { - struct ao_log_mega log; - ao_log_start(); - log.type = AO_LOG_FLIGHT; - log.tick = ao_time(); -#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_roll = ao_ground_roll; - log.u.flight.ground_pitch = ao_ground_pitch; - log.u.flight.ground_yaw = ao_ground_yaw; -#endif -#if HAS_FLIGHT - log.u.flight.ground_pres = ao_ground_pres; -#endif - log.u.flight.flight = ao_flight_number; - ao_log_mega(&log); + uint16_t new_telem_rate; + uint8_t new_gps_rate; + uint8_t new_telem_enabled; + uint32_t ground_distance; + int16_t height; + uint16_t speed; + + new_gps_rate = gps_rate; + new_telem_rate = telem_rate; + + new_telem_enabled = ao_tracker_force_telem || !ao_usb_connected(); + + /* Don't change anything if GPS isn't locked */ + if ((tracker->new & AO_GPS_NEW_DATA) && + (tracker->gps_data.flags & (AO_GPS_VALID|AO_GPS_COURSE_VALID)) == + (AO_GPS_VALID|AO_GPS_COURSE_VALID)) + { + switch (ao_flight_state) { + case ao_flight_startup: + /* startup to pad when GPS locks */ + + lat_sum += tracker->gps_data.latitude; + lon_sum += tracker->gps_data.longitude; + alt_sum += tracker->gps_data.altitude; + + if (++nsamples >= STARTUP_AVERAGE) { + ao_flight_state = ao_flight_pad; + ao_wakeup(&ao_flight_state); + ao_tracker_start_latitude = lat_sum / nsamples; + ao_tracker_start_longitude = lon_sum / nsamples; + ao_tracker_start_altitude = alt_sum / nsamples; + } + break; + case ao_flight_pad: + ground_distance = ao_distance(tracker->gps_data.latitude, + tracker->gps_data.longitude, + ao_tracker_start_latitude, + ao_tracker_start_longitude); + height = tracker->gps_data.altitude - ao_tracker_start_altitude; + if (height < 0) + height = -height; + + if (ground_distance >= ao_config.tracker_start_horiz || + height >= ao_config.tracker_start_vert || + ao_tracker_force_launch) + { + ao_flight_state = ao_flight_drogue; + ao_wakeup(&ao_flight_state); + ao_tracker_log_pos = ao_tracker_ring_next(ao_tracker_head); + ao_log_start(); + ao_log_gps_flight(); + } + break; + case ao_flight_drogue: + /* Modulate data rates based on speed (in cm/s) */ + if (tracker->gps_data.climb_rate < 0) + speed = -tracker->gps_data.climb_rate; + else + speed = tracker->gps_data.climb_rate; + speed += tracker->gps_data.ground_speed; + + if (speed < AO_TRACKER_NOT_MOVING) { + new_telem_rate = AO_SEC_TO_TICKS(10); + new_gps_rate = 10; + } else { + new_telem_rate = AO_SEC_TO_TICKS(1); + new_gps_rate = 1; + } + break; + default: + break; + } + } + + if (new_telem_rate != telem_rate || new_telem_enabled != telem_enabled) { + if (new_telem_enabled) + ao_telemetry_set_interval(new_telem_rate); + else + ao_telemetry_set_interval(0); + telem_rate = new_telem_rate; + telem_enabled = new_telem_enabled; + } + + if (new_gps_rate != gps_rate) { + ao_gps_set_rate(new_gps_rate); + gps_rate = new_gps_rate; + } } +#if HAS_LOG +static uint8_t ao_tracker_should_log; + +static void +ao_tracker_log(void) +{ + struct ao_tracker_data *tracker; + + if (ao_log_running) { + while (ao_tracker_log_pos != ao_tracker_head) { + tracker = &ao_tracker_data[ao_tracker_log_pos]; + if (tracker->new & AO_GPS_NEW_DATA) { + ao_tracker_should_log = ao_log_gps_should_log(tracker->gps_data.latitude, + tracker->gps_data.longitude, + tracker->gps_data.altitude); + if (ao_tracker_should_log) + ao_log_gps_data(tracker->tick, tracker->state, &tracker->gps_data); + } + if (tracker->new & AO_GPS_NEW_TRACKING) { + if (ao_tracker_should_log) + ao_log_gps_tracking(tracker->tick, &tracker->gps_tracking_data); + } + ao_tracker_log_pos = ao_tracker_ring_next(ao_tracker_log_pos); + } + } +} +#endif + static void ao_tracker(void) { - uint16_t telem_rate = AO_SEC_TO_TICKS(1), new_telem_rate; - uint8_t gps_rate = 1, new_gps_rate; - uint8_t telem_enabled = 0, new_telem_enabled; - int32_t start_latitude = 0, start_longitude = 0; - int16_t start_altitude = 0; - uint32_t ground_distance; - int16_t height; - uint16_t speed; - int64_t lat_sum = 0, lon_sum = 0; - int32_t alt_sum = 0; - int nsamples = 0; + uint8_t new; + struct ao_tracker_data *tracker; #if HAS_ADC ao_timer_set_adc_interval(100); @@ -91,94 +193,36 @@ ao_tracker(void) ao_log_scan(); ao_rdf_set(1); - ao_telemetry_set_interval(0); + telem_rate = AO_SEC_TO_TICKS(1); + telem_enabled = 0; + gps_rate = 1; ao_flight_state = ao_flight_startup; for (;;) { - ao_sleep(&ao_gps_new); - - new_gps_rate = gps_rate; - new_telem_rate = telem_rate; - - new_telem_enabled = ao_tracker_force_telem || !ao_usb_connected(); + while (!(new = ao_gps_new)) + ao_sleep(&ao_gps_new); + /* Stick GPS data into the ring */ ao_mutex_get(&ao_gps_mutex); + tracker = &ao_tracker_data[ao_tracker_head]; + tracker->tick = ao_gps_tick; + tracker->new = new; + tracker->state = ao_flight_state; + tracker->gps_data = ao_gps_data; + tracker->gps_tracking_data = ao_gps_tracking_data; + ao_tracker_head = ao_tracker_ring_next(ao_tracker_head); - /* Don't change anything if GPS isn't locked */ - if ((ao_gps_data.flags & (AO_GPS_VALID|AO_GPS_COURSE_VALID)) == - (AO_GPS_VALID|AO_GPS_COURSE_VALID)) - { - switch (ao_flight_state) { - case ao_flight_startup: - /* startup to pad when GPS locks */ - - lat_sum += ao_gps_data.latitude; - lon_sum += ao_gps_data.longitude; - alt_sum += ao_gps_data.altitude; - - if (++nsamples >= STARTUP_AVERAGE) { - ao_flight_state = ao_flight_pad; - ao_wakeup(&ao_flight_state); - start_latitude = lat_sum / nsamples; - start_longitude = lon_sum / nsamples; - start_altitude = alt_sum / nsamples; - } - break; - case ao_flight_pad: - ground_distance = ao_distance(ao_gps_data.latitude, - ao_gps_data.longitude, - start_latitude, - start_longitude); - height = ao_gps_data.altitude - start_altitude; - if (height < 0) - height = -height; - - if (ground_distance >= ao_config.tracker_start_horiz || - height >= ao_config.tracker_start_vert || - ao_tracker_force_launch) - { - ao_flight_state = ao_flight_drogue; - ao_wakeup(&ao_flight_state); - ao_log_start(); - ao_tracker_start_flight(); - } - break; - case ao_flight_drogue: - /* Modulate data rates based on speed (in cm/s) */ - if (ao_gps_data.climb_rate < 0) - speed = -ao_gps_data.climb_rate; - else - speed = ao_gps_data.climb_rate; - speed += ao_gps_data.ground_speed; - - if (speed < AO_TRACKER_NOT_MOVING) { - new_telem_rate = AO_SEC_TO_TICKS(10); - new_gps_rate = 10; - } else { - new_telem_rate = AO_SEC_TO_TICKS(1); - new_gps_rate = 1; - } - break; - default: - break; - } - } + ao_gps_new = 0; ao_mutex_put(&ao_gps_mutex); - if (new_telem_rate != telem_rate || new_telem_enabled != telem_enabled) { - if (new_telem_enabled) - ao_telemetry_set_interval(new_telem_rate); - else - ao_telemetry_set_interval(0); - telem_rate = new_telem_rate; - telem_enabled = new_telem_enabled; - } + /* Update state based on current GPS data */ + ao_tracker_state_update(tracker); - if (new_gps_rate != gps_rate) { - ao_gps_set_rate(new_gps_rate); - gps_rate = new_gps_rate; - } +#if HAS_LOG + /* Log all gps data */ + ao_tracker_log(); +#endif } } diff --git a/src/kernel/ao_tracker.h b/src/kernel/ao_tracker.h index 43556965..1adf0f33 100644 --- a/src/kernel/ao_tracker.h +++ b/src/kernel/ao_tracker.h @@ -21,6 +21,29 @@ #define AO_CONFIG_DEFAULT_TRACKER_START_HORIZ 1000 #define AO_CONFIG_DEFAULT_TRACKER_START_VERT 100 +/* Speeds for the various modes, 2m/s seems reasonable for 'not moving' */ +#define AO_TRACKER_NOT_MOVING 200 + +extern int32_t ao_tracker_start_latitude; +extern int32_t ao_tracker_start_longitude; +extern int16_t ao_tracker_start_altitude; + +#define AO_TRACKER_RING 8 + +struct ao_tracker_data { + uint16_t tick; + uint8_t new; + uint8_t state; + struct ao_telemetry_location gps_data; + struct ao_telemetry_satellite gps_tracking_data; +}; + +extern struct ao_tracker_data ao_tracker_data[AO_TRACKER_RING]; +extern uint8_t ao_tracker_head; + +#define ao_tracker_ring_next(n) (((n) + 1) & (AO_TRACKER_RING-1)) +#define ao_tracker_ring_prev(n) (((n) - 1) & (AO_TRACKER_RING-1)) + void ao_tracker_init(void); -- cgit v1.2.3 From 08550425fca3da73d8f16de567a2c956b85d676e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 7 Jun 2014 21:02:26 -0700 Subject: altos: Use 0x80 to indicate valid state value in the GPS location packet And only set this for tracker products; other products place state in separate state packets Signed-off-by: Keith Packard --- src/kernel/ao_telemetry.c | 4 +++- src/kernel/ao_telemetry.h | 2 ++ 2 files changed, 5 insertions(+), 1 deletion(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_telemetry.c b/src/kernel/ao_telemetry.c index 7850f738..2292d229 100644 --- a/src/kernel/ao_telemetry.c +++ b/src/kernel/ao_telemetry.c @@ -334,7 +334,9 @@ ao_send_location(void) ao_xmemcpy(&telemetry.location.flags, &ao_gps_data.flags, 26); - telemetry.location.state = ao_flight_state; +#if HAS_TRACKER + telemetry.location.state = ao_flight_state | AO_GPS_STATE_VALID; +#endif telemetry.location.tick = ao_gps_tick; ao_mutex_put(&ao_gps_mutex); ao_radio_send(&telemetry, sizeof (telemetry)); diff --git a/src/kernel/ao_telemetry.h b/src/kernel/ao_telemetry.h index 1aeda264..050bbf72 100644 --- a/src/kernel/ao_telemetry.h +++ b/src/kernel/ao_telemetry.h @@ -93,6 +93,8 @@ struct ao_telemetry_configuration { #define AO_GPS_MODE_MANUAL 'M' #define AO_GPS_MODE_SIMULATED 'S' +#define AO_GPS_STATE_VALID 0x80 + struct ao_telemetry_location { uint16_t serial; /* 0 */ uint16_t tick; /* 2 */ -- cgit v1.2.3 From bd9e4f30b2a491b030246943767960ab053ac94c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 7 Jun 2014 21:05:01 -0700 Subject: altos: Define lat/lon sum variables as 64-bit instead of 16 Oops. 16 bits won't hold position information... Signed-off-by: Keith Packard --- src/kernel/ao_tracker.c | 26 ++++++++++++++++++++++---- 1 file changed, 22 insertions(+), 4 deletions(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c index 4bc229b4..cdf147cd 100644 --- a/src/kernel/ao_tracker.c +++ b/src/kernel/ao_tracker.c @@ -52,7 +52,7 @@ static uint16_t telem_rate; static uint8_t gps_rate; static uint8_t telem_enabled; -static int16_t lat_sum, lon_sum; +static int64_t lat_sum, lon_sum; static int32_t alt_sum; static int nsamples; @@ -78,13 +78,18 @@ ao_tracker_state_update(struct ao_tracker_data *tracker) { switch (ao_flight_state) { case ao_flight_startup: + new_telem_rate = AO_SEC_TO_TICKS(1); + new_gps_rate = 1; + /* startup to pad when GPS locks */ lat_sum += tracker->gps_data.latitude; lon_sum += tracker->gps_data.longitude; alt_sum += tracker->gps_data.altitude; - if (++nsamples >= STARTUP_AVERAGE) { + ++nsamples; + + if (nsamples >= STARTUP_AVERAGE) { ao_flight_state = ao_flight_pad; ao_wakeup(&ao_flight_state); ao_tracker_start_latitude = lat_sum / nsamples; @@ -93,6 +98,9 @@ ao_tracker_state_update(struct ao_tracker_data *tracker) } break; case ao_flight_pad: + new_telem_rate = AO_SEC_TO_TICKS(1); + new_gps_rate = 1; + ground_distance = ao_distance(tracker->gps_data.latitude, tracker->gps_data.longitude, ao_tracker_start_latitude, @@ -190,6 +198,11 @@ ao_tracker(void) ao_tracker_force_telem = 1; #endif + nsamples = 0; + lat_sum = 0; + lon_sum = 0; + alt_sum = 0; + ao_log_scan(); ao_rdf_set(1); @@ -231,11 +244,16 @@ static struct ao_task ao_tracker_task; static void ao_tracker_set_telem(void) { + uint8_t telem, launch; + ao_cmd_hex(); + telem = ao_cmd_lex_i; ao_cmd_hex(); + launch = ao_cmd_lex_i; if (ao_cmd_status == ao_cmd_success) { - ao_tracker_force_telem = (ao_cmd_lex_i & 1) != 0; - ao_tracker_force_launch = (ao_cmd_lex_i & 2) != 0; + ao_tracker_force_telem = telem; + ao_tracker_force_launch = launch; } + ao_cmd_status = ao_cmd_success; printf ("flight %d force telem %d force launch %d\n", ao_flight_number, ao_tracker_force_telem, ao_tracker_force_launch); } -- cgit v1.2.3 From d165079b9275c69e727a1dac996ad1788c58ed40 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 7 Jun 2014 21:11:33 -0700 Subject: altos: Reduce tracker GPS buffer to 4 samples We just don't have enough RAM for 8 samples. Signed-off-by: Keith Packard --- src/kernel/ao_tracker.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_tracker.h b/src/kernel/ao_tracker.h index 1adf0f33..63bdaf2f 100644 --- a/src/kernel/ao_tracker.h +++ b/src/kernel/ao_tracker.h @@ -28,7 +28,7 @@ extern int32_t ao_tracker_start_latitude; extern int32_t ao_tracker_start_longitude; extern int16_t ao_tracker_start_altitude; -#define AO_TRACKER_RING 8 +#define AO_TRACKER_RING 4 struct ao_tracker_data { uint16_t tick; -- cgit v1.2.3 From ef85b3bc5300904ebfb878b1c7313a82b5b7aebf Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 7 Jun 2014 21:57:43 -0700 Subject: altos: Encode TeleGPS battery voltage in configuration packet TeleGPS doesn't need apogee delay, so re-purpose it for the battery voltage Signed-off-by: Keith Packard --- src/kernel/ao_telemetry.c | 6 ++++++ 1 file changed, 6 insertions(+) (limited to 'src/kernel') diff --git a/src/kernel/ao_telemetry.c b/src/kernel/ao_telemetry.c index 2292d229..dcda0dd7 100644 --- a/src/kernel/ao_telemetry.c +++ b/src/kernel/ao_telemetry.c @@ -305,8 +305,14 @@ ao_send_configuration(void) #endif telemetry.configuration.config_major = AO_CONFIG_MAJOR; telemetry.configuration.config_minor = AO_CONFIG_MINOR; +#if AO_idProduct_NUMBER == 0x25 && HAS_ADC + /* TeleGPS gets battery voltage instead of apogee delay */ + telemetry.configuration.apogee_delay = ao_data_ring[ao_data_ring_prev(ao_data_head)].adc.v_batt; +#else telemetry.configuration.apogee_delay = ao_config.apogee_delay; telemetry.configuration.main_deploy = ao_config.main_deploy; +#endif + telemetry.configuration.flight_log_max = ao_config.flight_log_max >> 10; ao_xmemcpy (telemetry.configuration.callsign, ao_config.callsign, -- cgit v1.2.3 From 75df97b5f6ade3310618a477b685d39b7fd4666e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 10 Jun 2014 09:37:43 -0700 Subject: altos: Report total available log space in version command This provides a more accurate means of determining available log space than guessing whether some portion of the flash chip holds configuration data. Signed-off-by: Keith Packard --- src/cc1111/ao_pins.h | 3 ++- src/kernel/ao_cmd.c | 6 ++++++ 2 files changed, 8 insertions(+), 1 deletion(-) (limited to 'src/kernel') diff --git a/src/cc1111/ao_pins.h b/src/cc1111/ao_pins.h index 298d6acc..2b19f1f6 100644 --- a/src/cc1111/ao_pins.h +++ b/src/cc1111/ao_pins.h @@ -18,7 +18,8 @@ #ifndef _AO_PINS_H_ #define _AO_PINS_H_ -#define HAS_RADIO 1 +#define HAS_RADIO 1 +#define DISABLE_LOG_SPACE 1 #if defined(TELEMETRUM_V_1_0) /* Discontinued and was never built with CC1111 chips needing this */ diff --git a/src/kernel/ao_cmd.c b/src/kernel/ao_cmd.c index 69372fed..5d8683e1 100644 --- a/src/kernel/ao_cmd.c +++ b/src/kernel/ao_cmd.c @@ -283,6 +283,9 @@ version(void) #endif #if HAS_LOG "log-format %u\n" +#if !DISABLE_LOG_SPACE + "log-space %lu\n" +#endif #endif #if defined(AO_BOOT_APPLICATION_BASE) && defined(AO_BOOT_APPLICATION_BOUND) "program-space %u\n" @@ -295,6 +298,9 @@ version(void) #endif #if HAS_LOG , ao_log_format +#if !DISABLE_LOG_SPACE + , (unsigned long) ao_storage_log_max +#endif #endif #if defined(AO_BOOT_APPLICATION_BASE) && defined(AO_BOOT_APPLICATION_BOUND) , (uint32_t) AO_BOOT_APPLICATION_BOUND - (uint32_t) AO_BOOT_APPLICATION_BASE -- cgit v1.2.3 From da9575fce5ff4dfe83522e290973a01c43e4661f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 10 Jun 2014 09:42:43 -0700 Subject: altos: Make extra pyro channel firing time configurable This adds a 'I' parameter to set the extra pyro channel firing time (in ticks). This has no effect on the main/drogue channels. Signed-off-by: Keith Packard --- src/kernel/ao_config.c | 26 ++++++++++++++++++++++++++ src/kernel/ao_config.h | 5 ++++- src/kernel/ao_pyro.c | 2 +- 3 files changed, 31 insertions(+), 2 deletions(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c index b4847cdd..72170555 100644 --- a/src/kernel/ao_config.c +++ b/src/kernel/ao_config.c @@ -44,6 +44,7 @@ __xdata uint8_t ao_config_mutex; #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 +#define AO_CONFIG_DEFAULT_PYRO_TIME AO_MS_TO_TICKS(50) #if HAS_EEPROM #ifndef USE_INTERNAL_FLASH #error Please define USE_INTERNAL_FLASH @@ -186,6 +187,10 @@ _ao_config_get(void) ao_config.tracker_start_horiz = AO_CONFIG_DEFAULT_TRACKER_START_HORIZ; ao_config.tracker_start_vert = AO_CONFIG_DEFAULT_TRACKER_START_VERT; } +#endif +#if AO_PYRO_NUM + if (minor < 18) + ao_config.pyro_time = AO_CONFIG_DEFAULT_PYRO_TIME; #endif ao_config.minor = AO_CONFIG_MINOR; ao_config_dirty = 1; @@ -713,6 +718,25 @@ ao_config_tracker_set(void) } #endif /* HAS_TRACKER */ +#if AO_PYRO_NUM +void +ao_config_pyro_time_show(void) +{ + printf ("Pyro time: %d\n", ao_config.pyro_time); +} + +void +ao_config_pyro_time_set(void) +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.pyro_time = ao_cmd_lex_i; + _ao_config_edit_finish(); +} +#endif + struct ao_config_var { __code char *str; void (*set)(void) __reentrant; @@ -778,6 +802,8 @@ __code struct ao_config_var ao_config_vars[] = { #if AO_PYRO_NUM { "P \0Pyro channels", ao_pyro_set, ao_pyro_show }, + { "I \0Pyro firing time", + ao_config_pyro_time_set, ao_config_pyro_time_show }, #endif #if HAS_APRS { "A \0APRS packet interval (0 disable)", diff --git a/src/kernel/ao_config.h b/src/kernel/ao_config.h index 7ad221c6..77f73fbe 100644 --- a/src/kernel/ao_config.h +++ b/src/kernel/ao_config.h @@ -53,7 +53,7 @@ #endif #define AO_CONFIG_MAJOR 1 -#define AO_CONFIG_MINOR 17 +#define AO_CONFIG_MINOR 18 #define AO_AES_LEN 16 @@ -99,6 +99,9 @@ struct ao_config { uint16_t tracker_start_horiz; /* minor version 17 */ uint16_t tracker_start_vert; /* minor version 17 */ #endif +#if AO_PYRO_NUM + uint16_t pyro_time; /* minor version 18 */ +#endif }; #define AO_IGNITE_MODE_DUAL 0 diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c index 0ee7fbee..85d88d98 100644 --- a/src/kernel/ao_pyro.c +++ b/src/kernel/ao_pyro.c @@ -213,7 +213,7 @@ ao_pyro_pins_fire(uint16_t fire) if (fire & (1 << p)) ao_pyro_pin_set(p, 1); } - ao_delay(AO_MS_TO_TICKS(50)); + ao_delay(ao_config.pyro_time); for (p = 0; p < AO_PYRO_NUM; p++) { if (fire & (1 << p)) { ao_pyro_pin_set(p, 0); -- cgit v1.2.3 From c5a7889a8da3da64deb0f118656784e0ee3fd511 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 10 Jun 2014 09:47:04 -0700 Subject: Revert adding state to GPS location packets TeleGPS no longer has ao_flight_state Signed-off-by: Keith Packard --- src/kernel/ao_telemetry.c | 3 --- src/kernel/ao_telemetry.h | 4 +--- 2 files changed, 1 insertion(+), 6 deletions(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_telemetry.c b/src/kernel/ao_telemetry.c index dcda0dd7..9f778b09 100644 --- a/src/kernel/ao_telemetry.c +++ b/src/kernel/ao_telemetry.c @@ -340,9 +340,6 @@ ao_send_location(void) ao_xmemcpy(&telemetry.location.flags, &ao_gps_data.flags, 26); -#if HAS_TRACKER - telemetry.location.state = ao_flight_state | AO_GPS_STATE_VALID; -#endif telemetry.location.tick = ao_gps_tick; ao_mutex_put(&ao_gps_mutex); ao_radio_send(&telemetry, sizeof (telemetry)); diff --git a/src/kernel/ao_telemetry.h b/src/kernel/ao_telemetry.h index 050bbf72..be7d0340 100644 --- a/src/kernel/ao_telemetry.h +++ b/src/kernel/ao_telemetry.h @@ -93,8 +93,6 @@ struct ao_telemetry_configuration { #define AO_GPS_MODE_MANUAL 'M' #define AO_GPS_MODE_SIMULATED 'S' -#define AO_GPS_STATE_VALID 0x80 - struct ao_telemetry_location { uint16_t serial; /* 0 */ uint16_t tick; /* 2 */ @@ -117,7 +115,7 @@ struct ao_telemetry_location { uint16_t ground_speed; /* 26 cm/s */ int16_t climb_rate; /* 28 cm/s */ uint8_t course; /* 30 degrees / 2 */ - uint8_t state; /* 31 flight state */ + uint8_t unused; /* 31 unused */ /* 32 */ }; -- cgit v1.2.3 From 9d7f4fb6af0fee843191766858e39a481aeda347 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 10 Jun 2014 09:52:15 -0700 Subject: altos: Simplify tracker logic, removing boost detect This removes the ao_flight_state value from the tracker code and makes it simply log position information when the device has moved within the last 10 log intervals. This also changes the configuration parameters to define what 'motionless' means, and what interval to configure the GPS receiver for, log data and send telemetry. Signed-off-by: Keith Packard --- src/kernel/ao_config.c | 20 +-- src/kernel/ao_config.h | 4 +- src/kernel/ao_log_gps.c | 37 +----- src/kernel/ao_log_gps.h | 9 +- src/kernel/ao_tracker.c | 277 ++++++++++++++---------------------------- src/kernel/ao_tracker.h | 27 +--- src/telegps-v0.3/ao_telegps.c | 1 - src/telegps-v1.0/ao_telegps.c | 1 - 8 files changed, 113 insertions(+), 263 deletions(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c index 72170555..71445335 100644 --- a/src/kernel/ao_config.c +++ b/src/kernel/ao_config.c @@ -184,8 +184,8 @@ _ao_config_get(void) #endif #if HAS_TRACKER if (minor < 17) { - ao_config.tracker_start_horiz = AO_CONFIG_DEFAULT_TRACKER_START_HORIZ; - ao_config.tracker_start_vert = AO_CONFIG_DEFAULT_TRACKER_START_VERT; + ao_config.tracker_motion = AO_TRACKER_MOTION_DEFAULT; + ao_config.tracker_interval = AO_TRACKER_INTERVAL_DEFAULT; } #endif #if AO_PYRO_NUM @@ -695,25 +695,25 @@ void ao_config_tracker_show(void) { printf ("Tracker setting: %d %d\n", - ao_config.tracker_start_horiz, - ao_config.tracker_start_vert); + ao_config.tracker_motion, + ao_config.tracker_interval); } void ao_config_tracker_set(void) { - uint16_t h, v; + uint16_t m, i; ao_cmd_decimal(); if (ao_cmd_status != ao_cmd_success) return; - h = ao_cmd_lex_i; + m = ao_cmd_lex_i; ao_cmd_decimal(); if (ao_cmd_status != ao_cmd_success) return; - v = ao_cmd_lex_i; + i = ao_cmd_lex_i; _ao_config_edit_start(); - ao_config.tracker_start_horiz = h; - ao_config.tracker_start_vert = v; + ao_config.tracker_motion = m; + ao_config.tracker_interval = i; _ao_config_edit_finish(); } #endif /* HAS_TRACKER */ @@ -814,7 +814,7 @@ __code struct ao_config_var ao_config_vars[] = { ao_config_beep_set, ao_config_beep_show }, #endif #if HAS_TRACKER - { "t \0Tracker start trigger distances", + { "t \0Tracker configuration", ao_config_tracker_set, ao_config_tracker_show }, #endif { "s\0Show", diff --git a/src/kernel/ao_config.h b/src/kernel/ao_config.h index 77f73fbe..2b5cd352 100644 --- a/src/kernel/ao_config.h +++ b/src/kernel/ao_config.h @@ -96,8 +96,8 @@ struct ao_config { uint8_t mid_beep; /* minor version 16 */ #endif #if HAS_TRACKER - uint16_t tracker_start_horiz; /* minor version 17 */ - uint16_t tracker_start_vert; /* minor version 17 */ + uint16_t tracker_motion; /* minor version 17 */ + uint8_t tracker_interval; /* minor version 17 */ #endif #if AO_PYRO_NUM uint16_t pyro_time; /* minor version 18 */ diff --git a/src/kernel/ao_log_gps.c b/src/kernel/ao_log_gps.c index e9e573a5..8bf529f4 100644 --- a/src/kernel/ao_log_gps.c +++ b/src/kernel/ao_log_gps.c @@ -60,51 +60,17 @@ ao_log_gps(__xdata struct ao_log_gps *log) __reentrant return wrote; } - -static int32_t prev_lat, prev_lon; -static int16_t prev_alt; -static uint8_t has_prev, unmoving; - -#define GPS_SPARSE_UNMOVING_REPORTS 10 -#define GPS_SPARSE_UNMOVING_GROUND 10 -#define GPS_SPARSE_UNMOVING_AIR 10 - -uint8_t -ao_log_gps_should_log(int32_t lat, int32_t lon, int16_t alt) -{ - if (has_prev && ao_log_running) { - uint32_t h = ao_distance(prev_lat, prev_lon, lat, lon); - uint16_t v = alt > prev_alt ? (alt - prev_alt) : (prev_alt - alt); - - if (h < GPS_SPARSE_UNMOVING_GROUND && v < GPS_SPARSE_UNMOVING_AIR) { - if (unmoving < GPS_SPARSE_UNMOVING_REPORTS) - ++unmoving; - } else - unmoving = 0; - } else - unmoving = 0; - - prev_lat = lat; - prev_lon = lon; - prev_alt = alt; - has_prev = 1; - return unmoving >= GPS_SPARSE_UNMOVING_REPORTS; -} - void ao_log_gps_flight(void) { log.type = AO_LOG_FLIGHT; log.tick = ao_time(); log.u.flight.flight = ao_flight_number; - log.u.flight.start_altitude = ao_tracker_start_altitude; - log.u.flight.start_latitude = ao_tracker_start_latitude; - log.u.flight.start_longitude = ao_tracker_start_longitude; ao_log_gps(&log); } void -ao_log_gps_data(uint16_t tick, uint8_t state, struct ao_telemetry_location *gps_data) +ao_log_gps_data(uint16_t tick, struct ao_telemetry_location *gps_data) { log.tick = tick; log.type = AO_LOG_GPS_TIME; @@ -126,7 +92,6 @@ ao_log_gps_data(uint16_t tick, uint8_t state, struct ao_telemetry_location *gps_ log.u.gps.hdop = gps_data->hdop; log.u.gps.vdop = gps_data->vdop; log.u.gps.mode = gps_data->mode; - log.u.gps.state = state; ao_log_gps(&log); } diff --git a/src/kernel/ao_log_gps.h b/src/kernel/ao_log_gps.h index 733db19b..5851f4d1 100644 --- a/src/kernel/ao_log_gps.h +++ b/src/kernel/ao_log_gps.h @@ -18,6 +18,9 @@ #include "ao.h" #include "ao_telemetry.h" +#ifndef _AO_LOG_GPS_H_ +#define _AO_LOG_GPS_H_ + uint8_t ao_log_gps_should_log(int32_t lat, int32_t lon, int16_t alt); @@ -25,8 +28,6 @@ void ao_log_gps_flight(void); void -ao_log_gps_data(uint16_t tick, uint8_t state, struct ao_telemetry_location *gps_data); - -void -ao_log_gps_tracking(uint16_t tick, struct ao_telemetry_satellite *gps_tracking_data); +ao_log_gps_data(uint16_t tick, struct ao_telemetry_location *gps_data); +#endif /* _AO_LOG_GPS_H_ */ diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c index cdf147cd..fb9e75d0 100644 --- a/src/kernel/ao_tracker.c +++ b/src/kernel/ao_tracker.c @@ -23,10 +23,7 @@ #include #include -enum ao_flight_state ao_flight_state; - -static uint8_t ao_tracker_force_telem; -static uint8_t ao_tracker_force_launch; +static uint8_t ao_tracker_force_telem; #if HAS_USB_CONNECT static inline uint8_t @@ -38,157 +35,22 @@ ao_usb_connected(void) #define ao_usb_connected() 1 #endif -#define STARTUP_AVERAGE 5 - -int32_t ao_tracker_start_latitude; -int32_t ao_tracker_start_longitude; -int16_t ao_tracker_start_altitude; - -struct ao_tracker_data ao_tracker_data[AO_TRACKER_RING]; -uint8_t ao_tracker_head; -static uint8_t ao_tracker_log_pos; - -static uint16_t telem_rate; -static uint8_t gps_rate; -static uint8_t telem_enabled; - -static int64_t lat_sum, lon_sum; -static int32_t alt_sum; -static int nsamples; - -static void -ao_tracker_state_update(struct ao_tracker_data *tracker) -{ - uint16_t new_telem_rate; - uint8_t new_gps_rate; - uint8_t new_telem_enabled; - uint32_t ground_distance; - int16_t height; - uint16_t speed; - - new_gps_rate = gps_rate; - new_telem_rate = telem_rate; - - new_telem_enabled = ao_tracker_force_telem || !ao_usb_connected(); - - /* Don't change anything if GPS isn't locked */ - if ((tracker->new & AO_GPS_NEW_DATA) && - (tracker->gps_data.flags & (AO_GPS_VALID|AO_GPS_COURSE_VALID)) == - (AO_GPS_VALID|AO_GPS_COURSE_VALID)) - { - switch (ao_flight_state) { - case ao_flight_startup: - new_telem_rate = AO_SEC_TO_TICKS(1); - new_gps_rate = 1; - - /* startup to pad when GPS locks */ - - lat_sum += tracker->gps_data.latitude; - lon_sum += tracker->gps_data.longitude; - alt_sum += tracker->gps_data.altitude; - - ++nsamples; - - if (nsamples >= STARTUP_AVERAGE) { - ao_flight_state = ao_flight_pad; - ao_wakeup(&ao_flight_state); - ao_tracker_start_latitude = lat_sum / nsamples; - ao_tracker_start_longitude = lon_sum / nsamples; - ao_tracker_start_altitude = alt_sum / nsamples; - } - break; - case ao_flight_pad: - new_telem_rate = AO_SEC_TO_TICKS(1); - new_gps_rate = 1; - - ground_distance = ao_distance(tracker->gps_data.latitude, - tracker->gps_data.longitude, - ao_tracker_start_latitude, - ao_tracker_start_longitude); - height = tracker->gps_data.altitude - ao_tracker_start_altitude; - if (height < 0) - height = -height; - - if (ground_distance >= ao_config.tracker_start_horiz || - height >= ao_config.tracker_start_vert || - ao_tracker_force_launch) - { - ao_flight_state = ao_flight_drogue; - ao_wakeup(&ao_flight_state); - ao_tracker_log_pos = ao_tracker_ring_next(ao_tracker_head); - ao_log_start(); - ao_log_gps_flight(); - } - break; - case ao_flight_drogue: - /* Modulate data rates based on speed (in cm/s) */ - if (tracker->gps_data.climb_rate < 0) - speed = -tracker->gps_data.climb_rate; - else - speed = tracker->gps_data.climb_rate; - speed += tracker->gps_data.ground_speed; - - if (speed < AO_TRACKER_NOT_MOVING) { - new_telem_rate = AO_SEC_TO_TICKS(10); - new_gps_rate = 10; - } else { - new_telem_rate = AO_SEC_TO_TICKS(1); - new_gps_rate = 1; - } - break; - default: - break; - } - } - - if (new_telem_rate != telem_rate || new_telem_enabled != telem_enabled) { - if (new_telem_enabled) - ao_telemetry_set_interval(new_telem_rate); - else - ao_telemetry_set_interval(0); - telem_rate = new_telem_rate; - telem_enabled = new_telem_enabled; - } - - if (new_gps_rate != gps_rate) { - ao_gps_set_rate(new_gps_rate); - gps_rate = new_gps_rate; - } -} - -#if HAS_LOG -static uint8_t ao_tracker_should_log; - -static void -ao_tracker_log(void) -{ - struct ao_tracker_data *tracker; - - if (ao_log_running) { - while (ao_tracker_log_pos != ao_tracker_head) { - tracker = &ao_tracker_data[ao_tracker_log_pos]; - if (tracker->new & AO_GPS_NEW_DATA) { - ao_tracker_should_log = ao_log_gps_should_log(tracker->gps_data.latitude, - tracker->gps_data.longitude, - tracker->gps_data.altitude); - if (ao_tracker_should_log) - ao_log_gps_data(tracker->tick, tracker->state, &tracker->gps_data); - } - if (tracker->new & AO_GPS_NEW_TRACKING) { - if (ao_tracker_should_log) - ao_log_gps_tracking(tracker->tick, &tracker->gps_tracking_data); - } - ao_tracker_log_pos = ao_tracker_ring_next(ao_tracker_log_pos); - } - } -} -#endif +static int32_t last_log_latitude, last_log_longitude; +static int16_t last_log_altitude; +static uint8_t unmoving; +static uint8_t log_started; +static struct ao_telemetry_location gps_data; +static uint8_t tracker_running; +static uint16_t tracker_interval; static void ao_tracker(void) { - uint8_t new; - struct ao_tracker_data *tracker; + uint8_t new; + int32_t ground_distance; + int16_t height; + uint16_t gps_tick; + uint8_t new_tracker_running; #if HAS_ADC ao_timer_set_adc_interval(100); @@ -197,45 +59,83 @@ ao_tracker(void) #if !HAS_USB_CONNECT ao_tracker_force_telem = 1; #endif - - nsamples = 0; - lat_sum = 0; - lon_sum = 0; - alt_sum = 0; - ao_log_scan(); + ao_log_start(); ao_rdf_set(1); - ao_telemetry_set_interval(0); - telem_rate = AO_SEC_TO_TICKS(1); - telem_enabled = 0; - gps_rate = 1; - ao_flight_state = ao_flight_startup; + tracker_interval = ao_config.tracker_interval; + ao_gps_set_rate(tracker_interval); + for (;;) { + + /** Wait for new GPS data + */ while (!(new = ao_gps_new)) ao_sleep(&ao_gps_new); - - /* Stick GPS data into the ring */ ao_mutex_get(&ao_gps_mutex); - tracker = &ao_tracker_data[ao_tracker_head]; - tracker->tick = ao_gps_tick; - tracker->new = new; - tracker->state = ao_flight_state; - tracker->gps_data = ao_gps_data; - tracker->gps_tracking_data = ao_gps_tracking_data; - ao_tracker_head = ao_tracker_ring_next(ao_tracker_head); - + gps_data = ao_gps_data; + gps_tick = ao_gps_tick; ao_gps_new = 0; ao_mutex_put(&ao_gps_mutex); - /* Update state based on current GPS data */ - ao_tracker_state_update(tracker); + new_tracker_running = ao_tracker_force_telem || !ao_usb_connected(); -#if HAS_LOG - /* Log all gps data */ - ao_tracker_log(); -#endif + if (ao_config.tracker_interval != tracker_interval) { + tracker_interval = ao_config.tracker_interval; + ao_gps_set_rate(tracker_interval); + + /* force telemetry interval to be reset */ + tracker_running = 0; + } + + if (new_tracker_running && !tracker_running) { + ao_telemetry_set_interval(AO_SEC_TO_TICKS(tracker_interval)); + } else if (!new_tracker_running && tracker_running) { + ao_telemetry_set_interval(0); + } + + tracker_running = new_tracker_running; + + if (!tracker_running) + continue; + + if (new & AO_GPS_NEW_DATA) { + if ((gps_data.flags & (AO_GPS_VALID|AO_GPS_COURSE_VALID)) == + (AO_GPS_VALID|AO_GPS_COURSE_VALID)) + { + if (log_started) { + ground_distance = ao_distance(gps_data.latitude, gps_data.longitude, + last_log_latitude, last_log_longitude); + height = last_log_altitude - gps_data.altitude; + if (height < 0) + height = -height; + if (ground_distance <= ao_config.tracker_motion && + height <= ao_config.tracker_motion) + { + if (unmoving < AO_TRACKER_MOTION_COUNT) + unmoving++; + } else + unmoving = 0; + } + } else { + if (!log_started) + continue; + if (unmoving < AO_TRACKER_MOTION_COUNT) + unmoving++; + } + + if (unmoving < AO_TRACKER_MOTION_COUNT) { + if (!log_started) { + ao_log_gps_flight(); + log_started = 1; + } + ao_log_gps_data(gps_tick, &gps_data); + last_log_latitude = gps_data.latitude; + last_log_longitude = gps_data.longitude; + last_log_altitude = gps_data.altitude; + } + } } } @@ -244,18 +144,23 @@ static struct ao_task ao_tracker_task; static void ao_tracker_set_telem(void) { - uint8_t telem, launch; + uint8_t telem; ao_cmd_hex(); telem = ao_cmd_lex_i; - ao_cmd_hex(); - launch = ao_cmd_lex_i; - if (ao_cmd_status == ao_cmd_success) { + if (ao_cmd_status == ao_cmd_success) ao_tracker_force_telem = telem; - ao_tracker_force_launch = launch; - } ao_cmd_status = ao_cmd_success; - printf ("flight %d force telem %d force launch %d\n", - ao_flight_number, ao_tracker_force_telem, ao_tracker_force_launch); + printf ("flight: %d\n", ao_flight_number); + printf ("force_telem: %d\n", ao_tracker_force_telem); + printf ("log_started: %d\n", log_started); + printf ("unmoving: %d\n", unmoving); + printf ("latitude: %ld\n", (long) gps_data.latitude); + printf ("longitude: %ld\n", (long) gps_data.longitude); + printf ("altitude: %d\n", gps_data.altitude); + printf ("log_running: %d\n", ao_log_running); + printf ("log_start_pos: %ld\n", (long) ao_log_start_pos); + printf ("log_cur_pos: %ld\n", (long) ao_log_current_pos); + printf ("log_end_pos: %ld\n", (long) ao_log_end_pos); } static const struct ao_cmds ao_tracker_cmds[] = { diff --git a/src/kernel/ao_tracker.h b/src/kernel/ao_tracker.h index 63bdaf2f..a0fd2f49 100644 --- a/src/kernel/ao_tracker.h +++ b/src/kernel/ao_tracker.h @@ -18,31 +18,12 @@ #ifndef _AO_TRACKER_H_ #define _AO_TRACKER_H_ -#define AO_CONFIG_DEFAULT_TRACKER_START_HORIZ 1000 -#define AO_CONFIG_DEFAULT_TRACKER_START_VERT 100 +/* Any motion more than this will result in a log entry */ -/* Speeds for the various modes, 2m/s seems reasonable for 'not moving' */ -#define AO_TRACKER_NOT_MOVING 200 +#define AO_TRACKER_MOTION_DEFAULT 10 +#define AO_TRACKER_INTERVAL_DEFAULT 1 -extern int32_t ao_tracker_start_latitude; -extern int32_t ao_tracker_start_longitude; -extern int16_t ao_tracker_start_altitude; - -#define AO_TRACKER_RING 4 - -struct ao_tracker_data { - uint16_t tick; - uint8_t new; - uint8_t state; - struct ao_telemetry_location gps_data; - struct ao_telemetry_satellite gps_tracking_data; -}; - -extern struct ao_tracker_data ao_tracker_data[AO_TRACKER_RING]; -extern uint8_t ao_tracker_head; - -#define ao_tracker_ring_next(n) (((n) + 1) & (AO_TRACKER_RING-1)) -#define ao_tracker_ring_prev(n) (((n) - 1) & (AO_TRACKER_RING-1)) +#define AO_TRACKER_MOTION_COUNT 10 void ao_tracker_init(void); diff --git a/src/telegps-v0.3/ao_telegps.c b/src/telegps-v0.3/ao_telegps.c index bc7eeea8..dd699ecf 100644 --- a/src/telegps-v0.3/ao_telegps.c +++ b/src/telegps-v0.3/ao_telegps.c @@ -52,7 +52,6 @@ main(void) ao_tracker_init(); ao_telemetry_init(); - ao_telemetry_set_interval(AO_SEC_TO_TICKS(1)); #if HAS_SAMPLE_PROFILE ao_sample_profile_init(); diff --git a/src/telegps-v1.0/ao_telegps.c b/src/telegps-v1.0/ao_telegps.c index 1185a5dd..7a71699b 100644 --- a/src/telegps-v1.0/ao_telegps.c +++ b/src/telegps-v1.0/ao_telegps.c @@ -55,7 +55,6 @@ main(void) ao_tracker_init(); ao_telemetry_init(); - ao_telemetry_set_interval(AO_SEC_TO_TICKS(1)); #if HAS_SAMPLE_PROFILE ao_sample_profile_init(); -- cgit v1.2.3 From 7e911c2afff78db2e385c6346c90bfcd72a8f3fb Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 12 Jun 2014 14:34:02 -0700 Subject: altos/telegps: Don't log data when plugged in to USB We don't want to accidentally log stuff when you're just trying to charge the battery. Signed-off-by: Keith Packard --- src/kernel/ao_tracker.c | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/kernel') diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c index fb9e75d0..f0229230 100644 --- a/src/kernel/ao_tracker.c +++ b/src/kernel/ao_tracker.c @@ -91,8 +91,10 @@ ao_tracker(void) if (new_tracker_running && !tracker_running) { ao_telemetry_set_interval(AO_SEC_TO_TICKS(tracker_interval)); + ao_log_start(); } else if (!new_tracker_running && tracker_running) { ao_telemetry_set_interval(0); + ao_log_stop(); } tracker_running = new_tracker_running; -- cgit v1.2.3 From 77b5c0cc7f085aa3c0fada5d4a943eeaf16cf6e0 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 12 Jun 2014 21:52:13 -0700 Subject: altos: Show current flight number for TeleGPS Signed-off-by: Keith Packard --- src/kernel/ao_cmd.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_cmd.c b/src/kernel/ao_cmd.c index 5d8683e1..0052bdce 100644 --- a/src/kernel/ao_cmd.c +++ b/src/kernel/ao_cmd.c @@ -278,7 +278,7 @@ version(void) printf("manufacturer %s\n" "product %s\n" "serial-number %u\n" -#if HAS_FLIGHT +#if HAS_FLIGHT || HAS_TRACKER "current-flight %u\n" #endif #if HAS_LOG @@ -293,7 +293,7 @@ version(void) , ao_manufacturer , ao_product , ao_serial_number -#if HAS_FLIGHT +#if HAS_FLIGHT || HAS_TRACKER , ao_flight_number #endif #if HAS_LOG -- cgit v1.2.3 From dcaaf51245b44a440ee8590512f71195c30c16ae Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 12 Jun 2014 21:54:13 -0700 Subject: altos/telegps: Keep ring of recent GPS positions to detect motion quickly Instead of comparing only against the last logged value, keep a ring and start logging as soon as we move away from the furthest one in the ring. Signed-off-by: Keith Packard --- src/kernel/ao_tracker.c | 74 ++++++++++++++++++++++++++++++------------------- 1 file changed, 45 insertions(+), 29 deletions(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c index f0229230..a6ad87a3 100644 --- a/src/kernel/ao_tracker.c +++ b/src/kernel/ao_tracker.c @@ -35,9 +35,21 @@ ao_usb_connected(void) #define ao_usb_connected() 1 #endif -static int32_t last_log_latitude, last_log_longitude; -static int16_t last_log_altitude; -static uint8_t unmoving; +struct gps_position { + int32_t latitude; + int32_t longitude; + int16_t altitude; +}; + +#define GPS_RING 16 + +struct gps_position gps_position[GPS_RING]; + +#define ao_gps_ring_next(n) (((n) + 1) & (GPS_RING - 1)) +#define ao_gps_ring_prev(n) (((n) - 1) & (GPS_RING - 1)) + +static uint8_t gps_head; + static uint8_t log_started; static struct ao_telemetry_location gps_data; static uint8_t tracker_running; @@ -60,7 +72,6 @@ ao_tracker(void) ao_tracker_force_telem = 1; #endif ao_log_scan(); - ao_log_start(); ao_rdf_set(1); @@ -106,36 +117,41 @@ ao_tracker(void) if ((gps_data.flags & (AO_GPS_VALID|AO_GPS_COURSE_VALID)) == (AO_GPS_VALID|AO_GPS_COURSE_VALID)) { - if (log_started) { + uint8_t ring; + uint8_t moving = 0; + + for (ring = ao_gps_ring_next(gps_head); ring != gps_head; ring = ao_gps_ring_next(ring)) { ground_distance = ao_distance(gps_data.latitude, gps_data.longitude, - last_log_latitude, last_log_longitude); - height = last_log_altitude - gps_data.altitude; + gps_position[ring].latitude, + gps_position[ring].longitude); + height = gps_position[ring].altitude - gps_data.altitude; if (height < 0) height = -height; - if (ground_distance <= ao_config.tracker_motion && - height <= ao_config.tracker_motion) + + if (ao_tracker_force_telem) + printf("head %d ring %d ground_distance %d height %d\n", gps_head, ring, ground_distance, height); + if (ground_distance > ao_config.tracker_motion || + height > (ao_config.tracker_motion << 1)) { - if (unmoving < AO_TRACKER_MOTION_COUNT) - unmoving++; - } else - unmoving = 0; + moving = 1; + break; + } } - } else { - if (!log_started) - continue; - if (unmoving < AO_TRACKER_MOTION_COUNT) - unmoving++; - } - - if (unmoving < AO_TRACKER_MOTION_COUNT) { - if (!log_started) { - ao_log_gps_flight(); - log_started = 1; + if (ao_tracker_force_telem) { + printf ("moving %d\n", moving); + flush(); + } + if (moving) { + if (!log_started) { + ao_log_gps_flight(); + log_started = 1; + } + ao_log_gps_data(gps_tick, &gps_data); + gps_position[gps_head].latitude = gps_data.latitude; + gps_position[gps_head].longitude = gps_data.longitude; + gps_position[gps_head].altitude = gps_data.altitude; + gps_head = ao_gps_ring_next(gps_head); } - ao_log_gps_data(gps_tick, &gps_data); - last_log_latitude = gps_data.latitude; - last_log_longitude = gps_data.longitude; - last_log_altitude = gps_data.altitude; } } } @@ -154,8 +170,8 @@ ao_tracker_set_telem(void) ao_cmd_status = ao_cmd_success; printf ("flight: %d\n", ao_flight_number); printf ("force_telem: %d\n", ao_tracker_force_telem); + printf ("tracker_running: %d\n", tracker_running); printf ("log_started: %d\n", log_started); - printf ("unmoving: %d\n", unmoving); printf ("latitude: %ld\n", (long) gps_data.latitude); printf ("longitude: %ld\n", (long) gps_data.longitude); printf ("altitude: %d\n", gps_data.altitude); -- cgit v1.2.3 From 8117ba3553789a2bae9beb92fbe9e14e3cc79389 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 12 Jun 2014 23:56:07 -0700 Subject: altos: Define ao_log_mutex in ao_log.c rather than every log product Signed-off-by: Keith Packard --- src/kernel/ao_log.c | 1 + src/kernel/ao_log.h | 2 +- src/kernel/ao_log_big.c | 1 - src/kernel/ao_log_gps.c | 1 - src/kernel/ao_log_mega.c | 1 - src/kernel/ao_log_metrum.c | 1 - src/kernel/ao_log_mini.c | 1 - 7 files changed, 2 insertions(+), 6 deletions(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_log.c b/src/kernel/ao_log.c index 3cf85a33..d9c3e00f 100644 --- a/src/kernel/ao_log.c +++ b/src/kernel/ao_log.c @@ -19,6 +19,7 @@ #include #include +__xdata uint8_t ao_log_mutex; __pdata uint32_t ao_log_current_pos; __pdata uint32_t ao_log_end_pos; __pdata uint32_t ao_log_start_pos; diff --git a/src/kernel/ao_log.h b/src/kernel/ao_log.h index 87fa0d4d..33cda3eb 100644 --- a/src/kernel/ao_log.h +++ b/src/kernel/ao_log.h @@ -29,7 +29,7 @@ * by sleeping on this variable. */ extern __xdata uint16_t ao_flight_number; - +extern __xdata uint8_t ao_log_mutex; extern __pdata uint32_t ao_log_current_pos; extern __pdata uint32_t ao_log_end_pos; extern __pdata uint32_t ao_log_start_pos; diff --git a/src/kernel/ao_log_big.c b/src/kernel/ao_log_big.c index db01f46c..8f57bf75 100644 --- a/src/kernel/ao_log_big.c +++ b/src/kernel/ao_log_big.c @@ -17,7 +17,6 @@ #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; diff --git a/src/kernel/ao_log_gps.c b/src/kernel/ao_log_gps.c index 8bf529f4..3b728c25 100644 --- a/src/kernel/ao_log_gps.c +++ b/src/kernel/ao_log_gps.c @@ -23,7 +23,6 @@ #include #include -static __xdata uint8_t ao_log_mutex; static __xdata struct ao_log_gps log; __code uint8_t ao_log_format = AO_LOG_FORMAT_TELEGPS; diff --git a/src/kernel/ao_log_mega.c b/src/kernel/ao_log_mega.c index 8997fd05..cb83be4b 100644 --- a/src/kernel/ao_log_mega.c +++ b/src/kernel/ao_log_mega.c @@ -20,7 +20,6 @@ #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; diff --git a/src/kernel/ao_log_metrum.c b/src/kernel/ao_log_metrum.c index 9b17adc2..08e7b8c4 100644 --- a/src/kernel/ao_log_metrum.c +++ b/src/kernel/ao_log_metrum.c @@ -20,7 +20,6 @@ #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; diff --git a/src/kernel/ao_log_mini.c b/src/kernel/ao_log_mini.c index 29e3bd9f..0ca3ed06 100644 --- a/src/kernel/ao_log_mini.c +++ b/src/kernel/ao_log_mini.c @@ -20,7 +20,6 @@ #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; -- cgit v1.2.3 From f49540acd48292bd9f68ded647561d0e800c619d Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 12 Jun 2014 23:59:37 -0700 Subject: altos/telegps: Create new flight if current flight is erased telegps is unique in that USB may be connected while a flight is active and sensible things should happen. If a flight is being recorded and gets erased, then a new flight should be started. This is done by hooking in the flight erase code and calling out to the tracker code to figure out whether to switch to a new flight or not. Signed-off-by: Keith Packard --- src/kernel/ao_log.c | 22 ++++++++++++++++------ src/kernel/ao_tracker.c | 43 ++++++++++++++++++++++++++++++++++++------- src/kernel/ao_tracker.h | 6 ++++++ 3 files changed, 58 insertions(+), 13 deletions(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_log.c b/src/kernel/ao_log.c index d9c3e00f..91617d93 100644 --- a/src/kernel/ao_log.c +++ b/src/kernel/ao_log.c @@ -18,6 +18,9 @@ #include "ao.h" #include #include +#if HAS_TRACKER +#include +#endif __xdata uint8_t ao_log_mutex; __pdata uint32_t ao_log_current_pos; @@ -251,6 +254,7 @@ ao_log_delete(void) __reentrant { uint8_t slot; uint8_t slots; + uint32_t log_current_pos, log_end_pos; ao_cmd_decimal(); if (ao_cmd_status != ao_cmd_success) @@ -261,10 +265,13 @@ ao_log_delete(void) __reentrant if (ao_cmd_lex_i) { for (slot = 0; slot < slots; slot++) { if (ao_log_flight(slot) == ao_cmd_lex_i) { +#if HAS_TRACKER + ao_tracker_erase_start(ao_cmd_lex_i); +#endif 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) { + log_current_pos = ao_log_pos(slot); + log_end_pos = log_current_pos + ao_config.flight_log_max; + while (log_current_pos < log_end_pos) { uint8_t i; static __xdata uint8_t b; @@ -274,15 +281,18 @@ ao_log_delete(void) __reentrant * memory over and over again */ for (i = 0; i < 16; i++) { - if (ao_storage_read(ao_log_current_pos + i, &b, 1)) + if (ao_storage_read(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; + ao_storage_erase(log_current_pos); + log_current_pos += ao_storage_block; } +#if HAS_TRACKER + ao_tracker_erase_end(); +#endif puts("Erased"); return; } diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c index a6ad87a3..9febc7f0 100644 --- a/src/kernel/ao_tracker.c +++ b/src/kernel/ao_tracker.c @@ -50,6 +50,7 @@ struct gps_position gps_position[GPS_RING]; static uint8_t gps_head; +static uint8_t tracker_mutex; static uint8_t log_started; static struct ao_telemetry_location gps_data; static uint8_t tracker_running; @@ -100,17 +101,19 @@ ao_tracker(void) tracker_running = 0; } - if (new_tracker_running && !tracker_running) { + if (new_tracker_running && !tracker_running) ao_telemetry_set_interval(AO_SEC_TO_TICKS(tracker_interval)); - ao_log_start(); - } else if (!new_tracker_running && tracker_running) { + else if (!new_tracker_running && tracker_running) ao_telemetry_set_interval(0); - ao_log_stop(); - } tracker_running = new_tracker_running; - if (!tracker_running) + if (new_tracker_running && !ao_log_running) + ao_log_start(); + else if (!new_tracker_running && ao_log_running) + ao_log_stop(); + + if (!ao_log_running) continue; if (new & AO_GPS_NEW_DATA) { @@ -138,10 +141,11 @@ ao_tracker(void) } } if (ao_tracker_force_telem) { - printf ("moving %d\n", moving); + printf ("moving %d started %d\n", moving, log_started); flush(); } if (moving) { + ao_mutex_get(&tracker_mutex); if (!log_started) { ao_log_gps_flight(); log_started = 1; @@ -151,12 +155,37 @@ ao_tracker(void) gps_position[gps_head].longitude = gps_data.longitude; gps_position[gps_head].altitude = gps_data.altitude; gps_head = ao_gps_ring_next(gps_head); + ao_mutex_put(&tracker_mutex); } } } } } +static uint8_t erasing_current; + +void +ao_tracker_erase_start(uint16_t flight) +{ + erasing_current = flight == ao_flight_number; + if (erasing_current) { + ao_mutex_get(&tracker_mutex); + ao_log_stop(); + if (++ao_flight_number == 0) + ao_flight_number = 1; + } +} + +void +ao_tracker_erase_end(void) +{ + if (erasing_current) { + ao_log_scan(); + log_started = 0; + ao_mutex_put(&tracker_mutex); + } +} + static struct ao_task ao_tracker_task; static void diff --git a/src/kernel/ao_tracker.h b/src/kernel/ao_tracker.h index a0fd2f49..78c40cb4 100644 --- a/src/kernel/ao_tracker.h +++ b/src/kernel/ao_tracker.h @@ -28,4 +28,10 @@ void ao_tracker_init(void); +void +ao_tracker_erase_start(uint16_t flight); + +void +ao_tracker_erase_end(void); + #endif /* _AO_TRACKER_H_ */ -- cgit v1.2.3